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

int servo_pin[] = {11, 10, 9, 6};

Servo servo[4];

int btn = 2;

int btn2 = 4;
int btn3 = 7;
int btn4 = 8;
int btn5 = 12;
int btn6 = 13;

int trucX = A0;
int trucY = A1;

LiquidCrystal_I2C lcd(0x27, 16, 2);

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

  pinMode(btn, INPUT_PULLUP);
  pinMode(btn2, INPUT_PULLUP);
  pinMode(btn3, INPUT_PULLUP);
  pinMode(btn4, INPUT_PULLUP);
  pinMode(btn5, INPUT_PULLUP);
  pinMode(btn6, INPUT_PULLUP);
  pinMode(trucX, INPUT);
  pinMode(trucY, INPUT);

  lcd.init();
  lcd.backlight();
}

unsigned long lasttime = 0;
bool ispress = false;

unsigned long lasttime2 = 0;
bool ispress2 = false;

unsigned long pretime3 = 0;
unsigned long pretime4 = 0;
unsigned long pretime5 = 0;
unsigned long pretime6 = 0;

void loop() {
  unsigned long time = millis();

  if(!digitalRead(btn)){
    if(ispress && time - lasttime >= 50){
      for(int i = 0; i < 4; i++){servo[i].write(180);};
    } else if (!ispress){
      ispress = true;
      lasttime = time;
    }
  } else {
    ispress = false;
  }

  if(!digitalRead(btn2)){
    if(ispress2 && time-lasttime2 >= 50){
      for(int i = 0; i < 4; i++){servo[i].write(0);};
    } else if(!ispress2){
      ispress2 = true;
      lasttime2 = time;
    }
  } else {
    ispress2 = false;
  }

  if(!digitalRead(btn3)){
    if(time-pretime3 >= 50){
      servo[0].write(0);
    }
  } else {
    pretime3 = time;
  }

  if(!digitalRead(btn4)){
    if(time-pretime4 >= 50){
      servo[1].write(90);
    }
  } else {
    pretime4 = time;
  }

  if(!digitalRead(btn5)){
    if(time-pretime5 >= 50){
      servo[2].write(180);
    }
  } else {
    pretime5 = time;
  }

  if(!digitalRead(btn6)){
    if(time-pretime6 >= 50){
      servo[3].write(45);
    }
  } else {
    pretime6 = time;
  }

  int xValue = analogRead(trucX);
  int yValue = analogRead(trucY);

  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);
}