//! Physical-sensor telemetry STM32F722 board-I/O state and ownership graph.

use core::sync::atomic::{AtomicU8, Ordering};

use super::peripherals::{
    DwtMicrosecondClock, I2c1, I2c1Config, ReceiveInterrupt, Tim2Periodic, Uart4,
    configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9,
    configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12,
    configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9,
    configure_usart2_pins_pa2_pa3,
};
use kaptan_board::{BoardConfig, VerticalSensingKind};
use kaptan_data_channel::{
    DataChannel, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, LatestRangeMailbox,
};
use kaptan_drivers::{
    lidar_lite_v3::{AcquisitionMode, LidarLiteV3, LidarLiteV3Service, LidarLiteV3ServiceEvent},
    mtf01::Mtf01Service,
};
use kaptan_log::{
    DeliveryClass, ErrorReport, EventHub, KaptanError, LogFilter, LogFlags, StaticError,
    StaticErrorMailbox, TransportAdmission,
};
use kaptan_log_mavlink::{DiagnosticPayload, encode_diagnostic};
use kaptan_spi::SpiCompletion;
use stm32f7::stm32f722;

use crate::{BoardError, F722BoardRoutes, F722Profile};

use super::{
    VerticalRoute, clock,
    data_channel::{
        F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG,
        vertical_data_config,
    },
    interrupts,
    sensors::{F722Imu, ImuEvent},
};

const UART4_CLOCK_HZ: u32 = 54_000_000;
const MTF01_BAUD: u32 = 115_200;
const I2C1_CONFIG: I2c1Config = I2c1Config::from_timing_register(0x2040_4768);
const LIDAR_MODE: AcquisitionMode = AcquisitionMode::WithReceiverBiasCorrection;
const LIDAR_STARTUP_TICKS: u8 = 3;
const ERROR_RECORDS_PER_SECOND: u8 = 10;
const LOG_RECORDS_PER_SECOND: u8 = 5;
const SCHEDULED_FRAMES_PER_SECOND: usize = 62;
const SCHEDULED_FRAME_MAXIMUM_BYTES: usize = 48;
const MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES: usize = 12;
const KAPTAN_ERROR_FRAME_MAXIMUM_BYTES: usize =
    kaptan_mavlink::message::kaptan::KAPTAN_ERROR_DATA::ENCODED_LEN
        + MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES;
const KAPTAN_LOG_FRAME_MAXIMUM_BYTES: usize =
    kaptan_mavlink::message::kaptan::KAPTAN_LOG_DATA::ENCODED_LEN
        + MAVLINK_V2_UNSIGNED_OVERHEAD_BYTES;
// The static F722 telemetry profile is bounded at 62 scheduled frames/s.
// It permits ten `KAPTAN_ERROR` and five `KAPTAN_LOG` frames/s. Both maxima
// are 163 payload bytes plus 12 unsigned MAVLink V2 framing bytes, so
// 62×48 + 15×175 = 5,601 bytes/s. MAVLink signing is not enabled in this
// telemetry-only image; any future signing profile must re-budget its
// signature overhead before it can enable the endpoint.
pub(crate) const WORST_CASE_OBSERVABILITY_BYTES_PER_SECOND: usize = SCHEDULED_FRAMES_PER_SECOND
    * SCHEDULED_FRAME_MAXIMUM_BYTES
    + ERROR_RECORDS_PER_SECOND as usize * KAPTAN_ERROR_FRAME_MAXIMUM_BYTES
    + LOG_RECORDS_PER_SECOND as usize * KAPTAN_LOG_FRAME_MAXIMUM_BYTES;

/// Returns whether the immutable board UART configuration can carry the fixed
/// telemetry and bounded diagnostic schedule at ten UART bits per byte.
pub(crate) const fn link_budget_fits(baud: u32) -> bool {
    WORST_CASE_OBSERVABILITY_BYTES_PER_SECOND < baud as usize / 10
}
static UART_COMPLETION: AtomicU8 = AtomicU8::new(0);
#[derive(Clone, Copy, Debug, KaptanError)]
enum F722TelemetryError {
    Mtf01Uart,
    TransmitMemory,
    TransmitEngine,
    TransmitTerminal,
}
// High-priority/ISR-adjacent producers never encode, lock, allocate, or send
// MAVLink. They retain the first fault text and coalesce later reports for
// the low-priority MAVLink worker to publish.
static IMU_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
static LIDAR_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
static MTF01_FAULTS: StaticErrorMailbox = StaticErrorMailbox::new();
static SAMPLES: LatestImuMailbox = LatestImuMailbox::new();
static RANGES: LatestRangeMailbox = LatestRangeMailbox::new();
static OPTICAL_FLOW: LatestOpticalFlowMailbox = LatestOpticalFlowMailbox::new();

