From 64bac76e29e5fb421961267845e1437223ba9014 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sun, 22 Jan 2023 16:58:23 +0100
Subject: [PATCH] add more scheduler unittests

---
 satrs-core/src/pool.rs           |  4 +-
 satrs-core/src/pus/scheduling.rs | 63 +++++++++++++++++++++++++++++---
 spacepackets                     |  2 +-
 3 files changed, 60 insertions(+), 9 deletions(-)

diff --git a/satrs-core/src/pool.rs b/satrs-core/src/pool.rs
index a37e406..c04ccfd 100644
--- a/satrs-core/src/pool.rs
+++ b/satrs-core/src/pool.rs
@@ -131,8 +131,8 @@ pub struct LocalPool {
 /// Simple address type used for transactions with the local pool.
 #[derive(Debug, Copy, Clone, PartialEq, Eq)]
 pub struct StoreAddr {
-    pool_idx: u16,
-    packet_idx: NumBlocks,
+    pub(crate) pool_idx: u16,
+    pub(crate) packet_idx: NumBlocks,
 }
 
 impl StoreAddr {
diff --git a/satrs-core/src/pus/scheduling.rs b/satrs-core/src/pus/scheduling.rs
index eae0690..98bb1bd 100644
--- a/satrs-core/src/pus/scheduling.rs
+++ b/satrs-core/src/pus/scheduling.rs
@@ -1,8 +1,9 @@
 use crate::pool::StoreAddr;
-use alloc::collections::btree_map::Range;
+use alloc::collections::btree_map::{Entry, Range};
 use core::time::Duration;
 use spacepackets::time::UnixTimestamp;
 use std::collections::BTreeMap;
+use std::time::SystemTimeError;
 use std::vec;
 use std::vec::Vec;
 
@@ -24,6 +25,14 @@ impl PusScheduler {
         }
     }
 
+    pub fn num_scheduled_telecommands(&self) -> u64 {
+        let mut num_entries = 0;
+        for entries in &self.tc_map {
+            num_entries += entries.1.len();
+        }
+        num_entries.into()
+    }
+
     pub fn is_enabled(&self) -> bool {
         self.enabled
     }
@@ -45,16 +54,18 @@ impl PusScheduler {
         self.current_time = current_time;
     }
 
+    pub fn current_time(&self) -> &UnixTimestamp {
+        &self.current_time
+    }
+
     pub fn insert_tc(&mut self, time_stamp: UnixTimestamp, addr: StoreAddr) -> bool {
         if time_stamp > self.current_time + self.time_margin {
             return false;
         }
-        if self.tc_map.contains_key(&time_stamp) {
-            let tc_vec = self.tc_map.get_mut(&time_stamp).unwrap();
-            tc_vec.push(addr);
-            return true;
+        match self.tc_map.entry(time_stamp) {
+            Entry::Vacant(e) => e.insert(vec![addr]),
+            Entry::Occupied(mut v) => v.get_mut().push(addr),
         }
-        self.tc_map.insert(time_stamp, vec![addr]);
         true
     }
 
@@ -62,6 +73,13 @@ impl PusScheduler {
         self.tc_map.range(..=self.current_time)
     }
 
+    #[cfg(feature = "std")]
+    #[cfg_attr(doc_cfg, doc(cfg(feature = "std")))]
+    pub fn update_time_from_now(&mut self) -> Result<(), SystemTimeError> {
+        self.current_time = UnixTimestamp::from_now()?;
+        Ok(())
+    }
+
     pub fn release_telecommands<R: FnMut(bool, &StoreAddr)>(&mut self, mut releaser: R) {
         let tcs_to_release = self.telecommands_to_release();
         for tc in tcs_to_release {
@@ -75,6 +93,7 @@ impl PusScheduler {
 
 #[cfg(test)]
 mod tests {
+    use crate::pool::StoreAddr;
     use crate::pus::scheduling::PusScheduler;
     use spacepackets::time::UnixTimestamp;
     use std::time::Duration;
@@ -87,4 +106,36 @@ mod tests {
         scheduler.disable();
         assert!(!scheduler.is_enabled());
     }
+
+    #[test]
+    fn reset() {
+        let mut scheduler =
+            PusScheduler::new(UnixTimestamp::new_only_seconds(0), Duration::from_secs(5));
+        scheduler.insert_tc(
+            UnixTimestamp::new_only_seconds(200),
+            StoreAddr {
+                pool_idx: 0,
+                packet_idx: 1,
+            },
+        );
+        scheduler.insert_tc(
+            UnixTimestamp::new_only_seconds(200),
+            StoreAddr {
+                pool_idx: 0,
+                packet_idx: 2,
+            },
+        );
+        scheduler.insert_tc(
+            UnixTimestamp::new_only_seconds(300),
+            StoreAddr {
+                pool_idx: 0,
+                packet_idx: 2,
+            },
+        );
+        assert_eq!(scheduler.num_scheduled_telecommands(), 3);
+        assert!(scheduler.is_enabled());
+        scheduler.reset();
+        assert!(!scheduler.is_enabled());
+        assert_eq!(scheduler.num_scheduled_telecommands(), 0);
+    }
 }
diff --git a/spacepackets b/spacepackets
index fc76a97..b55fe9f 160000
--- a/spacepackets
+++ b/spacepackets
@@ -1 +1 @@
-Subproject commit fc76a975c11c54697a30471ba0e921965af4de12
+Subproject commit b55fe9f443f9e10b219e7f09a377d9968c1b4ae5