#include "uart_mode.h"
#include "config.h"
#include "motion.h"
#include "pen.h"
#include "gcode_parser.h"

static char    s_lineBuf[128];
static uint8_t s_lineLen    = 0;
static float   s_lastX      = 0.0f;
static float   s_lastY      = 0.0f;
static float   s_lastFeed   = SPEED_DEFAULT_MM_S * 60.0f;
static GCodeCmd s_lastGCmd  = GCodeCmd::G0;
static bool    s_waitingIdle = false; // waiting for motion to finish before sending "ok"

void uart_mode_init() {
    motion_enable();
    pen_up();
    motion_home();
    Serial.println("UART MODE READY");
    Serial.println("Wysylaj komendy GCode (G0,G1,G28,M3,M5)");
}

static void executeGCode(const char* line) {
    GCodeLine gl;
    if (!gcode_parse_line(line, gl)) {
        Serial.println("ok");
        return;
    }

    GCodeCmd cmd = gl.cmd;
    if (cmd == GCodeCmd::NONE) {
        cmd = s_lastGCmd;
    } else if (cmd == GCodeCmd::G0 || cmd == GCodeCmd::G1) {
        s_lastGCmd = cmd;
    }

    float x = gl.hasX ? gl.x : s_lastX;
    float y = gl.hasY ? gl.y : s_lastY;
    float f = gl.hasF ? gl.f : s_lastFeed;

    s_lastX    = x;
    s_lastY    = y;
    s_lastFeed = f;

    switch (cmd) {
    case GCodeCmd::G0:
        pen_up();
        motion_move_to(x, y, SPEED_MAX_MM_S * 60.0f);
        s_waitingIdle = true;
        break;

    case GCodeCmd::G1:
        pen_down();
        motion_move_to(x, y, f);
        s_waitingIdle = true;
        break;

    case GCodeCmd::G28:
        pen_up();
        motion_home();
        Serial.println("ok");
        break;

    case GCodeCmd::M3:
        pen_down();
        Serial.println("ok");
        break;

    case GCodeCmd::M5:
        pen_up();
        Serial.println("ok");
        break;

    default:
        Serial.println("ok");
        break;
    }
}

void uart_mode_update() {
    // Always advance steppers
    motion_run();

    // Send "ok" once motion finishes
    if (s_waitingIdle && motion_is_idle()) {
        s_waitingIdle = false;
        Serial.println("ok");
    }

    // Accumulate incoming bytes into line buffer
    while (Serial.available()) {
        char c = (char)Serial.read();
        if (c == '\n' || c == '\r') {
            if (s_lineLen > 0) {
                s_lineBuf[s_lineLen] = '\0';
                s_lineLen = 0;
                if (!s_waitingIdle) {
                    executeGCode(s_lineBuf);
                } else {
                    Serial.println("error: busy");
                }
            }
        } else if (s_lineLen < sizeof(s_lineBuf) - 1) {
            s_lineBuf[s_lineLen++] = c;
        }
    }
}
