//! # Machine
//!
//! Models a **3-Axis Computer Numerical Control** (CNC) milling machine.
//!
//! The machine runs on a sequence of parsed [`CodeBlock`]s (represented as [`Parser`]),
//! generated by the [`parser`](crate::parser) module.

use crate::{
    lexer::*,
    parser::{Plane, *},
};
use std::{
    f64::consts::PI,
    ops::{Add, Neg, Sub},
};

/// Starting point of a machine with all axes at '0'.
pub const HOME_POS: Point = Point::new(0.0, 0.0, 0.0);

/// Represents **max** and **min** allowable values for a generic type `T`.
struct Limits<T> {
    max: T,
    min: T,
}

/// Maximum and Minimum values supported for [`Machine::max_travels`] for this module.
/// The fields of these [`Point`]s will be treated in [`Unit::default`] system.
///
/// All axes will be forced to have **positive** travel limits.
const TRAVEL_LIMITS: Limits<Point> = Limits {
    max: Point::new(1500.0, 1000.0, 1000.0),
    min: Point::new(150.0, 100.0, 100.0),
};

/// Maximum and Minimum **absolute** ratios to maintain between axes lengths.
///
/// The order of specific axes is as follows:
/// - X : Y
/// - X : Z
/// - Y : Z
const RATIO_LIMITS: Limits<Point> = Limits {
    max: Point::new(2.0, 2.0, 1.5),
    min: Point::new(1.0, 1.0, 0.5),
};

/// Possible motion types to move the machine with.
///
/// Represents **Group 1** G-Codes.
#[derive(Copy, Clone, Default, Debug, PartialEq)]
pub enum Motion {
    #[default]
    Rapid,
    Feed,
    Arc(CircularDirection),
}

/// Possible Arc types.
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum ArcType {
    Major,
    Minor,
}

/// Represents a straight line between two [`Point`]s.
#[derive(Debug, Clone, Copy)]
pub struct Line {
    pub start: Point,
    pub end: Point,
}

/// Represents an arc between two [`Point`]s on a single [`Plane`], with a specific [`CircularDirection`].
#[derive(Debug, Clone, Copy)]
pub struct Arc {
    pub start: Point,
    pub end: Point,
    pub dir: CircularDirection,
    pub center: PlanarPoint,
    pub radius: Float,
    /// signed angle made by arc in radians, positive being counterclockwise
    pub sweep: Float,
    pub arc_type: ArcType,
}

// method values must be converted to machine units before passing it into this function
impl Arc {
    /// Accepts:
    /// Tires to construct an [`Arc`] using:
    /// - Start & end [`Point`]s.
    /// - [`CircleMethod`]: Used to calculate the center and radius of the arc.
    /// - [`Plane`]: All calculations will be done in the said plane.
    /// - [`CircularDirection`]: Direction to use to get from `start` to `end` point.
    ///
    /// This function takes care of all the geometric math for circular interpolation paths.
    ///
    /// # Assumption
    /// This function has no way of knowing the [`Unit`]s of all the [`Point`]s and
    /// [`CircleMethod`]. Therefore, any conversions must be done **BEFORE** calling this function
    /// and all the values are assumed to be in the same [`Unit`].
    ///
    /// # Errors:
    /// - [`MachineError::PointsIntersect`] -- Both provided points have the same coordinates.
    /// - [`MachineError::CenterOffPlane`] -- The relative center provided does not lie on the
    ///   requested plane.
    /// - [`MachineError::InvalidCircle`] -- The provided or calculated radius does not satisfy both
    ///   the points.
    ///
    /// ## Reference
    /// [Math Stack Exchange](https://math.stackexchange.com/questions/1781438/finding-the-center-of-a-circle-given-two-points-and-a-radius-algebraically)
    pub fn build(
        start: Point,
        end: Point,
        method: CircleMethod,
        plane: Plane,
        dir: CircularDirection,
    ) -> Result<Self, MachineError> {
        if start == end {
            return Err(MachineError::PointsIntersect);
        }

        match method {
            CircleMethod::RelativePoint(ref rel_center) => {
                // all fields cannot be None or Some.
                if rel_center.are_none() || rel_center.are_some() {
                    unreachable!("Invalid Relative Center passed parser validation. Logic Error!");
                }

                // absolute center on the same plane as both the points
                let center = match plane {
                    Plane::XY if rel_center.z().is_some() => {
                        return Err(MachineError::CenterOffPlane);
                    }

                    Plane::XZ if rel_center.y().is_some() => {
                        return Err(MachineError::CenterOffPlane);
                    }

                    Plane::YZ if rel_center.x().is_some() => {
                        return Err(MachineError::CenterOffPlane);
                    }

                    Plane::XY => PlanarPoint::new(
                        plane,
                        rel_center.x().map_or(start.x(), |x| start.x() + x),
                        rel_center.y().map_or(start.y(), |y| start.y() + y),
                    ),

                    Plane::XZ => PlanarPoint::new(
                        plane,
                        rel_center.x().map_or(start.x(), |x| start.x() + x),
                        rel_center.z().map_or(start.z(), |z| start.z() + z),
                    ),

                    Plane::YZ => PlanarPoint::new(
                        plane,
                        rel_center.y().map_or(start.y(), |y| start.y() + y),
                        rel_center.z().map_or(start.z(), |z| start.z() + z),
                    ),
                };

                let start_planar = PlanarPoint::from_point(start, plane);
                let end_planar = PlanarPoint::from_point(end, plane);

                // distance on the specified plane, not in 3D
                let radius = start_planar.dist(&center);

                // direction towards start pos from center
                let to_start = start_planar - center;
                // direction towards end pos from center
                let to_end = end_planar - center;

                let cross = to_start.first() * to_end.second() - to_start.second() * to_end.first();
                let dot = to_start.first() * to_end.first() + to_start.second() * to_end.second();

                let cw_major = cross > 0.0 || (cross == 0.0 && dot < 0.0);
                let ccw_minor = cw_major;

                let arc_type = match dir {
                    CircularDirection::Clockwise => {
                        if cw_major {
                            ArcType::Major
                        } else {
                            ArcType::Minor
                        }
                    }
                    CircularDirection::CounterClockwise => {
                        if ccw_minor {
                            ArcType::Minor
                        } else {
                            ArcType::Major
                        }
                    }
                };

                // in this method the distance between the points cannot be greater than the diameter

                // new point not at the same distance.
                // tolerance should be a little lenient
                if (end_planar.dist(&center) - radius).abs() > 0.01 {
                    Err(MachineError::InvalidCircle(method))
                } else {
                    Ok(Self {
                        start,
                        end,
                        dir,
                        center,
                        radius,
                        sweep: Arc::sweep(to_start, to_end, arc_type, dir, radius),
                        arc_type,
                    })
                }
            }

            CircleMethod::FixedRadius(radius) => {
                assert!(
                    radius.abs() > 1e-10,
                    "Zero radius passed parser validation. Logic Error!."
                );

                // if radius is negative, make it positive and make the function choose the major arc
                // otherwise choose minor arc on positive R value
                let (radius, arc_type) = if radius.signum() == 1.0 {
                    (radius, ArcType::Minor)
                } else if radius.signum() == -1.0 {
                    (radius.neg(), ArcType::Major)
                } else {
                    panic!("Provided number is Not a Number (NaN).");
                };

                let start_planar = PlanarPoint::from_point(start, plane);
                let end_planar = PlanarPoint::from_point(end, plane);

                let dist = start_planar.dist(&end_planar);

                if dist > 2.0 * radius.abs() {
                    return Err(MachineError::InvalidCircle(method)); // distance more than diameter
                }

                let midpoint = PlanarPoint::new(
                    plane,
                    start_planar.first().midpoint(end_planar.first()),
                    start_planar.second().midpoint(end_planar.second()),
                );

                // how much is the midpoint off from the current position.
                // plane is irrelevant in this var.
                let delta = PlanarPoint::new(
                    plane,
                    midpoint.first() - start_planar.first(),
                    midpoint.second() - start_planar.second(),
                );

                let dist = dist / 2.0; // make dist as distance to midpoint only

                // length of line joining midpoint and center, the perpendicular bisector
                let bisector = (radius.powi(2) - dist.powi(2)).sqrt();

                // if points are on the diameter make midpoint is the center
                let center = if (2.0 * radius - dist).abs() < 0.01 {
                    midpoint
                } else {
                    match (arc_type, dir) {
                        (ArcType::Minor, CircularDirection::Clockwise)
                        | (ArcType::Major, CircularDirection::CounterClockwise) => {
                            PlanarPoint::new(
                                plane,
                                midpoint.first() + (bisector * delta.second()) / dist,
                                midpoint.second() - (bisector * delta.first()) / dist,
                            )
                        }

                        (ArcType::Minor, CircularDirection::CounterClockwise)
                        | (ArcType::Major, CircularDirection::Clockwise) => PlanarPoint::new(
                            plane,
                            midpoint.first() - (bisector * delta.second()) / dist,
                            midpoint.second() + (bisector * delta.first()) / dist,
                        ),
                    }
                };

                // direction towards start pos from center
                let to_start = start_planar - center;
                // direction towards end pos from center
                let to_end = end_planar - center;

                Ok(Self {
                    start,
                    end,
                    dir,
                    center,
                    radius,
                    sweep: Arc::sweep(to_start, to_end, arc_type, dir, radius),
                    arc_type,
                })
            }
        }
    }

