#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct ImuConfigPayload {
    pub accel_fsr: u16,
    pub gyro_range: u16,
    pub sample_rate: u16,
    pub use_hsi: u8,
    pub rsvd_1: u8,
    pub rsvd_2: u8,
    pub rsvd_3: u8,
    pub rsvd_4: u8,
    pub rsvd_5: u8,
    pub rsvd_6: u8,
    pub rsvd_7: u8,
    pub baro_use_i2c_aux_address: bool,
    pub mag_use_i2c_aux_address: bool,
}

impl ImuConfigPayload {
    pub const SYSTEM_ID: u8 = 1;
    pub const MODULE_ID: u8 = 0;
    pub const LEN: usize = 16;

    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<ImuConfigPayload, crate::gen::payload_io::PayloadError> {
        Self::validate_payload(payload)?;
        let mut cursor = crate::gen::payload_io::PayloadCursor::new(payload);
        Ok(Self {
            accel_fsr: cursor.read_u16(),
            gyro_range: cursor.read_u16(),
            sample_rate: cursor.read_u16(),
            use_hsi: cursor.read_u8(),
            rsvd_1: cursor.read_u8(),
            rsvd_2: cursor.read_u8(),
            rsvd_3: cursor.read_u8(),
            rsvd_4: cursor.read_u8(),
            rsvd_5: cursor.read_u8(),
            rsvd_6: cursor.read_u8(),
            rsvd_7: cursor.read_u8(),
            baro_use_i2c_aux_address: cursor.read_bool(),
            mag_use_i2c_aux_address: cursor.read_bool(),
        })
    }

    pub fn from_bytes_exact(payload: &[u8; Self::LEN]) -> ImuConfigPayload {
        // 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_u16(self.accel_fsr);
        writer.write_u16(self.gyro_range);
        writer.write_u16(self.sample_rate);
        writer.write_u8(self.use_hsi);
        writer.write_u8(self.rsvd_1);
        writer.write_u8(self.rsvd_2);
        writer.write_u8(self.rsvd_3);
        writer.write_u8(self.rsvd_4);
        writer.write_u8(self.rsvd_5);
        writer.write_u8(self.rsvd_6);
        writer.write_u8(self.rsvd_7);
        writer.write_bool(self.baro_use_i2c_aux_address);
        writer.write_bool(self.mag_use_i2c_aux_address);
        buf
    }
}
