////////////////////////////////////////////////////////////////////////////////////////////////////
// Sketch for a driving two steppers in sync with either graphical user interface or 
// simple LED-based interface.
////////////////////////////////////////////////////////////////////////////////////////////////////

#include <SPI.h>
#include <AccelStepper.h>

// Remove the following line to use LEDs instead.
#define __USE_TFT__ 
// Remove the following line to use full steps.
#define __USE_HALF_STEPS__ 

#ifdef __USE_TFT__
  #include <Adafruit_GFX.h>    
  #include <Adafruit_ST7735.h> 
  // Colors: BBBB | BGGG | GGGR | RRRR
  #define MINI_TFT_ORAN  0x441F
  #define MINI_TFT_GREEN 0x03E0 
  #define MINI_TFT_BLUE  ST7735_RED
  #define MINI_TFT_RED   ST7735_BLUE
  #define MINI_TFT_WHITE ST7735_WHITE
  #define MINI_TFT_BLACK ST7735_BLACK
  #define MINI_TFT_GRAY  0x2222
  // TFT Pins.
  #define TFT_DC         17 // A3
  #define TFT_CS         18 // A4
  #define TFT_RST        -1 //-1: Connected to Arduino RESET pin.
  // Connect Data ("SDA") to pin 11 and clock ("CLK" or "SCL") to pin 13.
  Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);
  #define NUM_MENUS      6
  const char* menuTitles[] = {"Execute", "F - Start", "R - Start", "F - End", "R - End", "Speed"};
  #define MENU_RGN_TITLE 0
  #define MENU_RGN_TEXT  1 
#else // use LEDs.
  #define AXIS_SELECT_LED_PIN 10
  #define MODE_SELECT_LED_PIN 11
  #define NUM_MENUS      5
  #define MAX_LED_LEVEL  160
  int lightLevel = 0;
  int lightInc = 1;
  int lightIncSign = 1;
#endif // __USE_TFT__

#define POS_START      0
#define POS_END        1
#define MAX_SPEED      1000
#define MAX_ACCEL      1000

#define MM_PER_ROT    40      // 40 mm/rot when using a 2GT 20 teeth pulley - 36 mm/rot when using a 18 teeth pulley.
#ifdef __USE_HALF_STEPS__ 
  #define STEPS_PER_ROT 4076  // For 28-BYJ48 according to most sources, half steps.
#else
  #define STEPS_PER_ROT 2038  // For 28-BYJ48 according to most sources, full steps.
#endif

AccelStepper* stepper[2];
long axisPos[2][2] = {{0, 0}, {0, 0}};
long machineSpeed = 680;
int selectedMenu = 0;
int buttonStatusOld[3] = {0, 0, 0};
int buttonStatus[3] = {0, 0, 0};;
bool machineIsMoving = false;

void executeMotion(int direction);
void moveAxisManually(int axis, long position, int direction);

////////////////////////////////////////////////////////////////////////////////////////////////////
// Initialize the conrtoller.
////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
  int i;

#ifndef __USE_TFT__
  // Change speed by poti, not by menu; + 1 for avoiding freezes.
  machineSpeed = (long)analogRead(17) * MAX_SPEED / 1023 + 1; 
#endif
  // Initialize the internal pull-up resistors for the push buttons.
  pinMode(14, INPUT_PULLUP); // Left button. (A0 = 14 ... A2 = 16)
  pinMode(15, INPUT_PULLUP); // Middle button.
  pinMode(16, INPUT_PULLUP); // Right button.

  // Initialize axes.
#ifdef __USE_HALF_STEPS__ 
  stepper[0] = new AccelStepper(AccelStepper::HALF4WIRE, 2, 4, 3, 5); // On ULN2003: Pin1, Pin3, Pin2, Pin4.
  stepper[1] = new AccelStepper(AccelStepper::HALF4WIRE, 6, 8, 7, 9); // On ULN2003: Pin1, Pin3, Pin2, Pin4.
#else
  stepper[0] = new AccelStepper(AccelStepper::FULL4WIRE, 2, 4, 3, 5); // On ULN2003: Pin1, Pin3, Pin2, Pin4.
  stepper[1] = new AccelStepper(AccelStepper::FULL4WIRE, 6, 8, 7, 9); // On ULN2003: Pin1, Pin3, Pin2, Pin4.
