--- a/python/extrapolation.py	Tue Jul 24 15:18:12 2012 -0400
+++ b/python/extrapolation.py	Tue Jul 24 18:07:23 2012 -0400
@@ -34,7 +34,6 @@
 
 class ExtrapolatedTrajectoryConstant(ExtrapolatedTrajectory):
     '''Extrapolated trajectory at constant speed or acceleration
-    TODO add limits if acceleration
     TODO generalize by passing a series of velocities/accelerations'''
 
     def __init__(self, initialPosition, initialVelocity, control = moving.NormAngle(0,0), probability = 1, maxSpeed = None):
@@ -68,10 +67,19 @@
     def __init__(self, name):
         self.name = name
 
-def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity, maxSpeed):
+def createExtrapolatedTrajectories(extrapolationParameters, initialPosition, initialVelocity):
     '''extrapolationParameters specific to each method (in name field)  '''
     if extrapolationParameters.name == 'constant velocity':
-        return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = maxSpeed)]
+        return [ExtrapolatedTrajectoryConstant(initialPosition, initialVelocity, maxSpeed = extrapolationParameters.maxSpeed)]
+    elif extrapolationParameters.name == 'normal adaptation':
+        # generate several and with the right distribution
+        extrapolatedTrajectories = []
+        for i in xrange(extrapolationParameters.nExtrapolatedTrajectories):
+            extrapolatedTrajectories.append(ExtrapolatedTrajectoryNormalAdaptation(initialPosition, initialVelocity, 
+                                                                                   extrapolationParameters.accelerationDistribution, 
+                                                                                   extrapolationParameters.steeringDistribution, 
+                                                                                   maxSpeed = extrapolationParameters.maxSpeed))
+        return extrapolatedTrajectories
     else:
         print('Unknown extrapolation hypothesis')
         return []
@@ -85,17 +93,20 @@
         self.probability = probability
         self.indicator = indicator
 
+def saveSafetyPoints(out, objNum1, objNum2, instant, points):
+    for p in points:
+        out.write('{0} {1} {2} {3} {4} {5} {6}\n'.format(objNum1, objNum2, instant, p.x, p.y, p.probability, p.indicator))
+
 def computeExpectedIndicator(points):
     from numpy import sum
     return sum([p.indicator*p.probability for p in points])/sum([p.probability for p in points])
 
-def computeCrossingsCollisions(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
+def computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon):
     '''returns the lists of collision points and crossing zones '''
     collisionPoints = []
     crossingZones = []
     for et1 in extrapolatedTrajectories1:
         for et2 in extrapolatedTrajectories2:
-            
             t = 1
             p1 = et1.predictPosition(t)
             p2 = et2.predictPosition(t)
@@ -124,7 +135,43 @@
                     t1 += 1                        
     return collisionPoints, crossingZones
     
+def computeCrossingsCollisions(obj1, obj2, extrapolationParameters, collisionDistanceThreshold, timeHorizon, outCP, outCZ, debug = False):
+    collisionPoints={}
+    crossingZones={}
+    #TTCs = {}
+    #pPETs = {}
+    commonTimeInterval = obj1.commonTimeInterval(obj2)
+    for i in list(commonTimeInterval)[:-1]: # do not look at the 1 last position/velocities, often with errors
+        extrapolatedTrajectories1 = createExtrapolatedTrajectories(extrapolationParameters, obj1.getPositionAtInstant(i), obj1.getVelocityAtInstant(i))
+        extrapolatedTrajectories2 = createExtrapolatedTrajectories(extrapolationParameters, obj2.getPositionAtInstant(i), obj2.getVelocityAtInstant(i))
 
+        collisionPoints[i], crossingZones[i] = computeCrossingsCollisionsAtInstant(extrapolatedTrajectories1, extrapolatedTrajectories2, collisionDistanceThreshold, timeHorizon)
+        # if len(collisionPoints[i]) > 0:
+        #     TTCs[i] = extrapolation.computeExpectedIndicator(collisionPoints[i])
+        # if len(crossingZones[i]) > 0:
+        #     pPETs[i] = extrapolation.computeExpectedIndicator(crossingZones[i])
+        saveSafetyPoints(outCP, obj1.num, obj2.num, i, collisionPoints[i])
+        saveSafetyPoints(outCZ, obj1.num, obj2.num, i, crossingZones[i])
+
+        if debug:
+            from matplotlib.pyplot import figure, axis
+            figure()
+            obj1.draw('r')
+            obj2.draw('b')
+            for et in extrapolatedTrajectories1:
+                et.predictPosition(timeHorizon)
+                et.draw('rx')
+            for et in extrapolatedTrajectories2:
+                et.predictPosition(timeHorizon)
+                et.draw('bx')
+            axis('equal')
+                
+
+        # probability of successful evasive action = 1-P(Collision) using larger control values
+
+    return collisionPoints, crossingZones
+
+###############
 
 # Default values: to remove because we cannot tweak that from a script where the value may be different
 FPS= 25  # No. of frame per second (FPS)