feat(core): Add casting and correct subtraction/cmp

argus-core/src/signals/utils.rs

@@ -6,6 +6,7 @@
 
 use core::ops::{Bound, RangeBounds};
 use core::time::Duration;
+use std::cmp::Ordering;
 
 use num_traits::NumCast;
 
@@ -30,7 +31,7 @@ pub struct Neighborhood<T: ?Sized + Copy> {
 /// lines.
 pub fn find_intersection<T>(a: &Neighborhood<T>, b: &Neighborhood<T>) -> Sample<T>
 where
-    T: Copy + std::fmt::Debug + NumCast,
+    T: Copy + NumCast,
 {
     // https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection#Given_two_points_on_each_line
     use num_traits::cast;
@@ -60,6 +61,54 @@ where
     Sample { time: t, value: y }
 }
 
+/// Augment synchronization points with time points where signals intersect
+pub fn sync_with_intersection<'a, T, Sig1, Sig2>(sig1: &'a Sig1, sig2: &'a Sig2) -> Option<Vec<Duration>>
+where
+    T: PartialOrd + Copy + NumCast + LinearInterpolatable,
+    Sig1: BaseSignal<Value = T> + SignalSyncPoints<Sig2>,
+    Sig2: BaseSignal<Value = T> + SignalSyncPoints<Sig1>,
+{
+    use Ordering::*;
+    let sync_points: Vec<&Duration> = sig1.synchronization_points(sig2)?.into_iter().collect();
+    // This will contain the new signal with an initial capacity of twice the input
+    // signals sample points (as that is the upper limit of the number of new points
+    // that will be added
+    let mut return_points = Vec::<Duration>::with_capacity(sync_points.len() * 2);
+    // this will contain the last sample point and ordering
+    let mut last_sample = None;
+    // We will now loop over the sync points, compare across signals and (if
+    // an intersection happens) we will have to compute the intersection point
+    for t in sync_points {
+        let lhs = sig1.at(*t).expect("value must be present at given time");
+        let rhs = sig2.at(*t).expect("values must be present at given time");
+        let ord = lhs.partial_cmp(rhs).unwrap();
+
+        // We will check for any intersections between the current sample and the
+        // previous one before we push the current sample time
+        if let Some((tm1, last)) = last_sample {
+            // Check if the signals crossed, this will happen essentially if the last
+            // and the current are opposites and were not Equal.
+            if let (Less, Greater) | (Greater, Less) = (last, ord) {
+                // Find the point of intersection between the points.
+                let a = Neighborhood {
+                    first: sig1.at(tm1).copied().map(|value| Sample { time: tm1, value }),
+                    second: sig1.at(*t).copied().map(|value| Sample { time: *t, value }),
+                };
+                let b = Neighborhood {
+                    first: sig2.at(tm1).copied().map(|value| Sample { time: tm1, value }),
+                    second: sig2.at(*t).copied().map(|value| Sample { time: *t, value }),
+                };
+                let intersect = find_intersection(&a, &b);
+                return_points.push(intersect.time);
+            }
+        }
+        return_points.push(*t);
+        last_sample = Some((*t, ord));
+    }
+    return_points.shrink_to_fit();
+    Some(return_points)
+}
+
 pub fn apply1<T, F>(signal: &Signal<T>, op: F) -> Signal<T>
 where
     T: Copy,