#include<LiquidCrystal.h> // Including the LCD Library
#include<Keypad.h> // Including the Keypad Library
const byte Rows = 4; // Defining the number of Rows
const byte columns = 3; // Defining the number of Columns
char hexakeys[Rows][columns] = {
  {'1','2','3'},
  {'4','5','6'},
  {'7','8','9'},
  {'*','0','#'}
}; // Defining the Symbols on the keybad
byte rowpins [Rows] = {22, 23, 24, 25}; // Defining pins for the Rows
byte colpins [columns] = {26, 27, 28}; // Defining pins for the columns
LiquidCrystal lcd(13,12,11,10,9,8); // Defining the pins for the LCD Char screen
Keypad CustomKeyPad = Keypad(makeKeymap(hexakeys), rowpins, colpins, Rows, columns);
int forwardpin = 7; //pin For moving Forward
int backwardpin = 6; //pin For moving Backwards
int rotateRightpin = 5; //pin For rotating right
int rotateLeftpin = 4; //pin For rotating Left
int F = 0, B = 0, R = 0, L = 0; //Vars taking input val
int mode = 3; //pin For Mode (Auto, Manual)
int applyspeed = 21; //pin For ApplyingSpeed
int M, A; //Vars for input val of PB
int StateHolder = 0;
byte Arr[3] = {0,0,0}; // declaring an array to hold the CustomKey
int Counter = 0; // Counter for the array
int GetSpeed(){ // Array that Gives me the value of the speed in integer of 3 digits
  int Speed = Arr[2];
  Speed = Speed + Arr[1] * 10;
  Speed = Speed + Arr[0] * 100;
  return Speed;
  }
void setup() {
  // put your setup code here, to run once:
  pinMode (forwardpin, INPUT);
  pinMode (backwardpin, INPUT);
  pinMode (rotateRightpin, INPUT);
  pinMode (rotateLeftpin, INPUT);
  pinMode (mode, INPUT);
  pinMode (applyspeed, INPUT);
  lcd.begin(16,2);
  lcd.print("Speed: "); // Prnting the word "speed:" on the first line
  Serial.begin(9600); //Defining the frequency (bits sent per second

}

void loop() {
  // put your main code here, to run repeatedly:
  F = digitalRead (forwardpin); //Read the F value
  if (F == HIGH){
    Serial.print('1'); // Sending 1 to serial out if F = high
  }
  else {
    Serial.print("10"); // Note : If serial Transfer doesn't support String , Replace with Char
  }
  B = digitalRead (backwardpin); //Read the B value
  if (B == HIGH){
    Serial.print('2'); // Sending 2 to serial out if B = high
  }
  else {
    Serial.print("20");
  }
  R = digitalRead (rotateRightpin); //Read the R value
  if (R == HIGH){
    Serial.print('3');
  }
  else {
    Serial.print("30");
  }
  L = digitalRead (rotateLeftpin); //Read the L value
  if (L == HIGH){
    Serial.print('4');
  }
  else {
    Serial.print("40");
  }
  M = digitalRead (mode);//Read the mode PB value
  if (M == HIGH){
    Serial.print('5');
  }
  else {
    Serial.print("50");
  }
  A = digitalRead (applyspeed);//Read the ApplySpeed PB value
  if (A == HIGH){
    Serial.print('6');
  }
  else {
    Serial.print("60");
  }
  char CustomKey = CustomKeyPad.getKey(); // Getting input from the keypad
  if (CustomKey >= '0' || CustomKey <= '9'){
    Arr[Counter] = CustomKey - '0'; // Converting to Integer
    Counter++;
    if (Counter == 3){ // if Counter reaches the limit of 3 it returns to zero
      Counter = 0;
    }
  }
    if (A == HIGH){
      StateHolder = HIGH;
  }
  if (StateHolder){ // if we press applyspeed button pressed excute the following code
    Serial.print('7');
    Serial.print(GetSpeed()); // Getting the speed in integer form
    lcd.clear(); // Clearing the LCD screen from old values
    lcd.setCursor(0, 1); // Setting the curser on the next line
    lcd.print(CustomKey); // Displaying the input value from the KeyPad on the LCD screen
  }
  else {
    lcd.clear();
  }
  if (M == LOW && StateHolder == HIGH){ // if the Mode is set low that activates the manual control
    if (CustomKey && F){
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print('F');
      lcd.print(CustomKey);
    }
    else if (CustomKey && B){
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print('B');
      lcd.print(CustomKey);
    }
    else if (CustomKey && R){
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print('R');
      lcd.print(CustomKey);
    }
    else if (CustomKey && L){
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print('L');
      lcd.print(CustomKey);
    }
    else {
      lcd.clear();
    }
  }
  else if (M == HIGH){ // When mode is high Automatic mode is on
      lcd.clear();
      lcd.setCursor(0, 1);
      lcd.print("Autonomus");
      StateHolder = LOW;
  }
}