#! /usr/bin/env python

import utils, storage, prediction, events

import sys, argparse

import matplotlib.pyplot as plt
import numpy as np

parser = argparse.ArgumentParser(description='The program processes indicators for all pairs of road users in the scene')
parser.add_argument('configFilename', help = 'name of the configuration file')
#parser.add_argument('--maxspeed', dest = 'maxSpeed', help = 'maximum speed when predicting future motion (km/h)', default = 50, type = int)
#parser.add_argument('--time-horizon', dest = 'maxSpeed', help = 'maximum speed when predicting future motion (km/h)', default = 50, type = int)
args = parser.parse_args()

# TODO work on the way to indicate an interaction definition

# if False: # test if there is a configuration file?
params = utils.TrackingParameters()
params.loadConfigFile(args.configFilename)

# configuration parameters # TODO from command line
maxSpeed = args.maxSpeed/3.6/params.videoFrameRate # speed limit 50 km/h for urban envt, 90km/hr = 25 m/sec for highways
timeHorizon= params.videoFrameRate*5 # prediction time Horizon = 1.5 s (reaction time) (5 second)
collisionDistanceThreshold= 1.8 # m
computeCZ = False

# display = False

# parameters for prediction methods
constantVelocityPredictionParameters = prediction.ConstantPredictionParameters(maxSpeed)

normalAdaptationPredictionParameters = prediction.NormalAdaptationPredictionParameters(maxSpeed, 100, 2./frameRate**2, # m/s2
                                                                                       0.2/frameRate) # rad/s

featurePredictionParameters = prediction.PointSetPredictionParameters(maxSpeed)

evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 100, -9.1/frameRate**2, # m/s2
                                                                                 4.3/frameRate**2, # m/s2
                                                                                 0.5/frameRate, # rad/s
                                                                                 False)

featureEvasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(maxSpeed, 10, -9.1/frameRate**2, # m/s2
                                                                                        4.3/frameRate**2, # m/s2
                                                                                        0.5/frameRate, # rad/s
                                                                                        True)



objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object')
# features = storage.loadTrajectoriesFromSqlite('amherst-10.sqlite','feature') # needed if normal adaptation

interactions = events.createInteractions(objects)
for inter in interactions:
    inter.computeIndicators()
    inter.computeCrossingsCollisions(constantVelocityPredictionParameters, collisionDistanceThreshold, timeHorizon, computeCZ)

storage.saveIndicators(params.databaseFilename, interactions)

# if display:
#     plt.figure()
#     plt.axis('equal')
#     for inter in interactions[:2]:
#         for collisionPoints in inter.collisionPoints.values():
#             for cp in collisionPoints:
#                 plot([cp.x], [cp.y], 'x')

# for the demo, output automatically a map
# possibility to process longitudinal coords only