sat-rs/satrs-example/src/pus/mod.rs

use crate::tmtc::MpscStoreAndSendError;
use satrs_core::events::EventU32;
use satrs_core::params::Params;
use satrs_core::pool::StoreAddr;
use satrs_core::pus::verification::{FailParams, StdVerifReporterWithSender};
use satrs_core::pus::AcceptedTc;
use satrs_core::spacepackets::ecss::PusServiceId;
use satrs_core::spacepackets::tc::PusTc;
use satrs_core::spacepackets::time::cds::TimeProvider;
use satrs_core::spacepackets::time::TimeWriter;
use satrs_core::tmtc::tm_helper::PusTmWithCdsShortHelper;
use satrs_example::{tmtc_err, CustomPusServiceId};
use std::sync::mpsc::Sender;

pub mod action;
pub mod event;
pub mod hk;
pub mod scheduler;
pub mod test;

pub struct PusTcMpscRouter {
    pub test_service_receiver: Sender<AcceptedTc>,
    pub event_service_receiver: Sender<AcceptedTc>,
    pub sched_service_receiver: Sender<AcceptedTc>,
    pub hk_service_receiver: Sender<AcceptedTc>,
    pub action_service_receiver: Sender<AcceptedTc>,
}

pub struct PusReceiver {
    pub tm_helper: PusTmWithCdsShortHelper,
    pub tm_args: PusTmArgs,
    pub tc_args: PusTcArgs,
    stamp_helper: TimeStampHelper,
}

pub struct PusTmArgs {
    /// All verification reporting is done with this reporter.
    pub verif_reporter: StdVerifReporterWithSender,
}

impl PusTmArgs {
    fn vr(&mut self) -> &mut StdVerifReporterWithSender {
        &mut self.verif_reporter
    }
}

pub struct PusTcArgs {
    /// This routes all telecommands to their respective recipients
    pub pus_router: PusTcMpscRouter,
    /// Used to send events from within the TC router
    pub event_sender: Sender<(EventU32, Option<Params>)>,
}

struct TimeStampHelper {
    stamper: TimeProvider,
    time_stamp: [u8; 7],
}

impl TimeStampHelper {
    pub fn new() -> Self {
        Self {
            stamper: TimeProvider::new_with_u16_days(0, 0),
            time_stamp: [0; 7],
        }
    }

    pub fn stamp(&self) -> &[u8] {
        &self.time_stamp
    }

    pub fn update_from_now(&mut self) {
        self.stamper
            .update_from_now()
            .expect("Updating timestamp failed");
        self.stamper
            .write_to_bytes(&mut self.time_stamp)
            .expect("Writing timestamp failed");
    }
}

impl PusReceiver {
    pub fn new(apid: u16, tm_arguments: PusTmArgs, tc_arguments: PusTcArgs) -> Self {
        Self {
            tm_helper: PusTmWithCdsShortHelper::new(apid),
            tm_args: tm_arguments,
            tc_args: tc_arguments,
            stamp_helper: TimeStampHelper::new(),
        }
    }
}

