#pragma once

#include <glm/vec3.hpp>
#include <vector>
#include <memory>
#include <cmath>
#include "glm/ext/scalar_constants.hpp"

namespace grbl {

    glm::vec<3, double> roll_components(glm::vec<3, double> v, int turns);

    enum class arc_plane {
        xy = 0,
        yz = 1,
        zx = 2
    };

    enum class arc_direction {
        cw,
        ccw
    };

    struct command {
        virtual ~command() = default;
        int line_number = 1;
    };

    struct mcode_cmd : public command {
        mcode_cmd(int code, int line_number);

        int code;
    };

    struct spindle_cmd : public command {
        spindle_cmd(double speed, int line_number);
        double speed;
    };

    struct dwell_cmd : public command {
        dwell_cmd(double seconds, int line_number);
        double seconds;
    };

    struct motion_cmd : public command {
        glm::vec<3, double> start = glm::vec3(0);
        glm::vec<3, double> end = glm::vec3(0);
        double feed = 0;

        glm::vec<3, double> delta() const { return end - start; }

        // Total travel distance of tool
        virtual double length() const = 0;
        virtual glm::vec<3, double> interpolate(double ratio) = 0;
        virtual std::vector<motion_cmd*> split(double length) = 0;
    };

    struct line_motion_cmd : public motion_cmd {

        bool rapid = false;
        // PositionValid[i] is true if the corresponding coordinate of the end position was defined in the file.
        // eg. for a file with "G0 Z15" as the first line, X and Y would still be false
        bool position_valid[3] = {false, false, false};
        bool start_valid = false;

        double length() const override;
        glm::vec<3, double> interpolate(double ratio) override;
        std::vector<motion_cmd*> split(double length) override;
    };

    struct arc_motion_cmd : public motion_cmd {
        arc_plane plane;
        arc_direction direction;
        double u;    //absolute position of center in first axis of plane
        double v;    //absolute position of center in second axis of plane

        double start_angle() const {
            glm::vec<3, double> start_in_plane = roll_components(start, -(int) plane);
            double X = start_in_plane.x - u;
            double Y = start_in_plane.y - v;
            return atan2(Y, X);
        }

        double end_angle() const {
            glm::vec<3, double> end_in_plane = roll_components(end, -(int) plane);
            double X = end_in_plane.x - u;
            double Y = end_in_plane.y - v;
            return atan2(Y, X);
        }

        double angle_span() const {
            double span = end_angle() - start_angle();
            if (direction == arc_direction::cw) {
                if (span >= 0)
                    span -= 2 * glm::pi<double>();
            } else {
                if (span <= 0)
                    span += 2 * glm::pi<double>();
            }

            return span;
        }

        double radius() const {
            glm::vec<3, double> start_plane = roll_components(start, -(int) plane);
            glm::vec<3, double> end_plane = roll_components(end, -(int) plane);

            return (
                           sqrt(pow(start_plane.x - u, 2) + pow(start_plane.y - v, 2)) +
                           sqrt(pow(end_plane.x - u, 2) + pow(end_plane.y - v, 2))
                   ) / 2;
        }

        glm::vec<3, double> interpolate(double ratio) override;
        std::vector<motion_cmd*> split(double length) override;

        double length() const override;

    };


}