    /// Calculates angle between `to_start` and `to_end` in **radians**,
    /// depending on the requested [`CircularDirection`] and [`ArcType`].
    ///
    /// `to_start` and `to_end` must be **relative distances** from the [`Arc`] center,
    /// on the **same [`Plane`]**.
    ///
    /// The returned sweep will be **negative** for [`CircularDirection::Clockwise`],
    /// and **positive** for [`CircularDirection::CounterClockwise`].
    ///
    /// ## Reference
    /// [Stackoverflow]( https://stackoverflow.com/questions/2994669/how-do-i-calculate-arc-angle-between-two-points-on-a-circle)
    fn sweep(
        to_start: PlanarPoint,
        to_end: PlanarPoint,
        arc_type: ArcType,
        dir: CircularDirection,
        radius: Float,
    ) -> Float {
        assert_eq!(to_start.plane(), to_end.plane());

        let minor_sweep = ((to_start.first() * to_end.first()
            + to_start.second() * to_end.second())
            / radius.powi(2))
        .clamp(-1.0, 1.0)
        .acos(); // in radians

        // total sweep can only be 2pie, 360 degs
        match (arc_type, dir) {
            (ArcType::Major, CircularDirection::Clockwise) => 0.0 - (2.0 * PI - minor_sweep),
            (ArcType::Major, CircularDirection::CounterClockwise) => 2.0 * PI - minor_sweep,
            (ArcType::Minor, CircularDirection::Clockwise) => 0.0 - minor_sweep,
            (ArcType::Minor, CircularDirection::CounterClockwise) => minor_sweep,
        }
    }
}

/// Represents a summary of movement based on [`Motion`] type.
#[derive(Debug, Clone, Copy)]
pub enum MotionSummary {
    Rapid(Line),
    Feed(Line),
    Arc(Arc),
}

// [`Plane`] represents **Group 2** G-Codes.

/// Possible ways to interpret axis interpolation commands.
///
/// Represents **Group 3** G-Codes.
#[derive(Default, Clone, Debug)]
pub enum Positioning {
    #[default]
    Absolute,
    Incremental,
}

/// Possible ways to interpret given feed rate.
///
/// Represents **Group 5** G-Codes.
#[derive(Copy, Clone, Default, Debug, PartialEq)]
pub enum FeedMode {
    #[default]
    PerMinute,
    PerRev,
}

/// Possible unit standards for measurable values.
///
/// Represents **Group 6** G-Codes.
#[derive(Copy, Clone, Default, Debug, PartialEq)]
pub enum Unit {
    Imperial,
    #[default]
    Metric,
}

