eurosim-obsw/src/hk.rs

use std::collections::HashMap;
use strum::IntoEnumIterator;

use crate::aocs::{CSSData, MGMData};
use crate::hk::AocsDataType::float_value;
use crate::requests::Request;
use crate::requests::RequestWithToken;
use crate::tmtc::{TmStore, AOCS_HK_APID};
use byteorder::{ByteOrder, LittleEndian};
use eurosim_obsw::hk_err;
use num_enum::FromPrimitive;
use satrs_core::hk::{HkRequest, UniqueId};
use satrs_core::pool::StoreAddr;
use satrs_core::pus::hk::Subservice;
use satrs_core::pus::verification::{FailParams, VerificationReporterWithSender};
use satrs_core::pus::MpscPusInStoreSendError;
use satrs_core::seq_count::{SeqCountProviderSyncClonable, SequenceCountProviderCore};
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::spacepackets::tm::{PusTm, PusTmSecondaryHeader};
use satrs_core::spacepackets::SpHeader;
use satrs_core::tmtc::AddressableId;
use serde::{Deserialize, Serialize};
use std::ops::Deref;
use std::sync::mpsc::{Receiver, Sender};
use std::sync::{Arc, Mutex};
use strum_macros::EnumIter;

pub type CollectionIntervalFactor = u32;

pub const MGM_VOLTAGE_1: UniqueId = 1;
pub const MGM_VOLTAGE_2: UniqueId = 2;
pub const MGM_VOLTAGE_3: UniqueId = 3;
pub const CSS_VOLTAGE_1: UniqueId = 4;
pub const CSS_VOLTAGE_2: UniqueId = 5;
pub const CSS_VOLTAGE_3: UniqueId = 6;
pub const CSS_VOLTAGE_4: UniqueId = 7;
pub const CSS_VOLTAGE_5: UniqueId = 8;
pub const CSS_VOLTAGE_6: UniqueId = 9;
pub const STR_QUATERNION_1: UniqueId = 10;
pub const STR_QUATERNION_2: UniqueId = 11;
pub const STR_QUATERNION_3: UniqueId = 12;
pub const STR_QUATERNION_4: UniqueId = 13;

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

#[derive(Clone)]
pub struct AocsDataMap {
    map: HashMap<UniqueId, AocsDataType>,
}

impl AocsDataMap {
    pub fn new() -> Self {
        let mut data_map = HashMap::new();

        data_map.insert(MGM_VOLTAGE_1, float_value(0.0));
        data_map.insert(MGM_VOLTAGE_2, float_value(0.0));
        data_map.insert(MGM_VOLTAGE_3, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_1, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_2, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_3, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_4, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_5, float_value(0.0));
        data_map.insert(CSS_VOLTAGE_6, float_value(0.0));
        data_map.insert(STR_QUATERNION_1, float_value(0.0));
        data_map.insert(STR_QUATERNION_2, float_value(0.0));
        data_map.insert(STR_QUATERNION_3, float_value(0.0));
        data_map.insert(STR_QUATERNION_4, float_value(0.0));

        Self { map: data_map }
    }

    pub fn update_value(&mut self, id: UniqueId, val: AocsDataType) -> Result<(), ()> {
        if self.map.contains_key(&id) {
            self.map.insert(id, val);
            return Ok(());
        }
        Err(())
    }

    pub fn get_value(&self, id: UniqueId) -> Option<&AocsDataType> {
        self.map.get(&id)
    }
}

#[derive(Debug, EnumIter, PartialEq, Copy, Clone)]
pub enum AocsDataType {
    float_value(f64),
}

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

impl AOCSSensorData {
    pub fn new() -> AOCSSensorData {
        let mgm_data = MGMData::default();
        let css_data = CSSData::default();
        let str_data = [0.0; 4];
        AOCSSensorData {
            mgm_data,
            css_data,
            str_data,
        }
    }

    pub fn update_mgm_data(&mut self, mgm_data: &Arc<Mutex<MGMData>>) {
        let data = mgm_data.lock().unwrap();
        self.mgm_data = *data;
    }

    pub fn update_css_data(&mut self, css_data: &Arc<Mutex<CSSData>>) {
        let data = css_data.lock().unwrap();
        self.css_data = *data;
    }

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

    pub fn get_mgm_data(&mut self) -> MGMData {
        self.mgm_data
    }

