---

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

// Для STM32 официального ядра вместо HID.h используем встроенные средства
#include <USBHID.h>

#define IMU_ADDRESS 0x68
MPU6050 IMU;
#define IMU_GEOMETRY 0

#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[] = {
  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]; 
USBHID HIDDevice; // Создаем объект HID для STM32

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 для STM32
  HIDDevice.begin(_hidReportDescriptor, sizeof(_hidReportDescriptor));
  
  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! IMU not found: ");
    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);
    filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);
  }
}

void loop() {
  // Калибровка через сериал
  if (Serial.available() > 0 && Serial.read() == 'y') {
    calib = { 0 };
    IMU.init(calib, IMU_ADDRESS);
    Serial.println("Calibrating...");
    delay(5000);
    IMU.calibrateAccelGyro(&calib);
    EEPROM.put(200, calib);
    EEPROM.put(100, 99);
    Serial.println("Done! Resetting...");
    delay(1000);
    NVIC_SystemReset(); 
  }
  
  IMU.update();
  IMU.getAccel(&IMUAccel);
  IMU.getGyro(&IMUGyro);

  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);

  filter.updateIMU(IMUGyro.gyroX, IMUGyro.gyroY, IMUGyro.gyroZ, IMUAccel.accelX, IMUAccel.accelY, IMUAccel.accelZ);

  quat[0] = filter.getQuatW();
  quat[1] = filter.getQuatX();
  quat[2] = filter.getQuatY();
  quat[3] = filter.getQuatZ();

  // Отправка данных в драйвер SteamVR
  memset(reportBuffer, 0, sizeof(reportBuffer));
  memcpy(reportBuffer, quat, sizeof(quat));
  HIDDevice.sendReport(1, reportBuffer, 63);
}