wifi-densepose-mat 0.3.2

//! Triangulation for 2D/3D position estimation from multiple sensors.

use crate::domain::{Coordinates3D, LocationUncertainty, SensorPosition};

/// Configuration for triangulation
#[derive(Debug, Clone)]
pub struct TriangulationConfig {
    /// Minimum number of sensors required
    pub min_sensors: usize,
    /// Maximum position uncertainty to accept (meters)
    pub max_uncertainty: f64,
    /// Path loss exponent for distance estimation
    pub path_loss_exponent: f64,
    /// Reference distance for path loss model (meters)
    pub reference_distance: f64,
    /// Reference RSSI at reference distance (dBm)
    pub reference_rssi: f64,
    /// Use weighted least squares
    pub weighted: bool,
}

impl Default for TriangulationConfig {
    fn default() -> Self {
        Self {
            min_sensors: 3,
            max_uncertainty: 5.0,
            path_loss_exponent: 3.0, // Indoor with obstacles
            reference_distance: 1.0,
            reference_rssi: -30.0,
            weighted: true,
        }
    }
}

/// Result of a distance estimation
#[derive(Debug, Clone)]
#[allow(dead_code)]
pub struct DistanceEstimate {
    /// Sensor ID
    pub sensor_id: String,
    /// Estimated distance in meters
    pub distance: f64,
    /// Estimation confidence
    pub confidence: f64,
}

/// Triangulator for position estimation
pub struct Triangulator {
    config: TriangulationConfig,
}

impl Triangulator {
    /// Create a new triangulator
    pub fn new(config: TriangulationConfig) -> Self {
        Self { config }
    }

    /// Create with default configuration
    pub fn with_defaults() -> Self {
        Self::new(TriangulationConfig::default())
    }

    /// Access the triangulation configuration.
    pub fn config(&self) -> &TriangulationConfig {
        &self.config
    }

    /// Estimate position from RSSI measurements
    pub fn estimate_position(
        &self,
        sensors: &[SensorPosition],
        rssi_values: &[(String, f64)], // (sensor_id, rssi)
    ) -> Option<Coordinates3D> {
        // Get distance estimates from RSSI
        let distances: Vec<(SensorPosition, f64)> = rssi_values
            .iter()
            .filter_map(|(id, rssi)| {
                let sensor = sensors.iter().find(|s| &s.id == id)?;
                if !sensor.is_operational {
                    return None;
                }
                let distance = self.rssi_to_distance(*rssi);
                Some((sensor.clone(), distance))
            })
            .collect();

        if distances.len() < self.config.min_sensors {
            return None;
        }

        // Perform trilateration
        self.trilaterate(&distances)
    }

    /// Estimate position from Time of Arrival measurements
    pub fn estimate_from_toa(
        &self,
        sensors: &[SensorPosition],
        toa_values: &[(String, f64)], // (sensor_id, time_of_arrival_ns)
    ) -> Option<Coordinates3D> {
        const SPEED_OF_LIGHT: f64 = 299_792_458.0; // m/s

        let distances: Vec<(SensorPosition, f64)> = toa_values
            .iter()
            .filter_map(|(id, toa)| {
                let sensor = sensors.iter().find(|s| &s.id == id)?;
                if !sensor.is_operational {
                    return None;
                }
                // Convert nanoseconds to distance
                let distance = (*toa * 1e-9) * SPEED_OF_LIGHT / 2.0; // Round trip
                Some((sensor.clone(), distance))
            })
            .collect();

        if distances.len() < self.config.min_sensors {
            return None;
        }

        self.trilaterate(&distances)
    }

    /// Convert RSSI to distance using path loss model
    fn rssi_to_distance(&self, rssi: f64) -> f64 {
        // Log-distance path loss model:
        // RSSI = RSSI_0 - 10 * n * log10(d / d_0)
        // Solving for d:
        // d = d_0 * 10^((RSSI_0 - RSSI) / (10 * n))

        let exponent =
            (self.config.reference_rssi - rssi) / (10.0 * self.config.path_loss_exponent);

        self.config.reference_distance * 10.0_f64.powf(exponent)
    }

