autopilot/crates/mission_executor/src/internal/geofence.rs

//! AZ-652 — geofence enforcement (INCLUSION + EXCLUSION).
//!
//! Symmetric semantics per the task spec: INCLUSION exit and EXCLUSION
//! entry are both faults that must trigger RTL within ≤500 ms. The
//! earlier C++ behaviour silently ignored EXCLUSION; the new design
//! rejects that.
//!
//! The monitor is **pure logic**: `evaluate(pos, geofences)` is
//! deterministic and side-effect-free. The driver in
//! [`GeofenceDriver`] is the wiring layer that subscribes to a
//! position stream, ticks the monitor, calls
//! [`MissionExecutorHandle::failsafe_trigger`] on violation, and
//! issues `MAV_CMD_NAV_RETURN_TO_LAUNCH` via the supplied command
//! issuer. Following AZ-651's separation pattern, each failsafe family
//! owns its own command-issuer trait (see
//! [`crate::internal::lost_link`] for the lost-link variant).

use std::sync::Arc;
use std::time::Duration;

use async_trait::async_trait;
use mavlink_layer::{CommandLong, MavlinkHandle, SendCommandError};
use tokio::sync::{broadcast, watch};
use tokio::task::JoinHandle;
use tokio::time::Instant;

use shared::error::AutopilotError;
use shared::models::mission::{Coordinate, Geofence, GeofenceKind};
use shared::models::telemetry::UavPosition;

use crate::internal::lost_link::MAV_CMD_NAV_RETURN_TO_LAUNCH;
use crate::FailsafeKind;
use crate::MissionExecutorHandle;

/// Outcome of a single tick.
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum GeofenceVerdict {
    /// Position satisfies every geofence (inside every INCLUSION,
    /// outside every EXCLUSION).
    Ok,
    /// Position has exited an INCLUSION polygon.
    InclusionExit,
    /// Position has entered an EXCLUSION polygon.
    ExclusionEntry,
}

impl GeofenceVerdict {
    pub fn is_violation(self) -> bool {
        !matches!(self, GeofenceVerdict::Ok)
    }

    pub fn failsafe_kind(self) -> Option<FailsafeKind> {
        match self {
            GeofenceVerdict::Ok => None,
            GeofenceVerdict::InclusionExit => Some(FailsafeKind::GeofenceInclusion),
            GeofenceVerdict::ExclusionEntry => Some(FailsafeKind::GeofenceExclusion),
        }
    }
}

/// Pure point-in-polygon evaluator for a fixed set of geofences.
///
/// Construction is cheap (no preprocessing); each `evaluate` call is
/// O(total vertices). With the operational `≤8` geofences × `≤32`
/// vertices typical for a single mission this is a few hundred
/// floating-point ops per tick — comfortably under the AZ-652
/// ≤500 ms response budget at the 10 Hz monitor cadence.
#[derive(Debug, Clone)]
pub struct GeofenceMonitor {
    geofences: Vec<Geofence>,
}

impl GeofenceMonitor {
    pub fn new(geofences: Vec<Geofence>) -> Self {
        Self { geofences }
    }

    pub fn geofence_count(&self) -> usize {
        self.geofences.len()
    }

    /// Evaluate the position against every fence. Returns the first
    /// violation encountered (inclusion-exit checked first so a UAV
    /// dropping past an inclusion boundary surfaces the more typical
    /// fault first).
    pub fn evaluate(&self, position: &UavPosition) -> GeofenceVerdict {
        let point = Coordinate {
            latitude: position.lat_e7 as f64 * 1.0e-7,
            longitude: position.lon_e7 as f64 * 1.0e-7,
            altitude_m: position.alt_m,
        };
        for fence in &self.geofences {
            let inside = point_in_polygon(&point, &fence.vertices);
            match (fence.kind, inside) {
                (GeofenceKind::Inclusion, false) => return GeofenceVerdict::InclusionExit,
                (GeofenceKind::Exclusion, true) => return GeofenceVerdict::ExclusionEntry,
                _ => {}
            }
        }
        GeofenceVerdict::Ok
    }
}

