From 9c3d987f071f02494c33ff5973632446d523ec5f Mon Sep 17 00:00:00 2001
From: Luca Della Vedova <lucadv@intrinsic.ai>
Date: Fri, 4 Aug 2023 11:26:52 +0800
Subject: [PATCH] Add comparison function for times, use reference to clock

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
---
 rclrs/src/clock.rs |  2 +-
 rclrs/src/time.rs  | 48 +++++++++++++++++++++++++++++++++++++---------
 2 files changed, 40 insertions(+), 10 deletions(-)

diff --git a/rclrs/src/clock.rs b/rclrs/src/clock.rs
index ea4ac4e7..3668516d 100644
--- a/rclrs/src/clock.rs
+++ b/rclrs/src/clock.rs
@@ -101,7 +101,7 @@ impl Clock {
         }
         Time {
             nsec: time_point,
-            clock_type: self._type,
+            clock: Arc::downgrade(&self._rcl_clock),
         }
     }
 
diff --git a/rclrs/src/time.rs b/rclrs/src/time.rs
index d5427470..14e1704d 100644
--- a/rclrs/src/time.rs
+++ b/rclrs/src/time.rs
@@ -1,20 +1,50 @@
-use crate::clock::ClockType;
 use crate::rcl_bindings::*;
+use std::sync::{Mutex, Weak};
 
 /// Struct that represents time.
 #[derive(Debug)]
 pub struct Time {
     /// Timestamp in nanoseconds.
     pub nsec: i64,
-    /// Clock type.
-    pub clock_type: ClockType,
+    /// Weak reference to the clock that generated this time
+    pub clock: Weak<Mutex<rcl_clock_t>>,
 }
 
-impl From<Time> for rcl_time_point_t {
-    fn from(time: Time) -> Self {
-        Self {
-            nanoseconds: time.nsec,
-            clock_type: time.clock_type.into(),
-        }
+impl Time {
+    /// Compares self to rhs, if they can be compared (originated from the same clock) calls f with
+    /// the values of the timestamps.
+    pub fn compare_with<U, F>(&self, rhs: &Time, f: F) -> Option<U>
+    where
+        F: FnOnce(i64, i64) -> U,
+    {
+        self.clock
+            .ptr_eq(&rhs.clock)
+            .then(|| f(self.nsec, rhs.nsec))
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use crate::clock::Clock;
+
+    #[test]
+    fn compare_times_from_same_clock() {
+        let clock = Clock::system().unwrap();
+        let t1 = clock.now();
+        std::thread::sleep(std::time::Duration::from_micros(1));
+        let t2 = clock.now();
+        assert_eq!(t1.compare_with(&t2, |t1, t2| t1 > t2), Some(false));
+        assert_eq!(t1.compare_with(&t2, |t1, t2| t2 > t1), Some(true));
+    }
+
+    #[test]
+    fn compare_times_from_different_clocks() {
+        // Times from different clocks, even if of the same type, can't be compared
+        let c1 = Clock::system().unwrap();
+        let c2 = Clock::system().unwrap();
+        let t1 = c1.now();
+        let t2 = c2.now();
+        assert!(t2.compare_with(&t1, |_, _| ()).is_none());
+        assert!(t1.compare_with(&t2, |_, _| ()).is_none());
     }
 }