eurosim-obsw/src/pld_handler.rs

use std::sync::mpsc;
use std::sync::mpsc::{Receiver, Sender, TryRecvError};
use log::info;
use satrs_core::power::{PowerSwitcherCommandSender, PowerSwitchInfo, SwitchId, SwitchState};
use satrs_core::pus::verification::{StdVerifSenderError, VerificationReporterWithSender};
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use eurosim_obsw::RequestTargetId;
use crate::action::ActionRequest;
use crate::can::CanTxHandler;
use crate::can_ids::{DeviceId, load_package_ids, PackageId, PackageModel, ThreadId};
use crate::pld_handler::CameraMode::PictureRequest;
use crate::pcdu::{DeviceState, PowerSwitcher};
use crate::requests::{Request, RequestWithToken};

#[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: CanTxHandler,
    can_rx: Receiver<PackageModel>,
    mode: CameraMode,
    mode_rx: Receiver<CameraMode>,
}

impl CameraHandler {
    pub fn new(power_switcher: PowerSwitcher, camera_device_id: DeviceId, can_tx: CanTxHandler, can_rx: Receiver<PackageModel>, mode_rx: Receiver<CameraMode>) -> 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}
    }

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

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

    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.tx_socket(PackageId::CameraImageRequest, &[1]);
                    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>, reporter_pld: &mut VerificationReporterWithSender<StdVerifSenderError>) {
    let (camera_mode_tx, camera_mode_rx) = mpsc::channel();
    let camera_can_tx =
        CanTxHandler::new_socket("can0", ThreadId::PLDThread, load_package_ids()).unwrap();

    let mut camera_handler = CameraHandler::new(power_switcher, DeviceId::Camera, camera_can_tx, pld_can_rx, camera_mode_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();
                                }

                                // 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(_) => {}
        }
    }

}