#pragma once

#include <stdint.h>
#include "esp_err.h"

// LSM6DSV Register Addresses
#define LSM6DSV_ADDR                0x6A    // I2C address (SA0=0)

#define LSM6DSV_EMB_FUNC_EN_A       0x04
#define SFLP_GAME_EN                (1 << 1)

#define LSM6DSV_FUNC_CFG_ACCESS     0x01
#define LSM6DSV_EMB_FUNC_REG_ACCESS (1 << 7)

// FIFO Registers
#define LSM6DSV_FIFO_CTRL1          0x07  // FIFO Control Register 1
#define LSM6DSV_FIFO_CTRL2          0x08  // FIFO Control Register 2
#define LSM6DSV_FIFO_CTRL3          0x09  // FIFO Control Register 3
#define LSM6DSV_FIFO_CTRL4          0x0A  // FIFO Control Register 4

#define FIFO_MODE_CONTINUOUS        0x06

#define LSM6DSV_FIFO_STATUS1        0x1B  // FIFO Status Register 1
#define LSM6DSV_FIFO_STATUS2        0x1C  // FIFO Status Register 2

#define LSM6DSV_FIFO_DATA_OUT_TAG   0x78  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_L   0x79  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_X_H   0x7A  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_L   0x7B  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Y_H   0x7C  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_L   0x7D  // FIFO Data Output Register
#define LSM6DSV_FIFO_DATA_OUT_Z_H   0x7E  // FIFO Data Output Register




#define LSM6DSV_WHO_AM_I            0x0F    // Device ID register
#define LSM6DSV_CTRL1               0x10    // Control register 1
#define LSM6DSV_CTRL2               0x11    // Control register 2
#define LSM6DSV_FIFO_STATUS1        0x1B
#define LSM6DSV_STATUS              0x1E    // Status register
#define LSM6DSV_OUTX_L_A            0x28    // Accelerometer X-axis LSB
#define LSM6DSV_OUTX_H_A            0x29    // Accelerometer X-axis MSB
#define LSM6DSV_OUTY_L_A            0x2A    // Accelerometer Y-axis LSB
#define LSM6DSV_OUTY_H_A            0x2B    // Accelerometer Y-axis MSB
#define LSM6DSV_OUTZ_L_A            0x2C    // Accelerometer Z-axis LSB
#define LSM6DSV_OUTZ_H_A            0x2D    // Accelerometer Z-axis MSB
#define LSM6DSV_OUTX_L_G            0x22    // Gyroscope X-axis LSB
#define LSM6DSV_OUTX_H_G            0x23    // Gyroscope X-axis MSB
#define LSM6DSV_OUTY_L_G            0x24    // Gyroscope Y-axis LSB
#define LSM6DSV_OUTY_H_G            0x25    // Gyroscope Y-axis MSB
#define LSM6DSV_OUTZ_L_G            0x26    // Gyroscope Z-axis LSB
#define LSM6DSV_OUTZ_H_G            0x27    // Gyroscope Z-axis MSB

#define LSM6DSV_SFLP_ODR            0x5E
#define SFLP_ODR_15HZ               (0 << 3)
#define SFLP_ODR_30HZ               (1 << 3)
#define SFLP_ODR_60HZ               (2 << 3)
#define SFLP_ODR_120HZ              (3 << 3)
#define SFLP_ODR_240HZ              (4 << 3)
#define SFLP_ODR_480HZ              (5 << 3)


#define LSM6DSV_EMB_FUNC_FIFO_EN_A  0x44
#define SFLP_GAME_FIFO_EN           (1 << 1)

#define LSM6DSV_EMB_FUNC_INIT_A     0x66
#define SFLP_GAME_INIT              (1 << 1)



typedef struct lsm6dsv_fifo_s
{
    uint8_t tag;
    uint8_t count;
    uint8_t data[6];
    uint16_t v[3];

} lsm6dsv_fifo_t;

// Sensor data structure
typedef struct {
    float acc_x;
    float acc_y;
    float acc_z;
    float gyro_x;
    float gyro_y;
    float gyro_z;
} lsm6dsv_data_t;

// Function declarations
esp_err_t lsm6dsv_init(int scl_pin, int sda_pin);
esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data);

uint8_t lsm6dsv_fifo_ready(void);

esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo);

esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw);