#include <AccelStepper.h>

int maxSteps = 200;
int stepPin = 3;
int dirPin = 2;
const int maxSpeed = 1000;
const int acceleration = 1000;

AccelStepper stepper1(1, stepPin, dirPin);

struct motorStructure{
  bool main;
  bool start;
  int speed;
  bool direction;
  int position;
  int counter;
};

motorStructure motor1;


// Prescribe Discretized Acceleration
int totalSteps = 100;
int numInstructions = 10;
int accelStep = totalSteps / numInstructions;

int accelSpeeds[] = {10,20,30,40,50,50,40,30,20,10};

int initSpeed = accelSpeeds[0];
int initPosition = accelStep;
bool initDirection = true;
int initCounter = 0;

void setup() {
  Serial.begin(9600);
  motorInit(stepper1);
  motor1 = {true, true, initSpeed, initDirection, initPosition, initCounter};
}

void loop() {
  motor1 = function(stepper1, motor1);
}

motorStructure function(AccelStepper &stepperMotor, struct motorStructure x){
  bool main = x.main;
  bool start = x.start;
  int speed = x.speed;
  bool direction = x.direction;
  int position = x.position;
  int counter = x.counter;

  Serial.println(counter);

  // Only Execute if 'main' boolean is triggered
  if(main){

    // Start First Motion as its own Action
    if(start){
      stepperMotor.moveTo(position);
      stepperMotor.setSpeed(speed);
      stepperMotor.runSpeedToPosition();
      x.start = false;
    }
    else{
      // Check to See if Motor is at End of Motion Before Re-Assigning New Position
      // [non-blocking workaround]
      if(stepperMotor.distanceToGo() == 0){
        
        // Check if Motor Needs to Turn Around
        if(position >= totalSteps){
          x.direction = false;
        }
        else if(position <= 0){
          x.direction = true;
        }
        direction = x.direction;

        // Different Directions of Motion
        if(direction){
          // Increment Position
          x.position += accelStep;
          // Increment or Reset Counter
          if(position <= 0){
            x.counter = 0;
          }
          else{
            x.counter = ++counter;
          }
        }
        else if(!direction){
          // Decrement Position
          x.position -= accelStep;
          // Increment or Reset Counter
          if(position >= totalSteps){
            x.counter = numInstructions-1;
          }
          else{
            x.counter = --counter;
          }
        }
        
        // Set New Position and Speed
        position = x.position;
        speed = accelSpeeds[x.counter];
        x.speed = speed;

        stepperMotor.moveTo(position);
        stepperMotor.setSpeed(speed);
      }  
      stepperMotor.runSpeedToPosition();
    }
  }
  return x;
}


void motorInit(AccelStepper &stepperMotor){
  stepperMotor.setMaxSpeed(maxSpeed);
  stepperMotor.setAcceleration(acceleration);
}


// motorStructure function(AccelStepper &stepperMotor, struct motorStructure x){
//   bool start = x.start;
//   bool direction = x.direction;

//   if(start){
//     int directionSign = plusMinus(direction);
//     stepperMotor.setSpeed(speed);
//     stepperMotor.moveTo(directionSign * waveSteps);
//     stepperMotor.runSpeedToPosition();
//     x.start = false;
    
//   }
//   else{
//     if(stepperMotor.distanceToGo() == 0){
//       direction = !direction;
//       x.direction = direction;
//       int directionSign = plusMinus(direction);
//       stepperMotor.moveTo(directionSign * waveSteps);
//     }
//     stepperMotor.setSpeed(speed);
//     stepperMotor.runSpeedToPosition();
//   }
//   return x;
// }

// int plusMinus(bool direction){
//   if(direction){
//     return +1;
//   }
//   else{
//     return -1;
//   }
// }