#endif
  stepper[0]->setCurrentPosition(0);
  stepper[0]->setMaxSpeed(machineSpeed); 
  stepper[0]->setAcceleration(machineSpeed);
  stepper[1]->setCurrentPosition(0);
  stepper[1]->setMaxSpeed(machineSpeed); 
  stepper[1]->setAcceleration(machineSpeed);

#ifdef __USE_TFT__
  // Initialize the display.
  tft.initR(INITR_MINI160x80);
  tft.setRotation(3);
  tft.setTextWrap(false);
  tft.setTextSize(2);
  tft.fillScreen(MINI_TFT_BLACK);

  updateDisplay(MENU_RGN_TITLE);
  updateDisplay(MENU_RGN_TEXT);
#else // use LEDs.
  pinMode(AXIS_SELECT_LED_PIN, OUTPUT);
  pinMode(MODE_SELECT_LED_PIN, OUTPUT);
  digitalWrite(AXIS_SELECT_LED_PIN, LOW);
  digitalWrite(MODE_SELECT_LED_PIN, LOW);
#endif // __USE_TFT__
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Main loop.
////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
  char textBuffer[64];
  int newStatus;
  int i;

#ifdef __USE_TFT__
#else // use LEDs.
  if(selectedMenu == 0){
    // Do "smooth blinking" if the system is in menu 0 (waiting for "execution").
    // First, adjust the blink speed according to the currently selected machine speed.
    lightInc = (analogRead(17) / 100) + 1; // Poti can go up to 1023, so this will go from 1 to 11
    analogWrite(AXIS_SELECT_LED_PIN, lightLevel);
    analogWrite(MODE_SELECT_LED_PIN, lightLevel);
    lightLevel += lightInc*lightIncSign;
    lightLevel = min(lightLevel, MAX_LED_LEVEL);
    lightLevel = max(lightLevel, 0);
    if((lightLevel == MAX_LED_LEVEL)||(lightLevel == 0)){
      lightIncSign *= -1;  
    }
  }
#endif // __USE_TFT__
  // Check whether a button has been pressed.
  for(i = 0; i < 3; i++){
    buttonStatus[i] = !digitalRead(14+i); // 14 == A0; "!" because button will be "HIGH" when _not_ being pressed.
  }
  if(buttonStatus[0] && !buttonStatusOld[0] && !buttonStatus[1]&& !buttonStatus[2]){  // Menu selection button was pressed and all other buttons are not pressed.
    // Increase menu if the selection button is not blocked by an ongoing operation.
    if(!machineIsMoving){
      selectedMenu ++;
      selectedMenu %= NUM_MENUS;
#ifdef __USE_TFT__
      updateDisplay(MENU_RGN_TITLE);
      updateDisplay(MENU_RGN_TEXT);
#else // use LEDs.
      // When using LEDs, switch them on/off here.
      switch(selectedMenu){
        case 1: // Axis 1 - Start Position.
          digitalWrite(AXIS_SELECT_LED_PIN, LOW);
          digitalWrite(MODE_SELECT_LED_PIN, LOW);
          break;
        case 2: // Axis 2 - Start Position.
          digitalWrite(AXIS_SELECT_LED_PIN, HIGH);
          digitalWrite(MODE_SELECT_LED_PIN, LOW);
          break;
        case 3: // Axis 1 - End Position.
          digitalWrite(AXIS_SELECT_LED_PIN, LOW);
          digitalWrite(MODE_SELECT_LED_PIN, HIGH);
          break;
        case 4: // Axis 2 - End Position.
          digitalWrite(AXIS_SELECT_LED_PIN, HIGH);
          digitalWrite(MODE_SELECT_LED_PIN, HIGH);
          break;
      }
#endif // __USE_TFT__
      goto FINALIZE_BUTTONS;
    }
  }
  // Check middle and right button.
  for(i = 1; i <= 2; i++){
    if(buttonStatus[i] != buttonStatusOld[i]){  // Go left/right or stop motion.
      if(buttonStatus[i] == 1){  // Button was pressed.
        switch(selectedMenu){
          case 0:
            executeMotion(i);
            break;
          case 1:
            // Move the selected axis forwards/backwards as long as the button is pressed.
            moveAxisManually(0, POS_START, i);
            break;
          case 2:
            moveAxisManually(1, POS_START, i);
            break;
          case 3:
            moveAxisManually(0, POS_END, i);
            break;
          case 4:
            moveAxisManually(1, POS_END, i);
            break;
#ifdef __USE_TFT__
          case 5:
            // Decrease / Increase speed while the button is pressed.
            changeSpeed(i);
            break;
#endif // __USE_TFT__
        }
      }
    }
  }
