--- a/python/prediction.py	Sun Dec 07 23:01:02 2014 -0500
+++ b/python/prediction.py	Wed Dec 10 15:27:08 2014 -0500
@@ -4,6 +4,8 @@
 import moving
 import math
 import random
+import numpy as np
+from utils import LCSS
 
 class PredictedTrajectory:
     '''Class for predicted trajectories with lazy evaluation
@@ -46,6 +48,14 @@
 
     def getControl(self):
         return self.control
+        
+def findNearestParams(initialPosition,prototypeTrajectory):
+    ''' nearest parameters are the index of minDistance and the orientation  '''
+    distances=[]
+    for position in prototypeTrajectory.positions:
+        distances.append(moving.Point.distanceNorm2(initialPosition, position))
+    minDistanceIndex= np.argmin(distances)
+    return minDistanceIndex, moving.NormAngle.fromPoint(prototypeTrajectory.velocities[minDistanceIndex]).angle
 
 class PredictedTrajectoryPrototype(PredictedTrajectory):
     '''Predicted trajectory that follows a prototype trajectory
@@ -64,15 +74,31 @@
         self.constantSpeed = constantSpeed
         self.probability = probability
         self.predictedPositions = {0: initialPosition}
-        self.predictedSpeedOrientations = {0: moving.NormAngle.fromPoint(initialVelocity)}
-
+        self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototypeTrajectory)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
+    
     def predictPosition(self, nTimeSteps):
         if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
-            if constantSpeed:
+            if self.constantSpeed:
                 # calculate cumulative distance
-                pass
+                speedNorm= self.predictedSpeedOrientations[0].norm #moving.NormAngle.fromPoint(initialVelocity).norm
+                anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
+                self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(speedNorm, anglePrototype)
+                self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
+            
             else: # see c++ code, calculate ratio
-                pass
+                speedNorm= self.predictedSpeedOrientations[0].norm
+                instant=findNearestParams(self.predictedPositions[0],self.prototypeTrajectory)[0]
+                prototypeSpeeds= self.prototypeTrajectory.getSpeeds()[instant:]
+                ratio=float(speedNorm)/prototypeSpeeds[0]
+                resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
+                anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototypeTrajectory)[1]
+                if nTimeSteps<len(resampledSpeeds):
+                    self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[nTimeSteps], anglePrototype)
+                    self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)                
+                else:
+                    self.predictedSpeedOrientations[nTimeSteps]= moving.NormAngle(resampledSpeeds[-1], anglePrototype)
+                    self.predictedPositions[nTimeSteps],tmp= moving.predictPosition(self.predictedPositions[nTimeSteps-1], self.predictedSpeedOrientations[nTimeSteps-1], moving.NormAngle(0,0), None)
+          
         return self.predictedPositions[nTimeSteps]
 
 class PredictedTrajectoryRandomControl(PredictedTrajectory):
@@ -141,10 +167,94 @@
     savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
     close()
 
-def computeCrossingsCollisionsAtInstant(predictionParams, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
+def calculateProbability(nMatching,similarity,objects):
+    sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
+    prototypeProbability={}
+    for i in similarity.keys():
+        prototypeProbability[i]= similarity[i] * float(nMatching[i])/sumFrequencies
+    sumProbabilities= sum([prototypeProbability[p] for p in prototypeProbability.keys()])
+    probabilities={}
+    for i in prototypeProbability.keys():
+        probabilities[objects[i]]= float(prototypeProbability[i])/sumProbabilities
+    return probabilities
+
+def findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,spatialThreshold=1.0, delta=180):
+    ''' behaviour prediction first step'''
+    if route[0] not in noiseEntryNums: 
+        prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]]
+    elif route[1] not in noiseExitNums:
+        prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]]
+    else:
+        prototypesRoutes=[x for x in sorted(prototypes.keys())]
+    lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
+    similarity={}
+    for y in prototypesRoutes: 
+        if y in prototypes.keys():
+            prototypesIDs=prototypes[y]            
+            for x in prototypesIDs:
+                s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
+                if s >= minSimilarity:
+                    similarity[x]=s
+    
+    if mostMatched==None:
+        probabilities= calculateProbability(nMatching,similarity,objects)        
+        return probabilities
+    else:
+        mostMatchedValues=sorted(similarity.values(),reverse=True)[:mostMatched]
+        keys=[k for k in similarity.keys() if similarity[k] in mostMatchedValues]
+        newSimilarity={}
+        for i in keys:
+            newSimilarity[i]=similarity[i]
+        probabilities= calculateProbability(nMatching,newSimilarity,objects)        
+        return probabilities        
+        
+def findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,spatialThreshold=1.0, delta=180):
+    if useDestination:
+        prototypesRoutes=[route]
+    else:
+        if route[0] not in noiseEntryNums: 
+            prototypesRoutes= [ x for x in sorted(prototypes.keys()) if route[0]==x[0]]
+        elif route[1] not in noiseExitNums:
+            prototypesRoutes=[ x for x in sorted(prototypes.keys()) if route[1]==x[1]]
+        else:
+            prototypesRoutes=[x for x in sorted(prototypes.keys())]
+    lcss = LCSS(similarityFunc=lambda x,y: (distanceForLCSS(x,y) <= spatialThreshold),delta=delta)
+    similarity={}
+    for y in prototypesRoutes: 
+        if y in prototypes.keys():
+            prototypesIDs=prototypes[y]    
+            for x in prototypesIDs:
+                s=lcss.computeNormalized(partialObjPositions, objects[x].positions)
+                if s >= minSimilarity:
+                    similarity[x]=s
+    
+    newSimilarity={}
+    for i in similarity.keys():
+        if i in secondStepPrototypes.keys():
+            for j in secondStepPrototypes[i]:
+                newSimilarity[j]=similarity[i]
+    probabilities= calculateProbability(nMatching,newSimilarity,objects)        
+    return probabilities
+    
+def getPrototypeTrajectory(obj,route,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
+    partialInterval=moving.Interval(obj.getFirstInstant(),currentInstant)
+    partialObjPositions= obj.getObjectInTimeInterval(partialInterval).positions    
+    if useSpeedPrototype:
+        prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
+    else:
+        prototypeTrajectories=findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
+    return prototypeTrajectories
+
+def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):  
     '''returns the lists of collision points and crossing zones'''
