
// Include the AccelStepper library:
#include <AccelStepper.h>

// Define stepper motor connections and motor interface type. Motor interface type must be set to 1 when using a driver:

// Set stepper 1 pins
#define xLimitSwitchPosPin 2
#define xLimitSwitchNegPin 3
#define xDirPin 4
#define xStepPin 5
#define xPowerPin 6

// Set stepper 2 pins
#define yLimitSwitchPosPin 9
#define yLimitSwitchNegPin 10
#define yDirPin 11
#define yStepPin 12
#define yPowerPin 13

#define motorInterfaceType 1

// Create new instances of the AccelStepper class:
AccelStepper xStepper = AccelStepper(motorInterfaceType, xStepPin, xDirPin);
AccelStepper yStepper = AccelStepper(motorInterfaceType, yStepPin, yDirPin);

unsigned long previousMillis = 0;
unsigned long currentMillis = 0;
bool printMode = false;

// safeMode means that the limit has been hit
bool xSafeMode = false;
long xLimitPos = 0;

bool ySafeMode = false;
long yLimitPos = 0;

int microsteps = 1;
float maxSpeedConstant = 350 * microsteps;
float accelerationConstant = 100 * microsteps;

void setup() {

  pinMode(xPowerPin, OUTPUT);
  pinMode(xLimitSwitchNegPin, INPUT);
  pinMode(xLimitSwitchPosPin, INPUT);

  pinMode(yPowerPin, OUTPUT);
  pinMode(yLimitSwitchNegPin, INPUT);
  pinMode(yLimitSwitchPosPin, INPUT);

  Serial.begin(115200);

  xStepper.setMaxSpeed(maxSpeedConstant);
  xStepper.setAcceleration(accelerationConstant);
  //xStepper.setCurrentPosition(0);

  yStepper.setMaxSpeed(maxSpeedConstant);
  yStepper.setAcceleration(accelerationConstant);
  //yStepper.setCurrentPosition(0);
}

void stepperLogic(int stepperIndex, bool printMode){

  // init vars
  AccelStepper *stepper;
  int powerPin;
  int limitSwitchNeg;
  int limitSwitchPos;
  bool *safeMode;
  long *limitPos;

  // set vars based on stepper index
  if(stepperIndex == 1){
    stepper = &xStepper;
    powerPin = xPowerPin;
    limitSwitchNeg = digitalRead(xLimitSwitchNegPin);
    limitSwitchPos = digitalRead(xLimitSwitchPosPin);
    safeMode = &xSafeMode;
    limitPos = &xLimitPos;
  } else {
    stepper = &yStepper;
    powerPin = yPowerPin;
    limitSwitchNeg = digitalRead(yLimitSwitchNegPin);
    limitSwitchPos = digitalRead(yLimitSwitchPosPin);
    safeMode = &ySafeMode;
    limitPos = &yLimitPos;
  }

  // stepper logic
  if (!*safeMode && (limitSwitchNeg == limitSwitchPos)){
    // just keep swimming
    digitalWrite(powerPin, LOW);
    stepper->run();
    //runLogic(stepperIndex, stepperFlip, stepper, last, distance, dir);

  } else if (!*safeMode && (limitSwitchNeg != limitSwitchPos)) {
    // limit hit; go into safeMode; power down
    *safeMode = true;
    *limitPos = stepper->currentPosition();
    stepper->setSpeed(0);
    stepper->moveTo(stepper->currentPosition());
    digitalWrite(powerPin, HIGH);
    Serial.println("!! limit hit, move the other direction");

  } else if (*safeMode && (abs(*limitPos - stepper->currentPosition()) < (1000 * microsteps))) {
    // just keep swimming for a while (only in the other direction), hoping to get out of safeMode within 1000 steps
    if((limitSwitchNeg == 0 && (stepper->currentPosition() > stepper->targetPosition())) || 
      (limitSwitchPos == 0 && (stepper->currentPosition() < stepper->targetPosition()))) {
      *safeMode = (limitSwitchNeg != limitSwitchPos);
      digitalWrite(powerPin, LOW);
      stepper->run();
      }

  } else if (stepper->distanceToGo() == 0) {
    // destination reached; power down
    digitalWrite(powerPin, HIGH);

  } else {
    // houston we have a problem; safeMode is still on after 10 steps from limitPos
    stepper->setSpeed(0);
    stepper->moveTo(stepper->currentPosition());
    digitalWrite(powerPin, HIGH);
    Serial.println("!!! limit switch still on after 1000 steps, this should NEVER happen");
  };

  if (printMode) {
    //Serial.println("[" + String(stepperIndex) + ", " + String(stepper->currentPosition()) + ", " + String(stepper->targetPosition()) + ", " +
    //String(limitSwitchNeg) + ", " + String(limitSwitchPos) + ", " + String(*safeMode) + ", " + String(*limitPos) + "]");

    Serial.println("[" + String(xStepper.currentPosition()) + ", " + String(yStepper.currentPosition()) + ", " +
    String(limitSwitchNeg) + ", " + String(limitSwitchPos) + "]");
    
    /*
    if (stepperIndex == 1){
      Serial.println("------Stepper 1------");
    } else {
      Serial.println("------Stepper 2------");
    };
    Serial.print("curPos:         ");
    Serial.println(stepper->currentPosition());
    Serial.print("tarPos:         ");
    Serial.println(stepper->targetPosition());
    Serial.print("limitSwitchNeg: ");
    Serial.println(limitSwitchNeg);
    Serial.print("limitSwitchPos: ");
    Serial.println(limitSwitchPos);
    Serial.print("safeMode: ");
    Serial.println(*safeMode);
    Serial.print("limitPos: ");
    Serial.println(*limitPos);
    Serial.print("speed: ");
    Serial.println(stepper->speed());
    Serial.println("");
    */
    
    
  }

}


long parseDest(char delimiter){
  long integerValue = 0;         // throw away previous integerValue
  bool negativeNumber = false;  // reset for negative
  char incomingByte;

  while(1) {                    // force into a loop until delimiter is received
    incomingByte = Serial.read();
    /*
     if (incomingByte == 'c') {
      xStepper.setCurrentPosition(0);
      yStepper.setCurrentPosition(0);
      continue;
    }
    */
    if (incomingByte == delimiter) break;   // exit the while(1), we're done receiving
    if (incomingByte == -1) continue;  // if no characters are in the buffer read() returns -1
    if (incomingByte == '-') {
      negativeNumber = true;
      continue;
    }
    integerValue *= 10;          // shift left 1 decimal place
    integerValue = ((incomingByte - 48) + integerValue);   // convert ASCII to integer, add, and shift left 1 decimal place
  }

  if (negativeNumber)
    integerValue = -integerValue;

  //integerValue = -integerValue; // this makes up for the fact that things are backwards
  return integerValue;
}


void loop() {

  currentMillis = millis();
  printMode = false;

  if (currentMillis - previousMillis >= 100 == true ) {
    printMode = true;
    previousMillis = currentMillis;
  }

  stepperLogic(1, printMode);
  stepperLogic(2, printMode);

  if (Serial.available() > 0) {   // something came across serial

    xStepper.moveTo(parseDest(' '));
    yStepper.moveTo(parseDest('\n'));

  };

  //delay(100);
}
