contourable 0.8.0

use super::Contour;
use nalgebra::{self as na, ComplexField};
use num_dual::{DualNum, DualNumFloat};

/// This module contains the implementation of the circle contour.
pub mod circle;
/// This module contains the implementation of the polygon contour.
pub mod polygon;

/// A trait representing a closed contour with operations to calculate its length and divide it into cycles.
pub trait Closed<D, F>: Contour<D, F>
where
    D: DualNum<F> + ComplexField<RealField = D> + PartialOrd,
    F: DualNumFloat,
{
    /// Divides the closed contour into `n` cycles and returns the points of the divided cycles.
    ///
    /// # Arguments
    ///
    /// * `n` - The number of cycles to divide the contour into.
    fn divide_cycle(&self, n: u32) -> Vec<na::Point2<D>> {
        let end = self.length() * (F::from(n - 1).unwrap() / F::from(n).unwrap());
        self.divide(D::zero(), end, n)
    }

    /// Divides the closed contour into `n` segments and returns the points of the divided segments.
    ///
    /// # Arguments
    ///
    /// * `start` - The starting point of the division.
    /// * `end` - The ending point of the division.
    /// * `n` - The number of segments to divide the contour into.
    fn is_inside(&self, point: &na::Point2<D>, n: u32) -> bool {
        // Check if the point is inside the polygon using the ray-casting algorithm
        // by casting a ray to the right from the point and counting intersections with the contour
        // The algorithm works by checking if the y-coordinate of the point is between the y-coordinates
        // of the vertices of the contour. If it is, we check if the x-coordinate of the point is less than
        // the x-coordinate of the intersection of the ray with the edge of the contour. If it is, we toggle
        // the inside variable.

        let mut inside = false;

        self.divide(self.s_interval().0, self.s_interval().1, n + 1)
            .windows(2)
            .for_each(|points| {
                let p0 = &points[0];
                let p1 = &points[1];
                let p0x = p0.x.re();
                let p0y = p0.y.re();
                let p1x = p1.x.re();
                let p1y = p1.y.re();
                let px = point.x.re();
                let py = point.y.re();

                if (p0y > py) == (p1y > py) {
                    return;
                }
                if p0x + (p1x - p0x) * (py - p0y) / (p1y - p0y) - px > F::zero() {
                    inside = !inside;
                }
            });
        inside
    }

    fn min_distance2_vec(&self, point: &na::Point2<D>, n: u32) -> MinDist<D> {
        let x = self
            .divide(self.s_interval().0, self.s_interval().1, n)
            .iter()
            .map(|p| {
                let v = p - point;
                let d2 = v.norm_squared(); // only once here
                (p.clone(), v, d2)
            })
            .min_by(|(_, _, d2a), (_, _, d2b)| d2a.partial_cmp(d2b).unwrap())
            .unwrap();
        MinDist {
            point: x.0,
            point_to_contour: x.1,
            distance_squared: x.2,
        }
    }
}

#[derive(Debug, Clone)]
pub struct MinDist<T>
where
    T: na::Scalar,
{
    pub point: na::Point2<T>,
    pub point_to_contour: na::Vector2<T>,
    pub distance_squared: T,
}

#[cfg(test)]
mod tests {
    use super::*;
    mod float {
        use approx::assert_relative_eq;

        use super::*;

        struct Circle {
            radius: f64,
        }

        impl Contour<f64, f64> for Circle {
            fn position(&self, lap: &f64) -> na::Point2<f64> {
                let angle = lap * 2.0 * std::f64::consts::PI;
                let x = self.radius * angle.cos();
                let y = self.radius * angle.sin();
                na::Point2::new(x, y)
            }

            fn s_interval(&self) -> (f64, f64) {
                (0.0, 1.0)
            }
        }

        impl Closed<f64, f64> for Circle {}