type F722Range = LidarLiteV3Service<DwtMicrosecondClock, I2c1>;
/// Opaque RTIC-local MTF-01 service type; its route is platform-owned.
pub type F722Mtf01 = Mtf01Service<DwtMicrosecondClock>;
/// Opaque RTIC-local UART4 owner; its construction is platform-owned.
pub type F722Mtf01Uart = Uart4;
/// Opaque RTIC-local service timer; its construction is platform-owned.
pub type F722ServiceTimer = Tim2Periodic;

/// RTIC-shared MAVLink state.
///
/// Only the MAVLink worker accesses this state, so its priority ceiling is 9
/// rather than the IMU or vertical-sensor IRQ ceiling.
pub struct F722MavlinkState {
    data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
    logs: EventHub<2, 4>,
    budget_ticks: u8,
    error_budget: u8,
    log_budget: u8,
    led: F722StatusLed,
}

/// RTIC-shared LIDAR state.
///
/// I2C event/error IRQs and the service timer are its only users. MTF-01 is a
/// separate UART-only local resource, so it cannot raise this ceiling.
pub struct F722LidarState {
    range: Option<F722Range>,
    startup_ticks: u8,
}

/// Explicit handoff from platform initialization to the RTIC resource graph.
///
/// This is not runtime state indirection: each field is moved exactly once
/// into the single RTIC owner that accesses it.
pub struct F722InitResources {
    pub(crate) imu: F722Imu,
    pub(crate) mtf01: Option<F722Mtf01>,
    pub(crate) mtf01_uart: Option<F722Mtf01Uart>,
    pub(crate) timer: F722ServiceTimer,
    pub(crate) lidar: F722LidarState,
    pub(crate) mavlink: F722MavlinkState,
}

/// Validates the relationship between portable metadata and selected sensor
/// route before platform initialization claims a peripheral.
pub(crate) fn validate_board_routes(
    config: BoardConfig,
    routes: F722BoardRoutes,
) -> Result<(), BoardError> {
    let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
    if telemetry.data_channel.service_hz != SERVICE_HZ {
        return Err(BoardError::UnsupportedServiceFrequency);
    }
    let expected = match routes.vertical {
        VerticalRoute::None => VerticalSensingKind::None,
        VerticalRoute::Mtf01 => VerticalSensingKind::RangeAndOpticalFlow,
        VerticalRoute::LidarLiteV3 => VerticalSensingKind::Range,
    };
    if telemetry.vertical_sensing != expected {
        return Err(BoardError::VerticalSensorCapabilityMismatch);
    }
    if telemetry.imu != routes.imu() {
        return Err(BoardError::ImuRouteMismatch);
    }
    if telemetry.imu_mount != routes.imu_mount() {
        return Err(BoardError::ImuMountMismatch);
    }
    if config.mavlink.port != routes.mavlink_port() {
        return Err(BoardError::MavlinkRouteMismatch);
    }
    if !link_budget_fits(config.mavlink.baud) {
        return Err(BoardError::TelemetryLinkOvercommitted);
    }
    Ok(())
}

pub(crate) fn initialize(
    mut core: cortex_m::Peripherals,
    device: stm32f722::Peripherals,
    config: BoardConfig,
    routes: F722BoardRoutes,
) -> Result<F722InitResources, BoardError> {
    let clock = clock::initialize(&mut core, &device).map_err(|_| BoardError::Initialization)?;

    // The profile is written once before any interrupt is enabled. ISR helpers
    // read it with Acquire ordering and only access the initialized route.
    interrupts::activate_profile(routes.profile());

    match routes.profile() {
        F722Profile::Mpu6000Spi1Usart1 => {
            initialize_mpu6000(device, clock, config, routes.vertical)
        }
        F722Profile::Icm42688Spi3Usart2 => {
            initialize_icm42688(device, clock, config, routes.vertical)
        }
    }
}

