#include <math.h>
#include "lsm6dsv.h"
#include "driver/i2c.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_system.h"
#include "esp_log.h"
#include "sdkconfig.h"

static const char *TAG = "LSM6DSV";
static const int I2C_PORT = 0;

// I2C helper functions
static esp_err_t i2c_write_reg(uint8_t reg_addr, uint8_t data) {
    uint8_t write_buf[2] = {reg_addr, data};
    return i2c_master_write_to_device(I2C_PORT, LSM6DSV_ADDR, write_buf, sizeof(write_buf), pdMS_TO_TICKS(100));
}

static esp_err_t i2c_read_reg(uint8_t reg_addr, uint8_t *data) {
    return i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg_addr, 1, data, 1, pdMS_TO_TICKS(100));
}

/**
 * @brief  Converts uint16_t half-precision number into a uint32_t single-precision number.
 * @param  h half-precision number
 * @param  f pointer where the result of the conversion is written
 * @retval 0
 */
esp_err_t npy_halfbits_to_floatbits(uint16_t h, uint32_t *f)
{
  uint16_t h_exp, h_sig;
  uint32_t f_sgn, f_exp, f_sig;

  h_exp = (h & 0x7c00u);
  f_sgn = ((uint32_t)h & 0x8000u) << 16;
  switch (h_exp) {
    case 0x0000u: /* 0 or subnormal */
      h_sig = (h & 0x03ffu);
      /* Signed zero */
      if (h_sig == 0) {
        *f = f_sgn;
        return ESP_OK;
      }
      /* Subnormal */
      h_sig <<= 1;
      while ((h_sig & 0x0400u) == 0) {
        h_sig <<= 1;
        h_exp++;
      }
      f_exp = ((uint32_t)(127 - 15 - h_exp)) << 23;
      f_sig = ((uint32_t)(h_sig & 0x03ffu)) << 13;
      *f = f_sgn + f_exp + f_sig;
      return ESP_OK;
    case 0x7c00u: /* inf or NaN */
      /* All-ones exponent and a copy of the significand */
      *f = f_sgn + 0x7f800000u + (((uint32_t)(h & 0x03ffu)) << 13);
      return ESP_OK;
    default: /* normalized */
      /* Just need to adjust the exponent and shift */
      *f = f_sgn + (((uint32_t)(h & 0x7fffu) + 0x1c000u) << 13);
      return ESP_OK;
  }
}

esp_err_t npy_half_to_float(uint16_t h, float *f)
{
  union {
    float ret;
    uint32_t retbits;
  } conv;
  npy_halfbits_to_floatbits(h, &conv.retbits);
  *f = conv.ret;
  return ESP_OK;
}

/**
 * @brief  Compute quaternions.
 * @param  quat results of the computation
 * @param  sflp raw value of the quaternions
 * @retval 0
 */
esp_err_t sflp2q(float quat[4], uint16_t sflp[3])
{
  float sumsq = 0;

  npy_half_to_float(sflp[0], &quat[0]);
  npy_half_to_float(sflp[1], &quat[1]);
  npy_half_to_float(sflp[2], &quat[2]);

  for (uint8_t i = 0; i < 3; i++) {
    sumsq += quat[i] * quat[i];
  }

  if (sumsq > 1.0f) {
    float n = sqrtf(sumsq);
    quat[0] /= n;
    quat[1] /= n;
    quat[2] /= n;
    sumsq = 1.0f;
  }

  quat[3] = sqrtf(1.0f - sumsq);
  return ESP_OK;
}

void quaternion_to_euler(float qx, float qy, float qz, float qw, float *roll, float *pitch, float *yaw) {
    // Roll (X-axis rotation)
    *roll = atan2f(2.0f * (qw * qx + qy * qz), 1.0f - 2.0f * (qx * qx + qy * qy));

    // Pitch (Y-axis rotation)
    *pitch = asinf(2.0f * (qw * qy - qz * qx));

    // Yaw (Z-axis rotation)
    *yaw = atan2f(2.0f * (qw * qz + qx * qy), 1.0f - 2.0f * (qy * qy + qz * qz));
}

