use super::traits::{SqrtExt, Tuple, VectorLike};
use super::{Float, NumFloat, PI};
use crate::utils::interval::Interval;
use crate::utils::math::{clamp, difference_of_products, quadratic, safe_asin};
use core::fmt;
use core::hash::{Hash, Hasher};
use core::iter::Sum;
use core::ops::{
    Add, AddAssign, Div, DivAssign, Index, IndexMut, Mul, MulAssign, Neg, Sub, SubAssign,
};
use num_traits::{AsPrimitive, FloatConst, Num, Signed, Zero};

pub trait MulAdd<M = Self, A = Self> {
    type Output;
    fn mul_add(self, multiplier: M, addend: A) -> Self::Output;
}

impl MulAdd<Float, Float> for Float {
    type Output = Float;
    #[inline(always)]
    fn mul_add(self, multiplier: Float, addend: Float) -> Self::Output {
        num_traits::Float::mul_add(self, multiplier, addend)
    }
}

impl MulAdd<f64, f64> for f64 {
    type Output = f64;
    #[inline(always)]
    fn mul_add(self, multiplier: f64, addend: f64) -> Self::Output {
        num_traits::Float::mul_add(self, multiplier, addend)
    }
}

// N-dimensional displacement
#[repr(C)]
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub struct Vector<T, const N: usize>(pub [T; N]);
// N-dimensional location
#[repr(C)]
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub struct Point<T, const N: usize>(pub [T; N]);
// N-dimensional surface normal
#[repr(C)]
#[derive(Debug, Copy, Clone, PartialEq, Eq)]
pub struct Normal<T, const N: usize>(pub [T; N]);

impl<T: fmt::Display, const N: usize> fmt::Display for Vector<T, N> {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "Vector(")?;
        for (i, item) in (&self.0).into_iter().enumerate() {
            if i > 0 {
                write!(f, ", ")?;
            }
            write!(f, "{}", item)?;
        }
        write!(f, ")")
    }
}

impl<T: fmt::Display, const N: usize> fmt::Display for Point<T, N> {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "Point(")?;
        for (i, item) in (&self.0).into_iter().enumerate() {
            if i > 0 {
                write!(f, ", ")?;
            }
            write!(f, "{}", item)?;
        }
        write!(f, ")")
    }
}

impl<T: fmt::Display, const N: usize> fmt::Display for Normal<T, N> {
    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
        write!(f, "Normal(")?;
        for (i, item) in (&self.0).into_iter().enumerate() {
            if i > 0 {
                write!(f, ", ")?;
            }
            write!(f, "{}", item)?;
        }
        write!(f, ")")
    }
}