fn initialize_mpu6000(
    device: stm32f722::Peripherals,
    clock: DwtMicrosecondClock,
    config: BoardConfig,
    vertical: VerticalRoute,
) -> Result<F722InitResources, BoardError> {
    configure_spi1_pins_pa5_pa6_pa7(&device.RCC, &device.GPIOA);
    configure_exti4_pin_pa4(&device.GPIOA);
    configure_exti4(&device.RCC, device.SYSCFG, device.EXTI);
    configure_usart1_tx_pin_pa9(&device.GPIOA);
    configure_usart1_rx_pin_pa10(&device.GPIOA);
    configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);

    let (range, mtf01, mtf01_uart) = initialize_vertical(
        &device.RCC,
        device.I2C1,
        device.UART4,
        clock.fork(),
        clock.fork(),
        vertical,
    )?;
    let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
    let imu = F722Imu::mpu6000(
        &device.RCC,
        device.SPI1,
        device.DMA2,
        device.GPIOB,
        clock.fork(),
        telemetry.imu_mount,
    );
    let transmitter =
        F722MavlinkTransmitter::usart1(&device.RCC, device.USART1, config.mavlink.baud)
            .map_err(|_| BoardError::Initialization)?;
    let led = F722StatusLed::Kakute(super::peripherals::ActiveLowLedPd2::new(
        &device.RCC,
        device.GPIOD,
    ));
    finish(
        &device.RCC,
        device.TIM2,
        config,
        vertical,
        imu,
        range,
        mtf01,
        mtf01_uart,
        transmitter,
        led,
    )
}

fn initialize_icm42688(
    device: stm32f722::Peripherals,
    clock: DwtMicrosecondClock,
    config: BoardConfig,
    vertical: VerticalRoute,
) -> Result<F722InitResources, BoardError> {
    configure_spi3_pins_pc10_pc11_pc12(&device.RCC, &device.GPIOC);
    configure_exti3_pin_pc3(&device.GPIOC);
    configure_exti3(&device.RCC, device.SYSCFG, device.EXTI);
    // PA2/PA3 writes take effect only after enabling the GPIOA bus clock.
    // The ICM route enables GPIOA later for chip select, which is too late for
    // the USART2 alternate-function configuration below.
    device
        .RCC
        .ahb1enr()
        .modify(|_, write| write.gpioaen().set_bit());
    configure_usart2_pins_pa2_pa3(&device.GPIOA);
    configure_vertical_pins(&device.RCC, &device.GPIOA, &device.GPIOB, vertical);

    let (range, mtf01, mtf01_uart) = initialize_vertical(
        &device.RCC,
        device.I2C1,
        device.UART4,
        clock.fork(),
        clock.fork(),
        vertical,
    )?;
    let telemetry = config.telemetry().ok_or(BoardError::Initialization)?;
    let imu = F722Imu::icm42688(
        &device.RCC,
        device.SPI3,
        device.DMA1,
        &device.GPIOA,
        clock.fork(),
        telemetry.imu_mount,
    );
    let transmitter =
        F722MavlinkTransmitter::usart2(&device.RCC, device.USART2, config.mavlink.baud)
            .map_err(|_| BoardError::Initialization)?;
    let led = F722StatusLed::Geprc(GeprcStatusLed::new(&device.RCC, device.GPIOA));
    finish(
        &device.RCC,
        device.TIM2,
        config,
        vertical,
        imu,
        range,
        mtf01,
        mtf01_uart,
        transmitter,
        led,
    )
}

