#pragma once
#include <Arduino.h>
#include <AccelStepper.h>
#include <functional>

class MotorController {
public:
    MotorController();
    void init();
    void update();

    void moveRelative1(long steps);
    void moveRelative2(long steps);
    void moveAbsolute(long target1, long target2);
    
    void setMaxSpeed(float spd);
    void setAcceleration(float acc);
    
    void home1();
    void home2();
    void homeAll();
    void zeroAll();
    void disableMotors();

    void setOnTimeoutCallback(std::function<void()> cb);

    bool isMoving();
    String getPositionString();
    
private:
    AccelStepper stepper1;
    AccelStepper stepper2;

    float currentMaxSpeed;
    float currentAccel;
    
    unsigned long lastMotorActive;
    bool motorsDisabled;
    std::function<void()> onTimeout;
    
    enum HomingState {
      HOME_IDLE,
      HOME_ALL_PHASE1,
      HOME_ALL_PHASE2,
      HOME_1_PHASE1,
      HOME_1_PHASE2,
      HOME_2_PHASE1,
      HOME_2_PHASE2
    };
    HomingState homingState;
    
    void processHoming();
    void checkIdleTimeout();
};