/// Ray-casting point-in-polygon. The polygon is treated as closed
/// (last vertex connects back to the first). Boundary semantics are
/// "boundary counts as inside" — flying exactly along a fence line is
/// considered compliant; the next tick that strays will surface the
/// violation.
fn point_in_polygon(point: &Coordinate, polygon: &[Coordinate]) -> bool {
    if polygon.len() < 3 {
        // Degenerate polygon — be safe: an INCLUSION with fewer than
        // 3 vertices means "no valid inside" → caller treats as exit
        // immediately. An EXCLUSION with fewer than 3 vertices is
        // unenforceable; treat as outside (no entry possible).
        return false;
    }
    let x = point.longitude;
    let y = point.latitude;
    let mut inside = false;
    let n = polygon.len();
    for i in 0..n {
        let a = &polygon[i];
        let b = &polygon[(i + 1) % n];
        let (xi, yi) = (a.longitude, a.latitude);
        let (xj, yj) = (b.longitude, b.latitude);
        let crosses = (yi > y) != (yj > y) && {
            // Avoid division by zero when the edge is horizontal —
            // such an edge cannot be crossed by a horizontal ray.
            let dy = yj - yi;
            if dy.abs() < f64::EPSILON {
                false
            } else {
                let x_at_y = (xj - xi) * (y - yi) / dy + xi;
                x < x_at_y
            }
        };
        if crosses {
            inside = !inside;
        }
    }
    inside
}

/// Broadcast event surfaced on every state transition or RTL trigger.
#[derive(Debug, Clone, Copy)]
#[non_exhaustive]
pub enum GeofenceEvent {
    Violation { kind: FailsafeKind },
    RtlIssued { kind: FailsafeKind },
    RtlSendFailed { kind: FailsafeKind },
}

/// Pluggable command issuer. Production wires this to
/// [`MavlinkGeofenceCommandIssuer`]; tests supply a spy. Separate
/// from the lost-link issuer so each failsafe family owns its own
/// command surface (mirrors the AZ-651 pattern).
#[async_trait]
pub trait GeofenceCommandIssuer: Send + Sync {
    async fn issue_rtl(&self) -> Result<(), AutopilotError>;
}

/// Production `GeofenceCommandIssuer` backed by `mavlink_layer`.
/// Issues `MAV_CMD_NAV_RETURN_TO_LAUNCH` (same command id the
/// lost-link path uses) targeting the configured airframe.
#[derive(Debug, Clone)]
pub struct MavlinkGeofenceCommandIssuer {
    pub handle: MavlinkHandle,
    pub target_system: u8,
    pub target_component: u8,
    pub ack_deadline: Option<Duration>,
}

impl MavlinkGeofenceCommandIssuer {
    pub fn new(handle: MavlinkHandle, target_system: u8, target_component: u8) -> Self {
        Self {
            handle,
            target_system,
            target_component,
            ack_deadline: None,
        }
    }
}

#[async_trait]
impl GeofenceCommandIssuer for MavlinkGeofenceCommandIssuer {
    async fn issue_rtl(&self) -> Result<(), AutopilotError> {
        let cmd = CommandLong {
            param1: 0.0,
            param2: 0.0,
            param3: 0.0,
            param4: 0.0,
            param5: 0.0,
            param6: 0.0,
            param7: 0.0,
            command: MAV_CMD_NAV_RETURN_TO_LAUNCH,
            target_system: self.target_system,
            target_component: self.target_component,
            confirmation: 0,
        };
        self.handle
            .send_command(cmd, self.ack_deadline)
            .await
            .map(|_ack| ())
            .map_err(|e| match e {
                SendCommandError::Timeout(d) => AutopilotError::Internal(format!(
                    "geofence RTL command ack timeout after {d:?}"
                )),
                SendCommandError::Duplicate(id) => AutopilotError::Internal(format!(
                    "geofence RTL command duplicate in flight (id={id})"
                )),
                SendCommandError::ChannelClosed(reason) => AutopilotError::Internal(format!(
                    "geofence RTL command channel closed: {reason}"
                )),
            })
    }
}

