#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>

// ======================================================
// UUID BLE
// ======================================================
#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"

// ======================================================
// GPIO
// ======================================================
// L298N
const int cmd1_mot = D4;
const int cmd2_mot = D5;
// Servo
const int cmd_servo = D2;
// LEDs
const int led_avant_g = D0;
const int led_avant_d = D1;
const int led_arriere_g = D9;
const int led_arriere_d = D10;

// ======================================================
// CONFIGURATION SERVO
// ======================================================
// Limites physiques de direction
const int SERVO_MIN = 70;
const int SERVO_CENTRE = 90;
const int SERVO_MAX = 110;

// Durée pendant laquelle le servo reste alimenté
// par les impulsions après un changement de position
const unsigned long SERVO_DUREE_ACTIVATION = 500;

// Période servo = 20 ms = 50 Hz
const unsigned long SERVO_PERIODE_US = 20000;

// ======================================================
// CONFIGURATION PWM MOTEURS
// ======================================================
const int canal_moteur_1 = 4;
const int canal_moteur_2 = 5;
const int frequence_moteur = 1000;
const int resolution_moteur = 8;

// ======================================================
// VARIABLES BLE
// ======================================================
BLEServer *pServer = nullptr;
bool deviceConnected = false;

// ======================================================
// VARIABLES SERVO
// ======================================================
// Position actuelle
int servoAngleActuel = SERVO_CENTRE;
// Position demandée par la télécommande
volatile int servoAngleDemande = SERVO_CENTRE;
// Nouvelle commande reçue
volatile bool nouvelleCommandeServo = false;
// Servo actuellement commandé ou non
bool servoActif = false;
// Largeur d'impulsion actuelle
unsigned int servoImpulsionUs = 1500;
// Heure du début d'activation
unsigned long servoDebutActivation = 0;
// Heure de la dernière impulsion
unsigned long servoDerniereImpulsion = 0;

// ======================================================
// VARIABLES ECLAIRAGE
// ======================================================
String data3_eclairage = "0";

// ======================================================
// CONVERSION ANGLE -> IMPULSION SERVO
// ======================================================
unsigned int angleVersImpulsion(int angle)
{
  angle = constrain(angle,SERVO_MIN,SERVO_MAX);
  /*
     Correspondance utilisée :
     0°   = 500 µs
     90°  = 1500 µs
     180° = 2500 µs

     Donc approximativement :
     70°  = 1278 µs
     90°  = 1500 µs
     110° = 1722 µs
  */
  return map(angle,0,180,500,2500);
}

// ======================================================
// DEMARRAGE D'UN MOUVEMENT SERVO
// ======================================================
void positionnerServo(int angle)
{
  angle = constrain(angle,SERVO_MIN,SERVO_MAX);
  servoAngleActuel = angle;
  servoImpulsionUs = angleVersImpulsion(angle);
  // Active le servo pendant 500 ms
  servoActif = true;
  servoDebutActivation = millis();
  // Permet d'envoyer immédiatement
  // la première impulsion
  servoDerniereImpulsion = micros() - SERVO_PERIODE_US;
  Serial.print("SERVO - Angle : ");
  Serial.print(angle);
  Serial.print(" | impulsion : ");
  Serial.print(servoImpulsionUs);
  Serial.println(" us");
}

// ======================================================
// GESTION DU SIGNAL SERVO
// ======================================================
void gererServo()
{
  if (!servoActif)
  {
    return;
  }

  // --------------------------------------------------
  // Protection : arrêt du signal après 500 ms
  // --------------------------------------------------
  if (millis() - servoDebutActivation >= SERVO_DUREE_ACTIVATION)
  {
    servoActif = false;
    digitalWrite(cmd_servo,LOW);
    Serial.println("SERVO - Signal coupe apres 500 ms");
    return;
  }

  // --------------------------------------------------
  // Génération d'une impulsion toutes les 20 ms
  // --------------------------------------------------
  unsigned long maintenant = micros();
  if (maintenant - servoDerniereImpulsion >= SERVO_PERIODE_US)
  {
    servoDerniereImpulsion = maintenant;
    digitalWrite(cmd_servo,HIGH);
    delayMicroseconds(servoImpulsionUs);
    digitalWrite(cmd_servo,LOW);
  }
}

// ======================================================
// FONCTIONS MOTEUR
// ======================================================
void avancer(int vitesse)
{
  vitesse = constrain(vitesse, 0, 255);
  Serial.print("MOTEUR - Avancer PWM = ");
  Serial.println(vitesse);
  ledcWrite(cmd1_mot,vitesse);
  ledcWrite(cmd2_mot,0);
}

