#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct NaviFilterPayload {
    pub p_min_quat: f32,
    pub p_max_quat: f32,
    pub p_min_vel: f32,
    pub p_max_vel: f32,
    pub p_min_pos: f32,
    pub p_max_pos: f32,
    pub p_min_ba: f32,
    pub p_max_ba: f32,
    pub p_min_bg: f32,
    pub p_max_bg: f32,
    pub p_min_mag: f32,
    pub p_max_mag: f32,
    pub p_min_bm: f32,
    pub p_max_bm: f32,
    pub x_min_quat: f32,
    pub x_min_vel: f32,
    pub x_max_vel: f32,
    pub x_min_pos: f32,
    pub x_max_pos: f32,
    pub x_min_ba: f32,
    pub x_max_ba: f32,
    pub x_min_bg: f32,
    pub x_max_bg: f32,
    pub x_min_mag: f32,
    pub x_max_mag: f32,
    pub x_min_bm: f32,
    pub x_max_bm: f32,
    pub r_grav: f32,
    pub r_baro: f32,
    pub r_mag: f32,
    pub r_mag_decl: f32,
    pub r_gps_vel: f32,
    pub r_gps_pos: f32,
    pub innov_gate_grav: f32,
    pub innov_gate_baro: f32,
    pub innov_gate_mag: f32,
    pub innov_gate_mag_decl: f32,
    pub innov_gate_gps_vel: f32,
    pub innov_gate_gps_pos: f32,
}

impl NaviFilterPayload {
    pub const SYSTEM_ID: u8 = 7;
    pub const MODULE_ID: u8 = 1;
    pub const LEN: usize = 156;

    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<NaviFilterPayload, crate::gen::payload_io::PayloadError> {
        Self::validate_payload(payload)?;
        let mut cursor = crate::gen::payload_io::PayloadCursor::new(payload);
        Ok(Self {
            p_min_quat: cursor.read_f32(),
            p_max_quat: cursor.read_f32(),
            p_min_vel: cursor.read_f32(),
            p_max_vel: cursor.read_f32(),
            p_min_pos: cursor.read_f32(),
            p_max_pos: cursor.read_f32(),
            p_min_ba: cursor.read_f32(),
            p_max_ba: cursor.read_f32(),
            p_min_bg: cursor.read_f32(),
            p_max_bg: cursor.read_f32(),
            p_min_mag: cursor.read_f32(),
            p_max_mag: cursor.read_f32(),
            p_min_bm: cursor.read_f32(),
            p_max_bm: cursor.read_f32(),
            x_min_quat: cursor.read_f32(),
            x_min_vel: cursor.read_f32(),
            x_max_vel: cursor.read_f32(),
            x_min_pos: cursor.read_f32(),
            x_max_pos: cursor.read_f32(),
            x_min_ba: cursor.read_f32(),
            x_max_ba: cursor.read_f32(),
            x_min_bg: cursor.read_f32(),
            x_max_bg: cursor.read_f32(),
            x_min_mag: cursor.read_f32(),
            x_max_mag: cursor.read_f32(),
            x_min_bm: cursor.read_f32(),
            x_max_bm: cursor.read_f32(),
            r_grav: cursor.read_f32(),
            r_baro: cursor.read_f32(),
            r_mag: cursor.read_f32(),
            r_mag_decl: cursor.read_f32(),
            r_gps_vel: cursor.read_f32(),
            r_gps_pos: cursor.read_f32(),
            innov_gate_grav: cursor.read_f32(),
            innov_gate_baro: cursor.read_f32(),
            innov_gate_mag: cursor.read_f32(),
            innov_gate_mag_decl: cursor.read_f32(),
            innov_gate_gps_vel: cursor.read_f32(),
            innov_gate_gps_pos: cursor.read_f32(),
        })
    }

    pub fn from_bytes_exact(payload: &[u8; Self::LEN]) -> NaviFilterPayload {
        // 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_f32(self.p_min_quat);
        writer.write_f32(self.p_max_quat);
        writer.write_f32(self.p_min_vel);
        writer.write_f32(self.p_max_vel);
        writer.write_f32(self.p_min_pos);
        writer.write_f32(self.p_max_pos);
        writer.write_f32(self.p_min_ba);
        writer.write_f32(self.p_max_ba);
        writer.write_f32(self.p_min_bg);
        writer.write_f32(self.p_max_bg);
        writer.write_f32(self.p_min_mag);
        writer.write_f32(self.p_max_mag);
        writer.write_f32(self.p_min_bm);
        writer.write_f32(self.p_max_bm);
        writer.write_f32(self.x_min_quat);
        writer.write_f32(self.x_min_vel);
        writer.write_f32(self.x_max_vel);
        writer.write_f32(self.x_min_pos);
        writer.write_f32(self.x_max_pos);
        writer.write_f32(self.x_min_ba);
        writer.write_f32(self.x_max_ba);
        writer.write_f32(self.x_min_bg);
        writer.write_f32(self.x_max_bg);
        writer.write_f32(self.x_min_mag);
        writer.write_f32(self.x_max_mag);
        writer.write_f32(self.x_min_bm);
        writer.write_f32(self.x_max_bm);
        writer.write_f32(self.r_grav);
        writer.write_f32(self.r_baro);
        writer.write_f32(self.r_mag);
        writer.write_f32(self.r_mag_decl);
        writer.write_f32(self.r_gps_vel);
        writer.write_f32(self.r_gps_pos);
        writer.write_f32(self.innov_gate_grav);
        writer.write_f32(self.innov_gate_baro);
        writer.write_f32(self.innov_gate_mag);
        writer.write_f32(self.innov_gate_mag_decl);
        writer.write_f32(self.innov_gate_gps_vel);
        writer.write_f32(self.innov_gate_gps_pos);
        buf
    }
}
