kaptan/libs/kaptan-drivers/src/lidar_lite_v3/service.rs

//! Timestamped LIDAR-Lite v3 acquisition service.

use super::{AcquisitionMode, Error, LidarLiteV3, LidarLiteV3Event, StartupError};
use kaptan_i2c::{I2cBus, I2cCompletion};
use kaptan_sensor::range::RangeSnapshot;
use kaptan_time::MonotonicClock;

/// One observable result from a LIDAR-Lite v3 acquisition service.
#[derive(Debug)]
pub enum LidarLiteV3ServiceEvent<BusError> {
    /// No observation or terminal fault was produced.
    Idle,
    /// One coherent range observation is ready for publication.
    Measurement(RangeSnapshot),
    /// The LIDAR-Lite v3 path entered a terminal fault state.
    Fault(Error<BusError>),
}

/// Owns LIDAR-Lite v3 state, its clock, and measurement acceptance.
pub struct LidarLiteV3Service<Clock, Bus> {
    clock: Clock,
    lidar: LidarLiteV3<Bus>,
    mode: AcquisitionMode,
}

impl<Clock, Bus> LidarLiteV3Service<Clock, Bus> {
    /// Creates an inactive LIDAR-Lite v3 acquisition service.
    pub const fn new(clock: Clock, lidar: LidarLiteV3<Bus>, mode: AcquisitionMode) -> Self {
        Self { clock, lidar, mode }
    }

    /// Completes the LIDAR-Lite v3 startup interval selected by the board.
    pub fn finish_startup(&mut self) -> Result<(), StartupError> {
        self.lidar.finish_startup()
    }
}

impl<Clock: MonotonicClock, Bus: I2cBus> LidarLiteV3Service<Clock, Bus> {
    /// Starts a new measurement, or polls one already in progress.
    pub fn on_periodic_tick(&mut self) -> LidarLiteV3ServiceEvent<Bus::StartError> {
        let now = self.clock.now();
        let result = if self.lidar.is_ready() {
            self.lidar.request_measurement(now, self.mode)
        } else {
            self.lidar.poll_measurement(now)
        };
        match result {
            Ok(()) | Err(Error::NotReady) => LidarLiteV3ServiceEvent::Idle,
            Err(error) => LidarLiteV3ServiceEvent::Fault(error),
        }
    }

    /// Advances the LIDAR-Lite v3 transaction state from an I²C terminal event.
    pub fn on_transaction_complete(
        &mut self,
        completion: I2cCompletion,
    ) -> LidarLiteV3ServiceEvent<Bus::StartError> {
        match self.lidar.on_transaction_complete(completion) {
            LidarLiteV3Event::Measurement(sample) => LidarLiteV3ServiceEvent::Measurement(sample),
            LidarLiteV3Event::Fault(error) => LidarLiteV3ServiceEvent::Fault(error),
            _ => LidarLiteV3ServiceEvent::Idle,
        }
    }

    /// Acknowledges a board-controller interrupt and advances the acquisition.
    ///
    /// The closure keeps controller-specific interrupt handling at the board
    /// boundary while preserving this service as the sole owner of LIDAR state.
    pub fn on_controller_interrupt<F>(
        &mut self,
        acknowledge: F,
    ) -> LidarLiteV3ServiceEvent<Bus::StartError>
    where
        F: FnOnce(&mut Bus) -> I2cCompletion,
    {
        let completion = self.lidar.acknowledge_with(acknowledge);
        self.on_transaction_complete(completion)
    }
}

#[cfg(test)]
mod tests {
    use super::{LidarLiteV3Service, LidarLiteV3ServiceEvent};
    use crate::lidar_lite_v3::{AcquisitionMode, LidarLiteV3};
    use kaptan_i2c::{I2cBus, I2cCompletion};
    use kaptan_time::{MonotonicClock, Timestamp};

    struct Clock;

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

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

    impl I2cBus for Bus {
        type StartError = ();

        fn start_write(&mut self, _: u8, _: &[u8]) -> Result<(), Self::StartError> {
            Ok(())
        }

        fn start_read(&mut self, _: u8, _: usize) -> Result<(), Self::StartError> {
            Ok(())
        }

        fn complete_transaction(&mut self, completion: I2cCompletion, read: Option<&mut [u8]>) {
            if completion == I2cCompletion::Complete
                && let Some(read) = read
            {
                read.copy_from_slice(&self.response[..read.len()]);
            }
        }

        fn cancel_transaction(&mut self) {}
    }

    #[test]
    fn produces_a_measurement_after_stop_separated_i2c_transactions() {
        let mut service = LidarLiteV3Service::new(
            Clock,
            LidarLiteV3::new(Bus { response: [0; 2] }),
            AcquisitionMode::WithReceiverBiasCorrection,
        );
        service.finish_startup().unwrap();

        assert!(matches!(
            service.on_periodic_tick(),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_transaction_complete(I2cCompletion::Complete),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_periodic_tick(),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_controller_interrupt(|bus| {
                bus.response = [1 << 5, 0];
                I2cCompletion::Complete
            }),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_periodic_tick(),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_controller_interrupt(|_| I2cCompletion::Complete),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_periodic_tick(),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_controller_interrupt(|bus| {
                bus.response = [0x01, 0x23];
                I2cCompletion::Complete
            }),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_periodic_tick(),
            LidarLiteV3ServiceEvent::Idle
        ));
        assert!(matches!(
            service.on_controller_interrupt(|_| I2cCompletion::Complete),
            LidarLiteV3ServiceEvent::Measurement(sample)
                if sample.timestamp == Timestamp::from_microseconds(123)
                    && sample.distance_millimetres == 2_910
        ));
    }
}