ruvnet--RuView/v2/crates/wifi-densepose-mat/src/localization/fusion.rs

//! Position fusion combining multiple localization techniques.

use super::{DepthEstimator, DepthEstimatorConfig, TriangulationConfig, Triangulator};
use crate::domain::{
    Coordinates3D, DebrisProfile, DepthEstimate, LocationUncertainty, ScanZone, VitalSignsReading,
};

/// Service for survivor localization
pub struct LocalizationService {
    triangulator: Triangulator,
    depth_estimator: DepthEstimator,
    #[allow(dead_code)]
    position_fuser: PositionFuser,
}

impl LocalizationService {
    /// Create a new localization service
    pub fn new() -> Self {
        Self {
            triangulator: Triangulator::with_defaults(),
            depth_estimator: DepthEstimator::with_defaults(),
            position_fuser: PositionFuser::new(),
        }
    }

    /// Create with custom configurations
    pub fn with_config(
        triangulation_config: TriangulationConfig,
        depth_config: DepthEstimatorConfig,
    ) -> Self {
        Self {
            triangulator: Triangulator::new(triangulation_config),
            depth_estimator: DepthEstimator::new(depth_config),
            position_fuser: PositionFuser::new(),
        }
    }

    /// Estimate survivor position from real per-sensor RSSI + debris-aware depth.
    ///
    /// `vitals` is currently used only as a presence guard (position is only
    /// meaningful for a real detection) — the position itself is derived from
    /// sensor geometry + RSSI and the zone debris profile, not from the vital
    /// waveform. It is retained in the signature so depth weighting can later
    /// incorporate breathing-amplitude SNR without a breaking API change.
    pub fn estimate_position(
        &self,
        vitals: &VitalSignsReading,
        zone: &ScanZone,
    ) -> Option<Coordinates3D> {
        // Only attempt localization for a real detection.
        if !vitals.has_vitals() {
            return None;
        }

        // Get sensor positions
        let sensors = zone.sensor_positions();

        if sensors.len() < 3 {
            return None;
        }

        // Estimate 2D position from triangulation using REAL per-sensor RSSI.
        // Sensors that have no live RSSI reading contribute nothing — we never
        // fabricate a measurement. If fewer than the triangulator's minimum
        // report real RSSI, `estimate_position` returns None and the caller
        // records the survivor with `location: None` (dedup then falls back to
        // the zone + vitals-signature path rather than inflating the count).
        let rssi_values = self.collect_rssi_measurements(sensors);
        let position_2d = self.triangulator.estimate_position(sensors, &rssi_values)?;

        // Estimate depth
        let debris_profile = self.estimate_debris_profile(zone);
        let signal_attenuation = self.calculate_signal_attenuation(&rssi_values);
        let depth_estimate =
            self.depth_estimator
                .estimate_depth(signal_attenuation, 0.0, &debris_profile)?;

        // Combine into 3D position
        let position_3d = Coordinates3D::new(
            position_2d.x,
            position_2d.y,
            -depth_estimate.depth, // Negative = below surface
            self.combine_uncertainties(&position_2d.uncertainty, &depth_estimate),
        );

        Some(position_3d)
    }

    /// Collect REAL per-sensor RSSI measurements for triangulation.
    ///
    /// Reads each operational sensor's most recent live RSSI (`last_rssi`,
    /// populated by the hardware layer from actual signal-strength readings).
    /// Sensors without a real reading are omitted — no value is fabricated. When
    /// the number of real measurements is below the triangulator's minimum the
    /// returned vector is short and `Triangulator::estimate_position` yields
    /// `None`, so the survivor is recorded with no location and de-duplicated by
    /// vitals signature instead of being counted multiple times.
    fn collect_rssi_measurements(
        &self,
        sensors: &[crate::domain::SensorPosition],
    ) -> Vec<(String, f64)> {
        let measurements: Vec<(String, f64)> = sensors
            .iter()
            .filter(|s| s.is_operational)
            .filter_map(|s| s.last_rssi.map(|rssi| (s.id.clone(), rssi)))
            .collect();

        if measurements.len() < self.triangulator.config().min_sensors {
            tracing::debug!(
                real_rssi_count = measurements.len(),
                required = self.triangulator.config().min_sensors,
                "Insufficient real RSSI measurements for triangulation; \
                 survivor will be recorded without a fixed location (no RSSI fabricated)."
            );
        }

        measurements
    }

