#include <Key.h>
#include <Keypad.h>
#include <LiquidCrystal.h>
//------------------------------------------------------------------
//------------------------------------------------------------------

// declare pins
const byte ROWS = 4; //Keypad rows 
const byte COLS = 3; //Keypad columns
byte rowsPins[ROWS] {8, 7, 6, 5}; //Keypad rows pins
byte colsPins[COLS] {4, 3, 2}; //Keypad columns pins

const int rs = A0, en = A1, d4 = A2, d5 = A3, d6 = A4, d7 = A5; //LCD pins

const int redPin = 9; //R LED pin
const int greenPin = 10; //G LED pin
const int bluePin = 11; //B LED pin

const int Echo = 13; //Ultrasonic ECHO pin
const int Trig = 12; //Ultrasonic TRIG pin
//------------------------------------------------------------------

//declare variables
int i = 0;
String passcode = "12345";
String code;

int redValue;
int greenValue;
int blueValue;

float duration, distance;

int menu = 0;
//------------------------------------------------------------------
//------------------------------------------------------------------

//keypad
char hexaKeys [ROWS][COLS] = {
    {'1', '2', '3'},
    {'4', '5', '6'},
    {'7', '8', '9'},
    {'*', '0', '#'}
};

Keypad numKeypad = Keypad(makeKeymap(hexaKeys), rowsPins, colsPins, ROWS, COLS);
//------------------------------------------------------------------

//LCD
LiquidCrystal lcd(rs, en, d4, d5, d6, d7);
//------------------------------------------------------------------

//RGB LED
void SetColor(int red, int green, int blue) {
    analogWrite(redPin, red);
    analogWrite(greenPin, green);
    analogWrite(bluePin, blue);
}
//------------------------------------------------------------------
//------------------------------------------------------------------

//Menu setup
void StateCheck(int state) {
    if (state == 0) {
        menu = 0;
        i = 0;
        code = "";
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Passcode:");
        lcd.setCursor(0, 1);
    } else if (state == 1) {
        menu = 1;
        i = 0;
        code = "";
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("1) Led test");
        lcd.setCursor(0, 1);
        lcd.print("2) Distance test");
        delay(3000);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("3) Keypad test");
        lcd.setCursor(0, 1);
        lcd.print("4) Logout");
        delay(3000);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("5) Set passcode");
        delay(3000);
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Select option:");
        lcd.setCursor(0, 1);
    }
}
//------------------------------------------------------------------
//------------------------------------------------------------------

void setup() {
    pinMode(Trig, OUTPUT);
    pinMode(Echo, INPUT);

    Serial.begin(9600);

    lcd.clear();
    lcd.begin(16, 2);
    StateCheck(0);
}

