eurosim-obsw/src/pld_handler.rs

use crate::action::ActionRequest;
use crate::can_ids::{DeviceId, PackageId, PackageModel};
use crate::power_handler::{DeviceState, PowerSwitcher};
use crate::requests::{Request, RequestWithToken};
use eurosim_obsw::RequestTargetId;
use log::info;
use satrs_core::power::{PowerSwitchInfo, PowerSwitcherCommandSender, SwitchId};
use satrs_core::pus::verification::{StdVerifSenderError, VerificationReporterWithSender};
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use std::sync::mpsc;
use std::sync::mpsc::{Receiver, Sender};

#[derive(Debug, PartialEq, Copy, Clone)]
pub enum CameraMode {
    Idle,
    PictureRequest,
    Verification,
    Start,
    End,
}

pub struct CameraHandler {
    power_switcher: PowerSwitcher,
    camera_device_id: DeviceId,
    camera_switch_id: SwitchId,
    device_state: DeviceState,
    can_tx: Sender<PackageModel>,
    can_rx: Receiver<PackageModel>,
    mode: CameraMode,
    mode_rx: Receiver<CameraMode>,
    action_rx: Receiver<RequestWithToken>,
}

impl CameraHandler {
    pub fn new(
        power_switcher: PowerSwitcher,
        camera_device_id: DeviceId,
        can_tx: Sender<PackageModel>,
        can_rx: Receiver<PackageModel>,
        mode_rx: Receiver<CameraMode>,
        action_rx: Receiver<RequestWithToken>,
    ) -> CameraHandler {
        let camera_switch_id = camera_device_id as u16;
        CameraHandler {
            power_switcher,
            camera_device_id,
            camera_switch_id,
            device_state: DeviceState::Off,
            can_tx,
            can_rx,
            mode: CameraMode::Idle,
            mode_rx,
            action_rx,
        }
    }

    pub fn set_mode(&mut self, mode: CameraMode) {
        if self.mode == CameraMode::Idle {
            if mode == CameraMode::PictureRequest {
                self.mode = CameraMode::PictureRequest;
            }
        }
    }

    pub fn get_mode(&self) -> CameraMode {
        self.mode
    }

    pub fn handle_mode_requests(&mut self) {
        match self.mode_rx.try_recv() {
            Ok(mode) => {
                self.set_mode(mode);
            }
            Err(_) => {}
        }
    }

    /*pub fn handle_action_requests(&mut self) {
        match self.action_rx.try_recv() {
            Ok(request_with_token) => {
                match request_with_token.0 {
                    Request::ActionRequest(action_id) => {
                        match action_id {
                            ActionRequest::ImageRequest(target_id) => {
                                assert_eq!(target_id, RequestTargetId::PldSubsystem);
                                camera_handler.set_mode(CameraMode::PictureRequest);

                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                                // send start verification and get token
                                let start_token = reporter_pld
                                    .start_success(request_with_token.1, Some(&time_stamp_buf))
                                    .expect("Error sending start success.");

                                info!("{:?}", camera_handler.get_mode());
                                while camera_handler.get_mode() != CameraMode::Idle {
                                    camera_handler.periodic_op();
                                }

                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                                // send end verification with token
                                reporter_pld
                                    .completion_success(
                                        start_token,
                                        Some(&time_stamp_buf),
                                    )
                                    .expect("Error sending start success.");
                            }
                            ActionRequest::OrientationRequest(_) => {}
                            ActionRequest::PointingRequest(_) => {}
                        }
                    }
                    _ => {}
                }
            }
            Err(_) => {}
        }
    }

     */

    pub fn take_picture(&mut self) {}

