#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include "BluetoothSerial.h"
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver();
#define SERVOMIN 125
#define SERVOMAX 575
uint8_t servonum = 0;

BluetoothSerial SerialBT;

void setup() {
Serial.begin(115200);
SerialBT.begin("ESP32"); //Nombre del dispositivo bluetooth
Serial.println("Dispositivo iniciado, ahora puede vincular...");
 #ifdef USE_PIN
    SerialBT.setPin(pin);
    Serial.println("Usando PIN");
  #endif

  pwm.begin();
  pwm.setPWMFreq(50);

  //yield();
}

void loop() {
  if (SerialBT.available()) {
    char command = SerialBT.read();
    executeCommand(command);
  }
  caminarAdelante();
  elevarBrazos();

  delay(1000);
}

void executeCommand(char command) {
  switch (command) {
    case 'F':
      caminarAdelante();
      break;
    case 'B':
      caminarAtras();
      break;
    case 'U':
      elevarBrazos();
      break;
    case 'D':
      bajarBrazos();
      break;
    case 'C':
      posicionCombateBrazos();
      break;
    default:
      break;
  }
}

void caminarAdelante() {
  int anguloMinimo = 90;
  int anguloMaximo = 180;
  int pulsoMinimo = angleToPulse(anguloMinimo);
  int pulsoMaximo = angleToPulse(anguloMaximo);

  for (int angulo = anguloMinimo; angulo <= anguloMaximo; angulo += 1) {
    int pulso = map(angulo, anguloMinimo, anguloMaximo, pulsoMinimo, pulsoMaximo);
    pwm.setPWM(0, 0, pulso); // Número de servo para la posición 0
    pwm.setPWM(2, 0, pulso); // Número de servo para la posición 2
    delay(10);
  }
}

void caminarAtras() {
  int anguloMinimo = 90;
  int anguloMaximo = 180;
  int pulsoMinimo = angleToPulse(anguloMinimo);
  int pulsoMaximo = angleToPulse(anguloMaximo);

  for (int angulo = anguloMaximo; angulo >= anguloMinimo; angulo -= 1) {
    int pulso = map(angulo, anguloMinimo, anguloMaximo, pulsoMinimo, pulsoMaximo);
    pwm.setPWM(0, 0, pulso); // Número de servo para la posición 0
    pwm.setPWM(2, 0, pulso); // Número de servo para la posición 2
    delay(10);
  }
}

void elevarBrazos() {
  int anguloMinimo = 0;
  int anguloMaximo = 180;
  int pulsoMinimo = angleToPulse(anguloMinimo);
  int pulsoMaximo = angleToPulse(anguloMaximo);

  for (int angulo = anguloMinimo; angulo <= anguloMaximo; angulo += 1) {
    int pulso = map(angulo, anguloMinimo, anguloMaximo, pulsoMinimo, pulsoMaximo);
    pwm.setPWM(4, 0, pulso); // Número de servo para el brazo izquierdo
    pwm.setPWM(5, 0, pulso); // Número de servo para el brazo derecho
    delay(10);
  }
}

void bajarBrazos() {
  int anguloMinimo = 0;
  int anguloMaximo = 180;
  int pulsoMinimo = angleToPulse(anguloMinimo);
  int pulsoMaximo = angleToPulse(anguloMaximo);

  for (int angulo = anguloMaximo; angulo >= anguloMinimo; angulo -= 1) {
    int pulso = map(angulo, anguloMinimo, anguloMaximo, pulsoMinimo, pulsoMaximo);
    pwm.setPWM(4, 0, pulso); // Número de servo para el brazo izquierdo
    pwm.setPWM(5, 0, pulso); // Número de servo para el brazo derecho
    delay(10);
  }
}

void posicionCombateBrazos() {
  int anguloBrazos = 45; // Ángulo intermedio para los brazos

  int pulsoBrazos = angleToPulse(anguloBrazos);
  pwm.setPWM(4, 0, pulsoBrazos); // Número de servo para el brazo izquierdo
  pwm.setPWM(5, 0, pulsoBrazos); // Número de servo para el brazo derecho
}

int angleToPulse(int ang) {
int pulse = map(ang, 0, 180, SERVOMIN, SERVOMAX);
Serial.print("Angle: ");
Serial.print(ang);
Serial.print(" pulse: ");
Serial.println(pulse);
return pulse;
}