#include "motion.h"
#include "config.h"
#include <AccelStepper.h>

// Kinematyka KARTEZJAŃSKA:
// stepperX → oś X (lewo/prawo)
// stepperY → oś Y (góra/dół)
// Każdy silnik steruje tylko swoją osią niezależnie.

static AccelStepper s_stepperX(AccelStepper::DRIVER, PIN_X_STEP, PIN_X_DIR);
static AccelStepper s_stepperY(AccelStepper::DRIVER, PIN_Y_STEP, PIN_Y_DIR);
static float        s_speedMult       = SPEED_MULT_DEFAULT;
static float        s_lastBaseFeedMmS = SPEED_DEFAULT_MM_S;
static uint32_t     s_idleSinceMs     = 0;
static bool         s_autoDisabled    = false;

static float clampSpeed(float s) {
    if (s < SPEED_MIN_MM_S) return SPEED_MIN_MM_S;
    if (s > SPEED_MAX_MM_S) return SPEED_MAX_MM_S;
    return s;
}

static void applySpeed(float baseFeedMmS) {
    float speed = clampSpeed(baseFeedMmS * s_speedMult);
    s_stepperX.setMaxSpeed(speed * STEPS_PER_MM_X);
    s_stepperY.setMaxSpeed(speed * STEPS_PER_MM_Y);
}

// ── Homing pomocniczy (jeden silnik) ──────────────────────────────────────────

static void homeAxisSingle(AccelStepper& stepper, int8_t dir,
                            uint8_t limitPin, float workspaceMm, float stepsMm) {
    long safeTravel = (long)((workspaceMm + 50.0f) * stepsMm) * dir;
    stepper.move(safeTravel);
    while (digitalRead(limitPin) != LOW) {
        if (!stepper.run()) break;
    }
    stepper.setCurrentPosition(0);

    long backoff = (long)(HOMING_BACKOFF_MM * stepsMm) * (-dir);
    stepper.move(backoff);
    while (stepper.run()) {}
    stepper.setCurrentPosition(0);
}

// ── Publiczne API ─────────────────────────────────────────────────────────────

void motion_init() {
    pinMode(PIN_STEPPER_EN,  OUTPUT);
    pinMode(PIN_LIMIT_X_MIN, INPUT_PULLUP);
    pinMode(PIN_LIMIT_X_MAX, INPUT_PULLUP);
    pinMode(PIN_LIMIT_Y_MIN, INPUT_PULLUP);
    pinMode(PIN_LIMIT_Y_MAX, INPUT_PULLUP);
    motion_enable();

    s_stepperX.setMaxSpeed(SPEED_DEFAULT_MM_S * STEPS_PER_MM_X);
    s_stepperX.setAcceleration(ACCEL_MM_S2    * STEPS_PER_MM_X);
    s_stepperY.setMaxSpeed(SPEED_DEFAULT_MM_S * STEPS_PER_MM_Y);
    s_stepperY.setAcceleration(ACCEL_MM_S2    * STEPS_PER_MM_Y);
}

void motion_move_to(float x_mm, float y_mm, float feedrate_mm_per_min) {
    if (s_autoDisabled) {
        motion_enable();
        s_autoDisabled = false;
    }
    s_idleSinceMs = 0;
    s_lastBaseFeedMmS = feedrate_mm_per_min / 60.0f;
    applySpeed(s_lastBaseFeedMmS);

    // Kartezjański: każdy silnik steruje tylko swoją osią
    long sx = (long)(x_mm * STEPS_PER_MM_X) * -HOMING_DIR_X;
    long sy = (long)(y_mm * STEPS_PER_MM_Y) * -HOMING_DIR_Y;
    s_stepperX.moveTo(sx);
    s_stepperY.moveTo(sy);
}