-    predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
-    predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)
+    if usePrototypes:
+        prototypeTrajectories1=getPrototypeTrajectory(obj1,route1,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+        prototypeTrajectories2= getPrototypeTrajectory(obj2,route2,currentInstant,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+        predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant,prototypeTrajectories1)
+        predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant,prototypeTrajectories2)     
+    else:
+        predictedTrajectories1 = predictionParams.generatePredictedTrajectories(obj1, currentInstant)
+        predictedTrajectories2 = predictionParams.generatePredictedTrajectories(obj2, currentInstant)        
 
     collisionPoints = []
     crossingZones = []
@@ -167,7 +277,8 @@
                         #    cz = (et1.predictPosition(t1)+et2.predictPosition(t2)).multiply(0.5)
                         cz = moving.segmentIntersection(et1.predictPosition(t1), et1.predictPosition(t1+1), et2.predictPosition(t2), et2.predictPosition(t2+1))
                         if cz != None:
-                            crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)))
+                            deltaV= (et1.predictPosition(t1)- et1.predictPosition(t1+1) - et2.predictPosition(t2)+ et2.predictPosition(t2+1)).norm2()
+                            crossingZones.append(SafetyPoint(cz, et1.probability*et2.probability, abs(t1-t2)-(float(collisionDistanceThreshold)/deltaV)))
                         t2 += 1
                     t1 += 1                        
 
@@ -176,7 +287,7 @@
 
     return currentInstant, collisionPoints, crossingZones
 
-def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
+def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
     '''Computes all crossing and collision points at each common instant for two road users. '''
     collisionPoints={}
     crossingZones={}
@@ -185,16 +296,35 @@
     else:
         commonTimeInterval = obj1.commonTimeInterval(obj2)
     if nProcesses == 1:
-        for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
-            i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
-            if len(cp) != 0:
-                collisionPoints[i] = cp
-            if len(cz) != 0:
-                 crossingZones[i] = cz
+        if usePrototypes:
+            firstInstant= next( (x for x in xrange(commonTimeInterval.first,commonTimeInterval.last) if x-obj1.getFirstInstant() >= acceptPartialLength and x-obj2.getFirstInstant() >= acceptPartialLength), commonTimeInterval.last)
+            commonTimeIntervalList1= list(xrange(firstInstant,commonTimeInterval.last-1)) # do not look at the 1 last position/velocities, often with errors
+            commonTimeIntervalList2= list(xrange(firstInstant,commonTimeInterval.last-1,step)) # do not look at the 1 last position/velocities, often with errors
+            for i in commonTimeIntervalList2: 
+                i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                if len(cp) != 0:
+                    collisionPoints[i] = cp
+                if len(cz) != 0:
+                    crossingZones[i] = cz
+            if collisionPoints!={} or crossingZones!={}:
+                for i in commonTimeIntervalList1:
+                    if i not in commonTimeIntervalList2:
+                        i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                        if len(cp) != 0:
+                            collisionPoints[i] = cp
+                        if len(cz) != 0:
+                            crossingZones[i] = cz                        
+        else:
+            for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
+                i, cp, cz = computeCrossingsCollisionsAtInstant(predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
+                if len(cp) != 0:
+                    collisionPoints[i] = cp
+                if len(cz) != 0:
+                    crossingZones[i] = cz
     else:
         from multiprocessing import Pool
         pool = Pool(processes = nProcesses)
-        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]]
+        jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (predictionParams, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)) for i in list(commonTimeInterval)[:-1]]
         #results = [j.get() for j in jobs]
         #results.sort()
         for j in jobs:
@@ -218,11 +348,11 @@
     def generatePredictedTrajectories(self, obj, instant):
         return []
 
-    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):
-        return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)
+    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):
+        return computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype)
 
-    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):
-        return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses)
+    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1,usePrototypes=True,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True,acceptPartialLength=30, step=1):
+        return computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug, timeInterval, nProcesses,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype,acceptPartialLength, step)
 
     def computeCollisionProbability(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, debug = False, timeInterval = None):
         '''Computes only collision probabilities
@@ -428,10 +558,20 @@
 ####
 # Other Methods
 ####
-
-
-
-
+class PrototypePredictionParameters(PredictionParameters):
+    def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
+        name = 'prototype'
+        PredictionParameters.__init__(self, name, maxSpeed)
+        self.nPredictedTrajectories = nPredictedTrajectories
+        self.constantSpeed = constantSpeed
+        
+    def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
+        predictedTrajectories = []
+        initialPosition = obj.getPositionAtInstant(instant)
+        initialVelocity = obj.getVelocityAtInstant(instant)
+        for prototypeTraj in prototypeTrajectories.keys():
+            predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, prototypeTraj, constantSpeed = self.constantSpeed, probability = prototypeTrajectories[prototypeTraj])) 
+        return predictedTrajectories
 
 if __name__ == "__main__":
     import doctest