use rin_math::scalar::SubsetOf;
use rin_math::{AsVec, NumCast, Pnt2, RealField, ToPnt, ToVec, Vec2, cast, clamp, convert, distance, distance_squared, iwrap, lerp, map, one, zero};
use std::cmp::Ordering;
use std::ops::{Index, IndexMut, RangeInclusive};
use std::convert::AsRef;
use std::slice;
use std::vec;
use std::iter::{FromIterator, IntoIterator};
use std::fmt::Debug;
#[cfg(feature="serialize")]
use serde_derive::{Serialize, Deserialize};

/// An open or closed collection of vertices that
/// represents a polyline or polygon
///
/// Has methods to do calculations over such geometrical
/// shapes
#[derive(Clone, Debug)]
#[cfg_attr(feature = "serialize", derive(Serialize, Deserialize))]
pub struct Polyline<T: RealField + Debug + 'static = f32>{
    pub(super) points: Vec<Pnt2<T>>,
    pub(super) closed: bool
}

impl<T: RealField + NumCast> Polyline<T>{
    /// creates an empty polyline
    pub fn new() -> Polyline<T>{
        Polyline{points:Vec::new(), closed:false}
    }

    /// creates a new polyline from the vector of points but orders them CCW first
    pub fn new_from_disordered_points(points: Vec<Pnt2<T>>, closed: bool) -> Polyline<T>{
        let mut polyline = Polyline{points: points, closed: closed};
        let centroid = polyline.centroid();
        polyline.points.sort_by(|p1,p2| less(p1,p2,&centroid));
        polyline
    }

    /// returns the area of the polygon, only works if the polyline represents
    /// a polygon
    pub fn area(&self) -> T{
        let mut area: T = zero();
        for i in 0 .. self.len()-1{
            area = area + (self.points[i].x * self.points[i+1].y - self.points[i+1].x * self.points[i].y);
        }
        area = area + (self.points[self.len()-1].x * self.points[0].y - self.points[0].x * self.points[self.len()-1].y);
        area = area * cast(0.5).unwrap();
        area
    }

    /// centroid of the polyline, should work for any collection of points
    /// although it will only make sense if it's a polygon
    pub fn centroid(&self) -> Pnt2<T>{
        let mut centroid = Pnt2::origin();
        if self.points.len()<3{
            return centroid;
        }

        let area = self.area();

        for i in 0 .. self.len()-1{
            let p = self.points[i];
            let next_p = self.points[i+1];
            centroid.x = centroid.x + ((p.x + next_p.x) * (p.x*next_p.y - next_p.x*p.y));
            centroid.y = centroid.y + ((p.y + next_p.y) * (p.x*next_p.y - next_p.x*p.y));
        }

        let p = self.points.last().unwrap();
        let next_p = self.points[0];
        centroid.x = centroid.x + ((p.x + next_p.x) * (p.x*next_p.y - next_p.x*p.y));
        centroid.y = centroid.y + ((p.y + next_p.y) * (p.x*next_p.y - next_p.x*p.y));

        let six: T = cast(6.0).unwrap();
        centroid.x = centroid.x / (six*area);
        centroid.y = centroid.y / (six*area);
        centroid
    }

    /// mark this polyline as being a closed shape, although not necesarily a
    /// polygon. Any rendering or calculation will take into account that the
    /// first and last points are joined
    pub fn close(&mut self){
        self.closed = true;
    }

    /// returns true if the polyline is closed
    pub fn is_closed(&self) -> bool{
        self.closed
    }

    /// returns total number of points
    pub fn len(&self) -> usize{
        self.points.len()
    }

    /// add a new point at the end of the polyline
    pub fn push(&mut self, p: Pnt2<T>){
        self.points.push(p);
    }

    /// removes a new point at the end of the polyline
    pub fn remove(&mut self, idx: usize) -> Pnt2<T> {
        self.points.remove(idx)
    }

    pub fn extend<I: IntoIterator<Item=Pnt2<T>>>(&mut self, vertices: I){
        self.points.extend(vertices)
    }


