// SERVO MOTOR AND JOYSTICK CODE
#include <ESP32Servo.h>
#define VRX_PIN      34 // ESP32 pin GPIO34 (ADC0) connected to VRX pin
#define VRY_PIN      35 // ESP32 pin GPIO35 (ADC0) connected to VRY pin
#define SEL_PIN      17 // ESP32 pin GPIO17 (ADC0) connected to VRY pin
#define SERVO_X_PIN  33 // ESP32 pin GPIO33 connected to Servo motor 1
#define SERVO_Y_PIN  26 // ESP32 pin GPIO26 connected to Servo motor 2

Servo xServo;  // create servo object to control a servo 1
Servo yServo;  // create servo object to control a servo 2
int bValue = 0; // To store value of the button

//LED 
#define LEFT_BUTTON_PIN  21 // ESP32 pin GPIO21, which connected to button
#define LEFT_LED_PIN     25// ESP32 pin GPIO25, which connected to led

#define RIGHT_BUTTON_PIN  15 // ESP32 pin GPIO15, which connected to button
#define RIGHT_LED_PIN     2// ESP32 pin GPIO2, which connected to led

// variables will change:
int LEFT_button_state = 0;   // variable for reading the button status
int RIGHT_button_state = 0;   // variable for reading the button status

void setup() {

  Serial.begin(115200);
  
  //LED CODE
  // initialize the LED pin as an output:
  pinMode(LEFT_LED_PIN, OUTPUT);
  pinMode(RIGHT_LED_PIN, OUTPUT);
  // initialize the button pin as an pull-up input:
  // the pull-up input pin will be HIGH when the button is open and LOW when the button is pressed.
  pinMode(LEFT_BUTTON_PIN, INPUT_PULLUP);
  pinMode(RIGHT_BUTTON_PIN, INPUT_PULLUP);

  //SERVO MOTOR AND JOTSTICK CODE
  xServo.attach(SERVO_X_PIN);
  yServo.attach(SERVO_Y_PIN);

}

void loop() {
  // LED CODE
  // read the state of the button value:
  LEFT_button_state = digitalRead(LEFT_BUTTON_PIN);
  RIGHT_button_state = digitalRead(RIGHT_BUTTON_PIN);

  // SERVO MOTOR AND JOYSTICK CODE
  // read X and Y analog values
  int valueX = analogRead(VRX_PIN);
  int valueY = analogRead(VRY_PIN);

  int xAngle = map(valueX, 0, 4095, 0, 180); // scale it to the servo's angle (0 to 180)
  int yAngle = map(valueY, 0, 4095, 0, 180); // scale it to the servo's angle (0 to 180)

  xServo.write(xAngle); // rotate servo motor 1
  yServo.write(yAngle); // rotate servo motor 2

  //LEFT BUTTON, LED AND SERVO MOTOR
  // control LED according to the state of button
  if (LEFT_button_state == LOW) 
  {   // if button is pressed
    digitalWrite(LEFT_LED_PIN, HIGH); // turn on LED
    xServo.write(180); // rotate servo motor 1
    delay(1000);
    xServo.write(0);
     Serial.println("Rotation Finished!");
     delay(1000);
  } else{                 // otherwise, button is not pressing
    digitalWrite(LEFT_LED_PIN, LOW);  // turn off LED
  }

    
  //RIGHT BUTTON, LED AND SERVO MOTOR
  // control LED according to the state of button
  if (RIGHT_button_state == LOW) {      // if button is pressed
    digitalWrite(RIGHT_LED_PIN, HIGH); // turn on LED
    yServo.write(180); // rotate servo motor 1
    delay(1000);
    yServo.write(0);
     Serial.println("Rotation Finished!");
     delay(1000);
  }else {                         // otherwise, button is not pressing
    digitalWrite(RIGHT_LED_PIN, LOW);  // turn off LED
  }

  // print data to Serial Monitor on Arduino IDE
  Serial.print("Joystick: ");
  Serial.println(valueX);
  Serial.print(", ");
  Serial.println(valueY);
  Serial.print(" => Servo Motor: ");
  Serial.println(xAngle);
  Serial.print("°, ");
  Serial.println(yAngle);
  Serial.println("°");
}