FINALIZE_BUTTONS:    
  for(i = 0; i < 3; i++){
    buttonStatusOld[i] = buttonStatus[i];
  }
  delay(10);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Move both motors in one stroke to start (middle / yellow button) or 
// end (green / right button) position while monitoring the 
// left (red) button for an emergency stop.
////////////////////////////////////////////////////////////////////////////////////////////////////
void executeMotion(int direction){
  double distance[2];
  double target[2];
  int i;

  target[0] = axisPos[0][direction - 1];
  target[1] = axisPos[1][direction - 1];
  // Calculate relative speed, so both drives will (approximately) stop at the same time.
  distance[0] = abs(target[0] - stepper[0]->currentPosition());
  distance[1] = abs(target[1] - stepper[1]->currentPosition());
  // The axis with the longer travel distance should use the currently defined machineSpeed.
  // The other axis must run slower. Use ceil to make sure that speed is never zero if the distance to go is very small but not zero.
#ifndef __USE_TFT__
  // Change speed by poti, not by menu; + 1 for avoiding freezes.
  machineSpeed = (long)analogRead(17) * MAX_SPEED / 1023 + 1; 
#endif
  if(distance[0] >= distance[1]){
    stepper[0]->setMaxSpeed(machineSpeed);    
    stepper[0]->setAcceleration(machineSpeed);
    stepper[1]->setMaxSpeed((float)(ceil(machineSpeed * distance[1]/distance[0])));    
    stepper[1]->setAcceleration((float)(ceil(machineSpeed * distance[1]/distance[0])));    
  }
  else{
    stepper[0]->setMaxSpeed((float)(ceil(machineSpeed * distance[0]/distance[1])));    
    stepper[0]->setAcceleration((float)(ceil(machineSpeed * distance[0]/distance[1])));    
    stepper[1]->setMaxSpeed(machineSpeed);    
    stepper[1]->setAcceleration(machineSpeed);    
  }
  stepper[0]->moveTo((long)target[0]);
  stepper[1]->moveTo((long)target[1]);
  machineIsMoving = true;
#ifdef __USE_TFT__
  updateDisplay(MENU_RGN_TEXT); // Show "Running".
#else // use LEDs.
  digitalWrite(AXIS_SELECT_LED_PIN, HIGH);
  digitalWrite(MODE_SELECT_LED_PIN, HIGH);
#endif // __USE_TFT__
  while(machineIsMoving){
    if((stepper[0]->distanceToGo() == 0)&&(stepper[1]->distanceToGo() == 0)){
      machineIsMoving = false;
      break; // Leave while loop.
    }
    stepper[0]->run();
    stepper[1]->run();
    if(!digitalRead(14)){ // If red button is pressed, do an "emergency stop".
      machineIsMoving = false;
      stepper[0]->stop();
      stepper[1]->stop();
      i = 0;
      while(i < 20000){ // Try 20,000 times = 2 seconds. If the motors are not in their destination by then, something has gone really wrong.
        if((stepper[0]->distanceToGo() == 0)&&(stepper[1]->distanceToGo() == 0)){
          break; // Leave while loop.
        }
        stepper[0]->run();
        stepper[1]->run();
        i++;
        delayMicroseconds(100);
      }
    }
  }
#ifdef __USE_TFT__
  updateDisplay(MENU_RGN_TEXT); // Show "Stopped".
#else // use LEDs.
  digitalWrite(AXIS_SELECT_LED_PIN, LOW);
  digitalWrite(MODE_SELECT_LED_PIN, LOW);
  lightLevel = 0;
  lightInc = 1;
#endif // __USE_TFT__
  waitUntilAllButtonsAreReleased();
}


