#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEScan.h>
#include <BLEAdvertisedDevice.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include <ESP32Servo.h>

// UUIDs pour le service et les caractéristiques
#define SERVICE_UUID "12345678-1234-5678-1234-56789abcdef0"
#define CHARACTERISTIC_UUID_ACTION "12345678-1234-5678-1234-56789abcdef1"
#define CHARACTERISTIC_UUID_DIRECTION "12345678-1234-5678-1234-56789abcdef2"
#define CHARACTERISTIC_UUID_ECLAIRAGE "12345678-1234-5678-1234-56789abcdef3"

// Définition des GPIOs
const int cmd1_mot = D4;
const int cmd2_mot = D5; 
const int led_avant_g = D0;
const int led_avant_d = D1; 
const int led_arriere_g = D9; 
const int led_arriere_d = D10; 
const int cmd_servo = D2; 


// Variables pour stocker les données reçues
String data1_action = "";
String data3_eclairage = "0";
int servoAngle = 90; // Angle initial du servo

Servo servo; // création de l'objet "servo"
BLEServer *pServer = nullptr;
bool deviceConnected = false;


// Callback pour gérer les écritures sur la caractéristique 1 : Action
class Characteristic1Callback : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String value1 = pCharacteristic->getValue();
    if (value1.length() > 0) {
      data1_action = String(value1.c_str());
      if (data1_action != "")
      {
        Serial.println(data1_action);
        if (data1_action == "A")
        {
          Serial.println("Avancer");
          avancer(128);
        }
        else
        {
          if (data1_action == "R")
          {
            Serial.println("Reculer");
            reculer(128);
          }
          else
          {
            Serial.println("Stopper");
            stopper();
          }
        }
      }
    }
    value1 = "";
  }

  void avancer(int vitesse){
    //analogWrite(cmd1_mot,vitesse);
    //analogWrite(cmd2_mot,0);
    digitalWrite(cmd1_mot, HIGH);
    digitalWrite(cmd2_mot, LOW);
  }

  void reculer(int vitesse){
    //analogWrite(cmd2_mot,vitesse);
    //analogWrite(cmd1_mot,0);
    digitalWrite(cmd2_mot, HIGH);
    digitalWrite(cmd1_mot, LOW);
  }

  void stopper(){
    //analogWrite(cmd1_mot,0);
    //analogWrite(cmd2_mot,0);
    digitalWrite(cmd2_mot, LOW);
    digitalWrite(cmd1_mot, LOW);
  }
};

// Callback pour gérer les écritures sur la caractéristique 2 : Direction
class Characteristic2Callback : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String value2 = pCharacteristic->getValue();
  
    if (value2.length() > 0) {
      int angle = atoi(value2.c_str()); // Convertir la commande reçue en entier
      if (angle >= 10 && angle <= 170) {
        servoAngle = angle; // Mettre à jour l'angle cible
        servo.write(servoAngle); // Déplacer le servo
        Serial.print("Angle du servo mis à jour : ");
        Serial.println(servoAngle);
      } else {
        Serial.println("Commande invalide. Envoyez un angle entre 0 et 180.");
      }
    }
    value2 = "";
  }
};

// Callbacks pour gérer les connexions et déconnexions BLE
class MyServerCallbacks : public BLEServerCallbacks {
  void onConnect(BLEServer* pServer) {
    deviceConnected = true;
    Serial.println("Un appareil est connecté.");
  }

  void onDisconnect(BLEServer* pServer) {
    deviceConnected = false;
    Serial.println("Appareil déconnecté, reconnexion en cours...");
    BLEDevice::startAdvertising(); // Relancer la publicité BLE
    Serial.println("En attente de connexion...");
  }
};

// Callback pour gérer les écritures sur la caractéristique 3 : Eclairage
class Characteristic3Callback : public BLECharacteristicCallbacks {
  void onWrite(BLECharacteristic *pCharacteristic) {
    String value3 = pCharacteristic->getValue();
    if (value3.length() > 0) {
      data3_eclairage = String(value3.c_str());
      if (data3_eclairage == "1")
      {
        Serial.println("allumer LEDS");
        allumer_leds_avant();
        allumer_leds_arriere();
      }
      else
      {
        Serial.println("eteindre LEDS");
        eteindre_leds_avant();
        eteindre_leds_arriere();
      }
     }
     value3 = "";
  }

