use embassy_sync::{channel::DynamicReceiver, pubsub::DynPublisher};
use log::*;
use embassy_time::{Duration, WithTimeout};

use crate::{ego::engine::BikeStates, events::{Measurement, Prediction}};

const TIMEOUT: Duration = Duration::from_millis(3);

#[embassy_executor::task]
pub async fn motion_task(src: DynamicReceiver<'static, Measurement>, prediction_sink: DynPublisher<'static, Prediction>) {
    let mut states = BikeStates::default();

    loop {
        // TODO: Should be on a timeout, so we can correctly warn when we aren't receiving sensor events after some time
        let next_measurement = src.receive().await;
        trace!("measurement={next_measurement:?}");
        match next_measurement {
            Measurement::IMU { accel, gyro } => {
                states.insert_imu(accel, gyro);
                states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit IMU data in time");
            },
            Measurement::GPS(Some(gps_pos)) => {
                states.insert_gps(gps_pos);
                states.commit(&prediction_sink).with_timeout(TIMEOUT).await.expect("Could not commit GPS data in time");
            },
            Measurement::GPS(None) => {
                states.has_gps_fix.set(false);
            },
            // FIXME: This needs harmonized with the automatic data timeout from above, somehow?
            Measurement::SensorHardwareStatus(source, state) => {
                warn!("Sensor {source:?} reports {state:?}!");
                prediction_sink.publish(Prediction::SensorStatus(source, state)).with_timeout(TIMEOUT).await.expect("Could not update sensor status in time");
            },
            Measurement::SimulationProgress(source, duration, pct) => debug!("{source:?} simulation time: {} {} / 255", duration.as_secs(), pct),
            Measurement::Annotation => ()
        }
    }
}