use models::ccsds::{CcsdsTcPacketOwned, CcsdsTmPacketOwned};
use models::mgm::MgmData;
use models::pcdu::SwitchId;
use models::{mgm, ComponentId, HkRequestType};
use satrs::mode_tree::{ModeChild, ModeNode};
use satrs::spacepackets::CcsdsPacketIdAndPsc;
use satrs_example::{DeviceMode, TimestampHelper};
use satrs_minisim::acs::lis3mdl::{
    MgmLis3MdlReply, MgmLis3RawValues, FIELD_LSB_PER_GAUSS_4_SENS, GAUSS_TO_MICROTESLA_FACTOR,
};
use satrs_minisim::acs::MgmRequestLis3Mdl;
use satrs_minisim::{SerializableSimMsgPayload, SimReply, SimRequest};
use std::fmt::Debug;
use std::sync::mpsc::{self};
use std::sync::{Arc, Mutex};
use std::time::Duration;

use satrs::mode::{
    ModeAndSubmode, ModeError, ModeProvider, ModeReply, ModeRequestHandler,
    ModeRequestHandlerMpscBounded,
};
use satrs::request::{GenericMessage, MessageMetadata};
use satrs_example::config::components::NO_SENDER;

use crate::ccsds::pack_ccsds_tm_packet_for_now;
use crate::eps::PowerSwitchHelper;
use crate::spi::SpiInterface;

pub const NR_OF_DATA_AND_CFG_REGISTERS: usize = 14;

// Register adresses to access various bytes from the raw reply.
pub const X_LOWBYTE_IDX: usize = 9;
pub const Y_LOWBYTE_IDX: usize = 11;
pub const Z_LOWBYTE_IDX: usize = 13;

#[derive(Default, Debug, PartialEq, Eq)]
pub enum TransitionState {
    #[default]
    Idle,
    PowerSwitching,
    Done,
}

#[derive(Default)]
pub struct SpiDummyInterface {
    pub dummy_values: MgmLis3RawValues,
}

impl SpiInterface for SpiDummyInterface {
    type Error = ();

    fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
        rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.x.to_le_bytes());
        rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.y.to_be_bytes());
        rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2].copy_from_slice(&self.dummy_values.z.to_be_bytes());
        Ok(())
    }
}

pub struct SpiSimInterface {
    pub sim_request_tx: mpsc::Sender<SimRequest>,
    pub sim_reply_rx: mpsc::Receiver<SimReply>,
}

impl SpiInterface for SpiSimInterface {
    type Error = ();

    // Right now, we only support requesting sensor data and not configuration of the sensor.
    fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
        let mgm_sensor_request = MgmRequestLis3Mdl::RequestSensorData;
        if let Err(e) = self
            .sim_request_tx
            .send(SimRequest::new_with_epoch_time(mgm_sensor_request))
        {
            log::error!("failed to send MGM LIS3 request: {e}");
        }
        match self.sim_reply_rx.recv_timeout(Duration::from_millis(50)) {
            Ok(sim_reply) => {
                let sim_reply_lis3 = MgmLis3MdlReply::from_sim_message(&sim_reply)
                    .expect("failed to parse LIS3 reply");
                rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
                    .copy_from_slice(&sim_reply_lis3.raw.x.to_le_bytes());
                rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
                    .copy_from_slice(&sim_reply_lis3.raw.y.to_le_bytes());
                rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
                    .copy_from_slice(&sim_reply_lis3.raw.z.to_le_bytes());
            }
            Err(e) => {
                log::warn!("MGM LIS3 SIM reply timeout: {e}");
            }
        }
        Ok(())
    }
}

pub enum SpiSimInterfaceWrapper {
    Dummy(SpiDummyInterface),
    Sim(SpiSimInterface),
}

impl SpiInterface for SpiSimInterfaceWrapper {
    type Error = ();

    fn transfer(&mut self, tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
        match self {
            SpiSimInterfaceWrapper::Dummy(dummy) => dummy.transfer(tx, rx),
            SpiSimInterfaceWrapper::Sim(sim_if) => sim_if.transfer(tx, rx),
        }
    }
}

#[derive(Default)]
pub struct BufWrapper {
    tx_buf: [u8; 32],
    rx_buf: [u8; 32],
}