    pub fn periodic_op(&mut self) {
        // Camera Device Handler State Machine

        match self.mode {
            CameraMode::Idle => {}
            CameraMode::PictureRequest => {
                if self.device_state == DeviceState::Off {
                    self.power_switcher
                        .send_switch_on_cmd(self.camera_switch_id)
                        .expect("sending switch cmd failed");
                    self.device_state = DeviceState::SwitchingPower;
                    info!("switching power");
                }
                if self.device_state == DeviceState::SwitchingPower {
                    if self
                        .power_switcher
                        .get_is_switch_on(self.camera_switch_id)
                        .expect("reading switch state failed")
                    {
                        self.device_state = DeviceState::On;
                        info!("device on");
                    }
                }
                if self.device_state == DeviceState::On {
                    self.can_tx.send(PackageModel::new(PackageId::CameraImageRequest, &[1]).unwrap());
                    info!("sent camera request");
                    self.mode = CameraMode::Verification;
                }
            }
            CameraMode::Verification => {
                if self.device_state == DeviceState::On {
                    info!("waiting for image request confirmation");
                    if let Ok(msg) = self.can_rx.recv() {
                        if msg.package_id() == PackageId::CameraImageRequestConfirmation {
                            self.mode = CameraMode::Start;
                        }
                    }
                }
            }
            CameraMode::Start => {
                if self.device_state == DeviceState::On {
                    info!("waiting for image start confirmation");
                    if let Ok(msg) = self.can_rx.recv() {
                        if msg.package_id() == PackageId::CameraImageExecutionStart {
                            self.mode = CameraMode::End;
                        }
                    }
                }
            }
            CameraMode::End => {
                if self.device_state == DeviceState::On {
                    info!("waiting for image end confirmation");
                    if let Ok(msg) = self.can_rx.recv() {
                        if msg.package_id() == PackageId::CameraImageExectutionEnd {
                            self.power_switcher
                                .send_switch_off_cmd(self.camera_switch_id)
                                .expect("sending switch command failed");
                            self.device_state = DeviceState::SwitchingPower;
                            info!("switching power");
                        }
                    }
                }
                if self.device_state == DeviceState::SwitchingPower {
                    if !self
                        .power_switcher
                        .get_is_switch_on(self.camera_switch_id)
                        .expect("reading switch state failed")
                    {
                        self.device_state = DeviceState::Off;
                        info!("device off");
                    }
                }
                if self.device_state == DeviceState::Off {
                    self.mode = CameraMode::Idle;
                }
            }
        }
    }
}

pub fn core_pld_task(
    power_switcher: PowerSwitcher,
    pld_thread_rx: Receiver<RequestWithToken>,
    pld_can_rx: Receiver<PackageModel>,
    pld_can_tx: Sender<PackageModel>,
    reporter_pld: &mut VerificationReporterWithSender<StdVerifSenderError>,
) {
    let (_camera_mode_tx, camera_mode_rx) = mpsc::channel();
    let (_action_tx, action_rx) = mpsc::channel();

    let mut camera_handler = CameraHandler::new(
        power_switcher,
        DeviceId::Camera,
        pld_can_tx,
        pld_can_rx,
        camera_mode_rx,
        action_rx,
    );

    let mut time_stamp_buf: [u8; 7] = [0; 7];
    loop {
        match pld_thread_rx.try_recv() {
            Ok(request_with_token) => {
                match request_with_token.0 {
                    Request::ActionRequest(action_id) => {
                        match action_id {
                            ActionRequest::ImageRequest(target_id) => {
                                assert_eq!(target_id, RequestTargetId::PldSubsystem);
                                camera_handler.set_mode(CameraMode::PictureRequest);

                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                                // send start verification and get token
                                let start_token = reporter_pld
                                    .start_success(request_with_token.1, Some(&time_stamp_buf))
                                    .expect("Error sending start success.");

                                info!("{:?}", camera_handler.get_mode());
                                while camera_handler.get_mode() != CameraMode::Idle {
                                    camera_handler.periodic_op();
                                }

                                let cds_stamp = TimeProvider::from_now_with_u16_days().unwrap();
                                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                                // send end verification with token
                                reporter_pld
                                    .completion_success(start_token, Some(&time_stamp_buf))
                                    .expect("Error sending start success.");
                            }
                            ActionRequest::OrientationRequest(_) => {}
                            ActionRequest::PointingRequest(_) => {}
                        }
                    }
                    _ => {}
                }
            }
            Err(_) => {}
        }
    }
}