contourable 0.8.0

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

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

use crate::Aabb;

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

/// A polygon represented by a list of vertices.
#[derive(Debug, Clone)]
pub struct Polygon<D, F>
where
    D: DualNum<F>,
    F: DualNumFloat,
{
    /// A vector of points representing the vertices of the polygon.
    /// Each point is of type `Point2<F>`, where `F` is a generic type parameter
    /// representing the numeric type of the coordinates (e.g., `f32` or `f64`).
    vertices: Vec<Point2<D>>,

    /// A Computed (for optimization) vector containing distances of type `F` from vertices to the next.
    distance: Vec<D>,

    _x: std::marker::PhantomData<F>,
}

impl<D, F> Polygon<D, F>
where
    D: DualNum<F> + ComplexField<RealField = D>,
    F: DualNumFloat,
{
    /// Creates a new [`Polygon<F>`].
    /// Returns a reference to the vector of distances between consecutive vertices.
    ///
    /// # Returns
    ///
    /// A slice of distances of type `F`.
    #[must_use]
    pub fn new(vertices: Vec<Point2<D>>) -> Self {
        let distance = circular_cumulative_distance(&vertices);
        Self {
            vertices,
            distance,
            _x: std::marker::PhantomData,
        }
    }

    pub fn new_centered_rectangle(width: D, height: D) -> Self {
        let vertices = vec![
            Point2::new(
                -width.clone() / D::from_f32(2.0).unwrap(),
                -height.clone() / D::from_f32(2.0).unwrap(),
            ),
            Point2::new(
                width.clone() / D::from_f32(2.0).unwrap(),
                -height.clone() / D::from_f32(2.0).unwrap(),
            ),
            Point2::new(
                width.clone() / D::from_f32(2.0).unwrap(),
                height.clone() / D::from_f32(2.0).unwrap(),
            ),
            Point2::new(
                -width.clone() / D::from_f32(2.0).unwrap(),
                height.clone() / D::from_f32(2.0).unwrap(),
            ),
        ];
        Self::new(vertices)
    }

    /// Returns a reference to the vector of cumulative distances.
    #[must_use]
    pub fn distance(&self) -> &[D] {
        &self.distance
    }
}

impl<P, C, F> Contour<C, F> for Polygon<P, F>
where
    P: DualNum<F> + PartialOrd,
    C: DualNum<F> + From<P> + PartialOrd,
    F: DualNumFloat,
    for<'a> &'a C: Sub<&'a C, Output = C>,
    for<'a> &'a P: Sub<&'a P, Output = P>,
{
    fn position(&self, s: &C) -> nalgebra::Point2<C> {
        let total_length = self.distance.last().unwrap();
        let laps = (s.re() / total_length.re()).floor();
        let m = s.clone() - (laps * total_length.re());

        self.distance
            .binary_search_by(|x| x.re().partial_cmp(&m.re()).unwrap())
            .map_or_else(
                |i1| {
                    let i0 = i1 - 1;
                    let j1 = i1 % self.vertices.len();
                    let p0 = &self.vertices[i0];
                    let p1 = &self.vertices[j1];
                    let s = m - C::from(self.distance[i0].clone());
                    p0.map(C::from)
                        + ((p1 - p0) / (&self.distance[i1] - &self.distance[i0])).map(C::from) * (s)
                },
                |i| self.vertices[i].map(C::from),
            )
    }

    fn s_interval(&self) -> (C, C) {
        (C::zero(), C::from(self.distance.last().unwrap().clone()))
    }

    fn aabb(&self, _n: u32, _f: PhantomData<C>) -> Aabb<C> {
        let dmin = C::from_f32(f32::MIN).unwrap();
        let dmax = C::from_f32(f32::MAX).unwrap();
        let mut min = Point2::new(dmax.clone(), dmax);
        let mut max = Point2::new(dmin.clone(), dmin);
        for vertex in &self.vertices {
            if C::from(vertex.x.re()) < min.x {
                min.x = C::from(vertex.x.clone())
            };
            if vertex.y.re() < min.y.re() {
                min.y = C::from(vertex.y.clone());
            };
            if vertex.x.re() > max.x.re() {
                max.x = C::from(vertex.x.clone())
            };
            if vertex.y.re() > max.y.re() {
                max.y = C::from(vertex.y.clone())
            };
        }

        Aabb { min, max }
    }
}