  void allumer_led_avant_g(){
    digitalWrite(led_avant_g, HIGH);   // turn the LED on  
  }

  void allumer_led_avant_d(){
    digitalWrite(led_avant_d, HIGH);   // turn the LED on  
  }

  void allumer_leds_avant(){
    allumer_led_avant_g();
    allumer_led_avant_d();
  }

  void eteindre_led_avant_g(){
    digitalWrite(led_avant_g, LOW);   // turn the LED on  
  }

  void eteindre_led_avant_d(){
    digitalWrite(led_avant_d, LOW);   // turn the LED on  
  }

  void eteindre_leds_avant(){
    eteindre_led_avant_g();
    eteindre_led_avant_d();
  }

  void allumer_led_arriere_g(){
    digitalWrite(led_arriere_g, HIGH);   // turn the LED on  
  }

  void allumer_led_arriere_d(){
    digitalWrite(led_arriere_d, HIGH);   // turn the LED on  
  }

  void allumer_leds_arriere(){
    allumer_led_arriere_g();
    allumer_led_arriere_d();
  }

  void eteindre_led_arriere_g(){
    digitalWrite(led_arriere_g, LOW);   // turn the LED on  
  }

  void eteindre_led_arriere_d(){
    digitalWrite(led_arriere_d, LOW);   // turn the LED on  
  }

  void eteindre_leds_arriere(){
    eteindre_led_arriere_g();
    eteindre_led_arriere_d();
  }

};

void setup() {
  // Configuration des GPIOs
  pinMode(cmd1_mot, OUTPUT);
  pinMode(cmd2_mot, OUTPUT);
  pinMode(led_avant_g, OUTPUT);
  pinMode(led_avant_d, OUTPUT);
  pinMode(led_arriere_g, OUTPUT);
  pinMode(led_arriere_d, OUTPUT);
  digitalWrite(led_arriere_d, LOW); // Éteindre la LED initialement
  digitalWrite(led_arriere_g, LOW); // Éteindre la LED initialement
  servo.attach(cmd_servo, 500, 2500);
  servo.write(servoAngle);

  // Initialisation du port série
  Serial.begin(115200);
  Serial.println("Démarrage du serveur BLE...");

  // Initialisation du périphérique BLE
  BLEDevice::init("Ma petite voiture bluetooth"); // vous pouvez changez le nom
  BLEDevice::setPower(ESP_PWR_LVL_P9);  // Puissance maximale (+9 dBm)
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());

  // Création du service BLE
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // Création des caractéristiques
  BLECharacteristic *pCharacteristic1 = pService->createCharacteristic(
      CHARACTERISTIC_UUID_ACTION, BLECharacteristic::PROPERTY_WRITE);
  pCharacteristic1->setCallbacks(new Characteristic1Callback());

  BLECharacteristic *pCharacteristic2 = pService->createCharacteristic(
      CHARACTERISTIC_UUID_DIRECTION, BLECharacteristic::PROPERTY_WRITE);
  pCharacteristic2->setCallbacks(new Characteristic2Callback());

  BLECharacteristic *pCharacteristic3 = pService->createCharacteristic(
      CHARACTERISTIC_UUID_ECLAIRAGE, BLECharacteristic::PROPERTY_WRITE);
  pCharacteristic3->setCallbacks(new Characteristic3Callback());

  // Ajout des descripteurs
  pCharacteristic1->addDescriptor(new BLE2902());
  pCharacteristic2->addDescriptor(new BLE2902());
  pCharacteristic3->addDescriptor(new BLE2902());

  // Démarrage du service
  pService->start();

  // Lancer la publicité BLE
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);  // Intervalle minimum (7.5 ms)
  pAdvertising->setMaxPreferred(0x12);  // Intervalle maximum (15 ms)
  pAdvertising->setMinInterval(0x20);   // Intervalle minimum de scan (32 * 0.625 ms = 20 ms)
  pAdvertising->setMaxInterval(0x40);   // Intervalle maximum de scan (64 * 0.625 ms = 40 ms)
  BLEDevice::startAdvertising();

  Serial.println("Serveur BLE actif et en attente de connexion...");
}

void loop() {
  // Indicateur de connexion BLE (peut être utilisé pour des LEDs, etc.)
  if (deviceConnected) {
    Serial.println("Appareil connecté");
  } else {
    Serial.println("En attente de connexion...");
  }
  delay(1000);
}