#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;
  bool direction;
  bool stroke;
  int position;
  int counter;
};

motorStructure motor1;


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

// int accelSpeeds[] = {10,20,30,40,50,50,40,30,20,10};
int accelSpeeds[] = {100,90,80,70,60,50,40,30,20,10};

int initSpeed = accelSpeeds[0];
int initPosition = accelStep;
int initCounter = 0;

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

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



motorStructure function(AccelStepper &stepperMotor, struct motorStructure x){

  int position = x.position;
  int counter = x.counter;

  Serial.println(x.direction);

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

    // Start First Motion as its own Action
    if(x.start){
      setPositionSpeed(stepperMotor, x.position, 0);
      stepperMotor.runSpeedToPosition();
      x.start = false;
    }
    else{
      // Check to See if Motor is at End of Motion
      if(stepperMotor.distanceToGo() == 0){
        
        x = checkStroke(x);                        // Check if Stroke Direction Needs to Change
        x = updateSettings(position, counter, x);  // Different Stroke Directions of Motion
        setPositionSpeed(stepperMotor, x.position, x.counter);
      }  
      stepperMotor.runSpeedToPosition();
    }
  }
  return x;
}

motorStructure checkStroke(struct motorStructure x){
  if(abs(x.position) >= totalSteps){
    x.stroke = false;}
  else if(x.position == 0){
    x.stroke = true;}
  return x;
}

motorStructure updateSettings(int position, int counter, struct motorStructure x){

  if(x.stroke){

    if(position == 0){
      x.direction = !x.direction;}

    // Increment Position
    if(x.direction){
      x.position += accelStep;}
    else if(!x.direction){
      x.position -= accelStep;}
    
    // Increment or Reset Counter
    if(position == 0){
      x.counter = 0;}
    else{
      x.counter = ++counter;}
  }
  else if(!x.stroke){
    // Decrement Position
    if(x.direction){
      x.position -= accelStep;}
    else if(!x.direction){
      x.position += accelStep;}

    // Increment or Reset Counter
    if(abs(position) >= totalSteps){
      x.counter = numInstructions-1;}
    else{
      x.counter = --counter;}
  }
  return x;
}

void setPositionSpeed(AccelStepper &stepperMotor, int position, int counter){
  stepperMotor.moveTo(position);
  stepperMotor.setSpeed(accelSpeeds[counter]);
}

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

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