#[macro_export]
macro_rules! impl_tuple_core {
    ($Struct:ident) => {
        impl<T: Copy, const N: usize> Tuple<T, N> for $Struct<T, N> {
            #[inline]
            fn data(&self) -> &[T; N] {
                &self.0
            }

            #[inline]
            fn data_mut(&mut self) -> &mut [T; N] {
                &mut self.0
            }

            #[inline]
            fn from_array(arr: [T; N]) -> Self {
                Self(arr)
            }
        }

        impl<T: Default + Copy, const N: usize> Default for $Struct<T, N> {
            fn default() -> Self {
                Self([T::default(); N])
            }
        }

        impl<T, const N: usize> $Struct<T, N>
        where
            T: Zero + Copy,
        {
            #[inline]
            pub fn zero() -> Self {
                Self([T::zero(); N])
            }
        }

        impl<T, const N: usize> From<T> for $Struct<T, N>
        where
            T: Copy,
        {
            #[inline]
            fn from(scalar: T) -> Self {
                Self([scalar; N])
            }
        }

        impl<const N: usize> $Struct<f32, N> {
            #[inline]
            pub fn floor(&self) -> $Struct<i32, N> {
                $Struct(self.0.map(|v| v.floor() as i32))
            }

            #[inline]
            pub fn average(&self) -> f32 {
                let sum: f32 = self.0.iter().sum();
                sum / (N as f32)
            }
        }

        impl<T, const N: usize> $Struct<T, N>
        where
            T: Copy + PartialOrd,
        {
            #[inline]
            pub fn min(&self, other: Self) -> Self {
                let mut out = self.0;
                for i in 0..N {
                    if other.0[i] < out[i] {
                        out[i] = other.0[i];
                    }
                }
                Self(out)
            }

            #[inline]
            pub fn max(&self, other: Self) -> Self {
                let mut out = self.0;
                for i in 0..N {
                    if other.0[i] > out[i] {
                        out[i] = other.0[i]
                    }
                }
                Self(out)
            }

            #[inline]
            pub fn max_component_value(&self) -> T {
                let mut m = self.0[0];
                for i in 1..N {
                    if self.0[i] > m {
                        m = self.0[i];
                    }
                }
                m
            }
        }

        impl<T, const N: usize> $Struct<T, N>
        where
            T: Copy,
        {
            #[inline]
            pub fn fill(value: T) -> Self {
                Self([value; N])
            }

            #[inline]
            pub fn cast<U>(&self) -> $Struct<U, N>
            where
                U: 'static + Copy,
                T: 'static + Copy + AsPrimitive<U>,
            {
                $Struct(self.0.map(|c| c.as_()))
            }
        }

        impl<T, const N: usize> Index<usize> for $Struct<T, N> {
            type Output = T;
            #[inline]
            fn index(&self, index: usize) -> &Self::Output {
                &self.0[index]
            }
        }
        impl<T, const N: usize> IndexMut<usize> for $Struct<T, N> {
            #[inline]
            fn index_mut(&mut self, index: usize) -> &mut Self::Output {
                &mut self.0[index]
            }
        }

        impl<T, const N: usize> Neg for $Struct<T, N>
        where
            T: Neg<Output = T> + Copy,
        {
            type Output = Self;
            fn neg(self) -> Self::Output {
                Self(self.0.map(|c| -c))
            }
        }
    };
}

#[macro_export]
macro_rules! impl_num_zero {
    ($Struct:ident) => {
        impl<T, const N: usize> num_traits::Zero for $Struct<T, N>
        where
            T: num_traits::Zero + Copy + PartialEq,
        {
            #[inline]
            fn zero() -> Self {
                Self([T::zero(); N])
            }
            #[inline]
            fn is_zero(&self) -> bool {
                self.0.iter().all(|c| c.is_zero())
            }
        }
    };
}
impl_num_zero!(Vector);
impl_num_zero!(Normal);

#[macro_export]
macro_rules! impl_scalar_ops {
    ($Struct:ident) => {
        impl<T, const N: usize> Mul<T> for $Struct<T, N>
        where
            T: Mul<Output = T> + Copy,
        {
            type Output = Self;
            fn mul(self, rhs: T) -> Self::Output {
                let mut result = self.0;
                for i in 0..N {
                    result[i] = result[i] * rhs;
                }
                Self(result)
            }
        }

        impl<const N: usize> Mul<$Struct<Float, N>> for Float {
            type Output = $Struct<Float, N>;
            fn mul(self, rhs: $Struct<Float, N>) -> Self::Output {
                rhs * self
            }
        }

        impl<T, const N: usize> MulAssign<T> for $Struct<T, N>
        where
            T: MulAssign + Copy,
        {
            fn mul_assign(&mut self, rhs: T) {
                for i in 0..N {
                    self.0[i] *= rhs;
                }
            }
        }

        impl<T, const N: usize> Div<T> for $Struct<T, N>
        where
            T: Div<Output = T> + Copy,
        {
            type Output = Self;
            fn div(self, rhs: T) -> Self::Output {
                let mut result = self.0;
                for i in 0..N {
                    result[i] = result[i] / rhs;
                }
                Self(result)
            }
        }
        impl<T, const N: usize> DivAssign<T> for $Struct<T, N>
        where
            T: DivAssign + Copy,
        {
            fn div_assign(&mut self, rhs: T) {
                for i in 0..N {
                    self.0[i] /= rhs;
                }
            }
        }
    };
}

#[macro_export]
macro_rules! impl_op {
    ($Op:ident, $op:ident, $Lhs:ident, $Rhs:ident, $Output:ident) => {
        impl<T, const N: usize> $Op<$Rhs<T, N>> for $Lhs<T, N>
        where
            T: $Op<Output = T> + Copy,
        {
            type Output = $Output<T, N>;
            fn $op(self, rhs: $Rhs<T, N>) -> Self::Output {
                let mut result = self.0;
                for i in 0..N {
                    result[i] = $Op::$op(self.0[i], rhs.0[i]);
                }
                $Output(result)
            }
        }
    };
}

