--- a/python/prediction.py	Tue Jul 18 18:01:16 2017 -0400
+++ b/python/prediction.py	Wed Jul 19 18:02:38 2017 -0400
@@ -82,7 +82,6 @@
     def predictPosition(self, nTimeSteps):
         if nTimeSteps > 0 and not nTimeSteps in self.predictedPositions.keys():
             if self.constantSpeed:
-                # calculate cumulative distance
                 traj = self.prototype.getPositions()
                 trajLength = traj.length()
                 traveledDistance = nTimeSteps*self.initialSpeed + traj.getCumulativeDistance(self.closestPointIdx)
@@ -161,8 +160,9 @@
     return collision, t, p1, p2
 
 def savePredictedTrajectoriesFigure(currentInstant, obj1, obj2, predictedTrajectories1, predictedTrajectories2, timeHorizon):
-    from matplotlib.pyplot import figure, axis, title, close, savefig
+    from matplotlib.pyplot import figure, axis, title, clf, savefig
     figure()
+    #clf()
     for et in predictedTrajectories1:
         et.predictPosition(int(np.round(timeHorizon)))
         et.plot('rx')
@@ -170,12 +170,11 @@
     for et in predictedTrajectories2:
         et.predictPosition(int(np.round(timeHorizon)))
         et.plot('bx')
-    obj1.plot('r')
-    obj2.plot('b')
+    obj1.plot('r', withOrigin = True)
+    obj2.plot('b', withOrigin = True)
     title('instant {0}'.format(currentInstant))
     axis('equal')
     savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
-    close()
 
 def calculateProbability(nMatching,similarity,objects):
     sumFrequencies=sum([nMatching[p] for p in similarity.keys()])
@@ -255,23 +254,22 @@
         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 = False,route1= (-1,-1),route2=(-1,-1),prototypes={},secondStepPrototypes={},nMatching={},objects=[],noiseEntryNums=[],noiseExitNums=[],minSimilarity=0.1,mostMatched=None,useDestination=True,useSpeedPrototype=True):  
+def computeCrossingsCollisionsAtInstant(predictionParams,currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#, usePrototypes = False,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'''
-    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)        
+    # 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 = []
     for et1 in predictedTrajectories1:
         for et2 in predictedTrajectories2:
             collision, t, p1, p2 = computeCollisionTime(et1, et2, collisionDistanceThreshold, timeHorizon)
-
             if collision:
                 collisionPoints.append(SafetyPoint((p1+p2)*0.5, et1.probability*et2.probability, t))
             elif computeCZ: # check if there is a crossing zone
@@ -309,10 +307,10 @@
     def generatePredictedTrajectories(self, obj, instant):
         return []
 
-    def computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False,usePrototypes = False,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 computeCrossingsCollisionsAtInstant(self, currentInstant, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False):#,usePrototypes = False,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,usePrototypes = False,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):
+    def computeCrossingsCollisions(self, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None, nProcesses = 1):#,usePrototypes = False,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):
         #def computeCrossingsCollisions(predictionParams, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ = False, debug = False, timeInterval = None,nProcesses = 1, usePrototypes = False,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={}
@@ -322,34 +320,34 @@
         else:
             commonTimeInterval = obj1.commonTimeInterval(obj2)
         if nProcesses == 1:
