//! MAVLink v2 frame encoder.
//!
//! The encoder owns the per-link outbound `tx_seq` counter and is the single
//! place that lays down the wire bytes.

use std::sync::atomic::{AtomicU8, Ordering};

use super::crc::frame_crc;
use super::messages::MavlinkMessage;
use super::{HEADER_LEN, MAVLINK_V2_STX};

#[derive(Debug)]
pub struct Encoder {
    sysid: u8,
    compid: u8,
    tx_seq: AtomicU8,
}

impl Encoder {
    pub fn new(sysid: u8, compid: u8) -> Self {
        Self {
            sysid,
            compid,
            tx_seq: AtomicU8::new(0),
        }
    }

    pub fn sysid(&self) -> u8 {
        self.sysid
    }

    pub fn compid(&self) -> u8 {
        self.compid
    }

    /// Encode `msg` into a self-contained MAVLink v2 frame on the wire.
    ///
    /// Trailing-zero payload bytes are truncated per the MAVLink spec. Each
    /// call advances the per-link tx sequence counter by 1 with wrap-around.
    pub fn encode(&self, msg: &MavlinkMessage) -> Vec<u8> {
        let mut full_payload = Vec::with_capacity(64);
        msg.encode_payload(&mut full_payload);

        let payload_len = trailing_zero_truncated_len(&full_payload);
        let msg_id = msg.msg_id();
        let seq = self.tx_seq.fetch_add(1, Ordering::Relaxed);

        let mut frame = Vec::with_capacity(HEADER_LEN + payload_len + 2);
        frame.push(MAVLINK_V2_STX);

        // Body that the CRC covers begins here.
        let body_start = frame.len();
        frame.push(payload_len as u8);
        frame.push(0); // incompat_flags (no signing in this task — AZ-643)
        frame.push(0); // compat_flags
        frame.push(seq);
        frame.push(self.sysid);
        frame.push(self.compid);
        frame.push((msg_id & 0xFF) as u8);
        frame.push(((msg_id >> 8) & 0xFF) as u8);
        frame.push(((msg_id >> 16) & 0xFF) as u8);
        frame.extend_from_slice(&full_payload[..payload_len]);

        let crc = frame_crc(&frame[body_start..], msg.crc_extra());
        frame.extend_from_slice(&crc.to_le_bytes());

        frame
    }
}

#[inline]
fn trailing_zero_truncated_len(payload: &[u8]) -> usize {
    let mut len = payload.len();
    while len > 1 && payload[len - 1] == 0 {
        len -= 1;
    }
    len
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::internal::codec::messages::Heartbeat;

    #[test]
    fn encoder_starts_at_seq_zero_and_increments() {
        // Arrange
        let enc = Encoder::new(1, 191);
        let m = MavlinkMessage::Heartbeat(Heartbeat {
            custom_mode: 1,
            mavtype: 2,
            autopilot: 3,
            base_mode: 4,
            system_status: 5,
            mavlink_version: 3,
        });

        // Act
        let f0 = enc.encode(&m);
        let f1 = enc.encode(&m);

        // Assert
        assert_eq!(f0[0], MAVLINK_V2_STX);
        assert_eq!(f0[4], 0, "first frame seq must be 0");
        assert_eq!(f1[4], 1, "second frame seq must be 1");
    }
}
