// Forum: https://forum.arduino.cc/t/using-accelstepper-to-execute-premade-set-of-movements/1248919
// Sketch by r3dex3
// At this moment, this is the sketch of post #10 from the forum.
// This Wokwi project: https://wokwi.com/projects/395587471345920001
// 

#include <AccelStepper.h>

AccelStepper StepperR(AccelStepper::DRIVER, 9, 6);
AccelStepper StepperL(AccelStepper::DRIVER, 3, 2);

const int halfTime = 10000;
const int fTime = 10000;
const int bTime = 10000;
const int rTime = 10000;
const int lTime = 10000;

int currentAction = 0;
unsigned long startTime;
bool actionComplete = false;
unsigned long lastPrintTime = 0;
const unsigned long printInterval = 1000;

const int numberOfMoveTos = 7;
int test[numberOfMoveTos] = {1,1,1,1,1,1,1};

int stepsPerRev = 400;
float revPerSq = (50/2*PI*3.81);

void setup() {

  Serial.begin(115200);

  StepperR.setMaxSpeed(200);
  StepperL.setMaxSpeed(200);

  StepperR.setAcceleration(10);
  StepperL.setAcceleration(10);

  startTime = millis();
  lastPrintTime = millis();

  pinMode(13, OUTPUT);
}

void loop() {
  unsigned long currentTime = millis();
  unsigned long elapsedTime = currentTime - startTime;

  switch (test[currentAction]) 
  {  
    case 1:
      {
        if (!actionComplete) 
        {
          startTime = millis();
          long desiredPositionR = (StepperR.currentPosition() + (stepsPerRev*revPerSq));
          long desiredPositionL = (StepperL.currentPosition() - (stepsPerRev*revPerSq));
          StepperR.moveTo(desiredPositionR);
          StepperL.moveTo(desiredPositionL);
          actionComplete = true;
        }
        else if (currentTime - startTime >= fTime) 
        {
          Serial.println("Forward");
          actionComplete = false;
          currentAction++;
        }
        break;
      }
    case 2:
      {
        if (!actionComplete) 
        {
          startTime = millis();
          long desiredPositionR = (StepperR.currentPosition() - (stepsPerRev*revPerSq));
          long desiredPositionL = (StepperL.currentPosition() + (stepsPerRev*revPerSq));
          StepperR.moveTo(desiredPositionR);
          StepperL.moveTo(desiredPositionL);
          actionComplete = true;
        }
        else if (currentTime - startTime >= bTime) 
        {
          Serial.println("Backward");
          actionComplete = false;
          currentAction++;
        }
        break;
      }

    case 3:
      {
        if (!actionComplete) 
        {
          startTime = millis();
          long desiredPositionR = (StepperR.currentPosition() + (stepsPerRev));
          long desiredPositionL = (StepperL.currentPosition() + (stepsPerRev));
          StepperR.moveTo(desiredPositionR);
          StepperL.moveTo(desiredPositionL);
          actionComplete = true;
        }
        else if (currentTime - startTime >= lTime) 
        {
          Serial.println("Left");
          actionComplete = false;
          currentAction++;
        }
        break;
      }

    case 4:
      {
        if (!actionComplete) 
        {
          startTime = millis();
          long desiredPositionR = (StepperR.currentPosition() - (stepsPerRev));
          long desiredPositionL = (StepperL.currentPosition() - (stepsPerRev));
          StepperR.moveTo(desiredPositionR);
          StepperL.moveTo(desiredPositionL);
          actionComplete = true;
        }
        else if (currentTime - startTime >= rTime) 
        {
          Serial.println("Right");
          actionComplete = false;
          currentAction++;
        }
        break;
      }

    case 5:
      {
        if (!actionComplete) 
        {
          startTime = millis();
          long desiredPositionR = (StepperR.currentPosition() - (0.5*stepsPerRev*revPerSq));
          long desiredPositionL = (StepperL.currentPosition() + (0.5*stepsPerRev*revPerSq));
          StepperR.moveTo(desiredPositionR);
          StepperL.moveTo(desiredPositionL);
          actionComplete = true;
        }
        else if (currentTime - startTime >= halfTime) 
        {
          Serial.println("halfForward");
          actionComplete = false;
          currentAction++;
        }
        break;
      }

    default:
      {
        digitalWrite(13, HIGH); //indicates an error (or the end of the array?)
        StepperR.stop();
        StepperL.stop();
        delay(10000);
        break;
      }
  }

  StepperR.run();
  StepperL.run();
}