#include <AccelStepper.h>

// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver
#define motorInterfaceType 1
#define dirPin 2
#define stepPin 3

// Stepper motor speed configuration
#define microStepsFactor 4
#define maxStepperSpeed 4000
#define inversionPause 50

// Create a new instance of the AccelStepper class:
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);

// Runtime data
int currentPosition[] = {-1, -1, -1};
int nextPosition[] = {0, 0, 0};
int stepperPosition = 0;

void setup() {
  Serial.begin(115200);
  
  // Wait for zeroing
  Serial.println("Please rotate to zero position and press Enter");
  while (Serial.available() == 0) {}
  Serial.flush();
  
  // Set the current position to 0:
  stepper.setCurrentPosition(0);
  // Set the maximum speed in steps per second:
  stepper.setMaxSpeed(maxStepperSpeed);
  stepper.setAcceleration(40000);

  // Rotate to 0,0,0
  setPosition();
}

void loop() { 

  incrementCombination();
  setPosition();

  delay(inversionPause);
}

void incrementCombination() {
  if (currentPosition[0] < 99) {
    nextPosition[0] = currentPosition[0] + 1;
    nextPosition[1] = currentPosition[1];
    nextPosition[2] = currentPosition[2];
  } else {
    nextPosition[0] = 0;
    if (currentPosition[1] < 99) {
      nextPosition[1] = currentPosition[1] + 1;
      nextPosition[2] = currentPosition[2];
    } else {
      nextPosition[1] = 0;
      if (currentPosition[2] < 99) {
        nextPosition[2] = currentPosition[2] + 1;
      } else {
        nextPosition[2] = 0;        
      }
    }
  }
}

void setPosition() {

  Serial.print("Testing ");
  Serial.print(currentPosition[0]);
  Serial.print(",");
  Serial.print(currentPosition[1]);
  Serial.print(",");
  Serial.println(currentPosition[2]);

  if (currentPosition[2] != nextPosition[2]) {
    rotateCcwTo(nextPosition[2], false);
    rotateCcwTo(nextPosition[2], true);
    rotateCcwTo(nextPosition[2], true);
    rotateCcwTo(nextPosition[2], true);
    currentPosition[1] = -1;
  }

  if (currentPosition[1] != nextPosition[1]) {
    rotateCwTo(nextPosition[1], false);
    rotateCwTo(nextPosition[1], true);
    rotateCwTo(nextPosition[1], true);
    currentPosition[0] = -1;
  }

  if (currentPosition[0] != nextPosition[0]) {
    rotateCcwTo(nextPosition[0], false);
    rotateCcwTo(nextPosition[0], true);
  }

  if (stepperPosition <= 10) {
    rotateCwTo(0, true);
  }
  rotateCwTo(10, false);

  currentPosition[0] = nextPosition[0];
  currentPosition[1] = nextPosition[1];
  currentPosition[2] = nextPosition[2];
}

void rotateCwTo(int pos, boolean forceFullRotation) {
  Serial.print("CW to ");
  Serial.println(pos);
  if (stepperPosition == pos && forceFullRotation) {
    // make sure we actually move one whole turn
    stepper.move(-200 * microStepsFactor);
    stepper.runToPosition();
  } else {
    stepper.runToNewPosition(-2 * pos * microStepsFactor);
    stepperPosition = pos;
  }
}

void rotateCcwTo(int pos, boolean forceFullRotation) {
  Serial.print("CCW to ");
  Serial.println(pos);
  if (stepperPosition == pos && forceFullRotation) {
    // make sure we actually move one whole turn
    stepper.move(200 * microStepsFactor);
    stepper.runToPosition();
  } else {
    stepper.runToNewPosition(2 * pos * microStepsFactor);
    stepperPosition = pos;
  }
}