---

#include "FastIMU.h"
#include "Madgwick.h"
#include "EEPROM.h"
#include <Wire.h>
#include "HID.h" 

// Конфигурация IMU
#define IMU_ADDRESS 0x68
MPU6050 IMU;

#define IMU_GEOMETRY 0

// Параметры фильтра Madgwick
#define FILTER_MAX_BETA 0.15
#define FILTER_MIN_BETA 0.015
#define FILTER_DROPOFF  0.85

calData calib = { 0 };
AccelData IMUAccel;
GyroData IMUGyro;
MagData IMUMag;
GyroData GyroVel;

Madgwick filter;
bool flag = false;

// HID-дескриптор для SteamVR (Relativty)
static const uint8_t _hidReportDescriptor[] PROGMEM = {
  0x06, 0x03, 0x00, 0x09, 0x00, 0xa1, 0x01, 0x15, 0x00, 0x26, 0xff, 0x00,
  0x85, 0x01, 0x75, 0x08, 0x95, 0x3f, 0x09, 0x00, 0x81, 0x02, 0xc0
};

float quat[4];
uint8_t reportBuffer[63]; 

void setup() {
  // Настройка I2C для STM32 (Blue Pill: PB6 - SCL, PB7 - SDA)
  Wire.setSDA(PB7);
  Wire.setSCL(PB6);
  Wire.begin();
  Wire.setClock(400000);

  Serial.begin(115200);

  // Инициализация USB HID
  static HIDSubDescriptor node(_hidReportDescriptor, sizeof(_hidReportDescriptor));
  HID().AppendDescriptor(&node);
  
  // Чтение калибровки из EEPROM
  int calStatus = 0;
  EEPROM.get(100, calStatus);
  if (calStatus == 99) {
    EEPROM.get(200, calib);
  }
  
  IMU.setIMUGeometry(IMU_GEOMETRY);
  int err = IMU.init(calib, IMU_ADDRESS);
  
  if (err != 0) {
    while(!Serial) { delay(10); }
    Serial.print("Error initializing IMU! e:");
    Serial.println(err);
    while (true) { delay(100); }
  }

  filter.begin(2.0f);
  
  // Разогрев фильтра
  for (int i = 0; i < 2000; i++) {
    IMU.update();
    IMU.getAccel(&IMUAccel);
    IMU.getGyro(&IMUGyro);
    if (IMU.hasMagnetometer()) {
      IMU.getMag(&IMUMag);
      filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);
    } else {
      filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
    }
  }
}

void loop() {
  // Вход в режим калибровки через терминал
  if (Serial) {
    if (!flag) {
      Serial.println("Send 'y' to enter calibration mode.");
    }
    flag = true;
    
    if (Serial.available() > 0 && Serial.read() == 'y') {
      calib = { 0 };
      IMU.init(calib, IMU_ADDRESS);
      Serial.println("Calibrating... Keep it still!");
      delay(5000);
      IMU.calibrateAccelGyro(&calib);
      
      if (IMU.hasMagnetometer()) {
        Serial.println("Calibrating Mag... Move in figure 8!");
        delay(2000);
        IMU.calibrateMag(&calib);
      }
      
      EEPROM.put(200, calib);
      EEPROM.put(100, 99);
      Serial.println("Done! Resetting...");
      delay(1000);
      NVIC_SystemReset(); // Перезагрузка STM32
    }
  }
  
  IMU.update();
  IMU.getAccel(&IMUAccel);
  IMU.getGyro(&IMUGyro);

  // Динамическое изменение Beta фильтра
  float Av = GyroVel.gyroX * GyroVel.gyroX + GyroVel.gyroY * GyroVel.gyroY + GyroVel.gyroZ * GyroVel.gyroZ;
  if (Av > 100.f) Av = 100.f;
  filter.changeBeta(Av * (FILTER_MAX_BETA - FILTER_MIN_BETA) / 100 + FILTER_MIN_BETA);

  if (IMU.hasMagnetometer()) {
    IMU.getMag(&IMUMag);
    filter.update(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ, IMUMag.magX, IMUMag.magY, IMUMag.magZ);
  } else {
    filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
  }

  // Обновление угловой скорости для фильтра
  GyroVel.gyroX = (GyroVel.gyroX + IMUGyro.gyroX * filter.delta_t) * FILTER_DROPOFF;
  GyroVel.gyroY = (GyroVel.gyroY + IMUGyro.gyroY * filter.delta_t) * FILTER_DROPOFF;
  GyroVel.gyroZ = (GyroVel.gyroZ + IMUGyro.gyroZ * filter.delta_t) * FILTER_DROPOFF;

  // Формирование кватерниона
  quat[0] = filter.getQuatW();
  quat[1] = filter.getQuatX();
  quat[2] = filter.getQuatY();
  quat[3] = filter.getQuatZ();

  // Отправка через HID (63 байта данных)
  memset(reportBuffer, 0, sizeof(reportBuffer));
  memcpy(reportBuffer, quat, sizeof(quat));
  HID().SendReport(1, reportBuffer, 63);
}