//! ICM-42688-P register protocol and asynchronous sample driver.

mod error;
mod sample;
pub mod service;

use kaptan_sensor::imu::ImuSnapshot;
use kaptan_spi::{SpiCompletion, SpiDevice};
use kaptan_time::Timestamp;

pub use error::Error;
pub use service::{Icm42688Service, Icm42688ServiceEvent};

const WHO_AM_I: u8 = 0x75;
const EXPECTED_WHO_AM_I: u8 = 0x47;
const DEVICE_CONFIG: u8 = 0x11;
const SOFT_RESET: u8 = 0x01;
const PWR_MGMT0: u8 = 0x4e;
const GYRO_CONFIG0: u8 = 0x4f;
const ACCEL_CONFIG0: u8 = 0x50;
const GYRO_ACCEL_CONFIG0: u8 = 0x52;
const INT_CONFIG: u8 = 0x14;
const INT_CONFIG0: u8 = 0x63;
const INT_CONFIG1: u8 = 0x64;
const INT_SOURCE0: u8 = 0x65;
const INTF_CONFIG1: u8 = 0x4d;
const TEMP_DATA1: u8 = 0x1d;
const SAMPLE_BYTES: usize = 14;
const TRANSFER_CAPACITY: usize = SAMPLE_BYTES + 1;

// SPI mode 0, active-high push-pull, pulsed INT1. Data-ready is cleared by
// the SPI burst read and its source is explicitly enabled on INT1.
const CONFIGURATION: [(u8, u8); 9] = [
    (INT_CONFIG, 0x03),
    (INT_CONFIG0, 0x00),
    (INT_CONFIG1, 0x60),
    (INTF_CONFIG1, 0x40),
    (GYRO_ACCEL_CONFIG0, 0xff),
    (INT_SOURCE0, 0x08),
    (PWR_MGMT0, 0x0f),
    // ±500°/s and ±4 g at 1 kHz.  The two range fields use the ICM-42688-P
    // encoding in bits 7:5 and the output data rate is in bits 3:0.
    (GYRO_CONFIG0, 0x46),
    (ACCEL_CONFIG0, 0x46),
];

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum State {
    Dormant,
    Resetting,
    WaitingForReset,
    ReadingIdentity,
    WritingConfiguration(usize),
    WaitingForSensorStart,
    Ready,
    ReadingSample { timestamp: Timestamp },
}

/// Observable outcome of one ICM-42688-P SPI terminal event.
#[derive(Debug, Eq, PartialEq)]
pub enum Icm42688Event<BusError> {
    /// The selected SPI interrupt had no terminal transfer event.
    NotPending,
    /// One initialization transfer completed; initialization continues.
    Initializing,
    /// The ICM-42688-P identity and configuration sequence completed.
    Initialized,
    /// One coherent IMU sample completed.
    Sample(ImuSnapshot),
    /// Initialization or acquisition failed.
    Fault(Error<BusError>),
}

/// Configured ICM-42688-P driver.
///
/// The driver owns fixed transfer buffers and progresses exclusively from
/// timer, data-ready and SPI-DMA completion events. It never waits for a
/// peripheral status flag.
pub struct Icm42688<Bus> {
    bus: Bus,
    state: State,
    transmit: [u8; TRANSFER_CAPACITY],
    receive: [u8; TRANSFER_CAPACITY],
}

impl<Bus> Icm42688<Bus> {
    /// Creates an inactive ICM-42688-P state machine.
    pub const fn new(bus: Bus) -> Self {
        Self {
            bus,
            state: State::Dormant,
            transmit: [0; TRANSFER_CAPACITY],
            receive: [0; TRANSFER_CAPACITY],
        }
    }
}