/// Represents an offset `address` with a `direction`.
///
/// Used in representing **Group 7** & **Group 8** G-Codes.
#[derive(Debug, Clone, PartialEq)]
pub struct Offset {
    address: Int,
    dir: Direction,
}

/// Possible directions for [`Offset`]s.
///
/// Used in representing **Group 7** & **Group 8** G-Codes.
#[derive(Clone, Debug, PartialEq)]
pub enum Direction {
    Left,
    Right,
    Up,
    Down,
}

/// Possible levels to return to in a canned cycle.
///
/// Represents **Group 10** G-Codes.
#[derive(Copy, Clone, Default, Debug, PartialEq)]
pub enum ReturnLevel {
    #[default]
    Initial,
    Retract,
}

/// Possible *spindle rotation* & *circular interpolation* directions.
#[derive(Copy, Clone, Default, Debug, PartialEq)]
pub enum CircularDirection {
    #[default]
    Clockwise,
    CounterClockwise,
}

impl Offset {
    /// Generates a new [`Offset`] to be used with a `D` address.
    ///
    /// The `dir` provided must be
    /// [`Direction::Left`] or [`Direction::Right`] for diameter offsets.
    ///
    /// # Errors
    /// Returns [`MachineError::OffsetDirection`] if the provided `dir` is invalid,
    /// indicating failure.
    pub fn build_d(address: Int, dir: Direction) -> Result<Self, MachineError> {
        if dir == Direction::Up || dir == Direction::Down {
            Err(MachineError::OffsetDirection(dir))
        } else {
            Ok(Self { address, dir })
        }
    }

    /// Generates a new [`Offset`] to be used with a `H` address.
    ///
    /// The `dir` provided must be
    /// [`Direction::Up`] or [`Direction::Down`] for height offsets.
    ///
    /// # Errors
    /// Returns [`MachineError::OffsetDirection`] if provided `dir` is invalid,
    /// indicating failure.
    pub fn build_h(address: Int, dir: Direction) -> Result<Self, MachineError> {
        if dir == Direction::Left || dir == Direction::Right {
            Err(MachineError::OffsetDirection(dir))
        } else {
            Ok(Self { address, dir })
        }
    }
}

/// Represents the current state of a [`Machine`](crate::machine).
#[derive(Debug, Clone)]
pub struct Machine {
    /// Interpret state values in the selected `units`.
    ///
    /// The [`Code`] the machine executes **MAY OR MAY NOT** be in the same units.
    /// All stored values must be in this unit standard.
    units: Unit,

    /// Interpret [`Code`]s in the selected `code_units`.
    code_units: Unit,

    /// **Positive extreme values** for each linear axis.
    ///
    /// The other extreme would naturally be `HOME_POS`.
    /// The values in this struct will be assumed to be in [`units`](Self::units).
    max_travels: Point,

    /// Workpiece offset, used to change relative zero point with `G54`.
    work_offset: Point,

    /// Current position of each axis.
    pos: Point,

    /// Current tool in the spindle.
    tool: Int,

    /// Optionally preload next tool, to be changed on *tool change (`M6`)* command.
    next_tool: Option<Int>,

    /// Commanded RPM for the spindle.
    speed: Option<Int>,
    spindle_on: bool,
    spindle_dir: CircularDirection,

    /// Active [`Motion`] type to use for moves.
    motion: Motion,

    /// Commanded cutting feed rate.
    feed: Option<Float>,

    /// Way in which to interpret the given feed value.
    feed_mode: FeedMode,

    coolant: bool,

    /// Plane to execute the [`Code`] on.
    plane: Plane,

    /// Interpret axis values absolutly or relatively.
    positioning: Positioning,

    /// Selected offset for tool diameter with a `D` code.
    d_offset: Option<Offset>,

    /// Selected offset for tool height with a `H` code.
    h_offset: Option<Offset>,

    /// Return level in canned cycles.
    ret_lvl: ReturnLevel,
}

impl Machine {
    /// Constructs a new instance of [`Machine`] with all axes at [`HOME_POS`].
    ///
    /// Accepts a [`Point`], indicating extremes of the Machine to construct,
    /// and the measuring system [`Unit`] to use for machine moves.
    ///
    /// The *G-Code* that the machine executes need not be in the same [`Unit`]s.
    ///
    /// Returns a new `Machine` instance on success or [`MachineError`] on failure.
    pub fn build(max_travels: Point, units: Unit) -> Result<Self, MachineError> {
        let ratio = max_travels.ratio();

        if max_travels.any_negative() {
            Err(MachineError::NegativeTravel)
        } else if max_travels.over_abs(&TRAVEL_LIMITS.max) {
            Err(MachineError::TravelsTooLong)
        } else if max_travels.under_abs(&TRAVEL_LIMITS.min) {
            Err(MachineError::TravelsTooShort)
        } else if ratio.over_abs(&RATIO_LIMITS.max) || ratio.under_abs(&RATIO_LIMITS.min) {
            Err(MachineError::WrongRatio)
        } else {
            Ok(Machine {
                units,
                code_units: units,
                max_travels,
                work_offset: Point::default(),
                pos: Point::default(),
                tool: 0,
                next_tool: None,
                speed: None,
                spindle_on: false,
                spindle_dir: CircularDirection::default(),
                motion: Motion::default(),
                feed: None,
                feed_mode: FeedMode::default(),
                coolant: false,
                plane: Plane::default(),
                positioning: Positioning::default(),
                d_offset: None,
                h_offset: None,
                ret_lvl: ReturnLevel::default(),
            })
        }
    }

    //
    // ########## PUBLIC GETTERS ##########
    //

    /// Return *maximum* travel for each axis of the [`Machine`].
    pub fn max_travels(&self) -> &Point {
        &self.max_travels
    }

    /// Returns `G54` work coordinate system.
    ///
    /// This offset is applied each time the machine has to move in [`Positioning::Absolute`] mode.
    pub fn work_offset(&self) -> &Point {
        &self.work_offset
    }

    /// Returns the current position of the machine.
    pub fn pos(&self) -> &Point {
        &self.pos
    }

