#! /usr/bin/env python

import cvutils, moving, ml, storage

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

# TODO add mode detection live

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('-u', dest = 'undistort', help = 'undistort the video (because features have been extracted that way)', action = 'store_true')
#parser.add_argument('-f', dest = 'firstFrameNum', help = 'number of first frame number to display', type = int)
#parser.add_argument('--last-frame', dest = 'lastFrameNum', help = 'number of last frame number to save (for image saving, no display is made)', type = int)
# parser.add_argument('--min-speed-equiprobable', dest = 'minSpeedEquiprobable', help = 'speed value below which all classes are equiprobable (distributions give odd values there) (km/h)', type = float, default = 3.33)
# parser.add_argument('--speed-aggregation', dest = 'speedAggregationMethod', help = 'method to aggregate road user speed', type = str, choices = ['median', 'mean', 'quantile'], default = 'median')
# parser.add_argument('--speed-aggregation-quantile', dest = 'speedAggregationQuantile', help = 'quantile for the speed aggregation, if quantile is chosen', type = int, default = 50)

args = parser.parse_args()
params = storage.ProcessParameters(args.configFilename)

params.convertToFrames(3.6)
invHomography = np.linalg.inv(params.homography)

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

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

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

objects = storage.loadTrajectoriesFromSqlite(params.databaseFilename, 'object')
features = storage.loadTrajectoriesFromSqlite(params.databaseFilename, 'feature')
intervals = []
for obj in objects:
    obj.setFeatures(features)
    intervals.append(obj.getTimeInterval())
timeInterval = moving.unionIntervals(intervals)

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

if params.undistort: # setup undistortion
    [map1, map2] = computeUndistortMaps(width, height, undistortedImageMultiplication, intrinsicCameraMatrix, 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)
            for obj in objects:
                if obj.existsAtInstant(frameNum):
                    if obj.getFirstInstant() == frameNum:
                        print 'first frame for obj {}'.format(obj.getNum())
                        obj.initClassifyUserTypeHoGSVM(speedAggregationFunc, pedBikeCarSVM, bikeCarSVM, params.maxPedestrianSpeed, params.maxCyclistSpeed, params.nFramesIgnoreAtEnds)
                    obj.classifyUserTypeHoGSVMAtInstant(img, frameNum, invHomography, width, height, 0.2, 0.2, 800) # px, py, pixelThreshold
        frameNum += 1
    
    for obj in objects:
        obj.classifyUserTypeHoGSVM(minSpeedEquiprobable = params.minSpeedEquiprobable, speedProbabilities = speedProbabilities)