yscv-video 0.1.8

//! Pure-Rust MAVLink v2 parser — no external crates.
//!
//! Supports parsing MAVLink v2 frames from byte streams, CRC validation with
//! per-message `CRC_EXTRA`, and typed deserialization for common flight
//! controller messages (HEARTBEAT, SYS_STATUS, ATTITUDE, GLOBAL_POSITION_INT,
//! RC_CHANNELS_RAW).
//!
//! On Linux a [`MavlinkSerial`] helper opens a serial port in raw mode and
//! feeds bytes into the streaming parser.

use crate::VideoError;
use crate::overlay::TelemetryData;

// ---------------------------------------------------------------------------
// MAVLink v2 constants
// ---------------------------------------------------------------------------

/// MAVLink v2 start-of-frame marker.
const MAVLINK_STX_V2: u8 = 0xFD;

/// Minimum frame size: STX(1) + header(9) + CRC(2) = 12 bytes (zero payload).
const MIN_FRAME_LEN: usize = 12;

// Message IDs (MAVLink common.xml)
const MSG_ID_HEARTBEAT: u32 = 0;
const MSG_ID_SYS_STATUS: u32 = 1;
const MSG_ID_ATTITUDE: u32 = 30;
const MSG_ID_GLOBAL_POSITION_INT: u32 = 33;
const MSG_ID_RC_CHANNELS_RAW: u32 = 35;

// CRC_EXTRA values generated by MAVLink XML toolchain
const CRC_EXTRA_HEARTBEAT: u8 = 50;
const CRC_EXTRA_SYS_STATUS: u8 = 124;
const CRC_EXTRA_ATTITUDE: u8 = 39;
const CRC_EXTRA_GLOBAL_POSITION_INT: u8 = 104;
const CRC_EXTRA_RC_CHANNELS_RAW: u8 = 244;

// ---------------------------------------------------------------------------
// Public types
// ---------------------------------------------------------------------------

/// MAVLink v2 message header (fields after STX).
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct MavlinkHeader {
    pub payload_len: u8,
    pub seq: u8,
    pub sysid: u8,
    pub compid: u8,
    /// 24-bit message ID assembled from three ID bytes.
    pub msgid: u32,
}

/// Parsed MAVLink message with typed payload.
#[derive(Debug, Clone, PartialEq)]
pub enum MavlinkMessage {
    Heartbeat {
        autopilot: u8,
        mav_type: u8,
        system_status: u8,
    },
    Attitude {
        roll: f32,
        pitch: f32,
        yaw: f32,
        rollspeed: f32,
        pitchspeed: f32,
        yawspeed: f32,
    },
    GlobalPositionInt {
        lat: i32,
        lon: i32,
        alt: i32,
        relative_alt: i32,
        vx: i16,
        vy: i16,
        vz: i16,
        hdg: u16,
    },
    SysStatus {
        voltage_battery: u16,
        current_battery: i16,
        battery_remaining: i8,
    },
    RcChannels {
        chan1_raw: u16,
        chan2_raw: u16,
        chan3_raw: u16,
        chan4_raw: u16,
    },
    Unknown {
        msgid: u32,
        payload: Vec<u8>,
    },
}

// ---------------------------------------------------------------------------
// CRC — X.25 / CRC-16-CCITT (0xFFFF init, 0x1021 poly, reflected/bit-by-bit)
// ---------------------------------------------------------------------------

/// Compute MAVLink X.25 CRC-16 over `data`, then fold in `crc_extra`.
///
/// The algorithm is "CRC-16/MCRF4XX" (init 0xFFFF, poly 0x1021 reflected):
///   for each byte b:
///     tmp = b ^ (crc & 0xFF)
///     tmp ^= (tmp << 4) & 0xFF
///     crc = (crc >> 8) ^ (u16(tmp) << 8) ^ (u16(tmp) << 3) ^ (u16(tmp) >> 4)
fn mavlink_crc(data: &[u8], crc_extra: u8) -> u16 {
    let mut crc: u16 = 0xFFFF;
    for &b in data {
        crc = crc_accumulate(crc, b);
    }
    crc = crc_accumulate(crc, crc_extra);
    crc
}

/// Accumulate one byte into a running X.25 CRC.
///
/// Mirrors the MAVLink C reference `crc_accumulate`:
/// ```c
/// tmp = data ^ (uint8_t)(crc & 0xff);
/// tmp ^= (tmp << 4);
/// crc = (crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4);
/// ```
#[inline]
fn crc_accumulate(crc: u16, byte: u8) -> u16 {
    let mut tmp = byte ^ (crc as u8);
    tmp ^= tmp.wrapping_shl(4);
    let t16 = tmp as u16;
    (crc >> 8) ^ (t16 << 8) ^ (t16 << 3) ^ (t16 >> 4)
}

// ---------------------------------------------------------------------------
// Lookup: message ID -> CRC_EXTRA
// ---------------------------------------------------------------------------

