contourable 0.8.0

use core::f32;
use std::{
    marker::PhantomData,
    ops::{Mul, SubAssign},
};

use nalgebra::{ComplexField, Point2};
use num_dual::{DualNum, DualNumFloat};

use crate::Aabb;

use super::{Closed, Contour, MinDist};

/// A `struct` representing a circle with a radius and center.
#[derive(Debug, Clone)]
pub struct Circle<D, F>
where
    D: DualNum<F>,
    F: DualNumFloat,
{
    /// The radius of the circle.
    pub radius: D,
    /// The center of the circle.
    pub center: Point2<D>,
    _f: std::marker::PhantomData<F>,
}

impl<D, F> Circle<D, F>
where
    D: DualNum<F>,
    F: DualNumFloat,
{
    pub fn new(radius: D, center: Point2<D>) -> Self {
        Self {
            radius,
            center,
            _f: std::marker::PhantomData,
        }
    }
    pub fn new_centered(radius: D) -> Self {
        Self {
            radius,
            center: Point2::origin(),
            _f: std::marker::PhantomData,
        }
    }
}

impl<C, D, F> Contour<D, F> for Circle<C, F>
where
    C: DualNum<F>,
    D: DualNum<F> + Mul<C, Output = D> + std::ops::Add<C, Output = D> + From<C>,
    F: DualNumFloat,
{
    fn position(&self, angle: &D) -> Point2<D> {
        let x = angle.cos() * self.radius.clone();
        let y = angle.sin() * self.radius.clone();
        Point2::new(x + self.center.x.clone(), y + self.center.y.clone())
    }

    fn s_interval(&self) -> (D, D) {
        (
            D::from_f32(0.0).unwrap(),
            D::from_f32(2.0 * f32::consts::PI).unwrap(),
        )
    }
    fn aabb(&self, _n: u32, _f: PhantomData<D>) -> Aabb<D> {
        Aabb {
            min: Point2::new(
                D::from(self.center.x.clone() - self.radius.clone()),
                D::from(self.center.y.clone() - self.radius.clone()),
            ),
            max: Point2::new(
                D::from(self.center.x.clone() + self.radius.clone()),
                D::from(self.center.y.clone() + self.radius.clone()),
            ),
        }
    }
}

impl<C, D, F> Closed<C, F> for Circle<D, F>
where
    C: DualNum<F>
        + std::ops::Mul<D, Output = C>
        + std::ops::Add<D, Output = C>
        + std::convert::From<D>
        + nalgebra::ComplexField<RealField = C>
        + std::cmp::PartialOrd,
    D: DualNum<F>,
    F: DualNumFloat + SubAssign + ComplexField<RealField = F>,
{
    fn is_inside(&self, point: &nalgebra::Point2<C>, _n: u32) -> bool {
        (point.map(|d| d.re()) - self.center.map(|d| d.re())).norm() < self.radius.re()
    }

    fn min_distance2_vec(&self, point: &nalgebra::Point2<C>, _n: u32) -> MinDist<C> {
        let point_center = &self.center.map(C::from) - point;
        let distance = point_center.norm() - C::from(self.radius.clone());
        let p = point + point_center.normalize() * distance.clone();
        MinDist {
            point: p.clone(),
            point_to_contour: p - point,
            distance_squared: distance.powd(C::from_i8(2).unwrap()),
        }
    }
}

#[cfg(test)]
mod tests {
    use super::*;
    use std::f64::consts::PI;

    use approx::assert_relative_eq;

    mod float {
        use num_dual::Dual64;

        use super::*;
        mod position {
            use super::*;

            #[test]
            fn test_circle_position_0() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let p = circle.position(&0.0);
                assert_relative_eq!(p.x, 3.0 + 2.0);
                assert_relative_eq!(p.y, 4.0 + 0.0);
            }

            #[test]
            fn test_circle_position_pi_over_2() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let p = circle.position(&(PI / 2.0));
                assert_relative_eq!(p.x, 3.0 + 0.0);
                assert_relative_eq!(p.y, 4.0 + 2.0);
            }

