spat: Add screen

src/applogic/mod.rs

@@ -5,15 +5,13 @@ use alloc::rc::Rc;
 #[cfg(feature = "spat")]
 use alloc::vec::Vec;
 
-#[cfg(all(target_arch = "riscv32", feature = "spat"))]
-use esp_println::println;
 #[cfg(feature = "spat")]
 use log::{debug, error};
 #[cfg(feature = "gnss")]
 use log::{info, warn};
 
 #[cfg(feature = "spat")]
-use crate::geo_alg;
+use crate::{applogic::v2x::map, geo_alg};
 
 pub mod v2x;
 
@@ -63,6 +61,54 @@ pub struct State {
     glosa_state: GlosaState,
 }
 
+#[cfg(feature = "spat")]
+#[derive(Debug, Default)]
+pub struct SignalGroupData {
+    pub phase: SignalPhase,
+    pub maneuver: map::Maneuvers,
+    pub min_end_sec: i16,
+}
+
+#[cfg(feature = "spat")]
+#[derive(Debug, Default)]
+pub enum SignalPhase {
+    #[default]
+    Unknown,
+    Red,
+    RedYellow,
+    Green,
+    Yellow,
+}
+
+impl core::fmt::Display for SignalPhase {
+    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
+        match self {
+            SignalPhase::Unknown => write!(f, "__"),
+            SignalPhase::Red => write!(f, "RE"),
+            SignalPhase::RedYellow => write!(f, "RY"),
+            SignalPhase::Green => write!(f, "GN"),
+            SignalPhase::Yellow => write!(f, "YE"),
+        }
+    }
+}
+
+impl From<c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState> for SignalPhase {
+    fn from(value: c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState) -> Self {
+        use c_its_parser::standards::dsrc_2_2_1::etsi_its_dsrc::MovementPhaseState;
+
+        match value {
+            MovementPhaseState::stop_And_Remain => SignalPhase::Red,
+            MovementPhaseState::pre_Movement => SignalPhase::RedYellow,
+            MovementPhaseState::permissive_Movement_Allowed
+            | MovementPhaseState::protected_Movement_Allowed => SignalPhase::Green,
+            MovementPhaseState::permissive_clearance | MovementPhaseState::protected_clearance => {
+                SignalPhase::Yellow
+            }
+            _ => SignalPhase::Unknown,
+        }
+    }
+}
+
 #[cfg(feature = "spat")]
 #[derive(Debug, Default)]
 enum GlosaState {
@@ -172,12 +218,12 @@ impl State {
     }
 
     #[cfg(feature = "spat")]
-    pub fn run_glosa(&mut self) {
+    pub fn run_glosa(&mut self) -> Vec<SignalGroupData> {
         let Some(own_heading) = self.heading_deg else {
-            return;
+            return alloc::vec![];
         };
 
-        let state_update = match &self.glosa_state {
+        let (state_update, sig_grps) = match &self.glosa_state {
             // search for upcoming intersection
             GlosaState::Idle => {
                 debug!(
@@ -193,9 +239,9 @@ impl State {
                         search_result.signal_groups
                     );
 
-                    Some(GlosaState::Locked(search_result))
+                    (Some(GlosaState::Locked(search_result)), None)
                 } else {
-                    None
+                    (None, None)
                 }
             }
             GlosaState::Locked(info) => {
@@ -213,7 +259,7 @@ impl State {
                     &self.position,
                 ) {
                     info!("GLOSA: Intersection was left -> back to idle");
-                    Some(GlosaState::Idle)
+                    (Some(GlosaState::Idle), None)
                 } else {
                     // get signal groups which are in our approach
                     let mut approach_sig_grps = intersection
@@ -231,22 +277,36 @@ impl State {
                     approach_sig_grps.sort_by_key(|v| v.maneuvers());
 
                     // display signal phases
-                    for sig in approach_sig_grps {
-                        if let Some(phase) = sig.phase_to_str()
-                            && let Some(manv) = sig.maneuvers()
-                        {
-                            use core::ops::Sub;
+                    let sig_grps = approach_sig_grps
+                        .iter()
+                        .filter_map(|sig| {
+                            if let Some(manv) = sig.maneuvers() {
+                                use core::ops::Sub;
 
-                            let min_end = sig.min_end_time();
-                            let dur = min_end.sub(self.time.and_utc()).as_seconds_f32();
-                            println!(
-                                "GLOSA: {manv} {phase} for {dur:.1} s (until {})",
-                                sig.min_end_time()
-                            );
-                        }
-                    }
+                                use num_traits::Float;
 
-                    None // keep state
+                                let min_end = sig.min_end_time();
+                                let dur = min_end.sub(self.time.and_utc()).as_seconds_f32();
+
+                                let phase = sig
+                                    .phase()
+                                    .map(core::convert::Into::into)
+                                    .unwrap_or_default();
+                                #[allow(clippy::cast_possible_truncation)]
+                                let min_end_sec = dur.round() as i16;
+
+                                Some(SignalGroupData {
+                                    phase,
+                                    maneuver: manv,
+                                    min_end_sec,
+                                })
+                            } else {
+                                None
+                            }
+                        })
+                        .collect::<Vec<_>>();
+
+                    (None, Some(sig_grps)) // keep state, return signal groups
                 }
             }
         };
@@ -255,6 +315,8 @@ impl State {
         if let Some(new) = state_update {
             self.glosa_state = new;
         }
+
+        sig_grps.unwrap_or_default()
     }
 
     #[cfg(feature = "spat")]