#include <Servo.h>
#include <LiquidCrystal_I2C.h>
// LiquidCrystal_I2C lcd1(0x27, 16, 2);  // จอ LCD ตัวแรก
LiquidCrystal_I2C lcd(0x27, 20, 4);  // จอ LCD ตัวที่สอง




Servo servo1, servo2, servo3, servo4, servo5;

// กำหนดขา Joystick
#define VERT_PIN A0  // Joystick แกน Y
#define HORZ_PIN A1  // Joystick แกน X
#define VERT_PIN2 A2  // Joystick แกน Y
#define HORZ_PIN2 A3  // Joystick แกน X
#define VERT_PIN3 A4  // Joystick แกน Y
#define HORZ_PIN3 A5  // Joystick แกน X
#define VERT_PIN4 A6  // Joystick แกน Y
#define HORZ_PIN4 A7  // Joystick แกน X
#define VERT_PIN5 A8  // Joystick แกน Y
#define HORZ_PIN5 A9  // Joystick แกน X
#define SEL_PIN 7    // ปุ่มกด

const int servoPins[5] = {2, 3, 4, 5, 6}; // ขาของเซอร์โว

int pos[5] = {0, 0, 0, 0, 0}; // ค่าเริ่มต้นที่ 0 องศา
bool lockServo = false; // ใช้ปุ่มกดเพื่อหยุดการเคลื่อนที่

void setup() {
    Serial.begin(115200);
    

    // ตั้งค่า LCD
    lcd.init();
    lcd.backlight();
    lcd.setCursor(7, 1.5);
    lcd.print("Ready");


    
    // ตั้งค่าขา Joystick
    pinMode(SEL_PIN, INPUT_PULLUP);

    // ผูกเซอร์โวเข้ากับขา
    servo1.attach(servoPins[0]);
    servo2.attach(servoPins[1]);
    servo3.attach(servoPins[2]);
    servo4.attach(servoPins[3]);
    servo5.attach(servoPins[4]);

    // ตั้งตำแหน่งเริ่มต้น
    servo1.write(pos[0]);
    servo3.write(pos[2]);
    servo4.write(pos[3]);
    servo2.write(pos[1]);
    servo5.write(pos[4]);

    Serial.println("Joystick control active!");
    Serial.println("Enter 5 servo angles (e.g., degree[30 60 30 60 90] speed[1]):");
}

