/*
 * MPU6000.c
 *
 *  Created on: Nov 24, 2021
 *      Author: angoosh
 */

#include "MPU6000.h"
#include "main.h"
#include <math.h>

MPU6000_Typedef IMU;

uint32_t sysclock = 0;

void MPU6000_readGyro();
void MPU6000_readAccel();
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout);
uint8_t MPU6000_readRegister(uint8_t reg);
void MPU6000_Calibrate();
void MPU6000_Gyro();
void MPU6000_Raw_Data_Convert();
void MPU6000_readAll();
void MPU6000_Pedometer_Init();
void MPU6000_Pedometer();
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c);
void IMU_Check_State();

madgwick_handle_t madgwick_handle;
madgwick_quat_data_t quat_data;

/**
 * @brief Converts raw sensor data to usable format
 * @retval none
 */
void MPU6000_Raw_Data_Convert(){
	for(int i = 0; i < 6; i++){
		IMU.accel_data[i] = IMU.all_data[i];
		IMU.gyro_data[i] = IMU.all_data[i+8];
	}

	IMU.g_x = (float)((int16_t)(IMU.gyro_data[0] * 256 + IMU.gyro_data[1]) - IMU.cal_g_x) / IMU.gyro_LSB;
	IMU.g_y = (float)((int16_t)(IMU.gyro_data[2] * 256 + IMU.gyro_data[3]) - IMU.cal_g_y) / IMU.gyro_LSB;
	IMU.g_z = (float)((int16_t)(IMU.gyro_data[4] * 256 + IMU.gyro_data[5]) - IMU.cal_g_z) / IMU.gyro_LSB;

	IMU.a_x = (float)((int16_t)(IMU.accel_data[0] * 256 + IMU.accel_data[1]) - IMU.cal_a_x) / IMU.accel_LSB;
	IMU.a_y = (float)((int16_t)(IMU.accel_data[2] * 256 + IMU.accel_data[3]) - IMU.cal_a_y) / IMU.accel_LSB;
	IMU.a_z = (float)((int16_t)(IMU.accel_data[4] * 256 + IMU.accel_data[5]) - IMU.cal_a_z) / IMU.accel_LSB;
}

/**
 * @brief Read all data on the sensor
 * @retval none
 */
void MPU6000_readAll(){
	HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.all_data,14);
}

/**
 * @brief Read only gyroscope
 * @retval none
 */
void MPU6000_readGyro(){
	switch(GYRO_RANGE){
	case 0:
		IMU.gyro_LSB = 131;
	case 1:
		IMU.gyro_LSB = 65.5;
	case 2:
		IMU.gyro_LSB = 32.8;
	case 3:
		IMU.gyro_LSB = 16.4;
	default:
		IMU.gyro_LSB = 16.4;
	}
	HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_XOUT_H,1,IMU.gyro_data,6);
}

/**
 * @brief Read only accelerometer
 * @retval none
 */
void MPU6000_readAccel(){
	switch(GYRO_RANGE){
	case 0:
		IMU.accel_LSB = 16384;
	case 1:
		IMU.accel_LSB = 8192;
	case 2:
		IMU.accel_LSB = 4096;
	case 3:
		IMU.accel_LSB = 2048;
	default:
		IMU.accel_LSB = 2048;
	}

	HAL_I2C_Mem_Read_IT(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_XOUT_H,1,IMU.accel_data,6);
}

/**
 * @brief Converts accelerometer data to roll pitch and yaw
 * @retval none
 */
void MPU6000_Gyro(){
	IMU.roll = atan2(IMU.a_y, IMU.a_z) * 180.0 / M_PI;
	IMU.pitch = atan2(IMU.a_x, IMU.a_z) * 180.0 / M_PI;
	IMU.yaw = atan2(IMU.a_x, IMU.a_y) * 180.0 / M_PI;
}

/**
 * @brief Initialization function of MPU6000 library
 * @param i2c peripheral to be used
 * @param i2c timeout
 * @retval none
 */