pub struct ModeHelpers {
    current: ModeAndSubmode,
    target: Option<ModeAndSubmode>,
    requestor_info: Option<MessageMetadata>,
    transition_state: TransitionState,
}

impl Default for ModeHelpers {
    fn default() -> Self {
        Self {
            current: ModeAndSubmode::new(DeviceMode::Off as u32, 0),
            target: Default::default(),
            requestor_info: Default::default(),
            transition_state: Default::default(),
        }
    }
}

/// Example MGM device handler strongly based on the LIS3MDL MEMS device.
#[allow(clippy::too_many_arguments)]
pub struct MgmHandlerLis3Mdl<ComInterface: SpiInterface> {
    id: ComponentId,
    dev_str: &'static str,
    mode_node: ModeRequestHandlerMpscBounded,
    tc_rx: mpsc::Receiver<CcsdsTcPacketOwned>,
    tm_tx: mpsc::SyncSender<CcsdsTmPacketOwned>,
    switch_helper: PowerSwitchHelper,
    pub com_interface: ComInterface,
    shared_mgm_set: Arc<Mutex<MgmData>>,
    //hk_helper: PusHkHelper,
    mode_helpers: ModeHelpers,
    bufs: BufWrapper,
    stamp_helper: TimestampHelper,
}

impl<ComInterface: SpiInterface> MgmHandlerLis3Mdl<ComInterface> {
    #[allow(clippy::too_many_arguments)]
    pub fn new(
        id: ComponentId,
        dev_str: &'static str,
        mode_node: ModeRequestHandlerMpscBounded,
        tc_rx: mpsc::Receiver<CcsdsTcPacketOwned>,
        tm_tx: mpsc::SyncSender<CcsdsTmPacketOwned>,
        switch_helper: PowerSwitchHelper,
        com_interface: ComInterface,
        shared_mgm_set: Arc<Mutex<MgmData>>,
    ) -> Self {
        Self {
            id,
            dev_str,
            mode_node,
            tc_rx,
            tm_tx,
            switch_helper,
            com_interface,
            shared_mgm_set,
            mode_helpers: ModeHelpers::default(),
            bufs: BufWrapper::default(),
            stamp_helper: TimestampHelper::default(),
        }
    }
    pub fn periodic_operation(&mut self) {
        self.stamp_helper.update_from_now();
        // Handle requests.
        self.handle_telecommands();
        self.handle_mode_requests();
        if let Some(target_mode_submode) = self.mode_helpers.target {
            self.handle_mode_transition(target_mode_submode);
        }
        if self.mode() == DeviceMode::Normal as u32 {
            log::trace!("polling LIS3MDL sensor {}", self.dev_str);
            self.poll_sensor();
        }
    }

    pub fn handle_telecommands(&mut self) {
        loop {
            match self.tc_rx.try_recv() {
                Ok(packet) => {
                    let tc_id = CcsdsPacketIdAndPsc::new_from_ccsds_packet(&packet.sp_header);
                    match postcard::from_bytes::<mgm::request::Request>(&packet.payload) {
                        Ok(request) => {
                            log::info!(
                                "received request {:?} with TC ID {:#010x}",
                                request,
                                tc_id.raw()
                            );
                            match request {
                                mgm::request::Request::Ping => {
                                    self.send_telemetry(Some(tc_id), mgm::response::Response::Ok)
                                }
                                mgm::request::Request::Hk(hk_request) => {
                                    self.handle_hk_request(Some(tc_id), &hk_request)
                                } //mgm::request::Request::Mo
                            }
                        }
                        Err(e) => {
                            log::warn!("failed to deserialize request: {}", e);
                        }
                    }
                }
                Err(e) => match e {
                    std::sync::mpsc::TryRecvError::Empty => break,
                    std::sync::mpsc::TryRecvError::Disconnected => {
                        log::warn!("packet sender disconnected")
                    }
                },
            }
        }
    }

    pub fn send_telemetry(
        &self,
        tc_id: Option<CcsdsPacketIdAndPsc>,
        response: mgm::response::Response,
    ) {
        match pack_ccsds_tm_packet_for_now(self.id, tc_id, &response) {
            Ok(packet) => {
                if let Err(e) = self.tm_tx.send(packet) {
                    log::warn!("failed to send TM packet: {}", e);
                }
            }
            Err(e) => {
                log::warn!("failed to pack TM packet: {}", e);
            }
        }
    }