#[macro_export]
macro_rules! impl_op_assign {
    ($OpAssign:ident, $op_assign:ident, $Lhs:ident, $Rhs:ident) => {
        impl<T, const N: usize> $OpAssign<$Rhs<T, N>> for $Lhs<T, N>
        where
            T: $OpAssign + Copy,
        {
            fn $op_assign(&mut self, rhs: $Rhs<T, N>) {
                for i in 0..N {
                    $OpAssign::$op_assign(&mut self.0[i], rhs.0[i]);
                }
            }
        }
    };
}

#[macro_export]
macro_rules! impl_mul_add {
    ($Struct:ident) => {
        impl<T, const N: usize> MulAdd<T, $Struct<T, N>> for $Struct<T, N>
        where
            T: MulAdd<T, T, Output = T> + Copy,
        {
            type Output = $Struct<T, N>;

            #[inline(always)]
            fn mul_add(self, multiplier: T, addend: $Struct<T, N>) -> Self::Output {
                let mut result = self.0;
                for i in 0..N {
                    result[i] = self.0[i].mul_add(multiplier, addend.0[i]);
                }
                Self(result)
            }
        }
    };
}

#[macro_export]
macro_rules! impl_float_vector_ops {
    ($Struct:ident) => {
        impl<T, const N: usize> VectorLike for $Struct<T, N>
        where
            T: Copy
                + Zero
                + Add<Output = T>
                + Mul<Output = T>
                + Sub<Output = T>
                + Div<Output = T>
                + SqrtExt,
        {
            type Scalar = T;
            fn dot(self, rhs: Self) -> T {
                let mut sum = T::zero();
                for i in 0..N {
                    sum = sum + self[i] * rhs[i];
                }
                sum
            }
        }
    };
}

macro_rules! impl_tuple_conversions {
    ($Struct:ident) => {
        impl<T: NumFloat> From<(T, T, T)> for $Struct<T, 3> {
            fn from(tuple: (T, T, T)) -> Self {
                Self([tuple.0, tuple.1, tuple.2])
            }
        }

        // Allow converting (x, y) -> Vector<T, 2>
        impl<T: NumFloat> From<(T, T)> for $Struct<T, 2> {
            fn from(tuple: (T, T)) -> Self {
                Self([tuple.0, tuple.1])
            }
        }
    };
}

macro_rules! impl_abs {
    ($Struct:ident) => {
        impl<T, const N: usize> $Struct<T, N>
        where
            T: Signed + Copy,
        {
            pub fn abs(self) -> Self {
                let mut result = self.0;
                for i in 0..N {
                    result[i] = result[i].abs();
                }
                Self(result)
            }
        }
    };
}

macro_rules! impl_accessors {
    ($Struct:ident) => {
        impl<T: Copy> $Struct<T, 2> {
            pub fn x(&self) -> T {
                self.0[0]
            }
            pub fn y(&self) -> T {
                self.0[1]
            }
        }
        impl<T: Copy> $Struct<T, 3> {
            pub fn x(&self) -> T {
                self.0[0]
            }
            pub fn y(&self) -> T {
                self.0[1]
            }
            pub fn z(&self) -> T {
                self.0[2]
            }
        }
    };
}

impl_tuple_core!(Vector);
impl_tuple_core!(Point);
impl_tuple_core!(Normal);

impl_scalar_ops!(Vector);
impl_scalar_ops!(Normal);

// Addition
impl_op!(Add, add, Vector, Vector, Vector);
impl_op!(Add, add, Point, Vector, Point);
impl_op!(Add, add, Vector, Point, Point);
impl_op!(Add, add, Normal, Normal, Normal);

// Subtraction
impl_op!(Sub, sub, Vector, Vector, Vector);
impl_op!(Sub, sub, Point, Vector, Point);
impl_op!(Sub, sub, Point, Point, Vector);
impl_op!(Sub, sub, Normal, Normal, Normal);

// AddAssign
impl_op_assign!(AddAssign, add_assign, Vector, Vector);
impl_op_assign!(AddAssign, add_assign, Point, Vector);
impl_op_assign!(AddAssign, add_assign, Normal, Normal);

