eurosim-obsw/src/hk.rs

use std::ops::{Deref, DerefMut};
use std::sync::{Arc, Mutex};
use std::sync::mpsc::{Receiver, Sender};
use satrs_core::pool::StoreAddr;
use satrs_core::pus::hk::Subservice;
use satrs_core::pus::verification::{FailParams, StdVerifSenderError, VerificationReporterWithSender};
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
use satrs_core::spacepackets::SpHeader;
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
use satrs_core::tmtc::AddressableId;
use crate::aocs_handler::{AocsSensorHandler, MGMHandler};
use crate::requests::RequestWithToken;
use crate::tmtc::TmStore;
use serde_json::json;
use serde::{Serialize, Deserialize};
use eurosim_obsw::{hk_err, tmtc_err};
use crate::requests::Request;

pub type CollectionIntervalFactor = u32;

#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum AocsHkIds {
    TestAocsSet = 1,
    TestMgmSet = 2,
}

#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub enum HkRequest {
    OneShot(AddressableId),
    Enable(AddressableId),
    Disable(AddressableId),
    ModifyCollectionInterval(AddressableId, CollectionIntervalFactor),
}

#[derive(Serialize, Deserialize)]
pub struct AocsSensorData {
    mgm_data: [f64; 3],     // Voltage for 3 axis
    css_data: [f64; 18],    // Voltage for 18 sun sensors
    str_data: [f64; 4],     // Quaternion for position of satellite
}

impl AocsSensorData {
    pub fn new() -> AocsSensorData {
        let mgm_data = [0.0; 3];
        let css_data = [0.0; 18];
        let str_data = [0.0; 4];
        AocsSensorData{mgm_data, css_data, str_data}
    }

    pub fn write_mgm_data(&mut self, mgm_data: [f64; 3]) {
        self.mgm_data = mgm_data;
    }

    pub fn write_css_data(&mut self, css_data: [f64; 18]) {
        self.css_data = css_data;
    }

    pub fn write_str_data(&mut self, str_data: [f64; 4]) {
        self.str_data = str_data;
    }

    pub fn read_mgm_data(&mut self) -> [f64; 3] {
        self.mgm_data
    }

    pub fn read_css_data(&mut self) -> [f64; 18] {
        self.css_data
    }

    pub fn read_str_data(&mut self) -> [f64; 4] {
        self.str_data
    }
}

pub struct AocsHkHandler {
    sensor_data_pool: Arc<Mutex<AocsSensorData>>,
    action_rx: Receiver<RequestWithToken>,
    seq_count_provider: SeqCountProviderSyncClonable,
    aocs_tm_store: TmStore,
    aocs_tm_funnel_tx: Sender<StoreAddr>,
    verif_reporter: &'static mut VerificationReporterWithSender<StdVerifSenderError>
}

impl AocsHkHandler {
    pub fn new(sensor_data_pool: Arc<Mutex<AocsSensorData>>,
               action_rx: Receiver<RequestWithToken>,
               seq_count_provider: SeqCountProviderSyncClonable,
               aocs_tm_store: TmStore,
               aocs_tm_funnel_tx: Sender<StoreAddr>,
               verif_reporter: &'static mut VerificationReporterWithSender<StdVerifSenderError>,
    ) -> AocsHkHandler {
        AocsHkHandler {
            sensor_data_pool,
            action_rx,
            seq_count_provider,
            aocs_tm_store,
            aocs_tm_funnel_tx,
            verif_reporter,
        }
    }

    pub fn handle_hk_request(&mut self) {
        let mut time_stamp_buf: [u8; 7] = [0; 7];

        if let Ok(request_with_token) = self.action_rx.try_recv() {
            if let Request::HkRequest(hk_req) = request_with_token.0 {
                let cds_stamp =
                    TimeProvider::from_now_with_u16_days().unwrap();
                cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                let start_token = self    //implement this for verification
                    .verif_reporter
                    .start_success(request_with_token.1, Some(&time_stamp_buf))
                    .expect("Error sending start success");
                if let Ok(()) = match hk_req {
                    HkRequest::OneShot(id) => {
                        self.one_shot_hk(id)
                    }
                    HkRequest::Enable(id) => {
                        self.enable_hk(id)
                    }
                    HkRequest::Disable(id) => {
                        self.disable_hk(id)
                    }
                    HkRequest::ModifyCollectionInterval(id, collection_interval) => {
                        Ok(())
                    }
                } {
                    let cds_stamp =
                        TimeProvider::from_now_with_u16_days().unwrap();
                    cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                    self.verif_reporter
                        .completion_success(start_token, Some(&time_stamp_buf))
                        .expect("Error sending completion success");
                } else {
                    let cds_stamp =
                        TimeProvider::from_now_with_u16_days().unwrap();
                    cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
                    self.verif_reporter
                        .completion_failure(start_token, FailParams::new(Some(&time_stamp_buf), &hk_err::UNKNOWN_TARGET_ID, None))
                        .expect("Error sending completion success");
                }
            }
        }
    }

    pub fn one_shot_hk(&mut self, id: AddressableId) -> Result<(), ()>{
        let json_string = self.aocs_data_to_str();
        self.send_hk_packet(id, &json_string);
        Ok(())
    }

    pub fn enable_hk(&mut self, id: AddressableId)-> Result<(), ()> {
        Ok(())
    }

    pub fn disable_hk(&mut self, id: AddressableId) -> Result<(), ()>{
        Ok(())
    }

    pub fn aocs_data_to_str(&mut self) -> String {
        let pool = self.sensor_data_pool.lock().unwrap();
        serde_json::to_string(pool.deref()).unwrap()
    }

    pub fn send_hk_packet(&mut self, id: AddressableId, data: &str) {
        let mut time_stamp_buf: [u8; 7] = [0; 7];
        let mut huge_buf: [u8; 8192] = [0; 8192];

        let mut sp_header = SpHeader::tm_unseg(
            0x02,
            self.seq_count_provider.get_and_increment(),
            0,
        ).unwrap();
        let cds_stamp =
            TimeProvider::from_now_with_u16_days().unwrap();
        cds_stamp.write_to_bytes(&mut time_stamp_buf).unwrap();
        let mut len = id.write_to_be_bytes(&mut huge_buf).unwrap();
        huge_buf[8..data.len() + 8]
            .copy_from_slice(data.as_bytes());
        len += data.len();
        let tm_sec_header = PusTmSecondaryHeader::new_simple(
            3,
            Subservice::TmHkPacket as u8,
            &time_stamp_buf,
        );
        let hk_tm = PusTm::new(
            &mut sp_header,
            tm_sec_header,
            Some(&huge_buf[0..len]),
            true,
        );
        let addr = self.aocs_tm_store.add_pus_tm(&hk_tm);
        self.aocs_tm_funnel_tx.send(addr).expect("sending failed");
    }
}