#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Keypad.h>
#include <Servo.h>
#include "SafeState.h"
#include "icons.h"

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

#define SERVO_PIN 6
#define SERVO_LOCK_POS   20
#define SERVO_UNLOCK_POS 90
Servo lockServo;

const byte KEYPAD_ROWS = 4;
const byte KEYPAD_COLS = 4;
byte rowPins[KEYPAD_ROWS] = {5, 4, 3, 2};
byte colPins[KEYPAD_COLS] = {A3, A2, A1, A0};
char keys[KEYPAD_ROWS][KEYPAD_COLS] = {
  {'1', '2', '3', 'A'},
  {'4', '5', '6', 'B'},
  {'7', '8', '9', 'C'},
  {'*', '0', '#', 'D'}
};

Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, KEYPAD_ROWS, KEYPAD_COLS);

SafeState safeState;

void lock() {
  lockServo.write(SERVO_LOCK_POS);
  safeState.lock();
}

void unlock() {
  lockServo.write(SERVO_UNLOCK_POS);
}

void setup() {
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Initialize with I2C address 0x3C
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);

  lockServo.attach(SERVO_PIN);

  Serial.begin(115200);
  if (safeState.locked()) {
    lock();
  } else {
    unlock();
  }

  showStartupMessage();
}

void loop() {
  if (safeState.locked()) {
    safeLockedLogic();
  } else {
    safeUnlockedLogic();
  }
}

void showStartupMessage() {
  display.clearDisplay();
  display.setCursor(4, 0);
  display.println("Welcome!");
  display.display();
  delay(1000);

  display.setCursor(0, 2);
  String message = "ArduinoSafe v1.0";
  for (byte i = 0; i < message.length(); i++) {
    display.print(message[i]);
    display.display();
    delay(100);
  }
  delay(500);
}

String inputSecretCode() {
  display.setCursor(5, 1);
  display.println("[____]");
  display.setCursor(6, 1);
  String result = "";
  while (result.length() < 4) {
    char key = keypad.getKey();
    if (key >= '0' && key <= '9') {
      display.print('*');
      display.display();
      result += key;
    }
  }
  return result;
}

void showWaitScreen(int delayMillis) {
  display.setCursor(2, 1);
  display.println("[..........]");
  display.setCursor(3, 1);
  for (byte i = 0; i < 10; i++) {
    delay(delayMillis);
    display.print("=");
    display.display();
  }
}

void showUnlockMessage() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.write(ICON_UNLOCKED_CHAR);
  display.setCursor(4, 0);
  display.println("Unlocked!");
  display.setCursor(15, 0);
  display.write(ICON_UNLOCKED_CHAR);
  display.display();
  delay(1000);
}

void safeUnlockedLogic() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.write(ICON_UNLOCKED_CHAR);
  display.setCursor(2, 0);
  display.println(" # to lock");
  display.setCursor(15, 0);
  display.write(ICON_UNLOCKED_CHAR);

  bool newCodeNeeded = true;

  if (safeState.hasCode()) {
    display.setCursor(0, 1);
    display.println("  A = new code");
    newCodeNeeded = false;
  }

  auto key = keypad.getKey();
  while (key != 'A' && key != '#') {
    key = keypad.getKey();
  }

  bool readyToLock = true;
  if (key == 'A' || newCodeNeeded) {
    readyToLock = setNewCode();
  }

  if (readyToLock) {
    display.clearDisplay();
    display.setCursor(5, 0);
    display.write(ICON_UNLOCKED_CHAR);
    display.println(" ");
    display.write(ICON_RIGHT_ARROW);
    display.println(" ");
    display.write(ICON_LOCKED_CHAR);

    safeState.lock();
    lock();
    showWaitScreen(100);
  }
}

void safeLockedLogic() {
  display.clearDisplay();
  display.setCursor(0, 0);
  display.write(ICON_LOCKED_CHAR);
  display.println(" Safe Locked! ");
  display.write(ICON_LOCKED_CHAR);

  String userCode = inputSecretCode();
  bool unlockedSuccessfully = safeState.unlock(userCode);
  showWaitScreen(200);

  if (unlockedSuccessfully) {
    showUnlockMessage();
    unlock();
  } else {
    display.clearDisplay();
    display.setCursor(0, 0);
    display.println("Access Denied!");
    showWaitScreen(1000);
  }
}