#include <ArduinoBLE.h>
#include <ESP32Servo.h>

Servo servo1;
Servo servo2;

const int numOfMoves = 6;

int servo1Pos[numOfMoves + 1] = {0, 30, 60, 90, 120, 150, 180};
int servo2Pos[numOfMoves + 1] = {180, 150, 120, 90, 60, 30, 0};

BLEService buttonService("180F");
BLEIntCharacteristic buttonCharacteristic("2A19", BLERead | BLEWrite | BLENotify);

void setup() {
  Serial.begin(115200);

  // Initialize servos
  servo1.setPeriodHertz(50);
  servo2.setPeriodHertz(50);
  servo1.attach(13);
  servo2.attach(12);

  // Initialize BLE
  if (!BLE.begin()) {
    Serial.println("Starting BLE failed!");
    while (1);
  }

  BLE.setLocalName("BLE_Button");
  BLE.setAdvertisedService(buttonService);
  buttonService.addCharacteristic(buttonCharacteristic);
  BLE.addService(buttonService);

  buttonCharacteristic.writeValue(0);

  BLE.advertise();

  Serial.println("Bluetooth device active, waiting for connections...");
}

void loop() {
  BLEDevice central = BLE.central();

  if (central) {
    Serial.print("Connected to central: ");
    Serial.println(central.address());
    while (central.connected()) {
      if (buttonCharacteristic.written()) {
        int buttonValue = buttonCharacteristic.value();
        if (buttonValue >= 1 && buttonValue <= 6) {
          servo1.write(servo1Pos[buttonValue]);
          servo2.write(servo2Pos[buttonValue]);
        } else if (buttonValue == 7) {
          static int currentMove = 0;
          currentMove = (currentMove + 1) % (numOfMoves + 1);
          servo1.write(servo1Pos[currentMove]);
          servo2.write(servo2Pos[currentMove]);
        }
      }
    }
    Serial.print("Disconnected from central: ");
    Serial.println(central.address());
  }
}