use nalgebra::{Point2 as NPoint2, Point3 as NPoint3, Unit, Vector2 as NVec2, Vector3 as NVec3};
use serde::{Deserialize, Serialize};
use uom::si::f64::Length;
use uom::si::length::millimeter;

use crate::{Result, Tolerance, TypesError};

#[must_use]
fn mm(value: f64) -> Length {
    Length::new::<millimeter>(value)
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Point2Wire", into = "Point2Wire")]
pub struct Point2(NPoint2<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Point2", deny_unknown_fields)]
struct Point2Wire {
    x: f64,
    y: f64,
}

impl From<Point2> for Point2Wire {
    fn from(p: Point2) -> Self {
        Self { x: p.0.x, y: p.0.y }
    }
}

impl From<Point2Wire> for Point2 {
    fn from(w: Point2Wire) -> Self {
        Self(NPoint2::new(w.x, w.y))
    }
}

impl Point2 {
    #[must_use]
    pub fn origin() -> Self {
        Self(NPoint2::origin())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64) -> Self {
        Self(NPoint2::new(x, y))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length) -> Self {
        Self(NPoint2::new(x.get::<millimeter>(), y.get::<millimeter>()))
    }

    #[must_use]
    pub fn x(self) -> Length {
        mm(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        mm(self.0.y)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }
}

impl core::fmt::Debug for Point2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "Point2({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::fmt::Display for Point2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::ops::Sub for Point2 {
    type Output = Vec2;
    fn sub(self, rhs: Self) -> Vec2 {
        Vec2(self.0 - rhs.0)
    }
}

impl core::ops::Add<Vec2> for Point2 {
    type Output = Self;
    fn add(self, rhs: Vec2) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub<Vec2> for Point2 {
    type Output = Self;
    fn sub(self, rhs: Vec2) -> Self {
        Self(self.0 - rhs.0)
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Vec2Wire", into = "Vec2Wire")]
pub struct Vec2(NVec2<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Vec2", deny_unknown_fields)]
struct Vec2Wire {
    x: f64,
    y: f64,
}

impl From<Vec2> for Vec2Wire {
    fn from(v: Vec2) -> Self {
        Self { x: v.0.x, y: v.0.y }
    }
}

impl From<Vec2Wire> for Vec2 {
    fn from(w: Vec2Wire) -> Self {
        Self(NVec2::new(w.x, w.y))
    }
}

impl Vec2 {
    #[must_use]
    pub fn zero() -> Self {
        Self(NVec2::zeros())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64) -> Self {
        Self(NVec2::new(x, y))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length) -> Self {
        Self(NVec2::new(x.get::<millimeter>(), y.get::<millimeter>()))
    }

    #[must_use]
    pub fn x(self) -> Length {
        mm(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        mm(self.0.y)
    }

    #[must_use]
    pub fn coords_mm(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }

    #[must_use]
    pub fn dot_mm2(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn cross_z_mm2(self, other: Self) -> f64 {
        self.0.x * other.0.y - self.0.y * other.0.x
    }

    #[must_use]
    pub fn norm_squared_mm2(self) -> f64 {
        self.0.norm_squared()
    }

    #[must_use]
    pub fn norm_mm(self) -> f64 {
        self.0.norm()
    }

    #[must_use]
    pub fn norm(self) -> Length {
        mm(self.0.norm())
    }

    #[must_use]
    pub fn perp_ccw(self) -> Self {
        Self(NVec2::new(-self.0.y, self.0.x))
    }

    pub fn try_normalize(self, tolerance: Tolerance) -> Result<UnitVec2> {
        Unit::try_new(self.0, tolerance.value())
            .map(UnitVec2)
            .ok_or(TypesError::ZeroLengthAxis)
    }
}

impl core::fmt::Debug for Vec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "Vec2({} mm, {} mm)", self.0.x, self.0.y)
    }
}

impl core::fmt::Display for Vec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "<{} mm, {} mm>", self.0.x, self.0.y)
    }
}

impl core::ops::Add for Vec2 {
    type Output = Self;
    fn add(self, rhs: Self) -> Self {
        Self(self.0 + rhs.0)
    }
}

impl core::ops::Sub for Vec2 {
    type Output = Self;
    fn sub(self, rhs: Self) -> Self {
        Self(self.0 - rhs.0)
    }
}

impl core::ops::Mul<f64> for Vec2 {
    type Output = Self;
    fn mul(self, rhs: f64) -> Self {
        Self(self.0 * rhs)
    }
}

impl core::ops::Mul<Vec2> for f64 {
    type Output = Vec2;
    fn mul(self, rhs: Vec2) -> Vec2 {
        Vec2(rhs.0 * self)
    }
}

impl core::ops::Neg for Vec2 {
    type Output = Self;
    fn neg(self) -> Self {
        Self(-self.0)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "UnitVec2Wire", into = "UnitVec2Wire")]
pub struct UnitVec2(Unit<NVec2<f64>>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "UnitVec2", deny_unknown_fields)]
struct UnitVec2Wire {
    x: f64,
    y: f64,
}

impl From<UnitVec2> for UnitVec2Wire {
    fn from(u: UnitVec2) -> Self {
        Self { x: u.0.x, y: u.0.y }
    }
}

impl TryFrom<UnitVec2Wire> for UnitVec2 {
    type Error = TypesError;
    fn try_from(w: UnitVec2Wire) -> Result<Self> {
        Self::try_from_components(w.x, w.y, Tolerance::new(f64::EPSILON))
    }
}

impl UnitVec2 {
    #[must_use]
    pub fn x_axis() -> Self {
        Self(Unit::new_unchecked(NVec2::new(1.0, 0.0)))
    }

    #[must_use]
    pub fn y_axis() -> Self {
        Self(Unit::new_unchecked(NVec2::new(0.0, 1.0)))
    }

    #[must_use]
    pub fn new_unchecked(x: f64, y: f64) -> Self {
        Self(Unit::new_unchecked(NVec2::new(x, y)))
    }

    pub fn try_from_components(x: f64, y: f64, tolerance: Tolerance) -> Result<Self> {
        Unit::try_new(NVec2::new(x, y), tolerance.value())
            .map(Self)
            .ok_or(TypesError::ZeroLengthAxis)
    }

    #[must_use]
    pub fn components(self) -> (f64, f64) {
        (self.0.x, self.0.y)
    }

    #[must_use]
    pub fn dot(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }

    #[must_use]
    pub fn perp_ccw(self) -> Self {
        Self(Unit::new_unchecked(NVec2::new(-self.0.y, self.0.x)))
    }

    #[must_use]
    pub fn into_vec(self, length: Length) -> Vec2 {
        let mm_value = length.get::<millimeter>();
        Vec2(self.0.into_inner() * mm_value)
    }
}

impl core::fmt::Display for UnitVec2 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "[{}, {}]", self.0.x, self.0.y)
    }
}