/// Returns the `CRC_EXTRA` seed for a known message ID, or `None`.
fn crc_extra_for(msgid: u32) -> Option<u8> {
    match msgid {
        MSG_ID_HEARTBEAT => Some(CRC_EXTRA_HEARTBEAT),
        MSG_ID_SYS_STATUS => Some(CRC_EXTRA_SYS_STATUS),
        MSG_ID_ATTITUDE => Some(CRC_EXTRA_ATTITUDE),
        MSG_ID_GLOBAL_POSITION_INT => Some(CRC_EXTRA_GLOBAL_POSITION_INT),
        MSG_ID_RC_CHANNELS_RAW => Some(CRC_EXTRA_RC_CHANNELS_RAW),
        _ => None,
    }
}

// ---------------------------------------------------------------------------
// Little-endian payload helpers
// ---------------------------------------------------------------------------

#[inline]
fn le_u16(buf: &[u8], off: usize) -> u16 {
    u16::from_le_bytes([buf[off], buf[off + 1]])
}

#[inline]
fn le_i16(buf: &[u8], off: usize) -> i16 {
    i16::from_le_bytes([buf[off], buf[off + 1]])
}

#[inline]
fn le_i32(buf: &[u8], off: usize) -> i32 {
    i32::from_le_bytes([buf[off], buf[off + 1], buf[off + 2], buf[off + 3]])
}

#[inline]
fn le_f32(buf: &[u8], off: usize) -> f32 {
    f32::from_le_bytes([buf[off], buf[off + 1], buf[off + 2], buf[off + 3]])
}

// ---------------------------------------------------------------------------
// Payload deserialization
// ---------------------------------------------------------------------------

fn decode_heartbeat(payload: &[u8]) -> MavlinkMessage {
    // MAVLink HEARTBEAT wire order (le):
    //   custom_mode  u32  [0..4]
    //   type         u8   [4]
    //   autopilot    u8   [5]
    //   base_mode    u8   [6]
    //   system_status u8  [7]
    //   mavlink_version u8 [8]
    let mav_type = if payload.len() > 4 { payload[4] } else { 0 };
    let autopilot = if payload.len() > 5 { payload[5] } else { 0 };
    let system_status = if payload.len() > 7 { payload[7] } else { 0 };
    MavlinkMessage::Heartbeat {
        autopilot,
        mav_type,
        system_status,
    }
}

fn decode_sys_status(payload: &[u8]) -> MavlinkMessage {
    // Interesting fields (le):
    //   sensors_present  u32 [0..4]
    //   sensors_enabled  u32 [4..8]
    //   sensors_health   u32 [8..12]
    //   load             u16 [12..14]
    //   voltage_battery  u16 [14..16]  (mV)
    //   current_battery  i16 [16..18]  (cA)
    //   battery_remaining i8 [18]      (%)
    let voltage_battery = if payload.len() >= 16 {
        le_u16(payload, 14)
    } else {
        0
    };
    let current_battery = if payload.len() >= 18 {
        le_i16(payload, 16)
    } else {
        0
    };
    let battery_remaining = if payload.len() >= 19 {
        payload[18] as i8
    } else {
        -1
    };
    MavlinkMessage::SysStatus {
        voltage_battery,
        current_battery,
        battery_remaining,
    }
}

fn decode_attitude(payload: &[u8]) -> MavlinkMessage {
    // Wire order (le):
    //   time_boot_ms u32 [0..4]
    //   roll         f32 [4..8]
    //   pitch        f32 [8..12]
    //   yaw          f32 [12..16]
    //   rollspeed    f32 [16..20]
    //   pitchspeed   f32 [20..24]
    //   yawspeed     f32 [24..28]
    if payload.len() < 28 {
        return MavlinkMessage::Attitude {
            roll: 0.0,
            pitch: 0.0,
            yaw: 0.0,
            rollspeed: 0.0,
            pitchspeed: 0.0,
            yawspeed: 0.0,
        };
    }
    MavlinkMessage::Attitude {
        roll: le_f32(payload, 4),
        pitch: le_f32(payload, 8),
        yaw: le_f32(payload, 12),
        rollspeed: le_f32(payload, 16),
        pitchspeed: le_f32(payload, 20),
        yawspeed: le_f32(payload, 24),
    }
}