    /// Estimate debris profile for the zone
    fn estimate_debris_profile(&self, _zone: &ScanZone) -> DebrisProfile {
        // Would use zone metadata and signal analysis
        DebrisProfile::default()
    }

    /// Calculate average signal attenuation
    fn calculate_signal_attenuation(&self, rssi_values: &[(String, f64)]) -> f64 {
        if rssi_values.is_empty() {
            return 0.0;
        }

        // Reference RSSI at surface (typical open-air value)
        const REFERENCE_RSSI: f64 = -30.0;

        let avg_rssi: f64 =
            rssi_values.iter().map(|(_, r)| r).sum::<f64>() / rssi_values.len() as f64;

        (REFERENCE_RSSI - avg_rssi).max(0.0)
    }

    /// Combine horizontal and depth uncertainties
    fn combine_uncertainties(
        &self,
        horizontal: &LocationUncertainty,
        depth: &DepthEstimate,
    ) -> LocationUncertainty {
        LocationUncertainty {
            horizontal_error: horizontal.horizontal_error,
            vertical_error: depth.uncertainty,
            confidence: (horizontal.confidence * depth.confidence).sqrt(),
        }
    }
}

impl Default for LocalizationService {
    fn default() -> Self {
        Self::new()
    }
}

/// Fuses multiple position estimates
pub struct PositionFuser {
    /// History of position estimates for smoothing
    history: parking_lot::RwLock<Vec<PositionEstimate>>,
    /// Maximum history size
    max_history: usize,
}

/// A position estimate with metadata
#[derive(Debug, Clone)]
pub struct PositionEstimate {
    /// The position
    pub position: Coordinates3D,
    /// Timestamp
    pub timestamp: chrono::DateTime<chrono::Utc>,
    /// Source of estimate
    pub source: EstimateSource,
    /// Weight for fusion
    pub weight: f64,
}

/// Source of a position estimate
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum EstimateSource {
    /// From RSSI-based triangulation
    RssiTriangulation,
    /// From time-of-arrival
    TimeOfArrival,
    /// From CSI fingerprinting
    CsiFingerprint,
    /// From angle of arrival
    AngleOfArrival,
    /// From depth estimation
    DepthEstimation,
    /// Fused from multiple sources
    Fused,
}

impl PositionFuser {
    /// Create a new position fuser
    pub fn new() -> Self {
        Self {
            history: parking_lot::RwLock::new(Vec::new()),
            max_history: 20,
        }
    }

    /// Add a position estimate
    pub fn add_estimate(&self, estimate: PositionEstimate) {
        let mut history = self.history.write();
        history.push(estimate);

        // Keep only recent history
        if history.len() > self.max_history {
            history.remove(0);
        }
    }

    /// Fuse multiple position estimates into one
    pub fn fuse(&self, estimates: &[PositionEstimate]) -> Option<Coordinates3D> {
        if estimates.is_empty() {
            return None;
        }

        if estimates.len() == 1 {
            return Some(estimates[0].position.clone());
        }

        // Weighted average based on uncertainty and source confidence
        let mut total_weight = 0.0;
        let mut sum_x = 0.0;
        let mut sum_y = 0.0;
        let mut sum_z = 0.0;

        for estimate in estimates {
            let weight = self.calculate_weight(estimate);
            total_weight += weight;
            sum_x += estimate.position.x * weight;
            sum_y += estimate.position.y * weight;
            sum_z += estimate.position.z * weight;
        }

        if total_weight == 0.0 {
            return None;
        }

        let fused_x = sum_x / total_weight;
        let fused_y = sum_y / total_weight;
        let fused_z = sum_z / total_weight;

        // Calculate fused uncertainty (reduced due to multiple estimates)
        let fused_uncertainty = self.calculate_fused_uncertainty(estimates);

        Some(Coordinates3D::new(
            fused_x,
            fused_y,
            fused_z,
            fused_uncertainty,
        ))
    }

