use crate::aocs::AOCSSensorMode::{Idle, SendingData};
use crate::can_ids::{DeviceId, MessageId, MessageModel};
use crate::helpers::{ModeHelper, VerifHelper};
use crate::hk::{AocsDataMap, AocsDataType, CSS_SUN_VECTOR_1, CSS_SUN_VECTOR_2, CSS_SUN_VECTOR_3, CSS_VOLTAGE_4, CSS_VOLTAGE_5, CSS_VOLTAGE_6, GPS_ALTITUDE, GPS_LATITUDE, GPS_LONGITUDE, MGM_FIELD_1, MGM_FIELD_2, MGM_FIELD_3, STR_QUATERNION_1, STR_QUATERNION_2, STR_QUATERNION_3, STR_QUATERNION_4};
use crate::power_handler::{DeviceState, PowerSwitcher};
use crate::requests::{Request, RequestWithToken};
use byteorder::{ByteOrder, LittleEndian};
use num_derive::ToPrimitive;
use satrs_core::mode::{ModeAndSubmode, ModeRequest};
use satrs_core::power::{PowerSwitcherCommandSender, SwitchId};
use satrs_core::seq_count::SeqCountProviderSyncClonable;
use std::sync::mpsc::{Receiver, Sender};
use std::sync::{Arc, Mutex};
use log::debug;

#[derive(ToPrimitive, PartialEq, Copy, Clone)]
pub enum AOCSSensorMode {
    Idle = 0,
    SendingData = 1,
}

pub trait AocsSensorHandler {
    type Error;

    fn get_package_id(&mut self) -> Result<MessageId, Self::Error>;
    fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error>;

    fn enable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
        let id = self.get_package_id()?;
        self.send_message(id, &[1])
    }

    fn disable_sensor_data_generation(&mut self) -> Result<(), Self::Error> {
        let id = self.get_package_id()?;
        self.send_message(id, &[0])
    }

    fn request_sensor_data_oneshot(&mut self) -> Result<(), Self::Error> {
        let id = self.get_package_id()?;
        self.send_message(id, &[2])
    }
}

pub struct CSSHandler {
    power_switcher: PowerSwitcher,
    device_id: DeviceId,
    switch_id: SwitchId,
    device_state: DeviceState,
    mode: AOCSSensorMode,
    aocs_data: Arc<Mutex<AocsDataMap>>,
    can_tx: Sender<MessageModel>,
    can_rx: Receiver<MessageModel>,
    request_rx: Receiver<RequestWithToken>,
    verif_helper: VerifHelper,
    mode_helper: ModeHelper,
}

impl AocsSensorHandler for CSSHandler {
    type Error = ();

    fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
        Ok(MessageId::AOCSDataRequestSunSensor1)
    }

    fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
        self.can_tx
            .send(MessageModel::new(id, buf).unwrap())
            .unwrap();
        return Ok(());
    }
}

impl CSSHandler {
    pub fn new(
        power_switcher: PowerSwitcher,
        device_id: DeviceId,
        device_state: DeviceState,
        aocs_data: Arc<Mutex<AocsDataMap>>,
        can_tx: Sender<MessageModel>,
        can_rx: Receiver<MessageModel>,
        request_rx: Receiver<RequestWithToken>,
        verif_helper: VerifHelper,
        mode_helper: ModeHelper,
    ) -> Self {
        let switch_id = device_id as u16;
        Self {
            power_switcher,
            device_id,
            switch_id,
            device_state,
            mode: Idle,
            aocs_data,
            can_tx,
            can_rx,
            request_rx,
            verif_helper,
            mode_helper,
        }
    }

    pub fn periodic_op(&mut self) {
        self.handle_requests();
        self.read_can();
    }