void reculer(int vitesse)
{
  vitesse = constrain(vitesse, 0, 255);
  Serial.print("MOTEUR - Reculer PWM = ");
  Serial.println(vitesse);
  ledcWrite(cmd1_mot,0);
  ledcWrite(cmd2_mot,vitesse);
}

void stopper()
{
  Serial.println("MOTEUR - Stop");
  ledcWrite(cmd1_mot,0);
  ledcWrite(cmd2_mot,0);
}

// ======================================================
// FONCTIONS ECLAIRAGE
// ======================================================
void allumer_leds_avant()
{
  digitalWrite(led_avant_g,HIGH);
  digitalWrite(led_avant_d,HIGH);
}

void eteindre_leds_avant()
{
  digitalWrite(led_avant_g,LOW);
  digitalWrite(led_avant_d,LOW);
}

void allumer_leds_arriere()
{
  digitalWrite(led_arriere_g,HIGH);
  digitalWrite(led_arriere_d,HIGH);
}

void eteindre_leds_arriere()
{
  digitalWrite(led_arriere_g,LOW);
  digitalWrite(led_arriere_d,LOW);
}

// ======================================================
// CALLBACK BLE : MOTEUR
// ======================================================
class Characteristic1Callback :
  public BLECharacteristicCallbacks
{
  void onWrite(BLECharacteristic *pCharacteristic)
  {
    String value = pCharacteristic->getValue();
    if (value.length() == 0)
    {
      return;
    }
    Serial.print("BLE - Commande moteur recue : ");
    Serial.println(value);
    if (value == "A")
    {
      avancer(255);
    }
    else if (value == "R")
    {
      reculer(255);
    }
    else
    {
      stopper();
    }
  }
};

// ======================================================
// CALLBACK BLE : SERVO
// ======================================================
class Characteristic2Callback :
  public BLECharacteristicCallbacks
{
  void onWrite(BLECharacteristic *pCharacteristic)
  {
    String value = pCharacteristic->getValue();
    if (value.length() == 0)
    {
      return;
    }
    int angle = atoi(value.c_str());
    Serial.print("BLE - Angle recu : ");
    Serial.println(angle);
    // Nouvelles limites : 70° à 110°
    if (angle >= SERVO_MIN && angle <= SERVO_MAX)
    {
      servoAngleDemande = angle;
      nouvelleCommandeServo = true;
    }
    else
    {
      Serial.print("BLE - Angle invalide : ");
      Serial.println(angle);
    }
  }
};

// ======================================================
// CALLBACK BLE : ECLAIRAGE
// ======================================================
class Characteristic3Callback : public BLECharacteristicCallbacks
{
  void onWrite(BLECharacteristic *pCharacteristic)
  {
    String value = pCharacteristic->getValue();
    if (value.length() == 0)
    {
      return;
    }
    data3_eclairage = value;
    Serial.print("BLE - Eclairage : ");
    Serial.println(data3_eclairage);
    if (data3_eclairage == "1")
    {
      Serial.println("LEDS - Allumage");
      allumer_leds_avant();
      allumer_leds_arriere();
    }
    else
    {
      Serial.println("LEDS - Extinction");
      eteindre_leds_avant();
      eteindre_leds_arriere();
    }
  }
};

// ======================================================
// CALLBACK CONNEXION BLE
// ======================================================
class MyServerCallbacks : public BLEServerCallbacks
{
  void onConnect(BLEServer *pServer)
  {
    deviceConnected = true;
    Serial.println();
    Serial.println("==============================");
    Serial.println("TELECOMMANDE CONNECTEE");
    Serial.println("==============================");
  }

  void onDisconnect(BLEServer *pServer)
  {
    deviceConnected = false;
    Serial.println();
    Serial.println("==============================");
    Serial.println("TELECOMMANDE DECONNECTEE");
    Serial.println("==============================");
    // Sécurité moteur
    stopper();
    // Retour au centre
    servoAngleDemande = SERVO_CENTRE;
    nouvelleCommandeServo = true;
    Serial.println("Servo demande au centre");
    BLEDevice::startAdvertising();
    Serial.println("Publicite BLE relancee");
    Serial.println("En attente de connexion...");
  }
};

