//! Point triangulation, mirroring `colmap/geometry/triangulation.h`.
//!
//! Provides linear (DLT) two-view triangulation, the mid-point method, a
//! multi-view DLT, and helpers to compute triangulation angles between observing
//! rays.

use crate::math::{Mat3x4, Vec2, Vec3};
use nalgebra::{Matrix4, Vector4};

/// Triangulates a 3D point from two views using the linear DLT (Direct Linear
/// Transform) method.
///
/// `p1`, `p2` are the `[R | t]` (or full projection) matrices and `x1`, `x2` the
/// corresponding image points (in the same coordinate system as the projection
/// matrices, typically normalized camera coordinates). Returns `None` if the
/// system is degenerate.
///
/// # Examples
/// ```
/// use colmap::geometry::triangulation::triangulate_point;
/// use colmap::geometry::Rigid3d;
/// use colmap::math::{UnitQuat, Vec2, Vec3};
///
/// let p1 = Rigid3d::identity().to_matrix();
/// let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
/// let p2 = pose2.to_matrix();
/// let point = Vec3::new(0.2, -0.1, 5.0);
/// let x1 = Vec2::new(point.x / point.z, point.y / point.z);
/// let pc2 = pose2.transform_point(&point);
/// let x2 = Vec2::new(pc2.x / pc2.z, pc2.y / pc2.z);
/// let tri = triangulate_point(&p1, &p2, &x1, &x2).unwrap();
/// assert!((tri - point).norm() < 1e-9);
/// ```
pub fn triangulate_point(p1: &Mat3x4, p2: &Mat3x4, x1: &Vec2, x2: &Vec2) -> Option<Vec3> {
    // Build the 4x4 system A X = 0, two rows per view.
    let mut a = Matrix4::<f64>::zeros();
    a.row_mut(0).copy_from(&(x1.x * p1.row(2) - p1.row(0)));
    a.row_mut(1).copy_from(&(x1.y * p1.row(2) - p1.row(1)));
    a.row_mut(2).copy_from(&(x2.x * p2.row(2) - p2.row(0)));
    a.row_mut(3).copy_from(&(x2.y * p2.row(2) - p2.row(1)));

    let svd = a.svd(false, true);
    let v_t = svd.v_t?;
    // The solution is the right-singular vector with the smallest singular value,
    // i.e. the last row of V^T.
    let xh: Vector4<f64> = v_t.row(3).transpose();
    if xh.w.abs() < f64::EPSILON {
        return None;
    }
    Some(Vec3::new(xh.x / xh.w, xh.y / xh.w, xh.z / xh.w))
}

/// Triangulates a 3D point (in camera-1 coordinates) from two bearing rays using
/// the mid-point method.
///
/// `cam2_from_cam1` is the relative pose, `ray1`/`ray2` are direction vectors in
/// each camera frame. Returns the point that minimizes the distance to both
/// rays, or `None` if the rays are (near) parallel.
pub fn triangulate_mid_point(
    cam2_from_cam1: &crate::geometry::Rigid3d,
    ray1: &Vec3,
    ray2: &Vec3,
) -> Option<Vec3> {
    // Camera 1 at the origin, looking along ray1.
    // Camera 2 center (in cam1 coords) and ray2 rotated into cam1 coords.
    let r = cam2_from_cam1.rotation.to_rotation_matrix();
    let c2 = cam2_from_cam1.target_origin_in_source(); // camera-2 center in cam1.
    let d1 = ray1.normalize();
    let d2 = (r.inverse() * ray2).normalize();

    // Solve for parameters t1, t2 minimizing || (t1 d1) - (c2 + t2 d2) ||.
    let b = c2;
    let d1d1 = d1.dot(&d1);
    let d1d2 = d1.dot(&d2);
    let d2d2 = d2.dot(&d2);
    let denom = d1d1 * d2d2 - d1d2 * d1d2;
    if denom.abs() < 1e-12 {
        return None;
    }
    let d1b = d1.dot(&b);
    let d2b = d2.dot(&b);
    let t1 = (d2d2 * d1b - d1d2 * d2b) / denom;
    let t2 = (d1d2 * d1b - d1d1 * d2b) / denom;

    let point1 = t1 * d1;
    let point2 = c2 + t2 * d2;
    Some(0.5 * (point1 + point2))
}