fn decode_global_position_int(payload: &[u8]) -> MavlinkMessage {
    // Wire order (le):
    //   time_boot_ms  u32 [0..4]
    //   lat           i32 [4..8]   (degE7)
    //   lon           i32 [8..12]  (degE7)
    //   alt           i32 [12..16] (mm MSL)
    //   relative_alt  i32 [16..20] (mm AGL)
    //   vx            i16 [20..22] (cm/s N)
    //   vy            i16 [22..24] (cm/s E)
    //   vz            i16 [24..26] (cm/s D)
    //   hdg           u16 [26..28] (cdeg)
    if payload.len() < 28 {
        return MavlinkMessage::GlobalPositionInt {
            lat: 0,
            lon: 0,
            alt: 0,
            relative_alt: 0,
            vx: 0,
            vy: 0,
            vz: 0,
            hdg: 0,
        };
    }
    MavlinkMessage::GlobalPositionInt {
        lat: le_i32(payload, 4),
        lon: le_i32(payload, 8),
        alt: le_i32(payload, 12),
        relative_alt: le_i32(payload, 16),
        vx: le_i16(payload, 20),
        vy: le_i16(payload, 22),
        vz: le_i16(payload, 24),
        hdg: le_u16(payload, 26),
    }
}

fn decode_rc_channels_raw(payload: &[u8]) -> MavlinkMessage {
    // Wire order (le):
    //   time_boot_ms  u32 [0..4]
    //   chan1_raw      u16 [4..6]
    //   chan2_raw      u16 [6..8]
    //   chan3_raw      u16 [8..10]
    //   chan4_raw      u16 [10..12]
    //   ...
    if payload.len() < 12 {
        return MavlinkMessage::RcChannels {
            chan1_raw: 0,
            chan2_raw: 0,
            chan3_raw: 0,
            chan4_raw: 0,
        };
    }
    MavlinkMessage::RcChannels {
        chan1_raw: le_u16(payload, 4),
        chan2_raw: le_u16(payload, 6),
        chan3_raw: le_u16(payload, 8),
        chan4_raw: le_u16(payload, 10),
    }
}

// ---------------------------------------------------------------------------
// Single-frame parser
// ---------------------------------------------------------------------------

/// Parse a single MAVLink v2 frame starting at the beginning of `data`.
///
/// On success returns `(message, bytes_consumed)`.  The parser scans forward
/// to the first `0xFD` start byte, so leading garbage is silently skipped.
pub fn parse_mavlink_frame(data: &[u8]) -> Result<(MavlinkMessage, usize), VideoError> {
    // Scan for STX
    let stx_pos = data
        .iter()
        .position(|&b| b == MAVLINK_STX_V2)
        .ok_or_else(|| VideoError::Codec("no MAVLink v2 STX marker found".into()))?;

    let buf = &data[stx_pos..];
    if buf.len() < MIN_FRAME_LEN {
        return Err(VideoError::Codec("incomplete MAVLink v2 frame".into()));
    }

    let payload_len = buf[1] as usize;
    let frame_len = MIN_FRAME_LEN + payload_len; // STX(1) + header(9) + payload + CRC(2)
    if buf.len() < frame_len {
        return Err(VideoError::Codec("incomplete MAVLink v2 frame".into()));
    }

    // Parse header fields
    let _incompat_flags = buf[2];
    let _compat_flags = buf[3];
    let seq = buf[4];
    let sysid = buf[5];
    let compid = buf[6];
    let msgid = (buf[7] as u32) | ((buf[8] as u32) << 8) | ((buf[9] as u32) << 16);

    let header = MavlinkHeader {
        payload_len: payload_len as u8,
        seq,
        sysid,
        compid,
        msgid,
    };

    let payload = &buf[10..10 + payload_len];

    // CRC validation: covers bytes 1..10+payload_len (header sans STX + payload)
    let crc_data = &buf[1..10 + payload_len];
    let expected_crc_lo = buf[10 + payload_len];
    let expected_crc_hi = buf[10 + payload_len + 1];
    let expected_crc = u16::from_le_bytes([expected_crc_lo, expected_crc_hi]);

    let crc_extra = crc_extra_for(msgid).unwrap_or(0);
    let computed = mavlink_crc(crc_data, crc_extra);

    if computed != expected_crc {
        return Err(VideoError::Codec(format!(
            "MAVLink CRC mismatch for msgid {msgid}: expected 0x{expected_crc:04X}, got 0x{computed:04X}"
        )));
    }

    // Decode typed payload
    let msg = match msgid {
        MSG_ID_HEARTBEAT => decode_heartbeat(payload),
        MSG_ID_SYS_STATUS => decode_sys_status(payload),
        MSG_ID_ATTITUDE => decode_attitude(payload),
        MSG_ID_GLOBAL_POSITION_INT => decode_global_position_int(payload),
        MSG_ID_RC_CHANNELS_RAW => decode_rc_channels_raw(payload),
        _ => MavlinkMessage::Unknown {
            msgid: header.msgid,
            payload: payload.to_vec(),
        },
    };

    // Total bytes consumed from the *original* slice
    let consumed = stx_pos + frame_len;
    Ok((msg, consumed))
}

// ---------------------------------------------------------------------------
// Streaming parser
// ---------------------------------------------------------------------------

/// Streaming MAVLink parser that accumulates bytes and yields complete messages.
///
/// Feed arbitrary chunks of data via [`feed`](MavlinkParser::feed); the parser
/// buffers incomplete frames internally and returns all messages that could be
/// fully parsed from the current buffer.
pub struct MavlinkParser {
    buf: Vec<u8>,
}

