use nalgebra::SMatrix;

use super::{distance_field::DistanceField, Point, Scalar, Vector, N};


type ProjectionMatrix = SMatrix<Scalar, N, N>;

pub struct Subspace<const DIM: usize> {
    point: Point,
    vectors: [Vector; DIM],
    projection_matrix: ProjectionMatrix,
}

pub type Line = Subspace<1>;
pub type Plane = Subspace<2>;

impl<const DIM: usize> Subspace<DIM> {
    pub fn new(point: Point, mut vectors: [Vector; DIM]) -> Self {
        for vector in &mut vectors {
            vector.normalize_mut();
        }

        Self {
            point,
            vectors,
            projection_matrix: {
                let v = SMatrix::<Scalar, N, DIM>::from_columns(&vectors);
                let transpose = v.transpose();
                let inverse = (transpose * v).try_inverse().unwrap();
                v * inverse * transpose
            }
        }
    }

    pub fn project_point(&self, point: Point) -> Point {
        self.projection_matrix * (point - self.point.coords) + self.point.coords
    }

}

impl<const DIM: usize> DistanceField for Subspace<DIM> {
    fn distance(&self, point: Point) -> Scalar {
        let projected = self.project_point(point);
        (projected - point).norm()
    }
}

#[cfg(test)]
mod tests {
    use crate::algebra::{subspace::Line, Point, Vector, distance_field::DistanceField};

    #[test]
    fn test_projection_onto_line() {
        let line = Line::new(Point::new(1.0, 0.0, 0.0), [Vector::new(3.0, 1.0, 0.0)]);
        let point = Point::new(3.0, 4.0, 0.0);
        
        let matrix = line.projection_matrix;
        {
            let diff = matrix * matrix - matrix;
            assert!(diff.norm() < 0.001, "Projection matrix squared should be equal to itself: {}, {}", matrix, matrix* matrix);
        }
        {
            let diff = matrix - matrix.transpose();
            assert!(diff.norm() < 0.001, "Projection matrix transpose should be equal to itself: {}, {}", matrix, matrix.transpose());
        }

        {
            let projected = line.project_point(point);
            let diff = projected - Point::new(4.0, 1.0, 0.0);
            assert!(diff.norm() < 0.001, "Point projected incorrectly");
        }
    }

    #[test]
    fn test_distance() {
        let origin = Point::new(0.0, 0.0, 0.0);
        let line = Line::new(origin, [Vector::x()]);
        
        let dq = 0.001;
        let dx = line.distance(origin + Vector::x() * dq);
        let dy = line.distance(origin + Vector::y() * dq);
        let dz = line.distance(origin + Vector::z() * dq);
        assert_eq!(dx, 0.0);
        assert_eq!(dy, dz);
    }
}