/// Triangulates a 3D point from an arbitrary number of views using the linear
/// DLT method (stacking two rows per observation and solving by SVD).
///
/// `proj_matrices` are the `[R | t]`/projection matrices and `points` the
/// corresponding image points. Requires at least two views; returns `None` for
/// fewer views or a degenerate system.
pub fn triangulate_multi_view_point(proj_matrices: &[Mat3x4], points: &[Vec2]) -> Option<Vec3> {
    let n = proj_matrices.len().min(points.len());
    if n < 2 {
        return None;
    }
    // Accumulate the normal-equation matrix A^T A (4x4) from all observations.
    let mut ata = Matrix4::<f64>::zeros();
    for i in 0..n {
        let p = &proj_matrices[i];
        let x = &points[i];
        let row0: Vector4<f64> = (x.x * p.row(2) - p.row(0)).transpose();
        let row1: Vector4<f64> = (x.y * p.row(2) - p.row(1)).transpose();
        ata += row0 * row0.transpose();
        ata += row1 * row1.transpose();
    }
    let eig = ata.symmetric_eigen();
    // Smallest eigenvalue's eigenvector is the solution.
    let mut best_idx = 0;
    let mut best_val = f64::INFINITY;
    for i in 0..4 {
        if eig.eigenvalues[i] < best_val {
            best_val = eig.eigenvalues[i];
            best_idx = i;
        }
    }
    let xh = eig.eigenvectors.column(best_idx);
    if xh[3].abs() < f64::EPSILON {
        return None;
    }
    Some(Vec3::new(xh[0] / xh[3], xh[1] / xh[3], xh[2] / xh[3]))
}

/// Computes the triangulation angle (in radians) at a 3D `point` as seen from two
/// camera projection centers `c1` and `c2`.
///
/// A larger angle means a better-conditioned triangulation; COLMAP uses this to
/// filter degenerate (near-zero parallax) points.
pub fn calculate_triangulation_angle(c1: &Vec3, c2: &Vec3, point: &Vec3) -> f64 {
    let ray1 = point - c1;
    let ray2 = point - c2;
    let baseline_sq = (c1 - c2).norm_squared();
    let ray1_sq = ray1.norm_squared();
    let ray2_sq = ray2.norm_squared();
    if ray1_sq < f64::EPSILON || ray2_sq < f64::EPSILON {
        return 0.0;
    }
    // Law of cosines: cos(angle) = (|ray1|^2 + |ray2|^2 - baseline^2) /
    //                              (2 |ray1| |ray2|).
    let denom = 2.0 * (ray1_sq * ray2_sq).sqrt();
    let cos_angle = ((ray1_sq + ray2_sq - baseline_sq) / denom).clamp(-1.0, 1.0);
    let angle = cos_angle.acos();
    // Return the acute angle, matching COLMAP (triangulation angle in [0, pi/2]).
    angle.min(std::f64::consts::PI - angle)
}

/// Computes the angle (in radians) between two vectors, in `[0, pi]`. Returns 0
/// if either vector is (near) zero.
pub fn calculate_angle_between_vectors(a: &Vec3, b: &Vec3) -> f64 {
    let na = a.norm();
    let nb = b.norm();
    if na < f64::EPSILON || nb < f64::EPSILON {
        return 0.0;
    }
    let cos_angle = (a.dot(b) / (na * nb)).clamp(-1.0, 1.0);
    cos_angle.acos()
}

#[cfg(test)]
mod tests {
    use super::*;
    use crate::geometry::Rigid3d;
    use crate::math::UnitQuat;
    use approx::assert_relative_eq;
    use std::f64::consts::FRAC_PI_2;

