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

use std::sync::mpsc;
use std::time::Duration;

use crate::pus::create_verification_reporter;
use log::{error, info, warn};
use satrs::pool::{PoolProvider, StaticMemoryPool};
use satrs::pus::scheduler::{PusScheduler, TcInfo};
use satrs::pus::scheduler_srv::PusSchedServiceHandler;
use satrs::pus::verification::VerificationReporter;
use satrs::pus::{
    EcssTcAndToken, EcssTcInMemConverter, EcssTcInSharedStoreConverter, EcssTcInVecConverter,
    EcssTmSender, MpscTcReceiver, MpscTmAsVecSender, PusPacketHandlerResult, PusServiceHelper,
};
use satrs::tmtc::{PacketAsVec, PacketInPool, PacketSenderWithSharedPool};
use satrs::ComponentId;
use satrs_example::config::components::PUS_SCHED_SERVICE;

use super::HandlingStatus;

pub trait TcReleaser {
    fn release(&mut self, sender_id: ComponentId, enabled: bool, info: &TcInfo, tc: &[u8]) -> bool;
}

impl TcReleaser for PacketSenderWithSharedPool {
    fn release(
        &mut self,
        sender_id: ComponentId,
        enabled: bool,
        _info: &TcInfo,
        tc: &[u8],
    ) -> bool {
        if enabled {
            let shared_pool = self.shared_pool.get_mut();
            // Transfer TC from scheduler TC pool to shared TC pool.
            let released_tc_addr = shared_pool
                .0
                .write()
                .expect("locking pool failed")
                .add(tc)
                .expect("adding TC to shared pool failed");
            self.sender
                .send(PacketInPool::new(sender_id, released_tc_addr))
                .expect("sending TC to TC source failed");
        }
        true
    }
}

impl TcReleaser for mpsc::Sender<PacketAsVec> {
    fn release(
        &mut self,
        sender_id: ComponentId,
        enabled: bool,
        _info: &TcInfo,
        tc: &[u8],
    ) -> bool {
        if enabled {
            // Send released TC to centralized TC source.
            self.send(PacketAsVec::new(sender_id, tc.to_vec()))
                .expect("sending TC to TC source failed");
        }
        true
    }
}

pub struct SchedulingServiceWrapper<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter>
{
    pub pus_11_handler: PusSchedServiceHandler<
        MpscTcReceiver,
        TmSender,
        TcInMemConverter,
        VerificationReporter,
        PusScheduler,
    >,
    pub sched_tc_pool: StaticMemoryPool,
    pub releaser_buf: [u8; 4096],
    pub tc_releaser: Box<dyn TcReleaser + Send>,
}

impl<TmSender: EcssTmSender, TcInMemConverter: EcssTcInMemConverter>
    SchedulingServiceWrapper<TmSender, TcInMemConverter>
{
    pub fn release_tcs(&mut self) {
        let id = self.pus_11_handler.service_helper.id();
        let releaser = |enabled: bool, info: &TcInfo, tc: &[u8]| -> bool {
            self.tc_releaser.release(id, enabled, info, tc)
        };

        self.pus_11_handler
            .scheduler_mut()
            .update_time_from_now()
            .unwrap();
        let released_tcs = self
            .pus_11_handler
            .scheduler_mut()
            .release_telecommands_with_buffer(
                releaser,
                &mut self.sched_tc_pool,
                &mut self.releaser_buf,
            )
            .expect("releasing TCs failed");
        if released_tcs > 0 {
            info!("{released_tcs} TC(s) released from scheduler");
        }
    }

    pub fn poll_and_handle_next_tc(&mut self, time_stamp: &[u8]) -> HandlingStatus {
        match self
            .pus_11_handler
            .poll_and_handle_next_tc(time_stamp, &mut self.sched_tc_pool)
        {
            Ok(result) => match result {
                PusPacketHandlerResult::RequestHandled => {}
                PusPacketHandlerResult::RequestHandledPartialSuccess(e) => {
                    warn!("PUS11 partial packet handling success: {e:?}")
                }
                PusPacketHandlerResult::CustomSubservice(invalid, _) => {
                    warn!("PUS11 invalid subservice {invalid}");
                }
                PusPacketHandlerResult::SubserviceNotImplemented(subservice, _) => {
                    warn!("PUS11: Subservice {subservice} not implemented");
                }
                PusPacketHandlerResult::Empty => return HandlingStatus::Empty,
            },
            Err(error) => {
                error!("PUS packet handling error: {error:?}")
            }
        }
        HandlingStatus::HandledOne
    }
}

pub fn create_scheduler_service_static(
    tm_sender: PacketSenderWithSharedPool,
    tc_releaser: PacketSenderWithSharedPool,
    pus_sched_rx: mpsc::Receiver<EcssTcAndToken>,
    sched_tc_pool: StaticMemoryPool,
) -> SchedulingServiceWrapper<PacketSenderWithSharedPool, EcssTcInSharedStoreConverter> {
    let scheduler = PusScheduler::new_with_current_init_time(Duration::from_secs(5))
        .expect("Creating PUS Scheduler failed");
    let pus_11_handler = PusSchedServiceHandler::new(
        PusServiceHelper::new(
            PUS_SCHED_SERVICE.id(),
            pus_sched_rx,
            tm_sender,
            create_verification_reporter(PUS_SCHED_SERVICE.id(), PUS_SCHED_SERVICE.apid),
            EcssTcInSharedStoreConverter::new(tc_releaser.shared_packet_store().0.clone(), 2048),
        ),
        scheduler,
    );
    SchedulingServiceWrapper {
        pus_11_handler,
        sched_tc_pool,
        releaser_buf: [0; 4096],
        tc_releaser: Box::new(tc_releaser),
    }
}

pub fn create_scheduler_service_dynamic(
    tm_funnel_tx: mpsc::Sender<PacketAsVec>,
    tc_source_sender: mpsc::Sender<PacketAsVec>,
    pus_sched_rx: mpsc::Receiver<EcssTcAndToken>,
    sched_tc_pool: StaticMemoryPool,
) -> SchedulingServiceWrapper<MpscTmAsVecSender, EcssTcInVecConverter> {
    //let sched_srv_receiver =
    //MpscTcReceiver::new(PUS_SCHED_SERVICE.raw(), "PUS_11_TC_RECV", pus_sched_rx);
    let scheduler = PusScheduler::new_with_current_init_time(Duration::from_secs(5))
        .expect("Creating PUS Scheduler failed");
    let pus_11_handler = PusSchedServiceHandler::new(
        PusServiceHelper::new(
            PUS_SCHED_SERVICE.id(),
            pus_sched_rx,
            tm_funnel_tx,
            create_verification_reporter(PUS_SCHED_SERVICE.id(), PUS_SCHED_SERVICE.apid),
            EcssTcInVecConverter::default(),
        ),
        scheduler,
    );
    SchedulingServiceWrapper {
        pus_11_handler,
        sched_tc_pool,
        releaser_buf: [0; 4096],
        tc_releaser: Box::new(tc_source_sender),
    }
}