--- a/trafficintelligence/moving.py	Tue Sep 25 17:08:37 2018 -0400
+++ b/trafficintelligence/moving.py	Tue Oct 16 12:46:29 2018 -0400
@@ -248,7 +248,7 @@
         'Projects point projected on v, v.orthogonal()'
         e1 = v/v.norm2()
         e2 = e1.orthogonal(clockwise)
-        return Point(Point.dot(self, e1), Point.doc(self, e2))
+        return Point(Point.dot(self, e1), Point.dot(self, e2))
 
     def rotate(self, theta):
         return Point(self.x*cos(theta)-self.y*sin(theta), self.x*sin(theta)+self.y*cos(theta))
@@ -409,42 +409,15 @@
 
 # Functions for coordinate transformation
 # From Paul St-Aubin's PVA tools
-def subsec_spline_dist(splines):
-    ''' Prepare list of spline subsegments from a spline list. 
-    
-    Output:
-    =======
-    ss_spline_d[spline #][mode][station]
-    
-    where:
-        mode=0: incremental distance
-        mode=1: cumulative distance
-        mode=2: cumulative distance with trailing distance
-    '''
-    ss_spline_d = []
-    #Prepare subsegment distances
-    for spline in range(len(splines)):
-        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
-            ss_spline_d[spline][0][spline_p] = utils.pointDistanceL2(splines[spline][spline_p][0],splines[spline][spline_p][1],splines[spline][(spline_p+1)][0],splines[spline][(spline_p+1)][1])
-            ss_spline_d[spline][1][spline_p] = sum(ss_spline_d[spline][0][0:spline_p])
-            ss_spline_d[spline][2][spline_p] = ss_spline_d[spline][1][spline_p]#sum(ss_spline_d[spline][0][0:spline_p])
-    
-    ss_spline_d[spline][2][-1] = ss_spline_d[spline][2][-2] + ss_spline_d[spline][0][-1]
-
-    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 range(len(spline)-1):
-            p2 = spline[i+1]
+def prepareAlignments(alignments):
+    '''Prepares alignments (list of splines, each typically represented as a Trajectory)
+    - computes cumulative distances
+    - approximates slope singularity by giving some slope roundoff (account for roundoff error)'''
+    for alignment in alignments:
+        alignment.computeCumulativeDistances()
+        p1 = alignment[0]
+        for i in range(len(alignment)-1):
+            p2 = alignment[i+1]
             if(round(p1.x, 10) == round(p2.x, 10)):
                 p2.x += 0.0000000001
             if(round(p1.y, 10) == round(p2.y, 10)):
@@ -472,95 +445,81 @@
         import pdb; pdb.set_trace()  
     return Point(X,Y)
 
-def getSYfromXY(p, splines, goodEnoughSplineDistance = 0.5):
-    ''' Snap a point p to its nearest subsegment of it's nearest spline (from the list splines). 
-    A spline is a list of points (class Point), most likely a trajectory. 
+def getSYfromXY(p, alignments, goodEnoughAlignmentDistance = 0.5):
+    ''' Snap a point p to its nearest subsegment of it's nearest alignment (from the list alignments). 
+    A alignment is a list of points (class Point), most likely a trajectory. 
     
     Output:
     =======