    /// Returns a smoothed version of the polyline.
	///
	/// `window_size` is the size of the smoothing window. So if
	/// `window_size` is 2, then 2 points from the left, 1 in the center,
	/// and 2 on the right (5 total) will be used for smoothing each point.
    ///
	/// `window_shape` describes whether to use a triangular window (0) or
	/// box window (1) or something in between (for example, .5).
    pub fn smoothed(&self, window_size: usize, window_shape: T) -> Polyline<T>{
        let n = self.points.len();
        let size = clamp(window_size, 0, n);
        let shape = clamp(window_shape, zero(), one());

        let weights = (0..size).map(|i| map(cast(i).unwrap(), zero(), cast(size).unwrap(), one(), shape))
            .collect::<Vec<_>>();

        let mut result = self.clone();
        for i in 0..n {
            let mut sum: T = one();
            for j in 1..size{
                let mut cur = Pnt2::origin();
                let mut left = i as isize - j as isize;
                let mut right = i + j;
                if left < 0 && self.closed{
                    left += n as isize;
                }
                if left >= 0 {
                    cur += self.points[left as usize].as_vec();
                    sum += weights[j];
                }
                if right > n && self.closed {
                    right -= n;
                }
                if right < n{
                    cur += self.points[right].as_vec();
                    sum += weights[j];
                }
                result[i] += cur.as_vec() * weights[j];
            }
            result[i] /= sum;
        }

        result
    }

    pub fn subdivide_linear(&self, resolution: usize) -> Polyline<T>{
        let points = self.points.windows(2).enumerate()
            .flat_map(|(segment, current_next)| (0..resolution).map(move |i|{
                let i_f: T = cast(i).unwrap();
                let t: T = i_f / if segment == self.points.len() - 2{
                    cast(resolution - 1).unwrap()
                }else{
                    cast(resolution).unwrap()
                };
                lerp(current_next[0].to_vec(), current_next[1].to_vec(), t).to_pnt()
            }));

        Polyline{
            points: points.collect(),
            closed: self.closed
        }
    }

    pub fn iter(&self) -> slice::Iter<Pnt2<T>>{
        self.points.iter()
    }

    pub fn iter_mut(&mut self) -> slice::IterMut<Pnt2<T>>{
        self.points.iter_mut()
    }

    pub fn windows(&self, size: usize) -> slice::Windows<Pnt2<T>> {
        self.points.windows(size)
    }

    pub fn first(&self) -> Option<&Pnt2<T>>{
        self.points.first()
    }

    pub fn first_mut(&mut self) -> Option<&mut Pnt2<T>>{
        self.points.first_mut()
    }

    pub fn last(&self) -> Option<&Pnt2<T>>{
        self.points.last()
    }

    pub fn last_mut(&mut self) -> Option<&mut Pnt2<T>>{
        self.points.last_mut()
    }

    pub fn is_empty(&self) -> bool {
        self.points.is_empty()
    }

    /// Returns the point at an index + a normalized pct
    pub fn lerped_point_at(&self, fidx: T) -> Option<Pnt2<T>>{
        let idx1 = fidx.floor();
        let pct = fidx - idx1;
        let idx1 = self.wrap_index(cast(idx1).unwrap())?;
        let idx2 = self.wrap_index(idx1 as isize + 1)?;
        Some(lerp(self[idx1].to_vec(), self[idx2].to_vec(), pct).to_pnt())
    }

    /// Returns the length of the segment at the passed index
    /// or None if such segment doesn't exist
    pub fn segment_length(&self, idx: usize) -> Option<T>{
        let idx2 = self.wrap_index(idx as isize + 1)?;
        let p1 = self.points.get(idx)?;
        let p2 = self.points.get(idx2)?;
        Some(distance(p2, p1))
    }

    /// Returns the square length of the segment at the passed index
    /// or None if such segment doesn't exist
    pub fn segment_length_squared(&self, idx: usize) -> Option<T>{
        let idx2 = self.wrap_index(idx as isize + 1)?;
        let p1 = self.points.get(idx)?;
        let p2 = self.points.get(idx2)?;
        Some(distance_squared(p2, p1))
    }

    /// Removes all points from the polyline
    pub fn clear(&mut self){
        self.points.clear()
    }

    /// Returns an index wrapped around a closed polygon or clamped
    /// on a polyline. Will return None if the polyline is empty
    pub fn wrap_index(&self, idx: isize) -> Option<usize> {
        if self.is_empty() {
            None
        } else if self.is_closed() {
            Some(iwrap(idx, 0, self.points.len() as isize) as usize)
        } else {
            Some(clamp(idx, 0, self.points.len() as isize - 1) as usize)
        }
    }