esp_err_t lsm6dsv_init(int scl_pin, int sda_pin) {
    // Configure I2C
    i2c_config_t conf = {
        .mode = I2C_MODE_MASTER,
        .sda_io_num = sda_pin,
        .scl_io_num = scl_pin,
        .sda_pullup_en = GPIO_PULLUP_ENABLE,
        .scl_pullup_en = GPIO_PULLUP_ENABLE,
        .master.clk_speed = 400000  // 400 KHz
    };
    
    esp_err_t ret = i2c_param_config(I2C_PORT, &conf);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C parameter configuration failed");
        return ret;
    }

    ret = i2c_driver_install(I2C_PORT, I2C_MODE_MASTER, 0, 0, 0);
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "I2C driver installation failed");
        return ret;
    }

    // Verify device ID
    uint8_t who_am_i;
    ret = i2c_read_reg(LSM6DSV_WHO_AM_I, &who_am_i);
    if (ret != ESP_OK || who_am_i != 0x70) {  // 0x70 is the expected WHO_AM_I value
        ESP_LOGE(TAG, "Failed to verify device ID or incorrect device (expected: 0x70, got: 0x%02x)", who_am_i);
        return ESP_FAIL;
    }

    // Configure accelerometer (±2g, 104 Hz)
    ret = i2c_write_reg(LSM6DSV_CTRL1, 0x44);  // ODR = 104 Hz (0x04), ±2g (0x40)
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure accelerometer");
        return ret;
    }

    // Verify accelerometer configuration
    uint8_t ctrl1_val;
    ret = i2c_read_reg(LSM6DSV_CTRL1, &ctrl1_val);
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "CTRL1 value: 0x%02x", ctrl1_val);
    }

    // Configure gyroscope (±250 dps, 104 Hz)
    ret = i2c_write_reg(LSM6DSV_CTRL2, 0x04);  // ODR = 104 Hz (0x04), ±250 dps (0x40)
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to configure gyroscope");
        return ret;
    }

    // Verify gyroscope configuration
    uint8_t regval, tmp1;
    ret = i2c_read_reg(LSM6DSV_CTRL2, &regval);
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "CTRL2 value: 0x%02x", regval);
    }


    // ret = i2c_read_reg(LSM6DSV_SFLP_ODR, &regval);
    // regval |= SFLP_ODR_15HZ;
    // ret = i2c_write_reg(LSM6DSV_SFLP_ODR, regval);

    // // get current value of FUNC_CFG_ACCESS
    // ret = i2c_read_reg(LSM6DSV_FUNC_CFG_ACCESS, &regval);

    // // enable embedded function access
    // regval |= LSM6DSV_EMB_FUNC_REG_ACCESS;
    // ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);
    
    
    // // enable game vector FIDO
    // ret = i2c_write_reg(LSM6DSV_EMB_FUNC_FIFO_EN_A, SFLP_GAME_FIFO_EN); 

    // // enable sensor fusion algorithm
    // ret = i2c_write_reg(LSM6DSV_EMB_FUNC_EN_A, SFLP_GAME_EN);  
    // ret = i2c_write_reg(LSM6DSV_EMB_FUNC_INIT_A, SFLP_GAME_INIT); 


    // // restore FUNC_CFG_ACCESS
    // regval &= ~LSM6DSV_EMB_FUNC_REG_ACCESS;
    // ret = i2c_write_reg(LSM6DSV_FUNC_CFG_ACCESS, regval);

 
    // ret = i2c_write_reg(LSM6DSV_FIFO_CTRL4, 0x06);  

    // ret = i2c_read_reg(LSM6DSV_FIFO_CTRL4, &regval);
    // if (ret == ESP_OK) {
    //     ESP_LOGI(TAG, "FIFO_CTRL4 value: 0x%02x", regval);
    // }
   
    // Add a small delay after configuration
    vTaskDelay(pdMS_TO_TICKS(200));
    
    ESP_LOGI(TAG, "LSM6DSV initialized successfully");
    return ESP_OK;
}

