#include "main.h"
#include "i2c.h"


//MPU
#define MPU9250_ADDR 0x68

extern I2C_HandleTypeDef hi2c1;  // Only declare

void IMU_Init(uint8_t a_range, uint8_t g_range, uint8_t delay)
{
	uint8_t buffer1[2];
		// Wake up MPU-9250
	 buffer1 [0] = 0x6B;
	 buffer1 [1] = 0x00;
	 HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);
	 HAL_Delay(delay);  // Wait 100ms after wake-up

	  // Set accelerometer to range
	  buffer1 [0] = 0x1C;
	  buffer1 [1] = a_range;
	  HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);

	  // Set gyroscope to range
	  buffer1 [0] = 0x1B;
	  buffer1 [1] = g_range;
	  HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);

	  // Set low-pass filter (5Hz)
	  buffer1 [0] = 0x1A;
	  buffer1 [1] = 0x06;
	  HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);

	  // Set sampling rate to 200 Hz
	  buffer1 [0] = 0x19;
	  buffer1 [1] = 0x04;
	  HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);

	  // Enable Data Ready Interrupt
	  buffer1 [0] = 0x38;
	  buffer1 [1] = 0x01;
	  HAL_I2C_Master_Transmit(&hi2c1, (MPU9250_ADDR << 1), buffer1, 2, HAL_MAX_DELAY);

//	  // new
//	  buffer1[0] = 0x6C;  // PWR_MGMT_2
//	  buffer1[1] = 0x00;  // All sensors ON
//	  HAL_I2C_Master_Transmit(&hi2c1, MPU9250_ADDR << 1, buffer1, 2, HAL_MAX_DELAY);

}

//get gyroscope data
AccelData IMU_Acc(uint8_t range)
{
    uint8_t buffer[6];
    float sensitivity;
    AccelData acc;
    int16_t xdata, ydata,zdata;

    switch (range)
    {
        case 0x00: sensitivity = 16384.0f; break;
        case 0x08: sensitivity = 8192.0f; break;
        case 0x10: sensitivity = 4096.0f; break;
        case 0x18: sensitivity = 2048.0f; break;
        default  : sensitivity = 16384.0f; break;
    }


	HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDR << 1, 0x3B, 1, buffer, 6, HAL_MAX_DELAY);
	xdata = (int16_t)((buffer[0] << 8) | buffer[1]);
	ydata = (int16_t)((buffer[2] << 8) | buffer[3]);
	zdata = (int16_t)((buffer[4] << 8) | buffer[5]);

		    acc.x = (float) xdata  /  sensitivity ;
			acc.y = (float) ydata  /  sensitivity ;
			acc.z = (float) zdata  /  sensitivity ;

//	    acc.x = (float) xdata * (9.80665 / sensitivity);
//	    acc.y = (float) ydata * (9.80665 / sensitivity);
//	    acc.z = (float) zdata * (9.80665 / sensitivity);
//
//	    HAL_Delay(1); //make sure there is no overflow

    return acc;
}

GyroData IMU_gyro(uint8_t range)
{
    uint8_t buffer[6];
    float gyro_sensitivity;

    switch (range)
    {
        case 0x00: gyro_sensitivity = 131.0f; break;
        case 0x08: gyro_sensitivity = 65.5f; break;
        case 0x10: gyro_sensitivity = 32.8f; break;
        case 0x18: gyro_sensitivity = 16.4f; break;
        default  : gyro_sensitivity = 131.0f; break;
    }

    HAL_I2C_Mem_Read(&hi2c1, MPU9250_ADDR << 1, 0x43, 1, buffer, 6, HAL_MAX_DELAY);

    int16_t gxdata = (int16_t)((buffer[0] << 8) | buffer[1]);
    int16_t gydata = (int16_t)((buffer[2] << 8) | buffer[3]);
    int16_t gzdata = (int16_t)((buffer[4] << 8) | buffer[5]);

    GyroData data;
    data.x = (float)gxdata / gyro_sensitivity;
    data.y = (float)gydata / gyro_sensitivity;
    data.z = (float)gzdata / gyro_sensitivity;

    return data;
}