//! MicoAir MTF-01 Microlink UART frame decoder.
//!
//! Device documentation belongs beside this module.
//!
//! The MTF-01 continuously transmits fixed-size Microlink range-sensor frames.
//! Feed each received UART byte to [`mtf01::Mtf01::receive_byte`]. The parser is
//! allocation-free, does not use packed structs, and resynchronizes on a new
//! frame header after malformed input.

use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot};
use kaptan_time::{MonotonicClock, Timestamp};

const FRAME_HEADER: u8 = 0xEF;
const RANGE_SENSOR_MESSAGE_ID: u8 = 0x51;
const PAYLOAD_LENGTH: usize = 20;

#[derive(Clone, Copy, Debug, Eq, PartialEq)]
enum State {
    Header,
    DeviceId,
    SystemId,
    MessageId,
    Sequence,
    Length,
    Payload { index: usize },
    Checksum,
}

/// One complete MTF-01 range and optical-flow report.
///
/// This preserves the MTF-01's device-specific fields. Use
/// [`Self::range_snapshot`] or [`Self::optical_flow_snapshot`] to obtain a
/// device-independent observation for the rest of the flight stack.
#[derive(Clone, Copy, Debug, Eq, PartialEq)]
pub struct Mtf01Report {
    /// Flight-controller monotonic time at frame reception.
    pub timestamp: Timestamp,
    /// MTF-01 system time, in milliseconds.
    pub sensor_time_milliseconds: u32,
    /// Measured distance in millimetres. Zero means unavailable.
    pub distance_millimetres: u32,
    /// MTF-01 range-signal strength, from 0 through 255.
    pub distance_strength: u8,
    /// MTF-01 distance precision; smaller values indicate higher accuracy.
    pub distance_precision: u8,
    /// Whether the MTF-01 marked the distance measurement valid.
    pub distance_valid: bool,
    /// Optical-flow velocity along the MTF-01 X axis, in centimetres/second
    /// at one metre above ground.
    pub flow_velocity_x_centimetres_per_second_at_one_metre: i16,
    /// Optical-flow velocity along the MTF-01 Y axis, in centimetres/second
    /// at one metre above ground.
    pub flow_velocity_y_centimetres_per_second_at_one_metre: i16,
    /// MTF-01 optical-flow quality, from 0 through 255.
    pub flow_quality: u8,
    /// Whether the MTF-01 marked the optical-flow measurement valid.
    pub flow_valid: bool,
}

impl Mtf01Report {
    /// Creates an MTF-01 observation from one validated Microlink frame.
    #[allow(clippy::too_many_arguments)]
    pub const fn new(
        timestamp: Timestamp,
        sensor_time_milliseconds: u32,
        distance_millimetres: u32,
        distance_strength: u8,
        distance_precision: u8,
        distance_valid: bool,
        flow_velocity_x_centimetres_per_second_at_one_metre: i16,
        flow_velocity_y_centimetres_per_second_at_one_metre: i16,
        flow_quality: u8,
        flow_valid: bool,
    ) -> Self {
        Self {
            timestamp,
            sensor_time_milliseconds,
            distance_millimetres,
            distance_strength,
            distance_precision,
            distance_valid,
            flow_velocity_x_centimetres_per_second_at_one_metre,
            flow_velocity_y_centimetres_per_second_at_one_metre,
            flow_quality,
            flow_valid,
        }
    }

    /// Returns the valid range portion of this report.
    pub const fn range_snapshot(self) -> Option<RangeSnapshot> {
        if self.distance_valid && self.distance_millimetres != 0 {
            Some(RangeSnapshot::new(
                self.timestamp,
                self.distance_millimetres,
            ))
        } else {
            None
        }
    }

    /// Returns the valid, ground-referenced optical-flow portion of this
    /// report. MTF-01 velocities are specified in centimetres per second at
    /// one metre; the device documentation scales them by measured height.
    pub fn optical_flow_snapshot(self) -> Option<OpticalFlowSnapshot> {
        if !self.distance_valid || self.distance_millimetres == 0 || !self.flow_valid {
            return None;
        }

        let velocity_x = i64::from(self.flow_velocity_x_centimetres_per_second_at_one_metre)
            * i64::from(self.distance_millimetres)
            / 100;
        let velocity_y = i64::from(self.flow_velocity_y_centimetres_per_second_at_one_metre)
            * i64::from(self.distance_millimetres)
            / 100;
        if velocity_x < i64::from(i32::MIN)
            || velocity_x > i64::from(i32::MAX)
            || velocity_y < i64::from(i32::MIN)
            || velocity_y > i64::from(i32::MAX)
        {
            return None;
        }

        Some(OpticalFlowSnapshot::new(
            self.timestamp,
            [velocity_x as i32, velocity_y as i32],
            self.distance_millimetres,
            self.flow_quality,
        ))
    }
}

