#include <LiquidCrystal.h>
#include <Servo.h>

#define LED_PIN_TOP_LEFT_RED A0
#define LED_PIN_BOT_LEFT_RED A1
#define LED_PIN_BOT_RIGHT_RED A2
#define LED_PIN_TOP_RIGHT_RED 9
#define LED_PIN_TOP_LEFT_GREEN A5
#define LED_PIN_BOT_LEFT_GREEN A4
#define LED_PIN_BOT_RIGHT_GREEN A3
#define LED_PIN_TOP_RIGHT_GREEN 8

#define ENCODER_BTN 13
// Define rotary encoder pins
#define ENC_A 3
#define ENC_B 2



LiquidCrystal lcd(12, 11, 7, 6, 5, 4);
Servo door_motor; 

unsigned long _lastIncReadTime = micros(); 
unsigned long _lastDecReadTime = micros(); 
int _pauseLength = 25000;
int _fastIncrement = 10;

volatile int counter = 0;

int state = 0;
int prev_state = 0;
int sub_state = 0;
int open_position = 0;
int closed_position = 90;
int current_cursor_pos = 0;
int start_cursor_pos = 0;
int end_cursor_pos = 0;

int solution_1[4] = {1, 2, 3, 4};
int solution_2[4] = {4, 3, 2, 1};
int solution_3[4] = {9, 8, 7, 6};
int solution_4[4] = {6, 7, 8, 9};

int current_code[4];

//const int buttonPin = 2;  // the number of the pushbutton pin
//const int ledPin = 13;    // the number of the LED pin

// Variables will change:
//int ledState = HIGH;        // the current state of the output pin
int buttonState;            // the current reading from the input pin
int lastButtonState = LOW;  // the previous reading from the input pin
int btn_ret_value = HIGH;

// the following variables are unsigned longs because the time, measured in
// milliseconds, will quickly become a bigger number than can be stored in an int.
unsigned long lastDebounceTime = 0;  // the last time the output pin was toggled
unsigned long debounceDelay = 50;    // the debounce time; increase if the output flickers


void setup() {
  Serial.begin(115200); // Any baud rate should work
  Serial.println("Hello Arduino\n");

  pinMode(LED_PIN_TOP_LEFT_RED, OUTPUT);
  pinMode(LED_PIN_BOT_LEFT_RED, OUTPUT);
  pinMode(LED_PIN_BOT_RIGHT_RED, OUTPUT);
  pinMode(LED_PIN_TOP_RIGHT_RED, OUTPUT);

  pinMode(LED_PIN_TOP_LEFT_GREEN, OUTPUT);
  pinMode(LED_PIN_BOT_LEFT_GREEN, OUTPUT);
  pinMode(LED_PIN_BOT_RIGHT_GREEN, OUTPUT);
  pinMode(LED_PIN_TOP_RIGHT_GREEN, OUTPUT);

  // Set encoder pins and attach interrupts
  pinMode(ENC_A, INPUT_PULLUP);
  pinMode(ENC_B, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(ENC_A), read_encoder, CHANGE);
  attachInterrupt(digitalPinToInterrupt(ENC_B), read_encoder, CHANGE);

  lcd.begin(16, 2);
  // you can now interact with the LCD, e.g.:
  lcd.print("Press the button");
  lcd.setCursor(0,1);
  lcd.print("to lock!");

  door_motor.attach(10);
  door_motor.write(open_position);
  delay(500);

  pinMode(ENCODER_BTN, INPUT_PULLUP);
  //pinMode(LED_BUILTIN, OUTPUT);
}