    /// Fuse with temporal smoothing
    pub fn fuse_with_history(&self, current: &PositionEstimate) -> Option<Coordinates3D> {
        // Add current to history
        self.add_estimate(current.clone());

        let history = self.history.read();

        // Use exponentially weighted moving average
        let alpha: f64 = 0.3; // Smoothing factor
        let mut smoothed = current.position.clone();

        for (i, estimate) in history.iter().rev().enumerate().skip(1) {
            let weight = alpha * (1.0_f64 - alpha).powi(i as i32);
            smoothed.x = smoothed.x * (1.0 - weight) + estimate.position.x * weight;
            smoothed.y = smoothed.y * (1.0 - weight) + estimate.position.y * weight;
            smoothed.z = smoothed.z * (1.0 - weight) + estimate.position.z * weight;
        }

        Some(smoothed)
    }

    /// Calculate weight for an estimate
    fn calculate_weight(&self, estimate: &PositionEstimate) -> f64 {
        // Base weight from source reliability
        let source_weight = match estimate.source {
            EstimateSource::TimeOfArrival => 1.0,
            EstimateSource::AngleOfArrival => 0.9,
            EstimateSource::CsiFingerprint => 0.8,
            EstimateSource::RssiTriangulation => 0.7,
            EstimateSource::DepthEstimation => 0.6,
            EstimateSource::Fused => 1.0,
        };

        // Adjust by uncertainty (lower uncertainty = higher weight)
        let uncertainty_factor = 1.0 / (1.0 + estimate.position.uncertainty.horizontal_error);

        // User-provided weight
        let user_weight = estimate.weight;

        source_weight * uncertainty_factor * user_weight
    }

    /// Calculate uncertainty after fusing multiple estimates
    fn calculate_fused_uncertainty(&self, estimates: &[PositionEstimate]) -> LocationUncertainty {
        if estimates.is_empty() {
            return LocationUncertainty::default();
        }

        // Combined uncertainty is reduced with multiple estimates
        let n = estimates.len() as f64;

        let avg_h_error: f64 = estimates
            .iter()
            .map(|e| e.position.uncertainty.horizontal_error)
            .sum::<f64>()
            / n;

        let avg_v_error: f64 = estimates
            .iter()
            .map(|e| e.position.uncertainty.vertical_error)
            .sum::<f64>()
            / n;

        // Uncertainty reduction factor (more estimates = more confidence)
        let reduction = (1.0 / n.sqrt()).max(0.5);

        LocationUncertainty {
            horizontal_error: avg_h_error * reduction,
            vertical_error: avg_v_error * reduction,
            confidence: (0.95 * (1.0 + (n - 1.0) * 0.02)).min(0.99),
        }
    }

    /// Clear history
    pub fn clear_history(&self) {
        self.history.write().clear();
    }
}

