#include <Keypad.h>
#define NUM_CHAR 4

const uint8_t ROWS = 4;
const uint8_t COLS = 4;
char keys[ROWS][COLS] = {
  { '1', '2', '3', 'A' },
  { '4', '5', '6', 'B' },
  { '7', '8', '9', 'C' },
  { '*', '0', '#', 'D' }
};

uint8_t colPins[COLS] = { 5, 4, 3, 2 }; // Pins connected to C1, C2, C3, C4
uint8_t rowPins[ROWS] = { 9, 8, 7, 6 }; // Pins connected to R1, R2, R3, R4

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

char answer[NUM_CHAR] = {'1', '2', '3', '4'};
char combination[] = {'?', '?', '?', '?', 0};

void setup() {
  Serial.begin(9600);

  pinMode(A0, OUTPUT);
  pinMode(A1, OUTPUT);
  pinMode(A2, OUTPUT);
  pinMode(A3, OUTPUT);

  pinMode(12, OUTPUT);
  pinMode(13, OUTPUT);

  setLocked();
}

void loop() {
  setNumberDigitsEntered(0);
  int numInput=0;

  while(numInput < NUM_CHAR) {
    char key = keypad.getKey();
    if (key != NO_KEY) {
      combination[numInput++] = key;
       setNumberDigitsEntered(numInput);
    }
  }

  Serial.println(combination);

  if (checkCorrect() == NUM_CHAR) {
    Serial.println("Match!");
    setUnLocked();
    for (;numInput>0;numInput--) {
      setNumberDigitsEntered(numInput);
      delay(500);
    }
    setLocked();
  } else {
    Serial.println("No match!");
  }

}

int checkCorrect() {
  bool used[NUM_CHAR];
  int right = 0;
  int contains = 0;
  for (int x=0;x<NUM_CHAR;x++) {
    if (combination[x] == answer[x]) {
      right ++;
      used[x]=true;
    } else {
      used[x]=false;
    }
  }

  for (int x=0;x<NUM_CHAR;x++) {
    for (int y=0;y<NUM_CHAR;y++){
      if (!used[y] && combination[x] == answer[y]) {
        contains++;
        used[y]=true;
        break;
      }
    }
  }

  Serial.print("Right place ");
  Serial.print(right);
  Serial.print(" Right colour ");
  Serial.println(contains);

  return right;
}

void setNumberDigitsEntered(int numDigits) {
  for (int x=0;x<NUM_CHAR;x++) {
    digitalWrite(A0+x, numDigits > x);
  }
}

void setUnLocked() {
  digitalWrite(12, HIGH);
  digitalWrite(13, LOW);
}

void setLocked() {
  digitalWrite(12, LOW);
  digitalWrite(13, HIGH);
}