    pub fn handle_hk_request(
        &mut self,
        tc_id: Option<CcsdsPacketIdAndPsc>,
        hk_request: &models::mgm::request::HkRequest,
    ) {
        match hk_request.req_type {
            HkRequestType::OneShot => {
                let mgm_snapshot = *self.shared_mgm_set.lock().unwrap();
                self.send_telemetry(
                    tc_id,
                    mgm::response::Response::Hk(mgm::response::HkResponse::MgmData(mgm_snapshot)),
                )
            }
            HkRequestType::EnablePeriodic(_duration) => todo!(),
            HkRequestType::DisablePeriodic => todo!(),
            HkRequestType::ModifyInterval(_duration) => todo!(),
            _ => todo!(),
        }
    }

    pub fn handle_mode_requests(&mut self) {
        loop {
            // TODO: Only allow one set mode request per cycle?
            match self.mode_node.try_recv_mode_request() {
                Ok(opt_msg) => {
                    if let Some(msg) = opt_msg {
                        let result = self.handle_mode_request(msg);
                        // TODO: Trigger event?
                        if result.is_err() {
                            log::warn!(
                                "{}: mode request failed with error {:?}",
                                self.dev_str,
                                result.err().unwrap()
                            );
                        }
                    } else {
                        break;
                    }
                }
                Err(e) => match e {
                    satrs::queue::GenericReceiveError::Empty => break,
                    satrs::queue::GenericReceiveError::TxDisconnected(e) => {
                        log::warn!("{}: failed to receive mode request: {:?}", self.dev_str, e);
                    }
                },
            }
        }
    }

