use crate::{Error, Result};
pub type DVec3 = [f64; 3];
#[derive(Clone, Copy, Debug)]
pub struct Policy {
    pub distance_epsilon: f64,
    pub angular_epsilon: f64,
}
impl Default for Policy {
    fn default() -> Self {
        Self {
            distance_epsilon: 1e-6,
            angular_epsilon: 1e-10,
        }
    }
}
impl Policy {
    pub fn validate(self) -> Result<Self> {
        if !self.distance_epsilon.is_finite()
            || self.distance_epsilon <= 0.
            || !self.angular_epsilon.is_finite()
            || self.angular_epsilon <= 0.
            || self.angular_epsilon >= 1.
        {
            Err(Error::InvalidPolicy)
        } else {
            Ok(self)
        }
    }
}
pub fn add(a: DVec3, b: DVec3) -> DVec3 {
    std::array::from_fn(|i| a[i] + b[i])
}
pub fn sub(a: DVec3, b: DVec3) -> DVec3 {
    std::array::from_fn(|i| a[i] - b[i])
}
pub fn scale(a: DVec3, b: f64) -> DVec3 {
    a.map(|x| x * b)
}
pub fn dot(a: DVec3, b: DVec3) -> f64 {
    a[0] * b[0] + a[1] * b[1] + a[2] * b[2]
}
pub fn cross(a: DVec3, b: DVec3) -> DVec3 {
    [
        a[1] * b[2] - a[2] * b[1],
        a[2] * b[0] - a[0] * b[2],
        a[0] * b[1] - a[1] * b[0],
    ]
}
pub fn finite(v: DVec3) -> Result<DVec3> {
    if v.iter().all(|x| x.is_finite()) {
        Ok(v)
    } else {
        Err(Error::NonFinite)
    }
}
pub fn length(v: DVec3) -> f64 {
    v[0].hypot(v[1]).hypot(v[2])
}
pub fn unit(v: DVec3) -> Result<DVec3> {
    finite(v)?;
    let n = length(v);
    if !n.is_finite() {
        return Err(Error::NonFinite);
    }
    if n == 0. {
        return Err(Error::Degenerate);
    }
    finite(scale(v, 1. / n))
}
pub fn from_f32(v: mb3_dat::Vec3) -> Result<DVec3> {
    finite(v.map(f64::from))
}
pub fn to_f32(v: DVec3) -> Result<mb3_dat::Vec3> {
    finite(v)?;
    let out = v.map(|n| n as f32);
    if out.iter().all(|n| n.is_finite()) {
        Ok(out)
    } else {
        Err(Error::FloatRange)
    }
}
pub fn map_to_native(v: DVec3) -> DVec3 {
    [v[0], v[2], -v[1]]
}
pub fn native_to_map(v: DVec3) -> DVec3 {
    [v[0], -v[2], v[1]]
}
#[derive(Clone, Copy, Debug)]
pub struct Equation {
    normal: DVec3,
    d: f64,
}
impl Equation {
    pub fn new(normal: DVec3, d: f64) -> Result<Self> {
        let n = length(finite(normal)?);
        if n == 0. {
            return Err(Error::Degenerate);
        }
        if !n.is_finite() || !d.is_finite() {
            return Err(Error::NonFinite);
        }
        let out = Self {
            normal: scale(normal, 1. / n),
            d: d / n,
        };
        finite(out.normal)?;
        if !out.d.is_finite() {
            return Err(Error::NonFinite);
        }
        Ok(out)
    }
    pub fn from_basis(p: &mb3_dat::Plane, policy: Policy) -> Result<Self> {
        let policy = policy.validate()?;
        let origin = from_f32(p.p0)?;
        let n = cross(unit(from_f32(p.u)?)?, unit(from_f32(p.v)?)?);
        if length(n) <= policy.angular_epsilon {
            return Err(Error::Degenerate);
        }
        let n = unit(n)?;
        Self::new(n, dot(n, origin))
    }
    pub fn normal(self) -> DVec3 {
        self.normal
    }
    pub fn offset(self) -> f64 {
        self.d
    }
    pub fn distance(self, p: DVec3) -> Result<f64> {
        let d = dot(self.normal, finite(p)?) - self.d;
        if d.is_finite() {
            Ok(d)
        } else {
            Err(Error::NonFinite)
        }
    }
    pub fn intersection(a: Self, b: Self, c: Self, policy: Policy) -> Result<Option<DVec3>> {
        let policy = policy.validate()?;
        let det = dot(a.normal, cross(b.normal, c.normal));
        if det.abs() <= policy.angular_epsilon {
            return Ok(None);
        }
        finite(scale(
            add(
                add(
                    scale(cross(b.normal, c.normal), a.d),
                    scale(cross(c.normal, a.normal), b.d),
                ),
                scale(cross(a.normal, b.normal), c.d),
            ),
            1. / det,
        ))
        .map(Some)
    }
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Aabb {
    pub min: DVec3,
    pub max: DVec3,
}
impl Aabb {
    pub fn new(min: DVec3, max: DVec3) -> Result<Self> {
        finite(min)?;
        finite(max)?;
        if (0..3).any(|i| min[i] > max[i]) {
            return Err(Error::InvalidDimensions);
        }
        Ok(Self { min, max })
    }
    pub fn from_points(points: impl IntoIterator<Item = DVec3>) -> Result<Option<Self>> {
        let mut out: Option<Self> = None;
        for p in points {
            finite(p)?;
            if let Some(b) = &mut out {
                for (i, value) in p.iter().enumerate() {
                    b.min[i] = b.min[i].min(*value);
                    b.max[i] = b.max[i].max(*value);
                }
            } else {
                out = Some(Self { min: p, max: p });
            }
        }
        Ok(out)
    }
    pub fn contains(self, p: DVec3, policy: Policy) -> Result<bool> {
        let e = policy.validate()?.distance_epsilon;
        Self::new(self.min, self.max)?;
        finite(p)?;
        Ok((0..3).all(|i| p[i] >= self.min[i] - e && p[i] <= self.max[i] + e))
    }
}
#[cfg(test)]
mod tests {
    use super::*;
    #[test]
    fn basis_orientation_and_intersections() {
        let p = mb3_dat::Plane {
            p0: [2., 0., 0.],
            u: [0., 1., 0.],
            v: [0., 0., 1.],
            ..Default::default()
        };
        let a = Equation::from_basis(&p, Policy::default()).unwrap();
        assert_eq!(a.normal(), [1., 0., 0.]);
        assert_eq!(a.distance([3., 0., 0.]).unwrap(), 1.);
        let b = Equation::new([0., 1., 0.], 3.).unwrap();
        let c = Equation::new([0., 0., 1.], 4.).unwrap();
        assert_eq!(
            Equation::intersection(a, b, c, Policy::default()).unwrap(),
            Some([2., 3., 4.])
        );
        assert!(
            Equation::intersection(a, a, c, Policy::default())
                .unwrap()
                .is_none()
        );
    }
    #[test]
    fn boxes_axes_and_empty() {
        assert_eq!(Aabb::from_points([]).unwrap(), None);
        let b = Aabb::from_points([[-3., 2., 1.], [4., -1., 8.]])
            .unwrap()
            .unwrap();
        assert_eq!(b.min, [-3., -1., 1.]);
        assert!(b.contains([0., 0., 2.], Policy::default()).unwrap());
        let v = [1., 2., 3.];
        assert_eq!(native_to_map(map_to_native(v)), v);
    }
    #[test]
    fn invalid_and_scaled_bases() {
        assert!(unit([0.; 3]).is_err());
        assert!(Equation::new([f64::NAN, 0., 0.], 1.).is_err());
        assert!(to_f32([f64::MAX, 0., 0.]).is_err());
        assert!(
            Policy {
                distance_epsilon: 0.,
                ..Policy::default()
            }
            .validate()
            .is_err()
        );
        let p = mb3_dat::Plane {
            u: [0., 1e-20, 0.],
            v: [0., 0., 1e20],
            ..Default::default()
        };
        assert_eq!(
            Equation::from_basis(&p, Policy::default())
                .unwrap()
                .normal(),
            [1., 0., 0.]
        );
    }
}