#include <Arduino.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEClient.h>
#include <Button.h>

//#define DEVICE_ADDRESS     "64:E8:33:8A:2F:A6" // Voiturette wood Grand Prix
#define DEVICE_ADDRESS     "58:8C:81:9F:FE:16" // VAN Wood Retro
#define SERVICE_UUID        "12345678-1234-5678-1234-56789abcdef0" // UUID du service du serveur
#define CHARACTERISTIC_UUID_ACTION "12345678-1234-5678-1234-56789abcdef1" // UUID de la caractéristique "Action"
#define CHARACTERISTIC_UUID_DIRECTION "12345678-1234-5678-1234-56789abcdef2" // UUID de la caractéristique "Direction"
#define CHARACTERISTIC_UUID_ECLAIRAGE "12345678-1234-5678-1234-56789abcdef3" // UUID de la caractéristique "Éclairage"

const int SW_pin = 1; // digital pin connected to switch output
Button buttonSW(SW_pin); // Connect your button between pin 1 and GND
const int X_pin = 0; // analog pin connected to X output
const int Y_pin = 3; // analog pin connected to Y output

int val_X = 2048;
int val_Y = 2048;
int val_Y_brute = 2048;
bool etat_SW = false;

BLEClient* pClient;
BLERemoteCharacteristic* pRemoteCharacteristicAction;
BLERemoteCharacteristic* pRemoteCharacteristicDirection;
BLERemoteCharacteristic* pRemoteCharacteristicEclairage;
bool connected = false;
bool doConnect = true; // Drapeau pour tenter la reconnexion

class ClientCallbacks : public BLEClientCallbacks {
    void onConnect(BLEClient* pClient) {
        connected = true;
        Serial.println("Connexion établie !");
    }

    void onDisconnect(BLEClient* pClient) {
        connected = false;
        doConnect = true; // Autoriser la tentative de reconnexion
        Serial.println("Déconnecté. Tentative de reconnexion...");
    }
};

void connectToServer() {
    if (!doConnect) return;

    Serial.println("Tentative de connexion au périphérique BLE spécifique...");
    BLEAddress bleAddress(DEVICE_ADDRESS);

    pClient = BLEDevice::createClient();
    pClient->setClientCallbacks(new ClientCallbacks()); // Définir les callbacks pour surveiller les connexions
    connected = pClient->connect(bleAddress);

    if (connected) {
        Serial.println("Connecté au périphérique BLE!");

        BLERemoteService* pRemoteService = pClient->getService(SERVICE_UUID);
        if (pRemoteService == nullptr) {
            Serial.println("Impossible de trouver le service spécifié!");
            pClient->disconnect();
            return;
        }

        pRemoteCharacteristicAction = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID_ACTION);
        pRemoteCharacteristicDirection = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID_DIRECTION);
        pRemoteCharacteristicEclairage = pRemoteService->getCharacteristic(CHARACTERISTIC_UUID_ECLAIRAGE);

        if (pRemoteCharacteristicAction == nullptr || pRemoteCharacteristicDirection == nullptr || pRemoteCharacteristicEclairage == nullptr) {
            Serial.println("Impossible de trouver une ou plusieurs caractéristiques spécifiées!");
            pClient->disconnect();
            return;
        }

        doConnect = false; // Connexion réussie, pas besoin de retenter
    } else {
        Serial.println("Échec de connexion au périphérique BLE spécifique.");
    }
}

void setup() {
    Serial.begin(115200);
    Serial.println("Démarrage BLE Client pour un périphérique spécifique");

    buttonSW.begin();
    digitalWrite(SW_pin, HIGH);

    BLEDevice::init("");
    connectToServer();
}

void loop() {
    if (!connected) {
        connectToServer(); // Tenter de se reconnecter si déconnecté
        delay(2000); // Délai avant la prochaine tentative
        return;
    }

    delay(50);

    val_X = analogRead(X_pin);
    val_Y_brute = analogRead(Y_pin);

    if ((val_Y_brute > 2000) && (val_Y_brute < 3000)) {
        val_Y_brute = 2048;
    }
    val_Y = map(val_Y_brute, 0, 4096, 70, 110);

    if (buttonSW.pressed()) {
        etat_SW = !etat_SW;
    }

    const char* actionValue;
    if (val_X > 3000) {
        actionValue = u8"A"; // Avancer
        //Serial.println("X-axis: AVANCER");
    } else if (val_X < 2000) {
        actionValue = u8"R"; // Reculer
        //Serial.println("X-axis: RECULER");
    } else {
        actionValue = u8"S"; // Stopper
        //Serial.println("X-axis: STOPPER");
    }

    if (pRemoteCharacteristicAction->canWrite()) {
        pRemoteCharacteristicAction->writeValue((uint8_t*)actionValue, strlen(actionValue), false);
        Serial.print("X-axis: ");
        Serial.println(actionValue);
    }

    char directionBuffer[10];
    snprintf(directionBuffer, sizeof(directionBuffer), u8"%d", val_Y);
    const char* directionValue = directionBuffer;
    if (pRemoteCharacteristicDirection->canWrite()) {
        pRemoteCharacteristicDirection->writeValue((uint8_t*)directionValue, strlen(directionValue), false);
        Serial.print("Y-axis: ");
        Serial.println(directionValue);
    }

    const char* eclairageValue = etat_SW ? u8"1" : u8"0";
    if (pRemoteCharacteristicEclairage->canWrite()) {
        pRemoteCharacteristicEclairage->writeValue((uint8_t*)eclairageValue, strlen(eclairageValue), false);
        Serial.print("SW: ");
        Serial.println(eclairageValue);
    }

    // Serial.print("Switch: ");
    // Serial.println(etat_SW);
    // Serial.print("Y-axis: ");
    // Serial.println(val_Y);
}