void loop() {
    char customKey = numKeypad.getKey();

    switch (menu) {
        case 0:
            if (customKey && i <= 4) {
                lcd.setCursor(i,1);
                lcd.print('*');
                //lcd.print(customKey);
                code = code + customKey;
                i++;
                delay(300);
            }

            if (i == 5) {
                Serial.println("Code entered!");
                if (code == passcode){
                    Serial.println("Correct code!");
                    SetColor(0, 255, 0); // Green
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Correct code!"); 
                    delay(2000);
                    SetColor(0, 0, 0);
                    StateCheck(1);
                } else {
                    Serial.println("Incorrect code!");
                    SetColor(255, 0, 0); // Red
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Incorrect code!");
                    delay(2000);
                    SetColor(0, 0, 0);
                    StateCheck(0);

                }
            }
        break;
//------------------------------------------------------------------ Led test
        case 1:                                                   
            if(customKey){
                Serial.println(customKey);
                if (customKey == '1'){
                    menu = 11;
                    Serial.println("LED test selected!");
                } else if (customKey == '2'){
                    menu = 12;
                    Serial.println("Distance test selected!");
                } else if (customKey == '3'){
                    menu = 13;
                    Serial.println("Keypad test selected!");
                } else if (customKey == '4'){
                    menu = 14;
                    Serial.println("Logout selected!");
                } else if (customKey == '5'){
                    menu = 15;
                    Serial.println("Setting is selected!");
                }
            }
        break;

        case 11:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("LED test");
            lcd.setCursor(0, 1);
            lcd.print("selected!");
            delay(1500);
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Red v. 0-255:");
            lcd.setCursor(0, 1);
            menu = 111;
        break;

        case 111: 
            if (customKey && i <= 3){
                lcd.setCursor(i, 1);
                lcd.print(customKey);
                code = code + customKey;
                i++;
                delay(300);
            } else if (i == 3) {
                redValue = code.toInt();
                Serial.println(redValue);
                i = 0;
                code = "";
                lcd.clear();
                lcd.print("Green v. 0-255:");
                lcd.setCursor(0, 1);
                menu = 112;
            }

        break;
                
        case 112: 
            if (customKey && i <= 3){
                lcd.setCursor(i, 1);
                lcd.print(customKey);
                code = code + customKey;
                i++;
                delay(300);
            } else if (i == 3) {
                greenValue = code.toInt();
                Serial.println(greenValue);
                i = 0;
                code = "";
                lcd.clear();
                lcd.print("Blue v. 0-255:");
                lcd.setCursor(0, 1);
                menu = 113;
            }
        break;

        case 113:
            if (customKey && i <= 3){
                lcd.setCursor(i, 1);
                lcd.print(customKey);
                code = code + customKey;
                i++;
                delay(300);
            } else if (i == 3) {
                blueValue = code.toInt();
                Serial.println(blueValue);
                i = 0;
                code = "";
                SetColor(redValue, greenValue, blueValue);
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("LED test done!");
                delay(4000);
                SetColor(0, 0, 0);
                StateCheck(1);
            }
        break;
//------------------------------------------------------------------ US
        case 12:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Distance test");
            lcd.setCursor(0, 1);
            lcd.print("selected!");
            delay(1500);
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Select mode:");
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("1) LED");
            lcd.setCursor(0, 1);
            lcd.print("2) Buzzer");
            menu = 121;
        break;

        case 121:
            if(customKey) {
                if (customKey == '1') {
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Switch LEFT!");
                    delay(2000);
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Distance: [cm]");
                    lcd.setCursor(0, 1);
                    menu = 122;
                    Serial.println("LED mode selected!");
                } else if (customKey == '2') {
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Switch RIGHT!");
                    delay(2000);
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Distance: [cm]");
                    lcd.setCursor(0, 1);
                    pinMode(9, OUTPUT);
                    menu = 123;
                    Serial.println("Buzzer mode selected!");
                }
            }
        break;
        case 122:
            digitalWrite(Trig, LOW);
            delayMicroseconds(2);
            digitalWrite(Trig, HIGH);
            delayMicroseconds(10);
            digitalWrite(Trig, LOW);
        
            duration = pulseIn(Echo, HIGH);
            distance = (duration*.0343)/2;
            lcd.setCursor(0, 1);
            lcd.print(distance);
            delay(500);
            if (distance >= 0 && distance <= 50) {
                SetColor(255, 0, 0); // Red
            } else if (distance >= 51 && distance <= 80) {
                SetColor(255, 165, 0); // Orange
            } else if(distance >= 81 && distance <= 120) {
                SetColor(255, 255, 0); // Yellow
            } else if (distance >= 121 && distance <= 400){
                SetColor(0, 255, 0); // Green
            }

            if (customKey == '#'){
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Distance test");
                lcd.setCursor(0, 1);
                lcd.print("done!");
                delay(2000);
                SetColor(0, 0, 0);
                StateCheck(1);
            }
        break;

        case 123:
            digitalWrite(Trig, LOW);
            delayMicroseconds(2);
            digitalWrite(Trig, HIGH);
            delayMicroseconds(10);
            digitalWrite(Trig, LOW);
        
            duration = pulseIn(Echo, HIGH);
            distance = (duration*.0343)/2;
            lcd.setCursor(0, 1);
            lcd.print(distance);
            delay(500);
            if (distance >= 0 && distance <= 50) {
                tone(9, 1000);
                delay(1000);
            } else if (distance >= 51 && distance <= 80) {
                tone(9, 750);
                delay(1000);
            } else if(distance >= 81 && distance <= 120) {
                tone(9, 500);
                delay(1000);
            } else if (distance >= 121 && distance <= 400){
                tone(9, 250);
                delay(1000);
            }
            noTone(9);
            delay(500);

            if (customKey == '#'){
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Distance test");
                lcd.setCursor(0, 1);
                lcd.print("done!");
                delay(2000);
                SetColor(0, 0, 0);
                StateCheck(1);
            }
        break;
//------------------------------------------------------------------ Kp test
        case 13:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Keypad test");
            lcd.setCursor(0, 1);
            lcd.print("selected!");
            delay(1500);
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("To esc press #");
            delay(3000);
            menu = 131;
        break;

        case 131:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Press any key:");
            lcd.setCursor(0, 1);
            i = 0;
            menu = 132;
        break;

        case 132:
            if (customKey && i <= 15) {
                lcd.setCursor(i, 1);
                lcd.print(customKey);
                i++;
                if (customKey == '#'){
                    lcd.clear();
                    lcd.setCursor(0, 0);
                    lcd.print("Keypad test done!");
                    delay(2000);
                    StateCheck(1);
                }
            }
            if (i == 15) {
                menu  = 131;
            }
        break;
//------------------------------------------------------------------ Log out
        case 14:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Logging out...");
            delay(1500);
            StateCheck(0);
        break;
//------------------------------------------------------------------ Set passcode
        case 15:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("Setting is");
            lcd.setCursor(0, 1);
            lcd.print("loading...");
            delay(1500);
            menu = 151;
        break;

        case 151:
            lcd.clear();
            lcd.setCursor(0, 0);
            lcd.print("New passcode:");
            lcd.setCursor(0, 1);
            code = "";
            i = 0;
            menu = 152;
        break;

        case 152:
            if (customKey && i <= 4) {
                lcd.setCursor(i, 1);
                lcd.print(customKey);
                code = code + customKey;
                i++;
                delay(300);
            }

            if (i == 5) {
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Correct? Y=1 N=0");
                lcd.setCursor(0, 1);
                lcd.print(code);
                menu = 153;
            }
        break;

        case 153:
            if (customKey == '1') {
                //Serial.println(code);
                passcode = code;
                Serial.println("New passcode: " + passcode);
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("Passcode set!");
                delay(2000);
                StateCheck(1);
            } else if (customKey == '0') {
                lcd.clear();
                lcd.setCursor(0, 0);
                lcd.print("False passcode!");
                delay(2000);
                menu = 151;
            }
        break;
//------------------------------------------------------------------
    break;
    }
}