#[derive(Copy, Clone, PartialEq, Serialize, Deserialize)]
#[serde(from = "Point3Wire", into = "Point3Wire")]
pub struct Point3(NPoint3<f64>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "Point3", deny_unknown_fields)]
struct Point3Wire {
    x: f64,
    y: f64,
    z: f64,
}

impl From<Point3> for Point3Wire {
    fn from(p: Point3) -> Self {
        Self {
            x: p.0.x,
            y: p.0.y,
            z: p.0.z,
        }
    }
}

impl From<Point3Wire> for Point3 {
    fn from(w: Point3Wire) -> Self {
        Self(NPoint3::new(w.x, w.y, w.z))
    }
}

impl Point3 {
    #[must_use]
    pub fn origin() -> Self {
        Self(NPoint3::origin())
    }

    #[must_use]
    pub fn from_mm(x: f64, y: f64, z: f64) -> Self {
        Self(NPoint3::new(x, y, z))
    }

    #[must_use]
    pub fn from_lengths(x: Length, y: Length, z: Length) -> Self {
        Self(NPoint3::new(
            x.get::<millimeter>(),
            y.get::<millimeter>(),
            z.get::<millimeter>(),
        ))
    }

    #[must_use]
    pub fn x(self) -> Length {
        Length::new::<millimeter>(self.0.x)
    }

    #[must_use]
    pub fn y(self) -> Length {
        Length::new::<millimeter>(self.0.y)
    }

    #[must_use]
    pub fn z(self) -> Length {
        Length::new::<millimeter>(self.0.z)
    }
}

