//! Single-owner MAVLink transmit service.

use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
use kaptan_mavlink::{MavlinkFrame, MavlinkSender, message::MavState};
use kaptan_sensor::ImuSnapshot;
use kaptan_tx::{FrameClass, PendingFrame, TransmitQueue};

/// Failure while preparing or starting one telemetry frame.
#[derive(Debug)]
pub enum TelemetryFault<MemoryError, EngineError> {
    /// DMA-visible frame preparation failed.
    Memory(MemoryError),
    /// The UART DMA route rejected a transfer start.
    Engine(EngineError),
}

/// Owns MAVLink packet sequencing and UART DMA frame lifecycle.
///
/// Exactly one worker invokes this service. It is not shared with sensor or
/// hardware IRQ execution.
pub struct TelemetryService<Memory, Engine, const N: usize> {
    mavlink: MavlinkSender,
    transmit: TransmitQueue<MavlinkFrame, N>,
    memory: Memory,
    engine: Engine,
}

impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N> {
    /// Creates an empty telemetry service.
    pub const fn new(system_id: u8, component_id: u8, memory: Memory, engine: Engine) -> Self {
        Self {
            mavlink: MavlinkSender::new(system_id, component_id),
            transmit: TransmitQueue::new(),
            memory,
            engine,
        }
    }
}

impl<Memory, Engine, const N: usize> TelemetryService<Memory, Engine, N>
where
    Memory: DmaTxBufferFactory,
    Engine: DmaTxEngine,
{
    /// Queues the newest accepted IMU observation.
    pub fn publish_sample(
        &mut self,
        sample: &ImuSnapshot,
    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
        let frame = self.mavlink.raw_imu_snapshot(sample);
        self.enqueue_and_start(FrameClass::BestEffort, frame)
    }

    /// Queues one liveness heartbeat.
    pub fn publish_heartbeat(
        &mut self,
    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
        let frame = self.mavlink.heartbeat(MavState::MAV_STATE_STANDBY);
        self.enqueue_and_start(FrameClass::Critical, frame)
    }

    /// Releases a terminal DMA frame and opportunistically starts the next.
    ///
    /// Telemetry is best-effort: a terminal route fault drops only the active
    /// frame and a rejected follow-up start is retried on later publication.
    pub fn on_dma_complete(&mut self, completion: TxCompletion) {
        match completion {
            TxCompletion::NotPending => {}
            TxCompletion::Complete => {
                let _ = self.transmit.complete();
                let _ = self.start_next();
            }
            TxCompletion::TransferFault | TxCompletion::Halted => {
                let _ = self.transmit.fail();
            }
        }
    }

    fn enqueue_and_start(
        &mut self,
        class: FrameClass,
        frame: MavlinkFrame,
    ) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
        let _ = self.transmit.enqueue(PendingFrame { class, frame });
        self.start_next()
    }

    fn start_next(&mut self) -> Result<(), TelemetryFault<Memory::Error, Engine::StartError>> {
        let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
        transmit
            .start_with(|frame| {
                let buffer = memory
                    .prepare(frame.raw_bytes())
                    .map_err(TelemetryFault::Memory)?;
                engine.start(buffer).map_err(TelemetryFault::Engine)
            })
            .map(|_| ())
    }
}

#[cfg(test)]
mod tests {
    use super::TelemetryService;
    use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
    use kaptan_sensor::ImuSnapshot;
    use kaptan_time::Timestamp;

    struct Memory;

    impl DmaTxBufferFactory for Memory {
        type Error = core::convert::Infallible;

        fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
            // SAFETY: this host-only test keeps the queue's active frame alive.
            Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
        }
    }

    struct Engine;

    impl DmaTxEngine for Engine {
        type StartError = core::convert::Infallible;

        fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
            Ok(())
        }
    }

    fn sample(timestamp: u64) -> ImuSnapshot {
        ImuSnapshot::new(Timestamp::from_microseconds(timestamp), [0; 3], [0; 3], 0)
    }

    #[test]
    fn transfer_fault_drops_only_the_active_frame() {
        let mut service = TelemetryService::<Memory, Engine, 2>::new(1, 1, Memory, Engine);
        service.publish_sample(&sample(1)).unwrap();
        service.publish_sample(&sample(2)).unwrap();
        assert!(service.transmit.is_active());

        service.on_dma_complete(TxCompletion::TransferFault);
        assert!(!service.transmit.is_active());
    }
}