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

LiquidCrystal_I2C lcd(0x27, 16, 2);
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

int button = 2;
int button2 = 13;

int gocX = A1;
int gocY = A0;

int controlJoyStick(int value) {
  return map(value, 0, 1023, 0, 180);
}

void setup() {
  servo1.attach(3);
  servo2.attach(5);
  servo3.attach(6);
  servo4.attach(9);

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Ready");

  pinMode(button, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);

  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);

}



void loop() {
    // Nút chuyển đổi câu e và câu f tránh gây xung đột
    int state2 = digitalRead(button2);
    static bool mode = false;

    if(state2 == LOW) {
      mode = !mode;
      delay(200);
    }

    if(!mode) {
        //Câu e
        int state = digitalRead(button);
        static unsigned long buttonTime = 0;
        static bool press = false;

        if(state == LOW && !press ) {
              press = true;
              buttonTime = millis();
        }

        if(state == HIGH && press) {
            press = false;
        }

        if(press && (millis() - buttonTime >= 3000 )) {
            servo1.write(180);
            servo2.write(180);
            servo3.write(180);
            servo4.write(180);
        }
    } else if(mode == true){
        // Câu f
        int joyXValue = analogRead(gocX);
        int joyYValue = analogRead(gocY);

        int valueX = controlJoyStick(joyXValue);
        int valueY = controlJoyStick(joyYValue);

        lcd.clear();
        lcd.setCursor(6,0);
        lcd.print("X: "); lcd.print(valueX);
        lcd.setCursor(6,1);
        lcd.print("Y: "); lcd.print(valueY);

    //     Đọc giá trị toạ độ X: Điều khiển động cơ Servo số 1 và số 2 với 2 chiều ngược
    // nhau

        servo1.write(valueX);
        servo2.write(180 - valueX);

    //     Đọc giá trị toạ độ Y: Điều khiển động cơ Servo số 3 và số 4 với 2 chiều thuận
    // nhau
        servo3.write(valueY);
        servo4.write(valueY);
    }
     delay(200);
}