#[allow(clippy::too_many_arguments)]
fn finish(
    rcc: &stm32f722::RCC,
    tim2: stm32f722::TIM2,
    config: BoardConfig,
    vertical: VerticalRoute,
    imu: F722Imu,
    range: Option<F722Range>,
    mtf01: Option<F722Mtf01>,
    mtf01_uart: Option<Uart4>,
    transmitter: F722MavlinkTransmitter,
    led: F722StatusLed,
) -> Result<F722InitResources, BoardError> {
    let data_channel = DataChannel::new(
        transmitter,
        config,
        vertical_data_config(vertical),
        DataSources {
            imu: &SAMPLES,
            range: &RANGES,
            optical_flow: &OPTICAL_FLOW,
        },
    )
    .map_err(|_| BoardError::Initialization)?;
    Ok(F722InitResources {
        imu,
        mtf01,
        mtf01_uart,
        timer: Tim2Periodic::new(rcc, tim2, SERVICE_TIMER_CONFIG),
        lidar: F722LidarState {
            range,
            startup_ticks: 0,
        },
        mavlink: F722MavlinkState {
            data_channel,
            logs: EventHub::new(LogFilter::new(
                config
                    .telemetry()
                    .ok_or(BoardError::Initialization)?
                    .maximum_log_level,
            )),
            budget_ticks: 0,
            error_budget: ERROR_RECORDS_PER_SECOND,
            log_budget: LOG_RECORDS_PER_SECOND,
            led,
        },
    })
}

fn configure_vertical_pins(
    rcc: &stm32f722::RCC,
    gpioa: &stm32f722::GPIOA,
    gpiob: &stm32f722::GPIOB,
    vertical: VerticalRoute,
) {
    match vertical {
        VerticalRoute::None => {}
        VerticalRoute::Mtf01 => configure_uart4_pins_pa0_pa1(gpioa),
        VerticalRoute::LidarLiteV3 => configure_i2c1_pins_pb8_pb9(rcc, gpiob),
    }
}

type VerticalResources = (Option<F722Range>, Option<F722Mtf01>, Option<Uart4>);

fn initialize_vertical(
    rcc: &stm32f722::RCC,
    i2c: stm32f722::I2C1,
    uart4: stm32f722::UART4,
    mtf01_clock: DwtMicrosecondClock,
    range_clock: DwtMicrosecondClock,
    vertical: VerticalRoute,
) -> Result<VerticalResources, BoardError> {
    match vertical {
        VerticalRoute::None => Ok((None, None, None)),
        VerticalRoute::Mtf01 => Ok((
            None,
            Some(Mtf01Service::new(mtf01_clock)),
            Some(
                Uart4::new(rcc, uart4, UART4_CLOCK_HZ, MTF01_BAUD)
                    .map_err(|_| BoardError::Initialization)?,
            ),
        )),
        VerticalRoute::LidarLiteV3 => Ok((
            Some(LidarLiteV3Service::new(
                range_clock,
                LidarLiteV3::new(I2c1::new(rcc, i2c, I2C1_CONFIG)),
                LIDAR_MODE,
            )),
            None,
            None,
        )),
    }
}

fn configure_exti4(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
    rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
    syscfg.exticr2().modify(|_, write| {
        // SAFETY: selector zero is GPIOA for EXTI4 on STM32F722.
        unsafe { write.exti4().bits(0) }
    });
    exti.imr().modify(|_, write| write.im4().set_bit());
    exti.rtsr().modify(|_, write| write.tr4().set_bit());
    exti.ftsr().modify(|_, write| write.tr4().clear_bit());
    exti.pr().write(|write| write.pr4().clear());
}

fn configure_exti3(rcc: &stm32f722::RCC, syscfg: stm32f722::SYSCFG, exti: stm32f722::EXTI) {
    rcc.apb2enr().modify(|_, write| write.syscfgen().set_bit());
    syscfg.exticr1().modify(|_, write| {
        // SAFETY: selector two is GPIOC for EXTI3 on STM32F722.
        unsafe { write.exti3().bits(2) }
    });
    exti.imr().modify(|_, write| write.im3().set_bit());
    exti.rtsr().modify(|_, write| write.tr3().set_bit());
    exti.ftsr().modify(|_, write| write.tr3().clear_bit());
    exti.pr().write(|write| write.pr3().clear());
}

