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

LiquidCrystal_I2C lcd(0x27,20,4);

Servo myservo[4];

#define Vert A1
#define Horz A0
#define BtnA 13
#define BtnB 12
#define BtnC 11

long WhenClick=0;
bool ispress=false;

int pinServo[]={3,5,6,9};
int angleNow[]={0,0,0,0};
int angleSaveA[]={0,0,0,0};
int angleSaveB[]={0,0,0,0};

bool BtnCClick=false;

void setup() {
  lcd.init();
  lcd.backlight();
  lcd.print("asd");

  for(int i=0;i<4;i++){
    myservo[i].attach(pinServo[i]);
    myservo[i].write(0);
  }
  pinMode(Vert, INPUT);
  pinMode(Horz, INPUT);
  pinMode(BtnA, INPUT_PULLUP);
  pinMode(BtnB, INPUT_PULLUP);
  pinMode(BtnC, INPUT_PULLUP);
}
bool issame(int A[],int B[]){
  if(A[0]==B[0] && A[1]==B[1] && A[2]==B[2] && A[3]==B[3])
    return true;
  return false;
}

void loop() {
  int TrucX=analogRead(Vert);
  int TrucY=analogRead(Horz);
  lcd.clear();
  lcd.print("X:");
  lcd.print(TrucX);
  lcd.print(" ");
  lcd.print("Y:");
  lcd.print(TrucY);

  //Tọa độ X tăng điều khiển Servo 1,2
  if(TrucX>600){
    angleNow[0]+=1;
    angleNow[1]+=1;
    if(angleNow[0]>180)
      angleNow[0]=180;
    if(angleNow[1]>180)
      angleNow[1]=180;
  }
  //Tọa độ X giảm điều khiển Servo 1,2
  if(TrucX<400){
    angleNow[0]-=1;
    angleNow[1]-=1;
    if(angleNow[0]<0)
      angleNow[0]=0;
    if(angleNow[1]<0)
      angleNow[1]=0;
  }


  //Tọa độ Y tăng điều khiển Servo 3,4
  if(TrucY>600){
    angleNow[2]+=1;
    angleNow[3]+=1;
    if(angleNow[2]>180)
      angleNow[2]=180;
    if(angleNow[3]>180)
      angleNow[3]=180;
  }
  //Tọa độ Y giảm điều khiển Servo 3,4
  if(TrucY<400){
    angleNow[2]-=1;
    angleNow[3]-=1;
    if(angleNow[2]<0)
      angleNow[2]=0;
    if(angleNow[3]<0)
      angleNow[3]=0;
  }
  //Lưu tọa độ khi bấm nút A
  if(!digitalRead(BtnA)){
    for(int i=0;i<4;i++){
      angleSaveA[i]=angleNow[i];
    }
  }
  //Lưu tọa độ khi bấm nút B
  if(!digitalRead(BtnB)){
    for(int i=0;i<4;i++){
      angleSaveB[i]=angleNow[i];
    }
  }
  //Nếu nút C đã được nhấn mà không nhấn giữ thì tiến
  //hành di chuyển các Servo từ tọa độ A đến B đã lưu
  if(BtnCClick){
    for(int i=0;i<4;i++){
      if(angleNow[i]<angleSaveB[i]){
        angleNow[i]+=1;
      }if(angleNow[i]>angleSaveB[i]){
        angleNow[i]-=1;
      }if(issame(angleNow,angleSaveB))
        BtnCClick=false;
    }
  }

  //Nếu nút C được nhấn
  if(!digitalRead(BtnC)){
    for(int i=0;i<4;i++){
      angleNow[i]=angleSaveA[i];
    }
    BtnCClick=true;

    //Kiểm tra có phải nhấn giữ 3s không
    // nếu phải reset về 0
    if(!ispress){
      WhenClick=millis();
      ispress=true;
    }else{
      if(millis()-WhenClick>=3000){
        for(int i=0;i<4;i++){
          angleNow[i]=0;
          myservo[i].write(angleNow[i]);
          BtnCClick=false;
        }
      }
    }
  }else{
    ispress=false;
  }


  //Cho Servo quay đến góc hiện tại
  for(int i=0;i<4;i++){
     myservo[i].write(angleNow[i]);
  }
  delay(50);
}