autopilot/crates/mapobjects_store/src/internal/store.rs

//! In-memory hashmap of known map objects, keyed by H3 cell.
//!
//! Classification logic (NEW / MOVED / EXISTING) lives here. Per
//! `architecture.md §7.12` the on-device map keeps the full per-mission
//! state in memory; persistence (AZ-668) lands later.
//!
//! Concurrency: this module is intentionally single-threaded and not
//! `Sync`. The public `MapObjectsStoreHandle` wraps it in an `Arc<Mutex<…>>`
//! so the lock surface is a single owned mutex instead of fine-grained
//! per-cell locking. With p99 ≤ 1 ms and detection rates < 30 Hz the
//! single mutex is comfortably within budget.

use std::collections::HashMap;

use chrono::{DateTime, Utc};
use h3o::CellIndex;
use shared::error::Result;
use uuid::Uuid;

use super::h3_index::{cell_of, grid_disk, haversine_m, DEFAULT_K_RING, DEFAULT_RESOLUTION};

/// Per-detection input to `classify`. This bundles the georeferenced
/// payload the architecture-level "detection" carries (gps, class, conf,
/// size — see `system-flows.md §F7`) without forcing the shared
/// `Detection` model to grow geolocation fields. `scan_controller` builds
/// this from `Detection` + GPS / MGRS context at the call site.
#[derive(Debug, Clone)]
pub struct ClassifyInput {
    pub gps_lat: f64,
    pub gps_lon: f64,
    pub mgrs: String,
    pub class: String,
    pub size_width_m: f32,
    pub size_length_m: f32,
    pub confidence: f32,
    pub mission_id: String,
    pub observed_at: DateTime<Utc>,
}

/// Configuration for the spatial-index + classification policy.
#[derive(Debug, Clone)]
pub struct MapObjectsStoreConfig {
    /// H3 cell resolution. Default 10 (~15 m edge).
    pub h3_resolution: u8,
    /// K-ring radius for boundary-safe lookups. Default 2.
    pub k_ring: u32,
    /// Maximum distance (m) between input and stored object for the pair
    /// to be considered a possible match. Beyond this → `NEW`.
    pub distance_threshold_m: f64,
    /// Above this delta (m) between input position and the matched
    /// object's stored position, classification flips to `MOVED`.
    pub move_threshold_m: f64,
    /// Class-similarity groups. Each inner vec is one group; classes in
    /// the same group are considered equivalent for matching (e.g.
    /// `tree` and `shrub` collapsed). A class not listed in any group
    /// is its own group of one.
    pub similar_classes: Vec<Vec<String>>,
}

impl Default for MapObjectsStoreConfig {
    fn default() -> Self {
        Self {
            h3_resolution: DEFAULT_RESOLUTION,
            k_ring: DEFAULT_K_RING,
            // Defaults follow `system-flows.md §F7` (distance 50 m,
            // move 10 m). The task brief lists different per-AC values
            // (30 m / 50 m) — callers override per scenario.
            distance_threshold_m: 50.0,
            move_threshold_m: 10.0,
            similar_classes: Vec::new(),
        }
    }
}

/// Outcome of `MapObjectsStore::classify`.
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum Classification {
    New {
        id: Uuid,
    },
    Moved {
        id: Uuid,
        from_mgrs: String,
        to_mgrs: String,
    },
    Existing {
        id: Uuid,
    },
    /// Reserved for AZ-666 end-of-pass sweep.
    RemovedCandidate {
        id: Uuid,
    },
    /// Reserved for AZ-666 ignored-suppression.
    Ignored,
}

/// Stored shape. Fields beyond what `classify` reads are kept for the
/// next batch in the same component (AZ-666 ignored-suppression / sweep,
/// AZ-667 hydrate / dump_pending) which will surface them via the engine
/// API. The lint allow is scoped to those forward-use fields.
#[allow(dead_code)]
#[derive(Debug, Clone)]
struct StoredMapObject {
    id: Uuid,
    h3_cell: CellIndex,
    mgrs: String,
    class: String,
    class_group: String,
    gps_lat: f64,
    gps_lon: f64,
    size_width_m: f32,
    size_length_m: f32,
    confidence: f32,
    first_seen: DateTime<Utc>,
    last_seen: DateTime<Utc>,
    mission_id: String,
}

/// In-memory spatial index of known map objects.
pub struct Store {
    config: MapObjectsStoreConfig,
    by_cell: HashMap<CellIndex, Vec<StoredMapObject>>,
    /// Total object count, maintained alongside `by_cell` for O(1) metrics.
    len: usize,
}

impl Store {
    pub fn new(config: MapObjectsStoreConfig) -> Self {
        Self {
            config,
            by_cell: HashMap::new(),
            len: 0,
        }
    }

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

    /// Exposed for AZ-666/AZ-667 engine plug-points (`internal::engine::*`).
    #[allow(dead_code)]
    pub fn config(&self) -> &MapObjectsStoreConfig {
        &self.config
    }

    /// Resolve a raw class string to its canonical group key.
    ///
    /// The first class listed in a `similar_classes` group is the group
    /// key. A class absent from all groups is its own group.
    fn group_key(&self, class: &str) -> String {
        for group in &self.config.similar_classes {
            if group.iter().any(|c| c == class) {
                // group[0] is guaranteed by Vec invariants once we filter
                // empty groups out (see new). But be defensive.
                if let Some(first) = group.first() {
                    return first.clone();
                }
            }
        }
        class.to_string()
    }