    /// Returns the current tool in the machine.
    pub fn tool(&self) -> Int {
        self.tool
    }

    /// Returns the current active [`Motion`] variant of the machine.
    pub fn motion(&self) -> &Motion {
        &self.motion
    }

    /// Returns the current active [`Plane`] of the machine.
    pub fn plane(&self) -> &Plane {
        &self.plane
    }

    /// Returns the current active [`Unit`] of the machine.
    /// This **CAN BE** different from the `code units` of the GCode.
    pub fn units(&self) -> &Unit {
        &self.units
    }

    /// Returns the current active [`Unit`] for the code running on the machine.
    pub fn code_units(&self) -> &Unit {
        &self.code_units
    }

    /// Returns the current active [`Positioning`] mode of the machine.
    pub fn positioning(&self) -> &Positioning {
        &self.positioning
    }

    /// Returns the current commanded `Feedrate` for the machine.
    pub fn feed(&self) -> &Option<Float> {
        &self.feed
    }

    /// Returns the current active [`FeedMode`] of the machine.
    pub fn feed_mode(&self) -> &FeedMode {
        &self.feed_mode
    }

    //
    // ########## PUBLIC SETTERS ##########
    //

    /// Interpret [`Code`] values in a new [`Unit`] standard.
    pub fn set_code_units(&mut self, code_units: Unit) {
        self.code_units = code_units;

        // change any existing feed rate to new units
        if let Some(feed) = self.feed {
            self.set_feed(feed);
        }
    }

    /// Set `G54` work coordinate system.
    ///
    /// **This is not verified to be inside the machine travels**.
    pub fn set_work_offset(&mut self, work_offset: Point) {
        self.work_offset = work_offset
    }

    /// Preload the *next tool* to use.
    pub fn set_next_tool(&mut self, next_tool: Int) {
        self.next_tool = Some(next_tool)
    }

    /// Set *spindle RPMs* to use.
    pub fn set_speed(&mut self, speed: Int) {
        self.speed = Some(speed)
    }

    /// Turn spindle off.
    pub fn spindle_off(&mut self) {
        self.spindle_on = false
    }

    /// Set *feedrate* to use.
    /// Performes any unit conversion if necessary.
    pub fn set_feed(&mut self, mut feed: Float) {
        if self.units == Unit::Imperial && self.code_units == Unit::Metric {
            feed /= 25.4;
        } else if self.units == Unit::Metric && self.code_units == Unit::Imperial {
            feed *= 25.4;
        }

        self.feed = Some(feed)
    }

    /// Set `coolant` on or off.
    pub fn set_coolant(&mut self, coolant: bool) {
        self.coolant = coolant
    }

    /// Set active plane.
    pub fn set_plane(&mut self, plane: Plane) {
        self.plane = plane
    }

    /// Toggle between `absolute` or `relative` positioning.
    pub fn set_positioning(&mut self, positioning: Positioning) {
        self.positioning = positioning
    }

    /// Cancel any diameter offset, if active.
    pub fn cancel_dia_offset(&mut self) {
        self.d_offset = None
    }

    /// Cancel any height offset, if active.
    pub fn cancel_height_offset(&mut self) {
        self.h_offset = None
    }

    /// Toggle between motion types.
    pub fn set_motion(&mut self, motion: Motion) {
        self.motion = motion
    }

    /// Cancel any canned cycles.
    ///
    /// For future use, for adding drilling cycles.
    pub fn cancel_canned(&mut self) {}

    /// Toggle between `feed per minute` or `feed per rev` modes.
    pub fn set_feed_mode(&mut self, mode: FeedMode) {
        self.feed_mode = mode
    }

    /// Toggle between `inital` or `retract` return planes.
    pub fn set_return_level(&mut self, level: ReturnLevel) {
        self.ret_lvl = level
    }

    /// Sets the following machine variables to their defaults:
    /// - **Spindle** -- Off
    /// - **Plane** -- XY
    /// - **Coolant** -- Off
    /// - **Feed Mode** -- Feed Per Minute
    ///
    /// Also moves the machine to [`HOME_POS`].
    pub fn reset(&mut self) {
        self.spindle_off();
        self.set_plane(Plane::default());
        self.set_coolant(false);
        self.set_feed_mode(FeedMode::default());
    }

    //
    // ########## PUBLIC SETTERS, CAN FAIL ##########
    //

    /// Turn spindle on, if not already on.
    ///
    /// Accepts [`CircularDirection`] as *spindle direction* and optionally accepts
    /// new *spindle speed*, otherwise uses speed from previous blocks, if present.
    ///
    /// Returns [`MachineError::NoSpindleSpeed`] if no spindle speed was commanded before and
    /// `speed` provided is also `None`.
    pub fn spindle_on(
        &mut self,
        dir: CircularDirection,
        speed: Option<Int>,
    ) -> Result<(), MachineError> {
        self.speed = speed.or(self.speed);
        self.spindle_dir = dir;

        match self.speed {
            Some(_) => {
                self.spindle_on = true;
                Ok(())
            }
            None => Err(MachineError::NoSpindleSpeed),
        }
    }

    /// Changes current `tool` to:
    /// - `new_tool`, if the value is `Some`.
    /// - `next_tool`, if a tool was preloaded.
    ///
    /// `next_tool` will be always be set to `None` after the tool change.
    ///
    /// Returns [`MachineError::NoTool`] if no `next_tool` was commanded before and
    /// `new_tool` provided is also `None`.
    pub fn tool_change(&mut self, new_tool: Option<Int>) -> Result<(), MachineError> {
        self.next_tool = new_tool.or(self.next_tool);

        match self.next_tool.take() {
            Some(t) => {
                self.tool = t;
                Ok(())
            }
            None => Err(MachineError::NoTool),
        }
    }

    /// Sets a `Some` value for `d_offset`.
    /// The `dir` provided must be [`Direction::Left`] or [`Direction::Right`] for diameter
    /// offsets.
    ///
    /// Returns [`MachineError::OffsetDirection`] if provided `dir` is invalid,
    /// indicating failure.
    pub fn set_dia_offset(&mut self, address: Int, dir: Direction) -> Result<(), MachineError> {
        self.d_offset = Some(Offset::build_d(address, dir)?);
        Ok(())
    }

