//! Bicycle car object and related methods.

use crate::geometry::Point;
use crate::geometry::Pose;
use crate::properties::Motion;
use crate::properties::Size;

/// Bicycle car object.
///
///
/// Examples
/// ========
///
/// ```
/// let bc = bcar::BCar::default();
/// assert_eq!(bc, bcar::BCar::new(
///     bcar::Pose::default(),
///     bcar::Size::default(),
///     bcar::Motion::default(),
/// ));
///
/// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
/// assert_eq!(bc.pose, bcar::Pose::new(1.0, 2.0, 3.0));
/// let default_size = bcar::Size::default();
/// assert_eq!(bc.size, default_size);
/// assert_eq!(bc.motion, bcar::Motion::default());
/// ```
#[derive(Copy, Clone, PartialEq, Debug)]
pub struct BCar {
    pub pose: Pose,
    pub size: Size,
    pub motion: Motion,
}

impl BCar {
    pub fn new(pose: Pose, size: Size, motion: Motion) -> BCar {
        BCar {pose, size, motion}
    }

    /// Return new `BCar`.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `x` -- Horizontal coordinate of rear axle center.
    /// - `y` -- Vertical coordinate of rear axle center.
    /// - `h` -- The heading of the car in the interval [-pi, +pi] radians.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
    /// let bc_pose = bc.pose;
    /// assert_eq!(1.0, bc_pose.x);
    /// assert_eq!(2.0, bc_pose.y);
    /// assert_eq!(3.0, bc_pose.h);
    /// ```
    pub fn new_xyh(x: f64, y: f64, h: f64) -> BCar {
        BCar {
            pose: Pose::new(x, y, h),
            size: Size::default(),
            motion: Motion::default(),
        }
    }

    /// Return default `BCar`.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let bc = bcar::BCar::default();
    /// let p = bcar::Pose::default();
    /// let s = bcar::Size::default();
    /// let m = bcar::Motion::default();
    /// assert_eq!(bc, bcar::BCar::new(p, s, m));
    /// ```
    pub fn default() -> BCar {
        BCar {
            pose: Pose::default(),
            size: Size::default(),
            motion: Motion::default(),
        }
    }

    /// Return `BCar` size.
    pub fn size(&self) -> Size {
        self.size
    }

    /// Set `BCar` dimensions.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `s` -- New size of the car.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let size = bcar::Size::new(10.0, 2.0, 3.0, 4.0, 5.0);
    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
    /// bc.set_size(size);
    /// assert_eq!(bc.size, size);
    /// ```
    pub fn set_size(&mut self, s: Size) {
        self.size = s;
    }

    /// Return `BCar` pose.
    pub fn pose(&self) -> Pose {
        self.pose
    }

    /// Set `BCar` pose.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `p` -- New pose of the car.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let pose = bcar::Pose::new(1.0, 4.0, 9.0);
    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
    /// bc.set_pose(pose);
    /// assert_eq!(bc.pose, pose);
    /// ```
    pub fn set_pose(&mut self, p: Pose) {
        self.pose = p
    }

    /// Return `BCar` motion.
    pub fn motion(&self) -> Motion {
        self.motion
    }

    /// Set `BCar` motion.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `m` -- New motion of the car.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let m = bcar::Motion::new(10.0, 0.1);
    /// let mut bc = bcar::BCar::new_xyh(1.0, 2.0, 3.0);
    /// bc.set_motion(m);
    /// assert_eq!(bc.motion, m);
    /// ```
    pub fn set_motion(&mut self, m: Motion) {
        self.motion = m
    }

