#! /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('--prediction-method', dest = 'predictionMethod', help = 'prediction method (constant velocity, normal adaptation, point set prediction)', choices = ['cv', 'na', 'ps'])
args = parser.parse_args()

params = utils.TrackingParameters()
params.loadConfigFile(args.configFilename)

# parameters for prediction methods
if args.predictionMethod:
    predictionMethod = args.predictionMethod
else:
    predictionMethod = params.predictionMethod

if predictionMethod == 'cv':
    predictionParameters = prediction.ConstantPredictionParameters(params.maxPredictedSpeed)
elif predictionMethod == 'na':
    predictionParameters = prediction.NormalAdaptationPredictionParameters(params.maxPredictedSpeed, 
                                                                           params.nPredictedTrajectories, 
                                                                           params.maxAcceleration,
                                                                           params.maxSteering,
                                                                           params.useFeaturesForPrediction)
elif predictionMethod == 'ps':
    predictionParameters = prediction.PointSetPredictionParameters(params.nPredictedTrajectories,
                                                                   params.maxPredictedSpeed)
else:
    print('Prediction method {} is not valid. See help.'.format(predictionMethod))
    sys.exit()

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

objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'object')
if params.useFeaturesForPrediction:
    features = storage.loadTrajectoriesFromSqlite(params.databaseFilename,'feature') # needed if normal adaptation
    for obj in objects:
        obj.setFeatures(features)

interactions = events.createInteractions(objects)
for inter in interactions[:1]:
    inter.computeIndicators()
    inter.computeCrossingsCollisions(predictionParameters, params.collisionDistance, params.predictionTimeHorizon, params.crossingZones)

storage.saveIndicators(params.databaseFilename, interactions)

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