#include <Wire.h>
#include <MPU6050.h>
#include <BleCompositeHID.h>
#include <MouseDevice.h>
#include <KeyboardDevice.h>

#define BUTTON_PIN 4
#define PAUSE_BUTTON_PIN 5
#define LED_PIN 2

MPU6050 mpu;
KeyboardDevice* keyboard;
MouseDevice* mouse;
BleCompositeHID compositeHID("fire_extinguisher_1", "Misfit", 100);

// =================== ПЕРЕМЕННЫЕ ===================
float current_yaw_angle = 0.0;
float current_pitch_angle = 0.0;
float gyroZ_offset = 0.0;
float accelX_offset = 0.0;
bool is_calibrated = false;
unsigned long last_micros = 0;
const float GYRO_SCALE = 65.5;

// =================== НАСТРОЙКИ СКОРОСТИ ===================
float YAW_SPEED_GAIN = 1.4;      // x2 от 0.8 (горизонталь)
float PITCH_SPEED_GAIN = 1.6;   // x1.2 от 0.6 (вертикаль)
const int MAX_SPEED = 50;

// Для вычисления изменения угла
float prev_yaw_angle = 0.0;
float prev_pitch_angle = 0.0;

// Улучшенное сглаживание против дрожания
float smoothed_speedX = 0;
float smoothed_speedY = 0;
float smoothing = 0.4;           // Более сильное сглаживание

// Дополнительный фильтр для мелких движений
const float MIN_MOVEMENT_THRESHOLD = 0.8;  // Игнорировать очень мелкие изменения

// =================== ИСПРАВЛЕНИЯ ДЛЯ НАКЛОНА ===================
const float PITCH_DEADZONE = 1.2;      // Мертвая зона для наклона (градусы)
const float PITCH_RESET_THRESHOLD = 1.0; // Порог сброса накопленной ошибки
float last_valid_pitch = 0.0;
bool pitch_was_reset = false;

// =================== КАЛИБРОВКА ===================
const int CALIB_SAMPLES = 250;

void setup() {
    Serial.begin(115200);
    pinMode(LED_PIN, OUTPUT);
    pinMode(BUTTON_PIN, INPUT_PULLUP);
    pinMode(PAUSE_BUTTON_PIN, INPUT_PULLUP);
    
    Wire.begin(21, 22);
    Wire.setClock(400000);
    
    mpu.initialize();
    
    // Оптимальные настройки для уменьшения шума
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);  // 
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    mpu.setDLPFMode(MPU6050_DLPF_BW_20);            // фильтр против дрожания
    
    KeyboardConfiguration keyboardConfig;
    keyboardConfig.setUseMediaKeys(true);
    keyboard = new KeyboardDevice(keyboardConfig);

    MouseConfiguration mouseConfig;
    mouseConfig.setAutoReport(false);
    mouse = new MouseDevice(mouseConfig);

    compositeHID.addDevice(keyboard);
    compositeHID.addDevice(mouse);
    compositeHID.begin();
    
    for(int i = 0; i < 3; i++) {
        digitalWrite(LED_PIN, HIGH);
        delay(100);
        digitalWrite(LED_PIN, LOW);
        delay(100);
    }
    
    delay(1000);
    calibrateSensors();
    
    last_micros = micros();
    prev_yaw_angle = current_yaw_angle;
    prev_pitch_angle = current_pitch_angle;
    
    Serial.println("=== УЛУЧШЕННАЯ ВЕРСИЯ С ИСПРАВЛЕНИЯМИ ===");
    Serial.println("1. Инверсия осей (лево-право)");
    Serial.println("2. Убрано дрожание");
    Serial.println("3. Исправлено 'залипание' наклона");
    Serial.println("4. Скорость X: x2 | Y: x1.2");
}

void calibrateSensors() {
    Serial.println("Точная калибровка...");
    digitalWrite(LED_PIN, LOW);
    
    long gz_sum = 0;
    long ax_sum = 0;
    
    for(int i = 0; i < CALIB_SAMPLES; i++) {
        int16_t ax, ay, az, gx, gy, gz;
        mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        
        // Отбрасываем выбросы
        if (abs(gz) < 30000 && abs(ax) < 30000) {
            gz_sum += gz;
            ax_sum += ax;
        }
        
        delay(4);
    }
    
    gyroZ_offset = gz_sum / CALIB_SAMPLES;
    accelX_offset = ax_sum / CALIB_SAMPLES;
    
    // Сброс
    current_yaw_angle = 0;
    current_pitch_angle = 0;
    prev_yaw_angle = 0;
    prev_pitch_angle = 0;
    smoothed_speedX = 0;
    smoothed_speedY = 0;
    last_valid_pitch = 0.0;
    pitch_was_reset = true;
    
    is_calibrated = true;
    
    digitalWrite(LED_PIN, HIGH);
    Serial.print("Калибровка OK. Offsets: GZ=");
    Serial.print(gyroZ_offset);
    Serial.print(" AX=");
    Serial.println(accelX_offset);
}

