use nalgebra::{Point2, SMatrix};

use crate::{
    algebra::subspace::Plane,
    algebra::{Point, Scalar, Vector, N},
};

pub struct Camera {
    plane: Plane,
    up: Vector,
    origin: Point,
    world_to_screen_space: SMatrix<Scalar, 2, N>,
    screen_space_to_world: SMatrix<Scalar, N, 2>,
}

impl Camera {
    pub fn new(origin: Point, up: Vector, right: Vector) -> Self {
        assert!(
            up.dot(&right) == 0.0,
            "Up and right vectors must be orthogonal"
        );
        let plane = Plane::new(origin, [up, right]);

        let screen_space_to_world = SMatrix::<Scalar, N, 2>::from_columns(&[right, up]);
        let world_to_screen_space = screen_space_to_world.pseudo_inverse(0.001).unwrap();

        Self {
            plane,
            up,
            origin,
            world_to_screen_space,
            screen_space_to_world,
        }
    }

    pub fn world_to_screen_space(&self, point: Point) -> Point2<Scalar> {
        let projected = self.plane.project_point(point);
        let in_screen_space = self.world_to_screen_space * (projected - self.origin);
        in_screen_space.into()
    }
    pub fn screen_space_to_world(&self, point: Point2<Scalar>) -> Point {
        (self.screen_space_to_world * point) + self.origin.coords
    }
}

#[cfg(test)]
mod tests {
    use nalgebra::Point2;

    use crate::algebra::{Point, Scalar, Vector};

    use super::Camera;

    #[test]
    fn test_projection() {
        let camera = Camera::new(
            Point::new(1.0, 0.0, 0.0),
            Vector::new(1.0, 2.0, 0.0),
            Vector::new(2.0, -1.0, 0.0),
        );

        let point = Point::new(3.0, 1.0, 0.0);

        let diff = camera.world_to_screen_space * point - Point2::<Scalar>::new(1.0, 1.0);
        assert!(
            diff.norm() < 0.001,
            "Camera translated point into screen_space incorrectly"
        );
    }
}