#define led_F1 2
#define led_F2 3
#define led_UP 4
#define led_DOWN 5
#define trigger 11
#define eccho 10

#include <Keypad.h>
#include <Servo.h> 
#include <LiquidCrystal_I2C.h>

#define LCD_ADDRESS 0x27  // Change this address if needed
#define LCD_COLUMNS 16
#define LCD_ROWS 2
LiquidCrystal_I2C lcd(LCD_ADDRESS, LCD_COLUMNS, LCD_ROWS);

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

byte rowPins[ROWS] = {53, 52, 51, 50};
byte colPins[COLS] = {49, 48, 47, 46};
Keypad keypad = Keypad(makeKeymap(keys), rowPins, colPins, ROWS, COLS);

bool isAtFloor1 = true;
bool isAtFloor2 = false;
bool isMoving = false; // track lift move
char key;

void setup() {
    Serial.begin(9600);
    pinMode(led_F1, OUTPUT);
    pinMode(led_F2, OUTPUT);
    pinMode(trigger, OUTPUT);
    pinMode(eccho, INPUT);

    digitalWrite(led_F1, HIGH);  // ไฟที่ชั้น 1 ติด
    digitalWrite(led_F2, LOW);   // ไฟที่ชั้น 2 ดับ

    doorservo.attach(13);
    doorservo.write(0);

    powerservo.attach(12);
    powerservo.write(180);

    lcd.init();
    lcd.backlight();
}

void loop() {
    key = keypad.getKey();

    if (key) {
        switch (key) {
            case '*':
                if (isAtFloor1) {
                    if (doordetected()) {
                        doorservo.write(90);
                        delay(5000);
                        doorservo.write(0);
                    }
                } else {
                    isMoving = true;  // เริ่มการเคลื่อนที่
                    moveToFloor(1);
                    isMoving = false; // การเคลื่อนที่เสร็จสิ้น
                }
                break;

            case '#':
                if (isAtFloor2) {
                    if (doordetected()) {
                        doorservo.write(90);
                        delay(5000);
                        doorservo.write(0);
                    }
                } else {
                    isMoving = true;  // เริ่มการเคลื่อนที่
                    moveToFloor(2);
                    isMoving = false; // การเคลื่อนที่เสร็จสิ้น
                }
                break;

            case '1':
                if (isAtFloor2) {
                    isMoving = true;  // เริ่มการเคลื่อนที่
                    moveToFloor(1);
                    isMoving = false; // การเคลื่อนที่เสร็จสิ้น
                }
                break;

            case '2':
                if (isAtFloor1) {
                    isMoving = true;  // เริ่มการเคลื่อนที่
                    moveToFloor(2);
                    isMoving = false; // การเคลื่อนที่เสร็จสิ้น
                }
                break;

            default:
                break;
        }
    }

    updateDisplay();
    delay(500); // หน่วงเวลา 500ms เพื่อลดความเร็วในการอัปเดต
}

void moveToFloor(int floor) {
    unsigned long startTime = millis(); // เก็บเวลาเริ่มต้น
    unsigned long durationcheck = 10000; // เวลาเคลื่อนที่ 10 วินาที

    if (floor == 1) {
        digitalWrite(led_F1, HIGH);
        digitalWrite(led_F2, LOW);

        while (millis() - startTime < durationcheck) {
            digitalWrite(led_DOWN, 1);
            unsigned long elapsedTime = millis() - startTime;
            int position = map(elapsedTime, 0, durationcheck, 0, 180);
            powerservo.write(position);
            delay(20); // ปรับความเร็วของเซอร์โว
            updateDisplay(); // อัปเดตหน้าจอ LCD ขณะลิฟต์ขยับ
        }

        // ตั้งค่าเซอร์โวให้ตรงตำแหน่งสุดท้าย
        powerservo.write(180);
        digitalWrite(led_DOWN, 0);

        isAtFloor1 = true;
        isAtFloor2 = false;

        doorservo.write(90);
        delay(5000);
        doorservo.write(0);
    }
    else if (floor == 2) {
        digitalWrite(led_F1, LOW);
        digitalWrite(led_F2, HIGH);

        while (millis() - startTime < durationcheck) {
            digitalWrite(led_UP, 1);
            unsigned long elapsedTime = millis() - startTime;
            int position = map(elapsedTime, 0, durationcheck, 180, 0);
            powerservo.write(position);
            delay(20); // ปรับความเร็วของเซอร์โว
            updateDisplay(); // อัปเดตหน้าจอ LCD ขณะลิฟต์ขยับ
        }

        // ตั้งค่าเซอร์โวให้ตรงตำแหน่งสุดท้าย
        powerservo.write(0);
        digitalWrite(led_UP, 0);

        isAtFloor2 = true;
        isAtFloor1 = false;

        doorservo.write(90);
        delay(5000);
        doorservo.write(0);
    }

    // ตรวจสอบและอัปเดตสถานะลิฟต์
    isMoving = false;
    updateDisplay();
}

bool doordetected() {
    long duration;
    int distance;
    
    // set ultrasonic
    digitalWrite(trigger, LOW);
    delayMicroseconds(2);
    digitalWrite(trigger, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigger, LOW);

    duration = pulseIn(eccho, HIGH);

    distance = duration * 0.0344 / 2;

    return distance <= 50;
}

void updateDisplay() {
    lcd.clear();
    if (isAtFloor1) {
        lcd.setCursor(0, 0);
        lcd.print("Floor 1");
    } else if (isAtFloor2) {
        lcd.setCursor(0, 0);
        lcd.print("Floor 2");
    }

    lcd.setCursor(0, 1);
    if (isMoving) {
        lcd.print("Moving");
    } else {
        lcd.print("Ready");
    }
}