#include "FastAccelStepper.h"
#define dirPinStepper 7
#define enablePinStepper 9
#define stepPinStepper 8

FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *stepper = NULL;


const int SW_PINS[] = {6,5,4,3,2};
// for debug
const char NAMES[5][16]  = {
    {"Upper Limit sw1"},
    {"Lower Limit sw2"},
    {"Stop sw3"},
  {"Go up sw4"},
  {"Go down sw5"}
};

int speed = 10000000000;
int swState[5];
int oldSwState[5];
bool isStop = false;
bool lastDir = false;

// function returns which button was triggered, or 0 if none
int checkSwitches()  {
  int sensNum = 0;
  for (int i = 0; i < 5; i++) {
    // check each switch
    swState[i] = digitalRead(SW_PINS[i]);
    if (swState[i] != oldSwState[i])  { // if it changed
      oldSwState[i] = swState[i];       // remember state for next time
      if (swState[i] == LOW)  {         // was just pressed
        sensNum = i + 1;
        //Serial.print("Switch ");
        //Serial.print(i + 1);
        //Serial.println(" triggered");
        Serial.println(NAMES[i]); // debug
      }
      delay(200);  // debounce
    }
  }
  return sensNum;
}

void setup() {
  engine.init();
  stepper = engine.stepperConnectToPin(stepPinStepper);
  if (stepper) {
    stepper->setDirectionPin(dirPinStepper);
    stepper->setEnablePin(enablePinStepper);
    stepper->setAutoEnable(true);
    // If auto enable/disable need delays, just add (one or both):
    // stepper->setDelayToEnable(50);
    // stepper->setDelayToDisable(1000);
    stepper->setSpeedInUs(500);  // the parameter is us/step !!!
    stepper->setAcceleration(50);
    stepper->move(25);
  }


  for (int i = 0; i < 5; i++) {
    pinMode(SW_PINS[i], INPUT_PULLUP);
  }
  Serial.begin(9600);
  Serial.println("Initializing");
  delay(2000);
 
  
}

void loop() {
  int swNum = checkSwitches();
  if (swNum)  {
    if (swNum == 1 && !isStop)  {
      // upper limit motor should stop
      isStop = true;
      lastDir = true;
    } else if (swNum == 2 && !isStop) {
      // lower limit motor should stop
      isStop = true;
      lastDir = false;
    } else if (swNum == 3)  {
      // e-stop / resume
      isStop = !isStop;
      if (!isStop) {
        stepper->move(speed);
      }
    } else if (swNum == 4 && !lastDir)  {
      // up
      isStop = false;
      lastDir = true;
      stepper->move(speed);
    } else if (swNum == 5 && lastDir) {
      // down
      isStop = false;
      lastDir = false;
      stepper->move(-speed);
    } else  {
      Serial.print("Exception: sw");
      Serial.println(swNum);
    }
  }

  if (!isStop) {
    // If motor is running, ensure it's moving at the correct speed
    if (!stepper->isRunning()) {
      stepper->move(speed);
    }
  } else {
    while (stepper->isRunning()) {
      // Do nothing, just wait
    }
  }
}