From ce6f804eed5dee86000b57d4a3188a8119ce07c9 Mon Sep 17 00:00:00 2001
From: Robin Mueller <robin.mueller.m@gmail.com>
Date: Sat, 21 Jan 2023 14:29:40 +0100
Subject: [PATCH] first basic scheduler impl

---
 Cargo.lock                       |  4 ++--
 satrs-core/Cargo.toml            |  4 ++--
 satrs-core/src/pus/scheduling.rs | 41 ++++++++++++++++++++++++++++----
 spacepackets                     |  2 +-
 4 files changed, 41 insertions(+), 10 deletions(-)

diff --git a/Cargo.lock b/Cargo.lock
index c16a4e9..254eec3 100644
--- a/Cargo.lock
+++ b/Cargo.lock
@@ -568,7 +568,7 @@ dependencies = [
  "postcard",
  "serde",
  "serde_json",
- "spacepackets 0.5.0 (git+https://egit.irs.uni-stuttgart.de/rust/spacepackets.git?rev=74e489bd074)",
+ "spacepackets 0.5.0 (git+https://egit.irs.uni-stuttgart.de/rust/spacepackets.git?rev=fc76a975c11c54697a30471ba0e921965af4de12)",
  "zerocopy",
 ]
 
@@ -698,7 +698,7 @@ dependencies = [
 [[package]]
 name = "spacepackets"
 version = "0.5.0"
-source = "git+https://egit.irs.uni-stuttgart.de/rust/spacepackets.git?rev=74e489bd074#74e489bd074bbd8139a1c8740aa1670a2ef9b671"
+source = "git+https://egit.irs.uni-stuttgart.de/rust/spacepackets.git?rev=fc76a975c11c54697a30471ba0e921965af4de12#fc76a975c11c54697a30471ba0e921965af4de12"
 dependencies = [
  "chrono",
  "crc",
diff --git a/satrs-core/Cargo.toml b/satrs-core/Cargo.toml
index 88bd2c4..13adac9 100644
--- a/satrs-core/Cargo.toml
+++ b/satrs-core/Cargo.toml
@@ -53,10 +53,10 @@ default-features = false
 optional = true
 
 [dependencies.spacepackets]
-git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git"
 # path = "../spacepackets"
 # version = "0.4.0"
-rev = "74e489bd074"
+git = "https://egit.irs.uni-stuttgart.de/rust/spacepackets.git"
+rev = "fc76a975c11c54697a30471ba0e921965af4de12"
 
 default-features = false
 
diff --git a/satrs-core/src/pus/scheduling.rs b/satrs-core/src/pus/scheduling.rs
index 7e1f1dc..eae0690 100644
--- a/satrs-core/src/pus/scheduling.rs
+++ b/satrs-core/src/pus/scheduling.rs
@@ -1,19 +1,25 @@
 use crate::pool::StoreAddr;
+use alloc::collections::btree_map::Range;
+use core::time::Duration;
 use spacepackets::time::UnixTimestamp;
 use std::collections::BTreeMap;
+use std::vec;
+use std::vec::Vec;
 
 #[derive(Debug)]
 pub struct PusScheduler {
-    tc_map: BTreeMap<UnixTimestamp, StoreAddr>,
+    tc_map: BTreeMap<UnixTimestamp, Vec<StoreAddr>>,
     current_time: UnixTimestamp,
+    time_margin: Duration,
     enabled: bool,
 }
 
 impl PusScheduler {
-    pub fn new(init_current_time: UnixTimestamp) -> Self {
+    pub fn new(init_current_time: UnixTimestamp, time_margin: Duration) -> Self {
         PusScheduler {
             tc_map: Default::default(),
             current_time: init_current_time,
+            time_margin,
             enabled: true,
         }
     }
@@ -39,8 +45,31 @@ impl PusScheduler {
         self.current_time = current_time;
     }
 
-    pub fn insert_tc(&mut self, time_stamp: UnixTimestamp, addr: StoreAddr) {
-        self.tc_map.insert(time_stamp, addr);
+    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;
+        }
+        self.tc_map.insert(time_stamp, vec![addr]);
+        true
+    }
+
+    pub fn telecommands_to_release(&self) -> Range<'_, UnixTimestamp, Vec<StoreAddr>> {
+        self.tc_map.range(..=self.current_time)
+    }
+
+    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 {
+            for addr in tc.1 {
+                releaser(self.enabled, addr);
+            }
+        }
+        self.tc_map.retain(|k, _| k > &self.current_time);
     }
 }
 
@@ -48,10 +77,12 @@ impl PusScheduler {
 mod tests {
     use crate::pus::scheduling::PusScheduler;
     use spacepackets::time::UnixTimestamp;
+    use std::time::Duration;
 
     #[test]
     fn basic() {
-        let mut scheduler = PusScheduler::new(UnixTimestamp::new_only_seconds(0));
+        let mut scheduler =
+            PusScheduler::new(UnixTimestamp::new_only_seconds(0), Duration::from_secs(5));
         assert!(scheduler.is_enabled());
         scheduler.disable();
         assert!(!scheduler.is_enabled());
diff --git a/spacepackets b/spacepackets
index 74e489b..fc76a97 160000
--- a/spacepackets
+++ b/spacepackets
@@ -1 +1 @@
-Subproject commit 74e489bd074bbd8139a1c8740aa1670a2ef9b671
+Subproject commit fc76a975c11c54697a30471ba0e921965af4de12