#include "gcode_commands.h"
#include "glm/geometric.hpp"

glm::vec<3, double> grbl::roll_components(glm::vec<3, double> v, int turns) {
    glm::vec<3, double> roll{};

    for (int i = 0; i < 3; i++) {
        roll[i] = v[(i - turns + 300) % 3];
    }

    return roll;
}

grbl::mcode_cmd::mcode_cmd(int code, int line)
        : code(code) {
    command::line_number = line;
}

grbl::spindle_cmd::spindle_cmd(double s, int line)
        : speed(s) {
    command::line_number = line;
}

grbl::dwell_cmd::dwell_cmd(double s, int line)
        : seconds(s) {
    command::line_number = line;
}

double grbl::line_motion_cmd::length() const {
    if (!start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2])
        return 0;

    return glm::length(delta());
}

glm::vec<3, double> grbl::line_motion_cmd::interpolate(double ratio) {
    return start + delta() * ratio;
}

std::vector<grbl::motion_cmd*> grbl::line_motion_cmd::split(double lngth) {
    //don't split up rapid or not fully defined motions
    if (rapid || !start_valid || !position_valid[0] || !position_valid[1] || !position_valid[2]) {
        return {this};
    }

    std::vector<motion_cmd*> result;
    int divisions = (int) std::ceil(length() / lngth);

    if (divisions < 1) {
        divisions = 1;
    }

    glm::vec3 last_end = start;

    for (int i = 1; i <= divisions; i++) {
        glm::vec3 end = interpolate(((double) i) / divisions);

        auto immediate = new line_motion_cmd;
        immediate->start = last_end;
        immediate->end = end;
        immediate->feed = feed;
        immediate->position_valid[0] = immediate->position_valid[1] = immediate->position_valid[2] = true;
        immediate->start_valid = true;
        result.push_back(immediate);

        last_end = end;
    }
    return result;
}

double grbl::arc_motion_cmd::length() const {
    return abs(angle_span() * radius());
}

glm::vec<3, double> grbl::arc_motion_cmd::interpolate(double ratio) {
    double angle = start_angle() + angle_span() * ratio;

    glm::vec<3, double> onPlane = {
            u + (radius() * cos(angle)),
            v + (radius() * sin(angle)),
            0
    };

    auto d = delta() * ratio;
    double helix = grbl::roll_components(start + d, -(int) plane).z;

    onPlane.z = helix;

    glm::vec3 interpolation = roll_components(onPlane, (int) plane);
    return interpolation;
}

std::vector<grbl::motion_cmd*> grbl::arc_motion_cmd::split(double lngth) {
    int divisions = (int) ceil(length() / lngth);

    if (divisions < 1)
        divisions = 1;

    glm::vec3 lastEnd = start;


    std::vector<grbl::motion_cmd*> result;

    for (int i = 1; i <= divisions; i++) {
        glm::vec<3, double> end = interpolate(((double) i) / divisions);

        auto immediate = new arc_motion_cmd;
        immediate->start = lastEnd;
        immediate->end = end;
        immediate->feed = feed;
        immediate->direction = direction;
        immediate->plane = plane;
        immediate->u = u;
        immediate->v = v;

        result.push_back(immediate);

        lastEnd = end;
    }
    return result;
}