#include <EEPROM.h>
#include <string.h>
#include "serial/CommandParser.h"
#include "motor/MotorAxis.h"
#include "motor/MotorDriver.h"
#include "motor/Homing.h"
#include "config/Constants.h"

#include "TMC5160.h"

// ---------------------------------------------------------------------------
// Serial command protocol
//
// Format:  <cmd> [y|p] [d] <arg1> , <arg2> , ...
//
//   Axis specifier (optional, default = both):
//     y  — yaw only
//     p  — pitch only
//
//   Base specifier (optional, default = hex):
//     d  — interpret numeric arguments as decimal
//
// Command table:
//   r   re-init registers and enable coils (safe recovery from any fault)
//   q   start homing sequence
//   !   disable motor coils (free-wheel)
//   *   enable motor coils (apply holding torque)
//   m   move to absolute encoder position:  m[y|p][d] <pos>, <speed>
//   ;   overwrite XACTUAL register directly:  ; <value>
//   s   stop (set velocity to 0)
//   b   clear encoder deviation warning flag
//   t   sync encoder to actual position (zero out tracking error)
//   e   clear stallguard event flag (both axes)
//   w   set stallguard threshold:  w <sgt>
//   u   enter AUTO stepping mode:  u <num_steps>  (must be > 2)
//   h   save heading calibration to EEPROM:  h 0, <heading_value>
//   y   load heading calibration from EEPROM:  y 0
//   d   enter MANUAL mode (disables AUTO stepping on yaw)
//   p   advance yaw by one step (AUTO mode)
//   v   set yaw v_max:  v <speed>
//   +   set yaw right endstop to current encoder position
//   -   set yaw left  endstop to current encoder position
//   /   set both yaw endstops manually:  / <right>, <left>
//   ?   print current yaw endstop positions
// ---------------------------------------------------------------------------

// Peek at the next byte to check for an axis specifier ('y' or 'p').
// Consumes the byte if found. Returns a Flash string label for Serial output.
static const __FlashStringHelper* parse_axis(uint8_t &out_yaw, uint8_t &out_pitch) {
    out_yaw   = 1;
    out_pitch = 1;
    if (Serial.peek() == 'y') {
        Serial.read();
        out_pitch = 0;
        return F(" [YAW]");
    }
    if (Serial.peek() == 'p') {
        Serial.read();
        out_yaw = 0;
        return F(" [PITCH]");
    }
    return F(" [YAW + PITCH]");
}

// Read the rest of the command line (up to newline) and tokenise on commas.
// Up to 4 arguments are parsed; extras are silently ignored.
// Returns the number of arguments found.
static uint8_t parse_args(int32_t args[4], uint8_t base) {
    static char buf[32]; // static to avoid stack allocation on every call
    size_t len = Serial.readBytesUntil('\n', buf, sizeof(buf) - 1);
    buf[len] = '\0';

    uint8_t argc = 0;
    char *tok = strtok(buf, ",");
    while (tok && argc < 4) {
        args[argc++] = strtol(tok, nullptr, base);
        tok = strtok(nullptr, ",");
    }
    return argc;
}

