#include "main.h"
#include "sensors.h"
//#include "stm32wbxx_hal_i2c.h"
#include <stdio.h>

#include "cmsis_os2.h"
#include "task.h"

#define HAL_I2C_TIMEOUT 100
//extern osThreadId_t defaultTaskHandle;
//TaskHandle_t xStartDefaultTask = NULL;
//extern osThreadId_t defaultTaskHandle
osThreadId_t xIMU_Task = NULL;
osThreadId_t xMagn_Task = NULL;
//osThreadId_t xData_Task = NULL;


void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
	 if (hspi->Instance == SPI1) {
		 HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);

		 // notify the task that the data is ready
		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
		 vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	 }
 }

void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *I2C_address) {
//	printf("HAL_I2C_MemRxCpltCallback");
	 if (I2C_address->Instance == I2C1) {
		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	 }

 }
void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
//	printf("HAL_I2C_MemTxCpltCallback");
	 if (I2C_address->Instance == I2C1) {
		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	 }
 }

void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
    if (hi2c->Instance == I2C1) {
        BaseType_t xHigherPriorityTaskWoken = pdFALSE;
        printf("ErrorCallback: %ld", HAL_I2C_GetError(hi2c));
        vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

// ICM 45686
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
	HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
	return status;
}

HAL_StatusTypeDef read_icm_dma (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[]) {
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
	HAL_StatusTypeDef status = HAL_SPI_TransmitReceive_DMA(imu->hspi, tx, rx, sizeof(rx));
	return status;
}

HAL_StatusTypeDef write_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[]) {
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
	HAL_StatusTypeDef status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_SET);
	return status;
}

void init_icm(ICM45686_HandleTypeDef *imu) {
	// TODO: Add settings as enum to choose from
	// FIFO
	// Interrupt
	// APEX???
	// EDMP???

	uint8_t tx[2], rx[2];
	HAL_StatusTypeDef status;

	printf("Initializing ICM45686\n");
	// WHO AM
	tx[0] = ICM45686_WHO_AM_I | 0x80;
//	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = read_icm(imu, tx, rx);
	if (status != HAL_OK) {
	  printf("Error in reading WHO AM I:%d\n", status);
	} else {
	  printf("WHO AM I (0xE9): 0x%02X \n", rx[1]);
	}

	// acc
	tx[0] = ICM45686_ACCEL_CONFIG0;
	tx[1] = imu->acc_fs | imu->odr;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = write_icm(imu, tx);
	if (status != HAL_OK) {
	  printf("Error in setting ACCEL_CONFIG0:%d\n", status);
	} else {
	  printf("ACCEL_CONFIG0 wrote: 0x%02X \n", tx[1]);
	}

	tx[0] = ICM45686_ACCEL_CONFIG0 | 0x80;
	tx[1] = 0x00;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = read_icm(imu, tx, rx);
	if (status != HAL_OK) {
	  printf("Error reading ACCEL_CONFIG0:%d\n", status);
	} else {
	  printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
	}

	// gyro
	tx[0] = ICM45686_GYRO_CONFIG0;
	tx[1] = imu->gyro_fs | imu->odr;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = write_icm(imu, tx);
	if (status != HAL_OK) {
	  printf("Error in setting GYRO_CONFIG0:%d\n", status);
	} else {
	  printf("GYRO_CONFIG0 wrote: 0x%02X \n", tx[1]);
	}

	tx[0] = ICM45686_GYRO_CONFIG0 | 0x80;
	tx[1] = 0x00;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = read_icm(imu, tx, rx);
	if (status != HAL_OK) {
	  printf("Error reading GYRO_CONFIG0:%d\n", status);
	} else {
	  printf("GYRO_CONFIG0 read: 0x%02X \n", rx[1]);
	}

	// power settings
	tx[0] = ICM45686_PWR_MGMT0;
	tx[1] =  0x0F; //0x10 | 0x03;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_Transmit(imu->hspi, tx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = write_icm(imu, tx);
	if (status != HAL_OK) {
	  printf("Error in setting PWR_MGMT0:%d\n", status);
	} else {
	  printf("PWR_MGMT0 wrote: 0x%02X \n", tx[1]);
	}

	tx[0] = ICM45686_PWR_MGMT0 | 0x80;
	tx[1] = 0x00;
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_RESET);
//	status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, sizeof(tx), 50);
//	HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
	status = read_icm(imu, tx, rx);
	if (status != HAL_OK) {
	  printf("Error reading PWR_MGMT0:%d\n", status);
	} else {
	  printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
	}

	osDelay(250);
}

