#! /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('--cfg', dest = '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)

# parameters for prediction methods
constantVelocityPredictionParameters = prediction.ConstantPredictionParameters(params.maxPredictedSpeed)

normalAdaptationPredictionParameters = prediction.NormalAdaptationPredictionParameters(params.maxPredictedSpeed, 
                                                                                       params.nPredictedTrajectories, 
                                                                                       params.maxAcceleration,
                                                                                       params.maxSteering)

featurePredictionParameters = prediction.PointSetPredictionParameters(params.maxPredictedSpeed, params.nPredictedTrajectories)

evasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(params.maxPredictedSpeed, 
                                                                                 params.nPredictedTrajectories, 
                                                                                 params.minAcceleration,
                                                                                 params.maxAcceleration,
                                                                                 params.maxSteering,
                                                                                 False)

featureEvasiveActionPredictionParameters = prediction.EvasiveActionPredictionParameters(params.maxPredictedSpeed, 
                                                                                        params.nPredictedTrajectories, 
                                                                                        params.minAcceleration,
                                                                                        params.maxAcceleration,
                                                                                        params.maxSteering,
                                                                                        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, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones)

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
    