    /// Perform trilateration using least squares
    fn trilaterate(&self, distances: &[(SensorPosition, f64)]) -> Option<Coordinates3D> {
        if distances.len() < 3 {
            return None;
        }

        // Use linearized least squares approach
        // Reference: https://en.wikipedia.org/wiki/Trilateration

        // Use first sensor as reference
        let (ref_sensor, ref_dist) = &distances[0];
        let x1 = ref_sensor.x;
        let y1 = ref_sensor.y;
        let r1 = *ref_dist;

        // Build system of linear equations: A * [x, y]^T = b
        let n = distances.len() - 1;
        let mut a_matrix = vec![vec![0.0; 2]; n];
        let mut b_vector = vec![0.0; n];

        for (i, (sensor, dist)) in distances.iter().skip(1).enumerate() {
            let xi = sensor.x;
            let yi = sensor.y;
            let ri = *dist;

            // Linearized equation from difference of squared distances
            a_matrix[i][0] = 2.0 * (xi - x1);
            a_matrix[i][1] = 2.0 * (yi - y1);
            b_vector[i] = r1 * r1 - ri * ri - x1 * x1 + xi * xi - y1 * y1 + yi * yi;
        }

        // Solve using least squares: (A^T * A)^-1 * A^T * b
        let solution = self.solve_least_squares(&a_matrix, &b_vector)?;

        // Calculate uncertainty from residuals
        let uncertainty = self.calculate_uncertainty(&solution, distances);

        if uncertainty.horizontal_error > self.config.max_uncertainty {
            return None;
        }

        Some(Coordinates3D::new(
            solution[0],
            solution[1],
            0.0, // Z estimated separately
            uncertainty,
        ))
    }

    /// Solve linear system using least squares
    #[allow(clippy::needless_range_loop)]
    fn solve_least_squares(&self, a: &[Vec<f64>], b: &[f64]) -> Option<Vec<f64>> {
        let n = a.len();
        if n < 2 || a[0].len() != 2 {
            return None;
        }

        // Calculate A^T * A (dual-index matrix multiplication — range loop required)
        let mut ata = vec![vec![0.0; 2]; 2];
        for i in 0..2 {
            for j in 0..2 {
                for k in 0..n {
                    ata[i][j] += a[k][i] * a[k][j];
                }
            }
        }

        // Calculate A^T * b (dual-index — range loop required)
        let mut atb = [0.0; 2];
        for i in 0..2 {
            for k in 0..n {
                atb[i] += a[k][i] * b[k];
            }
        }

        // Solve 2x2 system using Cramer's rule
        let det = ata[0][0] * ata[1][1] - ata[0][1] * ata[1][0];
        if det.abs() < 1e-10 {
            return None;
        }

        let x = (atb[0] * ata[1][1] - atb[1] * ata[0][1]) / det;
        let y = (ata[0][0] * atb[1] - ata[1][0] * atb[0]) / det;

        Some(vec![x, y])
    }

    /// Calculate position uncertainty from residuals
    fn calculate_uncertainty(
        &self,
        position: &[f64],
        distances: &[(SensorPosition, f64)],
    ) -> LocationUncertainty {
        // Calculate root mean square error
        let mut sum_sq_error = 0.0;

        for (sensor, measured_dist) in distances {
            let dx = position[0] - sensor.x;
            let dy = position[1] - sensor.y;
            let estimated_dist = (dx * dx + dy * dy).sqrt();
            let error = measured_dist - estimated_dist;
            sum_sq_error += error * error;
        }

        let rmse = (sum_sq_error / distances.len() as f64).sqrt();

        // Real, dimensionless GDOP (Geometric Dilution of Precision). Falls back
        // to a unit factor for a degenerate (collinear) geometry where (HᵀH) is
        // singular — that geometry already produces a large residual RMSE.
        let gdop = self
            .compute_gdop(position, distances)
            .unwrap_or(1.0)
            .max(1.0);

        LocationUncertainty {
            horizontal_error: rmse * gdop,
            vertical_error: rmse * gdop * 1.5, // Vertical typically less accurate
            confidence: 0.95,
        }
    }