    /// Sets a `Some` value for `h_offset`.
    /// The `dir` provided must be [`Direction::Up`] or [`Direction::Down`] for height
    /// offsets.
    ///
    /// Returns [`MachineError::OffsetDirection`] if provided `dir` is invalid,
    /// indicating failure.
    pub fn set_height_offset(&mut self, address: Int, dir: Direction) -> Result<(), MachineError> {
        self.h_offset = Some(Offset::build_h(address, dir)?);
        Ok(())
    }

    //
    // ########## PUBLIC INTERPOLATION METHODS ##########
    //

    /// Moves machine after checking for [`Positioning`] setting of the machine in **Rapid** rate.
    ///
    /// Respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions are performed as necessary.
    ///
    /// Returns a [`MotionSummary::Rapid`] on success,
    /// or [`MachineError::Overtravel`] if any axis exceeds `max_travels` or `HOME_POS` during
    /// the move, indicating failure.
    pub fn rapid_move(&mut self, pos: PartialPoint) -> Result<MotionSummary, MachineError> {
        self.set_motion(Motion::Rapid);
        self.move_machine(pos, None, None)
    }

    /// Moves machine after checking for [`Positioning`] setting of the machine in **Feed** rate.
    ///
    /// Respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions (including for **feedrate**) are performed as necessary.
    ///
    /// Returns a [`MotionSummary::Feed`] on success, or [`MachineError`] on failure.
    ///
    /// # Errors:
    /// - [`MachineError::Overtravel`] -- Atleast one axis exceeds `max_travels` or `HOME_POS`.
    /// - [`MachineError::NoFeed`] -- No feed was commanded in previous blocks and is found `None`.
    pub fn feed_move(
        &mut self,
        pos: PartialPoint,
        feed: Option<Float>,
    ) -> Result<MotionSummary, MachineError> {
        self.set_motion(Motion::Feed);
        self.move_machine(pos, feed, None)
    }

    /// Moves machine in a **circular path**,
    /// after checking for [`Positioning`] setting of the machine in **Feed** rate.
    ///
    /// Respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions (including for **feedrate**) are performed as necessary.
    ///
    /// Returns a [`MotionSummary::Arc`] on success, or [`MachineError`] on failure.
    ///
    /// # Errors:
    /// - [`MachineError::Overtravel`] -- Atleast one axis exceeds `max_travels` or `HOME_POS`.
    /// - [`MachineError::NoFeed`] -- No feed was commanded in previous blocks and is found `None`.
    pub fn arc_move(
        &mut self,
        pos: PartialPoint,
        method: CircleMethod,
        dir: CircularDirection,
        feed: Option<Float>,
    ) -> Result<MotionSummary, MachineError> {
        self.set_motion(Motion::Arc(dir));
        self.move_machine(pos, feed, Some(method))
    }

    /// Moves `Some` variants of `pos` to the value inside.
    ///
    /// This function differs from [`move_machine`](Self::move_machine) in the way that,
    /// it does not consider any `work_offset` & `positioning` setting of the [`Machine`] and just
    /// moves the machine axes to desired location.
    ///
    /// This function also respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions are performed as necessary.
    ///
    /// Returns [`MotionSummary::Rapid`] on success,
    /// or [`MachineError::Overtravel`] if any axis exceeds `max_travels` or `HOME_POS`,
    /// indicating failure.
    pub fn move_machine_pos(
        &mut self,
        mut pos: PartialPoint,
    ) -> Result<MotionSummary, MachineError> {
        self.to_machine_units(&mut pos);
        let org_pos = *self.pos();

        let new_pos = Point::new(
            pos.x().unwrap_or(self.pos().x()),
            pos.y().unwrap_or(self.pos().y()),
            pos.z().unwrap_or(self.pos().z()),
        );

        self.set_pos(new_pos)?;
        Ok(MotionSummary::Rapid(Line {
            start: org_pos,
            end: new_pos,
        }))
    }

    /// Moves machine using [`motion`](Machine::motion) type,
    /// after checking for [`Positioning`] setting of the machine.
    ///
    /// Respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions are performed as necessary.
    ///
    /// Since this is a general function for each [`Motion`] variant,
    /// therefore it optionally excepts `feed` & `circle method`,
    /// for [`Motion::Feed`] & [`Motion::Arc`] respectively.
    ///
    /// Returns a [`MotionSummary`] on success, or [`MachineError`] on failure.
    pub fn move_machine(
        &mut self,
        pos: PartialPoint,
        feed: Option<Float>,
        method: Option<CircleMethod>,
    ) -> Result<MotionSummary, MachineError> {
        if let Some(f) = feed {
            self.set_feed(f); // takes care of any unit conversion
        }

        let org_pos = *self.pos();

        match self.motion {
            Motion::Rapid => {
                let new_pos = self.new_pos(pos);
                self.set_pos(new_pos)?;
                Ok(MotionSummary::Rapid(Line {
                    start: org_pos,
                    end: new_pos,
                }))
            }

            Motion::Feed => {
                match self.feed {
                    Some(_) => {
                        // move only when feed is detected.
                        let new_pos = self.new_pos(pos);
                        self.set_pos(new_pos)?;
                        Ok(MotionSummary::Feed(Line {
                            start: org_pos,
                            end: new_pos,
                        }))
                    }
                    None => Err(MachineError::NoFeed),
                }
            }

            Motion::Arc(dir) => {
                if self.feed.is_none() {
                    return Err(MachineError::NoFeed);
                }

                let method = if let Some(circle) = method {
                    circle
                } else {
                    return Err(MachineError::NoCircleMethod);
                };

                let start_pos = &org_pos;
                // retain None variants as start_pos
                let end_pos = self.new_pos(pos);

                // convert `method` units
                let method = match method {
                    CircleMethod::RelativePoint(mut rel_center) => {
                        self.to_machine_units(&mut rel_center);
                        CircleMethod::RelativePoint(rel_center)
                    }
                    CircleMethod::FixedRadius(mut rad) => {
                        if self.units == Unit::Imperial && self.code_units == Unit::Metric {
                            rad /= 25.4;
                        } else if self.units == Unit::Metric && self.code_units == Unit::Imperial {
                            rad *= 25.4;
                        };

                        CircleMethod::FixedRadius(rad)
                    }
                };

                let arc = Arc::build(*start_pos, end_pos, method, self.plane, dir)?;

                // TODO verify if the arc overtravels anywhere between the points

                // now we know the circle is possible
                // and end_pos is already in machine units
                self.set_pos(end_pos)?;

                Ok(MotionSummary::Arc(arc))
            }
        }
    }