esp_err_t lsm6dsv_read_data(lsm6dsv_data_t *data) {
    if (data == NULL) {
        return ESP_ERR_INVALID_ARG;
    }

    esp_err_t ret;
    uint8_t buffer[12];
    uint8_t reg = LSM6DSV_OUTX_L_A;

    // Check status register
    uint8_t status;
    ret = i2c_read_reg(LSM6DSV_STATUS, &status);
    if (ret == ESP_OK) {
        ESP_LOGD(TAG, "Status register: 0x%02x", status);
        if (!(status & 0x01)) {  // Check if accelerometer data is ready
            ESP_LOGW(TAG, "Accelerometer data not ready");
            return ESP_ERR_NOT_FOUND;
        }
    }

    // Read accelerometer data (6 bytes)
    ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer, 6, pdMS_TO_TICKS(100));
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read accelerometer data");
        return ret;
    }

    ESP_LOGD(TAG, "ACC raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x", 
             buffer[1], buffer[0], buffer[3], buffer[2], buffer[5], buffer[4]);

    // Check if gyroscope data is ready
    ret = i2c_read_reg(LSM6DSV_STATUS, &status);
    if (ret == ESP_OK) {
        if (!(status & 0x02)) {  // Check if gyroscope data is ready
            ESP_LOGW(TAG, "Gyroscope data not ready");
            return ESP_ERR_NOT_FOUND;
        }
    }

    // Read gyroscope data (6 bytes)
    reg = LSM6DSV_OUTX_L_G;
    ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, buffer + 6, 6, pdMS_TO_TICKS(100));
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "Failed to read gyroscope data");
        return ret;
    }

    ESP_LOGD(TAG, "GYRO raw: x=0x%02x%02x y=0x%02x%02x z=0x%02x%02x",
             buffer[7], buffer[6], buffer[9], buffer[8], buffer[11], buffer[10]);

    // Convert accelerometer data (±2g scale)
    const float acc_scale = 2.0f / 32768.0f;  // ±2g range
    data->acc_x = -(int16_t)(buffer[0] | (buffer[1] << 8)) * acc_scale;
    data->acc_y = -(int16_t)(buffer[2] | (buffer[3] << 8)) * acc_scale;
    data->acc_z = -(int16_t)(buffer[4] | (buffer[5] << 8)) * acc_scale;

    // Convert gyroscope data (±250 dps scale)
    const float gyro_scale = 250.0f / 32768.0f;  // ±250 dps range
    data->gyro_x = (int16_t)(buffer[6] | (buffer[7] << 8)) * gyro_scale;
    data->gyro_y = (int16_t)(buffer[8] | (buffer[9] << 8)) * gyro_scale;
    data->gyro_z = (int16_t)(buffer[10] | (buffer[11] << 8)) * gyro_scale;

    return ESP_OK;
}

uint8_t lsm6dsv_fifo_ready(void)
{
    esp_err_t ret;
    uint8_t status;

    // Check if fifo data is ready
    ret = i2c_read_reg(LSM6DSV_FIFO_STATUS2, &status);
    if (ret == ESP_OK) {
        //ESP_LOGI(TAG, "FIFO_STATUS2: %d", status);
    }

    ret = i2c_read_reg(LSM6DSV_FIFO_STATUS1, &status);
    if (ret == ESP_OK) {
        return status;
    }

    return 0;
}



esp_err_t lsm6dsv_fifo_read(lsm6dsv_fifo_t *fifo)
{
    esp_err_t ret;
    uint8_t data;
    uint8_t reg;
    ret = i2c_read_reg(LSM6DSV_FIFO_DATA_OUT_TAG, &data);

    reg = LSM6DSV_FIFO_DATA_OUT_X_L;
    ret = i2c_master_write_read_device(I2C_PORT, LSM6DSV_ADDR, &reg, 1, fifo->data, 6, pdMS_TO_TICKS(100));

    if (ret == ESP_OK)
    {
        fifo->count = (data & 0x06) >> 1;
        fifo->tag = (data & 0xf8) >> 3;

        fifo->v[0] = (uint16_t)(fifo->data[0] | (fifo->data[1] << 8));
        fifo->v[1] = (uint16_t)(fifo->data[2] | (fifo->data[3] << 8));
        fifo->v[2] = (uint16_t)(fifo->data[4] | (fifo->data[5] << 8));

        return ESP_OK;
    }
    
    return ESP_FAIL;
}

esp_err_t lsm6dsv_getAttitude(lsm6dsv_fifo_t *fifo, float *roll, float *pitch, float *yaw)
{
    float q[4];
    sflp2q(q, fifo->v);

    float w = q[3];
    float x = q[0];
    float y = q[1];
    float z = q[2];
    
    // Yaw (z-axis rotation)
    *yaw = atan2(2.0f * (x * y + w * z), w * w + x * x - y * y - z * z);
    
    // Pitch (y-axis rotation)
    *pitch = -asin(2.0f * (x * z - w * y));
    
    // Roll (x-axis rotation)
    *roll = atan2(2.0f * (w * x + y * z), w * w - x * x - y * y + z * z);

    return ESP_OK;

}