#include "device.h"

esp_err_t Device::hardware_init()
{
  // HW Buses Init
  Serial.begin(SERIAL_BAUD);
  I2C0_BUS.setPins(I2C0_SDA, I2C0_SCL);
  SPI0_BUS.begin(SPI0_SCLK, SPI0_MISO, SPI0_MOSI);

  // IMU Init
  Imu.begin(SPI0_CS, SPI0_BUS);

  if (Imu.status != ICM_20948_Stat_Ok)
  {
    ESP_LOGE("IMU", "IMU failed to connect!");
    return ESP_ERR_NOT_FOUND;
  }

  // 1. Vytvorte si štruktúru pre delič frekvencie
ICM_20948_smplrt_t mySmplrt;

// 2. Vypočítajte a zadajte delič (Divider)
// Vzorec: Sample Rate = 1125 / (1 + Divider)
// Príklad pre ~100 Hz: 1125 / (1 + 10) = 102.2 Hz
mySmplrt.a = 4; // Delič pre akcelerometer
mySmplrt.g = 4; // Delič pre gyroskop

// 3. (Aktivujte DLPF - bez toho delič nefunguje!)
Imu.enableDLPF(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, true);

// 4. Aplikujte nastavenie
Imu.setSampleRate(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, mySmplrt);

  // ENV Init
  bool env_status = Env.begin(BME280_ADDR, &I2C0_BUS);

  if (env_status != 1)
  {
    ESP_LOGE("ENV", "ENV failed to connect!");
    return ESP_ERR_NOT_FOUND;
  }

  return ESP_OK;
}

Device Dev; // Global Instance