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

Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;

LiquidCrystal_I2C lcd(0x27,16,2);

int button1 = 2;
int button2 = 13;

int gocX = A1;
int gocY = A0;

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

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

  pinMode(button1, INPUT_PULLUP);
  pinMode(button2, INPUT_PULLUP);

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

}

void loop() {
  int state1 = digitalRead(button1);
  static int mode = 0;
  int state2 = digitalRead(button2);
  static bool press = false;
  static bool rl = false;
  static unsigned long buttonTime = 0;
  static unsigned long rlTime = 0;
  static bool firstTime = true;
  if(state1 == LOW) {
    delay(200);
    mode++;
    if(mode>3)
      mode = 1;
    lcd.setCursor(0,1);
    if(mode == 1){
        lcd.print("Mode Joystick");
        lcd.print("                ");
    } else if(mode == 2) {
      lcd.print("Mode Auto");
      lcd.print("                ");
      firstTime = true;
    } else if(mode == 3) {
      lcd.print("Pause");
      lcd.print("                ");
    }
  }
  
  if(state2 == LOW && !press) {
    press = true;
    rl = false;
    buttonTime = millis();
  }

  if(state2 == HIGH && press) {
    press = false;
    rl = true;
    rlTime = millis();
  }

  if(press && (millis() - buttonTime >= 5000)) {
    servo1.write(90);
    servo2.write(90);
    servo3.write(90);
    servo4.write(90);
  }

  // if(rl && (millis() - rlTime >= 3000)) {
  //   servo1.write(0);
  //   servo2.write(0);
  //   servo3.write(0);
  //   servo4.write(0);
  // }

//mode2: Servo 1: Xoay từ 0 độ đến 180 độ trong 3 giây, sau đó quay lại 0 độ trong 2 giây.
// Servo 2: Xoay từ 0 độ đến 90 độ trong 1.5 giây, giữ nguyên 90 độ trong 2 giây, sau đó quay lại 0 độ trong 1 giây.
// Servo 3 và Servo 4: Xoay từ 0 độ đến 180 độ trong 4 giây, giữ nguyên 180 độ trong 3 giây, sau đó quay lại 0 độ trong 2 giây.
// Lặp lại chuỗi này cho đến khi có thay đổi chế độ hoặc dừng lại bằng Button thứ hai.

  if(mode == 1) {
    int xValue = analogRead(gocX);
    int yValue = analogRead(gocY);

    if(xValue < 462) {
      servo1.write(min(servo1.read() + 20, 180));
      servo2.write(max(servo2.read() - 20, 0));
    } else if(xValue > 562) {
      servo1.write(max(servo1.read() - 20, 0));
      servo2.write(min(servo2.read() + 20, 180));
    }

    if(yValue < 462) {
      servo3.write(min(servo3.read() + 20, 180));
      servo4.write(min(servo4.read() + 20, 180));
    } else if(yValue > 562) {
      servo3.write(max(servo3.read() - 20, 0));
      servo4.write(max(servo4.read() - 20, 0));
    }

    delay(100);
  } else if(mode == 2){
      autoControl(firstTime);
      firstTime = false;
  } else if(mode == 3){
      reset();
  }

}

// Hàm cho mode 2
int getStep(int step) {
  switch(step) {
    case 0:
      return 3000;
    case 1:
      return 2000;
    case 2:
      return 1500;
    case 3:
      return 2000;
    case 4:
      return 1000;
    case 5:
      return 4000;
    case 6:
      return 3000;
    case 7:
      return 2000;
    default:
      return 0;
  }
}

void autoControl(bool firstTime){
  static unsigned long time = 0;
  static int step = 0;

  if(firstTime) {
    time = millis();
  }

  if(millis() - time >= getStep(step)) {
    time = millis();
    step = (step + 1) % 8;

    switch(step) {
        case 0:
          servo1.write(0);
          break;
        case 1:
          servo1.write(180);
          break;
        case 2:
          servo2.write(0);
          break;
        case 3:
          servo2.write(90);
          break;
        case 4:
          servo2.write(90);
          break;
        case 5:
          servo3.write(0);
          servo4.write(0);
          break;
        case 6:
          servo3.write(180);
          servo4.write(180);
          break;
        case 7:
          servo3.write(180);
          servo4.write(180);
          break;
    }
  }

  if(step == 0) {
        int quay = (180 * (millis() - time)) / 3000;
        servo1.write(quay);
  }

    if(step == 1) {
        int quay = (180 * (millis() - time)) / 2000;
        servo1.write(180-quay);
  }

    if(step == 2) {
        int quay = (90 * (millis() - time)) / 1500;
        servo2.write(quay);
  }


    if(step == 4) {
        int quay = (90 * (millis() - time)) / 1000;
        servo2.write(90 - quay);
  }

    if(step == 5) {
        int quay = (180 * (millis() - time)) / 4000;
        servo3.write(quay);
        servo4.write(quay);
  }

    if(step == 7) {
        int quay = (180 * (millis() - time)) / 2000;
        servo3.write(180 - quay);
        servo4.write(180 - quay);
  }
}

void reset(){
  servo1.write(0);
  servo2.write(0);
  servo3.write(0);
  servo4.write(0);
}