#include <Keypad.h> //keypad library by Mark Stanley and Alexander Brevig
#define A 2
#define B 3
#define C 4
#define D 5
#define E 14
#define F 15
#define G 16
#define D1 17
#define D2 18
#define D3 19
#define D4 20

char digit_1 = '1';
char digit_2 = '2';
char digit_3 = '3';
char digit_4 = '4';

char current_digit = D1;
char selected_digit = D1;

// CODE FOR THE KEYPAD
const byte ROWS = 4; //four rows on the keypad
const byte COLS = 4; //four columns on the keypad
char hexaKeys[ROWS][COLS] = { //matrix that mimics the keypad
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};
byte rowPins[ROWS] = {10, 11, 12, 13}; //define the pins connected to each row
byte colPins[COLS] = {6, 7, 8, 9}; //define the pins connected to each column
Keypad customKeypad = Keypad( makeKeymap(hexaKeys), rowPins, colPins, ROWS, COLS); //create keypad object
//END OF CODE FOR THE KEYPAD

void setup() {
  Serial.begin(9600); //start serial communication
  for (int i = 2; i < 6; i++) pinMode(i, OUTPUT);
  for (int i = 14; i < 17; i++) pinMode(i, OUTPUT);
  for (int i = 17; i <= 20; i++) pinMode(i, OUTPUT);
  off(); //clear display
}

void loop() {
  char Key = customKeypad.getKey(); //read key that is pressed
  if (Key) { //if a key is pressed
    Serial.println(Key); //print the key in serial communication
    if (Key == 'A'){selected_digit = D1;}
    else if (Key == 'B'){selected_digit = D2;}
    else if (Key == 'C'){selected_digit = D3;}
    else if (Key == 'D'){selected_digit = D4;}
    else if (Key == '*'){
      digit_1 = '1';
      digit_2 = '2';
      digit_3 = '3';
      digit_4 = '4';
    }
    else if (Key == '#'){Serial.print("moet nog aangevuld worden");}
    else if (selected_digit == D1){digit_1 = Key;}
    else if (selected_digit == D2){digit_2 = Key;}
    else if (selected_digit == D3){digit_3 = Key;}
    else if (selected_digit == D4){digit_4 = Key;}
  }
  else {
    current_digit = D1;
    number_picker();
    digitalWrite(current_digit, HIGH);
    digitalWrite(current_digit, LOW);
    
    current_digit = D2;
    number_picker(); 
    digitalWrite(current_digit, HIGH);
    digitalWrite(current_digit, LOW);
    
    current_digit = D3;
    number_picker();
    digitalWrite(current_digit, HIGH);
    digitalWrite(current_digit, LOW);

    current_digit = D4;
    number_picker(); 
    digitalWrite(current_digit, HIGH);
    digitalWrite(current_digit, LOW);
  }
}

void number_picker() {
  if (current_digit == D1)
  {
    if (digit_1 == '1'){one();}
    else if (digit_1 == '2'){two();}
    else if (digit_1 == '3'){three();}
    else if (digit_1 == '4'){four();}
    else if (digit_1 == '5'){five();}
    else if (digit_1 == '6'){six();}
    else if (digit_1 == '7'){seven();}
    else if (digit_1 == '8'){eight();}
    else if (digit_1 == '9'){nine();}
    else if (digit_1 == '0'){zero();}
  }
  if (current_digit == D2)
  {
    if (digit_2 == '1'){one();}
    else if (digit_2 == '2'){two();}
    else if (digit_2 == '3'){three();}
    else if (digit_2 == '4'){four();}
    else if (digit_2 == '5'){five();}
    else if (digit_2 == '6'){six();}
    else if (digit_2 == '7'){seven();}
    else if (digit_2 == '8'){eight();}
    else if (digit_2 == '9'){nine();}
    else if (digit_2 =='0'){zero();}
  }
  if (current_digit == D3)
  {
    if (digit_3 == '1'){one();}
    else if (digit_3 == '2'){two();}
    else if (digit_3 == '3'){three();}
    else if (digit_3 == '4'){four();}
    else if (digit_3 == '5'){five();}
    else if (digit_3 == '6'){six();}
    else if (digit_3 == '7'){seven();}
    else if (digit_3 == '8'){eight();}
    else if (digit_3 == '9'){nine();}
    else if (digit_3 == '0'){zero();}
  }
  if (current_digit == D4)
  {
    if (digit_4 == '1'){one();}
    else if (digit_4 == '2'){two();}
    else if (digit_4 == '3'){three();}
    else if (digit_4 == '4'){four();}
    else if (digit_4 == '5'){five();}
    else if (digit_4 == '6'){six();}
    else if (digit_4 == '7'){seven();}
    else if (digit_4 == '8'){eight();}
    else if (digit_4 == '9'){nine();}
    else if (digit_4 == '0'){zero();}
  }
}

void off() {
  digitalWrite(A, HIGH);
  digitalWrite(B, HIGH);
  digitalWrite(C, HIGH);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, HIGH);
  digitalWrite(G, HIGH);
}

void one() {
  digitalWrite(A, HIGH);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, HIGH);
  digitalWrite(G, HIGH);
}

void two() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, HIGH);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, HIGH);
  digitalWrite(G, LOW);
}

void three() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, HIGH);
  digitalWrite(F, HIGH);
  digitalWrite(G, LOW);
}

void four() {
  digitalWrite(A, HIGH);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void five() {
  digitalWrite(A, LOW);
  digitalWrite(B, HIGH);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, HIGH);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void six() {
  digitalWrite(A, LOW);
  digitalWrite(B, HIGH);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void seven() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, HIGH);
  digitalWrite(E, HIGH);
  digitalWrite(F, HIGH);
  digitalWrite(G, HIGH);
}

void eight() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void nine() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, HIGH);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void zero() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, HIGH);
}

void printA() {
  digitalWrite(A, LOW);
  digitalWrite(B, LOW);
  digitalWrite(C, LOW);
  digitalWrite(D, HIGH);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, LOW);
}

void printC() {
  digitalWrite(A, LOW);
  digitalWrite(B, HIGH);
  digitalWrite(C, HIGH);
  digitalWrite(D, LOW);
  digitalWrite(E, LOW);
  digitalWrite(F, LOW);
  digitalWrite(G, HIGH);
}