#include "main.h"
#include <stdbool.h>
#include "bmm350.h"
#include "bmm350_defs.h"
#include "bmm350_oor.h"
#include "FreeRTOS.h"
#include "cmsis_os2.h"


// I2C Handle
extern I2C_HandleTypeDef hi2c1;
extern osThreadId_t defaultTaskHandle;
extern osThreadId_t xIMU_Task;
extern osThreadId_t xMagn_Task;
//extern osThreadId_t xData_Task;

// ICM45686
typedef struct {
	SPI_HandleTypeDef *hspi;
	GPIO_TypeDef *GPIO_Port;
	uint16_t GPIO_Pin;

	uint8_t acc_fs;
	uint8_t gyro_fs;
	uint8_t odr;
	float acc_ssf;
	float gyro_ssf;

	bool calibated;
//	float accel_bias[3];
//	float gyro_bias[3];
	float imu_bias[7];
} ICM45686_HandleTypeDef;

typedef struct {
	float processed_imu_data[7];
} ICM45686_Data;

// dataframe for sending quaternions over usb
typedef struct __attribute__((packed)) {
	uint8_t StartByte;
	uint8_t SensorAddress;
	float qw;
	float qx;
	float qy;
	float qz;
	uint8_t EndByte;
} QuaternionData;

// enum to hold the packet dataframe data
typedef enum {
	PACKET_START_BYTE = 0xDE,
	PACKET_END_BYTE = 0xAD
} Packet_Bytes;


typedef enum {
	ICM45686_WHO_AM_I = 0x72,  // 0x72 = 01110010, dann den wert NIX(???)) einfach nur lesen
	ICM45686_ACCEL_CONFIG0 = 0x1B,
	ICM45686_GYRO_CONFIG0 = 0x1C,
	ICM45686_FIFO_CONFIG0 = 0x1D,
	ICM45686_PWR_MGMT0 = 0x10,
	ICM45686_ODR_DECIMATE_CONFIG = 0x40,
	ICM45686_ACCEL_DATA = 0x00,
	ICM45686_GYRO_DATA = 0x06,
	ICM45686_FIFO_COUNT = 0x12,
	ICM45686_FIFO_DATA = 0x14
} ICM45686_registers;

typedef enum {
	ICM45686_ACC_FS_32G = 0x00,
	ICM45686_ACC_FS_16G = 0x10,
	ICM45686_ACC_FS_8G = 0x20,
	ICM45686_ACC_FS_4G = 0x30,
	ICM45686_ACC_FS_2G = 0x40,
} ICM45686_ACC_CONF;

//typedef enum {
//	ICM45686_ACC_SSF_32G = 16384.0,
//	ICM45686_ACC_SSF_16G = 8192.0,
//	ICM45686_ACC_SSF_8G = 4096.0,
//	ICM45686_ACC_SSF_4G = 2048.0,
//	ICM45686_ACC_SSF_2G = 1024.0,
//} ICM45686_ACC_FACTORS;

static const float ICM45686_ACC_SSF [5] = {
		1024.0f, 2048.0f, 4096.0f, 8192.0f, 16384.0f
};

typedef enum {
	ICM45686_GYRO_FS_4000DPS = 0x00,
	ICM45686_GYRO_FS_2000DPS = 0x10,
	ICM45686_GYRO_FS_1000DPS = 0x20,
	ICM45686_GYRO_FS_500DPS = 0x30,
	ICM45686_GYRO_FS_250DPS = 0x40,
	ICM45686_GYRO_FS_125DPS = 0x50,
	ICM45686_GYRO_FS_62_5DPS = 0x60,
	ICM45686_GYRO_FS_31_25DPS = 0x70,
	ICM45686_GYRO_FS_15_625DPS = 0x80,
} ICM45686_GYRO_CONF;

//typedef enum {
//	ICM45686_GYRO_SSF_4000DPS = 2097.2,
//	ICM45686_GYRO_SSF_2000DPS = 1048.6,
//	ICM45686_GYRO_SSF_1000DPS = 524.3,
//	ICM45686_GYRO_SSF_500DPS = 262.0,
//	ICM45686_GYRO_SSF_250DPS = 131.0,
//	ICM45686_GYRO_SSF_125DPS = 65.8,
//	ICM45686_GYRO_SSF_62_5DPS = 32.8,
//	ICM45686_GYRO_SSF_31_25DPS = 16.4,
//	ICM45686_GYRO_SSF_15_626DPS = 8.2,
//} ICM45686_GYRO_FACTORS;

static const float ICM45686_GYRO_SSF [9] = {
		8.2f, 16.4f, 32.8f, 65.5f, 131.0f, 262.0f, 524.3f, 1048.6f, 2097.2f
};

typedef enum {
	ICM45686_ODR_6_4kHz_LN = 0x03 ,
	ICM45686_ODR_3_2kHz_LN = 0x04,
	ICM45686_ODR_1_6kHz_LN = 0x05,
	ICM45686_ODR_800Hz_LN = 0x06,
	ICM45686_ODR_400Hz_LNLP = 0x07,
	ICM45686_ODR_200Hz_LNLP = 0x08,
	ICM45686_ODR_100Hz_LNLP = 0x09,
	ICM45686_ODR_50Hz_LNLP = 0x0A,
	ICM45686_ODR_25Hz_LNLP = 0x0B,
	ICM45686_ODR_12_5Hz_LNLP = 0x0C,
	ICM45686_ODR_6_26Hz_LP = 0x0D,
	ICM45686_ODR_3_125Hz_LP = 0x0E,
	ICM45686_ODR_1_5625Hz_LP = 0x0F
} ICM45686_ODR;




void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi);
void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address);
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address);
void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c);

// ICM45686
void init_icm(ICM45686_HandleTypeDef *imu);
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]);
HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]);
void calibrate_icm (ICM45686_HandleTypeDef * imu, int samples);

// BMM350 - Declarations of read/write/delay functions for the BMM350 API
void bmm_init_DWT(void);
BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr);
BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr);
void bmm350_delay(uint32_t period_us, void *intf_ptr);
int8_t init_bmm(struct bmm350_dev *dev);

// structs that hold the sensor data
typedef struct {
	uint8_t raw_imu[14];
	uint8_t raw_magn [9];
} raw_sensor_data;