    //
    // ########## INTERPOLATION HELPER METHODS ##########
    //

    /// Calculates the new position for each axis,
    /// after checking for [`Positioning`] setting of the [`Machine`].
    ///
    /// Applies `work_offset` if present, for [`Positioning::Absolute`]
    /// and respects the current unit selections for both [`Machine`] and [`Code`],
    /// and any unit conversions are performed as necessary.
    ///
    /// Returns a new [`Point`] with each axis filled.
    fn new_pos(&self, mut pos: PartialPoint) -> Point {
        self.to_machine_units(&mut pos);

        match self.positioning {
            Positioning::Absolute => Point::new(
                pos.x()
                    .map_or(self.pos().x(), |x| self.work_offset().x() + x),
                pos.y()
                    .map_or(self.pos().y(), |y| self.work_offset().y() + y),
                pos.z()
                    .map_or(self.pos().z(), |z| self.work_offset().z() + z),
            ),

            Positioning::Incremental => Point::new(
                self.pos().x() + pos.x().unwrap_or(0.0),
                self.pos().y() + pos.y().unwrap_or(0.0),
                self.pos().z() + pos.z().unwrap_or(0.0),
            ),
        }
    }

    /// Set the current machine position for all axes.
    /// Does **NO UNIT CONVERSION**, even if the *machine* and *code* units differ.
    ///
    /// Returns [`MachineError::Overtravel`] if any axis exceeds `max_travels` or `HOME_POS`,
    /// indicating failure.
    fn set_pos(&mut self, pos: Point) -> Result<(), MachineError> {
        self.check_overtravel(pos)?;
        self.pos = pos;
        Ok(())
    }

    //
    // ########## OTHER HELPER METHODS ##########
    //

    /// Accepts a *mutable reference to [`Point`]* in `code_units`.
    /// Converts the units of all `Some` variant fields to Machine units.
    /// `None` variants are not changed.
    fn to_machine_units(&self, point: &mut PartialPoint) {
        if self.units == Unit::Imperial && self.code_units == Unit::Metric {
            point.to_imperial()
        } else if self.units == Unit::Metric && self.code_units == Unit::Imperial {
            point.to_metric()
        }
    }

    /// Checks if the given [`Point`] lies within the `machine_travels` or not.
    ///
    /// Returns [`MachineError::Overtravel`] the point is outside of `machine_travels`.
    fn check_overtravel(&self, point: Point) -> Result<(), MachineError> {
        if !(HOME_POS.x()..=self.max_travels.x()).contains(&point.x()) {
            Err(MachineError::Overtravel(b'X', point.x()))
        } else if !(HOME_POS.y()..=self.max_travels.y()).contains(&point.y()) {
            Err(MachineError::Overtravel(b'Y', point.y()))
        } else if !(HOME_POS.z()..=self.max_travels.z()).contains(&point.z()) {
            Err(MachineError::Overtravel(b'Z', point.z()))
        } else {
            Ok(())
        }
    }
}

/// Represents a **2D Point** on a specific **Plane**.
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct PlanarPoint(Plane, Float, Float);

impl PlanarPoint {
    /// Constructor for a [`PlanarPoint`].
    ///
    /// For a given [`Plane`], *first* represents the value for first axis letter and *second*
    /// represents the second axis letter.
    /// For example:
    /// ```ignore
    /// PlanarPoint::new(Plane::XY, 1.0, 2.0); // Constructs point with X = 1.0 & Y = 2.0.
    /// PlanarPoint::new(Plane::XZ, 3.0, 4.0); // Constructs point with X = 3.0 & Z = 4.0.
    /// PlanarPoint::new(Plane::YZ, 5.0, 6.0); // Constructs point with Y = 5.0 & Z = 6.0.
    /// ```
    pub fn new(plane: Plane, first: Float, second: Float) -> Self {
        Self(plane, first, second)
    }

    /// Constructs a new [`PlanarPoint`] from a [`Point`] and [`Plane`].
    /// Discards the coordinates for the axis that is not part of the provided [`Plane`].
    pub fn from_point(point: Point, plane: Plane) -> Self {
        match plane {
            Plane::XY => Self::new(plane, point.x(), point.y()),
            Plane::XZ => Self::new(plane, point.x(), point.z()),
            Plane::YZ => Self::new(plane, point.y(), point.z()),
        }
    }

    /// Returns selected plane for the point.
    pub fn plane(&self) -> Plane {
        self.0
    }

    /// Returns the first axis value for the selected plane.
    pub fn first(&self) -> Float {
        self.1
    }

    /// Returns the second axis value for the selected plane.
    pub fn second(&self) -> Float {
        self.2
    }

    /// Calculates distance between two [`PlanarPoint`]s that **MUST** be on the same [`Plane`].
    ///
    /// # Panics
    /// Panics if the `other` [`PlanarPoint`] is on a different [`Plane`].
    pub fn dist(&self, other: &Self) -> Float {
        assert_eq!(
            self.plane(),
            other.plane(),
            "Points must be on the same plane"
        );

        ((self.first() - other.first()).powi(2) + (self.second() - other.second()).powi(2)).sqrt()
    }
}

impl Sub for PlanarPoint {
    type Output = Self;

    fn sub(self, rhs: Self) -> Self::Output {
        Self::new(self.0, self.1 - rhs.1, self.2 - rhs.2)
    }
}

impl Add for PlanarPoint {
    type Output = Self;