void loop() {
  static int lastCounter = 0;

  // put your main code here, to run repeatedly:
  if (state == 0)
  {
    if (check_btn_state() == LOW) 
    {
      //digitalWrite(LED_BUILTIN, HIGH);
      //lock_vault();
      transition_states();
      state = 1;
    } 
  }
  else if (state == 1 || state == 2 || state == 3 || state == 4)
  {
    
    if (check_btn_state() == LOW) 
    {
      //digitalWrite(LED_BUILTIN, HIGH);
      if (current_cursor_pos == end_cursor_pos)
      {
        current_code[current_cursor_pos-start_cursor_pos] = counter;
        counter = 0;
        transition_states();
        state++;
      }
      else
      {
        current_code[current_cursor_pos-start_cursor_pos] = counter;
        counter = 0;
        current_cursor_pos++;
        lcd.setCursor(current_cursor_pos,1);
      }
    } 

    // If count has changed print the new value to serial
    if(counter != lastCounter){
      //lcd.clear();
      lcd.print(String(counter));
      lastCounter = counter;
      lcd.setCursor(current_cursor_pos,1);
    }
  }

}


void transition_states()
{
  if (state == 0)
  {
    //lock_vault();
    door_motor.write(closed_position);

    digitalWrite(LED_PIN_TOP_LEFT_RED, HIGH);
    digitalWrite(LED_PIN_BOT_LEFT_RED, HIGH);
    digitalWrite(LED_PIN_BOT_RIGHT_RED, HIGH);
    digitalWrite(LED_PIN_TOP_RIGHT_RED, HIGH);

    digitalWrite(LED_PIN_TOP_LEFT_GREEN, LOW);
    digitalWrite(LED_PIN_BOT_LEFT_GREEN, LOW);
    digitalWrite(LED_PIN_BOT_RIGHT_GREEN, LOW);
    digitalWrite(LED_PIN_TOP_RIGHT_GREEN, LOW);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("     Code 1:");
    lcd.setCursor(0, 1);
    lcd.print("      0000");
    start_cursor_pos = 6;
    end_cursor_pos = 9;
    
    current_cursor_pos = start_cursor_pos;
    lcd.setCursor(current_cursor_pos,1);
    lcd.cursor();
  }
  else if (state == 1)
  {
    if (prev_state == 0)
    {
      for (int i = 0;  i < 4; i++)
      {
        if( solution_1[i] != current_code[i] ) 
        {
          Serial.println("not equal");
          //set your boolean flag here
          state--;
          transition_states();
          //state++;
          return;
          //break;
        }
        Serial.println("equal");
      }
    }
    prev_state = 1;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("     Code 2:");
    lcd.setCursor(0, 1);
    lcd.print("      0000");
    start_cursor_pos = 6;
    end_cursor_pos = 9;
    
    current_cursor_pos = start_cursor_pos;
    lcd.setCursor(current_cursor_pos,1);
    lcd.cursor();

    digitalWrite(LED_PIN_TOP_LEFT_RED, LOW);
    digitalWrite(LED_PIN_TOP_LEFT_GREEN, HIGH);

    
  }
  else if (state == 2)
  {
    if (prev_state == 1)
    {
      for (int i = 0;  i < 4; i++)
      {
        if( solution_2[i] != current_code[i] ) 
        {
          Serial.println("not equal");
          //set your boolean flag here
          state--;
          transition_states();
          //state++;
          return;
          //break;
        }
        Serial.println("equal");
      }
    }
    prev_state = 2;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("     Code 3:");
    lcd.setCursor(0, 1);
    lcd.print("      0000");
    start_cursor_pos = 6;
    end_cursor_pos = 9;
    
    current_cursor_pos = start_cursor_pos;
    lcd.setCursor(current_cursor_pos,1);
    lcd.cursor();

    digitalWrite(LED_PIN_TOP_RIGHT_RED, LOW);
    digitalWrite(LED_PIN_TOP_RIGHT_GREEN, HIGH);
  }
  else if (state == 3)
  {
    if (prev_state == 2)
    {
      for (int i = 0;  i < 4; i++)
      {
        if( solution_3[i] != current_code[i] ) 
        {
          Serial.println("not equal");
          //set your boolean flag here
          state--;
          transition_states();
          //state++;
          return;
          //break;
        }
        Serial.println("equal");
      }
    }
    prev_state = 3;

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("     Code 4:");
    lcd.setCursor(0, 1);
    lcd.print("      0000");
    start_cursor_pos = 6;
    end_cursor_pos = 9;
    
    current_cursor_pos = start_cursor_pos;
    lcd.setCursor(current_cursor_pos,1);
    lcd.cursor();

    digitalWrite(LED_PIN_BOT_LEFT_RED, LOW);
    digitalWrite(LED_PIN_BOT_LEFT_GREEN, HIGH);
  }
  else if (state == 4)
  {
    if (prev_state == 3)
    {
      for (int i = 0;  i < 4; i++)
      {
        if( solution_4[i] != current_code[i] ) 
        {
          Serial.println("not equal");
          //set your boolean flag here
          state--;
          transition_states();
          //state++;
          return;
          //break;
        }
        Serial.println("equal");
      }
    }
    prev_state = 4;

    digitalWrite(LED_PIN_BOT_RIGHT_RED, LOW);
    digitalWrite(LED_PIN_BOT_RIGHT_GREEN, HIGH);

    lcd.clear();
    lcd.setCursor(0, 0);
    lcd.print("Congratulations!");
    lcd.setCursor(0, 1);
    lcd.print("Vault is open");
    delay(5000);
    lcd.print("Press the button");
    lcd.setCursor(0, 1);
    lcd.print("to lock!");
    //start_cursor_pos = 6;
    //end_cursor_pos = 9;
    
    //current_cursor_pos = start_cursor_pos;
    //lcd.setCursor(current_cursor_pos,1);
    //lcd.cursor();
    

    door_motor.write(open_position);
  }
}