impl Default for PositionFuser {
    fn default() -> Self {
        Self::new()
    }
}

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

    fn create_test_estimate(x: f64, y: f64, z: f64) -> PositionEstimate {
        PositionEstimate {
            position: Coordinates3D::with_default_uncertainty(x, y, z),
            timestamp: Utc::now(),
            source: EstimateSource::RssiTriangulation,
            weight: 1.0,
        }
    }

    #[test]
    fn test_single_estimate_fusion() {
        let fuser = PositionFuser::new();
        let estimate = create_test_estimate(5.0, 10.0, -2.0);

        let result = fuser.fuse(&[estimate]);
        assert!(result.is_some());

        let pos = result.unwrap();
        assert!((pos.x - 5.0).abs() < 0.001);
    }

    #[test]
    fn test_multiple_estimate_fusion() {
        let fuser = PositionFuser::new();

        let estimates = vec![
            create_test_estimate(4.0, 9.0, -1.5),
            create_test_estimate(6.0, 11.0, -2.5),
        ];

        let result = fuser.fuse(&estimates);
        assert!(result.is_some());

        let pos = result.unwrap();
        // Should be roughly in between
        assert!(pos.x > 4.0 && pos.x < 6.0);
        assert!(pos.y > 9.0 && pos.y < 11.0);
    }

    #[test]
    fn test_fused_uncertainty_reduction() {
        let fuser = PositionFuser::new();

        let estimates = vec![
            create_test_estimate(5.0, 10.0, -2.0),
            create_test_estimate(5.1, 10.1, -2.1),
            create_test_estimate(4.9, 9.9, -1.9),
        ];

        let single_uncertainty = estimates[0].position.uncertainty.horizontal_error;
        let fused_uncertainty = fuser.calculate_fused_uncertainty(&estimates);

        // Fused should have lower uncertainty
        assert!(fused_uncertainty.horizontal_error < single_uncertainty);
    }

    #[test]
    fn test_localization_service_creation() {
        let service = LocalizationService::new();
        // Just verify it creates without panic
        drop(service);
    }

    /// Real-RSSI localization: when ≥3 sensors carry live RSSI the service
    /// produces a position (exercises the real triangulator path, replacing the
    /// old `simulate_rssi_measurements` that always returned `vec![]`).
    #[test]
    fn test_estimate_position_uses_real_rssi() {
        use crate::domain::{
            BreathingPattern, BreathingType, MovementProfile, ScanZone, SensorPosition, SensorType,
            VitalSignsReading, ZoneBounds,
        };

        let mut zone = ScanZone::new("Z", ZoneBounds::rectangle(0.0, 0.0, 12.0, 12.0));
        for (id, x, y, rssi) in [
            ("s1", 0.0, 0.0, -55.0),
            ("s2", 10.0, 0.0, -60.0),
            ("s3", 5.0, 10.0, -58.0),
        ] {
            zone.add_sensor(SensorPosition {
                id: id.to_string(),
                x,
                y,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
                last_rssi: Some(rssi),
            });
        }

        let vitals = VitalSignsReading::new(
            Some(BreathingPattern {
                rate_bpm: 16.0,
                amplitude: 0.8,
                regularity: 0.9,
                pattern_type: BreathingType::Normal,
            }),
            None,
            MovementProfile::default(),
        );

        let service = LocalizationService::new();
        let pos = service.estimate_position(&vitals, &zone);
        assert!(pos.is_some(), "3 real RSSI sensors should yield a position");
    }

    /// Honest negative: sensors WITHOUT real RSSI yield no position (no
    /// fabrication). The caller then records `location: None`.
    #[test]
    fn test_estimate_position_none_without_real_rssi() {
        use crate::domain::{
            BreathingPattern, BreathingType, MovementProfile, ScanZone, SensorPosition, SensorType,
            VitalSignsReading, ZoneBounds,
        };

        let mut zone = ScanZone::new("Z", ZoneBounds::rectangle(0.0, 0.0, 12.0, 12.0));
        for (id, x, y) in [("s1", 0.0, 0.0), ("s2", 10.0, 0.0), ("s3", 5.0, 10.0)] {
            zone.add_sensor(SensorPosition {
                id: id.to_string(),
                x,
                y,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
                last_rssi: None, // no live signal
            });
        }

        let vitals = VitalSignsReading::new(
            Some(BreathingPattern {
                rate_bpm: 16.0,
                amplitude: 0.8,
                regularity: 0.9,
                pattern_type: BreathingType::Normal,
            }),
            None,
            MovementProfile::default(),
        );

        let service = LocalizationService::new();
        assert!(service.estimate_position(&vitals, &zone).is_none());
    }
}