    fn project(pose: &Rigid3d, p: &Vec3) -> Vec2 {
        let pc = pose.transform_point(p);
        Vec2::new(pc.x / pc.z, pc.y / pc.z)
    }

    #[test]
    fn dlt_triangulates_known_point() {
        let pose1 = Rigid3d::identity();
        let pose2 = Rigid3d::new(
            UnitQuat::new(Vec3::new(0.0, 0.05, 0.0)),
            Vec3::new(-1.0, 0.0, 0.0),
        );
        let point = Vec3::new(0.4, -0.7, 6.0);
        let x1 = project(&pose1, &point);
        let x2 = project(&pose2, &point);
        let tri = triangulate_point(&pose1.to_matrix(), &pose2.to_matrix(), &x1, &x2).unwrap();
        assert_relative_eq!(tri, point, epsilon = 1e-9);
    }

    #[test]
    fn midpoint_triangulates_known_point() {
        let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
        let point = Vec3::new(0.2, 0.1, 5.0);
        let ray1 = point; // cam1 == world.
        let ray2 = pose2.transform_point(&point);
        let tri = triangulate_mid_point(&pose2, &ray1, &ray2).unwrap();
        assert_relative_eq!(tri, point, epsilon = 1e-9);
    }

    #[test]
    fn midpoint_parallel_rays_none() {
        let pose2 = Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0));
        // Both rays point along +z => parallel in cam1 coords => no intersection.
        let ray1 = Vec3::new(0.0, 0.0, 1.0);
        let ray2 = Vec3::new(0.0, 0.0, 1.0);
        assert!(triangulate_mid_point(&pose2, &ray1, &ray2).is_none());
    }

    #[test]
    fn multi_view_triangulates_known_point() {
        let poses = [
            Rigid3d::identity(),
            Rigid3d::new(UnitQuat::identity(), Vec3::new(-1.0, 0.0, 0.0)),
            Rigid3d::new(UnitQuat::new(Vec3::new(0.0, 0.1, 0.0)), Vec3::new(-2.0, 0.3, 0.0)),
        ];
        let point = Vec3::new(-0.5, 0.8, 7.0);
        let mats: Vec<Mat3x4> = poses.iter().map(|p| p.to_matrix()).collect();
        let pts: Vec<Vec2> = poses.iter().map(|p| project(p, &point)).collect();
        let tri = triangulate_multi_view_point(&mats, &pts).unwrap();
        assert_relative_eq!(tri, point, epsilon = 1e-8);
    }

    #[test]
    fn multi_view_needs_two_views() {
        let mats = vec![Rigid3d::identity().to_matrix()];
        let pts = vec![Vec2::new(0.0, 0.0)];
        assert!(triangulate_multi_view_point(&mats, &pts).is_none());
    }

    #[test]
    fn triangulation_angle_right_angle() {
        // Two cameras viewing a point at 90 degrees.
        let c1 = Vec3::new(-1.0, 0.0, 0.0);
        let c2 = Vec3::new(0.0, 0.0, -1.0);
        let point = Vec3::new(0.0, 0.0, 0.0);
        // ray1 = point - c1 = (1,0,0), ray2 = (0,0,1) -> angle 90 deg.
        let angle = calculate_triangulation_angle(&c1, &c2, &point);
        assert_relative_eq!(angle, FRAC_PI_2, epsilon = 1e-9);
    }

    #[test]
    fn angle_between_vectors_basic() {
        let a = Vec3::new(1.0, 0.0, 0.0);
        let b = Vec3::new(0.0, 1.0, 0.0);
        assert_relative_eq!(
            calculate_angle_between_vectors(&a, &b),
            FRAC_PI_2,
            epsilon = 1e-12
        );
        assert_relative_eq!(calculate_angle_between_vectors(&a, &a), 0.0, epsilon = 1e-12);
    }
}