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

LiquidCrystal_I2C lcd(0x27, 16, 2); 

int servo_pin[] = {10, 9, 6, 5};
Servo servo[4];

int btn_pin = 2;
int TrTh_Btn;
bool DaHanhdong = false;
bool KhoaJoystick = false;

int pinX = A0;
int pinY = A1; 

void setup() {
  pinMode(pinX, INPUT);
  pinMode(pinY, INPUT);
  lcd.init();
  lcd.backlight();

  pinMode(btn_pin, INPUT_PULLUP);
  
  for(int i = 0; i < 4; i++) {
    servo[i].attach(servo_pin[i]);
  }

  for(int i = 0; i < 4; i++) {
    servo[i].write(0);
  }

  TrTh_Btn = digitalRead(btn_pin);
}

unsigned long ThG_Giu = 0;

void xulyButton() {
  int TrTh_HT = digitalRead(btn_pin);

  if(TrTh_HT == LOW && TrTh_Btn == HIGH) {
    ThG_Giu = millis();
    DaHanhdong = false;
  }

  if(TrTh_HT == HIGH && TrTh_Btn == LOW && !DaHanhdong) {
    if(millis() - ThG_Giu >= 3000) {
      // Đổi trạng thái khóa joystick
      KhoaJoystick = !KhoaJoystick;  
      for (int i = 0; i < 4; i++) {
        servo[i].write(180);
      }
      DaHanhdong = true;
    }
  }
  TrTh_Btn = TrTh_HT;
}

void xulyJoystick() {
  // Dừng xử lý joystick nếu đang khóa
  if (KhoaJoystick) {
    return; 
  }

  int xValue = analogRead(pinX);
  int yValue = analogRead(pinY);

  lcd.setCursor(5, 0);
  lcd.print("X: " + String(xValue));

  lcd.setCursor(5, 1); 
  lcd.print("Y: " + String(yValue)); 

  int angle1 = map(xValue, 0, 1023, 0, 180);
  int angle2 = 180 - angle1;
  servo[0].write(angle1);
  servo[1].write(angle2);
  
  int angle3 = map(yValue, 0, 1023, 0, 180);
  int angle4 = angle3;
  servo[2].write(angle3);
  servo[3].write(angle4);
}

void loop() {
  xulyButton();
  xulyJoystick();
}