use nalgebra::SMatrix;

use crate::{particle_system::Scalar, particle_system::N, Point, Vector};

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
    }

    pub 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};

    #[test]
    fn test_projection_onto_line() {
        let line = Line::new(Point::new(1.0, 0.0), [Vector::new(3.0, 1.0)]);
        let point = Point::new(3.0, 4.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);
            assert!(diff.norm() < 0.001, "Point projected incorrectly");
        }
    }
}