// Функция для сильного подавления шума
float applyDeadzoneAndFilter(float value, float &filtered, float threshold) {
    // Игнорируем очень маленькие значения
    if (abs(value) < threshold) {
        return 0;
    }
    
    // Экспоненциальное сглаживание
    filtered = filtered * 0.7 + value * 0.3;
    return filtered;
}

// Новая функция для обработки наклона с исправлением "залипания"
float processPitchWithStuckFix(float raw_pitch) {
    static float last_stable_pitch = 0.0;
    static unsigned long last_reset_time = 0;
    
    // Если изменение слишком маленькое - игнорируем (мертвая зона)
    if (abs(raw_pitch - last_stable_pitch) < PITCH_DEADZONE) {
        return last_stable_pitch;
    }
    
    // Если наклонились достаточно сильно - обновляем значение
    last_stable_pitch = raw_pitch;
    last_reset_time = millis();
    
    return raw_pitch;
}

void loop() {
    if (Serial.available() > 0) {
        String cmd = Serial.readStringUntil('\n');
        cmd.trim();
        
        if (cmd.startsWith("y")) {
            float val = cmd.substring(1).toFloat();
            if (val >= 0.5 && val <= 3.0) {
                YAW_SPEED_GAIN = val;
                Serial.print("Скорость вращения: ");
                Serial.println(YAW_SPEED_GAIN);
            }
        } else if (cmd.startsWith("p")) {
            float val = cmd.substring(1).toFloat();
            if (val >= 0.5 && val <= 3.0) {
                PITCH_SPEED_GAIN = val;
                Serial.print("Скорость наклона: ");
                Serial.println(PITCH_SPEED_GAIN);
            }
        } else if (cmd == "cal") {
            calibrateSensors();
        } else if (cmd == "reset") {
            current_yaw_angle = 0;
            current_pitch_angle = 0;
            prev_yaw_angle = 0;
            prev_pitch_angle = 0;
            smoothed_speedX = 0;
            smoothed_speedY = 0;
            last_valid_pitch = 0.0;
            pitch_was_reset = true;
            Serial.println("Полный сброс");
        }
    }
    
    if (!compositeHID.isConnected()) {
        static unsigned long lastBlink = 0;
        static bool ledState = LOW;
        if (millis() - lastBlink > 500) {
            lastBlink = millis();
            ledState = !ledState;
            digitalWrite(LED_PIN, ledState);
        }
        delay(50);
        return;
    }
    
    digitalWrite(LED_PIN, HIGH);
    
    // Пауза
    bool currentPause = digitalRead(PAUSE_BUTTON_PIN);
    static bool lastPauseState = HIGH;
    static bool mousePaused = false;
    
    if (currentPause != lastPauseState) {
        delay(20);
        if (currentPause == LOW) {
            mousePaused = !mousePaused;
            Serial.println(mousePaused ? "Пауза ВКЛ" : "Пауза ВЫКЛ");
        }
        lastPauseState = currentPause;
    }
    
    if (mousePaused || !is_calibrated) {
        delay(50);
        return;
    }
    
    // === ОБРАБОТКА ===
    
    unsigned long current_micros = micros();
    float dt = (current_micros - last_micros) / 1000000.0;
    last_micros = current_micros;
    
    if (dt > 0.05) dt = 0.05;
    if (dt < 0.001) dt = 0.001;
    
    // Чтение данных
    int16_t ax, ay, az, gx, gy, gz;
    mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    // 1. YAW из гироскопа Z
    float gyroZ = (gz - gyroZ_offset) / GYRO_SCALE;
    current_yaw_angle += gyroZ * dt;
    
    // 2. PITCH из акселерометра X (наклон вперед/назад) - ИСПРАВЛЕННАЯ ВЕРСИЯ
    float accelX = (ax - accelX_offset) / 16384.0;
    
    // Защита от переполнения
    if (accelX > 0.99) accelX = 0.99;
    if (accelX < -0.99) accelX = -0.99;
    
    float new_pitch = asin(accelX) * 180.0 / PI;
    
    // Применяем исправление от "залипания"
    new_pitch = processPitchWithStuckFix(new_pitch);
    
    // Сильная фильтрация pitch против дрожания
    static float filtered_pitch = 0;
    static float pitch_filter_alpha = 0.15;  // Оптимальный баланс
    filtered_pitch = filtered_pitch * (1-pitch_filter_alpha) + new_pitch * pitch_filter_alpha;
    current_pitch_angle = filtered_pitch;
    
    // Автоматический сброс наклона при возвращении в нейтральное положение
    if (abs(current_pitch_angle) < PITCH_RESET_THRESHOLD && !pitch_was_reset) {
        current_pitch_angle = 0;
        filtered_pitch = 0;
        last_valid_pitch = 0;
        pitch_was_reset = true;
    } else if (abs(current_pitch_angle) >= PITCH_RESET_THRESHOLD) {
        pitch_was_reset = false;
    }
    
    // 3. ВЫЧИСЛЕНИЕ ИЗМЕНЕНИЯ УГЛОВ
    float yaw_change = current_yaw_angle - prev_yaw_angle;
    float pitch_change = current_pitch_angle - prev_pitch_angle;
    
    // Сохраняем для следующего цикла
    prev_yaw_angle = current_yaw_angle;
    prev_pitch_angle = current_pitch_angle;
    
    // 4. ИНВЕРСИЯ ОСЕЙ И ПРИМЕНЕНИЕ СКОРОСТНЫХ КОЭФФИЦИЕНТОВ
    // ИНВЕРСИЯ ЛЕВО-ПРАВО: меняем знак для оси X (поворот влево = курсор вправо)
    float raw_speedX = yaw_change * YAW_SPEED_GAIN * 12.0;    // Инверсия лево-право
    
    // ИНВЕРСИЯ ВПЕРЕД-НАЗАД: меняем знак для оси Y (наклон вперед = курсор вверх)
    float raw_speedY = -pitch_change * PITCH_SPEED_GAIN * 12.0; // Инверсия вперед-назад
    
    // 5. ПОДАВЛЕНИЕ ДРОЖАНИЯ
    // Фильтрация очень мелких движений
    static float filtered_speedX = 0;
    static float filtered_speedY = 0;
    
    raw_speedX = applyDeadzoneAndFilter(raw_speedX, filtered_speedX, MIN_MOVEMENT_THRESHOLD);
    raw_speedY = applyDeadzoneAndFilter(raw_speedY, filtered_speedY, MIN_MOVEMENT_THRESHOLD);
    
    // Дополнительное экспоненциальное сглаживание
    smoothed_speedX = smoothed_speedX * smoothing + raw_speedX * (1-smoothing);
    smoothed_speedY = smoothed_speedY * smoothing + raw_speedY * (1-smoothing);
    
    // Округление до целых с порогом
    int mouseX = 0;
    int mouseY = 0;
    
    if (abs(smoothed_speedX) > 0.5) mouseX = round(smoothed_speedX);
    if (abs(smoothed_speedY) > 0.5) mouseY = round(smoothed_speedY);
    
    // Ограничение максимальной скорости
    mouseX = constrain(mouseX, -MAX_SPEED, MAX_SPEED);
    mouseY = constrain(mouseY, -MAX_SPEED, MAX_SPEED);
    
    // 6. ДИАГНОСТИКА
    static unsigned long lastDebug = 0;
    if (millis() - lastDebug > 1200) {
        lastDebug = millis();
        
        Serial.print("ΔX: ");
        Serial.print(yaw_change, 2);
        Serial.print("° -> speed: ");
        Serial.print(raw_speedX, 1);
        Serial.print(" -> mouseX: ");
        Serial.print(mouseX);
        
        Serial.print(" | ΔY: ");
        Serial.print(pitch_change, 2);
        Serial.print("° -> speed: ");
        Serial.print(raw_speedY, 1);
        Serial.print(" -> mouseY: ");
        Serial.print(mouseY);
        
        Serial.print(" | Pitch: ");
        Serial.print(current_pitch_angle, 1);
        Serial.print("°");
        
        Serial.println();
    }
    
    // 7. ДВИЖЕНИЕ МЫШИ
    if (mouseX != 0 || mouseY != 0) {
        mouse->mouseMove(mouseX, mouseY);
        mouse->sendMouseReport();
    }
    
    // 8. ОБРАБОТКА КНОПКИ
    bool currentButton = digitalRead(BUTTON_PIN);
    static bool lastButtonState = HIGH;
    static bool mousePressed = false;
    
    if (currentButton != lastButtonState) {
        delay(25);
        if (currentButton == LOW && !mousePressed) {
            mouse->mousePress(MOUSE_LOGICAL_LEFT_BUTTON);
            mouse->sendMouseReport();
            mousePressed = true;
            
            keyboard->keyPress(KEY_1);
            keyboard->sendKeyReport();
            delay(40);
            keyboard->keyRelease(KEY_1);
            keyboard->sendKeyReport();
            
        } else if (currentButton == HIGH && mousePressed) {
            mouse->mouseRelease(MOUSE_LOGICAL_LEFT_BUTTON);
            mouse->sendMouseReport();
            mousePressed = false;
        }
        lastButtonState = currentButton;
    }
    
    delay(8); // ~83Hz - оптимально для плавности
}