// Add library
#include <Keypad.h>
#include <LiquidCrystal.h>
#include <AccelStepper.h>
#include <EEPROM.h>

// Define digital pins for motors
#define dirPin 19
#define stepPin 18
#define dirPin2 24
#define stepPin2 22
#define stepsPerRevolution 200
#define motorInterfaceType 1

// define motors parameters
AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper stepper2 = AccelStepper(motorInterfaceType, stepPin2, dirPin2);
//const int RS = 13, EN = 12, D4 = 14, D5 = 15, D6 = 16, D7 = 17;

// define LCD digital pins
LiquidCrystal lcd(13, 12, 14, 15, 16, 17);

// define keypad digital pins
const byte ROWS = 4;       //four rows
const byte COLS = 4;       //four columns
char keys[ROWS][COLS] = {  //Defining keypad characters
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};
byte rowPins[ROWS] = { 11, 10, 9, 8 };  //digital connect to the row pins of the keypad
byte colPins[COLS] = { 7, 6, 5, 4 };  //digital connect to the column pints of the keypad

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);  //mapping digital with keypad

// define common variables
String displacementValue = "";    // User defined max displacement
double displacement;

int pressedA = 0;                 //Initialize key A state
int pressedB = 0;                 //Initialize key B state
int next = 0;                     //define motors operation
double steps = 0;                 //displacement translated as motor steps
int numCheck = 0;
double validMM = 100;             //Valid mm range
double validIN = 100;             //Valid in range
unsigned long int t1 = 0;         //time region
unsigned long int t2 = 0;
int i = 0;
int dir = 0;                      //motors direction
int mode2 = 0;                    //motors mode\

//holding variable
char key3;
char holdKey;
unsigned long t_hold;

// Initial function setup
void setup() {
  // Serial and LCD initial
  Serial.begin(9600);
  lcd.begin(20, 4);
  lcd.setCursor(0, 0);
  lcd.print("  Press A to Start");

  //digital pin for output
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
  stepper.setMaxSpeed(1000);
  stepper2.setMaxSpeed(1000);
  digitalWrite(dirPin, HIGH);
  digitalWrite(dirPin2, HIGH);
}