    /// Classify a single detection input. Mutates the store on `New` /
    /// `Moved` / `Existing` (insert / position-update / last_seen-update
    /// respectively). Returns the classification.
    pub fn classify(&mut self, input: ClassifyInput) -> Result<Classification> {
        let query_cell = cell_of(input.gps_lat, input.gps_lon, self.config.h3_resolution)?;
        let group = self.group_key(&input.class);

        // Find the nearest matching object across the k-ring.
        let mut best: Option<(CellIndex, usize, f64)> = None;
        let disk = grid_disk(query_cell, self.config.k_ring);
        for cell in &disk {
            if let Some(objects) = self.by_cell.get(cell) {
                for (idx, obj) in objects.iter().enumerate() {
                    if obj.class_group != group {
                        continue;
                    }
                    let d = haversine_m(input.gps_lat, input.gps_lon, obj.gps_lat, obj.gps_lon);
                    if d > self.config.distance_threshold_m {
                        continue;
                    }
                    if best.is_none_or(|(_, _, prev_d)| d < prev_d) {
                        best = Some((*cell, idx, d));
                    }
                }
            }
        }

        match best {
            Some((cell, idx, delta_m)) if delta_m >= self.config.move_threshold_m => {
                // MOVED — update stored position to the new observation.
                let bucket = self
                    .by_cell
                    .get_mut(&cell)
                    .expect("cell present during best-match scan");
                let obj = &mut bucket[idx];
                let from_mgrs = obj.mgrs.clone();
                let id = obj.id;
                obj.gps_lat = input.gps_lat;
                obj.gps_lon = input.gps_lon;
                obj.mgrs = input.mgrs.clone();
                obj.last_seen = input.observed_at;
                obj.confidence = input.confidence;

                // If the new GPS sits in a different H3 cell, re-bucket.
                if cell != query_cell {
                    let moved = bucket.remove(idx);
                    if bucket.is_empty() {
                        self.by_cell.remove(&cell);
                    }
                    self.by_cell
                        .entry(query_cell)
                        .or_default()
                        .push(StoredMapObject {
                            h3_cell: query_cell,
                            ..moved
                        });
                }
                Ok(Classification::Moved {
                    id,
                    from_mgrs,
                    to_mgrs: input.mgrs,
                })
            }
            Some((cell, idx, _)) => {
                // EXISTING — just refresh last_seen.
                let bucket = self
                    .by_cell
                    .get_mut(&cell)
                    .expect("cell present during best-match scan");
                let obj = &mut bucket[idx];
                obj.last_seen = input.observed_at;
                Ok(Classification::Existing { id: obj.id })
            }
            None => {
                // NEW — insert.
                let id = Uuid::new_v4();
                let stored = StoredMapObject {
                    id,
                    h3_cell: query_cell,
                    mgrs: input.mgrs.clone(),
                    class: input.class.clone(),
                    class_group: group,
                    gps_lat: input.gps_lat,
                    gps_lon: input.gps_lon,
                    size_width_m: input.size_width_m,
                    size_length_m: input.size_length_m,
                    confidence: input.confidence,
                    first_seen: input.observed_at,
                    last_seen: input.observed_at,
                    mission_id: input.mission_id.clone(),
                };
                self.by_cell.entry(query_cell).or_default().push(stored);
                self.len += 1;
                Ok(Classification::New { id })
            }
        }
    }
}

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

    fn input(lat: f64, lon: f64, class: &str) -> ClassifyInput {
        ClassifyInput {
            gps_lat: lat,
            gps_lon: lon,
            mgrs: format!("MGRS({lat},{lon})"),
            class: class.into(),
            size_width_m: 1.0,
            size_length_m: 1.0,
            confidence: 0.9,
            mission_id: "m1".into(),
            observed_at: Utc::now(),
        }
    }

    #[test]
    fn group_key_returns_class_when_unknown() {
        // Arrange
        let s = Store::new(MapObjectsStoreConfig::default());
        // Act + Assert
        assert_eq!(s.group_key("tank"), "tank");
    }

    #[test]
    fn group_key_collapses_similar_classes() {
        // Arrange
        let cfg = MapObjectsStoreConfig {
            similar_classes: vec![vec!["tree".into(), "shrub".into()]],
            ..MapObjectsStoreConfig::default()
        };
        let s = Store::new(cfg);
        // Assert
        assert_eq!(s.group_key("tree"), "tree");
        assert_eq!(s.group_key("shrub"), "tree");
        assert_eq!(s.group_key("rock"), "rock");
    }

    #[test]
    fn empty_store_has_zero_len() {
        // Arrange
        let s = Store::new(MapObjectsStoreConfig::default());
        // Assert
        assert_eq!(s.len(), 0);
    }

    #[test]
    fn first_classify_is_new() {
        // Arrange
        let mut s = Store::new(MapObjectsStoreConfig::default());
        // Act
        let c = s.classify(input(50.45, 30.52, "tank")).unwrap();
        // Assert
        assert!(matches!(c, Classification::New { .. }));
        assert_eq!(s.len(), 1);
    }
}