        #[test]
        fn test_circle_divide() {
            let circle = Circle { radius: 2.0 };
            let points = circle.divide(0.0, 1.0, 5);
            assert_relative_eq!(points[0].x, 2.0, epsilon = 1e-12);
            assert_relative_eq!(points[0].y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[1].x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(points[1].y, 2.0, epsilon = 1e-12);

            assert_relative_eq!(points[2].x, -2.0, epsilon = 1e-12);
            assert_relative_eq!(points[2].y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[3].x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(points[3].y, -2.0, epsilon = 1e-12);

            assert_relative_eq!(points[4].x, 2.0, epsilon = 1e-12);
            assert_relative_eq!(points[4].y, 0.0, epsilon = 1e-12);
        }
        #[test]
        fn test_circle_divide_cycle() {
            let circle = Circle { radius: 2.0 };
            let points = circle.divide_cycle(4);
            assert_relative_eq!(points[0].x, 2.0, epsilon = 1e-12);
            assert_relative_eq!(points[0].y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[1].x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(points[1].y, 2.0, epsilon = 1e-12);

            assert_relative_eq!(points[2].x, -2.0, epsilon = 1e-12);
            assert_relative_eq!(points[2].y, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[3].x, 0.0, epsilon = 1e-12);
            assert_relative_eq!(points[3].y, -2.0, epsilon = 1e-12);
        }
    }

    mod dual {
        use approx::assert_relative_eq;
        use nalgebra::allocator::Allocator;
        use nalgebra::{DefaultAllocator, Dim, U1};
        use num_dual::DualVec64;
        use num_dual::*;

        use super::*;

        struct Circle<D>
        where
            DefaultAllocator: Allocator<D>,
            D: Dim,
        {
            radius: DualVec64<D>,
        }

        impl Contour<DualVec64<U1>, f64> for Circle<U1> {
            fn position(&self, lap: &DualVec64<U1>) -> na::Point2<DualVec64<U1>> {
                let angle = *lap * 2.0 * std::f64::consts::PI;
                let x = angle.cos() * self.radius;
                let y = angle.sin() * self.radius;
                na::Point2::new(x, y)
            }

            fn s_interval(&self) -> (DualVec64<U1>, DualVec64<U1>) {
                (0.0.into(), 1.0.into())
            }
        }

        impl Closed<DualVec64<U1>, f64> for Circle<U1> {}

        use nalgebra::Vector1;

        #[test]
        fn test_circle_divide_cycle() {
            let circle = Circle {
                radius: DualVec::new(2.0, Derivative::new(Some(Vector1::new(5.0)))),
            };
            let points = circle.divide_cycle(4);

            assert_relative_eq!(points[0].x.re, 2.0, epsilon = 1e-12);
            let eps_x = points[0].x.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_x.x, 5.0, epsilon = 1e-12);

            assert_relative_eq!(points[0].y.re, 0.0, epsilon = 1e-12);
            let eps_y = points[0].y.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_y.x, 0.0);

            assert_relative_eq!(points[1].x.re, 0.0, epsilon = 1e-12);
            let eps_x = points[1].x.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_x.x, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[1].y.re, 2.0, epsilon = 1e-12);
            let eps_y = points[1].y.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_y.x, 5.0, epsilon = 1e-12);

            assert_relative_eq!(points[2].x.re, -2.0, epsilon = 1e-12);
            let eps_x = points[2].x.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_x.x, -5.0, epsilon = 1e-12);

            assert_relative_eq!(points[2].y.re, 0.0, epsilon = 1e-12);
            let eps_y = points[2].y.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_y.x, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[3].x.re, 0.0, epsilon = 1e-12);
            let eps_x = points[3].x.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_x.x, 0.0, epsilon = 1e-12);

            assert_relative_eq!(points[3].y.re, -2.0, epsilon = 1e-12);
            let eps_y = points[3].y.eps.unwrap_generic(U1 {}, U1 {});
            assert_relative_eq!(eps_y.x, -5.0, epsilon = 1e-12);
        }
    }

    mod is_inside {
        use std::f64::consts::PI;

        use super::*;
        use nalgebra::Point2;

        struct Circle {
            radius: f64,
        }
        impl Contour<f64, f64> for Circle {
            fn position(&self, s: &f64) -> na::Point2<f64> {
                let angle = s / self.radius;
                let x = self.radius * angle.cos();
                let y = self.radius * angle.sin();
                na::Point2::new(x, y)
            }

            fn s_interval(&self) -> (f64, f64) {
                (0.0, 2.0 * PI * self.radius)
            }
        }
        impl Closed<f64, f64> for Circle {}

        #[test]
        fn test_is_inside() {
            let circle = Circle { radius: 2.0 };
            let point_inside = Point2::new(1.0, 1.0);
            let point_outside = Point2::new(3.0, 3.0);
            let n = 10;

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