#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)]
pub struct NaviConfigPayload {
    pub ahrs_ekf_rate: u16,
    pub ins_ekf_rate: u16,
    pub imu_mount: u8,
    pub mag_mount: u8,
    pub mag_disable_gravity_fusion: u8,
    pub mag_disable_barometer_fusion: u8,
    pub mag_disable_mag_3d_fusion: u8,
    pub mag_disable_mag_decl_fusion: u8,
    pub mag_disable_gnss_vel_fusion: u8,
    pub mag_disable_gnss_pos_fusion: u8,
    pub mag_disable_rsvd_1: u8,
    pub mag_disable_rsvd_2: u8,
    pub mag_disable_rsvd_3: u8,
    pub mag_disable_rsvd_4: u8,
    pub static_state_acc_stdv: f32,
    pub static_state_gyro_stdv: f32,
    pub imu_sampling_time_ms: i64,
    pub mag_sampling_time_ms: i64,
    pub baro_sampling_time_ms: i64,
    pub acc_lpf_cutoff_freq: f32,
    pub gyro_lpf_cutoff_freq: f32,
    pub mag_lpf_cutoff_freq: f32,
    pub baro_lpf_cutoff_freq: f32,
    pub mag_decl_rad: f32,
    pub mag_norm: f32,
    pub home_lat: f64,
    pub home_lon: f64,
}

impl NaviConfigPayload {
    pub const SYSTEM_ID: u8 = 7;
    pub const MODULE_ID: u8 = 0;
    pub const LEN: usize = 88;

    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<NaviConfigPayload, crate::gen::payload_io::PayloadError> {
        Self::validate_payload(payload)?;
        let mut cursor = crate::gen::payload_io::PayloadCursor::new(payload);
        Ok(Self {
            ahrs_ekf_rate: cursor.read_u16(),
            ins_ekf_rate: cursor.read_u16(),
            imu_mount: cursor.read_u8(),
            mag_mount: cursor.read_u8(),
            mag_disable_gravity_fusion: cursor.read_u8(),
            mag_disable_barometer_fusion: cursor.read_u8(),
            mag_disable_mag_3d_fusion: cursor.read_u8(),
            mag_disable_mag_decl_fusion: cursor.read_u8(),
            mag_disable_gnss_vel_fusion: cursor.read_u8(),
            mag_disable_gnss_pos_fusion: cursor.read_u8(),
            mag_disable_rsvd_1: cursor.read_u8(),
            mag_disable_rsvd_2: cursor.read_u8(),
            mag_disable_rsvd_3: cursor.read_u8(),
            mag_disable_rsvd_4: cursor.read_u8(),
            static_state_acc_stdv: cursor.read_f32(),
            static_state_gyro_stdv: cursor.read_f32(),
            imu_sampling_time_ms: cursor.read_i64(),
            mag_sampling_time_ms: cursor.read_i64(),
            baro_sampling_time_ms: cursor.read_i64(),
            acc_lpf_cutoff_freq: cursor.read_f32(),
            gyro_lpf_cutoff_freq: cursor.read_f32(),
            mag_lpf_cutoff_freq: cursor.read_f32(),
            baro_lpf_cutoff_freq: cursor.read_f32(),
            mag_decl_rad: cursor.read_f32(),
            mag_norm: cursor.read_f32(),
            home_lat: cursor.read_f64(),
            home_lon: cursor.read_f64(),
        })
    }

    pub fn from_bytes_exact(payload: &[u8; Self::LEN]) -> NaviConfigPayload {
        // 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.ahrs_ekf_rate);
        writer.write_u16(self.ins_ekf_rate);
        writer.write_u8(self.imu_mount);
        writer.write_u8(self.mag_mount);
        writer.write_u8(self.mag_disable_gravity_fusion);
        writer.write_u8(self.mag_disable_barometer_fusion);
        writer.write_u8(self.mag_disable_mag_3d_fusion);
        writer.write_u8(self.mag_disable_mag_decl_fusion);
        writer.write_u8(self.mag_disable_gnss_vel_fusion);
        writer.write_u8(self.mag_disable_gnss_pos_fusion);
        writer.write_u8(self.mag_disable_rsvd_1);
        writer.write_u8(self.mag_disable_rsvd_2);
        writer.write_u8(self.mag_disable_rsvd_3);
        writer.write_u8(self.mag_disable_rsvd_4);
        writer.write_f32(self.static_state_acc_stdv);
        writer.write_f32(self.static_state_gyro_stdv);
        writer.write_i64(self.imu_sampling_time_ms);
        writer.write_i64(self.mag_sampling_time_ms);
        writer.write_i64(self.baro_sampling_time_ms);
        writer.write_f32(self.acc_lpf_cutoff_freq);
        writer.write_f32(self.gyro_lpf_cutoff_freq);
        writer.write_f32(self.mag_lpf_cutoff_freq);
        writer.write_f32(self.baro_lpf_cutoff_freq);
        writer.write_f32(self.mag_decl_rad);
        writer.write_f32(self.mag_norm);
        writer.write_f64(self.home_lat);
        writer.write_f64(self.home_lon);
        buf
    }
}
