#include "gcode_parser.h"
#include <string.h>
#include <ctype.h>
#include <stdlib.h>

bool gcode_extract_param(const char* line, char code, float& out) {
    char upper = (char)toupper((unsigned char)code);
    for (const char* p = line; *p; p++) {
        if (toupper((unsigned char)*p) == upper) {
            char* end;
            float val = strtof(p + 1, &end);
            if (end != p + 1) {
                out = val;
                return true;
            }
        }
    }
    return false;
}

bool gcode_parse_line(const char* line, GCodeLine& out) {
    out = {GCodeCmd::NONE, false, false, false, 0, 0, 0};

    // Skip leading whitespace
    while (*line == ' ' || *line == '\t') line++;

    // Skip empty lines and comments
    if (*line == '\0' || *line == ';' || *line == '(' || *line == '%') return false;

    // Strip inline comment — work on a local copy
    static char buf[128];
    strncpy(buf, line, sizeof(buf) - 1);
    buf[sizeof(buf) - 1] = '\0';
    char* comment = strchr(buf, ';');
    if (comment) *comment = '\0';
    comment = strchr(buf, '(');
    if (comment) *comment = '\0';

    // Convert to uppercase in-place
    for (char* p = buf; *p; p++) *p = (char)toupper((unsigned char)*p);

    const char* src = buf;

    // Detect command — order matters (G28 before G2x, M05 before M5 etc.)
    if (strstr(src, "G28"))       out.cmd = GCodeCmd::G28;
    else if (strstr(src, "G0"))   out.cmd = GCodeCmd::G0;
    else if (strstr(src, "G1"))   out.cmd = GCodeCmd::G1;
    else if (strstr(src, "M3"))   out.cmd = GCodeCmd::M3;
    else if (strstr(src, "M5"))   out.cmd = GCodeCmd::M5;
    else {
        // Line has no recognized command — might be modal (X Y only)
        // Check if it has coordinates at all
        float dummy;
        if (!gcode_extract_param(src, 'X', dummy) && !gcode_extract_param(src, 'Y', dummy)) {
            return false;
        }
        out.cmd = GCodeCmd::NONE; // caller must apply last modal cmd
    }

    out.hasX = gcode_extract_param(src, 'X', out.x);
    out.hasY = gcode_extract_param(src, 'Y', out.y);
    out.hasF = gcode_extract_param(src, 'F', out.f);

    return true;
}
