#include "MotorController.h"
#include "Config.h"

MotorController::MotorController() 
    : stepper1(AccelStepper::HALF4WIRE, M1_IN1, M1_IN3, M1_IN2, M1_IN4),
      stepper2(AccelStepper::HALF4WIRE, M2_IN1, M2_IN3, M2_IN2, M2_IN4),
      currentMaxSpeed(DEFAULT_SPEED),
      currentAccel(DEFAULT_ACCEL),
      lastMotorActive(0),
      motorsDisabled(true),
      homingState(HOME_IDLE) {}

void MotorController::init() {
    stepper1.setMaxSpeed(DEFAULT_SPEED);
    stepper1.setAcceleration(DEFAULT_ACCEL);
    stepper1.setCurrentPosition(0);

    stepper2.setMaxSpeed(DEFAULT_SPEED);
    stepper2.setAcceleration(DEFAULT_ACCEL);
    stepper2.setCurrentPosition(0);

    Serial.println("[STEP] Steppers initialized");
}

void MotorController::setOnTimeoutCallback(std::function<void()> cb) {
    onTimeout = cb;
}

void MotorController::update() {
    stepper1.run();
    stepper2.run();

    bool moving = isMoving();
    if (moving) {
        lastMotorActive = millis();
        motorsDisabled = false;
    }

    checkIdleTimeout();
    processHoming();
}

bool MotorController::isMoving() {
    return stepper1.isRunning() || stepper2.isRunning() || homingState != HOME_IDLE;
}

String MotorController::getPositionString() {
    bool isHolding = !motorsDisabled || isMoving();
    return "P:" + String(stepper1.currentPosition()) + "," +
                  String(stepper2.currentPosition()) + "," +
                  String(isHolding ? 1 : 0);
}

void MotorController::checkIdleTimeout() {
    if (!motorsDisabled && !isMoving() && (millis() - lastMotorActive > IDLE_TIMEOUT_MS)) {
        stepper1.disableOutputs();
        stepper2.disableOutputs();
        motorsDisabled = true;
        Serial.println("[SYS] Idle timeout reached. Steppers disabled.");
        if (onTimeout) {
            onTimeout();
        }
    }
}

void MotorController::moveRelative1(long steps) {
    stepper1.move(steps);
    Serial.printf("[S1] Move relative %+ld steps\n", steps);
}

void MotorController::moveRelative2(long steps) {
    stepper2.move(steps);
    Serial.printf("[S2] Move relative %+ld steps\n", steps);
}

void MotorController::moveAbsolute(long target1, long target2) {
    stepper1.moveTo(target1);
    stepper2.moveTo(target2);
    Serial.printf("[X] Move absolute to %ld, %ld\n", target1, target2);
}

void MotorController::setMaxSpeed(float spd) {
    currentMaxSpeed = spd;
    stepper1.setMaxSpeed(spd);
    stepper2.setMaxSpeed(spd);
    Serial.printf("[CFG] Max speed → %.0f steps/sec\n", spd);
}

void MotorController::setAcceleration(float acc) {
    currentAccel = acc;
    stepper1.setAcceleration(acc);
    stepper2.setAcceleration(acc);
    Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc);
}

void MotorController::homeAll() {
    Serial.println("[SYS] Homing all: Phase 1 (Stall OUTWARD)");
    stepper1.setMaxSpeed(MAX_SPEED);
    stepper2.setMaxSpeed(MAX_SPEED);
    stepper1.setAcceleration(MAX_ACCEL);
    stepper2.setAcceleration(MAX_ACCEL);
    stepper1.move(-4000); 
    stepper2.move(4000);
    homingState = HOME_ALL_PHASE1;
}

void MotorController::home1() {
    Serial.println("[S1] Homing Motor 1: Phase 1 (Stall OUTWARD)");
    stepper1.setMaxSpeed(MAX_SPEED);
    stepper1.setAcceleration(MAX_ACCEL);
    stepper1.move(-4000);
    homingState = HOME_1_PHASE1;
}

void MotorController::home2() {
    Serial.println("[S2] Homing Motor 2: Phase 1 (Stall OUTWARD)");
    stepper2.setMaxSpeed(MAX_SPEED);
    stepper2.setAcceleration(MAX_ACCEL);
    stepper2.move(4000);
    homingState = HOME_2_PHASE1;
}

void MotorController::zeroAll() {
    stepper1.setCurrentPosition(2048);
    stepper2.setCurrentPosition(0);
    Serial.println("[SYS] Position blindly reset to home (2048, 0).");
}

void MotorController::disableMotors() {
    stepper1.disableOutputs();
    stepper2.disableOutputs();
    motorsDisabled = true;
    Serial.println("[SYS] Steppers manually disabled.");
}

void MotorController::processHoming() {
    if (homingState == HOME_ALL_PHASE1) {
        if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
            stepper1.setCurrentPosition(2048);
            stepper2.setCurrentPosition(0);
            stepper1.moveTo(2048);
            stepper2.moveTo(0);
            homingState = HOME_ALL_PHASE2;
            Serial.println("[SYS] Homing all: Phase 2 (Hold Outward Home)");
        }
    } else if (homingState == HOME_ALL_PHASE2) {
        if (stepper1.distanceToGo() == 0 && stepper2.distanceToGo() == 0) {
            stepper1.setMaxSpeed(currentMaxSpeed);
            stepper2.setMaxSpeed(currentMaxSpeed);
            stepper1.setAcceleration(currentAccel);
            stepper2.setAcceleration(currentAccel);
            homingState = HOME_IDLE;
            Serial.println("[SYS] Homing all complete.");
        }
    } else if (homingState == HOME_1_PHASE1) {
        if (stepper1.distanceToGo() == 0) {
            stepper1.setCurrentPosition(2048);
            stepper1.moveTo(2048);
            homingState = HOME_1_PHASE2;
        }
    } else if (homingState == HOME_1_PHASE2) {
        if (stepper1.distanceToGo() == 0) {
            stepper1.setMaxSpeed(currentMaxSpeed);
            stepper1.setAcceleration(currentAccel);
            homingState = HOME_IDLE;
        }
    } else if (homingState == HOME_2_PHASE1) {
        if (stepper2.distanceToGo() == 0) {
            stepper2.setCurrentPosition(0);
            stepper2.moveTo(0);
            homingState = HOME_2_PHASE2;
        }
    } else if (homingState == HOME_2_PHASE2) {
        if (stepper2.distanceToGo() == 0) {
            stepper2.setMaxSpeed(currentMaxSpeed);
            stepper2.setAcceleration(currentAccel);
            homingState = HOME_IDLE;
        }
    }
}