use glam::{Mat3, Vec3, Vec4};

use crate::math;

//? Should this be normalised or does that risk introducing fpp errors
#[derive(Clone, Copy, Debug)]
pub struct Plane {
    pub normal: Vec3,
    pub offset: f32,
}

impl Plane {
    pub fn from_point_normal(point: Vec3, normal: Vec3) -> Self {
        Self {
            normal,
            offset: point.dot(normal),
        }
    }

    pub fn into_vec4(&self) -> Vec4 {
        glam::vec4(self.normal.x, self.normal.y, self.normal.z, -self.offset)
    }
}

impl PartialEq for Plane {
    fn eq(&self, other: &Self) -> bool {
        (self.normal.x - other.normal.x).abs() < math::EPSILON
            && (self.normal.y - other.normal.y).abs() < math::EPSILON
            && (self.normal.z - other.normal.z).abs() < math::EPSILON
            && (self.offset - other.offset).abs() < math::EPSILON
    }
}

impl Eq for Plane {}

#[derive(Clone, Copy, Debug)]
pub struct PlaneIntersection {
    pub planes: [Plane; 3],
}

impl PlaneIntersection {
    pub fn from_planes(p1: Plane, p2: Plane, p3: Plane) -> Option<Self> {
        let plane = Self {
            planes: [p1, p2, p3],
        };

        let determinant = plane.get_matrix().determinant();
        // println!("Determinant: {determinant}");
        if determinant.abs() > math::EPSILON {
            Some(plane)
        } else {
            None
        }
    }

    pub fn get_intersection_point(&self) -> Vec3 {
        let base_matrix = self.get_matrix();
        let base_det = base_matrix.determinant();
        let offsets = glam::vec3(
            self.planes[0].offset,
            self.planes[1].offset,
            self.planes[2].offset,
        );

        let mut vs = vec![];
        for idx in 0..3 {
            let mut matrix = base_matrix;
            *(matrix.col_mut(idx)) = offsets;
            vs.push(matrix.determinant() / base_det);
        }

        Vec3::from_slice(&vs)
    }

    pub fn in_halfspace_mat(&self, plane: &Plane) -> bool {
        // "Distance" and side of the point defined by plane against the plane defined
        // by the 3 points of the intersection planes.
        //* Note that the sign is dependant on the winding order of the intersection planes
        let m1 = glam::mat4(
            self.planes[0].into_vec4(),
            self.planes[1].into_vec4(),
            self.planes[2].into_vec4(),
            plane.into_vec4(),
        );
        let d1 = m1.determinant();

        // We can resolve the winding order problem by multiplying by the normal matrix determinant
        let m2 = self.get_matrix().transpose();
        let d2 = m2.determinant();
        let dist = d1 * d2;

        //* Note: dist is only euclidean accurate if all the normals are normalised :)
        dist < math::EPSILON
    }

    pub fn in_halfspace(&self, plane: &Plane) -> bool {
        plane.normal.dot(self.get_intersection_point()) - plane.offset <= math::EPSILON
    }

    fn get_matrix(&self) -> Mat3 {
        glam::mat3(
            self.planes[0].normal,
            self.planes[1].normal,
            self.planes[2].normal,
        )
        .transpose()
    }
}