// SubAssign
impl_op_assign!(SubAssign, sub_assign, Vector, Vector);
impl_op_assign!(SubAssign, sub_assign, Point, Vector);
impl_op_assign!(SubAssign, sub_assign, Normal, Normal);

impl_float_vector_ops!(Vector);
impl_float_vector_ops!(Normal);
impl_abs!(Vector);
impl_abs!(Normal);
impl_abs!(Point);
impl_accessors!(Vector);
impl_accessors!(Point);
impl_accessors!(Normal);

impl_mul_add!(Vector);
impl_mul_add!(Point);
impl_mul_add!(Normal);

// Convert from tuple of Floats, for parsing issues
impl_tuple_conversions!(Vector);
impl_tuple_conversions!(Point);
impl_tuple_conversions!(Normal);

impl<T: Copy, const N: usize> From<Vector<T, N>> for Normal<T, N> {
    fn from(v: Vector<T, N>) -> Self {
        Self(v.0)
    }
}
impl<T: Copy, const N: usize> From<Normal<T, N>> for Vector<T, N> {
    fn from(n: Normal<T, N>) -> Self {
        Self(n.0)
    }
}

impl<T: Copy, const N: usize> From<Vector<T, N>> for Point<T, N> {
    fn from(v: Vector<T, N>) -> Self {
        Self(v.0)
    }
}

impl<T: Copy, const N: usize> From<Point<T, N>> for Vector<T, N> {
    fn from(n: Point<T, N>) -> Self {
        Self(n.0)
    }
}

impl<T, const N: usize> Point<T, N>
where
    T: NumFloat + SqrtExt,
{
    pub fn distance(self, other: Self) -> T {
        (self - other).norm()
    }

    pub fn distance_squared(self, other: Self) -> T {
        (self - other).norm_squared()
    }
}

impl Point2f {
    pub fn invert_bilinear(p: Point2f, vert: &[Point2f]) -> Point2f {
        let a = vert[0];
        let b = vert[1];
        let c = vert[3];
        let d = vert[2];
        let e = b - a;
        let f = d - a;
        let g = (a - b) + (c - d);
        let h = p - a;

        let cross2d = |a: Vector2f, b: Vector2f| difference_of_products(a.x(), b.y(), a.y(), b.x());

        let k2 = cross2d(g, f);
        let k1 = cross2d(e, f) + cross2d(h, g);
        let k0 = cross2d(h, e);

        // if edges are parallel, this is a linear equation
        if k2.abs() < 0.001 {
            if (e.x() * k1 - g.x() * k0).abs() < 1e-5 {
                return Point2f::new(
                    (h.y() * k1 + f.y() * k0) / (e.y() * k1 - g.y() * k0),
                    -k0 / k1,
                );
            } else {
                return Point2f::new(
                    (h.x() * k1 + f.x() * k0) / (e.x() * k1 - g.x() * k0),
                    -k0 / k1,
                );
            }
        }

        if let Some((v0, v1)) = quadratic(k2, k1, k0) {
            let u = (h.x() - f.x() * v0) / (e.x() + g.x() * v0);
            if !(0.0..=1.).contains(&u) || !(0.0..=1.0).contains(&v0) {
                return Point2f::new((h.x() - f.x() * v1) / (e.x() + g.x() * v1), v1);
            }
            Point2f::new(u, v0)
        } else {
            Point2f::zero()
        }
    }
}

// Utility aliases and functions
pub type Point2<T> = Point<T, 2>;
pub type Point2f = Point2<Float>;
pub type Point2i = Point2<i32>;
pub type Point2fi = Point2<Interval>;
pub type Point3<T> = Point<T, 3>;
pub type Point3f = Point3<Float>;
pub type Point3i = Point3<i32>;
pub type Point3fi = Point3<Interval>;
pub type Vector2<T> = Vector<T, 2>;
pub type Vector2f = Vector2<Float>;
pub type Vector2i = Vector2<i32>;
pub type Vector2fi = Vector2<Interval>;
pub type Vector3<T> = Vector<T, 3>;
pub type Vector3f = Vector3<Float>;
pub type Vector3i = Vector3<i32>;
pub type Vector3fi = Vector3<Interval>;
pub type Normal2<T> = Normal<T, 2>;
pub type Normal3<T> = Normal<T, 3>;
pub type Normal3f = Normal3<Float>;
pub type Normal3i = Normal3<i32>;
pub type Vector4<T> = Vector<T, 4>;