int check_btn_state()
{
   int reading = digitalRead(ENCODER_BTN);
   

  // check to see if you just pressed the button
  // (i.e. the input went from LOW to HIGH), and you've waited long enough
  // since the last press to ignore any noise:

  // If the switch changed, due to noise or pressing:
  if (reading != lastButtonState) {
    // reset the debouncing timer
    lastDebounceTime = millis();
  }

  if ((millis() - lastDebounceTime) > debounceDelay) {
    // whatever the reading is at, it's been there for longer than the debounce
    // delay, so take it as the actual current state:
    btn_ret_value = HIGH;
    // if the button state has changed:
    if (reading != buttonState) {
      buttonState = reading;
      
      if (buttonState == LOW) 
      {
        btn_ret_value = LOW;
      }
      // only toggle the LED if the new button state is HIGH
      //if (buttonState == HIGH) {
      //  ledState = !ledState;
      //}
    }

  }

  // save the reading. Next time through the loop, it'll be the lastButtonState:
  lastButtonState = reading;

  
  //digitalWrite(LED_PIN_TOP_LEFT_RED, !ret_value);
  

  return btn_ret_value;
}

void read_encoder() {
  // Encoder interrupt routine for both pins. Updates counter
  // if they are valid and have rotated a full indent
 
  static uint8_t old_AB = 3;  // Lookup table index
  static int8_t encval = 0;   // Encoder value  
  static const int8_t enc_states[]  = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0}; // Lookup table

  old_AB <<=2;  // Remember previous state

  if (digitalRead(ENC_A)) old_AB |= 0x02; // Add current state of pin A
  if (digitalRead(ENC_B)) old_AB |= 0x01; // Add current state of pin B
  
  encval += enc_states[( old_AB & 0x0f )];

  // Update counter if encoder has rotated a full indent, that is at least 4 steps
  if( encval > 3 ) {        // Four steps forward
    // int changevalue = 1;
    // if((micros() - _lastIncReadTime) < _pauseLength) {
    //  changevalue = _fastIncrement * changevalue; 
    //}
    //_lastIncReadTime = micros();
    //counter = counter + changevalue;              // Update counter
    if (counter == 9) counter = 0;
    else counter++;
    encval = 0;
  }
  else if( encval < -3 ) {        // Four steps backward
    //int changevalue = -1;
    //if((micros() - _lastDecReadTime) < _pauseLength) {
    //  changevalue = _fastIncrement * changevalue; 
    //}
    //_lastDecReadTime = micros();
    //counter = counter + changevalue;              // Update counter
    if (counter == 0) counter = 9;
    else counter--;
    encval = 0;
  }
}