void read_command() {
    char c = Serial.read(); // command character — must be read before peek/parse_axis consume it

    uint8_t do_yaw, do_pitch;
    const __FlashStringHelper *axis_label = parse_axis(do_yaw, do_pitch);

    // 'd' immediately after the axis specifier switches argument parsing to decimal.
    // Default is hex, which is convenient for register values and encoder positions.
    uint8_t base = 16;
    if (Serial.peek() == 'd') { Serial.read(); base = 10; }

    int32_t args[4] = {0};
    parse_args(args, base);

    switch (c) {

    case 'r': // re-init registers and re-enable — useful after a fault or power issue
        Serial.print(F("Resetting")); Serial.println(axis_label);
        if (do_yaw)   { motor_regs_init_yaw();  motor_enable(AXIS_YAW);   ax_yaw.status   = GIMBAL_STATUS_RESET; }
        if (do_pitch) { motor_regs_init_pitch(); motor_enable(AXIS_PITCH); ax_pitch.status = GIMBAL_STATUS_RESET; }
        break;

    case 'q': // start homing — axis must be enabled first
        Serial.print(F("Homing")); Serial.println(axis_label);
        if (do_yaw)   do_homing_start(AXIS_YAW);
        if (do_pitch) do_homing_start(AXIS_PITCH);
        break;

    case '!': // disable coils — motor can be moved by hand
        Serial.print(F("Disabling motors")); Serial.println(axis_label);
        if (do_yaw)   motor_disable(AXIS_YAW);
        if (do_pitch) motor_disable(AXIS_PITCH);
        break;

    case '*': // enable coils — motor resists manual movement
        Serial.print(F("Enabling motors")); Serial.println(axis_label);
        if (do_yaw)   motor_enable(AXIS_YAW);
        if (do_pitch) motor_enable(AXIS_PITCH);
        break;

    case 'm': // absolute position move: args[0] = target, args[1] = speed
        Serial.print(F("Moving to ")); Serial.print(args[0]);
        Serial.print(F(" speed "));   Serial.print(args[1]);
        Serial.println(axis_label);
        if (do_yaw)   tmc5160_moveTo(AXIS_YAW,   args[0], args[1]);
        if (do_pitch) tmc5160_moveTo(AXIS_PITCH, args[0], args[1]);
        break;

    case ';': // directly overwrite XACTUAL — shifts the coordinate origin without moving
        Serial.print(F("Setting XACTUAL to ")); Serial.print(args[0]); Serial.println(axis_label);
        if (do_yaw)   tmc5160_writeRegister(AXIS_YAW,   TMC5160_XACTUAL, args[0]);
        if (do_pitch) tmc5160_writeRegister(AXIS_PITCH, TMC5160_XACTUAL, args[0]);
        break;

    case 's': // stop — set velocity to zero, motor decelerates on the ramp profile
        Serial.print(F("Stopping")); Serial.println(axis_label);
        if (do_yaw)   tmc5160_rotateMotor(AXIS_YAW,   0);
        if (do_pitch) tmc5160_rotateMotor(AXIS_PITCH, 0);
        break;

    case 'b': // clear the latched deviation warning so the next deviation can be detected
        Serial.print(F("Clearing encoder deviation flags")); Serial.println(axis_label);
        if (do_yaw)   ax_yaw.clear_enc_deviation();
        if (do_pitch) ax_pitch.clear_enc_deviation();
        break;

    case 't': // snap the step counter to the encoder position, eliminating tracking error
        Serial.print(F("Syncing encoder to actual")); Serial.println(axis_label);
        if (do_yaw)   { ax_yaw.clear_enc_deviation();   ax_yaw.sync_encoder_to_actual();   }
        if (do_pitch) { ax_pitch.clear_enc_deviation(); ax_pitch.sync_encoder_to_actual(); }
        break;

    case 'e': // clear stallguard latch on both axes (no axis specifier supported here)
        ax_yaw.clear_stallguard();
        ax_pitch.clear_stallguard();
        break;

    case 'w': // adjust stallguard sensitivity — lower = more sensitive to stalls
        ax_yaw.set_stallguard_threshold(args[0]);
        ax_pitch.set_stallguard_threshold(args[0]);
        break;

    case 'u': // enter AUTO mode and configure step count (yaw only)
        // Requires args[0] > 2 because set_step_size() divides by (n-1);
        // anything <= 1 would produce zero or negative step sizes.
        if (args[0] > 2) {
            ax_yaw.set_step_size(args[0]);
            ax_yaw.target = ax_yaw.enc_limit_l + 2000; // start just inside the left limit
            ax_yaw.status = GIMBAL_STATUS_AUTO;
        }
        break;

    case 'h': // save heading calibration — args[0] = axis (0=yaw), args[1] = heading value
        if (args[0] == AXIS_YAW) {
            ax_yaw.heading        = (float)args[1];
            ax_yaw.heading_origin = ax_yaw.X_enc;
            // Store the offset from the left limit rather than the absolute encoder count,
            // so the calibration remains valid after a re-home (which may shift the origin).
            int32_t dist = ax_yaw.heading_origin - ax_yaw.enc_limit_l;
            EEPROM.put(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading);
            EEPROM.put(EEPROM_ADDR_HDG_DIST,  dist);
        }
        break;

    case 'y': // restore heading calibration from EEPROM — args[0] = axis (0=yaw)
        if (args[0] == AXIS_YAW) {
            int32_t dist = 0;
            EEPROM.get(EEPROM_ADDR_HDG_FLOAT, ax_yaw.heading);
            EEPROM.get(EEPROM_ADDR_HDG_DIST,  dist);
            // Reconstruct heading_origin relative to the current left limit,
            // making it valid even if the absolute encoder counts shifted after re-homing.
            ax_yaw.heading_origin = ax_yaw.enc_limit_l + dist;
            Serial.println(ax_yaw.heading);
            Serial.println(ax_yaw.heading_origin);
        }
        break;

    case 'd': // enter MANUAL mode — disables AUTO stepping
        ax_yaw.status = GIMBAL_STATUS_MANUAL;
        break;

    case 'p': // advance yaw by one step in AUTO mode
        ax_yaw.sync_encoder_to_actual(); // zero tracking error before issuing the next target
        ax_yaw.target  += ax_yaw.step_size;
        ax_yaw.new_pos  = true;
        break;

    case 'v': // override yaw v_max at runtime
        ax_yaw.v_max = args[0];
        break;

    // Manual endstop overrides — useful for calibration or testing without homing:
    case '+': // mark current yaw encoder position as the right limit
        ax_yaw.enc_limit_r = ax_yaw.X_enc;
        break;

    case '-': // mark current yaw encoder position as the left limit
        ax_yaw.enc_limit_l = ax_yaw.X_enc;
        break;

    case '/': // set both yaw limits explicitly: args[0] = right, args[1] = left
        ax_yaw.enc_limit_r = args[0];
        ax_yaw.enc_limit_l = args[1];
        Serial.println(ax_yaw.enc_limit_r);
        Serial.println(ax_yaw.enc_limit_l);
        break;

    case '?': // query current yaw endstop positions
        Serial.println(ax_yaw.enc_limit_r);
        Serial.println(ax_yaw.enc_limit_l);
        Serial.println(ax_yaw.enc_limit_l - ax_yaw.enc_limit_r); // negative = right > left (expected)
        break;

    default:
        Serial.println(F("Unknown command"));
        break;
    }
}