sat-rs/satrs-example/minisim/src/controller.rs

use std::{sync::mpsc, time::Duration};

use nexosim::{
    simulation::{Address, Simulation},
    time::{Clock, MonotonicTime, SystemClock},
};
use satrs_minisim::{
    acs::{mgm, mgt},
    eps::PcduRequest,
    SimCtrlReply, SimCtrlRequest, SimReply, SimRequest, SimRequestWithTime,
};

use crate::{
    acs::{mgm::MgmModel, mgt::MgtModel},
    eps::PcduModel,
};

const WARNING_FOR_STALE_DATA: bool = false;

const SIM_CTRL_REQ_WIRETAPPING: bool = false;
const MGM_REQ_WIRETAPPING: bool = false;
const PCDU_REQ_WIRETAPPING: bool = false;
const MGT_REQ_WIRETAPPING: bool = false;

pub struct ModelAddrWrapper {
    mgm_0_addr: Address<MgmModel>,
    mgm_1_addr: Address<MgmModel>,
    pcdu_addr: Address<PcduModel>,
    mgt_addr: Address<MgtModel>,
}

// The simulation controller processes requests and drives the simulation.
pub struct SimController {
    pub sys_clock: SystemClock,
    pub request_receiver: mpsc::Receiver<SimRequestWithTime>,
    pub reply_sender: mpsc::Sender<SimReply>,
    pub simulation: Simulation,
    pub addr_wrapper: ModelAddrWrapper,
}

impl ModelAddrWrapper {
    pub fn new(
        mgm_0_addr: Address<MgmModel>,
        mgm_1_addr: Address<MgmModel>,
        pcdu_addr: Address<PcduModel>,
        mgt_addr: Address<MgtModel>,
    ) -> Self {
        Self {
            mgm_0_addr,
            mgm_1_addr,
            pcdu_addr,
            mgt_addr,
        }
    }
}
impl SimController {
    pub fn new(
        sys_clock: SystemClock,
        request_receiver: mpsc::Receiver<SimRequestWithTime>,
        reply_sender: mpsc::Sender<SimReply>,
        simulation: Simulation,
        addr_wrapper: ModelAddrWrapper,
    ) -> Self {
        Self {
            sys_clock,
            request_receiver,
            reply_sender,
            simulation,
            addr_wrapper,
        }
    }

    pub fn run(&mut self, start_time: MonotonicTime, udp_polling_interval_ms: u64) {
        let mut t = start_time;
        loop {
            let t_old = t;
            // Check for UDP requests every millisecond. Shift the simulator ahead here to prevent
            // replies lying in the past.
            t += Duration::from_millis(udp_polling_interval_ms);
            let _synch_status = self.sys_clock.synchronize(t);
            self.handle_sim_requests(t_old);
            self.simulation
                .step_until(t)
                .expect("simulation step failed");
        }
    }

    pub fn handle_sim_requests(&mut self, old_timestamp: MonotonicTime) {
        loop {
            match self.request_receiver.try_recv() {
                Ok(request) => {
                    if request.timestamp < old_timestamp && WARNING_FOR_STALE_DATA {
                        log::warn!("stale data with timestamp {:?} received", request.timestamp);
                    }
                    match request.request {
                        SimRequest::SimCtrl(request) => self.handle_ctrl_request(request),
                        SimRequest::Mgm { id, request } => self.handle_mgm_request(id, request),
                        SimRequest::Mgt(request) => self.handle_mgt_request(request),
                        SimRequest::Pcdu(request) => self.handle_pcdu_request(request),
                    }
                }
                Err(e) => match e {
                    mpsc::TryRecvError::Empty => break,
                    mpsc::TryRecvError::Disconnected => {
                        panic!("all request sender disconnected")
                    }
                },
            }
        }
    }

    fn handle_ctrl_request(&mut self, sim_ctrl_request: SimCtrlRequest) {
        if SIM_CTRL_REQ_WIRETAPPING {
            log::info!("received sim ctrl request: {sim_ctrl_request:?}");
        }
        match sim_ctrl_request {
            SimCtrlRequest::Ping => {
                log::info!("received ping request, a client is connecting");
                self.reply_sender
                    .send(SimReply::from(SimCtrlReply::Pong))
                    .expect("sending reply from sim controller failed");
            }
        }
    }

    fn handle_mgm_request(&mut self, mgm_id: mgm::Id, mgm_request: mgm::Request) {
        let addr = match mgm_id {
            mgm::Id::Mgm0 => &self.addr_wrapper.mgm_0_addr,
            mgm::Id::Mgm1 => &self.addr_wrapper.mgm_1_addr,
        };
        if MGM_REQ_WIRETAPPING {
            log::info!("received {mgm_id:?} request: {mgm_request:?}");
        }
        match mgm_request {
            mgm::Request::RequestSensorData => {
                self.simulation
                    .process_event(MgmModel::send_sensor_values, (), addr)
                    .expect("event execution error for mgm");
            }
            mgm::Request::SetSpiFault(fault_mode) => {
                log::info!("{mgm_id:?}: setting SPI fault mode to {fault_mode:?}");
                self.simulation
                    .process_event(MgmModel::set_spi_fault, fault_mode, addr)
                    .expect("event execution error for mgm");
            }
        }
    }

    fn handle_pcdu_request(&mut self, pcdu_request: PcduRequest) {
        if PCDU_REQ_WIRETAPPING {
            log::info!("received PCDU request: {pcdu_request:?}");
        }
        match pcdu_request {
            PcduRequest::RequestSwitchInfo => {
                self.simulation
                    .process_event(
                        PcduModel::request_switch_info,
                        (),
                        &self.addr_wrapper.pcdu_addr,
                    )
                    .unwrap();
            }
            PcduRequest::SwitchDevice { switch, state } => {
                self.simulation
                    .process_event(
                        PcduModel::switch_device,
                        (switch, state),
                        &self.addr_wrapper.pcdu_addr,
                    )
                    .unwrap();
            }
        }
    }

    fn handle_mgt_request(&mut self, mgt_request: mgt::Request) {
        if MGT_REQ_WIRETAPPING {
            log::info!("received MGT request: {mgt_request:?}");
        }
        match mgt_request {
            mgt::Request::ApplyTorque { duration, dipole } => self
                .simulation
                .process_event(
                    MgtModel::apply_torque,
                    (duration, dipole),
                    &self.addr_wrapper.mgt_addr,
                )
                .unwrap(),
            mgt::Request::RequestHk => self
                .simulation
                .process_event(
                    MgtModel::request_housekeeping_data,
                    (),
                    &self.addr_wrapper.mgt_addr,
                )
                .unwrap(),
        };
    }
}

#[cfg(test)]
mod tests {
    use crate::test_helpers::SimTestbench;

    use super::*;

    #[test]
    fn test_basic_ping() {
        let mut sim_testbench = SimTestbench::new();
        assert_eq!(
            sim_testbench.request_reply(SimCtrlRequest::Ping),
            Some(SimReply::SimCtrl(SimCtrlReply::Pong))
        );
    }
}