/// MTF-01 Microlink decoder.
///
/// One decoder is owned by one UART receive path. It accepts the MTF-01's
/// documented range-sensor message ID and 20-byte payload length. Device and
/// system IDs are deliberately accepted as transmitted, matching MicoAir's
/// reference decoder and allowing sensor IDs configured outside factory
/// defaults.
pub struct Mtf01 {
    state: State,
    checksum: u8,
    payload: [u8; PAYLOAD_LENGTH],
}

impl Mtf01 {
    /// Creates a decoder awaiting the `0xEF` Microlink frame header.
    pub const fn new() -> Self {
        Self {
            state: State::Header,
            checksum: 0,
            payload: [0; PAYLOAD_LENGTH],
        }
    }

    /// Accepts one UART byte and returns a complete, checksum-validated MTF-01
    /// report when available.
    ///
    /// `timestamp` should be the controller monotonic time at receipt of this
    /// byte. It is used only for a completed frame, so it records the arrival
    /// time of the report rather than an arbitrary intermediate byte.
    pub fn receive_byte(&mut self, timestamp: Timestamp, byte: u8) -> Option<Mtf01Report> {
        match self.state {
            State::Header => {
                if byte == FRAME_HEADER {
                    self.checksum = byte;
                    self.state = State::DeviceId;
                }
            }
            State::DeviceId => {
                self.accept(byte, State::SystemId);
            }
            State::SystemId => {
                self.accept(byte, State::MessageId);
            }
            State::MessageId => {
                if byte == RANGE_SENSOR_MESSAGE_ID {
                    self.accept(byte, State::Sequence);
                } else {
                    self.restart(byte);
                }
            }
            State::Sequence => self.accept(byte, State::Length),
            State::Length => {
                if usize::from(byte) == PAYLOAD_LENGTH {
                    self.accept(byte, State::Payload { index: 0 });
                } else {
                    self.restart(byte);
                }
            }
            State::Payload { index } => {
                self.payload[index] = byte;
                self.checksum = self.checksum.wrapping_add(byte);
                self.state = if index + 1 == PAYLOAD_LENGTH {
                    State::Checksum
                } else {
                    State::Payload { index: index + 1 }
                };
            }
            State::Checksum => {
                let valid = byte == self.checksum;
                // The checksum terminates the frame even when its value is
                // also the Microlink header. The reference parser returns to
                // its initial state here rather than treating checksum data as
                // the start of the next frame.
                self.reset();
                if valid {
                    return Some(self.snapshot(timestamp));
                }
            }
        }

        None
    }

    /// Drops any partial frame and awaits a new header.
    pub fn reset(&mut self) {
        self.state = State::Header;
        self.checksum = 0;
    }

    fn accept(&mut self, byte: u8, next: State) {
        self.checksum = self.checksum.wrapping_add(byte);
        self.state = next;
    }

    fn restart(&mut self, byte: u8) {
        if byte == FRAME_HEADER {
            self.checksum = FRAME_HEADER;
            self.state = State::DeviceId;
        } else {
            self.reset();
        }
    }

    fn snapshot(&self, timestamp: Timestamp) -> Mtf01Report {
        Mtf01Report::new(
            timestamp,
            u32::from_le_bytes(self.payload[0..4].try_into().unwrap()),
            u32::from_le_bytes(self.payload[4..8].try_into().unwrap()),
            self.payload[8],
            self.payload[9],
            self.payload[10] == 1,
            i16::from_le_bytes(self.payload[12..14].try_into().unwrap()),
            i16::from_le_bytes(self.payload[14..16].try_into().unwrap()),
            self.payload[16],
            self.payload[17] == 1,
        )
    }
}

impl Default for Mtf01 {
    fn default() -> Self {
        Self::new()
    }
}

/// Timestamping MTF-01 UART receive service.
///
/// Feed this service from the selected UART receive interrupt or DMA consumer.
/// The board retains ownership of pin muxing, buffering, and interrupt
/// acknowledgement.
pub struct Mtf01Service<Clock> {
    clock: Clock,
    decoder: Mtf01,
}

impl<Clock> Mtf01Service<Clock> {
    /// Creates an MTF-01 receive service awaiting a Microlink frame header.
    pub const fn new(clock: Clock) -> Self {
        Self {
            clock,
            decoder: Mtf01::new(),
        }
    }

    /// Discards a partial frame after a UART transport fault.
    pub fn reset(&mut self) {
        self.decoder.reset();
    }
}

impl<Clock: MonotonicClock> Mtf01Service<Clock> {
    /// Consumes one UART byte and returns one validated MTF-01 report.
    pub fn on_received_byte(&mut self, byte: u8) -> Option<Mtf01Report> {
        self.decoder.receive_byte(self.clock.now(), byte)
    }
}

#[cfg(test)]
mod tests {
    use super::{Mtf01, Mtf01Report, Mtf01Service};
    use kaptan_time::{MonotonicClock, Timestamp};

