//! 1 Hz outbound HEARTBEAT scheduling + inbound-heartbeat timeout tracking.
//!
//! Per AZ-641: emit a HEARTBEAT every 1 s ± 50 ms; if the autopilot stops
//! emitting heartbeats for longer than `link_lost_timeout` (default 3 s),
//! flip the link state to `lost` and fire a typed `LinkLost` signal.

use std::sync::atomic::{AtomicI64, AtomicU64, Ordering};
use std::sync::Arc;
use std::time::Duration;

use tokio::sync::broadcast;

use crate::internal::codec::messages::{Heartbeat, MavlinkMessage};

/// MAVLink type byte for "onboard companion computer" / generic offboard.
const MAV_TYPE_ONBOARD_COMPUTER: u8 = 18; // MAV_TYPE_ONBOARD_CONTROLLER
const MAV_AUTOPILOT_INVALID: u8 = 8;
const MAV_STATE_ACTIVE: u8 = 4;
const MAVLINK_VERSION: u8 = 3;

/// Default emit cadence in milliseconds (1 Hz).
pub const HEARTBEAT_PERIOD_MS: u64 = 1000;
/// Default wall-clock timeout before flipping to LinkLost.
pub const DEFAULT_LINK_TIMEOUT_MS: u64 = 3000;

#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum LinkEvent {
    /// The link became healthy (first inbound heartbeat after init or recovery).
    LinkUp,
    /// No inbound heartbeat within timeout.
    LinkLost,
}

/// Inbound-heartbeat watchdog shared between the run loop and the health surface.
#[derive(Debug)]
pub struct InboundWatchdog {
    /// Last inbound heartbeat at this monotonic ms-since-start (negative = never seen).
    last_inbound_ms: AtomicI64,
    /// Outbound heartbeats sent since start (for diagnostics).
    outbound_count: AtomicU64,
    /// Currently considered link-up.
    link_up: std::sync::atomic::AtomicBool,
    timeout_ms: u64,
    started: std::time::Instant,
    signal: broadcast::Sender<LinkEvent>,
}

impl InboundWatchdog {
    pub fn new(timeout_ms: u64) -> (Arc<Self>, broadcast::Receiver<LinkEvent>) {
        let (tx, rx) = broadcast::channel(16);
        (
            Arc::new(Self {
                last_inbound_ms: AtomicI64::new(-1),
                outbound_count: AtomicU64::new(0),
                link_up: std::sync::atomic::AtomicBool::new(false),
                timeout_ms,
                started: std::time::Instant::now(),
                signal: tx,
            }),
            rx,
        )
    }

    /// Record that we just observed an inbound heartbeat from the peer.
    pub fn note_inbound_heartbeat(&self) {
        let now = self.elapsed_ms();
        self.last_inbound_ms.store(now, Ordering::Relaxed);
        let was_up = self.link_up.swap(true, Ordering::SeqCst);
        if !was_up {
            let _ = self.signal.send(LinkEvent::LinkUp);
        }
    }

    /// Record an outbound heartbeat we just emitted (used in health detail).
    pub fn note_outbound_heartbeat(&self) {
        self.outbound_count.fetch_add(1, Ordering::Relaxed);
    }

    /// Returns true if the link timeout has been exceeded.
    pub fn check_timeout_now(&self) -> bool {
        let last = self.last_inbound_ms.load(Ordering::Relaxed);
        if last < 0 {
            // Never seen an inbound heartbeat — count from start so that we
            // surface `LinkLost` quickly when the peer is unreachable.
            return self.elapsed_ms() > self.timeout_ms as i64;
        }
        (self.elapsed_ms() - last) > self.timeout_ms as i64
    }

    /// Trip the link to LinkLost if currently up and timeout has elapsed; idempotent.
    pub fn maybe_trip_link_lost(&self) -> bool {
        if self.check_timeout_now() {
            let was_up = self.link_up.swap(false, Ordering::SeqCst);
            if was_up {
                let _ = self.signal.send(LinkEvent::LinkLost);
                return true;
            }
        }
        false
    }

    pub fn last_inbound_age_ms(&self) -> Option<u64> {
        let last = self.last_inbound_ms.load(Ordering::Relaxed);
        if last < 0 {
            None
        } else {
            Some((self.elapsed_ms() - last).max(0) as u64)
        }
    }

    pub fn outbound_total(&self) -> u64 {
        self.outbound_count.load(Ordering::Relaxed)
    }

    pub fn link_up(&self) -> bool {
        self.link_up.load(Ordering::Relaxed)
    }

    pub fn subscribe(&self) -> broadcast::Receiver<LinkEvent> {
        self.signal.subscribe()
    }

    fn elapsed_ms(&self) -> i64 {
        self.started.elapsed().as_millis() as i64
    }
}

/// Build the canonical outbound HEARTBEAT message announcing our identity to
/// the autopilot peer.
pub fn make_outbound_heartbeat() -> MavlinkMessage {
    MavlinkMessage::Heartbeat(Heartbeat {
        custom_mode: 0,
        mavtype: MAV_TYPE_ONBOARD_COMPUTER,
        autopilot: MAV_AUTOPILOT_INVALID,
        base_mode: 0,
        system_status: MAV_STATE_ACTIVE,
        mavlink_version: MAVLINK_VERSION,
    })
}

/// Period at which the outbound heartbeat is scheduled.
pub fn heartbeat_period() -> Duration {
    Duration::from_millis(HEARTBEAT_PERIOD_MS)
}

#[cfg(test)]
mod tests {
    use super::*;

    #[tokio::test]
    async fn trips_link_lost_after_timeout() {
        // Arrange — 100 ms timeout; we wait 150 ms of real time after one inbound HB.
        let (wd, mut rx) = InboundWatchdog::new(100);
        wd.note_inbound_heartbeat();
        assert_eq!(rx.recv().await.unwrap(), LinkEvent::LinkUp);
        assert!(wd.link_up());

        // Act
        tokio::time::sleep(Duration::from_millis(150)).await;
        let tripped = wd.maybe_trip_link_lost();

        // Assert
        assert!(tripped);
        assert!(!wd.link_up());
        assert_eq!(rx.recv().await.unwrap(), LinkEvent::LinkLost);
    }

    #[test]
    fn outbound_heartbeat_is_well_formed() {
        // Act
        let MavlinkMessage::Heartbeat(h) = make_outbound_heartbeat() else {
            panic!("expected Heartbeat");
        };

        // Assert
        assert_eq!(h.mavtype, MAV_TYPE_ONBOARD_COMPUTER);
        assert_eq!(h.mavlink_version, MAVLINK_VERSION);
    }
}