impl<Bus: SpiDevice> Icm42688<Bus> {
    /// Starts the sensor reset. A subsequent periodic tick provides the
    /// datasheet-required reset-settling interval before identification.
    pub fn begin_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
        if self.state != State::Dormant {
            return Err(Error::NotReady);
        }
        self.prepare_write(DEVICE_CONFIG, SOFT_RESET);
        self.submit(2)?;
        self.state = State::Resetting;
        Ok(())
    }

    /// Advances reset/wake delays from the board periodic event.
    pub fn advance_initialization(&mut self) -> Result<(), Error<Bus::StartError>> {
        match self.state {
            State::WaitingForReset => {
                self.prepare_read(WHO_AM_I, 2);
                self.submit(2)?;
                self.state = State::ReadingIdentity;
            }
            State::WaitingForSensorStart => {
                self.start_configuration(7)?;
            }
            _ => {}
        }
        Ok(())
    }

    /// Starts one temperature, accelerometer and gyroscope burst read.
    pub fn request_sample(&mut self, timestamp: Timestamp) -> Result<(), Error<Bus::StartError>> {
        if self.state != State::Ready {
            return Err(Error::NotReady);
        }
        self.prepare_read(TEMP_DATA1, TRANSFER_CAPACITY);
        self.submit(TRANSFER_CAPACITY)?;
        self.state = State::ReadingSample { timestamp };
        Ok(())
    }

    /// Advances the state machine from an already-acknowledged SPI completion.
    pub fn on_transfer_complete(
        &mut self,
        completion: SpiCompletion,
    ) -> Icm42688Event<Bus::StartError> {
        self.bus.complete_transfer(completion);
        match completion {
            SpiCompletion::NotPending => Icm42688Event::NotPending,
            SpiCompletion::TransferFault => {
                self.state = State::Dormant;
                Icm42688Event::Fault(Error::TransferFault)
            }
            SpiCompletion::Complete => self.advance_after_completion(),
        }
    }

    /// Returns whether an IMU data-ready event can start a sensor transfer.
    pub fn is_ready(&self) -> bool {
        self.state == State::Ready
    }

    fn advance_after_completion(&mut self) -> Icm42688Event<Bus::StartError> {
        match self.state {
            State::Dormant => Icm42688Event::NotPending,
            State::Resetting => {
                self.state = State::WaitingForReset;
                Icm42688Event::Initializing
            }
            State::WaitingForReset | State::WaitingForSensorStart => Icm42688Event::NotPending,
            State::ReadingIdentity => {
                let identity = self.receive[1];
                if identity != EXPECTED_WHO_AM_I {
                    self.state = State::Dormant;
                    return Icm42688Event::Fault(Error::UnexpectedIdentity(identity));
                }
                self.start_configuration_event(0)
            }
            State::WritingConfiguration(index) => {
                let next = index + 1;
                if next == 7 {
                    // PWR_MGMT0 starts both sensor paths.  The following
                    // timer tick gives them more than the required 200 µs
                    // before their output-rate configuration is written.
                    self.state = State::WaitingForSensorStart;
                    Icm42688Event::Initializing
                } else if next == CONFIGURATION.len() {
                    self.state = State::Ready;
                    Icm42688Event::Initialized
                } else {
                    self.start_configuration_event(next)
                }
            }
            State::Ready => Icm42688Event::NotPending,
            State::ReadingSample { timestamp } => {
                let mut bytes = [0; SAMPLE_BYTES];
                bytes.copy_from_slice(&self.receive[1..]);
                self.state = State::Ready;
                Icm42688Event::Sample(sample::decode(&bytes, timestamp))
            }
        }
    }

    fn start_configuration_event(&mut self, index: usize) -> Icm42688Event<Bus::StartError> {
        match self.start_configuration(index) {
            Ok(()) => Icm42688Event::Initializing,
            Err(error) => Icm42688Event::Fault(error),
        }
    }

    fn start_configuration(&mut self, index: usize) -> Result<(), Error<Bus::StartError>> {
        let (register, value) = CONFIGURATION[index];
        self.prepare_write(register, value);
        if let Err(error) = self.submit(2) {
            self.state = State::Dormant;
            return Err(error);
        }
        self.state = State::WritingConfiguration(index);
        Ok(())
    }

    fn prepare_read(&mut self, register: u8, length: usize) {
        self.transmit = [0; TRANSFER_CAPACITY];
        self.receive = [0; TRANSFER_CAPACITY];
        self.transmit[0] = register | 0x80;
        debug_assert!(length <= TRANSFER_CAPACITY);
    }

    fn prepare_write(&mut self, register: u8, value: u8) {
        self.transmit = [0; TRANSFER_CAPACITY];
        self.receive = [0; TRANSFER_CAPACITY];
        self.transmit[..2].copy_from_slice(&[register, value]);
    }

    fn submit(&mut self, length: usize) -> Result<(), Error<Bus::StartError>> {
        let (bus, transmit, receive) = (&mut self.bus, &self.transmit, &mut self.receive);
        bus.start_transfer(&transmit[..length], &mut receive[..length])
            .map_err(Error::Start)
    }
}

#[cfg(test)]
mod tests {
    use super::{Error, Icm42688, Icm42688Event, TRANSFER_CAPACITY};
    use kaptan_spi::{SpiCompletion, SpiDevice};
    use kaptan_time::Timestamp;

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

    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_identity_and_wake_delays() {
        let mut response = [0; TRANSFER_CAPACITY];
        response[1] = 0x47;
        let mut imu = Icm42688::new(Bus { response });
        imu.begin_initialization().unwrap();
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Initializing
        ));
        assert!(matches!(imu.advance_initialization(), Ok(())));
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Initializing
        ));
        for _ in 0..7 {
            assert!(matches!(
                imu.on_transfer_complete(SpiCompletion::Complete),
                Icm42688Event::Initializing
            ));
        }
        assert!(matches!(imu.advance_initialization(), Ok(())));
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Initializing
        ));
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Initialized
        ));
        assert!(imu.is_ready());
        imu.request_sample(Timestamp::from_microseconds(1)).unwrap();
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Sample(_)
        ));
    }

    #[test]
    fn rejects_a_wrong_identity() {
        let mut imu = Icm42688::new(Bus {
            response: [0; TRANSFER_CAPACITY],
        });
        imu.begin_initialization().unwrap();
        let _ = imu.on_transfer_complete(SpiCompletion::Complete);
        imu.advance_initialization().unwrap();
        assert!(matches!(
            imu.on_transfer_complete(SpiCompletion::Complete),
            Icm42688Event::Fault(Error::UnexpectedIdentity(0))
        ));
    }
}