impl MavlinkParser {
    /// Create a new streaming parser with an empty internal buffer.
    pub fn new() -> Self {
        Self {
            buf: Vec::with_capacity(512),
        }
    }

    /// Append `data` to the internal buffer and return all complete messages.
    pub fn feed(&mut self, data: &[u8]) -> Vec<MavlinkMessage> {
        self.buf.extend_from_slice(data);
        let mut messages = Vec::new();

        loop {
            // Find next STX
            let stx_pos = match self.buf.iter().position(|&b| b == MAVLINK_STX_V2) {
                Some(p) => p,
                None => {
                    self.buf.clear();
                    break;
                }
            };

            // Discard bytes before STX
            if stx_pos > 0 {
                self.buf.drain(..stx_pos);
            }

            // Need at least MIN_FRAME_LEN to read payload_len
            if self.buf.len() < MIN_FRAME_LEN {
                break;
            }

            let payload_len = self.buf[1] as usize;
            let frame_len = MIN_FRAME_LEN + payload_len;

            if self.buf.len() < frame_len {
                break; // wait for more data
            }

            match parse_mavlink_frame(&self.buf) {
                Ok((msg, consumed)) => {
                    messages.push(msg);
                    self.buf.drain(..consumed);
                }
                Err(_) => {
                    // Bad frame — skip this STX and try the next one
                    self.buf.drain(..1);
                }
            }
        }

        messages
    }
}

// ---------------------------------------------------------------------------
// Telemetry integration
// ---------------------------------------------------------------------------

/// Incremental telemetry update produced from a single MAVLink message.
///
/// Callers typically maintain a running [`TelemetryData`] and apply updates as
/// messages arrive.
pub enum TelemetryUpdate {
    Battery { voltage: f32, current: f32 },
    Position { lat: f64, lon: f64, alt_m: f32 },
    Attitude { heading_deg: f32 },
}

/// Convert a MAVLink message into a [`TelemetryUpdate`] for the OSD overlay.
///
/// Returns `None` for message types that do not map to any telemetry field.
pub fn telemetry_from_mavlink(msg: &MavlinkMessage) -> Option<TelemetryUpdate> {
    match msg {
        MavlinkMessage::SysStatus {
            voltage_battery,
            current_battery,
            ..
        } => Some(TelemetryUpdate::Battery {
            voltage: *voltage_battery as f32 / 1000.0, // mV -> V
            current: *current_battery as f32 / 100.0,  // cA -> A
        }),
        MavlinkMessage::GlobalPositionInt {
            lat,
            lon,
            relative_alt,
            ..
        } => Some(TelemetryUpdate::Position {
            lat: *lat as f64 / 1e7,
            lon: *lon as f64 / 1e7,
            alt_m: *relative_alt as f32 / 1000.0, // mm -> m
        }),
        MavlinkMessage::Attitude { yaw, .. } => {
            let mut deg = yaw.to_degrees();
            if deg < 0.0 {
                deg += 360.0;
            }
            Some(TelemetryUpdate::Attitude { heading_deg: deg })
        }
        _ => None,
    }
}

/// Apply a [`TelemetryUpdate`] to a mutable [`TelemetryData`] struct.
pub fn apply_telemetry_update(td: &mut TelemetryData, update: &TelemetryUpdate) {
    match update {
        TelemetryUpdate::Battery { voltage, current } => {
            td.battery_voltage = *voltage;
            td.battery_current = *current;
        }
        TelemetryUpdate::Position { lat, lon, alt_m } => {
            td.lat = *lat;
            td.lon = *lon;
            td.altitude_m = *alt_m;
        }
        TelemetryUpdate::Attitude { heading_deg } => {
            td.heading_deg = *heading_deg;
        }
    }
}

// ---------------------------------------------------------------------------
// Serial port reader (Linux only)
// ---------------------------------------------------------------------------

#[cfg(target_os = "linux")]
#[allow(unsafe_code)]
mod serial {
    use super::{MavlinkMessage, MavlinkParser};
    use crate::VideoError;

    // termios constants (Linux aarch64 / x86_64)
    const TCGETS2: u64 = 0x802C_542A;
    const TCSETS2: u64 = 0x402C_542B;
    const BOTHER: u32 = 0x1000;
    const CLOCAL: u32 = 0x0800;
    const CREAD: u32 = 0x0080;
    const CS8: u32 = 0x0030;

