use bone_types::{Aabb3, ChordHeightTolerance, Length, Parameter, Point3, Tolerance, Vec3};
use uom::si::length::millimeter;

use crate::KernelError;
use crate::closest::ClosestPoint3;
use crate::curve3::{Curve3, Curve3Kind};

#[derive(Copy, Clone, Debug, PartialEq)]
pub struct Line3 {
    start: Point3,
    end: Point3,
}

impl Line3 {
    pub fn new(start: Point3, end: Point3, tolerance: Tolerance) -> Result<Self, KernelError> {
        if (end - start).norm_mm() < tolerance.value() {
            return Err(KernelError::DegenerateLine);
        }
        Ok(Self { start, end })
    }

    #[must_use]
    pub const fn start(self) -> Point3 {
        self.start
    }

    #[must_use]
    pub const fn end(self) -> Point3 {
        self.end
    }

    #[must_use]
    pub fn length_mm(self) -> f64 {
        (self.end - self.start).norm_mm()
    }

    #[must_use]
    pub fn as_kind(self) -> Curve3Kind {
        Curve3Kind::Line(self)
    }
}

impl Curve3 for Line3 {
    fn evaluate(&self, t: Parameter) -> Point3 {
        let (sx, sy, sz) = self.start.coords_mm();
        let (ex, ey, ez) = self.end.coords_mm();
        let u = t.value();
        Point3::from_mm(sx + u * (ex - sx), sy + u * (ey - sy), sz + u * (ez - sz))
    }

    fn derivative(&self, _t: Parameter) -> Vec3 {
        self.end - self.start
    }

    fn bounding_box(&self) -> Aabb3 {
        Aabb3::from_corners(self.start, self.end)
    }

    fn closest_point(&self, p: Point3, _tolerance: Tolerance) -> ClosestPoint3 {
        let d = self.end - self.start;
        let v = p - self.start;
        let len_sq = d.norm_squared_mm2();
        let t = (v.dot_mm2(d) / len_sq).clamp(0.0, 1.0);
        let point = self.evaluate(Parameter::new(t));
        let dist_mm = (p - point).norm_mm();
        ClosestPoint3::new(Parameter::new(t), point, Length::new::<millimeter>(dist_mm))
    }

    fn tessellate(&self, _tolerance: ChordHeightTolerance) -> Vec<Point3> {
        vec![self.start, self.end]
    }
}

impl core::fmt::Display for Line3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "line3{{ {} -> {} }}", self.start, self.end)
    }
}
