#include <Arduino.h>

#include <BleCompositeHID.h>
#include <GamepadDevice.h>

BleCompositeHID compositeHID("Modulx Controller V4", "Nono", 100);
GamepadDevice gamepad;

// Joystick droit
const int JOY_RIGHT_X = 34; // VRX
const int JOY_RIGHT_Y = 35; // VRY

// Centre réel mesuré au démarrage
int centerRightX = 2048;
int centerRightY = 2048;

// Zone morte autour du centre réel
const int DEADZONE = 250;

unsigned long lastPrint = 0;
const unsigned long PRINT_INTERVAL = 300;

int16_t readJoystickAxisCalibrated(int pin, int centerValue) {
  int raw = analogRead(pin);
  int centered = raw - centerValue;

  if (abs(centered) < DEADZONE) {
    return 0;
  }

  // Conversion selon le côté
  if (centered > 0) {
    return map(centered, DEADZONE, 4095 - centerValue, 0, 32767);
  } else {
    return map(centered, -DEADZONE, -centerValue, 0, -32767);
  }
}

void calibrateRightJoystick() {
  long sumX = 0;
  long sumY = 0;
  const int samples = 100;

  Serial.println("Calibration joystick droit...");
  Serial.println("Ne touche pas au joystick.");

  delay(1000);

  for (int i = 0; i < samples; i++) {
    sumX += analogRead(JOY_RIGHT_X);
    sumY += analogRead(JOY_RIGHT_Y);
    delay(5);
  }

  centerRightX = sumX / samples;
  centerRightY = sumY / samples;

  Serial.print("Centre joystick droit X = ");
  Serial.println(centerRightX);

  Serial.print("Centre joystick droit Y = ");
  Serial.println(centerRightY);
}

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

  Serial.println();
  Serial.println("===== TEST BLE COMPOSITE HID + JOYSTICK DROIT =====");

  analogReadResolution(12);

  calibrateRightJoystick();

  compositeHID.addDevice(&gamepad);
  compositeHID.begin();

  Serial.println("BLE demarre.");
  Serial.println("Nom Bluetooth : Modulx Controller V4");
  Serial.println("Joystick droit : VRX GPIO34 / VRY GPIO35");
}

void loop() {
  static bool lastConnected = false;
  bool connected = compositeHID.isConnected();

  if (connected != lastConnected) {
    if (connected) {
      Serial.println("[BLE] Connecte !");
    } else {
      Serial.println("[BLE] Deconnecte.");
    }

    lastConnected = connected;
  }

  int rawX = analogRead(JOY_RIGHT_X);
  int rawY = analogRead(JOY_RIGHT_Y);

  int16_t rightX = readJoystickAxisCalibrated(JOY_RIGHT_X, centerRightX);
  int16_t rightY = readJoystickAxisCalibrated(JOY_RIGHT_Y, centerRightY);

  if (millis() - lastPrint >= PRINT_INTERVAL) {
    lastPrint = millis();

    Serial.print("[JOY DROIT] raw X=");
    Serial.print(rawX);
    Serial.print(" raw Y=");
    Serial.print(rawY);

    Serial.print(" | center X=");
    Serial.print(centerRightX);
    Serial.print(" center Y=");
    Serial.print(centerRightY);

    Serial.print(" | mapped X=");
    Serial.print(rightX);
    Serial.print(" mapped Y=");
    Serial.println(rightY);
  }

  if (connected) {
    gamepad.setRightThumb(rightX, rightY);
    gamepad.sendGamepadReport();
    delay(20);
  } else {
    delay(100);
  }
}