impl<C, P, F> Closed<C, F> for Polygon<P, F>
where
    C: DualNum<F>
        + std::convert::From<P>
        + PartialOrd
        + nalgebra::ComplexField<RealField = C>
        + RealField,
    P: DualNum<F> + PartialOrd,
    F: DualNumFloat,
    for<'a> &'a C: Sub<&'a C, Output = C>,
    for<'a> &'a P: Sub<&'a P, Output = P>,
{
    fn is_inside(&self, point: &Point2<C>, _n: u32) -> bool {
        let mut inside = false;
        let horz_cross = |point: &Point2<C>, p0: &Point2<P>, p1: &Point2<P>| {
            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 false;
            }
            p0x + (p1x - p0x) * (py - p0y) / (p1y - p0y) - px > F::zero()
        };
        self.vertices.windows(2).for_each(|points| {
            if horz_cross(point, &points[0], &points[1]) {
                inside = !inside;
            }
        });
        // Check the last point to the first point
        if horz_cross(
            point,
            self.vertices.last().unwrap(),
            self.vertices.first().unwrap(),
        ) {
            inside = !inside;
        }
        inside
    }

    fn min_distance2_vec(&self, point: &nalgebra::Point2<C>, _n: u32) -> MinDist<C> {
        let min_dist_to_segment = |p0: &Point2<C>, p1: &Point2<C>, point: &Point2<C>| {
            let p0p1 = p1 - p0;
            let p0p = point - p0;
            let pmin = p0
                + p0p1.clone() * (p0p.dot(&p0p1) / p0p1.norm_squared()).clamp(C::zero(), C::one());
            let dist = (point - pmin.clone()).norm_squared();
            MinDist {
                point: pmin.clone(),
                point_to_contour: pmin - point,
                distance_squared: dist,
            }
        };

        let all_but_last = self
            .vertices
            .windows(2)
            .map(|p| {
                min_dist_to_segment(
                    &p[0].map(<C as std::convert::From<P>>::from),
                    &p[1].map(<C as std::convert::From<P>>::from),
                    point,
                )
            })
            .min_by(|a, b| a.distance_squared.partial_cmp(&b.distance_squared).unwrap())
            .unwrap();
        let last = min_dist_to_segment(
            &self
                .vertices
                .last()
                .unwrap()
                .map(<C as std::convert::From<P>>::from),
            &self
                .vertices
                .first()
                .unwrap()
                .map(<C as std::convert::From<P>>::from),
            point,
        );
        match all_but_last.distance_squared < last.distance_squared {
            true => all_but_last,
            false => last,
        }
    }
}

/// Utility functions for the polygon module.
pub mod utils;

#[cfg(test)]
mod tests {
    use num_dual::Dual32;

    use super::*;
    mod compute_position {
        use approx::assert_relative_eq;
        use nalgebra as na;
        use num_dual::{Dual, Dual64};
        use num_traits::{One, Zero};

        use super::super::*;

        fn assert_point_rel_eq<T: na::RealField>(a: &Point2<T>, b: &Point2<T>) {
            assert_relative_eq!(a.x, b.x);
            assert_relative_eq!(a.y, b.y);
        }

        #[test]
        fn test_position_square() {
            let vertices = vec![
                Point2::new(0.0, 0.0),
                Point2::new(1.0, 0.0),
                Point2::new(1.0, 1.0),
                Point2::new(0.0, 1.0),
            ];
            let polygon = Polygon::new(vertices);
            assert_point_rel_eq(&polygon.position(&-8.5), &Point2::new(0.0, 0.5));
            assert_point_rel_eq(&polygon.position(&-0.5), &Point2::new(0.0, 0.5));
            assert_point_rel_eq(&polygon.position(&0.0), &Point2::new(0.0, 0.0));
            assert_point_rel_eq(&polygon.position(&0.5), &Point2::new(0.5, 0.0));
            assert_point_rel_eq(&polygon.position(&1.0), &Point2::new(1.0, 0.0));
            assert_point_rel_eq(&polygon.position(&1.5), &Point2::new(1.0, 0.5));
            assert_point_rel_eq(&polygon.position(&2.0), &Point2::new(1.0, 1.0));
            assert_point_rel_eq(&polygon.position(&2.5), &Point2::new(0.5, 1.0));
            assert_point_rel_eq(&polygon.position(&3.0), &Point2::new(0.0, 1.0));
            assert_point_rel_eq(&polygon.position(&3.5), &Point2::new(0.0, 0.5));
            assert_point_rel_eq(&polygon.position(&4.0), &Point2::new(0.0, 0.0));
            assert_point_rel_eq(&polygon.position(&4.5), &Point2::new(0.5, 0.0));
            assert_point_rel_eq(&polygon.position(&12.5), &Point2::new(0.5, 0.0));
        }