impl PusReceiver {
    pub fn handle_tc_packet(
        &mut self,
        store_addr: StoreAddr,
        service: u8,
        pus_tc: &PusTc,
    ) -> Result<(), MpscStoreAndSendError> {
        let init_token = self.tm_args.verif_reporter.add_tc(pus_tc);
        self.stamp_helper.update_from_now();
        let accepted_token = self
            .tm_args
            .vr()
            .acceptance_success(init_token, Some(self.stamp_helper.stamp()))
            .expect("Acceptance success failure");
        let service = PusServiceId::try_from(service);
        match service {
            Ok(standard_service) => match standard_service {
                PusServiceId::Test => {
                    let res = self
                        .tc_args
                        .pus_router
                        .test_service_receiver
                        .send((store_addr, accepted_token));
                    match res {
                        Ok(_) => {}
                        Err(e) => {
                            println!("Error {e}")
                        }
                    }
                }
                PusServiceId::Housekeeping => self
                    .tc_args
                    .pus_router
                    .hk_service_receiver
                    .send((store_addr, accepted_token))
                    .unwrap(),
                PusServiceId::Event => self
                    .tc_args
                    .pus_router
                    .event_service_receiver
                    .send((store_addr, accepted_token))
                    .unwrap(),
                PusServiceId::Scheduling => self
                    .tc_args
                    .pus_router
                    .sched_service_receiver
                    .send((store_addr, accepted_token))
                    .unwrap(),
                _ => self
                    .tm_args
                    .verif_reporter
                    .start_failure(
                        accepted_token,
                        FailParams::new(
                            Some(self.stamp_helper.stamp()),
                            &tmtc_err::PUS_SERVICE_NOT_IMPLEMENTED,
                            Some(&[standard_service as u8]),
                        ),
                    )
                    .expect("Start failure verification failed"),
            },
            Err(e) => {
                if let Ok(custom_service) = CustomPusServiceId::try_from(e.number) {
                    match custom_service {
                        CustomPusServiceId::Mode => {
                            //self.handle_mode_service(pus_tc, accepted_token)
                        }
                        CustomPusServiceId::Health => {}
                    }
                } else {
                    self.tm_args
                        .verif_reporter
                        .start_failure(
                            accepted_token,
                            FailParams::new(
                                Some(self.stamp_helper.stamp()),
                                &tmtc_err::INVALID_PUS_SUBSERVICE,
                                Some(&[e.number]),
                            ),
                        )
                        .expect("Start failure verification failed")
                }
            }
        }
        Ok(())
    }
}
// impl PusServiceProvider for PusReceiver {
//     type Error = ();
//
//     fn handle_pus_tc_packet(
//         &mut self,
//         service: u8,
//         _header: &SpHeader,
//         pus_tc: &PusTc,
//     ) -> Result<(), Self::Error> {
//         let init_token = self.tm_args.verif_reporter.add_tc(pus_tc);
//         self.stamp_helper.update_from_now();
//         let accepted_token = self
//             .tm_args
//             .vr()
//             .acceptance_success(init_token, Some(self.stamp_helper.stamp()))
//             .expect("Acceptance success failure");
//         let service = PusServiceId::try_from(service);
//         match service {
//             Ok(standard_service) => match standard_service {
//                 PusServiceId::Test => self
//                     .tc_args
//                     .pus_router
//                     .test_service_receiver
//                     .send_tc(*pus_tc),
//                 PusServiceId::Housekeeping => {
//                     self.tc_args.pus_router.hk_service_receiver.send_tc(*pus_tc)
//                 } //self.handle_hk_request(pus_tc, accepted_token),
//                 PusServiceId::Event => self
//                     .tc_args
//                     .pus_router
//                     .event_service_receiver
//                     .send_tc(*pus_tc), //self.handle_event_request(pus_tc, accepted_token),
//                 PusServiceId::Scheduling => self
//                     .tc_args
//                     .pus_router
//                     .sched_service_receiver
//                     .send_tc(*pus_tc), //self.handle_scheduled_tc(pus_tc, accepted_token),
//                 _ => self
//                     .tm_args
//                     .verif_reporter
//                     .start_failure(
//                         accepted_token,
//                         FailParams::new(
//                             Some(self.stamp_helper.stamp()),
//                             &tmtc_err::PUS_SERVICE_NOT_IMPLEMENTED,
//                             Some(&[standard_service as u8]),
//                         ),
//                     )
//                     .expect("Start failure verification failed"),
//             },
//             Err(e) => {
//                 if let Ok(custom_service) = CustomPusServiceId::try_from(e.number) {
//                     match custom_service {
//                         CustomPusServiceId::Mode => {
//                             self.handle_mode_service(pus_tc, accepted_token)
//                         }
//                         CustomPusServiceId::Health => {}
//                     }
//                 } else {
//                     self.tm_args
//                         .verif_reporter
//                         .start_failure(
//                             accepted_token,
//                             FailParams::new(
//                                 Some(self.stamp_helper.stamp()),
//                                 &tmtc_err::INVALID_PUS_SUBSERVICE,
//                                 Some(&[e.number]),
//                             ),
//                         )
//                         .expect("Start failure verification failed")
//                 }
//             }
//         }
//         Ok(())
//     }
// }

