#include <AccelStepper.h>
#include <timeObj.h>
#include <mechButton.h>
#include <mapper.h>
#include <autoPOT.h>

#define DIR_PIN     2
#define STEP_PIN    3
#define CW_PIN      10
#define CCW_PIN     11
#define RAPID_PIN   12
#define SPEED_PIN   A0

#define PRINT_MS    1000
#define MAX_SPEED   1000


AccelStepper  stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
timeObj       printTimer(PRINT_MS);
mechButton    CWBtn(CW_PIN);
mechButton    CCWBtn(CCW_PIN);
mechButton    RapidBtn(RAPID_PIN);
autoPOT       speedPOT(SPEED_PIN);
mapper        speedMap(0,1023,0,MAX_SPEED);
enum states   { waiting, moveCW, rapidCW, moveCCW, rapidCCW };
states        ourState;
float         moveSpeed; 

void setup() {

  Serial.begin(115200);
  CWBtn.setCallback(CWClick);
  CCWBtn.setCallback(CCWClick);
  RapidBtn.setCallback(RapidClk);
  speedPOT.setCallback(newSpeed);
  Serial.println("The sketch has started");
  stepper.setMaxSpeed(MAX_SPEED); 
  stepper.setSpeed(0);
  ourState = waiting;
}


void CWClick(void) {

  switch(ourState) {
    case waiting  : 
      if (!CWBtn.getState()) {          // Switch grounded..
        if (CCWBtn.getState()) {        // CCW is NOT grounded..
          comMoveCW();
        }
      } else if (!CCWBtn.getState()) {  // CCW is grounded..
        comMoveCCW();                   // We go the other way!
      }
    break;
    case moveCW   : // Any of these, means stop. 
    case rapidCW  : 
    case moveCCW  :
    case rapidCCW : comStop(); break;
  }
}


void CCWClick(void) {

  switch(ourState) {
    case waiting  : 
      if (!CCWBtn.getState()) {       // Switch grounded..
        if (CWBtn.getState()) {       // CW is NOT grounded..
          comMoveCCW();
        }
      } else if (!CWBtn.getState()) { // CW is grounded..
        comMoveCW();                  // We go the other way!
      }
    break;
    case moveCW   :  // Any of these, means stop.  
    case rapidCW  :
    case moveCCW  :
    case rapidCCW : comStop(); break;
  }
}


void RapidClk(void) {
  
  switch(ourState) {
    case waiting  : break;
    case moveCW   : 
      if (!RapidBtn.getState()) {  // Switch grounded.
        comRapidCW();                 // Run up to rapid speed.
      }
    break;
    case rapidCW  :
      if (RapidBtn.getState()) {   // Switch released.
        comMoveCW();                  // Slow down to move speed.
      }
    break;
    case moveCCW  : 
      if (!RapidBtn.getState()) {  // Switch grounded.
        comRapidCCW();                // Run up to rapid speed.      
      }
    break;
    case rapidCCW :
      if (RapidBtn.getState()) {   // Switch released.
        comMoveCCW();                 // Slow down to move speed.
      }
    break;
  }
}


void newSpeed(int newValue) {
  

  moveSpeed = speedMap.map(newValue);

  switch(ourState) {
    case waiting  : break;
    case moveCW   : stepper.setSpeed(moveSpeed); break;
    case moveCCW  : stepper.setSpeed(-moveSpeed); break;
    case rapidCW  : // These two do nothing.
    case rapidCCW : break;
  }
}


void comMoveCW(void) {

  stepper.setSpeed(moveSpeed);
  ourState = moveCW;
}


void comMoveCCW(void) {

  stepper.setSpeed(-moveSpeed);
  ourState = moveCCW;
}


void comRapidCW(void) {

  stepper.setSpeed(MAX_SPEED);
  ourState = rapidCW;
}


void comRapidCCW(void) {

  stepper.setSpeed(-MAX_SPEED);
  ourState = rapidCCW;
}


void comStop(void) {

  stepper.setSpeed(0);
  ourState = waiting;
}


void loop() {

  idle();
  stepper.runSpeed();
  if (printTimer.ding()) {
    switch(ourState) {
      case waiting  : Serial.print("waiting   ");  break;
      case moveCW   : Serial.print("Move CW   ");  break;
      case moveCCW  : Serial.print("Move CCW  ");  break;
      case rapidCW  : Serial.print("Rapid CW  ");  break;
      case rapidCCW : Serial.print("Rapid CCW ");  break;
    }
    Serial.print("Speed : ");
    Serial.print(stepper.speed());
    Serial.print("  Pos : ");
    Serial.println(stepper.currentPosition());
    printTimer.start();
  }
}