void calibrate_icm(ICM45686_HandleTypeDef *imu, int samples) {
	HAL_StatusTypeDef status;
	uint8_t raw_data[15];
	int16_t sensor_data[7] = {0};
	uint8_t tx[15] = {0x00};
	tx[0] = 0x00 | 0x80;

	for (int i = 1; i <= samples; i++) {
		  status = read_icm_dma(imu, tx, raw_data);
		  if (status != HAL_OK) {
		    printf("Calibration read ICM failed: %d\n", status);
		  }
		  sensor_data[0] += (int16_t)(raw_data[1] << 8 | raw_data[2]);    // a_x
		  sensor_data[1] += (int16_t)(raw_data[3] << 8 | raw_data[4]);    // a_y
		  sensor_data[2] += (int16_t)(raw_data[5] << 8 | raw_data[6]);    // a_z
		  sensor_data[3] += (int16_t)(raw_data[7] << 8 | raw_data[8]);    // g_x
		  sensor_data[4] += (int16_t)(raw_data[9] << 8 | raw_data[10]);   // g_y
		  sensor_data[5] += (int16_t)(raw_data[11] << 8 | raw_data[12]);  // g_z
		  sensor_data[6] += (int16_t)(raw_data[13] << 8 | raw_data[14]);  // t
	}

	  imu->imu_bias[0] = (float)sensor_data[0] / (float)samples;
	  imu->imu_bias[1] = (float)sensor_data[1] / (float)samples;
	  imu->imu_bias[2] = (float)sensor_data[2] / (float)samples;
	  imu->imu_bias[3] = (float)sensor_data[3] / (float)samples;
	  imu->imu_bias[4] = (float)sensor_data[4] / (float)samples;
	  imu->imu_bias[5] = (float)sensor_data[5] / (float)samples;
	  imu->imu_bias[6] = (float)sensor_data[6] / (float)samples;

	  // remrove gravity
	  imu->imu_bias[2] -= 1.0f;

	  imu->calibated = true;

	  printf("Calibration finished of ICM45686\n");
	  printf("a_x: %.2f a_y: %.2f a_z: %.2f g_x: %.2f g_y: %.2f g_z: %.2f t: %.2f\n",
			  	  	  	  	  	  	  	  	  	  	  	imu->imu_bias[0], imu->imu_bias[1], imu->imu_bias[2],
	  	  	  	  	  	  	  	  	  	  	  	  	  	imu->imu_bias[3], imu->imu_bias[4], imu->imu_bias[5],
														imu->imu_bias[6]);

}

// BMM350
void bmm_init_DWT(void){
	// Enable trace and debug block (TRCENA)
	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;

	// Reset cycle counter
	DWT->CYCCNT = 0;

	// Enable the cycle counter
	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
}

BMM350_INTF_RET_TYPE bmm350_i2c_read(uint8_t reg_addr, uint8_t *reg_data, uint32_t length, void *intf_ptr){
    // Extract I2C device address from intf_ptr
    uint8_t device_addr = *(uint8_t*)intf_ptr;

//     STM32 HAL: Write register address, then read data
//     Device address is left-shifted by 1 for STM32 HAL
//    if (HAL_I2C_Mem_Read(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
    if (HAL_I2C_Mem_Read_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
    	if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
    		return BMM350_INTF_RET_SUCCESS; // 0
    	}
    }
    return -1; // Communication failure

}

BMM350_INTF_RET_TYPE bmm350_i2c_write(uint8_t reg_addr, const uint8_t *reg_data, uint32_t length, void *intf_ptr){
    uint8_t device_addr = *(uint8_t*)intf_ptr;
//    uint8_t rslt = HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length);
//	printf("rslt write: %d\n", rslt);
//    // STM32 HAL: Write register address + data
    if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
//    if (HAL_I2C_Mem_Write(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length, HAL_I2C_TIMEOUT ) == HAL_OK) {
//	if (rslt == HAL_OK) {
		if (ulTaskNotifyTake(pdTRUE, pdMS_TO_TICKS(10)) == pdTRUE) {
			return BMM350_INTF_RET_SUCCESS; // 0
		}
	}
    return -1;
}

void bmm350_delay(uint32_t period_us, void *intf_ptr){
    (void)intf_ptr; // Unused

    // For STM32, use DWT cycle counter for accurate microsecond delays
    uint32_t start = DWT->CYCCNT;
    uint32_t cycles = period_us * (SystemCoreClock / 1000000);

    while ((DWT->CYCCNT - start) < cycles);
}

// Initialization function
int8_t init_bmm(struct bmm350_dev *dev) {
    int8_t rslt;

    // Static variable to hold I2C device address
    // Must be static or global because dev.intf_ptr will point to it
    static uint8_t dev_addr = BMM350_I2C_ADSEL_SET_LOW;
    // Assign function pointers
    dev->read = bmm350_i2c_read;
    dev->write = bmm350_i2c_write;
    dev->delay_us = bmm350_delay;
    dev->intf_ptr = &dev_addr;

    printf("Starting up and configuring BMM350\n");

    // Initialize BMM350 driver
    rslt = bmm350_init(dev);
    if (rslt != BMM350_OK)
    {
        // Handle error: chip ID mismatch, communication failure, etc.
        return rslt;
    }
    printf("Initialization: %d\n", rslt);
    printf("CHIP_ID (should be 0x33) - Read Register: 0x00  BMM350 Chip ID: 0x%X\n", dev->chip_id);

    // Set powermode, odr and avg
    rslt = bmm350_set_powermode(BMM350_NORMAL_MODE, dev);
    printf("Powermode (%d): %d\n", BMM350_NORMAL_MODE, rslt);
    rslt = bmm350_set_odr_performance(BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, dev);
    printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_200HZ, BMM350_AVERAGING_2, rslt);

    return BMM350_OK;
}