#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 1000 * microStepsFactor
#define inversionPause 20

// 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};

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);

  // 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 (nextPosition[2] != nextPosition[2]) {
    rotateCcwTo(nextPosition[2]);
    rotateCcwTo(nextPosition[2]);
    rotateCcwTo(nextPosition[2]);
  }

  if (nextPosition[1] != nextPosition[1]) {
    rotateCwTo(nextPosition[1]);
    rotateCwTo(nextPosition[1]);
  }

  if (nextPosition[0] != nextPosition[0]) {
    rotateCcwTo(nextPosition[0]);
  }

  rotateCwTo(0);
  rotateCwTo(10);

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

void rotateCwTo(int pos) {
  stepper.move(1);
  stepper.runToNewPosition(2 * pos * microStepsFactor);
}

void rotateCcwTo(int pos) {
  stepper.move(-1);
  stepper.runToNewPosition(-2 * pos * microStepsFactor);
}