    fn handle_requests(&mut self) {
        if let Ok(req) = self.request_rx.try_recv() {
            let (req, start_token) = self
                .verif_helper
                .start_and_unwrap(req)
                .expect("error sending start of execution");
            match req {
                Request::HkRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
                Request::ModeRequest(req) => match req {
                    ModeRequest::SetMode(mode_submode) => {
                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
                            self.update_mode(aocs_mode);
                        }

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::ReadMode => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::AnnounceMode => {
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion_failure(token)
                                .expect("error sending completion failure");
                        }
                    }
                    ModeRequest::AnnounceModeRecursive => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion failure");
                        }
                    }
                },
                Request::ActionRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
            }
        }
    }

    fn update_mode(&mut self, mode: AOCSSensorMode) {
        if mode != self.mode {
            match mode {
                Idle => { self.set_mode_idle() }
                SendingData => { self.set_mode_reading() }
            }
        }
    }

    fn set_mode_reading(&mut self) {
        if self.mode == SendingData {
            return;
        }

        // TODO: currently unrecoverable, should probably be changed
        self.power_switcher
            .send_switch_on_cmd(self.switch_id)
            .expect("error sending switch on cmd");
        self.enable_sensor_data_generation()
            .expect("error enabling sensor data generation");
        self.mode = SendingData;
    }

    fn set_mode_idle(&mut self) {
        if self.mode == Idle {
            return;
        }

        self.disable_sensor_data_generation()
            .expect("error disabling sensor data generation");
        self.power_switcher
            .send_switch_off_cmd(self.switch_id)
            .expect("error sending switch off cmd");
        self.mode = Idle;
    }

    fn read_can(&mut self) {
        if let Ok(package) = self.can_rx.try_recv() {
            let mut map = self.aocs_data.lock().expect("error locking data map");
            let float_data = self.decode_sensor_data(package.data());
            match package.message_id() {
                MessageId::AOCSDataSunSensor1 => {
                    map.update_value(CSS_SUN_VECTOR_1, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataSunSensor2 => {
                    map.update_value(CSS_SUN_VECTOR_2, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataSunSensor3 => {
                    map.update_value(CSS_SUN_VECTOR_3, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataSunSensor4 => {
                    map.update_value(CSS_VOLTAGE_4, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataSunSensor5 => {
                    map.update_value(CSS_VOLTAGE_5, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataSunSensor6 => {
                    map.update_value(CSS_VOLTAGE_6, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                _ => {}
            }
            drop(map);
        }
    }

    fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
        LittleEndian::read_f64(&buf)
    }
}

pub struct MGMHandler {
    power_switcher: PowerSwitcher,
    device_id: DeviceId,
    switch_id: SwitchId,
    device_state: DeviceState,
    mode: AOCSSensorMode,
    aocs_data: Arc<Mutex<AocsDataMap>>,
    can_tx: Sender<MessageModel>,
    can_rx: Receiver<MessageModel>,
    request_rx: Receiver<RequestWithToken>,
    verif_helper: VerifHelper,
    mode_helper: ModeHelper,
}

impl AocsSensorHandler for MGMHandler {
    type Error = ();

    fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
        Ok(MessageId::AOCSDataRequestMGM1)
    }

    fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
        self.can_tx
            .send(MessageModel::new(id, buf).unwrap())
            .unwrap();
        return Ok(());
    }
}

impl MGMHandler {
    pub fn new(
        power_switcher: PowerSwitcher,
        device_id: DeviceId,
        device_state: DeviceState,
        aocs_data: Arc<Mutex<AocsDataMap>>,
        can_tx: Sender<MessageModel>,
        can_rx: Receiver<MessageModel>,
        request_rx: Receiver<RequestWithToken>,
        verif_helper: VerifHelper,
        mode_helper: ModeHelper,
    ) -> Self {
        let switch_id = device_id as u16;
        Self {
            power_switcher,
            device_id,
            switch_id,
            device_state,
            mode: Idle,
            aocs_data,
            can_tx,
            can_rx,
            request_rx,
            verif_helper,
            mode_helper,
        }
    }

    pub fn periodic_op(&mut self) {
        self.handle_requests();
        self.read_can();
    }

    fn handle_requests(&mut self) {
        if let Ok(req) = self.request_rx.try_recv() {
            debug!("request received");
            let (req, start_token) = self
                .verif_helper
                .start_and_unwrap(req)
                .expect("error sending start of execution");
            match req {
                Request::HkRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
                Request::ModeRequest(req) => match req {
                    ModeRequest::SetMode(mode_submode) => {
                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
                            self.update_mode(aocs_mode);
                        }

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::ReadMode => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::AnnounceMode => {
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion_failure(token)
                                .expect("error sending completion failure");
                        }
                    }
                    ModeRequest::AnnounceModeRecursive => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion failure");
                        }
                    }
                },
                Request::ActionRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
            }
        }
    }

    fn update_mode(&mut self, mode: AOCSSensorMode) {
        if mode != self.mode {
            match mode {
                Idle => { self.set_mode_idle() }
                SendingData => { self.set_mode_reading() }
            }
        }
    }

    fn set_mode_reading(&mut self) {
        if self.mode == SendingData {
            return;
        }

        // TODO: currently unrecoverable, should probably be changed
        self.power_switcher
            .send_switch_on_cmd(self.switch_id)
            .expect("error sending switch on cmd");
        self.enable_sensor_data_generation()
            .expect("error enabling sensor data generation");
        self.mode = SendingData;
    }

    fn set_mode_idle(&mut self) {
        if self.mode == Idle {
            return;
        }

        self.disable_sensor_data_generation()
            .expect("error disabling sensor data generation");
        self.power_switcher
            .send_switch_off_cmd(self.switch_id)
            .expect("error sending switch off cmd");
        self.mode = Idle;
    }

    fn read_can(&mut self) {
        if let Ok(package) = self.can_rx.try_recv() {
            let mut map = self.aocs_data.lock().expect("error locking data map");
            let float_data = self.decode_sensor_data(package.data());
            match package.message_id() {
                MessageId::AOCSDataMGM1 => {
                    map.update_value(MGM_FIELD_1, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataMGM2 => {
                    map.update_value(MGM_FIELD_2, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataMGM3 => {
                    map.update_value(MGM_FIELD_3, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                _ => {}
            }
            drop(map);
        }
    }

    fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
        LittleEndian::read_f64(&buf)
    }
}

pub struct STRHandler {
    power_switcher: PowerSwitcher,
    device_id: DeviceId,
    switch_id: SwitchId,
    device_state: DeviceState,
    mode: AOCSSensorMode,
    aocs_data: Arc<Mutex<AocsDataMap>>,
    can_tx: Sender<MessageModel>,
    can_rx: Receiver<MessageModel>,
    request_rx: Receiver<RequestWithToken>,
    verif_helper: VerifHelper,
    mode_helper: ModeHelper,
}

impl AocsSensorHandler for STRHandler {
    type Error = ();

    fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
        Ok(MessageId::AOCSDataRequestStarTracker)
    }

    fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
        self.can_tx
            .send(MessageModel::new(id, buf).unwrap())
            .unwrap();
        return Ok(());
    }
}

impl STRHandler {
    pub fn new(
        power_switcher: PowerSwitcher,
        device_id: DeviceId,
        device_state: DeviceState,
        aocs_data: Arc<Mutex<AocsDataMap>>,
        can_tx: Sender<MessageModel>,
        can_rx: Receiver<MessageModel>,
        request_rx: Receiver<RequestWithToken>,
        verif_helper: VerifHelper,
        mode_helper: ModeHelper,
    ) -> Self {
        let switch_id = device_id as u16;
        Self {
            power_switcher,
            device_id,
            switch_id,
            device_state,
            mode: Idle,
            aocs_data,
            can_tx,
            can_rx,
            request_rx,
            verif_helper,
            mode_helper,
        }
    }

    pub fn periodic_op(&mut self) {
        self.handle_requests();
        self.read_can();
    }

    fn handle_requests(&mut self) {
        if let Ok(req) = self.request_rx.try_recv() {
            let (req, start_token) = self
                .verif_helper
                .start_and_unwrap(req)
                .expect("error sending start of execution");
            match req {
                Request::HkRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
                Request::ModeRequest(req) => match req {
                    ModeRequest::SetMode(mode_submode) => {
                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
                            self.update_mode(aocs_mode);
                        }

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::ReadMode => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::AnnounceMode => {
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion_failure(token)
                                .expect("error sending completion failure");
                        }
                    }
                    ModeRequest::AnnounceModeRecursive => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion failure");
                        }
                    }
                },
                Request::ActionRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
            }
        }
    }

    fn update_mode(&mut self, mode: AOCSSensorMode) {
        if mode != self.mode {
            match mode {
                Idle => { self.set_mode_idle() }
                SendingData => { self.set_mode_reading() }
            }
        }
    }

    fn set_mode_reading(&mut self) {
        if self.mode == SendingData {
            return;
        }

        // TODO: currently unrecoverable, should probably be changed
        self.power_switcher
            .send_switch_on_cmd(self.switch_id)
            .expect("error sending switch on cmd");
        self.enable_sensor_data_generation()
            .expect("error enabling sensor data generation");
        self.mode = SendingData;
    }

    fn set_mode_idle(&mut self) {
        if self.mode == Idle {
            return;
        }

        self.disable_sensor_data_generation()
            .expect("error disabling sensor data generation");
        self.power_switcher
            .send_switch_off_cmd(self.switch_id)
            .expect("error sending switch off cmd");
        self.mode = Idle;
    }

    fn read_can(&mut self) {
        if let Ok(package) = self.can_rx.try_recv() {
            let mut map = self.aocs_data.lock().expect("error locking data map");
            let float_data = self.decode_sensor_data(package.data());
            match package.message_id() {
                MessageId::AOCSDataStarTracker1 => {
                    map.update_value(STR_QUATERNION_1, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataStarTracker2 => {
                    map.update_value(STR_QUATERNION_2, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataStarTracker3 => {
                    map.update_value(STR_QUATERNION_3, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataStarTracker4 => {
                    map.update_value(STR_QUATERNION_4, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                _ => {}
            }
            drop(map);
        }
    }

    fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
        LittleEndian::read_f64(&buf)
    }
}

pub struct GPSHandler {
    power_switcher: PowerSwitcher,
    device_id: DeviceId,
    switch_id: SwitchId,
    device_state: DeviceState,
    mode: AOCSSensorMode,
    aocs_data: Arc<Mutex<AocsDataMap>>,
    can_tx: Sender<MessageModel>,
    can_rx: Receiver<MessageModel>,
    request_rx: Receiver<RequestWithToken>,
    verif_helper: VerifHelper,
    mode_helper: ModeHelper,
}

impl AocsSensorHandler for GPSHandler {
    type Error = ();

    fn get_package_id(&mut self) -> Result<MessageId, Self::Error> {
        Ok(MessageId::AOCSDataRequestGPS)
    }

    fn send_message(&mut self, id: MessageId, buf: &[u8]) -> Result<(), Self::Error> {
        self.can_tx
            .send(MessageModel::new(id, buf).unwrap())
            .unwrap();
        return Ok(());
    }
}

impl GPSHandler {
    pub fn new(
        power_switcher: PowerSwitcher,
        device_id: DeviceId,
        device_state: DeviceState,
        aocs_data: Arc<Mutex<AocsDataMap>>,
        can_tx: Sender<MessageModel>,
        can_rx: Receiver<MessageModel>,
        request_rx: Receiver<RequestWithToken>,
        verif_helper: VerifHelper,
        mode_helper: ModeHelper,
    ) -> Self {
        let switch_id = device_id as u16;
        Self {
            power_switcher,
            device_id,
            switch_id,
            device_state,
            mode: Idle,
            aocs_data,
            can_tx,
            can_rx,
            request_rx,
            verif_helper,
            mode_helper,
        }
    }

    pub fn periodic_op(&mut self) {
        self.handle_requests();
        self.read_can();
    }

    fn handle_requests(&mut self) {
        if let Ok(req) = self.request_rx.try_recv() {
            let (req, start_token) = self
                .verif_helper
                .start_and_unwrap(req)
                .expect("error sending start of execution");
            match req {
                Request::HkRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
                Request::ModeRequest(req) => match req {
                    ModeRequest::SetMode(mode_submode) => {
                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
                            self.update_mode(aocs_mode);
                        }

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::ReadMode => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::AnnounceMode => {
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion_failure(token)
                                .expect("error sending completion failure");
                        }
                    }
                    ModeRequest::AnnounceModeRecursive => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion failure");
                        }
                    }
                },
                Request::ActionRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
            }
        }
    }

    fn update_mode(&mut self, mode: AOCSSensorMode) {
        if mode != self.mode {
            match mode {
                Idle => { self.set_mode_idle() }
                SendingData => { self.set_mode_reading() }
            }
        }
    }

    fn set_mode_reading(&mut self) {
        if self.mode == SendingData {
            return;
        }

        // TODO: currently unrecoverable, should probably be changed
        self.power_switcher
            .send_switch_on_cmd(self.switch_id)
            .expect("error sending switch on cmd");
        self.enable_sensor_data_generation()
            .expect("error enabling sensor data generation");
        self.mode = SendingData;
    }

    fn set_mode_idle(&mut self) {
        if self.mode == Idle {
            return;
        }

        self.disable_sensor_data_generation()
            .expect("error disabling sensor data generation");
        self.power_switcher
            .send_switch_off_cmd(self.switch_id)
            .expect("error sending switch off cmd");
        self.mode = Idle;
    }

    fn read_can(&mut self) {
        if let Ok(package) = self.can_rx.try_recv() {
            let mut map = self.aocs_data.lock().expect("error locking data map");
            let float_data = self.decode_sensor_data(package.data());
            match package.message_id() {
                MessageId::AOCSDataGPSLatitude => {
                    map.update_value(GPS_LATITUDE, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataGPSLongitude => {
                    map.update_value(GPS_LONGITUDE, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                MessageId::AOCSDataGPSAltitude => {
                    map.update_value(GPS_ALTITUDE, AocsDataType::FloatValue(float_data))
                        .expect("error updating value");
                }
                _ => {}
            }
            drop(map);
        }
    }

    fn decode_sensor_data(&self, buf: &[u8]) -> f64 {
        LittleEndian::read_f64(&buf)
    }
}

pub struct AocsController {
    request_rx: Receiver<RequestWithToken>,
    mgm_request_tx: Sender<RequestWithToken>,
    css_request_tx: Sender<RequestWithToken>,
    str_request_tx: Sender<RequestWithToken>,
    gps_request_tx: Sender<RequestWithToken>,
    seq_count_provider: SeqCountProviderSyncClonable,
    verif_helper: VerifHelper,
    mode_helper: ModeHelper,
    mode: AOCSSensorMode,
}

impl AocsController {
    pub fn new(
        request_rx: Receiver<RequestWithToken>,
        mgm_request_tx: Sender<RequestWithToken>,
        css_request_tx: Sender<RequestWithToken>,
        str_request_tx: Sender<RequestWithToken>,
        gps_request_tx: Sender<RequestWithToken>,
        seq_count_provider: SeqCountProviderSyncClonable,
        verif_helper: VerifHelper,
        mode_helper: ModeHelper,
    ) -> Self {
        Self {
            request_rx,
            mgm_request_tx,
            css_request_tx,
            str_request_tx,
            gps_request_tx,
            seq_count_provider,
            verif_helper,
            mode_helper,
            mode: Idle,
        }
    }

    pub fn periodic_op(&mut self) {
        self.handle_requests();
    }

    fn handle_requests(&mut self) {
        if let Ok(req) = self.request_rx.try_recv() {
            let (req, start_token) = self
                .verif_helper
                .start_and_unwrap(req)
                .expect("error sending start of execution");
            match req {
                Request::HkRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
                Request::ModeRequest(req) => match req {
                    ModeRequest::SetMode(mode_submode) => {
                        if let Some(aocs_mode) = mode_submode_to_aocs_mode(mode_submode) {
                            self.mode = aocs_mode;
                        }
                        self.mgm_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending to mgm");
                        self.css_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending to css");
                        self.str_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending to str");
                        self.gps_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending to str");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::ReadMode => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion success");
                        }
                    }
                    ModeRequest::AnnounceMode => {
                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion_failure(token)
                                .expect("error sending completion failure");
                        }
                    }
                    ModeRequest::AnnounceModeRecursive => {
                        self.mode_helper
                            .make_and_send_mode_reply(aocs_mode_to_mode_submode(self.mode))
                            .expect("error sending mode reply");

                        self.mgm_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending mode request to mgm handler");
                        self.css_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending mode request to css handler");
                        self.str_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending mode request to str handler");
                        self.gps_request_tx
                            .send(RequestWithToken(Request::ModeRequest(req), None))
                            .expect("error sending mode request to str handler");

                        if let Some(token) = start_token {
                            self.verif_helper
                                .completion(token)
                                .expect("error sending completion failure");
                        }
                    }
                },
                Request::ActionRequest(_) => {
                    if let Some(token) = start_token {
                        self.verif_helper
                            .completion_failure(token)
                            .expect("error sending completion failure");
                    }
                }
            }
        }
    }
}

fn aocs_mode_to_mode_submode(sensor_mode: AOCSSensorMode) -> ModeAndSubmode {
    match sensor_mode {
        Idle => ModeAndSubmode::new(0, 0),
        SendingData => ModeAndSubmode::new(1, 0),
    }
}

fn mode_submode_to_aocs_mode(mode_submode: ModeAndSubmode) -> Option<AOCSSensorMode> {
    match mode_submode.mode() {
        0 => Some(Idle),
        1 => Some(SendingData),
        _ => None,
    }
}

pub fn core_aocs_loop() {}