bool motion_run() {
    float sx = s_stepperX.speed();
    float sy = s_stepperY.speed();

    // Zabezpieczenia krańcówek X
    if (digitalRead(PIN_LIMIT_X_MIN) == LOW && (float)HOMING_DIR_X * sx > 0)
        s_stepperX.setCurrentPosition(s_stepperX.currentPosition());
    if (digitalRead(PIN_LIMIT_X_MAX) == LOW && (float)HOMING_DIR_X * sx < 0)
        s_stepperX.setCurrentPosition(s_stepperX.currentPosition());

    // Zabezpieczenia krańcówek Y
    if (digitalRead(PIN_LIMIT_Y_MIN) == LOW && (float)HOMING_DIR_Y * sy > 0)
        s_stepperY.setCurrentPosition(s_stepperY.currentPosition());
    if (digitalRead(PIN_LIMIT_Y_MAX) == LOW && (float)HOMING_DIR_Y * sy < 0)
        s_stepperY.setCurrentPosition(s_stepperY.currentPosition());

    bool rx = s_stepperX.run();
    bool ry = s_stepperY.run();
    bool moving = rx || ry;

    if (moving) {
        s_idleSinceMs  = 0;
        s_autoDisabled = false;
    } else if (STEPPER_IDLE_TIMEOUT_MS > 0 && !s_autoDisabled) {
        if (s_idleSinceMs == 0) {
            s_idleSinceMs = millis();
        } else if (millis() - s_idleSinceMs >= STEPPER_IDLE_TIMEOUT_MS) {
            motion_disable();
            s_autoDisabled = true;
        }
    }

    return moving;
}

bool motion_is_idle() {
    return !s_stepperX.isRunning() && !s_stepperY.isRunning();
}

void motion_wait_idle() {
    while (!motion_is_idle()) motion_run();
}

// Homing kartezjański: każda oś niezależnie
void motion_home() {
    s_stepperX.setMaxSpeed(HOMING_SPEED_MM_S * STEPS_PER_MM_X);
    s_stepperX.setAcceleration(ACCEL_MM_S2   * STEPS_PER_MM_X);
    s_stepperY.setMaxSpeed(HOMING_SPEED_MM_S * STEPS_PER_MM_Y);
    s_stepperY.setAcceleration(ACCEL_MM_S2   * STEPS_PER_MM_Y);

    homeAxisSingle(s_stepperX, HOMING_DIR_X, PIN_LIMIT_X_MIN,
                   WORKSPACE_WIDTH_MM, STEPS_PER_MM_X);
    homeAxisSingle(s_stepperY, HOMING_DIR_Y, PIN_LIMIT_Y_MIN,
                   WORKSPACE_HEIGHT_MM, STEPS_PER_MM_Y);

    applySpeed(s_lastBaseFeedMmS > 0 ? s_lastBaseFeedMmS : SPEED_DEFAULT_MM_S);
    s_stepperX.setAcceleration(ACCEL_MM_S2 * STEPS_PER_MM_X);
    s_stepperY.setAcceleration(ACCEL_MM_S2 * STEPS_PER_MM_Y);
}

void motion_set_speed_multiplier(float mult) {
    if (mult < SPEED_MULT_MIN) mult = SPEED_MULT_MIN;
    if (mult > SPEED_MULT_MAX) mult = SPEED_MULT_MAX;
    s_speedMult = mult;
    applySpeed(s_lastBaseFeedMmS);
}

float motion_get_speed_multiplier() {
    return s_speedMult;
}

void motion_enable() {
    digitalWrite(PIN_STEPPER_EN, LOW);
}

void motion_disable() {
    digitalWrite(PIN_STEPPER_EN, HIGH);
}

float motion_get_x_mm() {
    return (float)s_stepperX.currentPosition() / STEPS_PER_MM_X * (-HOMING_DIR_X);
}

float motion_get_y_mm() {
    return (float)s_stepperY.currentPosition() / STEPS_PER_MM_Y * (-HOMING_DIR_Y);
}

bool motion_any_limit_triggered() {
    return digitalRead(PIN_LIMIT_X_MIN) == LOW ||
           digitalRead(PIN_LIMIT_X_MAX) == LOW ||
           digitalRead(PIN_LIMIT_Y_MIN) == LOW ||
           digitalRead(PIN_LIMIT_Y_MAX) == LOW;
}