////////////////////////////////////////////////////////////////////////////////
// Menu #2...4: Move the selected axis while the corresp. button is pressed.
////////////////////////////////////////////////////////////////////////////////
void moveAxisManually(int axis, long position, int direction){
  long axisLimitPosition;
  
  machineIsMoving = true;
#ifdef __USE_TFT__
  updateDisplay(MENU_RGN_TEXT); // Show "Running".
#else
  // Change speed by poti, not by menu; + 1 for avoiding freezes.
  machineSpeed = (long)analogRead(17) * MAX_SPEED / 1023 + 1; 
#endif
  // Travel to the start postion for the teach operation.
  // direction == 1 --> go to start, direction == 2 --> go to end.
  // The formula "(x-1)*2-1" results in -1 for x == 1 and +1 for x == 2.
  axisLimitPosition = stepper[axis]->currentPosition() + 20000 * ((direction - 1) * 2 - 1);
  // Now, go to the defined limit until the yellow/green button is released 
  /// (or the move of 20000 incfements is complete).
  stepper[axis]->moveTo(axisLimitPosition);
  stepper[axis]->setMaxSpeed(machineSpeed);    
  stepper[axis]->setAcceleration(machineSpeed);    
  while(!digitalRead(14 + direction)){ // 15 = A1 = yellow button, 16 = A2 = green button.
    // Execute the motion.
    stepper[axis]->run();
  }
  stepper[axis]->stop();
  stepper[axis]->runToPosition(); 
  axisPos[axis][position] = stepper[axis]->currentPosition();
  machineIsMoving = false;
#ifdef __USE_TFT__
  updateDisplay(MENU_RGN_TEXT); // Show new position.
#endif
  waitUntilAllButtonsAreReleased();
}

#ifdef __USE_TFT__
////////////////////////////////////////////////////////////////////////////////////////////////////
// Menu #5: Change the speed values.
////////////////////////////////////////////////////////////////////////////////////////////////////
void changeSpeed(int direction){
  double increment = 1;

  while(!digitalRead(14 + direction)){
    if(direction == 2){ // Increase.
      machineSpeed = min(machineSpeed +(int)increment, MAX_SPEED);
    }
    else{ // Decrease.
      // 1 == one increment per second --> slowest possible speed.
      machineSpeed = max(machineSpeed -(int)increment, 1); 
    }
    increment = min(increment * 1.2, 20); 
    updateDisplay(MENU_RGN_TEXT);
    delay(20);
  }
  waitUntilAllButtonsAreReleased();
}
#endif

////////////////////////////////////////////////////////////////////////////////////////////////////
// Wait until all buttons are released.
////////////////////////////////////////////////////////////////////////////////////////////////////
void waitUntilAllButtonsAreReleased(){
  int i;

  while(!(digitalRead(14) && digitalRead(15) && digitalRead(16))){
    delay(1);  
  }
  for(i = 0; i < 3; i++){
    buttonStatusOld[i] = 0;
  }
}

#ifdef __USE_TFT__
////////////////////////////////////////////////////////////////////////////////////////////////////
// Draw text center-aligned at given position.
////////////////////////////////////////////////////////////////////////////////////////////////////
void drawCenteredText(const char* text, int x, int y){
  int16_t x1, y1;
  uint16_t w, h;
  tft.getTextBounds(text, x, y, &x1, &y1, &w, &h); 
  tft.setCursor(x - w / 2, y);
  tft.print(text);
}