            #[test]
            fn test_circle_position_pi() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let p = circle.position(&PI);
                assert_relative_eq!(p.x, 3.0 - 2.0, epsilon = 1e-12);
                assert_relative_eq!(p.y, 4.0 + 0.0, epsilon = 1e-12);
            }

            #[test]
            fn test_circle_position_3_pi_over_2() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let p = circle.position(&(3.0 * PI / 2.0));
                assert_relative_eq!(p.x, 3.0 + 0.0, epsilon = 1e-12);
                assert_relative_eq!(p.y, 4.0 - 2.0, epsilon = 1e-12);
            }

            #[test]
            fn test_circle_position_2_pi() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let p = circle.position(&(1001.0 * 2.0 * PI));
                assert_relative_eq!(p.x, 3.0 + 2.0, epsilon = 1e-12);
                assert_relative_eq!(p.y, 4.0 + 0.0, epsilon = 1e-12);
            }
        }
        mod divide {
            use super::*;

            #[test]
            fn test_circle_divide() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let divided = circle.divide(0.0, 2.0 * PI, 4);
                assert_eq!(divided.len(), 4);
                // assert_relative_eq!(divided[0].radius, 2.0);
                // assert_relative_eq!(divided[0].center.x, 3.0);
                // assert_relative_eq!(divided[0].center.y, 4.0);

                // assert_relative_eq!(divided[1].radius, 2.0);
                // assert_relative_eq!(divided[1].center.x, 3.0);
                // assert_relative_eq!(divided[1].center.y, 6.0);

                // assert_relative_eq!(divided[2].radius, 2.0);
                // assert_relative_eq!(divided[2].center.x, 3.0);
                // assert_relative_eq!(divided[2].center.y, 4.0);

                // assert_relative_eq!(divided[3].radius, 2.0);
                // assert_relative_eq!(divided[3].center.x, 3.0);
                // assert_relative_eq!(divided[3].center.y, 2.0);
            }
        }
        mod aabb {
            use super::*;

            #[test]
            fn test_circle_aabb() {
                let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
                let aabb = circle.aabb(0, PhantomData::<f32>);
                assert_relative_eq!(aabb.min.x, 1.0);
                assert_relative_eq!(aabb.min.y, 2.0);
                assert_relative_eq!(aabb.max.x, 5.0);
                assert_relative_eq!(aabb.max.y, 6.0);
            }
        }

        #[test]
        fn test_new_centered() {
            let circle = Circle::new_centered(2.0);
            assert_relative_eq!(circle.radius, 2.0);
            assert_relative_eq!(circle.center.x, 0.0);
            assert_relative_eq!(circle.center.y, 0.0);
            let aabb = circle.aabb(0, PhantomData::<f32>);
            assert_relative_eq!(aabb.min.x, -2.0);
            assert_relative_eq!(aabb.min.y, -2.0);
            assert_relative_eq!(aabb.max.x, 2.0);
            assert_relative_eq!(aabb.max.y, 2.0);
        }
        #[test]
        fn test_dual_aabb() {
            let circle = Circle::new_centered(Dual64::new(2.0, 1.1));
            let aabb = circle.aabb(0, PhantomData::<Dual64>);
            assert_relative_eq!(aabb.min.x.re, -2.0);
            assert_relative_eq!(aabb.min.y.re, -2.0);
            assert_relative_eq!(aabb.max.x.re, 2.0);
            assert_relative_eq!(aabb.max.y.re, 2.0);

            assert_relative_eq!(aabb.min.x.eps, -1.1);
            assert_relative_eq!(aabb.min.y.eps, -1.1);
            assert_relative_eq!(aabb.max.x.eps, 1.1);
            assert_relative_eq!(aabb.max.y.eps, 1.1);
        }
    }
    mod dual {
        use std::f64::consts::PI;

        use approx::assert_relative_eq;
        use nalgebra::{Vector2, U1, U2};
        use num_dual::DualVec64;
        use num_dual::*;

        use super::super::*;

        fn make_dual_circle() -> Circle<DualVec64<U2>, f64> {
            Circle::new(
                DualVec64::from_re(2.0),
                Point2::new(DualVec64::from_re(3.0), DualVec64::from_re(4.0)),
            )
        }

        #[test]
        fn test_circle_position_0() {
            let circle = make_dual_circle();
            let d = Vector2::new(3.3, 0.0);
            let p = circle.position(&DualVec64::new(0.0, Derivative::new(Some(d))));
            assert_relative_eq!(p.x.re, 3.0 + 2.0);
            let eps_x = p.x.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_x.x, 0.0);
            assert_relative_eq!(eps_x.y, 0.0);

            assert_relative_eq!(p.y.re, 4.0 + 0.0);
            let eps_y = p.y.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_y.x, d.x * circle.radius.re);
            assert_relative_eq!(eps_y.y, 0.0);
        }

        #[test]
        fn test_circle_position_pi_over_2() {
            let circle = make_dual_circle();
            let d = Vector2::new(3.3, 0.0);
            let p = circle.position(&DualVec64::new(PI / 2.0, Derivative::new(Some(d))));
            assert_relative_eq!(p.x.re, 3.0 + 0.0);
            let eps_x = p.x.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_x.x, -d.x * circle.radius.re);
            assert_relative_eq!(eps_x.y, 0.0);

            assert_relative_eq!(p.y.re, 4.0 + circle.radius.re);
            let eps_y = p.y.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_y.x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(eps_y.y, 0.0, epsilon = 1e-12);
        }

        #[test]
        fn test_circle_position_pi() {
            let circle = make_dual_circle();
            let d = Vector2::new(3.3, 0.0);
            let p = circle.position(&DualVec64::new(PI, Derivative::new(Some(d))));
            assert_relative_eq!(p.x.re, 3.0 - circle.radius.re);
            let eps_x = p.x.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_x.x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(eps_x.y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(p.y.re, 4.0 + 0.0, epsilon = 1e-12);
            let eps_y = p.y.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_y.x, -d.x * circle.radius.re, epsilon = 1e-12);
            assert_relative_eq!(eps_y.y, 0.0, epsilon = 1e-12);
        }

        #[test]
        fn test_circle_position_3_pi_over_2() {
            let circle = make_dual_circle();
            let d = Vector2::new(3.3, 0.0);
            let p = circle.position(&DualVec64::new(3.0 * PI / 2.0, Derivative::new(Some(d))));
            assert_relative_eq!(p.x.re, 3.0 + 0.0, epsilon = 1e-12);
            let eps_x = p.x.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_x.x, d.x * circle.radius.re, epsilon = 1e-12);
            assert_relative_eq!(eps_x.y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(p.y.re, 4.0 - circle.radius.re);
            let eps_y = p.y.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_y.x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(eps_y.y, 0.0, epsilon = 1e-12);
        }

        #[test]
        fn test_circle_position_2_pi() {
            let circle = Circle::new(
                DualVec64::from_re(20.0),
                Point2::new(DualVec64::from_re(3.0), DualVec64::from_re(4.0)),
            );
            let d = Vector2::new(3.3, 0.0);
            let p = circle.position(&DualVec64::new(2.0 * PI * 321.0, Derivative::new(Some(d))));
            assert_relative_eq!(p.x.re, 3.0 + 20.0);
            let eps_x = p.x.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_x.x, 0.0, epsilon = 1e-10);
            assert_relative_eq!(eps_x.y, 0.0);

            assert_relative_eq!(p.y.re, 4.0 + 0.0, epsilon = 1e-10);
            let eps_y = p.y.eps.unwrap_generic(U2 {}, U1 {});
            assert_relative_eq!(eps_y.x, d.x * circle.radius.re);
            assert_relative_eq!(eps_y.y, 0.0);
        }
    }

    mod mixed {
        use num_dual::Dual32;

        use super::*;

        #[test]
        fn test_circle_position_0() {
            let circle = Circle::new(2.0, Point2::new(3.0, 4.0));
            let p = circle.position(&Dual32::new(1.1, 2.2));
            assert_relative_eq!(p.x.re, 3.0 + 2.0 * 1.1.cos());
            assert_relative_eq!(p.x.eps, -2.0 * 1.1.sin() * 2.2);
            assert_relative_eq!(p.y.re, 4.0 + 2.0 * 1.1.sin());
        }
    }

    #[test]
    fn test_is_inside() {
        let circle = Circle::new_centered(2.0);
        let point_inside = 1.999 * Point2::new(1.0.cos(), 1.0.sin());
        let point_outside = 2.001 * Point2::new(1.0.cos(), 1.0.sin());
        let n = 10;

        assert!(circle.is_inside(&point_inside, n));
        assert!(!circle.is_inside(&point_outside, n));
    }

    #[test]
    fn test_min_distance_squared() {
        let circle = Circle::new_centered(2.0);
        let point = 10.23 * Point2::new(4.56.cos(), 4.56.sin());
        assert_relative_eq!(
            circle.min_distance2_vec(&point, 0).distance_squared,
            8.23.powi(2)
        );
    }
}