// Include the libraries
#include <Arduino.h>
#include <ESP32Servo.h>

// Define the pins for X, Y, and Select (button) of the joystick
const int xPin = 34; // Change this to the pin you have connected the X axis of the joystick
//xPin = Horz = 34
const int yPin = 35; // Change this to the pin you have connected the Y axis of the joystick
//y pin = Vert = 35
const int xPin2 = 33;
//xPin = Horz = 33
const int yPin2 = 25;
//y pin = Vert = 25
const int selectPin = 32; // Change this to the pin you have connected the Select pin of the joystick
const int selectPin2 = 26;

//servo motori pinovi
const int servoPin = 18;
const int servoPin2 = 5;
const int servoPin3 = 17;
const int servoPin4 = 16;
Servo servo;
Servo servo2;
Servo servo3;
Servo servo4;


void setup() {
  Serial.begin(115200); // Initialize serial communication
  
  pinMode(xPin, INPUT); // Set X axis pin as input
  pinMode(yPin, INPUT); // Set Y axis pin as input
  pinMode(selectPin, INPUT_PULLUP); // Set Select pin as input with internal pull-up resistor
  pinMode(xPin2, INPUT); // Set X axis pin as input
  pinMode(yPin2, INPUT); // Set Y axis pin as input
  pinMode(selectPin2, INPUT_PULLUP); // Set Select pin as input with internal pull-up resistor
  servo.attach(servoPin);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);
}

int val = 0;

void loop() {
  // Read the analog values from X and Y axis of the joystick
  int xValue = analogRead(xPin);
  int yValue = analogRead(yPin);
  
  int xValue2 = analogRead(xPin2);
  int yValue2 = analogRead(yPin2);
  
  // Read the state of the select (button) pin
  int selectState = digitalRead(selectPin);
  int selectState2 = digitalRead(selectPin2);

  // Control the servo motor
  
  servo.write(90);
  servo2.write(90);
  servo3.write(90);
  servo4.write(90);

  if (xValue == 4095 && yValue == 4095){
    Serial.print("Top-Left\n");
        for (int i = 0; i <= 180; i++) {
      servo.write(i);
      servo3.write(180-i);
      servo2.write(i);
      delay(2); // Adjust the delay to control the speed of servo movement
    }
    for (int i = 180; i >= 0; i--) {
      servo.write(i);
      servo3.write(180-i);
      servo2.write(i);
      delay(2); // Adjust the delay to control the speed of servo movement
    }
  }

  // Check if the joystick is in the Top position
  if ( xValue == 2048 && yValue == 4095) {
    Serial.print("Top\n");
    for (int i = 0; i <= 180; i++) {
      servo.write(i);
      servo3.write(180-i);
      delay(2); // Adjust the delay to control the speed of servo movement
    }
    for (int i = 180; i >= 0; i--) {
      servo.write(i);
      servo3.write(180-i);
      delay(2); // Adjust the delay to control the speed of servo movement
    }

  }

  else if (xValue == 0 && yValue == 4095){
    Serial.print("Top-Right\n");
  }

  else if (xValue == 0 && yValue == 2048){
    Serial.print("Right\n");
  }

  else if (xValue == 0 && yValue == 0){
    Serial.print("Bottom-Right\n");
  }

  else if (xValue == 2048 && yValue == 0){
    Serial.print("Bottom\n");
  }

  else if (xValue == 4095 && yValue == 0){
    Serial.print("Bottom-Left\n");
  }

  else if (xValue == 4095 && yValue == 2048){
    Serial.print("Left\n");
  }

  else if (selectState == 0){
     Serial.print("Select: ");
    Serial.println(selectState);
  }

  //Vtor kontroler
  if (xValue2 == 4095 && yValue2 == 4095){
    Serial.print("Top-Left\n");
  }

  else if (xValue2 == 2048 && yValue2 == 4095){
    Serial.print("Top\n");
  }

  else if (xValue2 == 0 && yValue2 == 4095){
    Serial.print("Top-Right\n");
  }

  else if (xValue2 == 0 && yValue2 == 2048){
    Serial.print("Right\n");
  }

  else if (xValue2 == 0 && yValue2 == 0){
    Serial.print("Bottom-Right\n");
  }

  else if (xValue2 == 2048 && yValue2 == 0){
    Serial.print("Bottom\n");
  }

  else if (xValue2 == 4095 && yValue2 == 0){
    Serial.print("Bottom-Left\n");
  }

  else if (xValue2 == 4095 && yValue2 == 2048){
    Serial.print("Left\n");
  }

  else if (selectState2 == 0){
     Serial.print("Select: ");
    Serial.println(selectState2);
  }
  

  // Delay for a short time to avoid flooding the serial monitor
  delay(100);
}