#include <BleMouse.h>

#include <MPU6050_tockn.h>

#include <Wire.h>

MPU6050 mpu6050(Wire);

BleMouse mouse("ESP32-Air-Mouse", "ESP32", 100);

// Параметры микрофона

#define MIC_PIN 34

// Параметры управления

float sensitivity = 0.8;  // Уменьшена чувствительность

float deadZone = 0.15;    // Увеличена мертвая зона

bool connected = false;

// Фильтр для стабилизации

float filteredX = 0, filteredY = 0;

const float filterFactor = 0.2; // Коэффициент фильтрации

// Калибровочные смещения

float offsetX = 0, offsetY = 0;

bool calibrated = false;

// Переменные для обработки звука

unsigned long lastClapTime = 0;

const unsigned long clapCooldown = 500;

const int soundThreshold = 800;

void calibrateMPU() {

  Serial.println("Calibrating MPU6050... Keep device STILL on flat surface!");

  delay(1000);

  // Собираем 100 samples для калибровки

  float sumX = 0, sumY = 0;

  for (int i = 0; i < 100; i++) {

    mpu6050.update();

    sumX += mpu6050.getAccX();

    sumY += mpu6050.getAccY();

    delay(10);

  }

  offsetX = sumX / 100;

  offsetY = sumY / 100;

  Serial.printf("Calibration complete! Offsets: X=%.3f, Y=%.3f\n", offsetX, offsetY);

  calibrated = true;

}

void applyLowPassFilter(float &filtered, float newValue, float factor) {

  filtered = filtered  (1 - factor) + newValue  factor;

}

void processMovement() {

  if (!calibrated) return;

  mpu6050.update();

  // Получаем данные с компенсацией смещения

  float rawX = mpu6050.getAccX() - offsetX;

  float rawY = mpu6050.getAccY() - offsetY;

  // Применяем фильтр низких частот

  applyLowPassFilter(filteredX, rawX, filterFactor);

  applyLowPassFilter(filteredY, rawY, filterFactor);

  // Применяем мертвую зону

  if (abs(filteredX) < deadZone) filteredX = 0;

  if (abs(filteredY) < deadZone) filteredY = 0;

  // Преобразуем в движение мыши

  int moveX = 0, moveY = 0;

  if (filteredX != 0) {

    moveX = filteredX  sensitivity  15;

    moveX = constrain(moveX, -127, 127);

  }

  if (filteredY != 0) {

    moveY = filteredY  sensitivity  15;

    moveY = constrain(moveY, -127, 127);

  }

  // Отправляем движение только если есть значительное изменение

  if (moveX != 0 || moveY != 0) {

    mouse.move(moveX, moveY);

    // Вывод отладочной информации (редко, чтобы не засорять консоль)

    static unsigned long lastDebug = 0;

    if (millis() - lastDebug > 500) {

      Serial.printf("Move: X=%d, Y=%d (Raw: X=%.2f, Y=%.2f)\n",

                   moveX, moveY, rawX, rawY);

      lastDebug = millis();

    }

  }

}

void handleSoundCommands() {

  int micValue = analogRead(MIC_PIN);

  int amplitude = abs(micValue - 1800); // Компенсация смещения MAX9814

  if (amplitude > soundThreshold) {

    unsigned long currentTime = millis();

    if (currentTime - lastClapTime > clapCooldown) {

      // Одиночный хлопок - левый клик

      mouse.click(MOUSE_LEFT);

      Serial.println("Left click - clap detected");

      lastClapTime = currentTime;

    }

  }

}

void setup() {

  Serial.begin(115200);

  Serial.println("\n=== ESP32 BLE Air Mouse with Stabilization ===\n");

  // Инициализация MPU6050

  Wire.begin();

  mpu6050.begin();

  // Калибровка MPU6050

  calibrateMPU();

  // Дополнительная калибровка гироскопа

  Serial.println("Calibrating gyro... move device in figure 8 pattern");

  mpu6050.calcGyroOffsets(true);

  Serial.println("Gyro calibration complete");

  // Инициализация BLE мыши

  mouse.begin();

  Serial.println("BLE Mouse started");

  // Настройка пина микрофона

  pinMode(MIC_PIN, INPUT);

  Serial.println("\n=== Device Ready ===");

  Serial.println("Voice command: Clap for left click");

  Serial.println("Keep device steady when not in use");

  Serial.println("=================================");

}

void loop() {

  // Проверяем подключение

  if (mouse.isConnected() != connected) {

    connected = mouse.isConnected();

    if (connected) {

      Serial.println("Connected to host");

    } else {

      Serial.println("Disconnected from host");

    }

  }

  if (connected) {

    // Обработка движения

    processMovement();

    // Обработка звуковых команд

    handleSoundCommands();

  } else {

    // В режиме ожидания периодически калибруемся

    static unsigned long lastCalibration = 0;

    if (millis() - lastCalibration > 5000) {

      calibrateMPU();

      lastCalibration = millis();

    }

    delay(100);

  }

  // Небольшая задержка для стабильности

  delay(15);

}

// Функция для принудительной перекалибровки

void forceRecalibration() {

  Serial.println("Forcing recalibration...");

  calibrated = false;

  calibrateMPU();

}

// Функция для тестирования стабильности

void testStability() {

  Serial.println("Testing stability - device should be still");

  for (int i = 0; i < 20; i++) {

    mpu6050.update();

    float x = mpu6050.getAccX() - offsetX;

    float y = mpu6050.getAccY() - offsetY;

    Serial.printf("Stability: X=%.3f, Y=%.3f\n", x, y);

    delay(100);

  }

  Serial.println("Stability test completed");

}

// Функция для регулировки параметров

void adjustSensitivity(float newSensitivity) {

  sensitivity = newSensitivity;

  Serial.printf("Sensitivity set to: %.2f\n", sensitivity);

}

void adjustDeadZone(float newDeadZone) {

  deadZone = newDeadZone;

  Serial.printf("Dead zone set to: %.2f\n", deadZone);

}