use crate::common::points::dist;
use crate::geom2::Ray2;
use crate::{Iso2, Point2, Result, TransformBy, UnitVec2, Vector2};
use parry2d_f64::query::Ray;

/// Compute the intersection parameters between two parameterized lines. Will return None if
/// the two directions are parallel to each other
pub fn intersection_param(
    a0: &Point2,
    ad: &Vector2,
    b0: &Point2,
    bd: &Vector2,
) -> Option<(f64, f64)> {
    let det: f64 = bd.x * ad.y - bd.y * ad.x;
    if det.abs() < 1e-12 {
        return None;
    }

    let dx = b0.x - a0.x;
    let dy = b0.y - a0.y;

    Some(((dy * bd.x - dx * bd.y) / det, (dy * ad.x - dx * ad.y) / det))
}

pub fn intersect_rays(r0: &Ray, r1: &Ray) -> Option<(f64, f64)> {
    intersection_param(&r0.origin, &r0.dir, &r1.origin, &r1.dir)
}

pub trait Line2 {
    fn origin(&self) -> Point2;
    fn dir(&self) -> Vector2;
    fn at(&self, t: f64) -> Point2;

    fn projected_parameter(&self, p: &Point2) -> f64 {
        let n = p - self.origin();
        self.dir().dot(&n) / self.dir().dot(&self.dir())
    }

    fn projected_point(&self, p: &Point2) -> Point2 {
        self.at(self.projected_parameter(p))
    }

    /// Returns the direction of the vector turned in its orthogonal direction by rotating it
    /// -90 degrees, this is typically used as a normal
    fn orthogonal(&self) -> Vector2 {
        Iso2::rotation(-std::f64::consts::PI / 2.0) * self.dir()
    }

    // TODO: Remove if nothing breaks
    // /// Returns a ray that has rotated this entity by 90 degrees
    // fn turned(&self) -> Ray {
    //     Ray::new(self.origin(), self.orthogonal())
    // }
    //
    // fn reversed(&self) -> Ray {
    //     Ray::new(self.origin() + self.dir(), -self.dir())
    // }
}

impl Line2 for Ray2 {
    fn origin(&self) -> Point2 {
        self.origin
    }

    fn dir(&self) -> Vector2 {
        self.dir
    }

    fn at(&self, t: f64) -> Point2 {
        self.point_at(t)
    }
}

#[derive(Debug, Clone, Copy)]
pub struct Segment2 {
    pub a: Point2,
    pub b: Point2,
}

impl Segment2 {
    pub fn try_new(a: Point2, b: Point2) -> Result<Self> {
        if dist(&a, &b) < 1e-12 {
            Err("The two points are too close to each other".into())
        } else {
            Ok(Self { a, b })
        }
    }

    pub fn is_on(&self, p: &Point2) -> bool {
        let ap = p - self.a;
        let bp = p - self.b;
        ap.dot(&bp) <= 0.0
    }

    /// Create a new segment that is shifted by distance `d` in the direction of the segment
    /// normal vector. The normal vector is the direction vector rotated by 90 degrees clockwise,
    /// in keeping with the general convention of a normal vector pointing outwards from a
    /// counter-clockwise wound polyline.
    ///
    /// # Arguments
    ///
    /// * `d`: the distance to shift the segment along its normal vector
    ///
    /// returns: Segment2
    ///
    /// # Examples
    ///
    /// ```
    /// use approx::assert_relative_eq;
    /// use engeom::geom2::{Point2, Segment2};
    /// let a = Point2::new(0.0, 0.0);
    /// let b = Point2::new(1.0, 0.0);
    /// let s = Segment2::try_new(a, b).unwrap();
    ///
    /// let s1 = s.offsetted(1.0);
    ///
    /// assert_relative_eq!(s1.a, Point2::new(0.0, -1.0), epsilon = 1.0e-6);
    /// assert_relative_eq!(s1.b, Point2::new(1.0, -1.0), epsilon = 1.0e-6);
    /// ```
    pub fn offsetted(&self, d: f64) -> Self {
        let n = UnitVec2::new_normalize(self.orthogonal());
        Self {
            a: self.a + n.into_inner() * d,
            b: self.b + n.into_inner() * d,
        }
    }

    /// Create a new segment with the points reversed
    pub fn reversed(&self) -> Self {
        Self {
            a: self.b,
            b: self.a,
        }
    }
}

impl TransformBy<Iso2, Segment2> for Segment2 {
    fn transform_by(&self, t: &Iso2) -> Self {
        Self {
            a: t.transform_point(&self.a),
            b: t.transform_point(&self.b),
        }
    }
}

impl Line2 for Segment2 {
    fn origin(&self) -> Point2 {
        self.a
    }

    fn dir(&self) -> Vector2 {
        self.b - self.a
    }