    // Raw-mode mask bits to clear
    const IGNBRK: u32 = 0x0000_0001;
    const BRKINT: u32 = 0x0000_0002;
    const PARMRK: u32 = 0x0000_0008;
    const ISTRIP: u32 = 0x0000_0020;
    const INLCR: u32 = 0x0000_0040;
    const IGNCR: u32 = 0x0000_0080;
    const ICRNL: u32 = 0x0000_0100;
    const IXON: u32 = 0x0000_0400;
    const OPOST: u32 = 0x0000_0001;
    const ECHO_: u32 = 0x0000_0008;
    const ECHONL: u32 = 0x0000_0040;
    const ICANON: u32 = 0x0000_0002;
    const ISIG: u32 = 0x0000_0001;
    const IEXTEN: u32 = 0x0000_8000;
    const CSIZE: u32 = 0x0000_0030;
    const PARENB: u32 = 0x0000_0100;

    // cc indices
    const VMIN: usize = 6;
    const VTIME: usize = 5;

    // termios2 struct layout (Linux, 44 bytes)
    #[repr(C)]
    #[derive(Clone)]
    struct Termios2 {
        c_iflag: u32,
        c_oflag: u32,
        c_cflag: u32,
        c_lflag: u32,
        c_line: u8,
        c_cc: [u8; 19],
        c_ispeed: u32,
        c_ospeed: u32,
    }

    impl Termios2 {
        fn zeroed() -> Self {
            Self {
                c_iflag: 0,
                c_oflag: 0,
                c_cflag: 0,
                c_lflag: 0,
                c_line: 0,
                c_cc: [0; 19],
                c_ispeed: 0,
                c_ospeed: 0,
            }
        }
    }

    unsafe extern "C" {
        fn ioctl(fd: i32, request: u64, ...) -> i32;
        fn read(fd: i32, buf: *mut u8, count: usize) -> isize;
        fn open(path: *const u8, flags: i32) -> i32;
        fn close(fd: i32) -> i32;
    }

    const O_RDWR: i32 = 0x0002;
    const O_NOCTTY: i32 = 0x0100;
    const O_NONBLOCK: i32 = 0x0800;

    /// MAVLink serial port reader using raw termios (Linux only).
    ///
    /// Opens a serial device (e.g. `/dev/ttyACM0`) in raw mode at the given
    /// baud rate and reads MAVLink v2 frames.
    pub struct MavlinkSerial {
        fd: i32,
        parser: MavlinkParser,
    }

    impl MavlinkSerial {
        /// Open a serial device and configure it for raw MAVLink reception.
        ///
        /// # Safety concern
        /// Uses `extern "C"` FFI to call `open`, `ioctl` (TCGETS2/TCSETS2).
        /// All pointers are to stack-local structs owned by this function.
        pub fn open(device: &str, baud: u32) -> Result<Self, VideoError> {
            let mut path_buf = Vec::with_capacity(device.len() + 1);
            path_buf.extend_from_slice(device.as_bytes());
            path_buf.push(0); // NUL terminator

            // SAFETY: path_buf is a valid NUL-terminated C string on the stack.
            // open() returns a file descriptor or -1 on error.
            let fd = unsafe { open(path_buf.as_ptr(), O_RDWR | O_NOCTTY | O_NONBLOCK) };
            if fd < 0 {
                return Err(VideoError::Source(format!(
                    "cannot open serial device {device}"
                )));
            }

            // Get current termios2
            let mut tio = Termios2::zeroed();
            // SAFETY: tio is a valid mutable reference to a stack-local Termios2.
            // ioctl(TCGETS2) populates it from the kernel.
            let rc = unsafe { ioctl(fd, TCGETS2, &mut tio as *mut Termios2) };
            if rc < 0 {
                // SAFETY: fd is a valid open file descriptor.
                unsafe {
                    close(fd);
                }
                return Err(VideoError::Source("TCGETS2 failed".into()));
            }

            // Raw mode
            tio.c_iflag &= !(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
            tio.c_oflag &= !OPOST;
            tio.c_lflag &= !(ECHO_ | ECHONL | ICANON | ISIG | IEXTEN);
            tio.c_cflag &= !(CSIZE | PARENB);
            tio.c_cflag |= CS8 | CLOCAL | CREAD;

            // Custom baud via BOTHER
            tio.c_cflag &= !0x100F; // clear CBAUD bits
            tio.c_cflag |= BOTHER;
            tio.c_ispeed = baud;
            tio.c_ospeed = baud;

            // Non-blocking reads: VMIN=0, VTIME=1 (100ms timeout)
            tio.c_cc[VMIN] = 0;
            tio.c_cc[VTIME] = 1;

            // SAFETY: tio is a valid Termios2 pointer, fd is open.
            let rc = unsafe { ioctl(fd, TCSETS2, &tio as *const Termios2) };
            if rc < 0 {
                // SAFETY: fd is a valid open file descriptor.
                unsafe {
                    close(fd);
                }
                return Err(VideoError::Source("TCSETS2 failed".into()));
            }

            Ok(Self {
                fd,
                parser: MavlinkParser::new(),
            })
        }

        /// Read available bytes and return all complete MAVLink messages.
        pub fn read_messages(&mut self) -> Result<Vec<MavlinkMessage>, VideoError> {
            let mut tmp = [0u8; 1024];
            // SAFETY: tmp is a valid stack buffer, fd was opened by Self::open.
            let n = unsafe { read(self.fd, tmp.as_mut_ptr(), tmp.len()) };
            if n < 0 {
                // EAGAIN / EWOULDBLOCK is expected for non-blocking fd
                return Ok(Vec::new());
            }
            Ok(self.parser.feed(&tmp[..n as usize]))
        }
    }

