--- a/python/moving.py	Tue Nov 03 13:48:56 2015 -0500
+++ b/python/moving.py	Mon May 09 15:33:11 2016 -0400
@@ -12,7 +12,7 @@
 
 try:
     from shapely.geometry import Polygon, Point as shapelyPoint
-    from shapely.prepared import prep
+    from shapely.prepared import prep, PreparedGeometry
     shapelyAvailable = True
 except ImportError:
     print('Shapely library could not be loaded')
@@ -35,6 +35,9 @@
     def __repr__(self):
         return self.__str__()
 
+    def __eq__(self, other):
+        return ((self.first == other.first) and (self.last == other.last)) or ((self.first == other.last) and (self.last == other.first))
+
     def empty(self):
         return self.first > self.last
 
@@ -58,6 +61,10 @@
         '''Indicates if the temporal interval of self is comprised in interval2'''
         return (self.first >= interval2.first) and (self.last <= interval2.last)
 
+    def shift(self, offset):
+        self.first += offset
+        self.last += offset
+
     @classmethod
     def union(cls, interval1, interval2):
         '''Smallest interval comprising self and interval2'''
@@ -171,6 +178,9 @@
     def commonTimeInterval(self, obj2):
         return TimeInterval.intersection(self.getTimeInterval(), obj2.getTimeInterval())
 
+    def shiftTimeInterval(self, offset):
+        self.timeInterval.shift(offset)
+
 class Point(object):
     def __init__(self, x, y):
         self.x = x
@@ -182,6 +192,9 @@
     def __repr__(self):
         return self.__str__()
 
+    def __eq__(self, other):
+        return (self.x == other.x) and (self.y == other.y)
+
     def __add__(self, other):
         return Point(self.x+other.x, self.y+other.y)
 
@@ -217,6 +230,10 @@
     def plot(self, options = 'o', **kwargs):
         plot([self.x], [self.y], options, **kwargs)
 
+    @staticmethod
+    def plotSegment(p1, p2, options = 'o', **kwargs):
+        plot([p1.x, p2.x], [p1.y, p2.y], options, **kwargs)
+
     def norm2Squared(self):
         '''2-norm distance (Euclidean distance)'''
         return self.x**2+self.y**2
@@ -339,8 +356,11 @@
 
 if shapelyAvailable:
     def pointsInPolygon(points, polygon):
-        '''Optimized tests of a series of points within (Shapely) polygon '''
-        prepared_polygon = prep(polygon)
+        '''Optimized tests of a series of points within (Shapely) polygon (not prepared)'''
+        if type(polygon) == PreparedGeometry:
+            prepared_polygon = polygon
+        else:
+            prepared_polygon = prep(polygon)
         return filter(prepared_polygon.contains, points)
 
 # Functions for coordinate transformation
@@ -360,10 +380,10 @@
     ss_spline_d = []
     #Prepare subsegment distances
     for spline in range(len(splines)):
-        ss_spline_d.append([[],[],[]])
-        ss_spline_d[spline][0] = zeros(len(splines[spline])-1)  #Incremental distance
-        ss_spline_d[spline][1] = zeros(len(splines[spline])-1)  #Cumulative distance
-        ss_spline_d[spline][2] = zeros(len(splines[spline]))  #Cumulative distance with trailing distance
+        ss_spline_d[spline]=[]#.append([[],[],[]])
+        ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Incremental distance
+        ss_spline_d[spline].append(zeros(len(splines[spline])-1))  #Cumulative distance
+        ss_spline_d[spline].append(zeros(len(splines[spline])))  #Cumulative distance with trailing distance
         for spline_p in range(len(splines[spline])):
             if spline_p > (len(splines[spline]) - 2):
                 break
@@ -375,6 +395,18 @@
 
     return ss_spline_d
 
