#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct NaviFilterCovPayload {
    pub p0_hpos: f32,
    pub p0_vpos: f32,
    pub p0_hvel: f32,
    pub p0_vvel: f32,
    pub p0_euler_tilt: f32,
    pub p0_euler_yaw: f32,
    pub p0_acc_bias: f32,
    pub p0_gyro_bias: f32,
    pub p0_mag_ned: f32,
    pub p0_mag_bias: f32,
    pub cov_diag_size: u32,
    pub cov_diag_slot_000: f32,
    pub cov_diag_slot_001: f32,
    pub cov_diag_slot_002: f32,
    pub cov_diag_slot_003: f32,
    pub cov_diag_slot_004: f32,
    pub cov_diag_slot_005: f32,
    pub cov_diag_slot_006: f32,
    pub cov_diag_slot_007: f32,
    pub cov_diag_slot_008: f32,
    pub cov_diag_slot_009: f32,
    pub cov_diag_slot_010: f32,
    pub cov_diag_slot_011: f32,
    pub cov_diag_slot_012: f32,
    pub cov_diag_slot_013: f32,
    pub cov_diag_slot_014: f32,
    pub cov_diag_slot_015: f32,
    pub cov_diag_slot_016: f32,
    pub cov_diag_slot_017: f32,
    pub cov_diag_slot_018: f32,
    pub cov_diag_slot_019: f32,
    pub cov_diag_slot_020: f32,
    pub cov_diag_slot_021: f32,
}

impl NaviFilterCovPayload {
    pub const SYSTEM_ID: u8 = 7;
    pub const MODULE_ID: u8 = 2;
    pub const LEN: usize = 132;

    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<NaviFilterCovPayload, crate::gen::payload_io::PayloadError> {
        Self::validate_payload(payload)?;
        let mut cursor = crate::gen::payload_io::PayloadCursor::new(payload);
        Ok(Self {
            p0_hpos: cursor.read_f32(),
            p0_vpos: cursor.read_f32(),
            p0_hvel: cursor.read_f32(),
            p0_vvel: cursor.read_f32(),
            p0_euler_tilt: cursor.read_f32(),
            p0_euler_yaw: cursor.read_f32(),
            p0_acc_bias: cursor.read_f32(),
            p0_gyro_bias: cursor.read_f32(),
            p0_mag_ned: cursor.read_f32(),
            p0_mag_bias: cursor.read_f32(),
            cov_diag_size: cursor.read_u32(),
            cov_diag_slot_000: cursor.read_f32(),
            cov_diag_slot_001: cursor.read_f32(),
            cov_diag_slot_002: cursor.read_f32(),
            cov_diag_slot_003: cursor.read_f32(),
            cov_diag_slot_004: cursor.read_f32(),
            cov_diag_slot_005: cursor.read_f32(),
            cov_diag_slot_006: cursor.read_f32(),
            cov_diag_slot_007: cursor.read_f32(),
            cov_diag_slot_008: cursor.read_f32(),
            cov_diag_slot_009: cursor.read_f32(),
            cov_diag_slot_010: cursor.read_f32(),
            cov_diag_slot_011: cursor.read_f32(),
            cov_diag_slot_012: cursor.read_f32(),
            cov_diag_slot_013: cursor.read_f32(),
            cov_diag_slot_014: cursor.read_f32(),
            cov_diag_slot_015: cursor.read_f32(),
            cov_diag_slot_016: cursor.read_f32(),
            cov_diag_slot_017: cursor.read_f32(),
            cov_diag_slot_018: cursor.read_f32(),
            cov_diag_slot_019: cursor.read_f32(),
            cov_diag_slot_020: cursor.read_f32(),
            cov_diag_slot_021: cursor.read_f32(),
        })
    }

    pub fn from_bytes_exact(payload: &[u8; Self::LEN]) -> NaviFilterCovPayload {
        // 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.p0_hpos);
        writer.write_f32(self.p0_vpos);
        writer.write_f32(self.p0_hvel);
        writer.write_f32(self.p0_vvel);
        writer.write_f32(self.p0_euler_tilt);
        writer.write_f32(self.p0_euler_yaw);
        writer.write_f32(self.p0_acc_bias);
        writer.write_f32(self.p0_gyro_bias);
        writer.write_f32(self.p0_mag_ned);
        writer.write_f32(self.p0_mag_bias);
        writer.write_u32(self.cov_diag_size);
        writer.write_f32(self.cov_diag_slot_000);
        writer.write_f32(self.cov_diag_slot_001);
        writer.write_f32(self.cov_diag_slot_002);
        writer.write_f32(self.cov_diag_slot_003);
        writer.write_f32(self.cov_diag_slot_004);
        writer.write_f32(self.cov_diag_slot_005);
        writer.write_f32(self.cov_diag_slot_006);
        writer.write_f32(self.cov_diag_slot_007);
        writer.write_f32(self.cov_diag_slot_008);
        writer.write_f32(self.cov_diag_slot_009);
        writer.write_f32(self.cov_diag_slot_010);
        writer.write_f32(self.cov_diag_slot_011);
        writer.write_f32(self.cov_diag_slot_012);
        writer.write_f32(self.cov_diag_slot_013);
        writer.write_f32(self.cov_diag_slot_014);
        writer.write_f32(self.cov_diag_slot_015);
        writer.write_f32(self.cov_diag_slot_016);
        writer.write_f32(self.cov_diag_slot_017);
        writer.write_f32(self.cov_diag_slot_018);
        writer.write_f32(self.cov_diag_slot_019);
        writer.write_f32(self.cov_diag_slot_020);
        writer.write_f32(self.cov_diag_slot_021);
        buf
    }
}