    fn add(self, rhs: Self) -> Self::Output {
        Self::new(self.0, self.1 + rhs.1, self.2 + rhs.2)
    }
}

/// Possible errors that can happen during machine construction and interpolation.
#[derive(Debug, PartialEq, thiserror::Error)]
pub enum MachineError {
    /// Negative travel requested in [`Machine`] construction.
    #[error("negative travel requested")]
    NegativeTravel,
    #[error("machine travels too long")]
    TravelsTooLong,
    #[error("machine travels too short")]
    TravelsTooShort,
    /// The resulting machine would be an odd shape.
    #[error("unusual machine travel lengths")]
    WrongRatio,
    #[error("overtravel of '{}' axis due to last move to '{}' units", *.0 as char, .1)]
    Overtravel(u8, Float),
    /// Spindle On was commanded, but no spindle speed was provided.
    #[error("spindle on commanded without 'S' command throughout the program")]
    NoSpindleSpeed,
    /// Tool change was commanded, but no tool number was provided.
    #[error("tool change requested without 'T' command or tool preload")]
    NoTool,
    /// Feed move was commanded, but no feed was provided.
    #[error("feed move commanded without 'F' command throughout the program")]
    NoFeed,
    /// Both provided circle points are the same.
    #[error("start and end points of the arc are same")]
    PointsIntersect,
    /// Circle not possible due to method specific method requirements.
    #[error("{}", invalid_circle_msg(.0))]
    InvalidCircle(CircleMethod),
    /// No circle method provided for arc move.
    #[error("last move has no information about an arc while the machine is in arc mode")]
    NoCircleMethod,
    /// Provided center point does not lie on the selected plane.
    #[error("relative arc center does not lie on the active plane")]
    CenterOffPlane,
    /// Provided direction does not match with offset type.
    #[error("invalid offset direction: {0:?}")]
    OffsetDirection(Direction),
}

