#include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C lcd(0x27, 16, 2);
#define TrucNgang A0
#define TrucDoc A1
#define NutJoy 2 
#define NutReset 7 

#include <Servo.h>
Servo myservo[6]; 
int ServoPin[]={6,9,10,11};
int IdxServo=0; 
int Goc[]={0,0,0,0}; 

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.print("Servo: 0");
  lcd.setCursor(0, 1);
  lcd.print("Goc: 0");

  //Setup các nút joystick
  pinMode(TrucNgang, INPUT);
  pinMode(TrucDoc, INPUT);
  pinMode(NutJoy, INPUT_PULLUP);
  pinMode(NutReset, INPUT_PULLUP);
  Serial.begin(9600);
  //khai báo cho các servo và set tọa độ về 0
  for(int i=0;i<4;i++){
    myservo[i].attach(ServoPin[i]);
    myservo[i].write(Goc[i]);
  }
}

void loop() {

  //khi bấm nút ở giữ sẽ chuyển đổi sang con servo khác
  if(digitalRead(NutJoy) == LOW){
    IdxServo += 1;
    IdxServo = IdxServo % 4; 
  }

  //đọc trục ngang, dọc của joystick
  int TNgang = analogRead(TrucNgang);
  int TDoc = analogRead(TrucDoc);
  // Trục ngang từ 0 (phải) tới 1023 (trái)
  // Trục dọc từ 0 (dưới) tới 1023 (trên)
  // Khi không bấm gì thì tọa độ sẽ là ngang 512 và dọc là 512
  
  if(TDoc == 1023){ //Nếu lên thì tăng 1
    Goc[IdxServo] += 1;
  }else if(TDoc == 0){ //Nếu xuống thì giảm 1
    Goc[IdxServo] -= 1;
  }
  if(TNgang == 1023){ //Nếu trái thì tăng 3
    Goc[IdxServo] += 3;
  }else if(TNgang == 0){ //Nếu phải thì giảm 3
    Goc[IdxServo] -= 3;
  }

  //Kiểm tra xem có quá giới hạn từ 0-180 không
  if(Goc[IdxServo]>180){ //Nếu trên 180 thì cho về 180
    Goc[IdxServo]=180;
  }else if(Goc[IdxServo]<0){ //Nếu dưới 0 thì cho về 0
     Goc[IdxServo]=0;
  }

  myservo[IdxServo].write(Goc[IdxServo]);

  if(digitalRead(NutReset) == LOW){
    for(int i=0;i<4;i++){
      Goc[i]=180;
      myservo[i].write(Goc[i]);
    }
  }
  //Hiển thị Servo đg chọn và góc của servo đó
  lcd.setCursor(7, 0);
  lcd.print(IdxServo+1); //Servo đg chọn
  lcd.setCursor(5, 1);
  lcd.print(Goc[IdxServo]); //Góc của servo
  lcd.print("  ");
  
  Serial.print("Servo đang chọn: ");
  Serial.print(IdxServo);
  Serial.println();

  delay(50);
}