--- a/python/prediction.py	Thu Dec 04 18:52:07 2014 -0500
+++ b/python/prediction.py	Thu Dec 04 19:07:55 2014 -0500
@@ -5,7 +5,7 @@
 import math
 import random
 import numpy as np
-import trajLearning
+from utils import LCSS
 
 class PredictedTrajectory:
     '''Class for predicted trajectories with lazy evaluation
@@ -167,13 +167,82 @@
     savefig('predicted-trajectories-t-{0}.png'.format(currentInstant))
     close()
 
+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=trajLearning.findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
+        prototypeTrajectories=findPrototypesSpeed(prototypes,secondStepPrototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched,useDestination)
     else:
-        prototypeTrajectories=trajLearning.findPrototypes(prototypes,nMatching,objects,route,partialObjPositions,noiseEntryNums,noiseExitNums,minSimilarity,mostMatched)
+        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):