    fn frame(payload: [u8; 20]) -> [u8; 27] {
        let mut frame = [0_u8; 27];
        frame[..6].copy_from_slice(&[0xEF, 0x0F, 0x00, 0x51, 0xA5, 20]);
        frame[6..26].copy_from_slice(&payload);
        frame[26] = frame[..26]
            .iter()
            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));
        frame
    }

    fn sample_frame() -> [u8; 27] {
        frame([
            0x78, 0x56, 0x34, 0x12, // sensor time: 305419896 ms
            0xD2, 0x04, 0x00, 0x00, // distance: 1234 mm
            200, 7, 1, 0, // range metadata and reserved byte
            0x2E, 0xFB, // flow x: -1234
            0xD2, 0x04, // flow y: 1234
            250, 1, 0, 0, // flow metadata and reserved u16
        ])
    }

    #[test]
    fn decodes_a_valid_little_endian_range_and_flow_report() {
        let mut decoder = Mtf01::new();
        let timestamp = Timestamp::from_microseconds(9_876);
        let mut report = None;
        for byte in sample_frame() {
            report = decoder.receive_byte(timestamp, byte).or(report);
        }

        assert_eq!(
            report,
            Some(Mtf01Report::new(
                timestamp,
                0x1234_5678,
                1_234,
                200,
                7,
                true,
                -1_234,
                1_234,
                250,
                true,
            ))
        );
    }

    #[test]
    fn rejects_a_bad_checksum_without_publishing_a_report() {
        let mut decoder = Mtf01::new();
        let mut corrupt = sample_frame();
        corrupt[26] ^= 0xFF;

        assert!(corrupt.into_iter().all(|byte| {
            decoder
                .receive_byte(Timestamp::from_microseconds(1), byte)
                .is_none()
        }));
    }

    #[test]
    fn treats_a_header_valued_checksum_as_a_checksum() {
        let mut decoder = Mtf01::new();
        let mut payload = [0_u8; 20];
        // The fixed header through length sums to 0x08, making 0xE7 produce
        // the `0xEF` checksum value that otherwise resembles a new header.
        payload[0] = 0xE7;
        let checksum_is_header = frame(payload);
        assert_eq!(checksum_is_header[26], 0xEF);

        let first_report = checksum_is_header
            .into_iter()
            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte));
        assert!(first_report.is_some());
        let report = sample_frame()
            .into_iter()
            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(2), byte));

        assert_eq!(report.unwrap().distance_millimetres, 1_234);
    }

    #[test]
    fn resynchronizes_when_a_header_arrives_during_a_bad_frame() {
        let mut decoder = Mtf01::new();
        let valid = sample_frame();
        let mut bytes = [0_u8; 30];
        bytes[..3].copy_from_slice(&[0xEF, 0x0F, 0xEF]);
        bytes[3..].copy_from_slice(&valid);

        let mut report = None;
        for byte in bytes {
            report = decoder
                .receive_byte(Timestamp::from_microseconds(7), byte)
                .or(report);
        }
        assert_eq!(report.unwrap().distance_millimetres, 1_234);
    }

    #[test]
    fn does_not_accept_another_micolink_message_type() {
        let mut decoder = Mtf01::new();
        let mut other_message = sample_frame();
        other_message[3] = 0x52;
        other_message[26] = other_message[..26]
            .iter()
            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));

        assert!(other_message.into_iter().all(|byte| {
            decoder
                .receive_byte(Timestamp::from_microseconds(1), byte)
                .is_none()
        }));
    }

    #[test]
    fn accepts_configured_micolink_device_and_system_ids() {
        let mut decoder = Mtf01::new();
        let mut configured_ids = sample_frame();
        configured_ids[1] = 0x42;
        configured_ids[2] = 0x99;
        configured_ids[26] = configured_ids[..26]
            .iter()
            .fold(0_u8, |checksum, byte| checksum.wrapping_add(*byte));

        let report = configured_ids
            .into_iter()
            .find_map(|byte| decoder.receive_byte(Timestamp::from_microseconds(1), byte));
        assert_eq!(report.unwrap().distance_millimetres, 1_234);
    }

    #[test]
    fn exposes_only_valid_generic_observations() {
        let mut report = Mtf01Report::new(
            Timestamp::from_microseconds(123),
            7,
            2_000,
            8,
            9,
            true,
            50,
            -25,
            12,
            true,
        );
        assert_eq!(report.range_snapshot().unwrap().distance_millimetres, 2_000);
        assert_eq!(
            report
                .optical_flow_snapshot()
                .unwrap()
                .velocity_millimetres_per_second,
            [1_000, -500]
        );

        report.distance_valid = false;
        assert_eq!(report.range_snapshot(), None);
        assert_eq!(report.optical_flow_snapshot(), None);
    }

    struct Clock(u64);

    impl MonotonicClock for Clock {
        fn now(&mut self) -> Timestamp {
            self.0 += 1;
            Timestamp::from_microseconds(self.0)
        }
    }

    #[test]
    fn timestamps_a_report_with_its_last_received_byte() {
        let mut service = Mtf01Service::new(Clock(100));
        let report = sample_frame()
            .into_iter()
            .find_map(|byte| service.on_received_byte(byte))
            .unwrap();

        assert_eq!(report.timestamp, Timestamp::from_microseconds(127));
    }
}