From cc31d6226e829c61ce998ba447e9c2081987a87a Mon Sep 17 00:00:00 2001
From: Jannik Beyerstedt <code@jbeyerstedt.de>
Date: Mon, 22 Jun 2026 22:54:37 +0200
Subject: [PATCH] spat: ensure that "our" intersection is kept in cache

Sometimes we may not receive V2X data for longer than the TLC cache time.
This leads to no SPAT updates in the GLOSA algorithm since that uses a cached reference to the intersection instead of querying the tlc cache every time.
---
 src/applogic/mod.rs | 102 ++++++++++++++++++++++++--------------------
 src/applogic/tlc.rs |  11 ++++-
 2 files changed, 66 insertions(+), 47 deletions(-)

diff --git a/src/applogic/mod.rs b/src/applogic/mod.rs
index 120864f..18d2726 100644
--- a/src/applogic/mod.rs
+++ b/src/applogic/mod.rs
@@ -255,48 +255,57 @@ impl State {
                     info.intersection_id, info.lane_id
                 );
 
-                // determine if intersection is left
-                let intersection = info.intersection.borrow();
-                if Self::is_intersection_left(
-                    &intersection,
-                    info.lane_id,
-                    info.approach_id,
-                    &self.position,
-                ) {
-                    info!("GLOSA: Intersection was left -> back to idle");
+                // ensure that "our" intersection is kept in TLC cache
+                if self
+                    .tlc_cache
+                    .keep_intersection(info.intersection_id, self.time)
+                    .is_err()
+                {
+                    info!("GLOSA: Intersection not known any more");
                     (Some(GlosaState::Idle), None)
                 } else {
-                    // get signal groups which are in our approach
-                    let mut approach_sig_grps = intersection
-                        .signal_groups()
-                        .iter()
-                        .filter(|i| {
-                            info.signal_groups
-                                .iter()
-                                .find(|sg_id| **sg_id == i.id())
-                                .is_some()
-                        })
-                        .collect::<Vec<_>>();
+                    // determine if intersection is left
+                    let intersection = info.intersection.borrow();
+                    if Self::is_intersection_left(
+                        &intersection,
+                        info.lane_id,
+                        info.approach_id,
+                        &self.position,
+                    ) {
+                        info!("GLOSA: Intersection was left -> back to idle");
+                        (Some(GlosaState::Idle), None)
+                    } else {
+                        // get signal groups which are in our approach
+                        let mut approach_sig_grps = intersection
+                            .signal_groups()
+                            .iter()
+                            .filter(|i| {
+                                info.signal_groups
+                                    .iter()
+                                    .find(|sg_id| **sg_id == i.id())
+                                    .is_some()
+                            })
+                            .collect::<Vec<_>>();
 
-                    // sort signal groups by maneuver
-                    approach_sig_grps.sort_by_key(|v| v.maneuvers());
+                        // sort signal groups by maneuver
+                        approach_sig_grps.sort_by_key(|v| v.maneuvers());
 
-                    // display signal phases
-                    let sig_grps = approach_sig_grps
-                        .iter()
-                        .filter_map(|sig| {
-                            if let Some(manv) = sig.maneuvers() {
-                                use core::ops::Sub;
+                        // display signal phases
+                        let sig_grps = approach_sig_grps
+                            .iter()
+                            .filter_map(|sig| {
+                                if let Some(manv) = sig.maneuvers() {
+                                    use core::ops::Sub;
 
-                                use num_traits::Float;
+                                    use num_traits::Float;
 
-                                let phase = sig
-                                    .phase()
-                                    .map(core::convert::Into::into)
-                                    .unwrap_or_default();
+                                    let phase = sig
+                                        .phase()
+                                        .map(core::convert::Into::into)
+                                        .unwrap_or_default();
 
-                                let end_sec =
-                                    sig.likely_time()
+                                    let end_sec = sig
+                                        .likely_time()
                                         .or_else(|| sig.min_end_time())
                                         .map(|time| {
                                             let dur =
@@ -308,18 +317,19 @@ impl State {
                                             end_sec
                                         });
 
-                                Some(SignalGroupData {
-                                    phase,
-                                    maneuver: manv,
-                                    end_sec,
-                                })
-                            } else {
-                                None
-                            }
-                        })
-                        .collect::<Vec<_>>();
+                                    Some(SignalGroupData {
+                                        phase,
+                                        maneuver: manv,
+                                        end_sec,
+                                    })
+                                } else {
+                                    None
+                                }
+                            })
+                            .collect::<Vec<_>>();
 
-                    (None, Some(sig_grps)) // keep state, return signal groups
+                        (None, Some(sig_grps)) // keep state, return signal groups
+                    }
                 }
             }
         };
diff --git a/src/applogic/tlc.rs b/src/applogic/tlc.rs
index 96cbd8a..d8671c8 100644
--- a/src/applogic/tlc.rs
+++ b/src/applogic/tlc.rs
@@ -10,7 +10,7 @@ use chrono::Datelike;
 use esp_println::println;
 
 use super::v2x::map;
-use crate::cache::Cache;
+use crate::cache::{Cache, CacheError};
 
 pub struct Data {
     intersections: Cache<u16, map::Intersection>,
@@ -86,6 +86,15 @@ impl Data {
         self.intersections.prune(time_now);
     }
 
+    /// Bumps the timestamp of a certain intersection to keep it cached
+    pub fn keep_intersection(
+        &mut self,
+        intersection_id: u16,
+        time_now: chrono::NaiveDateTime,
+    ) -> Result<(), CacheError<u16>> {
+        self.intersections.update(time_now, intersection_id, |_| {})
+    }
+
     pub fn handle_spatem(&mut self, time_now: chrono::NaiveDateTime, etsi: &SPATEM) {
         for int in &etsi.spat.intersections.0 {
             let int_id = int.id.id.0;