    impl Drop for MavlinkSerial {
        fn drop(&mut self) {
            // SAFETY: self.fd is a valid open file descriptor obtained from open().
            unsafe {
                close(self.fd);
            }
        }
    }
}

#[cfg(target_os = "linux")]
pub use serial::MavlinkSerial;

// ---------------------------------------------------------------------------
// Tests
// ---------------------------------------------------------------------------

#[cfg(test)]
mod tests {
    use super::*;

    // Helper: build a complete MAVLink v2 frame from payload + msgid
    fn build_frame(msgid: u32, payload: &[u8], seq: u8, sysid: u8, compid: u8) -> Vec<u8> {
        let payload_len = payload.len() as u8;
        let mut frame = Vec::with_capacity(12 + payload.len());
        frame.push(MAVLINK_STX_V2);
        frame.push(payload_len);
        frame.push(0); // incompat_flags
        frame.push(0); // compat_flags
        frame.push(seq);
        frame.push(sysid);
        frame.push(compid);
        frame.push((msgid & 0xFF) as u8);
        frame.push(((msgid >> 8) & 0xFF) as u8);
        frame.push(((msgid >> 16) & 0xFF) as u8);
        frame.extend_from_slice(payload);

        // CRC over header (bytes 1..10) + payload
        let crc_data = &frame[1..];
        let crc_extra = crc_extra_for(msgid).unwrap_or(0);
        let crc = mavlink_crc(crc_data, crc_extra);
        frame.push((crc & 0xFF) as u8);
        frame.push((crc >> 8) as u8);
        frame
    }

    #[test]
    fn crc_known_data() {
        // Verify CRC is deterministic
        let crc_a = mavlink_crc(&[0x01, 0x02, 0x03], 50);
        let crc_b = mavlink_crc(&[0x01, 0x02, 0x03], 50);
        assert_eq!(crc_a, crc_b, "CRC should be deterministic");

        // Different data must produce different CRC
        let crc_c = mavlink_crc(&[0x01, 0x02, 0x04], 50);
        assert_ne!(crc_a, crc_c, "different data should yield different CRC");

        // Different crc_extra must produce different CRC
        let crc_d = mavlink_crc(&[0x01, 0x02, 0x03], 51);
        assert_ne!(
            crc_a, crc_d,
            "different crc_extra should yield different CRC"
        );

        // Verify round-trip via build_frame/parse_mavlink_frame (the CRC in
        // build_frame is computed by mavlink_crc, and parse_mavlink_frame
        // recomputes it and checks equality — so all other tests implicitly
        // validate CRC correctness).  Here we just verify the init value
        // matches the spec: init=0xFFFF, so CRC of zero-length data with
        // crc_extra=0 is crc_accumulate(0xFFFF, 0x00).
        let crc_empty = mavlink_crc(&[], 0);
        // Step through manually:
        //   tmp = 0x00 ^ (0xFFFF as u8) = 0xFF
        //   tmp ^= (0xFF << 4) as u8   = 0xFF ^ 0xF0 = 0x0F  (wrapping_shl 4 on u8: 0xF0)
        //   t16 = 0x000F
        //   (0xFFFF >> 8) ^ (0x0F << 8) ^ (0x0F << 3) ^ (0x0F >> 4)
        //   = 0x00FF ^ 0x0F00 ^ 0x0078 ^ 0x0000 = 0x0F87  -- wait, let me recalculate:
        //   Actually 0x00FF ^ 0x0F00 = 0x0FFF, ^ 0x0078 = 0x0F87, ^ 0x0000 = 0x0F87
        assert_eq!(crc_empty, 0x0F87);
    }

    #[test]
    fn parse_heartbeat_frame() {
        // HEARTBEAT payload: custom_mode(4) + type(1) + autopilot(1) + base_mode(1) + system_status(1) + mavlink_version(1) = 9 bytes
        let mut payload = [0u8; 9];
        payload[0..4].copy_from_slice(&0u32.to_le_bytes()); // custom_mode
        payload[4] = 2; // type = MAV_TYPE_QUADROTOR
        payload[5] = 3; // autopilot = MAV_AUTOPILOT_ARDUPILOTMEGA
        payload[6] = 0; // base_mode
        payload[7] = 4; // system_status = MAV_STATE_ACTIVE
        payload[8] = 3; // mavlink_version

        let frame = build_frame(MSG_ID_HEARTBEAT, &payload, 0, 1, 1);
        let (msg, consumed) = parse_mavlink_frame(&frame).expect("parse heartbeat");
        assert_eq!(consumed, frame.len());
        match msg {
            MavlinkMessage::Heartbeat {
                autopilot,
                mav_type,
                system_status,
            } => {
                assert_eq!(mav_type, 2);
                assert_eq!(autopilot, 3);
                assert_eq!(system_status, 4);
            }
            _ => panic!("expected Heartbeat"),
        }
    }

