--- a/python/prediction.py	Tue Jul 18 16:21:39 2017 -0400
+++ b/python/prediction.py	Tue Jul 18 18:01:16 2017 -0400
@@ -50,14 +50,6 @@
 
     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
@@ -84,36 +76,34 @@
         self.closestPointIdx = prototype.getPositions().getClosestPoint(initialPosition)
         self.deltaPosition = initialPosition-prototype.getPositionAt(self.closestPointIdx)
         self.initialSpeed = initialVelocity.norm2()
-        #self.predictedSpeedOrientations = {0: moving.NormAngle(moving.NormAngle.fromPoint(initialVelocity).norm, findNearestParams(initialPosition,prototype)[1])}#moving.NormAngle.fromPoint(initialVelocity)}
+        if not constantSpeed:
+            self.ratio = self.initialSpeed/prototype.getVelocityAt(self.closestPointIdx).norm2()
     
     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)
                 i = self.closestPointIdx
-                while i < traj.length() and traj.getCumulativeDistance(i) < traveledDistance:
+                while i < trajLength and traj.getCumulativeDistance(i) < traveledDistance:
                     i += 1
-                if i == traj.length():
+                if i == trajLength:
                     v = self.prototype.getVelocityAt(-1)
                     self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+v*((traveledDistance-traj.getCumulativeDistance(i-1))/v.norm2())
                 else:
                     self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i-1]+(traj[i]-traj[i-1])*((traveledDistance-traj.getCumulativeDistance(i-1))/traj.getDistance(i-1))
-            else: # see c++ code, calculate ratio
-                speedNorm= self.predictedSpeedOrientations[0].norm
-                instant=findNearestParams(self.predictedPositions[0],self.prototype)[0]
-                prototypeSpeeds= self.prototype.getSpeeds()[instant:]
-                ratio=float(speedNorm)/prototypeSpeeds[0]
-                resampledSpeeds=[sp*ratio for sp in prototypeSpeeds]
-                anglePrototype = findNearestParams(self.predictedPositions[nTimeSteps-1],self.prototype)[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:
+                traj = self.prototype.getPositions()
+                trajLength = traj.length()
+                nSteps = self.ratio*nTimeSteps+self.closestPointIdx
+                i = int(np.floor(nSteps))
+                if nSteps < trajLength-1:
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[i]+(traj[i+1]-traj[i])*(nSteps-i)
                 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)
-          
+                    v = self.prototype.getVelocityAt(-1)
+                    self.predictedPositions[nTimeSteps] = self.deltaPosition+traj[-1]+v*(nSteps-trajLength+1)
         return self.predictedPositions[nTimeSteps]
 
 class PredictedTrajectoryRandomControl(PredictedTrajectory):