#include "BluetoothSerial.h"
#include <ESP32Servo.h>

char readBluetooth = ' '; //Variável para armazenar o caractere recebido


int pos1 = 90;
int pos2 = 90;


Servo Servo1;
Servo Servo2;



#define USE_PIN // Uncomment this to use PIN during pairing. The pin is specified on the line below
const char *pin = "1234"; // Change this to more secure PIN.

String device_name = "ROBOTTEEN2";

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#if !defined(CONFIG_BT_SPP_ENABLED)
#error Serial Bluetooth not available or not enabled. It is only available for the ESP32 chip.
#endif

BluetoothSerial SerialBT;

void setup() {

  Servo1.attach(26);
  Servo2.attach(27);

  Servo1.write (pos1);
  Servo2.write(pos2);


  Serial.begin(115200);
  SerialBT.begin(device_name); //Bluetooth device name
  Serial.printf("The device with name \"%s\" is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str());
  //Serial.printf("The device with name \"%s\" and MAC address %s is started.\nNow you can pair it with Bluetooth!\n", device_name.c_str(), SerialBT.getMacString()); // Use this after the MAC method is implemented
#ifdef USE_PIN
  SerialBT.setPin(pin);
  Serial.println("Using PIN");
#endif




  delay(5000);




}

void loop() {
  if (Serial.available()) {
    SerialBT.write(Serial.read());
  }
  if (SerialBT.available()) {
    readBluetooth = SerialBT.read();
    Serial.write(readBluetooth);
  }

  switch (readBluetooth)//Verifica se o caractere recebido é igual a algum dos listados abaixo
  {

    case 'A': // CIMA
      Serial.println("A");
      pos1 -= 1; // Move o servo de -1 em -1 grau,  se ficar invertido, mudar para +=
      Servo1.write(pos1);
      delay(15); // um pequeno delay para o servo alcançar a posição
      break;

    case 'B': // BAIXO
      Serial.println("B");
      pos1 += 1; //Move o servo de +1 em +1 grau,  se ficar invertido, mudar para -=
      Servo1.write(pos1);
      delay(15); // um pequeno delay para o servo alcançar a posição
      break;


    case 'C': // ESQUERDA
      Serial.println("C");
      pos2 += 1; //Move o servo de +1 em +1 grau,  se ficar invertido, mudar para -=
      Servo2.write(pos2);
      delay(15); // um pequeno delay para o servo alcançar a posição
      break;

    case 'D': // DIREITA
      Serial.println("D");
      pos2 -= 1; //Move o servo de -1 em -1 grau, ,  se ficar invertido, mudar para -=
      Servo2.write(pos2);
      delay(15); // um pequeno delay para o servo alcançar a posição
      break;

    case 'E': // PONTO 0
      Serial.println("E");
      pos1 = 90; //Move o servo de +1 em +1 grau
      pos2 = 90; //Move o servo de +1 em +1 grau
      Servo1.write(pos1);
      Servo2.write(pos2);
      delay(15); // um pequeno delay para o servo alcançar a posição
      break;


  }
}