int MPU6000_Init(I2C_HandleTypeDef *hi2c, uint32_t timeout){
	uint8_t data;

	IMU.accel_LSB = 16384;
	IMU.gyro_LSB = 131;
	IMU.peripheral = hi2c;
	IMU.timeout = timeout;

	IMU.dataReady = 0;

	madgwick_cfg_t madgwick_cfg;

	madgwick_cfg.beta = MADGWICK_BETA;
	madgwick_cfg.sample_freq = MADGWICK_SAMPLE_RATE;
	madgwick_handle = madgwick_init(&madgwick_cfg);

	if(HAL_I2C_IsDeviceReady(hi2c, (uint16_t)(MPU6000_ADDRESS << 1), 3, IMU.timeout) != 0){
		return 1;
	}

	data = 0x00;
	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
	HAL_Delay(200);

	data = 0x07;
	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),SMPLRT_DIV,1,&data,1,IMU.timeout);
	HAL_Delay(50);
	data = 0x00;
	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),ACCEL_CONFIG,1,&data,1,IMU.timeout);
	HAL_Delay(50);
	data = 0x00;
	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),GYRO_CONFIG,1,&data,1,IMU.timeout);
	HAL_Delay(50);
//	data = 0;
//	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
//	HAL_Delay(50);
//	data = 0x08;//0x28 pro lowpower rezim s frekvenci 1.25Hz
//	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_1,1,&data,1,IMU.timeout);
//	HAL_Delay(50);
	data = 0x00;
	HAL_I2C_Mem_Write(IMU.peripheral,(MPU6000_ADDRESS << 1),PWR_MGMT_2,1,&data,1,IMU.timeout);
	HAL_Delay(50);

	MPU6000_readAll();

	return 0;
}

/**
 * @brief Read certain register of sensor
 * @param register address
 * @retval value of register
 */
uint8_t MPU6000_readRegister(uint8_t reg){
	uint8_t reg_data[1];

	HAL_I2C_Mem_Read(IMU.peripheral,(MPU6000_ADDRESS << 1),reg,1,reg_data,1,IMU.timeout);
	HAL_Delay(50);
	return reg_data[0];
}

/**
 * @brief Calibrate the sensor
 * @retval none
 */
void MPU6000_Calibrate(){
	uint8_t data[1] = {GYRO_XOUT_H};
	uint8_t data_out[6];

	HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
	HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);

	IMU.cal_g_x = data_out[0] * 256 + data_out[1];
	IMU.cal_g_y = data_out[2] * 256 + data_out[3];
	IMU.cal_g_z = data_out[4] * 256 + data_out[5];

	data[0] = ACCEL_XOUT_H;
	HAL_I2C_Master_Transmit(IMU.peripheral, (MPU6000_ADDRESS << 1), data, 1, IMU.timeout);
	HAL_I2C_Master_Receive(IMU.peripheral, (MPU6000_ADDRESS << 1), data_out, 6, IMU.timeout);

	IMU.cal_a_x = data_out[0] * 256 + data_out[1];
	IMU.cal_a_y = data_out[2] * 256 + data_out[3];
	IMU.cal_a_z = data_out[4] * 256 + data_out[5];
}

/**
 * @brief Initialize pedometer
 * @retval none
 */
void MPU6000_Pedometer_Init(){
	IMU.pedo_index = 1;
	IMU.pedo_steps = 0;
	IMU.pedo_vector[0] = 0;
	IMU.pedo_flag = 0;
	IMU.pedo_threshold = 2000;
	IMU.pedo_vector_max = 0;
	IMU.pedo_vector_avg = 0;
	IMU.pedo_vector_samples = 0;

	sysclock = HAL_RCC_GetSysClockFreq();
}

/**
 * @brief Pedometer calculations
 * @retval none
 */