    pub fn read_css_data(&mut self) -> CSSData {
        self.css_data
    }

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

pub struct AocsHousekeeper {
    data_map: Arc<Mutex<AocsDataMap>>,
    id_list: Vec<UniqueId>,
    request_rx: Receiver<RequestWithToken>,
    seq_count_provider: SeqCountProviderSyncClonable,
    aocs_tm_store: TmStore,
    aocs_tm_funnel_tx: Sender<StoreAddr>,
    verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
    periodic_hk_ids: Option<Vec<UniqueId>>,
}

impl AocsHousekeeper {
    pub fn new(
        sensor_data_pool: Arc<Mutex<AocsDataMap>>,
        request_rx: Receiver<RequestWithToken>,
        seq_count_provider: SeqCountProviderSyncClonable,
        aocs_tm_store: TmStore,
        aocs_tm_funnel_tx: Sender<StoreAddr>,
        verif_reporter: VerificationReporterWithSender<MpscPusInStoreSendError>,
    ) -> AocsHousekeeper {
        let id_list = vec![
            MGM_VOLTAGE_1,
            MGM_VOLTAGE_2,
            MGM_VOLTAGE_3,
            CSS_VOLTAGE_1,
            CSS_VOLTAGE_2,
            CSS_VOLTAGE_3,
            CSS_VOLTAGE_4,
            CSS_VOLTAGE_5,
            CSS_VOLTAGE_6,
            STR_QUATERNION_1,
            STR_QUATERNION_2,
            STR_QUATERNION_3,
            STR_QUATERNION_4,
        ];

        AocsHousekeeper {
            data_map: sensor_data_pool,
            id_list,
            request_rx,
            seq_count_provider,
            aocs_tm_store,
            aocs_tm_funnel_tx,
            verif_reporter,
            periodic_hk_ids: None,
        }
    }

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

        if let Ok(request_with_token) = self.request_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.unwrap(), 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 periodic_hk(&mut self) {
        //let json_string = self.aocs_data_to_str();
        let data = self.data_map.lock().unwrap();
        let data_copy = data.clone();
        drop(data);
        let mut buf: [u8; 8] = [0; 8];
        if let Some(ids) = &self.periodic_hk_ids {
            for id in ids.clone() {
                if let Some(float_value(field)) = data_copy.get_value(id) {
                    let data_str = LittleEndian::write_f64(&mut buf, *field);
                    &self.send_hk_packet(id, &buf);
                }
            }
        }
    }

    pub fn one_shot_hk(&mut self, id: UniqueId) -> Result<(), ()> {
        let data = self.data_map.lock().unwrap();
        let data_copy = data.clone();
        drop(data);
        let mut buf: [u8; 8] = [0; 8];
        if let Some(float_value(field)) = data_copy.get_value(id) {
            let data_str = LittleEndian::write_f64(&mut buf, *field);
            self.send_hk_packet(id, &buf);
            return Ok(());
        }
        Err(())
    }

    pub fn enable_hk(&mut self, id: UniqueId) -> Result<(), ()> {
        if !self.id_list.contains(&id) {
            return Err(());
        }

        let periodic_hk_ids = self.periodic_hk_ids.clone();
        match periodic_hk_ids {
            None => self.periodic_hk_ids = Some(vec![id]),
            Some(mut ids) => {
                ids.push(id);
                self.periodic_hk_ids = Some(ids);
            }
        }
        Ok(())
    }

    pub fn disable_hk(&mut self, id: UniqueId) -> Result<(), ()> {
        if !self.id_list.contains(&id) {
            return Err(());
        }

        let periodic_hk_ids = self.periodic_hk_ids.clone();
        match periodic_hk_ids {
            None => return Err(()),
            Some(mut ids) => {
                if !ids.contains(&id) {
                    return Err(());
                }
                // .unwrap is allowed, since existence check is performed
                let index = ids.iter().position(|x| *x == id).unwrap();
                ids.remove(index);
                if ids.len() < 1 {
                    self.periodic_hk_ids = None;
                } else {
                    self.periodic_hk_ids = Some(ids);
                }
            }
        }
        Ok(())
    }

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

     */

    pub fn send_hk_packet_from_str(&mut self, id: UniqueId, 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();
        huge_buf[0..4].copy_from_slice(&id.to_be_bytes());
        let mut len = 4;
        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");
    }

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

        let mut sp_header =
            SpHeader::tm_unseg(AOCS_HK_APID, 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();
        huge_buf[0..4].copy_from_slice(&id.to_be_bytes());
        let mut len = 4;
        huge_buf[8..data.len() + 8].copy_from_slice(data);
        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");
    }
}