#include "mpu9250.h"
#include "esp_timer.h"

i2c_device_config_t MPU9250_DEV_CFG = {
    .dev_addr_length = I2C_ADDR_BIT_LEN_7,
    .device_address = 0x68,
    .scl_speed_hz = 100000,
};

#define MAG_SCALE (4912.0f / 32760.0f)

i2c_master_dev_handle_t MPU9250_DEV_HANDLE;

esp_err_t mpu9250_read_sensor_data(i2c_master_dev_handle_t dev_handle, int16_t *accel, int16_t *gyro, int16_t *temp, int16_t *magnet)
{
    uint8_t buffer[14]; // 6 (Accel) + 2 (Temp) + 6 (Gyro)
    esp_err_t ret = i2c_read_register(dev_handle, 0x3B, buffer, 14);
    if (ret != ESP_OK)
        return ret;

    // Convert raw data (Big-Endian)
    accel[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Accel X
    accel[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Accel Y
    accel[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Accel Z
    *temp = (((uint16_t)(buffer[6] & 0xFF)) << 8) | (buffer[7] & 0xFF);    // Temperature
    gyro[0] = (((uint16_t)(buffer[8] & 0xFF)) << 8) | (buffer[9] & 0xFF);
    ; // Gyro X
    gyro[1] = (((uint16_t)(buffer[10] & 0xFF)) << 8) | (buffer[11] & 0xFF);
    ; // Gyro Y
    gyro[2] = (((uint16_t)(buffer[12] & 0xFF)) << 8) | (buffer[13] & 0xFF);
    ; // Gyro Z

    ret = i2c_read_register(dev_handle, 0x03, buffer, 6);
    if (ret != ESP_OK)
        return ret;
    magnet[0] = (((uint16_t)(buffer[0] & 0xFF)) << 8) | (buffer[1] & 0xFF); // Magnet X
    magnet[1] = (((uint16_t)(buffer[2] & 0xFF)) << 8) | (buffer[3] & 0xFF); // Magnet Y
    magnet[2] = (((uint16_t)(buffer[4] & 0xFF)) << 8) | (buffer[5] & 0xFF); // Magnet Z

    return ESP_OK;
}

void mpu9250_convert_data(int16_t *accel, int16_t *gyro, int16_t temp, int16_t *magnet, float *accel_out, float *gyro_out, float *temp_out, float *magnet_out)
{
    accel_out[0] = accel[0] / 16384.0; // Accel X in g
    accel_out[1] = accel[1] / 16384.0; // Accel Y in g
    accel_out[2] = accel[2] / 16384.0; // Accel Z in g

    gyro_out[0] = gyro[0] / 131.0; // Gyro X in deg/s
    gyro_out[1] = gyro[1] / 131.0; // Gyro Y in deg/s
    gyro_out[2] = gyro[2] / 131.0; // Gyro Z in deg/s

    magnet_out[0] = magnet[0] * MAG_SCALE; // Gyro X in deg/s
    magnet_out[1] = magnet[1] * MAG_SCALE; // Gyro Y in deg/s
    magnet_out[2] = magnet[2] * MAG_SCALE; // Gyro Z in deg/s

    *temp_out = (temp / 333.87) + 21.0; // Temperature in °C
}

void mpu9250_init()
{
    ESP_ERROR_CHECK(i2c_master_bus_add_device(i2c0_bus_hdl, &MPU9250_DEV_CFG, &MPU9250_DEV_HANDLE));
    i2c_write_register_16(MPU9250_DEV_HANDLE, 0x6B, 0x0001); // zapni uz tu hovadinu
    i2c_write_register_16(MPU9250_DEV_HANDLE, 0x0A, 0x0012); // zapni uz tu hovadinu
}