#ifndef MPU6886_H
#define MPU6886_H

#include "driver/i2c.h"
#include "esp_err.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include <math.h>
#include "mqtt_client.h"
#include <string.h>
#include <stdio.h>

#define I2C_MASTER_SCL_IO           CONFIG_I2C_MASTER_SCL       /*!< GPIO number used for I2C master clock */
#define I2C_MASTER_SDA_IO           CONFIG_I2C_MASTER_SDA       /*!< GPIO number used for I2C master data  */
#define I2C_MASTER_NUM              I2C_NUM_0                   /*!< I2C port number for master dev */
#define I2C_MASTER_FREQ_HZ          CONFIG_I2C_MASTER_FREQUENCY /*!< I2C master clock frequency */
#define I2C_MASTER_TX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                           /*!< I2C master doesn't need buffer */
#define I2C_MASTER_TIMEOUT_MS       1000

#define MPU6886_ADDR 0x68

// MPU6886 Registers
#define MPU6886_PWR_MGMT_1     0x6B
#define MPU6886_WHO_AM_I       0x75
#define MPU6886_ACCEL_XOUT_H   0x3B
#define MPU6886_GYRO_XOUT_H    0x43
#define MPU6886_TEMP_OUT_H     0x41
#define MPU6886_ACCEL_CONFIG   0x1C
#define MPU6886_GYRO_CONFIG    0x1B

// Sensitivity scales
#define ACCEL_SO_2G  16384.0f
#define ACCEL_SO_4G   8192.0f
#define ACCEL_SO_8G   4096.0f
#define ACCEL_SO_16G  2048.0f

#define GYRO_SO_250DPS  131.0f
#define GYRO_SO_500DPS   65.5f
#define GYRO_SO_1000DPS  32.8f
#define GYRO_SO_2000DPS  16.4f

#define TEMP_SO 326.8f
#define TEMP_OFFSET 25.0f

// Scale factors
#define SF_M_S2 9.80665f
#define SF_RAD_S 0.017453292519943f // pi/180

typedef struct {
    float x;
    float y;
    float z;
} vec3_t;

typedef struct {
    i2c_port_t i2c_port;
    uint8_t address;
    float accel_div;
    float gyro_div;
    vec3_t gyro_offset;
    vec3_t accel_offset;
} mpu6886_t;

static esp_mqtt_client_handle_t s_mqtt_client = NULL;

// Function declarations
esp_err_t mpu6886_init(mpu6886_t *dev, i2c_port_t i2c_port);
esp_err_t mpu6886_read_accel(mpu6886_t *dev, vec3_t *accel);
esp_err_t mpu6886_read_gyro(mpu6886_t *dev, vec3_t *gyro);
esp_err_t mpu6886_read_temp(mpu6886_t *dev, float *temp);
esp_err_t mpu6886_calibrate_gyro(mpu6886_t *dev, int samples, int delay_ms);
esp_err_t mpu6886_calibrate_accel(mpu6886_t *dev, int samples, int delay_ms);

#endif