//! Basic geometry for bicycle car.

/// Basic point geometry.
///
///
/// Examples
/// ========
///
/// ```
/// let point = bcar::Point::new(1.0, 2.0);
/// assert_eq!(1.0, point.x);
/// assert_eq!(2.0, point.y);
/// ```
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct Point {
    pub x: f64,
    pub y: f64,
}

impl Point {
    pub fn new(x: f64, y: f64) -> Point {
        Point {x, y}
    }

    /// Return default point
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let point = bcar::Point::default();
    /// assert_eq!(point, bcar::Point::new(0.0, 0.0));
    /// ```
    pub fn default() -> Point {
        Point {x: 0.0, y: 0.0}
    }

    /// Return if `self` is on right side of line.
    ///
    /// Examples
    /// ========
    ///
    ///     let p = bcar::Point::new(1.0, -2.0);
    ///     let s = bcar::Point::new(1.0, 1.0);
    ///     let e = bcar::Point::new(-1.0, -1.0);
    ///     assert!(!p.on_right_side_of(s, e));
    ///     assert!(p.on_right_side_of(e, s));
    pub fn on_right_side_of(&self, s: Point, e: Point) -> bool {
        let x = self.x;
        let y = self.y;
        ((x - s.x) * (e.y - s.y) - (y - s.y) * (e.x - s.x)).signum() >= 0.0
    }
}

/// Basic pose geometry.
///
///
/// Examples
/// ========
///
/// ```
/// let pose = bcar::Pose::new(1.0, 2.0, 3.0);
/// assert_eq!(1.0, pose.x);
/// assert_eq!(2.0, pose.y);
/// assert_eq!(3.0, pose.h);
/// ```
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct Pose {
    pub x: f64,
    pub y: f64,
    pub h: f64,
}

impl Pose {
    pub fn new(x: f64, y: f64, h: f64) -> Pose {
        let mut h = h;
        while h < -std::f64::consts::PI {
            h += 2.0 * std::f64::consts::PI;
        }
        while h > std::f64::consts::PI {
            h -= 2.0 * std::f64::consts::PI;
        }
        Pose {x, y, h}
    }

    /// Return default pose.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let pose = bcar::Pose::default();
    /// assert_eq!(pose, bcar::Pose::new(0.0, 0.0, 0.0));
    /// ```
    pub fn default() -> Pose {
        Pose {x: 0.0, y: 0.0, h: 0.0}
    }

    /// Return `Point` from pose.
    pub fn point(&self) -> Point {
        Point::new(self.x, self.y)
    }

    /// Rotate `self` around `p` by `a`.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `p` -- Point to rotate `self` around.
    /// - `a` -- Angle to rotate `self` by.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let mut pose = bcar::Pose::new(0.0, 0.0, 0.0);
    /// pose.rotate(bcar::Point::new(-1.0, 0.0), std::f64::consts::PI);
    /// assert!(pose.x - -2.0 < 0.00001);
    /// assert!(pose.y - 0.0 < 0.00001);
    /// assert!(pose.h - std::f64::consts::PI < 0.00001);
    /// ```
    pub fn rotate(&mut self, p: Point, a: f64) {
        let cx = p.x;
        let cy = p.y;
        let mut px = self.x;
        let mut py = self.y;
        px -= cx;
        py -= cy;
        let nx = px * a.cos() - py * a.sin();
        let ny = px * a.sin() + py * a.cos();
        self.x = nx + cx;
        self.y = ny + cy;
        self.h += a;
    }
}

/// Basic pose range geometry.
///
/// Examples
/// ========
///
///     let pr = bcar::PoseRange::default();
///     assert_eq!(pr.x, 0.0);
///     assert_eq!(pr.y, 0.0);
///     assert_eq!(pr.b, 0.0);
///     assert_eq!(pr.e, 0.0);
///
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct PoseRange {
    pub x: f64,
    pub y: f64,
    pub b: f64,
    pub e: f64,
}

impl PoseRange {
    pub fn default() -> PoseRange {
        PoseRange {x: 0.0, y: 0.0, b: 0.0, e: 0.0}
    }

    pub fn from_poses(p1: Pose, p2: Pose) -> PoseRange {
        let b = p1.h.min(p2.h);
        let e = p1.h.max(p2.h);
        if (p1.x - p2.x).abs() > 1e-10 || (p1.y - p2.y).abs() > 1e-10 {
            let bp1 = Point::new(p1.x - 9.9*p1.h.cos(), p1.y - 9.9*p1.h.sin());
            let bp2 = Point::new(p2.x - 9.9*p2.h.cos(), p2.y - 9.9*p2.h.sin());
            let fp1 = Point::new(p1.x + 9.9*p1.h.cos(), p1.y + 9.9*p1.h.sin());
            let fp2 = Point::new(p2.x + 9.9*p2.h.cos(), p2.y + 9.9*p2.h.sin());
            let i = match intersect_line_line(bp1, fp1, bp2, fp2) {
                Some(i) => i,
                None => panic!("No intersection, no range!"),
            };
            return PoseRange {x: i.x, y: i.y, b: b, e: e};
        }
        PoseRange {x: p1.x, y: p1.y, b: b, e: e}
    }
}