    /// Finds the next segment which length is different than 0
    /// starting from the passed index and wrapping around on
    /// closed polygons
    pub fn next_non_zero_segment(&self, idx: usize) -> Option<usize>{
        let mut next_idx = self.wrap_index(idx as isize)?;
        loop {
            let segment_length = self.segment_length_squared(next_idx);
            if segment_length.map(|s| s > zero()).unwrap_or(false) {
                return Some(next_idx)
            }else if next_idx < self.len() - 1 {
                next_idx += 1;
            }else if self.is_closed(){
                next_idx = self.wrap_index(next_idx as isize + 1).unwrap();
            }else{
                return None;
            }
            if next_idx != idx {
                return None
            }
        }
    }

    /// Finds the previous segment which length is different than 0
    /// starting from the passed index and wrapping around on
    /// closed polygons
    pub fn prev_non_zero_segment(&self, idx: usize) -> Option<usize>{
        let mut next_idx = self.wrap_index(idx as isize - 1)?;
        while next_idx != idx {
            let segment_length = self.segment_length_squared(next_idx);
            if segment_length.map(|s| s > zero()).unwrap_or(false) {
                return Some(next_idx)
            }else if next_idx > 0 {
                next_idx -= 1;
            }else if self.is_closed(){
                next_idx = self.wrap_index(next_idx as isize - 1).unwrap();
            }else{
                return None;
            }
        }
        None
    }

    pub fn simplify(&mut self, epsilon: T) {
        /// Computes the distance to a line.
        ///
        /// Can be used to calculate the distance to many points.
        /// Pre-calculates a bit of the algorithm for speed.
        #[derive(Copy, Clone, PartialEq, Debug)]
        pub struct LineDistance<T> {
            a: T,
            b: T,
            c: T,
            /// The distance between 2 points
            pub length: T,
        }

        impl<T> LineDistance<T>
        where
            T: RealField + NumCast,
        {
            /// Creates a new LineDistance
            pub fn new(p1: Pnt2<T>, p2: Pnt2<T>) -> Self {
                let a = p2.y - p1.y;
                let b = p2.x - p1.x;
                let c = (p2.x * p1.y) - (p2.y * p1.x);
                let length = (p1 - p2).norm();

                Self { a, b, c, length }
            }

            /// Returns the perpendicular distance from the line to the point.
            ///
            /// None is returned if the line has no length.
            pub fn to(&self, point: &Pnt2<T>) -> Option<T> {
                let Self { a, b, c, length } = self;

                if *length == zero() {
                    None
                } else {
                    // https://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line#Line_defined_by_two_points
                    Some(((*a * point.x) - (*b * point.y) + *c).abs() / *length)
                }
            }
        }

        if self.points.len() < 3 {
            return;
        }

        let mut ranges = Vec::<RangeInclusive<usize>>::new();

        let mut results = Vec::new();
        results.push(0); // We always keep the starting point

        // Set of ranges to work through
        ranges.push(0..=self.points.len() - 1);

        while let Some(range) = ranges.pop() {
            let range_start = *range.start();
            let range_end = *range.end();

            let start = self.points[range_start];
            let end = self.points[range_end];

            // Caches a bit of the calculation to make the loop quicker
            let line = LineDistance::new(start, end);

            let (max_distance, max_index) = self.points[range_start + 1..range_end].iter().enumerate().fold(
                (zero(), 0),
                move |(max_distance, max_index), (index, point)| {
                    let distance = match line.to(point) {
                        Some(distance) => distance,
                        None => {
                            let base = point.x - start.x;
                            let height = point.y - start.y;
                            base.hypot(height)
                        }
                    };

                    if distance > max_distance {
                        // new max distance!
                        // +1 to the index because we start enumerate()ing on the 1st element
                        return (distance, index + 1);
                    }

                    // no new max, pass the previous through
                    (max_distance, max_index)
                },
            );

            // If there is a point outside of epsilon, subdivide the range and try again
            if max_distance > epsilon {
                // We add range_start to max_index because the range needs to be in
                // the space of the whole vector and not the range
                let division_point = range_start + max_index;

                let first_section = range_start..=division_point;
                let second_section = division_point..=range_end;

                // Process the second one first to maintain the stack
                // The order of ranges and results are opposite, hence the awkwardness
                let should_keep_second_half = division_point - range_start > 2;
                if should_keep_second_half {
                    ranges.push(second_section);
                }

                if division_point - range_start > 2 {
                    ranges.push(first_section);
                } else {
                    results.push(division_point);
                }

                if !should_keep_second_half {
                    results.push(range_end);
                }
            } else {
                // Keep the end point for the results
                results.push(range_end);
            }
        }

        for i in (0..self.points.len()).rev() {
            if !results.contains(&i) {
                self.points.remove(i);
            }
        }
    }

