#include <ContinuousStepper.h>
//#include <timeObj.h>
#include <mechButton.h>
#include <mapper.h>
#include <autoPOT.h>

#define stepPin     3
#define dirPin      2
#define CW_PIN      10
#define CCW_PIN     11
#define RAPID_PIN   12
#define SPEED_PIN   A0
#define ENABLE_PIN  4
#define RESET_PIN   5

//#define PRINT_MS    1000
#define MAX_SPEED   1000

ContinuousStepper/*<StepperDriver>*/  stepper;
//timeObj              printTimer(PRINT_MS);
mechButton           CWBtn(CW_PIN);
mechButton           CCWBtn(CCW_PIN);
mechButton           RapidBtn(RAPID_PIN);
autoPOT              speedPOT(SPEED_PIN);
mapper               speedMap(25,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);
  pinMode(ENABLE_PIN, OUTPUT);
  pinMode(RESET_PIN, OUTPUT);
//  Serial.println("The sketch has started");
  stepper.begin(stepPin, dirPin);
  stepper.setAcceleration(2000);
  stepper.spin(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  : 
    if (!RapidBtn.getState()) {  // Switch grounded.
       digitalWrite(RESET_PIN, HIGH);
    }else{
       digitalWrite(RESET_PIN, LOW);
     }
    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.spin(moveSpeed); break;
    case moveCCW  : stepper.spin(-moveSpeed); break;
    case rapidCW  : // These two do nothing.
    case rapidCCW : break;
  }
}


void comMoveCW(void) {

  stepper.spin(-moveSpeed);
  digitalWrite(ENABLE_PIN, LOW);
  ourState = moveCW;
}


void comMoveCCW(void) {

  stepper.spin(moveSpeed);
  digitalWrite(ENABLE_PIN, LOW);
  ourState = moveCCW;
}


void comRapidCW(void) {

  stepper.spin(-MAX_SPEED);
  ourState = rapidCW;
}


void comRapidCCW(void) {

  stepper.spin(MAX_SPEED);
  ourState = rapidCCW;
}


void comStop(void) {

  stepper.spin(0);
  digitalWrite(ENABLE_PIN, HIGH);
  ourState = waiting;
}



void loop() {

  idle();
  stepper.loop();
// 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('\n');
//    printTimer.start();
//  }
}