Evaluate remaining red/green time for upcoming intersection

src/main.rs

@@ -23,65 +23,13 @@ use log::{debug, error, info, warn};
 extern crate alloc;
 
 mod applogic;
+#[cfg(feature = "spat")]
+mod cache;
+mod geo_alg;
 mod radio;
 
 const WIFI_CHANNEL: radio::Channel = 180;
 
-#[derive(Debug, Default)]
-struct State {
-    initialized: bool,
-    time: chrono::NaiveDateTime,
-
-    lat_deg: f32,
-    lon_deg: f32,
-    heading_deg: Option<f32>,
-    speed_mps: Option<f32>,
-}
-
-impl State {
-    fn update_with_gpsfix(&mut self, fix: &embassy_gps::types::GpsFix) {
-        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
-        let lat_deg = fix.latitude_deg as f32;
-        #[allow(clippy::cast_precision_loss, clippy::cast_possible_truncation)]
-        let lon_deg = fix.longitude_deg as f32;
-
-        self.lat_deg = lat_deg;
-        self.lon_deg = lon_deg;
-
-        if let Some(heading) = fix.true_course_deg {
-            self.heading_deg = Some(heading);
-        } else {
-            warn!("State: Course missing from GpsFix");
-        }
-
-        if let Some(speed) = fix.speed_over_ground {
-            self.speed_mps = Some(speed);
-        } else {
-            warn!("State: Speed missing from GpsFix");
-        }
-
-        if let Some(time) = fix.get_timestamp() {
-            self.time = time;
-
-            if !self.initialized {
-                info!("GNSS fix: {}", self.print_fix());
-            }
-            self.initialized = true;
-        }
-    }
-
-    fn print_fix(&self) -> alloc::string::String {
-        alloc::format!(
-            "{:?}, {:.4} N, {:.4} E, {:.0}°, {:.2} m/s",
-            self.time,
-            self.lat_deg,
-            self.lon_deg,
-            self.heading_deg.unwrap_or_default(),
-            self.speed_mps.unwrap_or_default(),
-        )
-    }
-}
-
 // This creates a default app-descriptor required by the esp-idf bootloader.
 // For more information see: <https://docs.espressif.com/projects/esp-idf/en/stable/esp32/api-reference/system/app_image_format.html#application-description>
 esp_bootloader_esp_idf::esp_app_desc!();
@@ -157,7 +105,7 @@ async fn main(spawner: Spawner) -> ! {
     );
     info!("GNSS task started, waiting for GNSS fix...");
 
-    let mut state = State::default();
+    let mut state = applogic::State::new();
     loop {
         embassy_time::Timer::after(embassy_time::Duration::from_millis(5)).await;
 
@@ -167,9 +115,12 @@ async fn main(spawner: Spawner) -> ! {
             if let Some(fix) = gnss_update_ref.replace(None) {
                 state.update_with_gpsfix(&fix);
 
-                if let Some(time) = fix.get_timestamp() {
-                    state.time = time;
-                    info!("{}", state.print_fix()); // TODO: degrate to debug later?
+                if fix.get_timestamp().is_some() {
+                    info!("{}", state.print_fix());
+
+                    // run GLOSA algorithm
+                    #[cfg(feature = "spat")]
+                    state.run_glosa();
                 }
             }
         });
@@ -193,15 +144,15 @@ async fn main(spawner: Spawner) -> ! {
                                 geonetworking: _,
                                 transport: _,
                                 etsi,
-                            } => applogic::tlc::handle_mapem(&etsi),
+                            } => state.handle_mapem(&etsi),
                             #[cfg(feature = "spat")]
                             ItsMessage::Spatem {
                                 geonetworking: _,
                                 transport: _,
                                 etsi,
                             } => {
-                                if state.initialized {
-                                    applogic::tlc::handle_spatem(&etsi, &state.time);
+                                if state.initialized() {
+                                    state.handle_spatem(&etsi);
                                 }
                             }
 
@@ -235,6 +186,9 @@ async fn main(spawner: Spawner) -> ! {
                 }
             }
         });
+
+        // maintenance
+        state.prune();
     }
 }