#include <Arduino.h>

#include <SPI.h>  //Import libraries to control the OLED display
#include <Wire.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>

Servo lockServo;  //Create a servo object for the lock servo

#define SCREEN_WIDTH 128  // OLED display width, in pixels
#define SCREEN_HEIGHT 64  // OLED display height, in pixels

#define OLED_RESET -1                                                      // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);  // Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)

#define CLK 2
#define DT 3
#define SW 4

#define RED_PIN_1 6
#define RED_PIN_2 8
#define RED_PIN_3 10
#define RED_PIN_4 12

#define GRN_PIN_1 5
#define GRN_PIN_2 7
#define GRN_PIN_3 9
#define GRN_PIN_4 11

#define LOCK_SERVO_PIN 13

String currentDir = "";
unsigned long lastButtonPress = 0;

bool safeLocked = true;

int iSecretPIN = 0;
int PIN_CODE_ADDR = 0;

int MAX_POS_SERVO = 60; // rotations for a complete lock

enum wheel_modes {
  LOCK_MODE,
  PIN_CHANGE_MODE
};

int lastStateCLK;
wheel_modes mode;

void setup() {
  Serial.begin(115200);  //Starts the Serial monitor for debugging

  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))  //Connect to the OLED display
  {
    Serial.println(F("SSD1306 allocation failed"));  //If connection fails
    for (;;)
      ;  //Don't proceed, loop forever
  }
  display.clearDisplay();  //Clear display

  // Set encoder pins as inputs
  pinMode(CLK, INPUT);
  pinMode(DT, INPUT);
  pinMode(SW, INPUT_PULLUP);

  digitalWrite(LOCK_SERVO_PIN, LOW);

  // RED
  digitalWrite(RED_PIN_1, LOW);
  digitalWrite(RED_PIN_2, LOW);
  digitalWrite(RED_PIN_3, LOW);
  digitalWrite(RED_PIN_4, LOW);

  // GREEN
  digitalWrite(GRN_PIN_1, LOW);
  digitalWrite(GRN_PIN_2, LOW);
  digitalWrite(GRN_PIN_3, LOW);
  digitalWrite(GRN_PIN_4, LOW);

  lockServo.attach(LOCK_SERVO_PIN);

  display.setTextColor(SSD1306_WHITE);  //Set the text colour to white

  startupAnimation();

  // Read the PIN code memory location to see if we ave a value set
  int value = EEPROM.read(PIN_CODE_ADDR);

  // Check if PIN code has been set
  if (value == 255) {

    // PIN not set yet. Generate one and store it in memory

    // Gnerate PIN
    iSecretPIN = GetPin("Set up PIN");

    // Store PIN in memory
    EEPROM.put(PIN_CODE_ADDR, iSecretPIN);

    // Display confirmation message on screen
    display.clearDisplay();
    display.setCursor(20, 10);  //Set the display cursor position
    display.println("PIN SET");
    display.setCursor(50, 30);  //Set the display cursor position
    display.println("OK");
    display.display();

  } else {

    // PIN already set. Read it from memory
    EEPROM.get(PIN_CODE_ADDR, iSecretPIN);
  }

  lastStateCLK = digitalRead(CLK);
}


void loop() {

  int currentStateCLK = digitalRead(CLK);

  // If last and current state of CLK are different, then pulse occurred
  // React to only 1 state change to avoid double count
  if (currentStateCLK != lastStateCLK && currentStateCLK == 1) {

    // If the DT state is different than the CLK state then
    // the encoder is rotating CCW so decrement
    if (digitalRead(DT) != currentStateCLK) {

      display.clearDisplay();
      display.setTextSize(2);        //Set the display text size
      display.setCursor(30, 10);     //Set the display cursor position
      display.println(F("CHANGE"));  //Set the display text
      display.setCursor(45, 30);     //Set the display cursor position
      display.println(F("PIN"));     //Set the display text
      display.display();             //Output the display text

      mode = PIN_CHANGE_MODE;
    } else {

      display.clearDisplay();
      display.setTextSize(2);  //Set the display text size

      if (safeLocked == true) 
      {
        display.setCursor(30, 10);   //Set the display cursor position
        display.println(F("UNLOCK"));  //Set the display text
      }
      else
      {
        display.setCursor(35, 10);   //Set the display cursor position
        display.println(F("LOCK"));  //Set the display text
      }

      display.display();  //Output the display text

      mode = LOCK_MODE;
    }
  }
  lastStateCLK = currentStateCLK;

  // Read the button state
  int btnState = digitalRead(SW);

  //If we detect LOW signal, button is pressed
  if (btnState == LOW) {

    //if 50ms have passed since last LOW pulse, it means that the
    //button has been pressed, released and pressed again
    if (millis() - lastButtonPress > 50) {
      Serial.println("Button pressed!");

      if (mode == LOCK_MODE) {
        if (safeLocked == false)
          lock();
        else
          unlock();
      } else if (mode == PIN_CHANGE_MODE) {
        int attempt = GetPin("Enter old    PIN");

        if (attempt == iSecretPIN) {
          iSecretPIN = GetPin("Enter new    PIN");
          display.clearDisplay();
          display.setTextSize(2);        //Set the display text size
          display.setCursor(35, 10);     //Set the display cursor position
          display.println(F("PIN OK"));  //Set the display text
          display.display();             //Output the display text

          EEPROM.update(PIN_CODE_ADDR, iSecretPIN);
        } else {
          DisplayWrongPinMsg();
        }
      }
    }

    Serial.print("safeLock: " + (String)safeLocked);
    // Remember last button press event
    lastButtonPress = millis();
  }

  // Put in a slight delay to help debounce the reading
  delay(1);
}