    /// Compute the real Geometric Dilution of Precision (GDOP).
    ///
    /// GDOP is the dimensionless factor by which measurement (range) noise is
    /// amplified into position error by the sensor geometry. For range-based 2D
    /// positioning the measurement Jacobian `H` has one row per sensor equal to
    /// the unit bearing vector from the target to that sensor,
    /// `[ (xₛ-xₜ)/d , (yₛ-yₜ)/d ]`. The position covariance (per unit measurement
    /// variance) is `(HᵀH)⁻¹`, and
    ///
    /// ```text
    /// GDOP = sqrt( trace( (HᵀH)⁻¹ ) )
    /// ```
    ///
    /// This is the same quantity ADR-156 §2.3 corrected elsewhere — a genuine
    /// dimensionless dilution, not the previous ad-hoc average-angle factor that
    /// was merely *labelled* GDOP. Returns `None` when `HᵀH` is singular
    /// (collinear / coincident geometry), which the caller treats as no
    /// dilution information (factor 1.0).
    fn compute_gdop(&self, position: &[f64], distances: &[(SensorPosition, f64)]) -> Option<f64> {
        let (tx, ty) = (position[0], position[1]);

        // Accumulate HᵀH (2×2, symmetric) from unit bearing vectors.
        let (mut hxx, mut hxy, mut hyy) = (0.0_f64, 0.0_f64, 0.0_f64);
        let mut rows = 0usize;
        for (sensor, _dist) in distances {
            let dx = sensor.x - tx;
            let dy = sensor.y - ty;
            let d = (dx * dx + dy * dy).sqrt();
            if d <= f64::EPSILON {
                continue; // target coincident with sensor: undefined bearing
            }
            let ux = dx / d;
            let uy = dy / d;
            hxx += ux * ux;
            hxy += ux * uy;
            hyy += uy * uy;
            rows += 1;
        }

        if rows < 2 {
            return None;
        }

        // Invert the 2×2 HᵀH. trace((HᵀH)⁻¹) = (hxx + hyy) / det.
        let det = hxx * hyy - hxy * hxy;
        if det.abs() < 1e-12 {
            return None; // singular: collinear geometry
        }
        let trace_inv = (hxx + hyy) / det;
        if trace_inv <= 0.0 {
            return None;
        }
        Some(trace_inv.sqrt())
    }
}

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

    fn create_test_sensors() -> Vec<SensorPosition> {
        vec![
            SensorPosition {
                id: "s1".to_string(),
                x: 0.0,
                y: 0.0,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
                last_rssi: None,
            },
            SensorPosition {
                id: "s2".to_string(),
                x: 10.0,
                y: 0.0,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
                last_rssi: None,
            },
            SensorPosition {
                id: "s3".to_string(),
                x: 5.0,
                y: 10.0,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
                last_rssi: None,
            },
        ]
    }

    #[test]
    fn test_rssi_to_distance() {
        let triangulator = Triangulator::with_defaults();

        // At reference distance, RSSI should equal reference RSSI
        let distance = triangulator.rssi_to_distance(-30.0);
        assert!((distance - 1.0).abs() < 0.1);

        // Weaker signal = further distance
        let distance2 = triangulator.rssi_to_distance(-60.0);
        assert!(distance2 > distance);
    }

    #[test]
    fn test_trilateration() {
        let triangulator = Triangulator::with_defaults();
        let sensors = create_test_sensors();

        // Target at (5, 4) - calculate distances
        let target: (f64, f64) = (5.0, 4.0);
        let distances: Vec<(&str, f64)> = vec![
            (
                "s1",
                ((target.0 - 0.0_f64).powi(2) + (target.1 - 0.0_f64).powi(2)).sqrt(),
            ),
            (
                "s2",
                ((target.0 - 10.0_f64).powi(2) + (target.1 - 0.0_f64).powi(2)).sqrt(),
            ),
            (
                "s3",
                ((target.0 - 5.0_f64).powi(2) + (target.1 - 10.0_f64).powi(2)).sqrt(),
            ),
        ];

        let dist_vec: Vec<(SensorPosition, f64)> = distances
            .iter()
            .filter_map(|(id, d)| {
                let sensor = sensors.iter().find(|s| s.id == *id)?;
                Some((sensor.clone(), *d))
            })
            .collect();

        let result = triangulator.trilaterate(&dist_vec);
        assert!(result.is_some());

        let pos = result.unwrap();
        assert!((pos.x - target.0).abs() < 0.5);
        assert!((pos.y - target.1).abs() < 0.5);
    }

    #[test]
    fn test_insufficient_sensors() {
        let triangulator = Triangulator::with_defaults();
        let sensors = create_test_sensors();

        // Only 2 distance measurements
        let rssi_values = vec![("s1".to_string(), -40.0), ("s2".to_string(), -45.0)];

        let result = triangulator.estimate_position(&sensors, &rssi_values);
        assert!(result.is_none());
    }

    fn sensor_at(id: &str, x: f64, y: f64) -> SensorPosition {
        SensorPosition {
            id: id.to_string(),
            x,
            y,
            z: 1.5,
            sensor_type: SensorType::Transceiver,
            is_operational: true,
            last_rssi: None,
        }
    }

    /// Real GDOP: dimensionless, geometry-dependent, and matches the closed-form
    /// sqrt(trace((HᵀH)⁻¹)). A well-spread (near-orthogonal) array must give a
    /// LOWER GDOP than a near-collinear one. (The old ad-hoc angle factor was not
    /// a true dilution and is replaced.)
    #[test]
    fn test_gdop_is_real_dilution() {
        let t = Triangulator::with_defaults();
        let target = [5.0_f64, 5.0_f64];

        // Well-distributed: an equilateral-ish triangle around the target.
        let good = vec![
            (sensor_at("a", 5.0, 15.0), 10.0),
            (sensor_at("b", -3.66, 0.0), 10.0),
            (sensor_at("c", 13.66, 0.0), 10.0),
        ];
        let gdop_good = t.compute_gdop(&target, &good).expect("good geometry");

        // Near-collinear: bearings nearly all along ±y with a tiny x-spread, so
        // HᵀH is ill-conditioned (invertible but with a small eigenvalue) and the
        // GDOP is large but finite.
        let bad = vec![
            (sensor_at("a", 5.3, 15.0), 10.0),
            (sensor_at("b", 4.7, 15.0), 10.0),
            (sensor_at("c", 5.1, -5.0), 10.0),
        ];
        let gdop_bad = t.compute_gdop(&target, &bad).expect("bad geometry");

        assert!(gdop_good >= 1.0, "GDOP must be >= 1 (dilution, dimensionless)");
        assert!(
            gdop_good < gdop_bad,
            "well-spread GDOP {gdop_good} must be < near-collinear GDOP {gdop_bad}"
        );

        // Closed-form cross-check for the well-spread case: each unit bearing
        // vector contributes to HᵀH; verify trace((HᵀH)⁻¹) explicitly.
        let (mut hxx, mut hxy, mut hyy) = (0.0, 0.0, 0.0);
        for (s, _d) in &good {
            let dx = s.x - target[0];
            let dy = s.y - target[1];
            let d = (dx * dx + dy * dy).sqrt();
            let (ux, uy) = (dx / d, dy / d);
            hxx += ux * ux;
            hxy += ux * uy;
            hyy += uy * uy;
        }
        let det = hxx * hyy - hxy * hxy;
        let expected = ((hxx + hyy) / det).sqrt();
        assert!((gdop_good - expected).abs() < 1e-9);
    }

    /// Collinear geometry makes HᵀH singular -> compute_gdop returns None,
    /// and the uncertainty path falls back to a unit factor (no fabrication).
    #[test]
    fn test_gdop_singular_collinear_is_none() {
        let t = Triangulator::with_defaults();
        let target = [0.0_f64, 0.0_f64];
        // All sensors on the +x axis through the target: bearings all ±x -> rank 1.
        let collinear = vec![
            (sensor_at("a", 1.0, 0.0), 1.0),
            (sensor_at("b", 2.0, 0.0), 2.0),
            (sensor_at("c", 3.0, 0.0), 3.0),
        ];
        assert!(t.compute_gdop(&target, &collinear).is_none());
    }
}

