#include <Keypad.h>
#include <AccelStepper.h>
#include <LiquidCrystal_I2C.h>
#define MotorInterfaceType 4

AccelStepper stepper = AccelStepper(MotorInterfaceType, 10, 11, 12, 13);

LiquidCrystal_I2C lcd(0x27, 20, 4);

 volatile int firstnumber=99;  // used to tell how many numbers were entered on keypad
volatile int secondnumber=99;
volatile int thirdnumber=99;
volatile int fourthnumber=99;

// Variables to hold Distance and CurrentPosition
int keyfullnumber=0;  // used to store the final calculated distance value
String currentposition = "";  // Used for display on Nokia LCD

const byte ROWS = 4; //four rows
const byte COLS = 4; //three columns
char keys[ROWS][COLS] = {
     '1','2','3','\a',
     '4','5','6','\b',
     '7','8','9','\e',
     '*','0','#','\f'
};

//byte rowPins[ROWS] = {22, 24, 26, 28}; //connect to the row pinouts of the keypad
//byte colPins[COLS] = {30, 32, 34, 36}; //connect to the column pinouts of the keypad

byte rowPins[ROWS] = {9, 8, 7, 6}; //connect to the row pinouts of the keypad
byte colPins[COLS] = {5, 4, 3, 2}; //connect to the column pinouts of the keypad

Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, ROWS, COLS );




void setup() {

Serial.begin(9600); 
  
  // Set the maximum steps per second:
  stepper.setMaxSpeed(1000);

  
lcd.init();
  lcd.backlight();
  lcd.print("  Posicao em mm");
  lcd.setCursor(0,1); 
  lcd.print("--------------------");
  lcd.setCursor(0, 2);
  lcd.print("===>       mm");
  lcd.setCursor(0, 3);
  lcd.print("Posicao final= 0");
  lcd.setCursor(5, 2);

}

void loop() {

  
 char keypressed = keypad.getKey();  // Get value of keypad button if pressed
  if (keypressed != NO_KEY){  // If keypad button pressed check which key it was
    switch (keypressed) {
      
      case '1':
        checknumber(1);
      break;
        
      case '2':
        checknumber(2);
      break;

      case '3':
        checknumber(3);
      break;

      case '4':
        checknumber(4);
      break;

      case '5':
        checknumber(5);
      break;

      case '6':
        checknumber(6);
      break;

      case '7':
        checknumber(7);
      break;

      case '8':
        checknumber(8);
      break;

      case '9':
        checknumber(9);
      break;

      case '0':
        checknumber(0);
      break;

      case '*':
        deletenumber();
      break;

      case '#':
        calculatedistance();
      break;
    }
  }

}

void checknumber(int x){
  if (firstnumber == 99) { // Check if this is the first number entered
    firstnumber=x;
    String displayvalue = String(firstnumber);  //  Transform int to a string for display
    screen(displayvalue); // Redraw Nokia lcd
    
  } else {
    if (secondnumber == 99) {  // Check if it's the second number entered
      secondnumber=x;
      String displayvalue = (String(firstnumber) + String(secondnumber));
      screen(displayvalue);
      
      } else {
    if (thirdnumber == 99) {  // Check if it's the third number entered
      thirdnumber=x;
      String displayvalue = (String(firstnumber) + String(secondnumber) + String(thirdnumber));
      screen(displayvalue); 

    } else {  // It must be the 4rd number entered
      fourthnumber=x;
      String displayvalue = (String(firstnumber) + String(secondnumber) + String(thirdnumber) + String(fourthnumber));
      screen(displayvalue);

    }
  }
}
}
 
void deletenumber() {  // Used to backspace entered numbers
  if (fourthnumber !=99) {
      String displayvalue = (String(firstnumber) + String(secondnumber) + String(thirdnumber));
      screen(displayvalue);

    fourthnumber=99;
  } 
  else {
    if (thirdnumber !=99) {
      String displayvalue = (String(firstnumber) + String(secondnumber));
      screen(displayvalue);

      thirdnumber=99;
   } 
  else {
    if (secondnumber !=99) {
      String displayvalue = String(firstnumber);
      screen(displayvalue);

      secondnumber=99;
   } 
   else {
     if (firstnumber !=99) {
       String displayvalue = "";
       screen(displayvalue);

       firstnumber=99;
      }
    }
  }
}
}  
void calculatedistance() {  // Used to create a full number from entered numbers

    if (fourthnumber == 99 && thirdnumber == 99 && secondnumber == 99 && firstnumber != 99) {
      keyfullnumber=firstnumber;
      movestepper(keyfullnumber);
    }
    
    if (secondnumber  != 99 && thirdnumber == 99 && fourthnumber == 99) {
      keyfullnumber=(firstnumber*10)+secondnumber;
      movestepper(keyfullnumber);
    }
    
    if (thirdnumber != 99 && fourthnumber == 99) {
      keyfullnumber=(firstnumber*100)+(secondnumber*10)+thirdnumber;
      movestepper(keyfullnumber);
    }

    if (fourthnumber != 99) {
      keyfullnumber=(firstnumber*1000)+(secondnumber*100)+(thirdnumber*10)+fourthnumber;
      movestepper(keyfullnumber);
    }
    
    resetnumbers(); // Reset numbers to get ready for new entry
  }
  void movestepper(int z) {  //  Move the stepper

  int calculatedmove=((z*50)/50);  //  Calculate number of steps needed in mm
  stepper.runToNewPosition(calculatedmove);
  currentposition = String(z);
  lcd.setCursor(5, 2);
     lcd.setCursor(14, 3);
     lcd.print("       ");
     lcd.setCursor(14, 3);
     lcd.print(currentposition);
     lcd.setCursor(5, 2);  
}
void resetnumbers() {  // Reset numbers for next entry
  firstnumber=99;
  secondnumber=99;
  thirdnumber=99;
  fourthnumber=99;
} 

void screen(String y) {
   lcd.setCursor(5, 2);
     lcd.print(y);
     lcd.setCursor(14, 3);
     lcd.print("       ");
     lcd.setCursor(14, 3);
     lcd.print(currentposition);
     lcd.setCursor(5, 2); 
}