impl core::fmt::Debug for Point3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "Point3({} mm, {} mm, {} mm)",
            self.0.x, self.0.y, self.0.z,
        )
    }
}

impl core::fmt::Display for Point3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "({} mm, {} mm, {} mm)", self.0.x, self.0.y, self.0.z)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "UnitVec3Wire", into = "UnitVec3Wire")]
pub struct UnitVec3(Unit<NVec3<f64>>);

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "UnitVec3", deny_unknown_fields)]
struct UnitVec3Wire {
    x: f64,
    y: f64,
    z: f64,
}

impl From<UnitVec3> for UnitVec3Wire {
    fn from(u: UnitVec3) -> Self {
        Self {
            x: u.0.x,
            y: u.0.y,
            z: u.0.z,
        }
    }
}

impl TryFrom<UnitVec3Wire> for UnitVec3 {
    type Error = TypesError;
    fn try_from(w: UnitVec3Wire) -> Result<Self> {
        Self::try_from_components(w.x, w.y, w.z)
    }
}

impl UnitVec3 {
    #[must_use]
    pub fn x_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(1.0, 0.0, 0.0)))
    }

    #[must_use]
    pub fn y_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(0.0, 1.0, 0.0)))
    }

    #[must_use]
    pub fn z_axis() -> Self {
        Self(Unit::new_unchecked(NVec3::new(0.0, 0.0, 1.0)))
    }

    pub fn try_from_components(x: f64, y: f64, z: f64) -> Result<Self> {
        Unit::try_new(NVec3::new(x, y, z), f64::EPSILON)
            .map(Self)
            .ok_or(TypesError::ZeroLengthAxis)
    }

    #[must_use]
    pub fn components(self) -> (f64, f64, f64) {
        (self.0.x, self.0.y, self.0.z)
    }

    #[must_use]
    pub fn dot(self, other: Self) -> f64 {
        self.0.dot(&other.0)
    }
}

impl core::fmt::Display for UnitVec3 {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(f, "[{}, {}, {}]", self.0.x, self.0.y, self.0.z)
    }
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(try_from = "SketchPlaneBasisWire", into = "SketchPlaneBasisWire")]
pub struct SketchPlaneBasis {
    origin: Point3,
    x: UnitVec3,
    y: UnitVec3,
}

#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
#[serde(rename = "SketchPlaneBasis", deny_unknown_fields)]
struct SketchPlaneBasisWire {
    origin: Point3,
    x_axis: UnitVec3,
    y_axis: UnitVec3,
}

impl From<SketchPlaneBasis> for SketchPlaneBasisWire {
    fn from(b: SketchPlaneBasis) -> Self {
        Self {
            origin: b.origin,
            x_axis: b.x,
            y_axis: b.y,
        }
    }
}

impl TryFrom<SketchPlaneBasisWire> for SketchPlaneBasis {
    type Error = TypesError;
    fn try_from(w: SketchPlaneBasisWire) -> Result<Self> {
        Self::new(w.origin, w.x_axis, w.y_axis, Tolerance::new(1e-9))
    }
}

impl SketchPlaneBasis {
    pub fn new(origin: Point3, x: UnitVec3, y: UnitVec3, tolerance: Tolerance) -> Result<Self> {
        let dot = x.dot(y);
        if dot.abs() > tolerance.value() {
            return Err(TypesError::NonOrthogonalPlaneAxes(dot.abs()));
        }
        let y_projected = y.0.into_inner() - x.0.into_inner() * dot;
        let y_orthonormal =
            UnitVec3(Unit::try_new(y_projected, f64::EPSILON).ok_or(TypesError::ZeroLengthAxis)?);
        Ok(Self {
            origin,
            x,
            y: y_orthonormal,
        })
    }

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

    #[must_use]
    pub fn x_axis(self) -> UnitVec3 {
        self.x
    }

    #[must_use]
    pub fn y_axis(self) -> UnitVec3 {
        self.y
    }

    #[must_use]
    pub fn normal(self) -> UnitVec3 {
        UnitVec3(Unit::new_normalize(self.x.0.cross(&self.y.0)))
    }
}

impl core::fmt::Display for SketchPlaneBasis {
    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
        write!(
            f,
            "basis{{ o={}, x={}, y={} }}",
            self.origin, self.x, self.y,
        )
    }
}
