#include <Arduino.h>

// Para controle do servo motor
#include <ESP32Servo.h>
#include <Wire.h>

// JoyStick variables
int valueX = 0;
int valueY = 0;
int valueSW = 0;

// Arm position variables
int arm_position_x = 4096;
int arm_position_y = 4096;
int arm_position_z = 0;
int angle = 0;

// JoyStick pins
const int xPin = 20;
const int yPin = 19;
const int swPin = 18;

// Servo pins
const int servo_01_pin = 21;
const int servo_02_pin = 26;
const int servo_03_pin = 33;

// Servo variables
int valor_servo_01;
int valor_servo_02;
int valor_servo_03;

// Servo objects
Servo servo_01;
Servo servo_02;
Servo servo_03;

bool moving  = false;

void setup()
{
    // Wire.begin();
    Serial.begin(9600); // Inicia a comunicação serial. Baud rate: 9600 is the default for the Arduino Serial Monitor

    servo_01.attach(servo_01_pin);
    servo_02.attach(servo_02_pin);
    servo_03.attach(servo_03_pin);

    valor_servo_01 = map(arm_position_x, 0, 8191, 0, 180);
    valor_servo_02 = map(arm_position_y, 0, 8191, 0, 180);
    valor_servo_03 = map(arm_position_z, 0, 8191, 0, 180);

    // Define o pin mode do botão swPin
    pinMode(swPin, INPUT);

    // Posição inicial do braço
    
    servo_01.write(valor_servo_01);  
    servo_02.write(valor_servo_02);
    servo_03.write(valor_servo_03);
}

void getDirection()
{
    valueX = analogRead(xPin);
    valueY = analogRead(yPin);
    valueSW = analogRead(swPin);

    Serial.print("X: ");
    Serial.print(valueX);
    Serial.print(" Y: ");
    Serial.print(valueY);
    Serial.print(" SW: ");
    Serial.println(valueSW);

    if (valueX < 4096)
    {
        // RIGHT
        Serial.println("RIGHT");
        arm_position_x -= 180;
        if (arm_position_x > 0)
        {
            valor_servo_01 = map(arm_position_x, 0, 8191, 0, 180);
            servo_01.write(valor_servo_01);
        }
    }
    if (valueX > 4096)
    {
        // LEFT
        Serial.println("LEFT");
        arm_position_x += 180;
        if (arm_position_x < 8191)
        {
            valor_servo_01 = map(arm_position_x, 0, 8191, 0, 180);
            servo_01.write(valor_servo_01);
        }
    }
    if (valueY < 4096)
    {
        // UP
        Serial.println("UP");
        arm_position_y -= 180;
        if (arm_position_y > 0)
        {
            valor_servo_02 = map(arm_position_y, 0, 8191, 0, 180);
            servo_02.write(valor_servo_02);
        }
    }
    if (valueY > 4096)
    {
        // DOWN
        Serial.println("DOWN");
        arm_position_y += 180;
        if (arm_position_y < 8191)
        {
            valor_servo_02 = map(arm_position_y, 0, 8191, 0, 180);
            servo_02.write(valor_servo_02);
        }
    }


    if (valueSW == 0)
    {   
        moving = true;
        angle = servo_03.read();     
        Serial.print("SWITCH ON - Angle:");
        Serial.println(angle);

      if (moving = false)         
      {
        arm_position_z = 8191;
        valor_servo_03 = map(arm_position_z, 0, 8191, 0, 180);
        servo_03.write(valor_servo_03);
      }

      if (moving)
      {
        
      }         

    } else {
        arm_position_z = 4096;
        valor_servo_03 = map(arm_position_z, 0, 8191, 0, 180);
        servo_03.write(valor_servo_03);
    }
}

void loop()
{
    getDirection();
    delay(150);
}