void MPU6000_Pedometer(){
	IMU.pedo_a_x[IMU.pedo_index] = IMU.a_x * IMU.accel_LSB;
	IMU.pedo_a_y[IMU.pedo_index] = IMU.a_y * IMU.accel_LSB;
	IMU.pedo_a_z[IMU.pedo_index] = IMU.a_z * IMU.accel_LSB;

	IMU.pedo_work_vector[IMU.pedo_index] = sqrt(((IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x) * (IMU.pedo_a_x[IMU.pedo_index] - IMU.pedo_avg_a_x)) + ((IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y) * (IMU.pedo_a_y[IMU.pedo_index] - IMU.pedo_avg_a_y)) + ((IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z) * (IMU.pedo_a_z[IMU.pedo_index] - IMU.pedo_avg_a_z)));

	IMU.pedo_vector[0] = (IMU.pedo_work_vector[IMU.pedo_index] + IMU.pedo_work_vector[IMU.pedo_index - 1]) / 2 ;

	if(IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 0){
		IMU.pedo_steps = IMU.pedo_steps + 1;
		IMU.pedo_last_step_timestamp = HAL_GetTick();
		IMU.pedo_flag = 1;
	}
	else if (IMU.pedo_vector[0] > IMU.pedo_threshold && IMU.pedo_flag == 1){
		// Don't Count
	}
	if (IMU.pedo_vector[0] < IMU.pedo_threshold && IMU.pedo_flag == 1){
		IMU.pedo_flag = 0;
	}
	if (IMU.pedo_steps < 0) {
		IMU.pedo_steps = 0;
	}
	IMU.pedo_index++;

	if(IMU.pedo_work_vector[IMU.pedo_index] > IMU.pedo_vector_max){
		IMU.pedo_vector_max = IMU.pedo_work_vector[IMU.pedo_index];
	}

	IMU.pedo_vector_avg = IMU.pedo_vector_avg + (IMU.pedo_vector[0] - IMU.pedo_vector_avg) / ((float)IMU.pedo_vector_samples + 1);
	IMU.pedo_vector_samples ++;

	if(IMU.pedo_index >= 100){
		IMU.pedo_index = 1;
		IMU.pedo_vector[0] = 0;

		float sum[3] = {0};
		for(int i = 0; i < 100; i++){
			sum[0] = IMU.pedo_a_x[i] + sum[0];
			sum[1] = IMU.pedo_a_y[i] + sum[1];
			sum[2] = IMU.pedo_a_z[i] + sum[2];
		}
		IMU.pedo_avg_a_x = sum[0] / 100.0;
		IMU.pedo_avg_a_y = sum[1] / 100.0;
		IMU.pedo_avg_a_z = sum[2] / 100.0;
	}
}

/**
 * @brief Function to be put into i2c callback function
 * @retval none
 */
void MPU6000_I2C_CallbackFunc(I2C_HandleTypeDef *hi2c){
	if(hi2c -> Instance == IMU.peripheral->Instance){
		IMU.peripheral->Instance->CR1 &= ~(1 << 1);
		IMU.dataReady = 1;
//		MPU6000_Raw_Data_Convert();
//		madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y,  IMU.a_z);
//		madgwick_get_quaternion(madgwick_handle, &quat_data);

//		IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
//		IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
//		IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);
//
//		IMU.dataReady = 0;
//		MPU6000_readAll();
	}
}

void IMU_Check_State(){
	if(IMU.dataReady){
		MPU6000_Raw_Data_Convert();
		madgwick_update_6dof(madgwick_handle, IMU.g_x * DEG2RAD, IMU.g_y * DEG2RAD, IMU.g_z * DEG2RAD, IMU.a_x, IMU.a_y,  IMU.a_z);
		madgwick_get_quaternion(madgwick_handle, &quat_data);

		IMU.roll = 180.0 / 3.14 * atan2(2 * (quat_data.q0 * quat_data.q1 + quat_data.q2 * quat_data.q3), 1 - 2 * (quat_data.q1 * quat_data.q1 + quat_data.q2 * quat_data.q2));
		IMU.pitch = 180.0 / 3.14 * asin(2 * (quat_data.q0 * quat_data.q2 - quat_data.q3 * quat_data.q1));
		IMU.yaw = 180.0 / 3.14 * atan2f(quat_data.q0 * quat_data.q3 + quat_data.q1 * quat_data.q2, 0.5f - quat_data.q2 * quat_data.q2 - quat_data.q3 * quat_data.q3);

		IMU.dataReady = 0;
		MPU6000_readAll();
	}
}