#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct NaviOutputPayload {
    pub status: i32,
    pub timestamp: u32,
    pub lat: f64,
    pub lon: f64,
    pub alt: f32,
    pub pos_n: f32,
    pub pos_e: f32,
    pub pos_d: f32,
    pub vel_n: f32,
    pub vel_e: f32,
    pub vel_d: f32,
    pub acc_x: f32,
    pub acc_y: f32,
    pub acc_z: f32,
    pub mag_n: f32,
    pub mag_e: f32,
    pub mag_d: f32,
    pub yaw: f32,
    pub pitch: f32,
    pub roll: f32,
    pub qw: f32,
    pub qx: f32,
    pub qy: f32,
    pub qz: f32,
    pub ang_rate_x: f32,
    pub ang_rate_y: f32,
    pub ang_rate_z: f32,
    pub acc_bias_x: f32,
    pub acc_bias_y: f32,
    pub acc_bias_z: f32,
    pub gyro_bias_x: f32,
    pub gyro_bias_y: f32,
    pub gyro_bias_z: f32,
    pub mag_bias_x: f32,
    pub mag_bias_y: f32,
    pub mag_bias_z: f32,
}

impl NaviOutputPayload {
    pub const SYSTEM_ID: u8 = 7;
    pub const MODULE_ID: u8 = 15;
    pub const LEN: usize = 152;

    pub fn validate_payload(payload: &[u8]) -> Result<(), crate::gen::payload_io::PayloadError> {
        if payload.len() != Self::LEN {
            return Err(crate::gen::payload_io::PayloadError::LengthMismatch { expected: Self::LEN, actual: payload.len() });
        }
        Ok(())
    }

    pub fn parse(payload: &[u8]) -> Result<NaviOutputPayload, crate::gen::payload_io::PayloadError> {
        Self::validate_payload(payload)?;
        let mut cursor = crate::gen::payload_io::PayloadCursor::new(payload);
        Ok(Self {
            status: cursor.read_i32(),
            timestamp: cursor.read_u32(),
            lat: cursor.read_f64(),
            lon: cursor.read_f64(),
            alt: cursor.read_f32(),
            pos_n: cursor.read_f32(),
            pos_e: cursor.read_f32(),
            pos_d: cursor.read_f32(),
            vel_n: cursor.read_f32(),
            vel_e: cursor.read_f32(),
            vel_d: cursor.read_f32(),
            acc_x: cursor.read_f32(),
            acc_y: cursor.read_f32(),
            acc_z: cursor.read_f32(),
            mag_n: cursor.read_f32(),
            mag_e: cursor.read_f32(),
            mag_d: cursor.read_f32(),
            yaw: cursor.read_f32(),
            pitch: cursor.read_f32(),
            roll: cursor.read_f32(),
            qw: cursor.read_f32(),
            qx: cursor.read_f32(),
            qy: cursor.read_f32(),
            qz: cursor.read_f32(),
            ang_rate_x: cursor.read_f32(),
            ang_rate_y: cursor.read_f32(),
            ang_rate_z: cursor.read_f32(),
            acc_bias_x: cursor.read_f32(),
            acc_bias_y: cursor.read_f32(),
            acc_bias_z: cursor.read_f32(),
            gyro_bias_x: cursor.read_f32(),
            gyro_bias_y: cursor.read_f32(),
            gyro_bias_z: cursor.read_f32(),
            mag_bias_x: cursor.read_f32(),
            mag_bias_y: cursor.read_f32(),
            mag_bias_z: cursor.read_f32(),
        })
    }

    pub fn from_bytes_exact(payload: &[u8; Self::LEN]) -> NaviOutputPayload {
        // Length already guaranteed by the array type
        Self::parse(payload).expect("slice length matches payload size")
    }

    pub fn to_bytes(&self) -> [u8; Self::LEN] {
        let mut buf = [0u8; Self::LEN];
        let mut writer = crate::gen::payload_io::PayloadWriter::new(&mut buf);
        writer.write_i32(self.status);
        writer.write_u32(self.timestamp);
        writer.write_f64(self.lat);
        writer.write_f64(self.lon);
        writer.write_f32(self.alt);
        writer.write_f32(self.pos_n);
        writer.write_f32(self.pos_e);
        writer.write_f32(self.pos_d);
        writer.write_f32(self.vel_n);
        writer.write_f32(self.vel_e);
        writer.write_f32(self.vel_d);
        writer.write_f32(self.acc_x);
        writer.write_f32(self.acc_y);
        writer.write_f32(self.acc_z);
        writer.write_f32(self.mag_n);
        writer.write_f32(self.mag_e);
        writer.write_f32(self.mag_d);
        writer.write_f32(self.yaw);
        writer.write_f32(self.pitch);
        writer.write_f32(self.roll);
        writer.write_f32(self.qw);
        writer.write_f32(self.qx);
        writer.write_f32(self.qy);
        writer.write_f32(self.qz);
        writer.write_f32(self.ang_rate_x);
        writer.write_f32(self.ang_rate_y);
        writer.write_f32(self.ang_rate_z);
        writer.write_f32(self.acc_bias_x);
        writer.write_f32(self.acc_bias_y);
        writer.write_f32(self.acc_bias_z);
        writer.write_f32(self.gyro_bias_x);
        writer.write_f32(self.gyro_bias_y);
        writer.write_f32(self.gyro_bias_z);
        writer.write_f32(self.mag_bias_x);
        writer.write_f32(self.mag_bias_y);
        writer.write_f32(self.mag_bias_z);
        buf
    }
}
