//! 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())
    }

    /// 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();

        // GDOP (Geometric Dilution of Precision) approximation
        let gdop = self.estimate_gdop(position, distances);

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

    /// Estimate Geometric Dilution of Precision
    fn estimate_gdop(&self, position: &[f64], distances: &[(SensorPosition, f64)]) -> f64 {
        // Simplified GDOP based on sensor geometry
        let mut sum_angle = 0.0;
        let n = distances.len();

        for i in 0..n {
            for j in (i + 1)..n {
                let dx1 = distances[i].0.x - position[0];
                let dy1 = distances[i].0.y - position[1];
                let dx2 = distances[j].0.x - position[0];
                let dy2 = distances[j].0.y - position[1];

                let dot = dx1 * dx2 + dy1 * dy2;
                let mag1 = (dx1 * dx1 + dy1 * dy1).sqrt();
                let mag2 = (dx2 * dx2 + dy2 * dy2).sqrt();

                if mag1 > 0.0 && mag2 > 0.0 {
                    let cos_angle = (dot / (mag1 * mag2)).clamp(-1.0, 1.0);
                    let angle = cos_angle.acos();
                    sum_angle += angle;
                }
            }
        }

        // Average angle between sensor pairs
        let num_pairs = (n * (n - 1)) as f64 / 2.0;
        let avg_angle = if num_pairs > 0.0 {
            sum_angle / num_pairs
        } else {
            std::f64::consts::PI / 4.0
        };

        // GDOP is better when sensors are spread out (angle closer to 90 degrees)
        // GDOP gets worse as sensors are collinear
        let optimal_angle = std::f64::consts::PI / 2.0;
        let angle_factor = (avg_angle / optimal_angle - 1.0).abs() + 1.0;

        angle_factor.max(1.0)
    }
}

#[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,
            },
            SensorPosition {
                id: "s2".to_string(),
                x: 10.0,
                y: 0.0,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
            },
            SensorPosition {
                id: "s3".to_string(),
                x: 5.0,
                y: 10.0,
                z: 1.5,
                sensor_type: SensorType::Transceiver,
                is_operational: true,
            },
        ]
    }

    #[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());
    }
}

// ---------------------------------------------------------------------------
// 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());
    }
}