    #[test]
    fn parse_attitude_floats() {
        // ATTITUDE: time_boot_ms(4) + roll(4) + pitch(4) + yaw(4) + rollspeed(4) + pitchspeed(4) + yawspeed(4) = 28 bytes
        let mut payload = [0u8; 28];
        payload[0..4].copy_from_slice(&1000u32.to_le_bytes());
        payload[4..8].copy_from_slice(&0.1f32.to_le_bytes());
        payload[8..12].copy_from_slice(&(-0.05f32).to_le_bytes());
        payload[12..16].copy_from_slice(&1.57f32.to_le_bytes());
        payload[16..20].copy_from_slice(&0.01f32.to_le_bytes());
        payload[20..24].copy_from_slice(&(-0.02f32).to_le_bytes());
        payload[24..28].copy_from_slice(&0.005f32.to_le_bytes());

        let frame = build_frame(MSG_ID_ATTITUDE, &payload, 1, 1, 1);
        let (msg, _) = parse_mavlink_frame(&frame).expect("parse attitude");
        match msg {
            MavlinkMessage::Attitude {
                roll,
                pitch,
                yaw,
                rollspeed,
                pitchspeed,
                yawspeed,
            } => {
                assert!((roll - 0.1).abs() < 1e-6);
                assert!((pitch - (-0.05)).abs() < 1e-6);
                assert!((yaw - 1.57).abs() < 1e-6);
                assert!((rollspeed - 0.01).abs() < 1e-6);
                assert!((pitchspeed - (-0.02)).abs() < 1e-6);
                assert!((yawspeed - 0.005).abs() < 1e-6);
            }
            _ => panic!("expected Attitude"),
        }
    }

    #[test]
    fn parse_global_position_int() {
        // 28 bytes payload
        let mut payload = [0u8; 28];
        payload[0..4].copy_from_slice(&2000u32.to_le_bytes()); // time
        payload[4..8].copy_from_slice(&557_532_150i32.to_le_bytes()); // lat 55.7532150
        payload[8..12].copy_from_slice(&376_225_040i32.to_le_bytes()); // lon 37.6225040
        payload[12..16].copy_from_slice(&150_000i32.to_le_bytes()); // alt 150m MSL
        payload[16..20].copy_from_slice(&45_500i32.to_le_bytes()); // rel_alt 45.5m
        payload[20..22].copy_from_slice(&100i16.to_le_bytes()); // vx 1.0 m/s N
        payload[22..24].copy_from_slice(&(-50i16).to_le_bytes()); // vy -0.5 m/s E
        payload[24..26].copy_from_slice(&(-10i16).to_le_bytes()); // vz -0.1 m/s D
        payload[26..28].copy_from_slice(&27000u16.to_le_bytes()); // hdg 270.00 deg

        let frame = build_frame(MSG_ID_GLOBAL_POSITION_INT, &payload, 2, 1, 1);
        let (msg, _) = parse_mavlink_frame(&frame).expect("parse gpi");
        match msg {
            MavlinkMessage::GlobalPositionInt {
                lat,
                lon,
                alt,
                relative_alt,
                vx,
                vy,
                vz,
                hdg,
            } => {
                assert_eq!(lat, 557_532_150);
                assert_eq!(lon, 376_225_040);
                assert_eq!(alt, 150_000);
                assert_eq!(relative_alt, 45_500);
                assert_eq!(vx, 100);
                assert_eq!(vy, -50);
                assert_eq!(vz, -10);
                assert_eq!(hdg, 27000);
            }
            _ => panic!("expected GlobalPositionInt"),
        }
    }

    #[test]
    fn streaming_parser_partial_feed() {
        let mut payload = [0u8; 9];
        payload[4] = 6; // MAV_TYPE_GCS
        payload[5] = 8; // autopilot
        payload[7] = 3; // system_status

        let frame = build_frame(MSG_ID_HEARTBEAT, &payload, 42, 255, 0);

        let mut parser = MavlinkParser::new();

        // Feed first half — should yield nothing
        let mid = frame.len() / 2;
        let msgs = parser.feed(&frame[..mid]);
        assert!(msgs.is_empty(), "partial feed should yield no messages");

        // Feed second half — should yield exactly one message
        let msgs = parser.feed(&frame[mid..]);
        assert_eq!(
            msgs.len(),
            1,
            "completing the frame should yield one message"
        );
        match &msgs[0] {
            MavlinkMessage::Heartbeat {
                mav_type,
                autopilot,
                system_status,
            } => {
                assert_eq!(*mav_type, 6);
                assert_eq!(*autopilot, 8);
                assert_eq!(*system_status, 3);
            }
            _ => panic!("expected Heartbeat"),
        }
    }