/// Return intersection point of two lines if exists.
///
/// See https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
///
/// Examples
/// ========
///
///     match bcar::intersect_line_line(
///         bcar::Point::new(1.0, 1.0), bcar::Point::new(3.0, 3.0),
///         bcar::Point::new(1.0, 3.0), bcar::Point::new(3.0, 1.0),
///     ) {
///         Some(p) => { assert!(p == bcar::Point::new(2.0, 2.0)); },
///         None => { assert!(false); }
///     }
///     match bcar::intersect_line_line(
///         bcar::Point::new(1.0, 1.0), bcar::Point::new(1.0, 3.0),
///         bcar::Point::new(3.0, 3.0), bcar::Point::new(3.0, 1.0),
///     ) {
///         Some(_p) => { assert!(false); },
///         None => { assert!(true); }
///     }
///
pub fn intersect_line_line(
    p1: Point, p2: Point, // line 1
    p3: Point, p4: Point, // line 2
) -> Option<Point> {
    let deno = (p1.x - p2.x) * (p3.y - p4.y) - (p1.y - p2.y) * (p3.x - p4.x);
    if deno == 0.0 {
        return None;
    }
    let mut t = (p1.x - p3.x) * (p3.y - p4.y) - (p1.y - p3.y) * (p3.x - p4.x);
    t /= deno;
    let mut u = (p1.x - p2.x) * (p1.y - p3.y) - (p1.y - p2.y) * (p1.x - p3.x);
    u *= -1.0;
    u /= deno;
    if t < 0.0 || t > 1.0 || u < 0.0 || u > 1.0 {
        return None;
    }
    Some(Point::new(p1.x + t * (p2.x - p1.x),  p1.y + t * (p2.y - p1.y)))
}

/// Return intersection points of circle and line if exist.
///
/// See https://mathworld.wolfram.com/Circle-LineIntersection.html
///
/// Examples
/// ========
///
///     match bcar::intersect_circle_line(
///         bcar::Point::new(0.0, 0.0), 0.5,
///         bcar::Point::new(-1.0, 0.0), bcar::Point::new(1.0, 0.0),
///     ) {
///         Some((p1, p2)) => {
///             assert!((p1.x - -0.5).abs() < 1e-5);
///             assert!((p1.y - 0.0).abs() < 1e-5);
///             assert!((p2.x - 0.5).abs() < 1e-5);
///             assert!((p2.y - 0.0).abs() < 1e-5);
///         },
///         None => { assert!(false); }
///     }
///     match bcar::intersect_circle_line(
///         bcar::Point::new(1.0, 1.0), 0.5,
///         bcar::Point::new(-1.0, 0.0), bcar::Point::new(1.0, 0.0),
///     ) {
///         Some(_t) => { assert!(false); },
///         None => { assert!(true); }
///     }
///
pub fn intersect_circle_line(
    c: Point, r: f64,
    p1: Point, p2: Point,
) -> Option<(Point, Point)> {
    let x2 = p2.x - c.x;
    let x1 = p1.x - c.x;
    let y2 = p2.y - c.y;
    let mut y1 = p1.y - c.y;
    if y1 == y2 {
        y1 += 0.00001;
    }
    let dx = x2 - x1;
    let dy = y2 - y1;
    let dr = (dx*dx + dy*dy).sqrt();
    let d = x1*y2 - x2*y1;
    let r2 = r*r;
    let dr2 = dr*dr;
    let d2 = d*d;
    if r2 * dr2 - d2 < 0.0 {
        return None;
    }
    // intersection coordinates
    let ix1 = (d*dy + dy.signum()*dx*(r2 * dr2 - d2).sqrt()) / (dr2) + c.x;
    let ix2 = (d*dy - dy.signum()*dx*(r2 * dr2 - d2).sqrt()) / (dr2) + c.x;
    let iy1 = (-d*dx + dy.abs()*(r2 * dr2 - d2).sqrt()) / (dr2) + c.y;
    let iy2 = (-d*dx - dy.abs()*(r2 * dr2 - d2).sqrt()) / (dr2) + c.y;
    Some((Point::new(ix1, iy1), Point::new(ix2, iy2)))
}

/// Return the angle between three points
///
/// Examples
/// ========
///
///     let a = bcar::angle_between(
///         bcar::Point::new(1.0, 1.0),
///         bcar::Point::new(0.0, 0.0),
///         bcar::Point::new(1.0, -1.0),
///     );
///     assert!((a - std::f64::consts::PI/2.0).abs() < 0.00001);
///
pub fn angle_between(p1: Point, p2: Point, p3: Point) -> f64 {
    let d1x = p2.x - p1.x;
    let d1y = p2.y - p1.y;
    let d2x = p3.x - p2.x;
    let d2y = p3.y - p2.y;

    let dot = d1x*d2x + d1y*d2y;
    let d1 = (d1x*d1x + d1y*d1y).sqrt();
    let d2 = (d2x*d2x + d2y*d2y).sqrt();

    let delta = (dot / (d1 * d2)).acos();
    delta.min(std::f64::consts::PI - delta)
}

/// Return euclidean distance between two points.
///
/// Examples
/// ========
///
///     let p1 = bcar::Point::new(0.0, 0.0);
///     let p2 = bcar::Point::new(3.0, 4.0);
///     assert!((bcar::edist(p1, p2) - 5.0).abs() < 1e-10);
pub fn edist(p1: Point, p2: Point) -> f64 {
    ((p2.y - p1.y).powi(2) + (p2.x - p1.x).powi(2)).sqrt()
}

/// Return angle of point closer to the start.
///
/// TODO ut
pub fn angle_between_closer(
    common_start: Point,
    common_middle: Point,
    first_end: Point,
    second_end: Point,
) -> f64 {
    if edist(common_start, first_end) < edist(common_start, second_end) {
        return angle_between(common_start, common_middle, first_end);
    } else {
        return angle_between(common_start, common_middle, second_end);
    }
}