#include <Keypad.h>
#include <LiquidCrystal.h>
#include <AccelStepper.h>
#include <EEPROM.h>

#define dirPin 19
#define stepPin 18
#define dirPin2 24
#define stepPin2 22
#define stepsPerRevolution 200
#define motorInterfaceType 1

AccelStepper stepper = AccelStepper(motorInterfaceType, stepPin, dirPin);
AccelStepper stepper2 = AccelStepper(motorInterfaceType, stepPin2, dirPin2);
//const int RS = 13, EN = 12, D4 = A2, D5 = A3, D6 = A4, D7 = A5;
LiquidCrystal lcd(13,12,14,15,16,17);

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

String displacementValue = "";
double displacement;

int pressedA = 0;  //Initialize key A state
int pressedB = 0;
int next =0;
double steps = 0;
int numCheck = 0;
double validMM = 100;
double validIN = 100;
int t1 = 0;
int t2 = 0;
int i = 0;
int run = 0;
int speed = 100;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  lcd.begin(16, 2);
  lcd.setCursor(0,0);
  lcd.print("Press A to Start");
  pinMode(stepPin, OUTPUT);
  pinMode(dirPin, OUTPUT);
  pinMode(stepPin2, OUTPUT);
  pinMode(dirPin2, OUTPUT);
stepper.setMaxSpeed(1000);
stepper2.setMaxSpeed(1000);
        digitalWrite(dirPin, HIGH);
        digitalWrite(dirPin2, HIGH);
}

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
displacement = displacementValue.toDouble();
   if(pressedA ==1){
    steps = 1/0.31496063*displacement*200;
    }
    if(pressedB == 1){
     steps = 0.125*displacement*200;  
    }
if(next==1){
        if ( (i % 2) == 0) {
        digitalWrite(dirPin, HIGH);
        digitalWrite(dirPin2, HIGH);
        }

  // Set the current position to 0:
 // stepper.setCurrentPosition(0);
 // stepper2.setCurrentPosition(0);

  // Run the motor forward at 200 steps/second until the motor reaches 400 steps (2 revolutions):
  while(stepper.currentPosition() < steps)
  {
    if(pressedA ==1){
          lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(stepper.currentPosition()/(1/0.31496063)/200);
      lcd.setCursor(0,1);
      lcd.print(stepper2.currentPosition()/(1/0.31496063)/200); 
      Serial.print("X = ");
      Serial.print(stepper.currentPosition()/(1/0.31496063)/200);
      Serial.print(" in  Y = ");
      Serial.print(stepper2.currentPosition()/(1/0.31496063)/200);  
      Serial.print(" in  t = ");  
    }
    if(pressedB == 1){
           lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(stepper.currentPosition()/0.125/200);
      lcd.setCursor(0,1);
      lcd.print(stepper2.currentPosition()/0.125/200); 
      Serial.print("X = ");
      Serial.print(stepper.currentPosition()/0.125/200);
      Serial.print(" mm  Y = ");
      Serial.print(stepper2.currentPosition()/0.125/200);  
      Serial.print(" mm  t = ");    
    } 

    stepper.setSpeed(speed);
    stepper.runSpeed();

    stepper2.setSpeed(speed);
    stepper2.runSpeed();
    t2 = millis();
    Serial.print(t2-t1);
    Serial.println(" ms"); 
  }
        if ( (i % 2) != 0) {
        digitalWrite(dirPin, LOW);
        digitalWrite(dirPin2, LOW);
        steps = 0;
        
    while(stepper.currentPosition() > steps)
  {
    if(pressedA ==1){
          lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(stepper.currentPosition()/(1/0.31496063)/200);
      lcd.setCursor(0,1);
      lcd.print(stepper2.currentPosition()/(1/0.31496063)/200); 
      Serial.print("X = ");
      Serial.print(stepper.currentPosition()/(1/0.31496063)/200);
      Serial.print(" in  Y = ");
      Serial.print(stepper2.currentPosition()/(1/0.31496063)/200);  
      Serial.print(" in  t = ");  
    }
    if(pressedB == 1){
           lcd.clear();
      lcd.setCursor(0,0);
      lcd.print(stepper.currentPosition()/0.125/200);
      lcd.setCursor(0,1);
      lcd.print(stepper2.currentPosition()/0.125/200); 
      Serial.print("X = ");
      Serial.print(stepper.currentPosition()/0.125/200);
      Serial.print(" mm  Y = ");
      Serial.print(stepper2.currentPosition()/0.125/200);  
      Serial.print(" mm  t = ");    
    } 

    stepper.setSpeed(-speed);
    stepper.runSpeed();

    stepper2.setSpeed(-speed);
    stepper2.runSpeed();
    t2 = millis();
    Serial.print(t2-t1);
    Serial.println(" ms"); 
  }
        }
  delay(2000);
  i++;
}

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

    if (key == 'A' & next!=1) {              //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;
    }
    if (key == 'B' & next!=1) {              //Continue after A 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;
    }
    if (key != 'A' && key != 'B' && key != 'C' && key != 'D' && key != '*' && key != '#') {
      displacementValue = displacementValue + key;  //Reset value
      lcd.clear();
      lcd.setCursor(0,0);      
      lcd.print(displacementValue);        //display key input
      numCheck = 1;

    }

    if(key == '*' && dot ==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(); 
    }
    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="";
    }
    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="";
    }
    else{
        lcd.clear();
        lcd.setCursor(0,0); 
        lcd.print("Displacement: ");
        lcd.setCursor(0,1);
        lcd.print(displacementValue);        //display key input
        lcd.println(" in");
        next = 1;
        t1 = millis(); 
    } 
    }
  }
}
}