    #[test]
    fn unknown_message_id_passthrough() {
        let payload = [0xAA, 0xBB, 0xCC];
        let frame = build_frame(9999, &payload, 0, 1, 1);
        let (msg, consumed) = parse_mavlink_frame(&frame).expect("parse unknown");
        assert_eq!(consumed, frame.len());
        match msg {
            MavlinkMessage::Unknown { msgid, payload: p } => {
                assert_eq!(msgid, 9999);
                assert_eq!(p, vec![0xAA, 0xBB, 0xCC]);
            }
            _ => panic!("expected Unknown"),
        }
    }

    #[test]
    fn crc_mismatch_returns_error() {
        let frame = build_frame(MSG_ID_HEARTBEAT, &[0u8; 9], 0, 1, 1);
        let mut corrupted = frame.clone();
        // Flip a bit in the payload
        corrupted[10] ^= 0xFF;
        assert!(parse_mavlink_frame(&corrupted).is_err());
    }

    #[test]
    fn telemetry_from_sys_status() {
        let msg = MavlinkMessage::SysStatus {
            voltage_battery: 12400, // 12.4 V
            current_battery: 210,   // 2.1 A
            battery_remaining: 75,
        };
        let update = telemetry_from_mavlink(&msg).expect("should map sys_status");
        match update {
            TelemetryUpdate::Battery { voltage, current } => {
                assert!((voltage - 12.4).abs() < 0.01);
                assert!((current - 2.1).abs() < 0.01);
            }
            _ => panic!("expected Battery update"),
        }
    }

    #[test]
    fn telemetry_from_global_position() {
        let msg = MavlinkMessage::GlobalPositionInt {
            lat: 557_532_150,
            lon: 376_225_040,
            alt: 150_000,
            relative_alt: 45_500,
            vx: 0,
            vy: 0,
            vz: 0,
            hdg: 0,
        };
        let update = telemetry_from_mavlink(&msg).expect("should map gpi");
        match update {
            TelemetryUpdate::Position { lat, lon, alt_m } => {
                assert!((lat - 55.753215).abs() < 1e-6);
                assert!((lon - 37.622504).abs() < 1e-6);
                assert!((alt_m - 45.5).abs() < 0.01);
            }
            _ => panic!("expected Position update"),
        }
    }

    #[test]
    fn telemetry_from_attitude() {
        let msg = MavlinkMessage::Attitude {
            roll: 0.0,
            pitch: 0.0,
            yaw: -std::f32::consts::FRAC_PI_2, // -90 deg -> 270 deg
            rollspeed: 0.0,
            pitchspeed: 0.0,
            yawspeed: 0.0,
        };
        let update = telemetry_from_mavlink(&msg).expect("should map attitude");
        match update {
            TelemetryUpdate::Attitude { heading_deg } => {
                assert!((heading_deg - 270.0).abs() < 0.1);
            }
            _ => panic!("expected Attitude update"),
        }
    }

    #[test]
    fn apply_update_modifies_telemetry() {
        let mut td = TelemetryData {
            battery_voltage: 0.0,
            battery_current: 0.0,
            altitude_m: 0.0,
            speed_ms: 0.0,
            lat: 0.0,
            lon: 0.0,
            heading_deg: 0.0,
            ai_detections: 0,
        };
        apply_telemetry_update(
            &mut td,
            &TelemetryUpdate::Battery {
                voltage: 11.1,
                current: 3.5,
            },
        );
        assert!((td.battery_voltage - 11.1).abs() < 0.01);
        assert!((td.battery_current - 3.5).abs() < 0.01);
    }

    #[test]
    fn streaming_parser_multiple_frames() {
        let frame1 = build_frame(MSG_ID_HEARTBEAT, &[0u8; 9], 0, 1, 1);
        let frame2 = build_frame(MSG_ID_HEARTBEAT, &[0u8; 9], 1, 1, 1);
        let mut combined = Vec::new();
        combined.extend_from_slice(&frame1);
        combined.extend_from_slice(&frame2);

        let mut parser = MavlinkParser::new();
        let msgs = parser.feed(&combined);
        assert_eq!(msgs.len(), 2);
    }

    #[test]
    fn streaming_parser_garbage_before_frame() {
        let frame = build_frame(MSG_ID_HEARTBEAT, &[0u8; 9], 0, 1, 1);
        let mut data = vec![0xAA, 0xBB, 0xCC]; // garbage
        data.extend_from_slice(&frame);

        let mut parser = MavlinkParser::new();
        let msgs = parser.feed(&data);
        assert_eq!(msgs.len(), 1);
    }
}