void startupAnimation() {
  display.setTextSize(2);       //Set the display text size
  display.setCursor(35, 5);     //Set the display cursor position
  display.println(F("Crack"));  //Set the display text
  display.display();            //Output the display text
  delay(500);

  display.setCursor(45, 25);
  display.println(F("The"));
  display.display();
  delay(500);

  display.setCursor(40, 45);
  display.println(F("Safe"));
  display.display();
  delay(500);
  display.clearDisplay();
}

void lock() {
  if (safeLocked == false) {

    for (int pos = 0; pos <= MAX_POS_SERVO; pos += 1) {  // goes from 0 degrees to 180 degrees
      // in steps of 1 degree
      lockServo.write(pos);  // tell servo to go to position in variable 'pos'
      delay(15);             // waits 15ms for the servo to reach the position
    }
    safeLocked = true;

    int textSize = 2;
    display.clearDisplay();
    display.setTextSize(textSize);  //Set the display text size
    display.setCursor(25, 25);      //Set the display cursor position
    display.println("LOCKED");      //Set the display text
    display.display();

    // turn on all red leds
    digitalWrite(RED_PIN_1, HIGH);
    digitalWrite(RED_PIN_2, HIGH);
    digitalWrite(RED_PIN_3, HIGH);
    digitalWrite(RED_PIN_4, HIGH);

    // turn off all green leds

    digitalWrite(GRN_PIN_1, LOW);
    digitalWrite(GRN_PIN_2, LOW);
    digitalWrite(GRN_PIN_3, LOW);
    digitalWrite(GRN_PIN_4, LOW);
  }
}

void unlock() {

  // Is the safe locked?
  if (safeLocked == true) {

    // Safe is locked ->  ask for PIN
    //dont print anything unless value changed

    int textSize = 2;
    display.clearDisplay();
    display.setCursor(10, 10);  //Set the display cursor position
    display.println("Enter PIN");
    display.display();

    int finalNumber = GetPin("Enter PIN");

    if (finalNumber != iSecretPIN) {
      DisplayWrongPinMsg();
    } else {
      for (int pos = MAX_POS_SERVO; pos >= 0; pos -= 1) {  // goes from 180 degrees to 0 degrees
        lockServo.write(pos);                   // tell servo to go to position in variable 'pos'
        delay(15);                              // waits 15ms for the servo to reach the position
      }
      safeLocked = false;

      display.clearDisplay();
      display.setTextSize(textSize);  //Set the display text size
      display.setCursor(17, 25);      //Set the display cursor position
      display.println("UNLOCKED");    //Set the display text
      display.display();

      // turn off all red leds
      digitalWrite(RED_PIN_1, LOW);
      digitalWrite(RED_PIN_2, LOW);
      digitalWrite(RED_PIN_3, LOW);
      digitalWrite(RED_PIN_4, LOW);

      // turn on all green leds
      digitalWrite(GRN_PIN_1, HIGH);
      digitalWrite(GRN_PIN_2, HIGH);
      digitalWrite(GRN_PIN_3, HIGH);
      digitalWrite(GRN_PIN_4, HIGH);
    }
  }
}

int GetPin(String displayedText) {

  int digit = 0;
  int cnt = 0;
  int lastStateCLK;
  int currentStateCLK;
  int counter;

  int digitToPrint = 0;

  int pottentionalvalue[4] = { 0 };

  lastButtonPress = millis();

  while (cnt < 4) {
    counter = 0;
    while (1) {
      currentStateCLK = digitalRead(CLK);

      // If last and current state of CLK are different, then pulse occurred
      // React to only 1 state change to avoid double count
      if (currentStateCLK != lastStateCLK && currentStateCLK == 1) {

        // If the DT state is different than the CLK state then
        // the encoder is rotating CCW so decrement
        if (digitalRead(DT) != currentStateCLK) {
          counter--;
          currentDir = "CCW";
        } else {
          // Encoder is rotating CW so increment
          counter++;
          currentDir = "CW";
        }

        display.clearDisplay();
        pottentionalvalue[digitToPrint] = abs(counter);
        display.setCursor(10, 10);  //Set the display cursor position
        display.println(displayedText);
        display.setCursor(40, 45);                                                                                                                   //Set the display cursor position
        display.println(String(pottentionalvalue[0]) + String(pottentionalvalue[1]) + String(pottentionalvalue[2]) + String(pottentionalvalue[3]));  //Set the display text

        display.display();
        delay(300);
      }

      // Remember last CLK state
      lastStateCLK = currentStateCLK;

      // Read the button state
      int btnState = digitalRead(SW);

      //If we detect LOW signal, button is pressed
      if (btnState == LOW) {
        //if 50ms have passed since last LOW pulse, it means that the
        //button has been pressed, released and pressed again
        if (millis() - lastButtonPress > 500) {
          digitToPrint = (digitToPrint + 1);

          delay(500);
          break;
        }
        // Remember last button press event
        lastButtonPress = millis();

        // Put in a slight delay to help debounce the reading
      }
    }
    digit = counter;
    cnt++;
  }

  int number = pottentionalvalue[0] * pow(10, 3) + pottentionalvalue[1] * pow(10, 2) + pottentionalvalue[2] * pow(10, 1) + pottentionalvalue[3] * pow(10, 0);
  Serial.println("unlock " + String(number));

  return number;
}

void DisplayWrongPinMsg() {
  display.clearDisplay();
  display.setTextSize(2);        //Set the display text size
  display.setCursor(10, 20);     //Set the display cursor position
  display.println("WRONG PIN");  //Set the display text
  display.display();
}