#include <SpeedyStepper.h>

#define NUM_STEPPERS 3
// [X, Y, Head]
const int stepPins[NUM_STEPPERS] = {60, 26};
const int dirPins[NUM_STEPPERS] = {61, 28};
const int enablePins[NUM_STEPPERS] = {56, 24};


enum StepperAxis{
    Y,
    Head
};

SpeedyStepper steppers[NUM_STEPPERS];
bool isMoving[NUM_STEPPERS];

void initSteppers(){
    for (int i = 0; i < NUM_STEPPERS; i++) {
        pinMode(stepPins[i], OUTPUT);
        pinMode(dirPins[i], OUTPUT);
        pinMode(enablePins[i], OUTPUT);
 
        // Enable the stepper motor
        digitalWrite(enablePins[i], HIGH); // Assuming HIGH disables the motor
    }

    for (int i = 0; i < NUM_STEPPERS; i++) {
        digitalWrite(enablePins[i], LOW); // Assuming LOW enables the motor

        // Attach the stepper motor to the pins
        steppers[i].connectToPins(stepPins[i], dirPins[i]);
        steppers[i].setStepsPerRevolution(200);
    }

    moveStepper(StepperAxis::Y, 10000, 1000);
    steppers[StepperAxis::Y]
}

// void stepperLoop(){
//     for (int i = 0; i < NUM_STEPPERS; i++) {
//         SpeedyStepper* selectedStepper = &(steppers[i]);
//         if (!selectedStepper->motionComplete())
//         {
//             selectedStepper->processMovement();
//         }
//     }
// }

void moveStepperByIndex(int stepperIndex, int speed, int position, int accel = 2000){
    isMoving[stepperIndex] = true;

    SpeedyStepper* selectedStepper = &(steppers[stepperIndex]);
    selectedStepper->setAccelerationInMillimetersPerSecondPerSecond(accel);
    selectedStepper->setSpeedInStepsPerSecond(speed);

    selectedStepper->setupMoveInMillimeters(position);

    while (!selectedStepper->motionComplete())
    {
        selectedStepper->processMovement();
    }
}
void moveStepper(StepperAxis axis, int speed, int position, int accel = 2000) {moveStepperByIndex(axis, speed, position, accel);}

void stopStepperByIndex(int stepperIndex){
    if (!isMoving[stepperIndex]) return;

    if (!steppers[stepperIndex].motionComplete()) return;
    
    steppers[stepperIndex].setupStop();
    isMoving[stepperIndex] = false;
}
void stopStepper(StepperAxis axis){stopStepperByIndex(axis);}