void loop() {
    int buttonState = digitalRead(SEL_PIN);

    if (buttonState == LOW) {
        lockServo = !lockServo; // กดปุ่มเพื่อสลับ lock/unlock
        Serial.println(lockServo ? "Servo Locked" : "Servo Unlocked");
        delay(300);
    }

    if (!lockServo) {
        int vertValue = analogRead(VERT_PIN); // อ่านค่าแกน Y
        int horzValue = analogRead(HORZ_PIN); // อ่านค่าแกน X
        int vertValue2 = analogRead(VERT_PIN2); // อ่านค่าแกน Y
        int horzValue2 = analogRead(HORZ_PIN2); // อ่านค่าแกน X
        int vertValue3 = analogRead(VERT_PIN3); // อ่านค่าแกน Y
        int horzValue3 = analogRead(HORZ_PIN3); // อ่านค่าแกน X
        int vertValue4 = analogRead(VERT_PIN4); // อ่านค่าแกน Y
        int horzValue4 = analogRead(HORZ_PIN4); // อ่านค่าแกน X
        int vertValue5 = analogRead(VERT_PIN5); // อ่านค่าแกน Y
        int horzValue5 = analogRead(HORZ_PIN5); // อ่านค่าแกน X

        // แปลงค่าจาก 0-1023 เป็น 0-180 องศา

        if (Serial.available() > 0) {
        // อ่านค่ามุมจาก Serial Monitor
        for (int i = 0; i < 5; i++) {
            pos[i] = Serial.parseInt(); // อ่านค่าทีละตัว
        }
        int Speed = Serial.parseInt(); // ต่ำสุดที่ 1 เร็วสุด 

        while (Serial.available() > 0) Serial.read();  // ล้างค่าค้างใน Serial
        
        // ตรวจสอบค่าที่รับมา
        Serial.print("Received: ");
        for (int i = 0; i < 5; i++) {
            Serial.print(pos[i]);
            Serial.print(" ");
        }
        Serial.println(Speed);

        // ปรับมุมเซอร์โว

        int maxValue = findMax(pos,5);
        
        for (int i = 0; i <= maxValue; i++) {
        lcd.clear();
        lcd.setCursor(6, 0);
        lcd.print("Servo(S)");   
        if (i <= pos[0]){
        servo2.write(i);
        lcd.setCursor(0, 1);
        lcd.print("S1: ");
        lcd.print(i);
        }else{
        lcd.setCursor(0, 1);
        lcd.print("S1: ");
        lcd.print(pos[0]);
        }
        if (i <= pos[1]){
        servo3.write(i);
        lcd.print("       S2: ");
        lcd.print(i);
        }else{
          
        lcd.print("       S2: ");
        lcd.print(pos[1]);
        }
        if (i <= pos[2]){
        servo1.write(i);
        lcd.setCursor(0, 2);
        lcd.print("S3: ");
        lcd.print(i);
        }else{
          lcd.setCursor(0, 2);
        lcd.print("S3: ");
        lcd.print(pos[2]);
        }
        if (i <= pos[3]){
        servo4.write(i);
        lcd.print("       S4: ");
        lcd.print(i);
        }else{
         
        lcd.print("       S4: ");
        lcd.print(pos[3]);
        }
        if (i <= pos[4]){
        servo5.write(i);
        lcd.setCursor(7, 3);
        lcd.print("S5: ");
        lcd.print(i);
        }else{
         
        lcd.setCursor(7, 3);
        lcd.print("S5: ");
        lcd.print(pos[4]);
        }
             
        
        delay(Speed);

        }

        

        // แสดงค่าบน LCD
        // lcd.clear();
        // lcd.setCursor(6, 0);
        // lcd.print("Servo(S)");        
        
        // lcd.setCursor(0, 1);
        // lcd.print("S1: ");
        // lcd.print(pos[0]);
        // lcd.print("       S2: ");
        // lcd.print(pos[1]);

        // lcd.setCursor(0, 2);
        // lcd.print("S3: ");
        // lcd.print(pos[2]);
        // lcd.print("       S4: ");
        // lcd.print(pos[3]);

        // lcd.setCursor(7, 3);
        // lcd.print("S5: ");
        // lcd.print(pos[4]);
    }

        if ((horzValue == 0) || (vertValue == 0)) {       // กดขึ้น
            pos[0] = min(pos[0] + 1, 180);
        } else if ((horzValue == 1023) || (vertValue == 1023)) {  // กดลง
            pos[0] = max(pos[0] - 1, 0);
        }

        if ((horzValue2 == 0) || (vertValue2 == 0)) {      
            pos[1] = min(pos[1] + 1, 180);
        } else if ((horzValue2 == 1023) || (vertValue2 == 1023)) { 
            pos[1] = max(pos[1] - 1, 0);
        }

        if ((horzValue3 == 0) || (vertValue3 == 0)) {       // กดขึ้น
            pos[2] = min(pos[2] + 1, 180);
        } else if ((horzValue3 == 1023) || (vertValue3 == 1023)) {  // กดลง
            pos[2] = max(pos[2] - 1, 0);
        }

        if ((horzValue4 == 0) || (vertValue4 == 0)) {       // กดขึ้น
            pos[3] = min(pos[3] + 1, 180);
        } else if ((horzValue4 == 1023) || (vertValue4 == 1023)) {  // กดลง
            pos[3] = max(pos[3] - 1, 0);
        }

        if ((horzValue5 == 0) || (vertValue5 == 0)) {       // กดขึ้น
            pos[4] = min(pos[4] + 1, 180);
        } else if ((horzValue5 == 1023) || (vertValue5 == 1023)) {  // กดลง
            pos[4] = max(pos[4] - 1, 0);
        }
        // pos[0] = (vertValue != 512) ? map(vertValue, 512, 0, 0, 180) : map(horzValue, 512, 0, 0, 180);
        // pos[1] = (vertValue2 != 512) ? map(vertValue2, 512, 0, 0, 180) : map(horzValue2, 512, 0, 0, 180);
        // pos[2] = (vertValue3 != 512) ? map(vertValue3, 512, 0, 0, 180) : map(horzValue3, 512, 0, 0, 180);
        // pos[3] = (vertValue4 != 512) ? map(vertValue4, 512, 0, 0, 180) : map(horzValue4, 512, 0, 0, 180);
        // pos[4] = (vertValue5 != 512) ? map(vertValue5, 512, 0, 0, 180) : map(horzValue5, 512, 0, 0, 180);

       
        servo2.write(pos[0]);
        servo3.write(pos[1]);
        servo1.write(pos[2]);
        servo4.write(pos[3]);
        servo5.write(pos[4]);

        lcd.clear();
        lcd.setCursor(6, 0);
        lcd.print("Servo(S)");        
        
        lcd.setCursor(0, 1.5);
        lcd.print("S1: ");
        lcd.print(pos[0]);
        lcd.print("       S2: ");
        lcd.print(pos[1]);

        lcd.setCursor(0, 2.5);
        lcd.print("S3: ");
        lcd.print(pos[2]);
        lcd.print("       S4: ");
        lcd.print(pos[3]);

        lcd.setCursor(7, 3.5);
        lcd.print("S5: ");
        lcd.print(pos[4]);
        
       
        delay(50);
    }
}


int findMax(int arr[], int size) {
    int maxValue = arr[0];  // เริ่มจากค่าตัวแรกของอาร์เรย์
    for (int i = 1; i < size; i++) {  
        if (arr[i] > maxValue) {
            maxValue = arr[i];  // อัปเดตค่ามากสุด
        }
    }
    return maxValue;  // คืนค่ามากสุด
}