// ======================================================
// SETUP
// ======================================================
void setup()
{
  // ==================================================
  // PORT SERIE
  // ==================================================
  Serial.begin(115200);
  delay(500);
  Serial.println();
  Serial.println("==============================");
  Serial.println("DEMARRAGE RECEPTEUR");
  Serial.println("==============================");

  // ==================================================
  // SERVO
  // ==================================================
  pinMode(cmd_servo,OUTPUT);
  digitalWrite(cmd_servo,LOW);

  // ==================================================
  // LEDs
  // ==================================================
  pinMode(led_avant_g,OUTPUT);
  pinMode(led_avant_d,OUTPUT);
  pinMode(led_arriere_g,OUTPUT);
  pinMode(led_arriere_d,OUTPUT);
  eteindre_leds_avant();
  eteindre_leds_arriere();

  // ==================================================
  // INITIALISATION PWM MOTEURS
  // ==================================================
  Serial.println();
  Serial.println("==============================");
  Serial.println("INITIALISATION MOTEURS");
  Serial.println("==============================");
  bool moteur1OK = ledcAttachChannel(cmd1_mot,frequence_moteur,resolution_moteur,canal_moteur_1);
  bool moteur2OK = ledcAttachChannel(cmd2_mot,frequence_moteur,resolution_moteur, canal_moteur_2);
  Serial.print("PWM moteur 1 : ");
  Serial.println(moteur1OK ? "OK" : "ERREUR");
  Serial.print("PWM moteur 2 : ");
  Serial.println(moteur2OK ? "OK" : "ERREUR");
  ledcWrite(cmd1_mot,0);
  ledcWrite(cmd2_mot,0);

  // ==================================================
  // INITIALISATION BLE
  // ==================================================
  Serial.println();
  Serial.println("==============================");
  Serial.println("INITIALISATION BLE");
  Serial.println("==============================");
  BLEDevice::init("Mon petit van bluetooth");
  BLEDevice::setPower(ESP_PWR_LVL_P9);
  pServer = BLEDevice::createServer();
  pServer->setCallbacks(new MyServerCallbacks());
  BLEService *pService = pServer->createService(SERVICE_UUID);

  // ==================================================
  // CARACTERISTIQUE MOTEUR
  // ==================================================
  BLECharacteristic *pCharacteristic1 = pService->createCharacteristic(CHARACTERISTIC_UUID_ACTION,BLECharacteristic::PROPERTY_WRITE);
  pCharacteristic1->setCallbacks(new Characteristic1Callback());

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

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

  // ==================================================
  // DESCRIPTEURS
  // ==================================================
  pCharacteristic1->addDescriptor(new BLE2902());
  pCharacteristic2->addDescriptor(new BLE2902());
  pCharacteristic3->addDescriptor(new BLE2902());

  // ==================================================
  // DEMARRAGE SERVICE
  // ==================================================
  pService->start();

  // ==================================================
  // PUBLICITE BLE
  // ==================================================
  BLEAdvertising *pAdvertising = BLEDevice::getAdvertising();
  pAdvertising->addServiceUUID(SERVICE_UUID);
  pAdvertising->setScanResponse(true);
  pAdvertising->setMinPreferred(0x06);
  pAdvertising->setMaxPreferred(0x12);
  pAdvertising->setMinInterval(0x20);
  pAdvertising->setMaxInterval(0x40);
  BLEDevice::startAdvertising();
  Serial.println();
  Serial.println("==============================");
  Serial.println("SERVEUR BLE ACTIF");
  Serial.println("==============================");
  Serial.println("En attente de connexion...");

  // ==================================================
  // CENTRAGE INITIAL DU SERVO
  // ==================================================
  /*
     On lance le centrage seulement maintenant,
     juste avant d'entrer dans loop().

     Le servo recevra donc les impulsions pendant
     500 ms puis le signal sera automatiquement
     coupé.
  */
  servoAngleActuel = SERVO_CENTRE;
  positionnerServo(SERVO_CENTRE);
}

// ======================================================
// LOOP
// ======================================================
void loop()
{
  // ==================================================
  // TRAITEMENT DES COMMANDES SERVO BLE
  // ==================================================
  if (nouvelleCommandeServo)
  {
    int nouvelAngle = servoAngleDemande;
    nouvelleCommandeServo = false;
    nouvelAngle = constrain(nouvelAngle,SERVO_MIN,SERVO_MAX);
    /*
       IMPORTANT :
       On ne réactive le servo que si
       l'angle a réellement changé.
       Si la télécommande renvoie continuellement
       "90, 90, 90, 90...", cela ne maintient donc
       pas le servo alimenté en permanence.
    */
    if (nouvelAngle != servoAngleActuel)
    {
      Serial.print("MAIN - Nouvelle position servo : ");
      Serial.println(nouvelAngle);
      positionnerServo(nouvelAngle);
    }
  }

  // ==================================================
  // GENERATION DES IMPULSIONS SERVO
  // ==================================================
  gererServo();
  delay(1);
}