    fn at(&self, t: f64) -> Point2 {
        self.a + self.dir() * t
    }
}

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

    /// These tests check that the intersection parameter calculation between two parameterized
    /// lines works as expected. The test cases were generated by starting with a random
    /// intersection point and selecting random orthogonal vector and parameter values that rounded
    /// to a single decimal place.
    #[test_case((11.0, 0.7, -4.2, -2.7), (-0.1, -4.7, 1.8, 0.0), (2.0, 1.5))]
    #[test_case((6.2, 3.0, 1.3, -1.9), (-1.3, 8.3, 3.1, -1.7), (-1.0, 2.0))]
    #[test_case((3.6, -3.3, -4.1, 3.2), (9.9, 0.9, 3.0, 2.0), (0.0, -2.1))]
    #[test_case((7.4, -10.1, -2.4, 3.2), (-7.0, -7.7, 4.2, 2.8), (2.5, 2.0))]
    #[test_case((-1.6, 4.3, -0.0, -2.3), (-9.8, 7.1, -4.1, 3.7), (2.0, -2.0))]
    #[test_case((1.7, -0.5, -0.5, -3.3), (3.9, -2.3, 0.5, -1.5), (-1.0, -3.4))]
    #[test_case((4.7, 0.4, -0.6, 0.9), (4.7, 1.3, -3.0, -0.0), (1.0, 0.2))]
    #[test_case((-4.8, -2.0, -1.1, -1.3), (-0.8, -21.0, -0.4, 4.8), (-2.0, 4.5))]
    #[test_case((9.1, 5.7, 4.9, 1.6), (-9.1, -15.5, 2.1, 4.5), (-2.0, 4.0))]
    #[test_case((2.8, 15.7, 0.7, 3.0), (-3.9, -7.1, 1.3, 3.6), (-4.0, 3.0))]
    #[test_case((-5.0, 6.4, -2.6, 0.6), (5.3, 2.3, -2.4, 4.0), (-3.5, 0.5))]
    #[test_case((0.4, -1.9, 2.5, 1.5), (7.6, -19.0, -3.2, 4.2), (-1.6, 3.5))]
    #[test_case((10.6, 5.9, 2.0, 0.7), (-2.0, 7.1, 2.6, -4.7), (-5.0, 1.0))]
    #[test_case((7.3, -0.1, -3.5, 0.9), (14.4, 14.6, -4.4, -3.0), (3.0, 4.0))]
    #[test_case((-5.3, 11.5, -0.8, 4.6), (-15.7, 10.0, -3.1, 2.5), (-2.5, -4.0))]
    #[test_case((4.8, 1.9, 1.0, 1.0), (4.8, 1.9, -1.1, 1.0), (0.0, 0.0))]
    fn inter_param_success(av: (f64, f64, f64, f64), bv: (f64, f64, f64, f64), p: (f64, f64)) {
        let a = Point2::new(av.0, av.1);
        let an = Vector2::new(av.2, av.3);
        let b = Point2::new(bv.0, bv.1);
        let bn = Vector2::new(bv.2, bv.3);

        let (ap, bp) = intersection_param(&a, &an, &b, &bn).unwrap();

        assert_relative_eq!(p.0, ap, epsilon = 1.0e-6);
        assert_relative_eq!(p.1, bp, epsilon = 1.0e-6);
    }

    /// These tests check that the intersection parameter calculation between two parameterized
    /// lines which are parallel returns a None
    #[test_case((-5.0, 2.8, 2.2, 1.8), (-4.2, -0.2, 6.6, 5.4))]
    #[test_case((3.3, 2.5, 4.0, 1.0), (3.2, 0.7, -20.0, -5.0))]
    #[test_case((4.2, -2.3, -0.6, 1.4), (-1.0, 0.5, -2.4, 5.6))]
    #[test_case((-1.1, 2.0, 5.0, 4.0), (4.9, -2.8, 19.5, 15.6))]
    #[test_case((2.4, -3.0, -1.8, -2.6), (0.1, 0.7, 7.2, 10.4))]
    #[test_case((1.2, 2.1, 4.3, -1.0), (-1.4, 3.9, 8.6, -2.0))]
    #[test_case((-4.8, -2.0, -0.1, -0.5), (3.0, -3.6, 0.4, 2.0))]
    #[test_case((-4.4, -0.4, 3.1, 1.1), (3.1, 4.9, -3.1, -1.1))]
    #[test_case((-1.0, -0.1, 0.1, 1.0), (1.2, -2.8, 0.3, 3.0))]
    #[test_case((4.7, -3.7, 4.0, -2.5), (2.5, -0.4, -11.2, 7.0))]
    #[test_case((-1.2, 0.4, 3.9, 0.9), (2.7, -4.6, -11.7, -2.7))]
    #[test_case((-4.8, 4.1, 4.4, -3.0), (4.3, 3.2, -6.6, 4.5))]
    #[test_case((-3.7, 4.7, -1.4, 2.6), (1.0, -4.5, -2.1, 3.9))]
    #[test_case((-0.1, 2.6, 1.4, -0.3), (-0.6, -1.5, -4.2, 0.9))]
    #[test_case((1.8, -2.0, 4.5, -2.0), (0.5, -4.7, 18.0, -8.0))]
    fn inter_parallel_fail(av: (f64, f64, f64, f64), bv: (f64, f64, f64, f64)) {
        let a = Point2::new(av.0, av.1);
        let an = Vector2::new(av.2, av.3);
        let b = Point2::new(bv.0, bv.1);
        let bn = Vector2::new(bv.2, bv.3);

        let result = intersection_param(&a, &an, &b, &bn);

        assert_eq!(None, result);
    }
}