    /// Return if it's possible to drive to the `pose` trivially.
    ///
    /// Trivially in this context means to reach the `pose` with combination of
    /// line-arc-line segments, where arc is less than pi / 2.
    ///
    /// Arguments
    /// ---------
    ///
    /// - `pose` -- Pose to drive to.
    pub fn drivable(&self, pose: Pose) -> bool {
        let pi = std::f64::consts::PI;
        let pi2 = pi / 2.0;

        let mut a_1 = (pose.y - self.pose.y).atan2(pose.x - self.pose.x);
        a_1 -= self.pose.h;
        while a_1 < -pi { a_1 += 2.0 * pi; }
        while a_1 > pi { a_1 -= 2.0 * pi; }

        let mut h_d = pose.h - self.pose.h;
        while h_d < -pi { h_d += 2.0 * pi; }
        while h_d > pi { h_d -= 2.0 * pi; }

        let mut a_2 = 0.0;
        let mut zb = self.pose;

        if h_d == 0.0 && (a_1 == 0.0 || a_2 == pi || a_2 == -pi) {
            return true;
        } else if 0.0 < a_1 && a_1 <= pi2 { // Q2
            zb.rotate(self.ccl(), h_d);
            // assert zb.h == self.pose.h
            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
                return true;
            }
            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
            while a_2 < -pi { a_2 += 2.0 * pi; }
            while a_2 > pi { a_2 -= 2.0 * pi; }
            if zb.h >= a_2 && a_2 >= self.pose.h {
                return true;
            }
        } else if pi2 < a_1 && a_1 <= pi { // Q3
            zb.rotate(self.ccl(), h_d);
            // assert zb.h == self.pose.h
            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
                return true;
            }
            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
            a_2 -= pi;
            while a_2 < -pi { a_2 += 2.0 * pi; }
            while a_2 > pi { a_2 -= 2.0 * pi; }
            if self.pose.h >= a_2 && a_2 >= zb.h {
                return true;
            }
        } else if 0.0 > a_1 && a_1 >= -pi2 { // Q1
            zb.rotate(self.ccr(), h_d);
            // assert zb.h == self.pose.h
            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
                return true;
            }
            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
            while a_2 < -pi { a_2 += 2.0 * pi; }
            while a_2 > pi { a_2 -= 2.0 * pi; }
            if self.pose.h >= a_2 && a_2 >= zb.h {
                return true;
            }
        } else if -pi2 > a_1 && a_1 >= -pi { // Q4
            zb.rotate(self.ccr(), h_d);
            // assert zb.h == self.pose.h
            if pose.y == zb.y && pose.x == zb.x { // pose on zone border
                return true;
            }
            a_2 = (pose.y - zb.y).atan2(pose.x - zb.x);
            a_2 -= pi;
            while a_2 < -pi { a_2 += 2.0 * pi; }
            while a_2 > pi { a_2 -= 2.0 * pi; }
            if zb.h >= a_2 && a_2 >= self.pose.h {
                return true;
            }
        }

        false
    }

    /// Compute minimum turning radius (MTR).
    pub fn mtr(&self) -> f64 {
        (
            (self.size.curb_to_curb / 2.0).powi(2)
            - self.size.wheelbase.powi(2)
        ).sqrt()
        - self.size.width / 2.0
    }

    /// Compute MTR circle center on the left side of the car.
    pub fn ccl(&self) -> Point {
        let x = self.pose.x;
        let y = self.pose.y;
        let h = self.pose.h;
        Point::new(
            x + self.mtr() * (h + std::f64::consts::PI / 2.0).cos(),
            y + self.mtr() * (h + std::f64::consts::PI / 2.0).sin(),
        )
    }

    /// Compute MTR circle center on the right side of the car.
    pub fn ccr(&self) -> Point {
        let x = self.pose.x;
        let y = self.pose.y;
        let h = self.pose.h;
        Point::new(
            x + self.mtr() * (h - std::f64::consts::PI / 2.0).cos(),
            y + self.mtr() * (h - std::f64::consts::PI / 2.0).sin(),
        )
    }

    /// Return car frame's center front.
    pub fn cf(&self) -> Point {
        let mut x = self.pose.x;
        x += self.size.distance_to_front * self.pose.h.cos();
        let mut y = self.pose.y;
        y += self.size.distance_to_front * self.pose.h.sin();
        Point {x, y}
    }

    /// Return car frame's left front corner.
    pub fn lf(&self) -> Point {
        let pi = std::f64::consts::PI;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
        x += self.size.distance_to_front * self.pose.h.cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
        y += self.size.distance_to_front * self.pose.h.sin();
        Point {x, y}
    }

    /// Return car frame's coordinate for rear axle on left.
    pub fn la(&self) -> Point {
        let pi = std::f64::consts::PI;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
        Point {x, y}
    }

    /// Return car frame's left rear corner.
    pub fn lr(&self) -> Point {
        let pi = std::f64::consts::PI;
        let distance_to_rear = self.size.length - self.size.distance_to_front;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h + pi/2.0).cos();
        x += -distance_to_rear * self.pose.h.cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h + pi/2.0).sin();
        y += -distance_to_rear * self.pose.h.sin();
        Point {x, y}
    }

    /// Return car frame's right rear corner.
    pub fn rr(&self) -> Point {
        let pi = std::f64::consts::PI;
        let distance_to_rear = self.size.length - self.size.distance_to_front;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
        x += -distance_to_rear * self.pose.h.cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
        y += -distance_to_rear * self.pose.h.sin();
        Point {x, y}
    }

    /// Return car frame's coordinate for rear axle on right.
    pub fn ra(&self) -> Point {
        let pi = std::f64::consts::PI;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
        Point {x, y}
    }

    /// Return car frame's right front corner.
    pub fn rf(&self) -> Point {
        let pi = std::f64::consts::PI;
        let mut x = self.pose.x;
        x += self.size.width/2.0 * (self.pose.h - pi/2.0).cos();
        x += self.size.distance_to_front * self.pose.h.cos();
        let mut y = self.pose.y;
        y += self.size.width/2.0 * (self.pose.h - pi/2.0).sin();
        y += self.size.distance_to_front * self.pose.h.sin();
        Point {x, y}
    }
}

impl Iterator for BCar {
    type Item = BCar;

    /// Return `BCar` new state.
    ///
    ///
    /// Examples
    /// ========
    ///
    /// ```
    /// let mut bc = bcar::BCar::default();
    /// bc.set_motion(bcar::Motion::new(1.0, 0.0));
    /// bc.next();
    /// assert_eq!(bc, bcar::BCar::new(
    ///     bcar::Pose::new(1.0, 0.0, 0.0),
    ///     bcar::Size::default(),
    ///     bcar::Motion::new(1.0, 0.0),
    /// ));
    /// ```
    fn next(&mut self) -> Option<Self::Item> {
        self.pose.x += self.motion.speed * self.pose.h.cos();
        self.pose.y += self.motion.speed * self.pose.h.sin();
        self.pose.h += self.motion.speed
            / self.size.wheelbase
            * self.motion.steer.tan();
        Some(*self)
    }
}