-    [spline index, 
+    [alignment index, 
     subsegment leading point index, 
     snapped point, 
     subsegment distance, 
-    spline distance,
+    alignment distance,
     orthogonal point offset]
 
     or None
     '''
     minOffsetY = float('inf')
-    #For each spline
-    for splineIdx in range(len(splines)):
-        #For each spline point index
-        for spline_p in range(len(splines[splineIdx])-1):
-            #Get closest point on spline
-            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])
+    #For each alignment
+    for alignmentIdx in range(len(alignments)):
+        #For each alignment point index
+        for alignment_p in range(len(alignments[alignmentIdx])-1):
+            #Get closest point on alignment
+            closestPoint = ppldb2p(p.x,p.y,alignments[alignmentIdx][alignment_p][0],alignments[alignmentIdx][alignment_p][1],alignments[alignmentIdx][alignment_p+1][0],alignments[alignmentIdx][alignment_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(splineIdx, spline_p))
+                print('Error: Alignment {0}, segment {1} has identical bounds and therefore is not a vector. Projection cannot continue.'.format(alignmentIdx, alignment_p))
                 return None
             # 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): 
+            if utils.inBetween(alignments[alignmentIdx][alignment_p][0], alignments[alignmentIdx][alignment_p+1][0], closestPoint.x) and utils.inBetween(alignments[alignmentIdx][alignment_p][1], alignments[alignmentIdx][alignment_p+1][1], closestPoint.y): 
                 offsetY = Point.distanceNorm2(closestPoint, p)
                 if offsetY < minOffsetY:
                     minOffsetY = offsetY
-                    snappedSplineIdx = splineIdx
-                    snappedSplineLeadingPoint = spline_p
+                    snappedAlignmentIdx = alignmentIdx
+                    snappedAlignmentLeadingPoint = alignment_p
                     snappedPoint = Point(closestPoint.x, closestPoint.y)
                 #Jump loop if significantly close
-                if offsetY < goodEnoughSplineDistance: 
+                if offsetY < goodEnoughAlignmentDistance: 
                     break
 
     #Get sub-segment distance
     if minOffsetY != float('inf'):
-        subsegmentDistance = Point.distanceNorm2(snappedPoint, splines[snappedSplineIdx][snappedSplineLeadingPoint])
+        subsegmentDistance = Point.distanceNorm2(snappedPoint, alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint])
         #Get cumulative alignment distance (total segment distance)
-        splineDistanceS = splines[snappedSplineIdx].getCumulativeDistance(snappedSplineLeadingPoint) + subsegmentDistance
-        orthogonalSplineVector = (splines[snappedSplineIdx][snappedSplineLeadingPoint+1]-splines[snappedSplineIdx][snappedSplineLeadingPoint]).orthogonal()
+        alignmentDistanceS = alignments[snappedAlignmentIdx].getCumulativeDistance(snappedAlignmentLeadingPoint) + subsegmentDistance
+        orthogonalAlignmentVector = (alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint+1]-alignments[snappedAlignmentIdx][snappedAlignmentLeadingPoint]).orthogonal()
         offsetVector = p-snappedPoint
-        if Point.dot(orthogonalSplineVector, offsetVector) < 0:
+        if Point.dot(orthogonalAlignmentVector, offsetVector) < 0:
             minOffsetY = -minOffsetY
-        return [snappedSplineIdx, snappedSplineLeadingPoint, snappedPoint, subsegmentDistance, splineDistanceS, minOffsetY]
+        return [snappedAlignmentIdx, snappedAlignmentLeadingPoint, snappedPoint, subsegmentDistance, alignmentDistanceS, minOffsetY]
     else:
-        print('Offset for point {} is infinite (check with prepareSplines if some spline segments are aligned with axes)'.format(p))
+        print('Offset for point {} is infinite (check with prepareAlignments if some alignment segments are aligned with axes)'.format(p))
         return None
 
-def getXYfromSY(s, y, splineNum, splines, mode = 0):
+def getXYfromSY(s, y, alignmentNum, alignments):
     ''' Find X,Y coordinate from S,Y data. 
     if mode = 0 : return Snapped X,Y
     if mode !=0 : return Real X,Y
-    ''' 
-    
-    #(buckle in, it gets ugly from here on out)
-    ss_spline_d = subsec_spline_dist(splines)
+    '''    
+    alignment = alignments[alignmentNum]
+    i = 1
+    while s > alignment.getCumulativeDistance(i) and i < len(alignment):
+        i += 1
+    if i < len(alignment):
+        d = s - alignment.getCumulativeDistance(i-1) # distance on subsegment
+        #Get difference vector and then snap
+        dv = alignment[i] - alignment[i-1]
+        magnitude  = dv.norm2()
+        normalizedV = dv.divide(magnitude)
+        #snapped = alignment[i-1] + normalizedV*d # snapped point coordinate along alignment
+        # add offset finally
+        orthoNormalizedV = normalizedV.orthogonal()
+        return alignment[i-1] + normalizedV*d + orthoNormalizedV*y
+    else:
+        print('Curvilinear point {} is past the end of the alignement'.format((s, y, alignmentNum)))
+        return None
+
     
-    #Find subsegment
-    snapped_x = None
-    snapped_y = None
-    for spline_ss_index in range(len(ss_spline_d[splineNum][1])):
-        if(s < ss_spline_d[splineNum][1][spline_ss_index]):
-            ss_value = s - ss_spline_d[splineNum][1][spline_ss_index-1]
-            #Get normal vector and then snap
-            vector_l_x = (splines[splineNum][spline_ss_index][0] - splines[splineNum][spline_ss_index-1][0])
-            vector_l_y = (splines[splineNum][spline_ss_index][1] - splines[splineNum][spline_ss_index-1][1])
-            magnitude  = sqrt(vector_l_x**2 + vector_l_y**2)
-            n_vector_x = vector_l_x/magnitude
-            n_vector_y = vector_l_y/magnitude
-            snapped_x  = splines[splineNum][spline_ss_index-1][0] + ss_value*n_vector_x
-            snapped_y  = splines[splineNum][spline_ss_index-1][1] + ss_value*n_vector_y
-
-            #Real values (including orthogonal projection of y))
-            real_x = snapped_x - y*n_vector_y 
-            real_y = snapped_y + y*n_vector_x            
-            break
-    
-    if mode == 0 or (not snapped_x):
-        if(not snapped_x):
-            snapped_x = splines[splineNum][-1][0]
-            snapped_y = splines[splineNum][-1][1]                
-        return [snapped_x,snapped_y]
-    else:
-        return [real_x,real_y]
-
-
 class NormAngle(object):
     '''Alternate encoding of a point, by its norm and orientation'''
 
@@ -695,7 +654,7 @@
         for i in range(nPoints-1):
             p0 += v
             t.addPosition(p0)
-        return t, Trajectory([[v.x]*nPoints, [v.y]*nPoints])
+        return t
 
     @staticmethod
     def load(line1, line2):
@@ -1090,6 +1049,55 @@
         lanes = [lane]*nPoints
         return CurvilinearTrajectory(S, Y, lanes)
 
+    @staticmethod
+    def fromTrajectoryProjection(t, alignments, halfWidth = 3):
+        ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
+            (curvilinearPositions instance) which holds information about the
+            curvilinear coordinates using alignment metadata.
+            From Paul St-Aubin's PVA tools
+            ======
+
+            Input:
+            ======
+            alignments   = a list of alignments, where each alignment is a list of
+                           points (class Point).
+            halfWidth = moving average window (in points) in which to smooth
+                           lane changes. As per tools_math.cat_mvgavg(), this term
+                           is a search *radius* around the center of the window.
+
+            '''
+        curvilinearPositions = CurvilinearTrajectory()
+
+        #For each point
+        for i in range(int(t.length())):
+            result = getSYfromXY(t[i], alignments)
+
+            # Error handling
+            if(result is None):
+                print('Warning: trajectory at point {} {} has alignment errors (alignment snapping)\nCurvilinear trajectory could not be computed'.format(i, t[i]))
+            else:
+                [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
+                curvilinearPositions.addPositionSYL(S, Y, align)
+
+        ## Go back through points and correct lane  
+        #Run through objects looking for outlier point
+        smoothed_lanes = utils.filterCategoricalMovingWindow(curvilinearPositions.getLanes(), halfWidth)
+        ## Recalculate projected point to new lane
+        lanes = curvilinearPositions.getLanes()
+        if(lanes != smoothed_lanes):
+            for i in range(len(lanes)):
+                if(lanes[i] != smoothed_lanes[i]):
+                    result = getSYfromXY(t[i],[alignments[smoothed_lanes[i]]])
+
+                    # Error handling
+                    if(result is None):
+                        ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
+                        print('    Warning: trajectory at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(i, t[i]))
+                    else:
+                        [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
+                        curvilinearPositions.setPosition(i, S, Y, align)
+        return curvilinearPositions
+
     def __getitem__(self,i): 
         if isinstance(i, int):
             return [self.positions[0][i], self.positions[1][i], self.lanes[i]]
@@ -1220,8 +1228,9 @@
 
     @staticmethod
     def generate(num, p, v, timeInterval):
-        positions, velocities = Trajectory.generate(p, v, int(timeInterval.length())) 
-        return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = velocities)
+        nPoints = int(timeInterval.length())
+        positions = Trajectory.generate(p, v, nPoints)
+        return MovingObject(num = num, timeInterval = timeInterval, positions = positions, velocities = Trajectory([[v.x]*nPoints, [v.y]*nPoints]))
 
     def updatePositions(self):
         inter, self.positions, self.velocities = MovingObject.aggregateTrajectories(self.features, self.getTimeInterval())
@@ -1608,53 +1617,8 @@
         at constant speed'''
         return predictPositionNoLimit(nTimeSteps, self.getPositionAtInstant(instant), self.getVelocityAtInstant(instant), externalAcceleration)
 
-    def projectCurvilinear(self, alignments, ln_mv_av_win=3):
-        ''' Add, for every object position, the class 'moving.CurvilinearTrajectory()'
-            (curvilinearPositions instance) which holds information about the
-            curvilinear coordinates using alignment metadata.
-            From Paul St-Aubin's PVA tools
-            ======
-
-            Input:
-            ======
-            alignments   = a list of alignments, where each alignment is a list of
-                           points (class Point).
-            ln_mv_av_win = moving average window (in points) in which to smooth
-                           lane changes. As per tools_math.cat_mvgavg(), this term
-                           is a search *radius* around the center of the window.
-
-            '''
-
-        self.curvilinearPositions = CurvilinearTrajectory()
-
-        #For each point
-        for i in range(int(self.length())):
-            result = getSYfromXY(self.getPositionAt(i), alignments)
-
-            # Error handling
-            if(result is None):
-                print('Warning: trajectory {} at point {} {} has alignment errors (spline snapping)\nCurvilinear trajectory could not be computed'.format(self.getNum(), i, self.getPositionAt(i)))
-            else:
-                [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
-                self.curvilinearPositions.addPositionSYL(S, Y, align)
-
-        ## Go back through points and correct lane  
-        #Run through objects looking for outlier point
-        smoothed_lanes = utils.cat_mvgavg(self.curvilinearPositions.getLanes(),ln_mv_av_win)
-        ## Recalculate projected point to new lane
-        lanes = self.curvilinearPositions.getLanes()
-        if(lanes != smoothed_lanes):
-            for i in range(len(lanes)):
-                if(lanes[i] != smoothed_lanes[i]):
-                    result = getSYfromXY(self.getPositionAt(i),[alignments[smoothed_lanes[i]]])
-
-                    # Error handling
-                    if(result is None):
-                        ## This can be triggered by tracking errors when the trajectory jumps around passed another alignment.
-                        print('    Warning: trajectory {} at point {} {} has alignment errors during trajectory smoothing and will not be corrected.'.format(self.getNum(), i, self.getPositionAt(i)))
-                    else:
-                        [align, alignPoint, snappedPoint, subsegmentDistance, S, Y] = result
-                        self.curvilinearPositions.setPosition(i, S, Y, align)
+    def projectCurvilinear(self, alignments, halfWidth = 3):
+        self.curvilinearPositions = CurvilinearTrajectory.fromTrajectoryProjection(self.getPositions(), alignments, halfWidth)
 
     def computeSmoothTrajectory(self, minCommonIntervalLength):
         '''Computes the trajectory as the mean of all features