/// Genertes error message for [`MachineError::InvalidCircle`]
fn invalid_circle_msg(method: &CircleMethod) -> &'static str {
    match method {
        CircleMethod::RelativePoint(_) => {
            "start and end points are not at the same distance from center"
        }
        CircleMethod::FixedRadius(_) => {
            "distance between start and end points is more than arc diameter"
        }
    }
}

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

    #[test]
    fn construct_negative() {
        assert_eq!(
            Machine::build(Point::new(150.0, 500.0, -500.0), Unit::default()).unwrap_err(),
            MachineError::NegativeTravel
        );
    }

    #[test]
    fn construct_long_travels() {
        assert_eq!(
            Machine::build(Point::new(2000.0, 1000.0, 500.0), Unit::default()).unwrap_err(),
            MachineError::TravelsTooLong
        );
    }

    #[test]
    fn construct_short_travels() {
        assert_eq!(
            Machine::build(Point::new(100.0, 75.0, 75.0), Unit::default()).unwrap_err(),
            MachineError::TravelsTooShort
        );
    }

    #[test]
    fn construct_wrong_ratio() {
        assert_eq!(
            Machine::build(Point::new(1500.0, 100.0, 100.0), Unit::default()).unwrap_err(),
            MachineError::WrongRatio
        );
    }

    #[test]
    // Must pass
    fn construct_good() {
        assert!(Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::default()).is_ok());
    }

    // Test Machine methods

    #[test]
    fn overtravel_set() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::default()).unwrap();

        assert_eq!(
            m.set_pos(Point::new(-100.0, 500.0, 300.0)).unwrap_err(),
            MachineError::Overtravel(b'X', -100.0)
        );

        assert_eq!(
            m.set_pos(Point::new(1200.0, 500.0, 300.0)).unwrap_err(),
            MachineError::Overtravel(b'X', 1200.0)
        );
    }

    #[test]
    fn unit_standards() {
        // machine metric and code metric
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Metric).unwrap();
        m.set_code_units(Unit::Metric);
        m.move_machine(
            PartialPoint::new(Some(25.4), Some(25.4), Some(25.4)),
            None,
            None,
        )
        .unwrap();
        assert_eq!(m.pos(), &Point::new(25.4, 25.4, 25.4));

        // machine imperial and code imperial
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();
        m.set_code_units(Unit::Imperial);
        m.move_machine(
            PartialPoint::new(Some(1.0), Some(1.0), Some(1.0)),
            None,
            None,
        )
        .unwrap();
        assert_eq!(m.pos(), &Point::new(1.0, 1.0, 1.0));

        // machine imperial and code metric
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();
        m.set_code_units(Unit::Metric);
        m.move_machine(
            PartialPoint::new(Some(25.4), Some(25.4), Some(25.4)),
            None,
            None,
        )
        .unwrap();
        assert_eq!(m.pos(), &Point::new(1.0, 1.0, 1.0));

        // machine metric and code imperial
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Metric).unwrap();
        m.set_code_units(Unit::Imperial);
        m.move_machine(
            PartialPoint::new(Some(1.0), Some(1.0), Some(1.0)),
            None,
            None,
        )
        .unwrap();
        assert_eq!(m.pos(), &Point::new(25.4, 25.4, 25.4));
    }

    #[test]
    fn absolute_move() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Metric).unwrap();
        m.set_positioning(Positioning::Absolute);

        // with no g54 offset
        m.move_machine(PartialPoint::new(Some(100.0), None, None), None, None)
            .unwrap();
        assert_eq!(m.pos(), &Point::new(100.0, 0.0, 0.0));

        // with g54 offset
        m.set_work_offset(Point::new(100.0, 100.0, 100.0));
        m.move_machine(PartialPoint::new(Some(100.0), Some(0.0), None), None, None)
            .unwrap();
        assert_eq!(m.pos(), &Point::new(200.0, 100.0, 0.0));

        // overtravel
        assert_eq!(
            m.move_machine(PartialPoint::new(Some(901.0), Some(0.0), None), None, None)
                .unwrap_err(),
            MachineError::Overtravel(b'X', 1001.0)
        ); // tries to move X to 1001.0
    }

    #[test]
    fn incremental_move() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();
        m.set_positioning(Positioning::Incremental);

        // with no g54 offset
        m.move_machine(PartialPoint::new(Some(100.0), None, None), None, None)
            .unwrap();
        assert_eq!(m.pos(), &Point::new(100.0, 0.0, 0.0));

        // with g54 offset
        m.set_work_offset(Point::new(100.0, 100.0, 100.0)); // has no effect
        m.move_machine(PartialPoint::new(Some(200.0), Some(0.0), None), None, None)
            .unwrap();
        assert_eq!(m.pos(), &Point::new(300.0, 0.0, 0.0));

        // overtravel
        assert_eq!(
            m.move_machine(
                PartialPoint::new(Some(100.0), Some(751.0), None),
                None,
                None
            )
            .unwrap_err(),
            MachineError::Overtravel(b'Y', 751.0)
        ); // tries to move Y to 751.0
    }

    #[test]
    fn machine_move() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();

        // with no g54 offset
        m.move_machine_pos(PartialPoint::new(Some(100.0), None, Some(500.0)))
            .unwrap();
        assert_eq!(m.pos(), &Point::new(100.0, 0.0, 500.0));

        // with g54 offset
        m.set_work_offset(Point::new(100.0, 100.0, 100.0));
        m.move_machine_pos(PartialPoint::new(Some(200.0), Some(100.0), None))
            .unwrap(); // Z will be saved from previous move
        assert_eq!(m.pos(), &Point::new(200.0, 100.0, 500.0));

        // overtravel
        assert_eq!(
            m.move_machine_pos(PartialPoint::new(Some(0.0), Some(0.0), Some(-1.0)))
                .unwrap_err(),
            MachineError::Overtravel(b'Z', -1.0)
        ); // tries to move Z to -1.0
    }

    #[test]
    fn spindle() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();

        assert_eq!(
            m.spindle_on(CircularDirection::default(), None)
                .unwrap_err(),
            MachineError::NoSpindleSpeed
        );

        // no speed required when preset before
        m.set_speed(1000);
        assert!(m.spindle_on(CircularDirection::default(), None).is_ok());
        assert_eq!(m.speed, Some(1000));

        // providing new speed overwrites previous speed
        assert!(
            m.spindle_on(CircularDirection::default(), Some(500))
                .is_ok()
        );
        assert_eq!(m.speed, Some(500));
    }

    #[test]
    fn tool() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::Imperial).unwrap();

        assert_eq!(m.tool_change(None).unwrap_err(), MachineError::NoTool);

        // no tool required when preset before
        m.set_next_tool(1);
        assert!(m.tool_change(None).is_ok());
        assert_eq!(m.tool, 1);
        assert_eq!(m.next_tool, None); // next tool should always be reset

        // providing new tool overwrites any preloaded tool
        m.set_next_tool(5);
        assert!(m.tool_change(Some(10)).is_ok());
        assert_eq!(m.tool, 10);
        assert_eq!(m.next_tool, None);
    }

    // test to retrieve circle properties.
    #[test]
    fn test_arc_info() {
        // rel center
        let start = Point::new(-3.0, 4.0, 0.0);
        let end = Point::new(3.0, 4.0, 0.0);

        let method = CircleMethod::RelativePoint(PartialPoint::new(Some(3.0), Some(-4.0), None));
        let arc = Arc::build(start, end, method, Plane::XY, CircularDirection::Clockwise).unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 0.0);

        let method = CircleMethod::RelativePoint(PartialPoint::new(Some(3.0), Some(4.0), None));
        let arc = Arc::build(start, end, method, Plane::XY, CircularDirection::Clockwise).unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 8.0);

        // positive radius should get minor arc
        let method = CircleMethod::FixedRadius(5.0);
        let arc = Arc::build(start, end, method, Plane::XY, CircularDirection::Clockwise).unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 0.0);
        assert_eq!(arc.arc_type, ArcType::Minor);

        let arc = Arc::build(
            start,
            end,
            method,
            Plane::XY,
            CircularDirection::CounterClockwise,
        )
        .unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 8.0);
        assert_eq!(arc.arc_type, ArcType::Minor);

        // negative radius should get major arc
        let method = CircleMethod::FixedRadius(-5.0);
        let arc = Arc::build(start, end, method, Plane::XY, CircularDirection::Clockwise).unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 8.0);
        assert_eq!(arc.arc_type, ArcType::Major);

        let arc = Arc::build(
            start,
            end,
            method,
            Plane::XY,
            CircularDirection::CounterClockwise,
        )
        .unwrap();

        assert_eq!(arc.radius, 5.0);
        assert_eq!(arc.center.first(), 0.0);
        assert_eq!(arc.center.second(), 0.0);
        assert_eq!(arc.arc_type, ArcType::Major);
    }

    #[test]
    fn offset_set() {
        let mut m = Machine::build(Point::new(1000.0, 750.0, 500.0), Unit::default()).unwrap();

        assert_eq!(
            m.set_dia_offset(2, Direction::Up).unwrap_err(),
            MachineError::OffsetDirection(Direction::Up)
        );
        assert_eq!(
            m.set_dia_offset(3, Direction::Down).unwrap_err(),
            MachineError::OffsetDirection(Direction::Down)
        );
        m.set_dia_offset(3, Direction::Left).unwrap();
        assert_eq!(
            m.d_offset,
            Some(Offset {
                address: 3,
                dir: Direction::Left
            })
        );
        m.cancel_dia_offset();
        assert_eq!(m.d_offset, None);

        assert_eq!(
            m.set_height_offset(2, Direction::Left).unwrap_err(),
            MachineError::OffsetDirection(Direction::Left)
        );
        assert_eq!(
            m.set_height_offset(3, Direction::Right).unwrap_err(),
            MachineError::OffsetDirection(Direction::Right)
        );
        m.set_height_offset(5, Direction::Up).unwrap();
        assert_eq!(
            m.h_offset,
            Some(Offset {
                address: 5,
                dir: Direction::Up
            })
        );
        m.cancel_height_offset();
        assert_eq!(m.h_offset, None);
    }
}
