#! /usr/bin/env python

import cvutils, moving, ml, storage

import numpy as np
import sys, argparse
#from cv2.ml import SVM_RBF, SVM_C_SVC
import cv2
from scipy.stats import norm, lognorm

# TODO add mode detection live, add choice of kernel and svm type (to be saved in future classifier format)

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', required = True)
parser.add_argument('-d', dest = 'databaseFilename', help = 'name of the Sqlite database file (overrides the configuration file)')
parser.add_argument('-i', dest = 'videoFilename', help = 'name of the video file (overrides the configuration file)')
parser.add_argument('-n', dest = 'nObjects', help = 'number of objects to classify', type = int, default = None)
parser.add_argument('--plot-speed-distributions', dest = 'plotSpeedDistribution', help = 'simply plots the distributions used for each user type', action = 'store_true')
parser.add_argument('--max-speed-distribution-plot', dest = 'maxSpeedDistributionPlot', help = 'if plotting the user distributions, the maximum speed to display', type = float, default = 50.)

args = parser.parse_args()
params = storage.ProcessParameters(args.configFilename)
classifierParams = storage.ClassifierParameters(params.classifierFilename)

if args.videoFilename is not None:
    videoFilename = args.videoFilename
else:
    videoFilename = params.videoFilename
if args.databaseFilename is not None:
    databaseFilename = args.databaseFilename
else:
    databaseFilename = params.databaseFilename

classifierParams.convertToFrames(params.videoFrameRate, 3.6) # conversion from km/h to m/s
if params.homography is not None:
    invHomography = np.linalg.inv(params.homography)
else:
    invHomography = None

if classifierParams.speedAggregationMethod == 'median':
    speedAggregationFunc = np.median
elif classifierParams.speedAggregationMethod == 'mean':
    speedAggregationFunc = np.mean
elif classifierParams.speedAggregationMethod == 'quantile':
    speedAggregationFunc = lambda speeds: np.percentile(speeds, args.speedAggregationQuantile)
else:
    print('Unknown speed aggregation method: {}. Exiting'.format(classifierParams.speedAggregationMethod))
    sys.exit()

pedBikeCarSVM = ml.SVM()
pedBikeCarSVM.load(classifierParams.pedBikeCarSVMFilename)
bikeCarSVM = ml.SVM()
bikeCarSVM.load(classifierParams.bikeCarSVMFilename)

# log logistic for ped and bik otherwise ((pedBeta/pedAlfa)*((sMean/pedAlfa)**(pedBeta-1)))/((1+(sMean/pedAlfa)**pedBeta)**2.)
speedProbabilities = {'car': lambda s: norm(classifierParams.meanVehicleSpeed, classifierParams.stdVehicleSpeed).pdf(s),
                      'pedestrian': lambda s: norm(classifierParams.meanPedestrianSpeed, classifierParams.stdPedestrianSpeed).pdf(s), 
                      'bicycle': lambda s: lognorm(classifierParams.scaleCyclistSpeed, loc = 0., scale = np.exp(classifierParams.locationCyclistSpeed)).pdf(s)} # numpy lognorm shape, loc, scale: shape for numpy is scale (std of the normal) and scale for numpy is location (mean of the normal)

if args.plotSpeedDistribution:
    import matplotlib.pyplot as plt
    plt.figure()
    for k in speedProbabilities:
        plt.plot(np.arange(0.1, args.maxSpeedDistributionPlot, 0.1), [speedProbabilities[k](s/3.6/25) for s in np.arange(0.1, args.maxSpeedDistributionPlot, 0.1)], label = k)
    plt.xlabel('Speed (km/h)')
    plt.ylabel('Probability')
    plt.legend()
    plt.title('Probability Density Function')
    plt.show()
    sys.exit()

objects = storage.loadTrajectoriesFromSqlite(databaseFilename, 'object', args.nObjects, withFeatures = True)
#features = storage.loadTrajectoriesFromSqlite(databaseFilename, 'feature')
intervals = []
for obj in objects:
    #obj.setFeatures(features)
    intervals.append(obj.getTimeInterval())
timeInterval = moving.TimeInterval.unionIntervals(intervals)

capture = cv2.VideoCapture(videoFilename)
width = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH))
height = int(capture.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT))

pastObjects = []
if params.undistort: # setup undistortion
    [map1, map2] = cvutils.computeUndistortMaps(width, height, params.undistortedImageMultiplication, params.intrinsicCameraMatrix, params.distortionCoefficients)
if capture.isOpened():
    ret = True
    frameNum = timeInterval.first
    capture.set(cv2.cv.CV_CAP_PROP_POS_FRAMES, frameNum)
    lastFrameNum = timeInterval.last

    while ret and frameNum <= lastFrameNum:
        ret, img = capture.read()
        if ret:
            if frameNum%50 == 0:
                print('frame number: {}'.format(frameNum))
            if params.undistort:
                img = cv2.remap(img, map1, map2, interpolation=cv2.INTER_LINEAR)
            currentObjects = []
            for obj in objects:
                inter = obj.getTimeInterval()
                if inter.contains(frameNum):
                    if inter.first == frameNum:
                        obj.initClassifyUserTypeHoGSVM(speedAggregationFunc, pedBikeCarSVM, bikeCarSVM, classifierParams.maxPedestrianSpeed, classifierParams.maxCyclistSpeed, classifierParams.nFramesIgnoreAtEnds)
                        currentObjects.append(obj)
                    elif inter.last == frameNum:
                        obj.classifyUserTypeHoGSVM(minSpeedEquiprobable = classifierParams.minSpeedEquiprobable, speedProbabilities = speedProbabilities)
                        pastObjects.append(obj)
                    else:
                        obj.classifyUserTypeHoGSVMAtInstant(img, frameNum, invHomography, width, height, classifierParams.percentIncreaseCrop, classifierParams.percentIncreaseCrop, classifierParams.minNPixels, classifierParams.hogRescaleSize, classifierParams.hogNOrientations, classifierParams.hogNPixelsPerCell, classifierParams.hogNCellsPerBlock)
                        currentObjects.append(obj)
            objects = currentObjects
        frameNum += 1
    
    for obj in objects:
        obj.classifyUserTypeHoGSVM(minSpeedEquiprobable = classifierParams.minSpeedEquiprobable, speedProbabilities = speedProbabilities)
        pastObjects.append(obj)
    print('Saving user types')
    storage.setRoadUserTypes(databaseFilename, pastObjects)