/// Services the MTF-01 UART route. It is a task-local resource, so it needs
/// no RTIC lock and cannot extend another resource's priority ceiling.
pub(crate) fn on_uart4_interrupt(mtf01: &mut Option<F722Mtf01>) {
    let Some(mtf01) = mtf01.as_mut() else {
        return;
    };
    match Uart4::acknowledge_receive_interrupt() {
        ReceiveInterrupt::Byte(byte) => {
            if let Some(report) = mtf01.on_received_byte(byte) {
                if let Some(range) = report.range_snapshot() {
                    RANGES.publish(range);
                }
                if let Some(flow) = report.optical_flow_snapshot() {
                    OPTICAL_FLOW.publish(flow);
                }
            }
        }
        ReceiveInterrupt::Fault => {
            mtf01.reset();
            record_mtf01_fault(F722TelemetryError::Mtf01Uart.static_error());
        }
        ReceiveInterrupt::NotPending => {}
    }
}

impl F722LidarState {
    pub(crate) fn on_i2c_event(&mut self, error: bool) {
        let Some(range) = self.range.as_mut() else {
            return;
        };
        let event = range.on_controller_interrupt(|i2c| {
            if error {
                i2c.acknowledge_error_interrupt()
            } else {
                i2c.acknowledge_event_interrupt()
            }
        });
        publish_lidar_event(event);
    }

    pub(crate) fn on_timer(&mut self) {
        if let Some(range) = self.range.as_mut() {
            if self.startup_ticks < LIDAR_STARTUP_TICKS {
                self.startup_ticks += 1;
                if self.startup_ticks == LIDAR_STARTUP_TICKS
                    && let Err(error) = range.finish_startup()
                {
                    record_lidar_fault(error.static_error());
                }
            } else {
                publish_lidar_event(range.on_periodic_tick());
            }
        }
    }
}

/// Advances the IMU state machine in its dedicated worker-local resource.
pub(crate) fn process_imu(imu: &mut F722Imu, events: u8) {
    const START: u8 = 1 << 4;
    const TRANSFER_FAULT: u8 = 1 << 2;
    const TRANSFER_COMPLETE: u8 = 1 << 1;
    const TIMER: u8 = 1 << 3;
    const DRDY: u8 = 1 << 0;

    if events & START != 0 {
        accept_imu_event(imu.begin());
    }
    if events & TRANSFER_FAULT != 0 {
        accept_imu_event(imu.on_transfer_complete(SpiCompletion::TransferFault));
    }
    if events & TRANSFER_COMPLETE != 0 {
        accept_imu_event(imu.on_transfer_complete(SpiCompletion::Complete));
    }
    if events & TIMER != 0 {
        accept_imu_event(imu.on_periodic_tick());
    }
    if events & DRDY != 0 {
        accept_imu_event(imu.on_data_ready());
    }
}

fn accept_imu_event(event: ImuEvent) {
    match event {
        ImuEvent::Sample(sample) => {
            SAMPLES.publish(sample);
        }
        ImuEvent::Fault(message) => record_imu_fault(message),
        ImuEvent::Idle | ImuEvent::Initialized => {}
    }
}

impl F722MavlinkState {
    /// Handles bounded DMA and timer work for the telemetry image.
    pub(crate) fn process_worker_events(&mut self, events: u8) {
        const TIMER: u8 = 1 << 0;
        const DMA: u8 = 1 << 2;

        if events & DMA != 0 {
            let completion = take_mavlink_completion();
            let _ = self.data_channel.on_dma_complete(completion);
            if completion == kaptan_dma::TxCompletion::Complete {
                self.led.toggle();
            }
        }

        if events & TIMER != 0 {
            // The board is initialized and transmitting physical telemetry,
            // but has no flight runtime or output authority. `STANDBY` with
            // an unarmed base mode communicates that accurately; `UNINIT`
            // would incorrectly claim initialization never completed.
            self.data_channel
                .on_service_tick(kaptan_mavlink::message::MavState::MAV_STATE_STANDBY, false);
            self.drain_driver_faults();
            self.budget_ticks = self.budget_ticks.saturating_add(1);
            if self.budget_ticks == SERVICE_HZ as u8 {
                self.budget_ticks = 0;
                self.error_budget = ERROR_RECORDS_PER_SECOND;
                self.log_budget = LOG_RECORDS_PER_SECOND;
            }
            self.drain_one_log_record();
        }
    }

    fn drain_driver_faults(&mut self) {
        self.drain_fault_mailbox(&IMU_FAULTS);
        self.drain_fault_mailbox(&LIDAR_FAULTS);
        self.drain_fault_mailbox(&MTF01_FAULTS);
        self.drain_transport_faults();
    }