+def prepareSplines(splines):
+    'Approximates slope singularity by giving some slope roundoff; account for roundoff error'
+    for spline in splines:
+        p1 = spline[0]
+        for i in xrange(len(spline)-1):
+            p2 = spline[i+1]
+            if(round(p1.x, 10) == round(p2.x, 10)):
+                p2.x += 0.0000000001
+            if(round(p1.y, 10) == round(p2.y, 10)):
+                p2.y += 0.0000000001            
+            p1 = p2
+
 def ppldb2p(qx,qy, p0x,p0y, p1x,p1y):
     ''' Point-projection (Q) on line defined by 2 points (P0,P1). 
         http://cs.nyu.edu/~yap/classes/visual/03s/hw/h2/math.pdf
@@ -383,10 +415,10 @@
         return None
     try:
         #Approximate slope singularity by giving some slope roundoff; account for roundoff error
-        if(round(p0x, 10) == round(p1x, 10)):
-            p1x += 0.0000000001
-        if(round(p0y, 10) == round(p1y, 10)):
-            p1y += 0.0000000001            
+        # if(round(p0x, 10) == round(p1x, 10)):
+        #     p1x += 0.0000000001
+        # if(round(p0y, 10) == round(p1y, 10)):
+        #     p1y += 0.0000000001            
         #make the calculation
         Y = (-(qx)*(p0y-p1y)-(qy*(p0y-p1y)**2)/(p0x-p1x)+p0x**2*(p0y-p1y)/(p0x-p1x)-p0x*p1x*(p0y-p1y)/(p0x-p1x)-p0y*(p0x-p1x))/(p1x-p0x-(p0y-p1y)**2/(p0x-p1x))
         X = (-Y*(p1y-p0y)+qx*(p1x-p0x)+qy*(p1y-p0y))/(p1x-p0x)
@@ -397,8 +429,9 @@
     return Point(X,Y)
 
 def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
-    ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). A spline is a list of points (class Point), most likely a trajectory. 
-        
+    ''' Snap a point p to it's nearest subsegment of it's nearest spline (from the list splines). 
+    A spline is a list of points (class Point), most likely a trajectory. 
+    
     Output:
     =======
     [spline index, 
@@ -412,36 +445,38 @@
     '''
     minOffsetY = float('inf')
     #For each spline
-    for spline in range(len(splines)):
+    for splineIdx in range(len(splines)):
         #For each spline point index
-        for spline_p in range(len(splines[spline])-1):
+        for spline_p in range(len(splines[splineIdx])-1):
             #Get closest point on spline
-            closestPoint = ppldb2p(p.x,p.y,splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][spline_p+1][0],splines[spline][spline_p+1][1])
+            closestPoint = ppldb2p(p.x,p.y,splines[splineIdx][spline_p][0],splines[splineIdx][spline_p][1],splines[splineIdx][spline_p+1][0],splines[splineIdx][spline_p+1][1])
             if closestPoint is None:
-                print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(spline, spline_p))
+                print('Error: Spline {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(splineIdx, spline_p))
                 return None
-            # check if the 
-            if utils.inBetween(splines[spline][spline_p][0], splines[spline][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[spline][spline_p][1], splines[spline][spline_p+1][1], closestPoint.y): 
+            # check if the projected point is in between the current segment of the alignment bounds
+            if utils.inBetween(splines[splineIdx][spline_p][0], splines[splineIdx][spline_p+1][0], closestPoint.x) and utils.inBetween(splines[splineIdx][spline_p][1], splines[splineIdx][spline_p+1][1], closestPoint.y): 
                 offsetY = Point.distanceNorm2(closestPoint, p)
                 if offsetY < minOffsetY:
                     minOffsetY = offsetY
-                    snappedSpline = spline
+                    snappedSplineIdx = splineIdx
                     snappedSplineLeadingPoint = spline_p
                     snappedPoint = Point(closestPoint.x, closestPoint.y)
                 #Jump loop if significantly close
                 if offsetY < goodEnoughSplineDistance: 
                     break
+
     #Get sub-segment distance
     if minOffsetY != float('inf'):
-        subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSpline][snappedSplineLeadingPoint])
+        subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint])
         #Get cumulative alignment distance (total segment distance)
-        splineDistanceS = splines[snappedSpline].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
-        orthogonalSplineVector = (splines[snappedSpline][snappedSplineLeadingPoint+1]-splines[snappedSpline][snappedSplineLeadingPoint]).orthogonal()
+        splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
+        orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal()
         offsetVector = p-snappedPoint
         if Point.dot(orthogonalSplineVector, offsetVector) < 0:
             minOffsetY = -minOffsetY
-        return [snappedSpline, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
+        return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
     else:
+        print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p))
         return None
 
 def getXYfromSY(s, y, splineNum, splines, mode = 0):
@@ -656,7 +691,6 @@
     def __repr__(self):
         return self.__str__()
 
-
     def __iter__(self):
         self.iterInstantNum = 0
         return self
@@ -668,6 +702,15 @@
             self.iterInstantNum += 1
             return self[self.iterInstantNum-1]
 
+    def __eq__(self, other):
+        if self.length() == other.length():
+            result = True
+            for p, po in zip(self, other):
+                result = result and (p == po)
+            return result
+        else:
+            return False
+
     def setPositionXY(self, i, x, y):
         if i < self.__len__():
             self.positions[0][i] = x
@@ -758,6 +801,26 @@
             diff.addPosition(diff[-1])
         return diff
 
+    def differentiateSG(self, window_length, polyorder, deriv=0, delta=1.0, axis=-1, mode='interp', cval=0.0, removeBothEnds = 2):
+        '''Differentiates the trajectory using the Savitsky Golay filter
+
+        window_length : The length of the filter window (i.e. the number of coefficients). window_length must be a positive odd integer.
+        polyorder : The order of the polynomial used to fit the samples. polyorder must be less than window_length.
+        deriv : The order of the derivative to compute. This must be a nonnegative integer. The default is 0, which means to filter the data without differentiating.
+        delta : The spacing of the samples to which the filter will be applied. This is only used if deriv > 0. Default is 1.0.
+        axis : The axis of the array x along which the filter is to be applied. Default is -1.
+        mode : Must be mirror, constant, nearest, wrap or interp. This determines the type of extension to use for the padded signal to which the filter is applied. When mode is constant, the padding value is given by cval. See the Notes for more details on mirror, constant, wrap, and nearest. When the interp mode is selected (the default), no extension is used. Instead, a degree polyorder polynomial is fit to the last window_length values of the edges, and this polynomial is used to evaluate the last window_length // 2 output values.
+        cval : Value to fill past the edges of the input if mode is constant. Default is 0.0.'''
+        from scipy.signal import savgol_filter
+
+        if removeBothEnds >=1:
+            pos = [self.positions[0][removeBothEnds:-removeBothEnds],
+                   self.positions[1][removeBothEnds:-removeBothEnds]]
+        else:
+            pos = self.positions
+        filtered = savgol_filter(pos, window_length, polyorder, deriv, delta, axis, mode, cval)
+        return Trajectory(filtered)
+
     def norm(self):
         '''Returns the list of the norms at each instant'''
 #        def add(x, y): return x+y
@@ -800,6 +863,11 @@
         else:
             print('Index {} beyond trajectory length {}'.format(i, self.length()))
 
+    def getMaxDistance(self, metric):
+        'Returns the maximum distance between points in the trajectory' 
+        positions = self.getPositions().asArray().T
+        return cdist(positions, positions, metric = metric).max()
+
     def similarOrientation(self, refDirection, cosineThreshold, minProportion = 0.5):
         '''Indicates whether the minProportion (<=1.) (eg half) of the trajectory elements (vectors for velocity) 
         have a cosine with refDirection is smaller than cosineThreshold'''
@@ -811,19 +879,23 @@
         return count >= lengthThreshold
 
     def wiggliness(self):
-        return self.getCumulativeDistance(self.length()-1)/float(Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1)))
+        straightDistance = Point.distanceNorm2(self.__getitem__(0),self.__getitem__(self.length()-1))
+        if straightDistance > 0:
+            return self.getCumulativeDistance(self.length()-1)/float(straightDistance)
+        else:
+            return None
 
     def getIntersections(self, p1, p2):
         '''Returns a list of the indices at which the trajectory 
         intersects with the segment of extremities p1 and p2 
-        the list is empty if there is no crossing'''
+        Returns an empty list if there is no crossing'''
         indices = []
         intersections = []
 
         for i in xrange(self.length()-1):
             q1=self.__getitem__(i)
             q2=self.__getitem__(i+1)
-            p = utils.segmentIntersection(q1, q2, p1, p2)
+            p = segmentIntersection(q1, q2, p1, p2)
             if p is not None:
                 if q1.x != q2.x:
                     ratio = (p.x-q1.x)/(q2.x-q1.x)
@@ -833,19 +905,19 @@
                     ratio = 0
                 indices.append(i+ratio)
                 intersections.append(p)
-        return indices
+        return indices, intersections
 
     def getLineIntersections(self, p1, p2):
         '''Returns a list of the indices at which the trajectory 
-        intersects with the segment of extremities p1 and p2 
-        the list is empty if there is no crossing'''
+        intersects with the line going through p1 and p2 
+        Returns an empty list if there is no crossing'''
         indices = []
         intersections = []
         
         for i in xrange(self.length()-1):
             q1=self.__getitem__(i)
             q2=self.__getitem__(i+1)
-            p = utils.segmentLineIntersection(p1, p2, q1, q2)
+            p = segmentLineIntersection(p1, p2, q1, q2)
             if p is not None:
                 if q1.x != q2.x:
                     ratio = (p.x-q1.x)/(q2.x-q1.x)
@@ -864,34 +936,57 @@
                                self.positions[1][inter.first:inter.last+1]])
         else:
             return None
-    
+
+    def subSample(self, step):
+        'Returns the positions very step'
+        return Trajectory([self.positions[0][::step],
+                           self.positions[1][::step]])
+
     if shapelyAvailable:
-        def getTrajectoryInPolygon(self, polygon):
-            '''Returns the trajectory built with the set of points inside the (shapely) polygon'''
+        def getTrajectoryInPolygon(self, polygon, t2 = None):
+            '''Returns the trajectory built with the set of points inside the (shapely) polygon
+            The polygon could be a prepared polygon (faster) from prepared.prep
+
+            t2 is another trajectory (could be velocities) 
+            which is filtered based on the first (self) trajectory'''
             traj = Trajectory()
-            points = [p.asShapely() for p in self]
-            for p in pointsInPolygon(points, polygon):
-                    traj.addPositionXY(p.x, p.y)
-            return traj
+            inPolygon = []
+            for x, y in zip(self.positions[0], self.positions[1]):
+                inPolygon.append(polygon.contains(shapelyPoint(x, y)))
+                if inPolygon[-1]:
+                    traj.addPositionXY(x, y)
+            traj2 = Trajectory()
+            if t2 is not None:
+                for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
+                    if inp:
+                        traj2.addPositionXY(x, y)
+            return traj, traj2
 
         def proportionInPolygon(self, polygon, minProportion = 0.5):
-            pointsIn = pointsInPolygon([p.asShapely() for p in self], polygon)
+            inPolygon = [polygon.contains(shapelyPoint(x, y)) for x, y in zip(self.positions[0], self.positions[1])]
             lengthThreshold = float(self.length())*minProportion
-            return len(pointsIn) >= lengthThreshold
+            return sum(inPolygon) >= lengthThreshold
     else:
-        def getTrajectoryInPolygon(self, polygon):
+        def getTrajectoryInPolygon(self, polygon, t2 = None):
             '''Returns the trajectory built with the set of points inside the polygon
             (array of Nx2 coordinates of the polygon vertices)'''
             traj = Trajectory()
+            inPolygon = []
             for p in self:
-                if p.inPolygon(polygon):
+                inPolygon.append(p.inPolygon(polygon))
+                if inPolygon[-1]:
                     traj.addPosition(p)
-            return traj
+            traj2 = Trajectory()
+            if t2 is not None:
+                for inp, x, y in zip(inPolygon, t2.positions[0], t2.positions[1]):
+                    if inp:
+                        traj2.addPositionXY(p.x, p.y)
+            return traj, traj2
 
         def proportionInPolygon(self, polygon, minProportion = 0.5):
-            pointsInPolygon = [p.inPolygon(polygon) for p in self]
+            inPolygon = [p.inPolygon(polygon) for p in self]
             lengthThreshold = float(self.length())*minProportion
-            return len(pointsInPolygon) >= lengthThreshold
+            return sum(inPolygon) >= lengthThreshold
 
     @staticmethod
     def lcss(t1, t2, lcss):
@@ -955,7 +1050,7 @@
         '''Returns a list of the indices at which the trajectory 
         goes past the curvilinear coordinate S1
         (in provided lane if lane is not None)
-        the list is empty if there is no crossing'''
+        Returns an empty list if there is no crossing'''
         indices = []
         for i in xrange(self.length()-1):
             q1=self.__getitem__(i)
@@ -1526,15 +1621,19 @@
 # Annotations
 ##################
 
-class BBAnnotation(MovingObject):
-    '''Class for a series of ground truth annotations using bounding boxes
-    Its center is the center of the containing shape
+class BBMovingObject(MovingObject):
+    '''Class for a moving object represented as a bounding box
+    used for series of ground truth annotations using bounding boxes
+     and for the output of Urban Tracker http://www.jpjodoin.com/urbantracker/
 
     By default in image space
+
+    Its center is the center of the box (generalize to other shapes?) 
+    (computed after projecting if homography available)
     '''
 
     def __init__(self, num = None, timeInterval = None, topLeftPositions = None, bottomRightPositions = None, userType = userType2Num['unknown']):
-        super(BBAnnotation, self).__init__(num, timeInterval, userType = userType)
+        super(BBMovingObject, self).__init__(num, timeInterval, userType = userType)
         self.topLeftPositions = topLeftPositions.getPositions()
         self.bottomRightPositions = bottomRightPositions.getPositions()
 
@@ -1561,7 +1660,7 @@
     Keni, Bernardin, and Stiefelhagen Rainer. "Evaluating multiple object tracking performance: the CLEAR MOT metrics." EURASIP Journal on Image and Video Processing 2008 (2008)
 
     objects and annotations are supposed to in the same space
-    current implementation is BBAnnotations (bounding boxes)
+    current implementation is BBMovingObject (bounding boxes)
     mathingDistance is threshold on matching between annotation and object
 
     TO: tracker output (objects)