//! Single-owner MPU acquisition service.

use kaptan_drivers::mpu6000::{Error as Mpu6000Error, Mpu6000, Mpu6000Event};
use kaptan_sensor::ImuSnapshot;
use kaptan_spi::{SpiCompletion, SpiDevice};
use kaptan_time::MonotonicClock;

/// One observable result from the IMU worker.
#[derive(Debug)]
pub enum ImuEvent<BusError> {
    /// No accepted sample or terminal fault was produced.
    Idle,
    /// MPU configuration completed and the DRDY route may be enabled.
    Initialized,
    /// A coherent IMU observation is ready for telemetry or control.
    Sample(ImuSnapshot),
    /// The selected sensor path entered a terminal fault state.
    Fault(Mpu6000Error<BusError>),
}

/// Owns MPU state, its clock, and sample acceptance.
///
/// Exactly one worker invokes this service. Hardware ISRs only report deferred
/// DRDY, timer, and SPI-DMA terminal events to that worker.
pub struct ImuService<Clock, Bus> {
    clock: Clock,
    imu: Mpu6000<Bus>,
}

impl<Clock, Bus> ImuService<Clock, Bus> {
    /// Creates an inactive acquisition service.
    pub const fn new(clock: Clock, imu: Mpu6000<Bus>) -> Self {
        Self { clock, imu }
    }
}

impl<Clock, Bus> ImuService<Clock, Bus>
where
    Clock: MonotonicClock,
    Bus: SpiDevice,
{
    /// Submits the asynchronous sensor reset transaction.
    pub fn begin(&mut self) -> Result<(), Mpu6000Error<Bus::StartError>> {
        self.imu.begin_initialization()
    }

    /// Advances the reset-settling state on a board periodic event.
    pub fn on_periodic_tick(&mut self) -> ImuEvent<Bus::StartError> {
        let _ = self.clock.now();
        match self.imu.advance_initialization() {
            Ok(()) => ImuEvent::Idle,
            Err(error) => ImuEvent::Fault(error),
        }
    }

    /// Handles one coalesced data-ready event.
    pub fn on_data_ready(&mut self) -> ImuEvent<Bus::StartError> {
        if !self.imu.is_ready() {
            return ImuEvent::Idle;
        }

        match self.imu.request_sample(self.clock.now()) {
            Ok(()) | Err(Mpu6000Error::NotReady) => ImuEvent::Idle,
            Err(error) => ImuEvent::Fault(error),
        }
    }

    /// Advances the MPU transaction state from a DMA terminal result.
    pub fn on_transfer_complete(&mut self, completion: SpiCompletion) -> ImuEvent<Bus::StartError> {
        match self.imu.on_transfer_complete(completion) {
            Mpu6000Event::NotPending | Mpu6000Event::Initializing => ImuEvent::Idle,
            Mpu6000Event::Initialized => ImuEvent::Initialized,
            Mpu6000Event::Sample(sample) => ImuEvent::Sample(sample),
            Mpu6000Event::Fault(error) => ImuEvent::Fault(error),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::{ImuEvent, ImuService};
    use kaptan_drivers::mpu6000::{Mpu6000, Mpu6000Config};
    use kaptan_spi::{SpiCompletion, SpiDevice};
    use kaptan_time::{MonotonicClock, Timestamp};

    struct Clock;

    impl MonotonicClock for Clock {
        fn now(&mut self) -> Timestamp {
            Timestamp::from_microseconds(42)
        }
    }

    struct Bus {
        response: [u8; 15],
    }

    impl SpiDevice for Bus {
        type StartError = ();

        fn start_transfer(&mut self, _: &[u8], receive: &mut [u8]) -> Result<(), Self::StartError> {
            receive.copy_from_slice(&self.response[..receive.len()]);
            Ok(())
        }

        fn complete_transfer(&mut self, _: SpiCompletion) {}
    }

    #[test]
    fn accepts_a_sample_only_after_initialization() {
        let mut response = [0; 15];
        response[1] = 0x68;
        let mut service = ImuService::new(
            Clock,
            Mpu6000::new(Bus { response }, Mpu6000Config::default()),
        );

        service.begin().unwrap();
        assert!(matches!(service.on_data_ready(), ImuEvent::Idle));
        assert!(matches!(
            service.on_transfer_complete(SpiCompletion::Complete),
            ImuEvent::Idle
        ));
        assert!(matches!(service.on_periodic_tick(), ImuEvent::Idle));
        for _ in 0..9 {
            assert!(matches!(
                service.on_transfer_complete(SpiCompletion::Complete),
                ImuEvent::Idle
            ));
        }
        assert!(matches!(
            service.on_transfer_complete(SpiCompletion::Complete),
            ImuEvent::Initialized
        ));
        assert!(matches!(service.on_data_ready(), ImuEvent::Idle));
    }
}