    pub fn retain<F>(&mut self, f: F)
    where F: FnMut(&Pnt2<T>) -> bool
    {
        self.points.retain(f)
    }
}

impl<T: NumCast + RealField> Polyline<T>{
    /// Tangent at the point in the passed index if it exists
    pub fn tangent_at(&self, idx: usize) -> Option<Vec2<T>>{
        let idx1 = self.prev_non_zero_segment(idx);
        let idx2 = self.wrap_index(idx as isize)?;
        let idx3 = self.next_non_zero_segment(idx + 1);

        let p2 = &self[idx2];
        match (idx1, idx3) {
            (Some(idx1), Some(idx3)) => {
                let p1 = &self[idx1];
                let p3 = &self[idx3];
                let v1 = (p1 - p2).normalize();
                let v2 = (p3 - p2).normalize();
                let tangent = if (v2 - v1).norm_squared() > zero() {
                    (v2 - v1).normalize()
                }else{
                    -v1
                };
                Some(tangent)
            }
            (Some(idx1), None) => {
                let p1 = &self[idx1];
                Some((p2 - p1).normalize())
            }
            (None, Some(idx3)) => {
                let p3 = &self[idx3];
                Some((p3 - p2).normalize())
            }
            _ => None
        }
    }

    /// Tangent at the lerped point at the passed index + normalized pct
    pub fn lerped_tangent_at(&self, fidx: T) -> Option<Vec2<T>>{
        let idx1 = fidx.floor();
        let pct = fidx - idx1;
        let idx1 = self.wrap_index(cast(idx1).unwrap())?;
        let idx2 = self.wrap_index(idx1 as isize + 1)?;
        Some(lerp(self.tangent_at(idx1)?, self.tangent_at(idx2)?, pct))
    }
}

impl<T: RealField> AsRef<[Pnt2<T>]> for Polyline<T>{
	fn as_ref(&self) -> &[Pnt2<T>]{
		self.points.as_ref()
	}
}

impl<T: RealField> Index<usize> for Polyline<T>{
	type Output = Pnt2<T>;
    fn index(&self, idx: usize) -> &Pnt2<T>{
        self.points.index(idx)
    }
}

impl<T: RealField> IndexMut<usize> for Polyline<T>{
    fn index_mut(&mut self, idx: usize) -> &mut Pnt2<T>{
        self.points.index_mut(idx)
    }
}

impl<T> FromIterator<Pnt2<T>> for Polyline<T>
    where T: RealField
{
    fn from_iter<I>(iter: I) -> Polyline<T>
        where I: IntoIterator<Item=Pnt2<T>>
    {
        Polyline{
            points: iter.into_iter().collect(),
            closed: false,
        }
    }
}

impl<T: RealField> IntoIterator for Polyline<T>{
    type Item = Pnt2<T>;
    type IntoIter = vec::IntoIter<Pnt2<T>>;
    fn into_iter(self) -> vec::IntoIter<Pnt2<T>>{
        self.points.into_iter()
    }
}

impl<T> Into<Vec<Pnt2<T>>> for Polyline<T>
    where T: RealField
{
    fn into(self) -> Vec<Pnt2<T>>{
        self.points
    }
}

fn less<T: RealField>(a: &Pnt2<T>, b: &Pnt2<T>, center: &Pnt2<T>) -> Ordering{
    if a.x - center.x >= zero() && b.x - center.x < zero(){
        return Ordering::Less;
    }
    if a.x - center.x < zero() && b.x - center.x >= zero(){
        return Ordering::Greater;
    }
    if a.x - center.x == zero() && b.x - center.x == zero(){
        if a.y - center.y >= zero() || b.y - center.y >= zero(){
            return if a.y > b.y { Ordering::Less } else { Ordering::Greater };
        }
        return if b.y > a.y  {Ordering::Less} else {Ordering::Greater};
    }

    // compute the cross product of vectors (center -> a) x (center -> b)
    let det = (a.x - center.x) * (b.y - center.y) - (b.x - center.x) * (a.y - center.y);
    if det < zero(){
        return Ordering::Less;
    }
    if det > zero(){
        return Ordering::Greater;
    }

    // points a and b are on the same line from the center
    // check which point is closer to the center
    let d1 = (a.x - center.x) * (a.x - center.x) + (a.y - center.y) * (a.y - center.y);
    let d2 = (b.x - center.x) * (b.x - center.x) + (b.y - center.y) * (b.y - center.y);
    if d1 > d2 { Ordering::Less } else { Ordering::Greater }
}