    fn drain_transport_faults(&mut self) {
        while let Some(fault) = self.data_channel.take_transport_fault() {
            let message = match fault {
                kaptan_mavlink::transmitter::TransmitFaultKind::Memory => {
                    F722TelemetryError::TransmitMemory.static_error()
                }
                kaptan_mavlink::transmitter::TransmitFaultKind::Engine => {
                    F722TelemetryError::TransmitEngine.static_error()
                }
                kaptan_mavlink::transmitter::TransmitFaultKind::Terminal => {
                    F722TelemetryError::TransmitTerminal.static_error()
                }
            };
            let _ = self.logs.publish_error_static(LogFlags::NONE, message);
        }
    }

    fn drain_fault_mailbox(&mut self, mailbox: &StaticErrorMailbox) {
        let (report, coalesced) = mailbox.take();
        let Some(message) = report else {
            return;
        };
        let flags = if coalesced {
            LogFlags::COALESCED
        } else {
            LogFlags::NONE
        };
        let _ = self.logs.publish_error_static(flags, message);
    }

    fn drain_one_log_record(&mut self) {
        let Some(record) = self.logs.peek() else {
            return;
        };
        let delivery = record.delivery();
        if (delivery == DeliveryClass::Critical && self.error_budget == 0)
            || (delivery == DeliveryClass::BestEffort && self.log_budget == 0)
        {
            return;
        }
        let data_channel = &mut self.data_channel;
        let mut accepted = None;
        let _ = self.logs.submit_next(|record| {
            let class = match record.delivery() {
                DeliveryClass::Critical => kaptan_mavlink::transmitter::FrameClass::Critical,
                DeliveryClass::BestEffort => kaptan_mavlink::transmitter::FrameClass::BestEffort,
            };
            let payload = encode_diagnostic(record);
            let admission = match &payload {
                DiagnosticPayload::Log(payload) => data_channel.send_custom(class, payload),
                DiagnosticPayload::Error(payload) => data_channel.send_custom(class, payload),
            };
            let outcome = if admission.is_retained() {
                accepted = Some(record.delivery());
                TransportAdmission::Accepted
            } else if record.delivery() == DeliveryClass::BestEffort {
                TransportAdmission::DropBestEffort
            } else {
                TransportAdmission::Retry
            };
            (outcome, ())
        });
        match accepted {
            Some(DeliveryClass::Critical) => {
                self.error_budget = self.error_budget.saturating_sub(1);
            }
            Some(DeliveryClass::BestEffort) => {
                self.log_budget = self.log_budget.saturating_sub(1);
            }
            None => {}
        }
    }
}

pub(crate) fn take_mavlink_completion() -> kaptan_dma::TxCompletion {
    match UART_COMPLETION.swap(0, Ordering::AcqRel) {
        1 => kaptan_dma::TxCompletion::Complete,
        2 => kaptan_dma::TxCompletion::TransferFault,
        _ => kaptan_dma::TxCompletion::NotPending,
    }
}

pub fn record_mavlink_completion(completion: kaptan_dma::TxCompletion) {
    let code = match completion {
        kaptan_dma::TxCompletion::Complete => 1,
        kaptan_dma::TxCompletion::TransferFault | kaptan_dma::TxCompletion::Halted => 2,
        kaptan_dma::TxCompletion::NotPending => 0,
    };
    if code != 0 {
        UART_COMPLETION.store(code, Ordering::Release);
    }
}

fn publish_lidar_event<E: ErrorReport>(event: LidarLiteV3ServiceEvent<E>) {
    match event {
        LidarLiteV3ServiceEvent::Measurement(snapshot) => RANGES.publish(snapshot),
        LidarLiteV3ServiceEvent::Fault(error) => record_lidar_fault(error.static_error()),
        LidarLiteV3ServiceEvent::Idle => {}
    }
}

fn record_imu_fault(message: &'static StaticError) {
    IMU_FAULTS.record(message);
}

fn record_lidar_fault(message: &'static StaticError) {
    LIDAR_FAULTS.record(message);
}

fn record_mtf01_fault(message: &'static StaticError) {
    MTF01_FAULTS.record(message);
}