impl<T: Copy> Vector2<T> {
    pub fn new(x: T, y: T) -> Self {
        Self([x, y])
    }
}
impl<T: Copy> Point2<T> {
    pub fn new(x: T, y: T) -> Self {
        Self([x, y])
    }
}
impl<T: Copy> Normal2<T> {
    pub fn new(x: T, y: T) -> Self {
        Self([x, y])
    }
}

impl<T: Copy> Vector3<T> {
    pub fn new(x: T, y: T, z: T) -> Self {
        Self([x, y, z])
    }
}
impl<T: Copy> Point3<T> {
    pub fn new(x: T, y: T, z: T) -> Self {
        Self([x, y, z])
    }
}
impl<T: Copy> Normal3<T> {
    pub fn new(x: T, y: T, z: T) -> Self {
        Self([x, y, z])
    }
}
impl<T: Copy> Vector4<T> {
    pub fn new(x: T, y: T, z: T, w: T) -> Self {
        Self([x, y, z, w])
    }
}

// Vector operations
impl<T> Vector3<T>
where
    T: Num + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
{
    pub fn cross(self, rhs: Self) -> Self {
        Self([
            difference_of_products(self[1], rhs[2], self[2], rhs[1]),
            difference_of_products(self[2], rhs[0], self[0], rhs[2]),
            difference_of_products(self[0], rhs[1], self[1], rhs[0]),
        ])
    }
}

impl<T> Normal3<T>
where
    T: Num + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
{
    pub fn cross(self, rhs: Self) -> Self {
        Self([
            difference_of_products(self[1], rhs[2], self[2], rhs[1]),
            difference_of_products(self[2], rhs[0], self[0], rhs[2]),
            difference_of_products(self[0], rhs[1], self[1], rhs[0]),
        ])
    }
}

impl<T> Vector3<T>
where
    T: Num + NumFloat + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
{
    pub fn coordinate_system(&self) -> (Self, Self)
    where
        T: NumFloat,
    {
        let v2 = if self[0].abs() > self[1].abs() {
            Self::new(-self[2], T::zero(), self[0]) / (self[0] * self[0] + self[2] * self[2]).sqrt()
        } else {
            Self::new(T::zero(), self[2], -self[1]) / (self[1] * self[1] + self[2] * self[2]).sqrt()
        };
        (v2, self.cross(v2))
    }

    pub fn coordinate_system_from_cpp(&self) -> (Self, Self) {
        let sign = self.z().copysign(T::one());
        let a = -T::one() / (sign + self.z());
        let b = self.x() * self.y() * a;
        let v2 = Self::new(
            T::one() + sign * self.x().powi(2) * a,
            sign * b,
            -sign * self.x(),
        );
        let v3 = Self::new(b, sign + self.y().powi(2) * a, -self.y());
        (v2, v3)
    }
}

impl<T> Normal3<T>
where
    T: Num + NumFloat + Copy + Neg<Output = T> + Zero + MulAdd<T, T, Output = T>,
{
    pub fn coordinate_system(&self) -> (Self, Self)
    where
        T: NumFloat,
    {
        let v2 = if self[0].abs() > self[1].abs() {
            Self::new(-self[2], T::zero(), self[0]) / (self[0] * self[0] + self[2] * self[2]).sqrt()
        } else {
            Self::new(T::zero(), self[2], -self[1]) / (self[1] * self[1] + self[2] * self[2]).sqrt()
        };
        (v2, self.cross(v2))
    }

    pub fn coordinate_system_from_cpp(&self) -> (Self, Self) {
        let sign = self.z().copysign(T::one());
        let a = -T::one() / (sign + self.z());
        let b = self.x() * self.y() * a;
        let v2 = Self::new(
            T::one() + sign * self.x().powi(2) * a,
            sign * b,
            -sign * self.x(),
        );
        let v3 = Self::new(b, sign + self.y().powi(2) * a, -self.y());
        (v2, v3)
    }
}