// ---------------------------------------------------------------------------
// Integration 5: Multi-AP TDoA triangulation via NeumannSolver
// ---------------------------------------------------------------------------

#[cfg(feature = "ruvector")]
use ruvector_solver::neumann::NeumannSolver;
#[cfg(feature = "ruvector")]
use ruvector_solver::types::CsrMatrix;

/// Solve multi-AP TDoA survivor localization using NeumannSolver.
///
/// For N access points with TDoA measurements, linearizes the hyperbolic
/// equations and solves the 2×2 normal equations system. Complexity is O(1)
/// in AP count (always solves a 2×2 system regardless of N).
///
/// # Arguments
/// * `tdoa_measurements` - Vec of (ap_i_idx, ap_j_idx, tdoa_seconds)
///   where tdoa = t_i - t_j (positive if closer to AP_i)
/// * `ap_positions` - Vec of (x_metres, y_metres) for each AP
///
/// # Returns
/// Some((x, y)) estimated survivor position in metres, or None if underdetermined
#[cfg(feature = "ruvector")]
pub fn solve_tdoa_triangulation(
    tdoa_measurements: &[(usize, usize, f32)],
    ap_positions: &[(f32, f32)],
) -> Option<(f32, f32)> {
    let n_meas = tdoa_measurements.len();
    if n_meas < 3 || ap_positions.len() < 2 {
        return None;
    }

    const C: f32 = 3e8_f32; // speed of light m/s
    let (x_ref, y_ref) = ap_positions[0];

    // Accumulate (A^T A) and (A^T b) for 2×2 normal equations
    let mut ata = [[0.0_f32; 2]; 2];
    let mut atb = [0.0_f32; 2];

    for &(i, j, tdoa) in tdoa_measurements {
        let (xi, yi) = ap_positions.get(i).copied().unwrap_or((x_ref, y_ref));
        let (xj, yj) = ap_positions.get(j).copied().unwrap_or((x_ref, y_ref));

        // Row of A: [xi - xj, yi - yj] (linearized TDoA)
        let ai0 = xi - xj;
        let ai1 = yi - yj;

        // RHS: C * tdoa / 2 + (xi^2 - xj^2 + yi^2 - yj^2) / 2 - x_ref*(xi-xj) - y_ref*(yi-yj)
        let bi = C * tdoa / 2.0 + ((xi * xi - xj * xj) + (yi * yi - yj * yj)) / 2.0
            - x_ref * ai0
            - y_ref * ai1;

        ata[0][0] += ai0 * ai0;
        ata[0][1] += ai0 * ai1;
        ata[1][0] += ai1 * ai0;
        ata[1][1] += ai1 * ai1;
        atb[0] += ai0 * bi;
        atb[1] += ai1 * bi;
    }

    // Tikhonov regularization
    let lambda = 0.01_f32;
    ata[0][0] += lambda;
    ata[1][1] += lambda;

    let csr = CsrMatrix::<f32>::from_coo(
        2,
        2,
        vec![
            (0, 0, ata[0][0]),
            (0, 1, ata[0][1]),
            (1, 0, ata[1][0]),
            (1, 1, ata[1][1]),
        ],
    );

    // Attempt the Neumann-series solver first; fall back to Cramer's rule for
    // the 2×2 case when the iterative solver cannot converge (e.g. the
    // diagonal is very large relative to f32 precision).
    if let Ok(r) = NeumannSolver::new(1e-5, 500).solve(&csr, &atb) {
        return Some((r.solution[0] + x_ref, r.solution[1] + y_ref));
    }

    // Cramer's rule fallback for the 2×2 normal equations.
    let det = ata[0][0] * ata[1][1] - ata[0][1] * ata[1][0];
    if det.abs() < 1e-10 {
        return None;
    }
    let x_sol = (atb[0] * ata[1][1] - atb[1] * ata[0][1]) / det;
    let y_sol = (ata[0][0] * atb[1] - ata[1][0] * atb[0]) / det;
    Some((x_sol + x_ref, y_sol + y_ref))
}

#[cfg(all(test, feature = "ruvector"))]
mod triangulation_tests {
    use super::*;

    #[test]
    fn tdoa_triangulation_insufficient_data() {
        let result = solve_tdoa_triangulation(&[(0, 1, 1e-9)], &[(0.0, 0.0), (5.0, 0.0)]);
        assert!(result.is_none());
    }

    #[test]
    fn tdoa_triangulation_symmetric_case() {
        // Target at centre (2.5, 2.5), APs at corners of 5m×5m square
        let aps = vec![(0.0_f32, 0.0), (5.0, 0.0), (5.0, 5.0), (0.0, 5.0)];
        // Target equidistant from all APs → TDoA ≈ 0 for all pairs
        let measurements = vec![
            (0_usize, 1_usize, 0.0_f32),
            (1, 2, 0.0),
            (2, 3, 0.0),
            (0, 3, 0.0),
        ];
        let result = solve_tdoa_triangulation(&measurements, &aps);
        assert!(result.is_some(), "should solve symmetric case");
        let (x, y) = result.unwrap();
        assert!(x.is_finite() && y.is_finite());
    }
}