/// Public read-side handle.
#[derive(Debug, Clone)]
pub struct GeofenceMonitorHandle {
    events_tx: broadcast::Sender<GeofenceEvent>,
    last_verdict_rx: watch::Receiver<GeofenceVerdict>,
}

impl GeofenceMonitorHandle {
    pub fn subscribe(&self) -> broadcast::Receiver<GeofenceEvent> {
        self.events_tx.subscribe()
    }

    pub fn last_verdict(&self) -> GeofenceVerdict {
        *self.last_verdict_rx.borrow()
    }
}

/// Driver — ticks the monitor against an incoming `UavPosition`
/// stream and dispatches RTL on violation.
pub struct GeofenceDriver<C: GeofenceCommandIssuer + 'static> {
    monitor: GeofenceMonitor,
    executor: MissionExecutorHandle,
    command_issuer: Arc<C>,
    position_rx: watch::Receiver<Option<UavPosition>>,
    tick_interval: Duration,
}

impl<C: GeofenceCommandIssuer + 'static> GeofenceDriver<C> {
    pub fn new(
        monitor: GeofenceMonitor,
        executor: MissionExecutorHandle,
        command_issuer: Arc<C>,
        position_rx: watch::Receiver<Option<UavPosition>>,
    ) -> Self {
        Self {
            monitor,
            executor,
            command_issuer,
            position_rx,
            // 100 ms tick → ≤500 ms detect-to-RTL with healthy
            // ground-station latency.
            tick_interval: Duration::from_millis(100),
        }
    }

    pub fn with_tick_interval(mut self, interval: Duration) -> Self {
        self.tick_interval = interval;
        self
    }

    /// Spawn the driver task and return the read-side handle plus the
    /// task's join handle.
    pub fn spawn(
        self,
        mut shutdown: watch::Receiver<bool>,
    ) -> (GeofenceMonitorHandle, JoinHandle<()>) {
        let (events_tx, _events_rx) = broadcast::channel::<GeofenceEvent>(64);
        let (verdict_tx, verdict_rx) = watch::channel(GeofenceVerdict::Ok);
        let handle = GeofenceMonitorHandle {
            events_tx: events_tx.clone(),
            last_verdict_rx: verdict_rx,
        };

        let GeofenceDriver {
            monitor,
            executor,
            command_issuer,
            mut position_rx,
            tick_interval,
        } = self;

        let join = tokio::spawn(async move {
            let mut ticker =
                tokio::time::interval_at(Instant::now() + tick_interval, tick_interval);
            ticker.set_missed_tick_behavior(tokio::time::MissedTickBehavior::Skip);
            let mut last_verdict = GeofenceVerdict::Ok;
            loop {
                tokio::select! {
                    biased;
                    _ = shutdown.changed() => {
                        tracing::info!("geofence driver shutdown");
                        return;
                    }
                    _ = ticker.tick() => {
                        let pos_snapshot = *position_rx.borrow_and_update();
                        let Some(position) = pos_snapshot else {
                            // No position yet — cannot evaluate.
                            continue;
                        };
                        let verdict = monitor.evaluate(&position);
                        let _ = verdict_tx.send(verdict);
                        let entering_violation =
                            verdict.is_violation() && !last_verdict.is_violation();
                        last_verdict = verdict;
                        if !entering_violation {
                            continue;
                        }
                        let Some(kind) = verdict.failsafe_kind() else { continue };
                        let _ = events_tx.send(GeofenceEvent::Violation { kind });
                        tracing::warn!(
                            ?kind,
                            "geofence violation; issuing RTL"
                        );
                        match command_issuer.issue_rtl().await {
                            Ok(()) => {
                                let _ = events_tx.send(GeofenceEvent::RtlIssued { kind });
                            }
                            Err(e) => {
                                tracing::error!(error=%e, ?kind, "geofence RTL send failed");
                                let _ = events_tx.send(GeofenceEvent::RtlSendFailed { kind });
                            }
                        }
                        if let Err(e) = executor.failsafe_trigger(kind).await {
                            tracing::error!(error=%e, ?kind, "geofence executor.failsafe_trigger failed");
                        }
                    }
                }
            }
        });

        (handle, join)
    }
}

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

    fn coord(lat: f64, lon: f64) -> Coordinate {
        Coordinate {
            latitude: lat,
            longitude: lon,
            altitude_m: 0.0,
        }
    }

    fn square_inclusion() -> Geofence {
        Geofence {
            kind: GeofenceKind::Inclusion,
            vertices: vec![
                coord(50.0, 30.0),
                coord(50.0, 31.0),
                coord(51.0, 31.0),
                coord(51.0, 30.0),
            ],
        }
    }

    fn square_exclusion() -> Geofence {
        Geofence {
            kind: GeofenceKind::Exclusion,
            vertices: vec![
                coord(50.4, 30.4),
                coord(50.4, 30.6),
                coord(50.6, 30.6),
                coord(50.6, 30.4),
            ],
        }
    }

    fn pos_at(lat: f64, lon: f64) -> UavPosition {
        UavPosition {
            lat_e7: (lat * 1.0e7) as i32,
            lon_e7: (lon * 1.0e7) as i32,
            alt_m: 100.0,
            relative_alt_m: 50.0,
            vx_mps: 0.0,
            vy_mps: 0.0,
            vz_mps: 0.0,
            heading_deg: 0.0,
            ts_boot_ms: 0,
        }
    }

    #[test]
    fn inclusion_inside_is_ok() {
        // Arrange
        let m = GeofenceMonitor::new(vec![square_inclusion()]);

        // Act
        let v = m.evaluate(&pos_at(50.5, 30.5));

        // Assert
        assert_eq!(v, GeofenceVerdict::Ok);
    }

    #[test]
    fn inclusion_outside_is_exit() {
        // Arrange
        let m = GeofenceMonitor::new(vec![square_inclusion()]);

        // Act
        let v = m.evaluate(&pos_at(52.0, 30.5));

        // Assert
        assert_eq!(v, GeofenceVerdict::InclusionExit);
        assert_eq!(v.failsafe_kind(), Some(FailsafeKind::GeofenceInclusion));
    }

    #[test]
    fn exclusion_outside_is_ok() {
        // Arrange
        let m = GeofenceMonitor::new(vec![square_inclusion(), square_exclusion()]);

        // Act — inside INCLUSION, outside EXCLUSION
        let v = m.evaluate(&pos_at(50.2, 30.2));

        // Assert
        assert_eq!(v, GeofenceVerdict::Ok);
    }

    #[test]
    fn exclusion_inside_is_entry() {
        // Arrange
        let m = GeofenceMonitor::new(vec![square_inclusion(), square_exclusion()]);

        // Act — inside both INCLUSION and EXCLUSION
        let v = m.evaluate(&pos_at(50.5, 30.5));

        // Assert
        assert_eq!(v, GeofenceVerdict::ExclusionEntry);
        assert_eq!(v.failsafe_kind(), Some(FailsafeKind::GeofenceExclusion));
    }

    #[test]
    fn degenerate_polygon_inclusion_is_exit() {
        // Arrange — fewer than 3 vertices
        let fence = Geofence {
            kind: GeofenceKind::Inclusion,
            vertices: vec![coord(0.0, 0.0), coord(1.0, 0.0)],
        };

        // Act
        let v = GeofenceMonitor::new(vec![fence]).evaluate(&pos_at(0.5, 0.5));

        // Assert
        assert_eq!(v, GeofenceVerdict::InclusionExit);
    }

    #[test]
    fn no_geofences_is_ok() {
        // Arrange
        let m = GeofenceMonitor::new(vec![]);

        // Act
        let v = m.evaluate(&pos_at(50.5, 30.5));

        // Assert
        assert_eq!(v, GeofenceVerdict::Ok);
    }
}