impl<const N: usize> Hash for Vector<Float, N> {
    fn hash<H: Hasher>(&self, state: &mut H) {
        for item in self.0.iter() {
            item.to_bits().hash(state);
        }
    }
}

impl<const N: usize> Hash for Point<Float, N> {
    fn hash<H: Hasher>(&self, state: &mut H) {
        for item in self.0.iter() {
            item.to_bits().hash(state);
        }
    }
}

impl<const N: usize> Hash for Normal<Float, N> {
    fn hash<H: Hasher>(&self, state: &mut H) {
        for item in self.0.iter() {
            item.to_bits().hash(state);
        }
    }
}

// INTERVAL STUFF
impl<const N: usize> Point<Interval, N> {
    pub fn new_from_point(p: Point<Float, N>) -> Self {
        let mut arr = [Interval::default(); N];
        for i in 0..N {
            arr[i] = Interval::new(p[i]);
        }
        Self(arr)
    }

    pub fn new_with_error(p: Point<Float, N>, e: Vector<Float, N>) -> Self {
        let mut arr = [Interval::default(); N];
        for i in 0..N {
            arr[i] = Interval::new_from_value_and_error(p[i], e[i]);
        }
        Self(arr)
    }

    pub fn error(&self) -> Vector<Float, N> {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = self[i].width() / 2.0;
        }
        Vector(arr)
    }

    pub fn midpoint(&self) -> Point<Float, N> {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = self[i].midpoint();
        }
        Point(arr)
    }

    pub fn is_exact(&self) -> bool {
        self.0.iter().all(|interval| interval.width() == 0.0)
    }
}

impl<const N: usize> Vector<Interval, N> {
    pub fn new_from_vector(v: Vector<Float, N>) -> Self {
        let mut arr = [Interval::default(); N];
        for i in 0..N {
            arr[i] = Interval::new(v[i]);
        }
        Self(arr)
    }

    pub fn new_with_error(v: Vector<Float, N>, e: Vector<Float, N>) -> Self {
        let mut arr = [Interval::default(); N];
        for i in 0..N {
            arr[i] = Interval::new_from_value_and_error(v[i], e[i]);
        }
        Self(arr)
    }

    pub fn error(&self) -> Vector<Float, N> {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = self[i].width() / 2.0;
        }
        Vector(arr)
    }

    pub fn midpoint(&self) -> Vector<Float, N> {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = self[i].midpoint();
        }
        Vector(arr)
    }

    pub fn is_exact(&self) -> bool {
        self.0.iter().all(|interval| interval.width() == 0.0)
    }
}

impl<const N: usize> From<Point<Interval, N>> for Point<Float, N> {
    fn from(pi: Point<Interval, N>) -> Self {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = pi[i].midpoint();
        }
        Point(arr)
    }
}

impl<const N: usize> From<Vector<Interval, N>> for Vector<Float, N> {
    fn from(pi: Vector<Interval, N>) -> Self {
        let mut arr = [0.0; N];
        for i in 0..N {
            arr[i] = pi[i].midpoint();
        }
        Vector(arr)
    }
}

impl<const N: usize> From<Vector<Float, N>> for Vector<Interval, N> {
    fn from(v: Vector<Float, N>) -> Self {
        let mut arr = [Interval::default(); N];
        for i in 0..N {
            arr[i] = Interval::new(v[i]);
        }
        Self(arr)
    }
}

impl<const N: usize> Mul<Vector<Interval, N>> for Interval {
    type Output = Vector<Interval, N>;
    fn mul(self, rhs: Vector<Interval, N>) -> Self::Output {
        rhs * self
    }
}

impl<const N: usize> Div<Vector<Interval, N>> for Interval {
    type Output = Vector<Interval, N>;
    fn div(self, rhs: Vector<Interval, N>) -> Self::Output {
        let mut result = rhs.0;
        for i in 0..N {
            result[i] = self / rhs[i];
        }
        Vector(result)
    }
}

impl<const N: usize> From<Vector<i32, N>> for Vector<f32, N> {
    fn from(v: Vector<i32, N>) -> Self {
        Self(v.0.map(|c| c as f32))
    }
}

impl<const N: usize> From<Point<i32, N>> for Point<Float, N> {
    fn from(p: Point<i32, N>) -> Self {
        Point(p.0.map(|c| c as Float))
    }
}

