# HG changeset patch
# User Nicolas Saunier <nicolas.saunier@polymtl.ca>
# Date 1720471350 14400
# Node ID 8915747a4fc8750b0b3ffa3e944d7b6a3c9d5c61
# Parent  2abeccdbb985cab8b5ee1366ca06f2e63dec938e
adding a method to detect stationary objects (or sub time intervals)

diff -r 2abeccdbb985 -r 8915747a4fc8 trafficintelligence/moving.py
--- a/trafficintelligence/moving.py	Wed Jul 03 15:13:15 2024 -0400
+++ b/trafficintelligence/moving.py	Mon Jul 08 16:42:30 2024 -0400
@@ -4,7 +4,7 @@
 import copy
 from math import sqrt, atan2, cos, sin, inf
 
-from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, minimum,  issubdtype, integer as npinteger, percentile, full
+from numpy import median, mean, array, arange, zeros, ones, hypot, NaN, std, floor, ceil, float32, argwhere, flatnonzero, minimum,  issubdtype, integer as npinteger, percentile, quantile, full
 from matplotlib.pyplot import plot, text, arrow
 from scipy.spatial.distance import cdist
 from scipy.signal import savgol_filter
@@ -1835,6 +1835,33 @@
     def getYCoordinates(self):
         return self.positions.getYCoordinates()
 
+    def isStationary(self, speedThreshold, distanceThreshold):
+        '''Indicates if object is not moving
+        if speed on average below threshold and final-initial position close enough
+
+        TODO: returns time interval stationary if starts moving or stops moving'''
+        speeds = self.getSpeeds()
+        if quantile(speeds, 0.5) <= speedThreshold and Point.distanceNorm2(self.getPositionAt(0),self.getPositionAt(-1)) <= distanceThreshold:
+            return True, None
+        else:
+            indices = flatnonzero(speeds < speedThreshold).tolist()
+            if len(indices) >= 2:
+                i = 0
+                j = len(indices)-1
+                #incrementI = True
+                while i<j and (quantile(speeds[indices[i]:indices[j]+1], 0.5) > speedThreshold or Point.distanceNorm2(self.getPositionAt(indices[i]),self.getPositionAt(indices[j])) > distanceThreshold):
+                    #if incrementI:
+                    i+=1
+                    #else:
+                    j-=1
+                    #incrementI = not incrementI
+                if i<j: # we found a smaller subset
+                    return True, [indices[i], indices[j]]
+                else:
+                    return False, None
+            else:
+                return False, None
+        
     def setStationary(self):
         '''Resets the position to the mean of existing positions and sets speeds to 0
         And does the same to the features