// loop function for the system
void loop() {

  // 8mm per rev; 0.31496063 in per rev;
  // 200 steps per rev; 0.125 rev per mm; 1/0.31496063 rev per in;
  // steps = 0.125*displacement*200
  // steps = 1/0.31496063*displacement*200

  // translate User defined max displacement to step
  displacement = displacementValue.toDouble();
  if (pressedA == 1) {
    steps = 1 / 0.31496063 * displacement * 200;
  }
  if (pressedB == 1) {
    steps = 0.125 * displacement * 200;
  }

  // Enter mode 1 of the system
  if (next == 1) {
    //       if ( (i % 2) == 0) {
    //       digitalWrite(dirPin, HIGH);
    //       digitalWrite(dirPin2, HIGH);
    //       }
    int speed = 100;                    // motor initial speed

    // Enter mode 2 of the system
    if (mode2 == 1) {
      speed = 0;                            // motor inital speed
    }

    while (next == 1)
    {
      lcd.setCursor(0, 0);
      if (mode2 == 0) {
        lcd.print("Mode1:Extend/Retract");
      }
      if (mode2 == 1) {
        lcd.print("Mode2:Manual Control");
      }
      // display data using imperial
      if (pressedA == 1) {
        //lcd.clear();
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        lcd.print(" in");
        Serial.print("X = ");
        Serial.print(stepper.currentPosition() / (1 / 0.31496063) / 200, 4);
        Serial.print(" in  Y = ");
        Serial.print(stepper2.currentPosition() / (1 / 0.31496063) / 200, 4);
        Serial.print(" in  t = ");
      }
      // display data using metrics
      if (pressedB == 1) {
        //lcd.clear();
        lcd.setCursor(0, 1);
        lcd.print("X = ");
        lcd.print(stepper.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        lcd.setCursor(0, 2);
        lcd.print("Y = ");
        lcd.print(stepper2.currentPosition() / 0.125 / 200, 4);
        lcd.print(" mm");
        Serial.print("X = ");
        Serial.print(stepper.currentPosition() / 0.125 / 200, 4);
        Serial.print(" mm  Y = ");
        Serial.print(stepper2.currentPosition() / 0.125 / 200, 4);
        Serial.print(" mm  t = ");
      }
      // display time data
      t2 = millis();
      Serial.print(t2 - t1);
      Serial.println(" ms");
      lcd.setCursor(0, 3);
      lcd.print("t = ");
      lcd.print(t2 - t1);
      lcd.print(" ms");

      // enter mode 2 operation
      if (mode2 == 1) {

        if (key3) {
          holdKey = key3;
        }
        // Enter if key is pressed
        if (keypad.getState() == PRESSED) {
          if (holdKey == 'B' && stepper.currentPosition() < steps) {
            speed = 100;
          }
          else if (holdKey == 'C' && stepper.currentPosition() > 0) {
            speed = -100;
          }
          else {
            speed = 0;
          }
        }
        // Enter if key is held
        else if (keypad.getState() == HOLD) {
          if ((millis() - t_hold) > 0 ) {
            if (holdKey == 'B' && stepper.currentPosition() < steps) {
              speed = 100;
            }
            else if (holdKey == 'C' && stepper.currentPosition() > 0) {
              speed = -100;
            }
            else {
              speed = 0;
            }
            t_hold = millis();
          }
        }
        // Enter if idle
        else {
          speed = 0;
        }
        /*
          if (key3 == 'B' && stepper.currentPosition() < steps){
          KeyState state = keypad.getState();
          if(state == PRESSED){
            speed = 100;
          }
          }
          else if (key3 == 'C'&& stepper.currentPosition() > 0){
          KeyState state = keypad.getState();
          if(state == PRESSED){
          speed = -100;
          }
          }
          else{
            speed = 0;
          }
        */
        //motor control
        stepper.setSpeed(speed);
        stepper.runSpeed();

        stepper2.setSpeed(speed);
        stepper2.runSpeed();
      }

      // Enter mode 1 operation if operation 2 didnt run
      // Motor extension
      else if (stepper.currentPosition() <= steps && dir == 0) {
        stepper.setSpeed(speed);
        stepper.runSpeed();

        stepper2.setSpeed(speed);
        stepper2.runSpeed();
        //t2 = millis();
        //Serial.print(t2-t1);
        //Serial.println(" ms");
        // change direction
        if (stepper.currentPosition() > steps) {
          dir = 1;
          delay(1000);
        }

      }
      // Motor retraction
      else if (stepper.currentPosition() >= 0 && dir == 1) {
        stepper.setSpeed(-speed);
        stepper.runSpeed();

        stepper2.setSpeed(-speed);
        stepper2.runSpeed();
        //t2 = millis();
        //Serial.print(t2-t1);
        //Serial.println(" ms");

        // change direction
        if (stepper.currentPosition() < 0) {
          dir = 0;
          delay(1000);
        }
      }

      // stop motors
      key3 = keypad.getKey();
      if (key3 == 'A') {
        speed = 0;
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Motors Stopped");
        lcd.setCursor(0, 2);
        lcd.print("Press A to Continue");
        next = 0;
        validMM = 100;
        validIN = 100;
      }
      i++;
    }
  }

  // User interaction
  while (validIN > 1 && validMM > 25.4) {
    char key = keypad.getKey();               //key is input
    int dot = 1;
    if (key != NO_KEY) {                      // When pressed

      // Request max displacement
      if (key == 'A' & next == 0) {                           //Continue after A is pressed
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: in");
        displacementValue = "";
        numCheck = 0;
        pressedA = 1;
        pressedB = 0;
      }
      // Change to metrics
      if (key == 'B' & next == 0 & pressedA == 1) {            //Continue after B is pressed
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Enter Max");
        lcd.setCursor(0, 1);
        lcd.print("Displacement: mm");
        displacementValue = "";
        numCheck = 0;
        pressedA = 0;
        pressedB = 1;
      }

      // Enter numerical values
      if (key != 'A' && key != 'B' && key != 'C' && key != 'D' && key != '*' && key != '#' && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + key;  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        numCheck = 1;

      }
      // Allow decimal values
      if (key == '*' && dot == 1 && (pressedA == 1 || pressedB == 1)) {
        displacementValue = displacementValue + '.';  //Reset value
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print(displacementValue);        //display key input
        dot = 0;
      }

      // Key evaluation
      if (key == 'D' && numCheck == 1) {  //if key D is pressed
        if (pressedA == 1) {
          validIN = displacementValue.toDouble();
        }
        if (pressedB == 1) {
          validMM = displacementValue.toDouble();
        }
        // Display input in imperial (Incorrect ver.)
        if (validIN > 1 && pressedA == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: in");
          displacementValue = "";
        }
        // Display input in metrics (Incorrect ver.)
        else if (validMM > 25.4 && pressedB == 1) {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Max Displacement");
          lcd.setCursor(0, 1);
          lcd.print("is too large");
          delay (2000);
          lcd.setCursor(0, 0);
          lcd.print("Enter Max");
          lcd.setCursor(0, 1);
          lcd.print("Displacement: mm");
          displacementValue = "";
        }
        // Display inputs
        else {
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Displacement: ");
          lcd.setCursor(0, 1);
          lcd.print(displacementValue);        //display key input
          if (pressedA == 1) {
            lcd.println(" in");
          }
          else {
            lcd.println(" mm");
          }

          delay(2000);

          // display motors mode option
          lcd.clear();
          lcd.setCursor(0, 0);
          lcd.print("Mode1:Extend/Retract");
          lcd.setCursor(0, 1);
          lcd.print("Mode2:Manual Control");

          // Leading to mode 1 or mode 2
          char key2 = keypad.getKey();
          while (next == 0) {
            key2 = keypad.getKey();
            if (key2 == '1') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode1:Extend/Retract");
              next = 1;
              mode2 = 0;                  // enter mode 1
            }
            if (key2 == '2') {
              lcd.clear();
              lcd.setCursor(0, 0);
              lcd.print("Mode2:Manual Control");
              next = 1;
              mode2 = 1;                  // enter mode 2

            }
          }
          delay(1000);
          t1 = millis();                  // record time at then end of loop
        }
      }
    }
  }
}