    pub fn poll_sensor(&mut self) {
        // Communicate with the device. This is actually how to read the data from the LIS3 device
        // SPI interface.
        self.com_interface
            .transfer(
                &self.bufs.tx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
                &mut self.bufs.rx_buf[0..NR_OF_DATA_AND_CFG_REGISTERS + 1],
            )
            .expect("failed to transfer data");
        let x_raw = i16::from_le_bytes(
            self.bufs.rx_buf[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
                .try_into()
                .unwrap(),
        );
        let y_raw = i16::from_le_bytes(
            self.bufs.rx_buf[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
                .try_into()
                .unwrap(),
        );
        let z_raw = i16::from_le_bytes(
            self.bufs.rx_buf[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
                .try_into()
                .unwrap(),
        );
        // Simple scaling to retrieve the float value, assuming the best sensor resolution.
        let mut mgm_guard = self.shared_mgm_set.lock().unwrap();
        mgm_guard.x = x_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        mgm_guard.y = y_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        mgm_guard.z = z_raw as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        mgm_guard.valid = true;
        drop(mgm_guard);
    }

    pub fn handle_mode_transition(&mut self, target_mode_submode: ModeAndSubmode) {
        if target_mode_submode.mode() == DeviceMode::On as u32
            || target_mode_submode.mode() == DeviceMode::Normal as u32
        {
            if self.mode_helpers.transition_state == TransitionState::Idle {
                let result = self
                    .switch_helper
                    .send_switch_on_cmd(MessageMetadata::new(0, self.id as u32), SwitchId::Mgm0);
                if result.is_err() {
                    // Could not send switch command.. still continue with transition.
                    log::error!("failed to send switch on command");
                }
                self.mode_helpers.transition_state = TransitionState::PowerSwitching;
            }
            if self.mode_helpers.transition_state == TransitionState::PowerSwitching
                && self.switch_helper.is_switch_on(SwitchId::Mgm0)
            {
                self.mode_helpers.transition_state = TransitionState::Done;
            }
            if self.mode_helpers.transition_state == TransitionState::Done {
                self.mode_helpers.current = self.mode_helpers.target.unwrap();
                self.handle_mode_reached(self.mode_helpers.requestor_info)
                    .expect("failed to handle mode reached");
                self.mode_helpers.transition_state = TransitionState::Idle;
            }
        }
    }
}

impl<ComInterface: SpiInterface> ModeProvider for MgmHandlerLis3Mdl<ComInterface> {
    fn mode_and_submode(&self) -> ModeAndSubmode {
        self.mode_helpers.current
    }
}

impl<ComInterface: SpiInterface> ModeRequestHandler for MgmHandlerLis3Mdl<ComInterface> {
    type Error = ModeError;

    fn start_transition(
        &mut self,
        requestor: MessageMetadata,
        mode_and_submode: ModeAndSubmode,
        _forced: bool,
    ) -> Result<(), satrs::mode::ModeError> {
        log::info!(
            "{}: transitioning to mode {:?}",
            self.dev_str,
            mode_and_submode
        );
        self.mode_helpers.current = mode_and_submode;
        if mode_and_submode.mode() == DeviceMode::Off as u32 {
            self.shared_mgm_set.lock().unwrap().valid = false;
            self.handle_mode_reached(Some(requestor))?;
        } else if mode_and_submode.mode() == DeviceMode::Normal as u32
            || mode_and_submode.mode() == DeviceMode::On as u32
        {
            // TODO: Write helper method for the struct? Might help for other handlers as well..
            self.mode_helpers.transition_state = TransitionState::Idle;
            self.mode_helpers.requestor_info = Some(requestor);
            self.mode_helpers.target = Some(mode_and_submode);
        }
        Ok(())
    }

    fn announce_mode(&self, _requestor_info: Option<MessageMetadata>, _recursive: bool) {
        log::info!(
            "{} announcing mode: {:?}",
            self.dev_str,
            self.mode_and_submode()
        );
    }

    fn handle_mode_reached(
        &mut self,
        requestor: Option<MessageMetadata>,
    ) -> Result<(), Self::Error> {
        self.mode_helpers.target = None;
        self.announce_mode(requestor, false);
        if let Some(requestor) = requestor {
            if requestor.sender_id() == NO_SENDER {
                return Ok(());
            }
            if requestor.sender_id() != ComponentId::Ground as u32 {
                log::warn!(
                    "can not send back mode reply to sender {:x}",
                    requestor.sender_id()
                );
            } else {
                self.send_mode_reply(requestor, ModeReply::ModeReply(self.mode_and_submode()))?;
            }
        }
        Ok(())
    }

    fn send_mode_reply(
        &self,
        requestor: MessageMetadata,
        reply: ModeReply,
    ) -> Result<(), Self::Error> {
        if requestor.sender_id() != ComponentId::Ground as u32 {
            log::warn!(
                "can not send back mode reply to sender {}",
                requestor.sender_id()
            );
        }
        self.mode_node
            .send_mode_reply(requestor, reply)
            .map_err(ModeError::Send)?;
        Ok(())
    }

    fn handle_mode_info(
        &mut self,
        _requestor_info: MessageMetadata,
        _info: ModeAndSubmode,
    ) -> Result<(), Self::Error> {
        Ok(())
    }
}

impl<ComInterface: SpiInterface> ModeNode for MgmHandlerLis3Mdl<ComInterface> {
    fn id(&self) -> satrs::ComponentId {
        self.id as u32
    }
}

impl<ComInterface: SpiInterface> ModeChild for MgmHandlerLis3Mdl<ComInterface> {
    type Sender = mpsc::SyncSender<GenericMessage<ModeReply>>;

    fn add_mode_parent(&mut self, id: satrs::ComponentId, reply_sender: Self::Sender) {
        self.mode_node.add_message_target(id, reply_sender);
    }
}

#[cfg(test)]
mod tests {
    use std::{
        collections::HashMap,
        sync::{mpsc, Arc},
    };

    use models::{
        pcdu::{SwitchRequest, SwitchState, SwitchStateBinary},
        ComponentId,
    };
    use satrs::{
        mode::{ModeReply, ModeRequest},
        mode_tree::ModeParent,
        request::GenericMessage,
        tmtc::PacketAsVec,
    };
    use satrs_minisim::acs::lis3mdl::MgmLis3RawValues;

    use crate::eps::pcdu::{SharedSwitchSet, SwitchMap, SwitchSet};

    use super::*;

    #[derive(Default)]
    pub struct TestSpiInterface {
        pub call_count: u32,
        pub next_mgm_data: MgmLis3RawValues,
    }

    impl SpiInterface for TestSpiInterface {
        type Error = ();

        fn transfer(&mut self, _tx: &[u8], rx: &mut [u8]) -> Result<(), Self::Error> {
            rx[X_LOWBYTE_IDX..X_LOWBYTE_IDX + 2]
                .copy_from_slice(&self.next_mgm_data.x.to_le_bytes());
            rx[Y_LOWBYTE_IDX..Y_LOWBYTE_IDX + 2]
                .copy_from_slice(&self.next_mgm_data.y.to_le_bytes());
            rx[Z_LOWBYTE_IDX..Z_LOWBYTE_IDX + 2]
                .copy_from_slice(&self.next_mgm_data.z.to_le_bytes());
            self.call_count += 1;
            Ok(())
        }
    }

    #[allow(dead_code)]
    pub struct MgmTestbench {
        pub mode_request_tx: mpsc::SyncSender<GenericMessage<ModeRequest>>,
        pub mode_reply_rx_to_ground: mpsc::Receiver<GenericMessage<ModeReply>>,
        pub mode_reply_rx_to_parent: mpsc::Receiver<GenericMessage<ModeReply>>,
        pub shared_switch_set: SharedSwitchSet,
        pub tc_tx: mpsc::SyncSender<CcsdsTcPacketOwned>,
        pub tm_rx: mpsc::Receiver<PacketAsVec>,
        pub switch_rx: mpsc::Receiver<GenericMessage<SwitchRequest>>,
        pub handler: MgmHandlerLis3Mdl<TestSpiInterface>,
    }

    #[derive(Default)]
    #[allow(dead_code)]
    pub struct MgmAssemblyMock(
        pub HashMap<satrs::ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
    );

    impl ModeNode for MgmAssemblyMock {
        fn id(&self) -> satrs::ComponentId {
            ComponentId::AcsMgmAssembly as u32
        }
    }

    impl ModeParent for MgmAssemblyMock {
        type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;

        fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
            self.0.insert(id, request_sender);
        }
    }

    #[derive(Default)]
    #[allow(dead_code)]
    pub struct GroundMock {
        pub request_sender_map:
            HashMap<satrs::ComponentId, mpsc::SyncSender<GenericMessage<ModeRequest>>>,
    }

    impl ModeNode for GroundMock {
        fn id(&self) -> satrs::ComponentId {
            ComponentId::Ground as u32
        }
    }

    impl ModeParent for GroundMock {
        type Sender = mpsc::SyncSender<GenericMessage<ModeRequest>>;

        fn add_mode_child(&mut self, id: satrs::ComponentId, request_sender: Self::Sender) {
            self.request_sender_map.insert(id, request_sender);
        }
    }

    impl MgmTestbench {
        pub fn new() -> Self {
            let (request_tx, request_rx) = mpsc::sync_channel(5);
            let (reply_tx_to_ground, reply_rx_to_ground) = mpsc::sync_channel(5);
            let (reply_tx_to_parent, reply_rx_to_parent) = mpsc::sync_channel(5);
            let mode_node =
                ModeRequestHandlerMpscBounded::new(ComponentId::Ground as u32, request_rx);
            let (tc_tx, tc_rx) = mpsc::sync_channel(10);
            let (hk_reply_tx, _hk_reply_rx) = mpsc::sync_channel(10);
            let (_tm_tx, tm_rx) = mpsc::sync_channel(10);
            let (switcher_tx, switch_rx) = mpsc::sync_channel(10);
            let shared_mgm_set = Arc::default();
            let mut switch_map = SwitchMap::new();
            switch_map.insert(SwitchId::Mgm0, SwitchState::Off);
            let switch_map = SwitchSet::new(switch_map);
            let shared_switch_set = SharedSwitchSet::new(Mutex::new(switch_map));
            let mut handler = MgmHandlerLis3Mdl::new(
                ComponentId::AcsMgm0,
                "TEST_MGM",
                mode_node,
                tc_rx,
                hk_reply_tx,
                PowerSwitchHelper::new(switcher_tx, shared_switch_set.clone()),
                TestSpiInterface::default(),
                shared_mgm_set,
            );
            handler.add_mode_parent(ComponentId::Ground as u32, reply_tx_to_ground);
            handler.add_mode_parent(ComponentId::AcsMgmAssembly as u32, reply_tx_to_parent);
            Self {
                mode_request_tx: request_tx,
                mode_reply_rx_to_ground: reply_rx_to_ground,
                mode_reply_rx_to_parent: reply_rx_to_parent,
                shared_switch_set,
                switch_rx,
                handler,
                tm_rx,
                tc_tx,
            }
        }
    }

    #[test]
    fn test_basic_handler() {
        let mut testbench = MgmTestbench::new();
        assert_eq!(testbench.handler.com_interface.call_count, 0);
        assert_eq!(
            testbench.handler.mode_and_submode().mode(),
            DeviceMode::Off as u32
        );
        assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
        testbench.handler.periodic_operation();
        // Handler is OFF, no changes expected.
        assert_eq!(testbench.handler.com_interface.call_count, 0);
        assert_eq!(
            testbench.handler.mode_and_submode().mode(),
            DeviceMode::Off as u32
        );
        assert_eq!(testbench.handler.mode_and_submode().submode(), 0_u16);
    }

    #[test]
    fn test_normal_handler() {
        let mut testbench = MgmTestbench::new();
        testbench
            .mode_request_tx
            .send(GenericMessage::new(
                MessageMetadata::new(0, ComponentId::Ground as u32),
                ModeRequest::SetMode {
                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
                    forced: false,
                },
            ))
            .expect("failed to send mode request");
        testbench.handler.periodic_operation();
        assert_eq!(
            testbench.handler.mode_and_submode().mode(),
            DeviceMode::Normal as u32
        );
        assert_eq!(testbench.handler.mode_and_submode().submode(), 0);

        // Verify power switch handling.
        let switch_req = testbench.switch_rx.try_recv().expect("no switch request");
        assert_eq!(switch_req.message.switch_id, SwitchId::Mgm0);
        assert_eq!(switch_req.message.target_state, SwitchStateBinary::On);

        // This simulates one cycle for the power switch to update.
        testbench
            .shared_switch_set
            .lock()
            .unwrap()
            .set_switch_state(SwitchId::Mgm0, SwitchState::On);

        // Now the power switch is updated and the mode request should be completed.
        testbench.handler.periodic_operation();

        let mode_reply = testbench
            .mode_reply_rx_to_ground
            .try_recv()
            .expect("no mode reply generated");
        match mode_reply.message {
            ModeReply::ModeReply(mode) => {
                assert_eq!(mode.mode(), DeviceMode::Normal as u32);
                assert_eq!(mode.submode(), 0);
            }
            _ => panic!("unexpected mode reply"),
        }
        // The device should have been polled once.
        assert_eq!(testbench.handler.com_interface.call_count, 2);
        let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap();
        assert!(mgm_set.x < 0.001);
        assert!(mgm_set.y < 0.001);
        assert!(mgm_set.z < 0.001);
        assert!(mgm_set.valid);
    }

    #[test]
    fn test_normal_handler_mgm_set_conversion() {
        let mut testbench = MgmTestbench::new();
        let raw_values = MgmLis3RawValues {
            x: 1000,
            y: -1000,
            z: 1000,
        };
        testbench.handler.com_interface.next_mgm_data = raw_values;
        testbench
            .mode_request_tx
            .send(GenericMessage::new(
                MessageMetadata::new(0, ComponentId::Ground as u32),
                ModeRequest::SetMode {
                    mode_and_submode: ModeAndSubmode::new(DeviceMode::Normal as u32, 0),
                    forced: false,
                },
            ))
            .expect("failed to send mode request");
        testbench.handler.periodic_operation();
        let mgm_set = *testbench.handler.shared_mgm_set.lock().unwrap();
        let expected_x =
            raw_values.x as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        let expected_y =
            raw_values.y as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        let expected_z =
            raw_values.z as f32 * GAUSS_TO_MICROTESLA_FACTOR as f32 * FIELD_LSB_PER_GAUSS_4_SENS;
        let x_diff = (mgm_set.x - expected_x).abs();
        let y_diff = (mgm_set.y - expected_y).abs();
        let z_diff = (mgm_set.z - expected_z).abs();
        assert!(x_diff < 0.001, "x diff too large: {}", x_diff);
        assert!(y_diff < 0.001, "y diff too large: {}", y_diff);
        assert!(z_diff < 0.001, "z diff too large: {}", z_diff);
        assert!(mgm_set.valid);
    }
}