// impl PusReceiver {
//
//     fn handle_hk_request(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
//         if pus_tc.user_data().is_none() {
//             self.tm_args
//                 .verif_reporter
//                 .start_failure(
//                     token,
//                     FailParams::new(
//                         Some(self.stamp_helper.stamp()),
//                         &tmtc_err::NOT_ENOUGH_APP_DATA,
//                         None,
//                     ),
//                 )
//                 .expect("Sending start failure TM failed");
//             return;
//         }
//         let user_data = pus_tc.user_data().unwrap();
//         if user_data.len() < 8 {
//             let err = if user_data.len() < 4 {
//                 &hk_err::TARGET_ID_MISSING
//             } else {
//                 &hk_err::UNIQUE_ID_MISSING
//             };
//             self.tm_args
//                 .verif_reporter
//                 .start_failure(
//                     token,
//                     FailParams::new(Some(self.stamp_helper.stamp()), err, None),
//                 )
//                 .expect("Sending start failure TM failed");
//             return;
//         }
//         let addressable_id = AddressableId::from_raw_be(user_data).unwrap();
//         if !self
//             .tc_args
//             .request_map
//             .contains_key(&addressable_id.target_id)
//         {
//             self.tm_args
//                 .verif_reporter
//                 .start_failure(
//                     token,
//                     FailParams::new(
//                         Some(self.stamp_helper.stamp()),
//                         &hk_err::UNKNOWN_TARGET_ID,
//                         None,
//                     ),
//                 )
//                 .expect("Sending start failure TM failed");
//             return;
//         }
//         let send_request = |target: TargetId, request: HkRequest| {
//             let sender = self
//                 .tc_args
//                 .request_map
//                 .get(&addressable_id.target_id)
//                 .unwrap();
//             sender
//                 .send(RequestWithToken::new(
//                     target,
//                     Request::HkRequest(request),
//                     token,
//                 ))
//                 .unwrap_or_else(|_| panic!("Sending HK request {request:?} failed"));
//         };
//         if PusPacket::subservice(pus_tc) == hk::Subservice::TcEnableHkGeneration as u8 {
//             send_request(
//                 addressable_id.target_id,
//                 HkRequest::Enable(addressable_id.unique_id),
//             );
//         } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcDisableHkGeneration as u8 {
//             send_request(
//                 addressable_id.target_id,
//                 HkRequest::Disable(addressable_id.unique_id),
//             );
//         } else if PusPacket::subservice(pus_tc) == hk::Subservice::TcGenerateOneShotHk as u8 {
//             send_request(
//                 addressable_id.target_id,
//                 HkRequest::OneShot(addressable_id.unique_id),
//             );
//         } else if PusPacket::subservice(pus_tc)
//             == hk::Subservice::TcModifyHkCollectionInterval as u8
//         {
//             if user_data.len() < 12 {
//                 self.tm_args
//                     .verif_reporter
//                     .start_failure(
//                         token,
//                         FailParams::new(
//                             Some(self.stamp_helper.stamp()),
//                             &hk_err::COLLECTION_INTERVAL_MISSING,
//                             None,
//                         ),
//                     )
//                     .expect("Sending start failure TM failed");
//                 return;
//             }
//             send_request(
//                 addressable_id.target_id,
//                 HkRequest::ModifyCollectionInterval(
//                     addressable_id.unique_id,
//                     CollectionIntervalFactor::from_be_bytes(user_data[8..12].try_into().unwrap()),
//                 ),
//             );
//         }
//     }
//
//
//     fn handle_mode_service(&mut self, pus_tc: &PusTc, token: VerificationToken<TcStateAccepted>) {
//         let mut app_data_len = 0;
//         let app_data = pus_tc.user_data();
//         if app_data.is_some() {
//             app_data_len = pus_tc.user_data().unwrap().len();
//         }
//         if app_data_len < 4 {
//             self.tm_args
//                 .verif_reporter
//                 .start_failure(
//                     token,
//                     FailParams::new(
//                         Some(self.stamp_helper.stamp()),
//                         &tmtc_err::NOT_ENOUGH_APP_DATA,
//                         Some(format!("expected {} bytes, found {}", 4, app_data_len).as_bytes()),
//                     ),
//                 )
//                 .expect("Sending start failure TM failed");
//         }
//         let app_data = app_data.unwrap();
//         let mut invalid_subservice_handler = || {
//             self.tm_args
//                 .verif_reporter
//                 .start_failure(
//                     token,
//                     FailParams::new(
//                         Some(self.stamp_helper.stamp()),
//                         &tmtc_err::INVALID_PUS_SUBSERVICE,
//                         Some(&[PusPacket::subservice(pus_tc)]),
//                     ),
//                 )
//                 .expect("Sending start failure TM failed");
//         };
//         let subservice = mode::Subservice::try_from(PusPacket::subservice(pus_tc));
//         if let Ok(subservice) = subservice {
//             let forward_mode_request = |target_id, mode_request: ModeRequest| match self
//                 .tc_args
//                 .request_map
//                 .get(&target_id)
//             {
//                 None => warn!("not mode request recipient for target ID {target_id} found"),
//                 Some(sender_to_recipient) => {
//                     sender_to_recipient
//                         .send(RequestWithToken::new(
//                             target_id,
//                             Request::ModeRequest(mode_request),
//                             token,
//                         ))
//                         .expect("sending mode request failed");
//                 }
//             };
//             let mut valid_subservice = true;
//             match subservice {
//                 Subservice::TcSetMode => {
//                     let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
//                     let min_len = ModeAndSubmode::raw_len() + 4;
//                     if app_data_len < min_len {
//                         self.tm_args
//                             .verif_reporter
//                             .start_failure(
//                                 token,
//                                 FailParams::new(
//                                     Some(self.stamp_helper.stamp()),
//                                     &tmtc_err::NOT_ENOUGH_APP_DATA,
//                                     Some(
//                                         format!("expected {min_len} bytes, found {app_data_len}")
//                                             .as_bytes(),
//                                     ),
//                                 ),
//                             )
//                             .expect("Sending start failure TM failed");
//                     }
//                     // Should never fail after size check
//                     let mode_submode = ModeAndSubmode::from_be_bytes(
//                         app_data[4..4 + ModeAndSubmode::raw_len()]
//                             .try_into()
//                             .unwrap(),
//                     )
//                     .unwrap();
//                     forward_mode_request(target_id, ModeRequest::SetMode(mode_submode));
//                 }
//                 Subservice::TcReadMode => {
//                     let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
//                     forward_mode_request(target_id, ModeRequest::ReadMode);
//                 }
//                 Subservice::TcAnnounceMode => {
//                     let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
//                     forward_mode_request(target_id, ModeRequest::AnnounceMode);
//                 }
//                 Subservice::TcAnnounceModeRecursive => {
//                     let target_id = u32::from_be_bytes(app_data[0..4].try_into().unwrap());
//                     forward_mode_request(target_id, ModeRequest::AnnounceModeRecursive);
//                 }
//                 _ => {
//                     warn!("Can not process mode request with subservice {subservice:?}");
//                     invalid_subservice_handler();
//                     valid_subservice = false;
//                 }
//             }
//             if valid_subservice {
//                 self.tm_args
//                     .verif_reporter
//                     .start_success(token, Some(self.stamp_helper.stamp()))
//                     .expect("sending start success TM failed");
//             }
//         } else {
//             invalid_subservice_handler();
//         }
//     }
// }