-            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= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
-                commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
-                for i in commonTimeIntervalList2: 
-                    i, cp, cz = self.computeCrossingsCollisionsAtInstant(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 = self.computeCrossingsCollisionsAtInstant(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 = self.computeCrossingsCollisionsAtInstant(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 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= range(firstInstant,commonTimeInterval.last-1) # do not look at the 1 last position/velocities, often with errors
+            #     commonTimeIntervalList2= range(firstInstant,commonTimeInterval.last-1,step) # do not look at the 1 last position/velocities, often with errors
+            #     for i in commonTimeIntervalList2: 
+            #         i, cp, cz = self.computeCrossingsCollisionsAtInstant(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 = self.computeCrossingsCollisionsAtInstant(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(self, 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:
             pool = Pool(processes = nProcesses)
-            jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, 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]]
+            jobs = [pool.apply_async(computeCrossingsCollisionsAtInstant, args = (self, i, obj1, obj2, collisionDistanceThreshold, timeHorizon, computeCZ, debug)) for i in list(commonTimeInterval)[:-1]] #,usePrototypes,route1,route2,prototypes,secondStepPrototypes,nMatching,objects,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination,useSpeedPrototype
             #results = [j.get() for j in jobs]
             #results.sort()
             for j in jobs:
@@ -437,7 +435,6 @@
         return predictedTrajectories
 
 class PointSetPredictionParameters(PredictionParameters):
-    # todo generate several trajectories with normal adaptatoins from each position (feature)
     def __init__(self, maxSpeed):
         PredictionParameters.__init__(self, 'point set', maxSpeed)
         #self.nPredictedTrajectories = nPredictedTrajectories
@@ -456,6 +453,7 @@
             print('Object {} has no features'.format(obj.getNum()))
             return None
 
+        
 class EvasiveActionPredictionParameters(PredictionParameters):
     def __init__(self, maxSpeed, nPredictedTrajectories, accelerationDistribution, steeringDistribution, useFeatures = False):
         '''Suggested acceleration distribution may not be symmetric, eg
@@ -580,22 +578,36 @@
 
         return currentInstant, collisionPoints, crossingZones
 
-####
-# Other Methods
-####
 class PrototypePredictionParameters(PredictionParameters):
-    def __init__(self, maxSpeed, nPredictedTrajectories, constantSpeed = True):
-        name = 'prototype'
-        PredictionParameters.__init__(self, name, maxSpeed)
+    def __init__(self, prototypes, nPredictedTrajectories, pointSimilarityDistance, minSimilarity, lcssMetric = 'cityblock', minFeatureTime = 10, constantSpeed = False, useFeatures = True): # lcss parameters
+        PredictionParameters.__init__(self, 'prototypes', None)
+        self.prototypes = prototypes
         self.nPredictedTrajectories = nPredictedTrajectories
+        self.lcss = LCSS(metric = lcssMetric, epsilon = pointSimilarityDistance)
+        #self.similarityFunc = lambda x,y : self.lcss.computeNormalized(x, y)
+        self.minSimilarity = minSimilarity
+        self.minFeatureTime = minFeatureTime
         self.constantSpeed = constantSpeed
+        self.useFeatures = useFeatures
         
-    def generatePredictedTrajectories(self, obj, instant,prototypeTrajectories):
+    def generatePredictedTrajectories(self, obj, instant):
         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])) 
+        if instant-obj.getFirstInstant()+1 >= self.minFeatureTime:
+            if self.useFeatures and obj.hasFeatures():
+                # get current features existing for the most time, sort on first instant of feature and take n first
+                pass
+            else:
+                if not hasattr(obj, 'prototypeSimilarities'):
+                    obj.prototypeSimilarities = []
+                    for proto in self.prototypes:
+                        self.lcss.similarities(proto.getMovingObject().getPositions().asArray().T, obj.getPositions().asArray().T)
+                        similarities = self.lcss.similarityTable[-1, :-1].astype(float)
+                        obj.prototypeSimilarities.append(similarities/np.minimum(np.arange(1.,len(similarities)+1), proto.getMovingObject().length()*np.ones(len(similarities))))
+                for proto, similarities in zip(self.prototypes, obj.prototypeSimilarities):
+                    if similarities[instant-obj.getFirstInstant()] >= self.minSimilarity:
+                        initialPosition = obj.getPositionAtInstant(instant)
+                        initialVelocity = obj.getVelocityAtInstant(instant)
+                        predictedTrajectories.append(PredictedTrajectoryPrototype(initialPosition, initialVelocity, proto.getMovingObject(), constantSpeed = self.constantSpeed, probability = proto.getNMatchings())) 
         return predictedTrajectories
 
 if __name__ == "__main__":