impl<T> Normal3<T>
where
    T: Num + PartialOrd + Copy + Neg<Output = T> + SqrtExt,
{
    pub fn face_forward(self, v: impl Into<Vector3<T>>) -> Self {
        let v: Vector3<T> = v.into();
        if Vector3::<T>::from(self).dot(v) < T::zero() {
            -self
        } else {
            self
        }
    }
}

#[repr(C)]
#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
pub struct OctahedralVector {
    x: u16,
    y: u16,
}

impl OctahedralVector {
    pub fn new(mut v: Vector3f) -> Self {
        v /= v.x().abs() + v.y().abs() + v.z().abs();

        let (x_enc, y_enc) = if v.z() >= 0.0 {
            (Self::encode(v.x()), Self::encode(v.y()))
        } else {
            (
                Self::encode((1.0 - v.y().abs()) * Self::sign(v.x())),
                Self::encode((1.0 - v.x().abs()) * Self::sign(v.y())),
            )
        };

        Self { x: x_enc, y: y_enc }
    }

    pub fn to_vector(self) -> Vector3f {
        let mut v = Vector3f::default();

        // Map [0, 65535] back to [-1, 1]
        v[0] = -1.0 + 2.0 * (self.x as Float / 65535.0);
        v[1] = -1.0 + 2.0 * (self.y as Float / 65535.0);
        v[2] = 1.0 - (v.x().abs() + v.y().abs());

        if v.z() < 0.0 {
            let xo = v.x();
            v[0] = (1.0 - v.y().abs()) * Self::sign(xo);
            v[1] = (1.0 - xo.abs()) * Self::sign(v.y());
        }

        v.normalize()
    }

    #[inline]
    pub fn sign(v: Float) -> Float {
        1.0.copysign(v)
    }

    #[inline]
    pub fn encode(f: Float) -> u16 {
        (clamp((f + 1.0) / 2.0, 0.0, 1.0) * 65535.0).round() as u16
    }
}

impl From<Vector3f> for OctahedralVector {
    fn from(v: Vector3f) -> Self {
        Self::new(v)
    }
}

impl From<OctahedralVector> for Vector3f {
    fn from(ov: OctahedralVector) -> Self {
        ov.to_vector()
    }
}

#[repr(C)]
#[derive(Copy, Clone, Debug, Default, PartialEq)]
pub struct Frame {
    pub x: Vector3f,
    pub y: Vector3f,
    pub z: Vector3f,
}

impl Frame {
    pub fn new(x: Vector3f, z: Vector3f) -> Self {
        Self {
            x,
            y: z.cross(x),
            z,
        }
    }

    pub fn from_x(x: Vector3f) -> Self {
        let (y, z) = x.normalize().coordinate_system();
        Self {
            x: x.normalize(),
            y,
            z,
        }
    }

    pub fn from_xz(x: Vector3f, z: Vector3f) -> Self {
        Self {
            x,
            y: z.cross(x),
            z,
        }
    }

    pub fn from_xy(x: Vector3f, y: Vector3f) -> Self {
        Self {
            x,
            y,
            z: x.cross(y),
        }
    }

    pub fn from_y(y: Vector3f) -> Self {
        let (z, x) = y.normalize().coordinate_system();
        Self {
            x,
            y: y.normalize(),
            z,
        }
    }

    pub fn from_z(z: Vector3f) -> Self {
        let (x, y) = z.normalize().coordinate_system();
        Self {
            x,
            y,
            z: z.normalize(),
        }
    }

    pub fn to_local(&self, v: Vector3f) -> Vector3f {
        Vector3f::new(v.dot(self.x), v.dot(self.y), v.dot(self.z))
    }

    pub fn to_local_normal(&self, n: Normal3f) -> Normal3f {
        let n: Vector3f = n.into();
        Normal3f::new(n.dot(self.x), n.dot(self.y), n.dot(self.z))
    }

    pub fn from_local(&self, v: Vector3f) -> Vector3f {
        self.x * v.x() + self.y * v.y() + self.z * v.z()
    }

    pub fn from_local_normal(&self, v: Normal3f) -> Normal3f {
        Normal3f::from(self.x * v.x() + self.y * v.y() + self.z * v.z())
    }
}