        #[test]
        fn test_position_triangle() {
            let vertices = vec![
                Point2::new(0.0, 0.0),
                Point2::new(3.0, 0.0),
                Point2::new(3.0, 4.0),
            ];
            let polygon = Polygon::new(vertices);
            let s = 5.0;
            let position = polygon.position(&s);
            let expected_position = Point2::new(3.0, 2.0);
            assert_relative_eq!(position.x, expected_position.x);
            assert_relative_eq!(position.y, expected_position.y);
        }

        #[test]
        fn test_position_line() {
            let vertices = vec![Point2::new(0.0, 0.0), Point2::new(1.0, 0.0)];
            let polygon = Polygon::new(vertices);
            let s = 1.5;
            let position = polygon.position(&s);
            let expected_position = Point2::new(0.5, 0.0);
            assert_relative_eq!(position.x, expected_position.x);
            assert_relative_eq!(position.y, expected_position.y);
        }

        #[test]
        fn test_position_dual() {
            let vertices = vec![
                Point2::new(Dual64::zero(), Dual64::zero()),
                Point2::new(Dual::one(), Dual::zero()),
                Point2::new(Dual::zero(), Dual::new(2.0, 0.0)),
            ];
            let polygon = Polygon::new(vertices);
            let s = Dual::new(1.5, 10.0);
            let position = polygon.position(&s);
            let alpha = 2.0.atan2(-1.0);
            let expected_position = Point2::new(1.0 + 0.5 * alpha.cos(), 0.0 + 0.5 * alpha.sin());
            assert_relative_eq!(position.x.re(), expected_position.x);
            assert_relative_eq!(position.y.re(), expected_position.y);
            assert_relative_eq!(position.x.eps, 10.0 * alpha.cos());
            assert_relative_eq!(position.y.eps, 10.0 * alpha.sin());
            // assert_relative_eq!(position.y.d, 0.0);
        }
    }

    #[test]
    fn test_dist() {
        let vertices = vec![
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 0.0),
            Point2::new(1.0, 1.0),
            Point2::new(0.0, 1.0),
        ];
        let polygon = Polygon::new(vertices);
        assert_eq!(polygon.distance(), &[0.0, 1.0, 2.0, 3.0, 4.0]);
    }
    #[test]
    fn test_length() {
        let vertices = vec![
            Point2::new(0.0, 0.0),
            Point2::new(1.0, 0.0),
            Point2::new(1.0, 1.0),
            Point2::new(0.0, 1.0),
        ];
        let polygon = Polygon::new(vertices);
        assert_eq!(
            <Polygon<f64, f64> as Contour<f64, f64>>::length(&polygon),
            4.0
        );
    }

    #[test]
    fn test_aabb() {
        let vertices = vec![
            Point2::new(1.1, 2.2),
            Point2::new(-10.0, -8.0),
            Point2::new(22.0, 1.0),
            Point2::new(2.0, 4.0),
        ];
        let polygon = Polygon::new(vertices);
        polygon.position(&Dual32::from_re(0.0));
        let aabb = polygon.aabb(0, PhantomData::<Dual32>);
        assert_eq!(aabb.min.x.re, -10.0);
        assert_eq!(aabb.min.y.re, -8.0);
        assert_eq!(aabb.max.x.re, 22.0);
        assert_eq!(aabb.max.y.re, 4.0);
    }

    #[test]
    fn test_centered_rectangle() {
        let width = 4.0;
        let height = 2.0;
        let polygon = Polygon::new_centered_rectangle(width, height);
        assert_eq!(polygon.vertices.len(), 4);
        assert_eq!(polygon.vertices[0], Point2::new(-2.0, -1.0));
        assert_eq!(polygon.vertices[1], Point2::new(2.0, -1.0));
        assert_eq!(polygon.vertices[2], Point2::new(2.0, 1.0));
        assert_eq!(polygon.vertices[3], Point2::new(-2.0, 1.0));
    }

    #[test]
    fn test_is_inside() {
        let polygon = Polygon::new(vec![
            Point2::new(0.0, 0.0),
            Point2::new(4.0, 0.0),
            Point2::new(4.0, 3.0),
            Point2::new(0.0, 3.0),
        ]);
        let point_inside = Point2::new(2.0, 2.9999);
        let point_outside = Point2::new(2.0, 3.0001);
        let n = 0;

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

    #[test]
    fn test_min_distance() {
        let polygon = Polygon::new(vec![
            Point2::new(0.0, 0.0),
            Point2::new(4.0, 0.0),
            Point2::new(4.0, 3.0),
            Point2::new(0.0, 3.0),
        ]);
        let point = Point2::new(3.123, 5.0);
        assert_eq!(polygon.min_distance2_vec(&point, 0).distance_squared, 4.0);

        let point = Point2::new(-2.0, 1.21);
        assert_eq!(polygon.min_distance2_vec(&point, 0).distance_squared, 4.0);
    }
}