////////////////////////////////////////////////////////////////////////////////////////////////////
// Update the display.
////////////////////////////////////////////////////////////////////////////////////////////////////
void updateDisplay(int region){
  int newStatus;
  char textBuffer[64];
  long distance;
  int selectedAxis;
  int selectedPosition;
  // The following lines can be used to scale to different gear ratios or to linear distances.
  const double scaleFactor[2] = {MM_PER_ROT, 360};
  const char formatStrA[][12] PROGMEM = {"% 4ld mm", "% 4ld deg"};  
  const char formatStrB[][12] PROGMEM = {"(% 4ld mm)", "(% 4ld deg)"};  

  // Update Menu Title Region.
  if(region == MENU_RGN_TITLE){
    tft.fillRect(0, 0, 160, 25, MINI_TFT_BLUE);
    tft.setTextColor(MINI_TFT_WHITE);
    drawCenteredText(menuTitles[selectedMenu], 80, 5);
    tft.fillRect(0, 26, 160, 79, MINI_TFT_WHITE);
    return;
  }
  // Update Menu Body Region.
  // Clear a part of the background.
  // The new text will come with a white background, 
  // but it might be smaller than the text that has been displayed before.
  tft.fillRect(0, 26, 160, 79, MINI_TFT_WHITE);
  if(selectedMenu == 0){ // "Execution".  
    tft.fillRect(0, 26, 160, 79, MINI_TFT_WHITE);
    if(machineIsMoving){
      tft.setTextColor(MINI_TFT_GREEN, MINI_TFT_WHITE);
      sprintf(textBuffer, "Running!");
    }
    else{
      tft.setTextColor(MINI_TFT_RED, MINI_TFT_WHITE);
      sprintf(textBuffer, "Stopped");
    }
    drawCenteredText(textBuffer, 80, 45);
    return;
  }
  if((selectedMenu >= 1) && (selectedMenu <= 4)){ // "Feed or Rot. - Set Start/End".
    if(machineIsMoving){
      tft.setTextColor(MINI_TFT_GREEN, MINI_TFT_WHITE);
      sprintf(textBuffer, "Running!");
      drawCenteredText(textBuffer, 80, 45);
      return;
    }
    selectedAxis      = 1 - (selectedMenu % 2);   // selectedMenu == 1 or 3: first motor (0); selectedMenu == 2 or 4: second motor (1).
    selectedPosition  = (selectedMenu-1) / 2;     // selectedMenu == 1 or 2: POS_START (0); selectedMenu == 3 or 4: POS_END (1).
    // Show the _stored_ position as first line.
    sprintf(textBuffer, formatStrA[selectedAxis], (long)(axisPos[selectedAxis][selectedPosition] * scaleFactor[selectedAxis] / STEPS_PER_ROT));
    if(axisPos[selectedAxis][POS_START] == axisPos[selectedAxis][POS_END]){
      tft.setTextColor(MINI_TFT_ORAN, MINI_TFT_WHITE); // Highlight if start and end position are identical.
    }
    else{
      tft.setTextColor(MINI_TFT_GREEN, MINI_TFT_WHITE);
    }
    drawCenteredText(textBuffer, 80, 35);
    // Show the _current_ position as second line.
    tft.setTextColor(MINI_TFT_GRAY, MINI_TFT_WHITE); 
    sprintf(textBuffer, formatStrB[selectedAxis], (long)(stepper[selectedAxis]->currentPosition() * scaleFactor[selectedAxis] / STEPS_PER_ROT));
    drawCenteredText(textBuffer, 80, 60);
    return;
  }
  if(selectedMenu == 5){ // "Set Speed".  
    // Highlight limits.
    if((machineSpeed == 0) || (machineSpeed == MAX_SPEED)){
      tft.setTextColor(MINI_TFT_ORAN, MINI_TFT_WHITE); 
    }
    else{
      tft.setTextColor(MINI_TFT_GREEN, MINI_TFT_WHITE);
    }
    // Show feed rate in the first line.
    sprintf(textBuffer, "%s mm/min", String((double)machineSpeed * (double)MM_PER_ROT * 60.0 / (double)STEPS_PER_ROT, 0).c_str());
    drawCenteredText(textBuffer, 80, 35);
    // Show the estimated time to arrival in the second line.
    if(machineSpeed > 0){ 
      tft.setTextColor(MINI_TFT_GRAY, MINI_TFT_WHITE); 
      distance = max(abs(axisPos[0][POS_START]-axisPos[0][POS_END]), abs(axisPos[1][POS_START]-axisPos[1][POS_END]));
      sprintf(textBuffer, "(%s s)", String((double)distance/machineSpeed, 0).c_str());
      drawCenteredText(textBuffer, 80, 60);
    }
  }
}
#endif