#include "main.h"
#include "sensors.h"
#include "trinity_helperlib.h"
#include <stdio.h>

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


#define HAL_I2C_TIMEOUT 100
volatile osThreadId_t xIMU_Task = NULL;
volatile osThreadId_t xMagn_Task = NULL;


void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
	 if (hspi->Instance == SPI1 && xIMU_Task != NULL) {
		 HAL_GPIO_WritePin(GPIOA, GPIO_SPI1_IMU_CS_Pin, GPIO_PIN_SET);
		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
		 vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	 }
 }

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

void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *I2C_address) {
	 if (I2C_address->Instance == I2C1 && xMagn_Task != NULL) {
		 BaseType_t xHigherPriorityTaskWoken = pdFALSE;
		 vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_DMA, &xHigherPriorityTaskWoken);
		 portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
	 }
 }

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

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;
	if (GPIO_Pin == GPIO_IMU_INT_Pin && xIMU_Task != NULL) {
		vTaskNotifyGiveIndexedFromISR(xIMU_Task, ICM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
	}
	if (GPIO_Pin == GPIO_MAGN_INT_Pin && xMagn_Task != NULL) {
		 vTaskNotifyGiveIndexedFromISR(xMagn_Task, BMM_NOTIFY_INT_DRDY, &xHigherPriorityTaskWoken);
	}
	portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

// ICM 45686
HAL_StatusTypeDef read_icm (ICM45686_HandleTypeDef *imu, uint8_t tx[], uint8_t rx[], uint16_t len_tx) {
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
	HAL_StatusTypeDef status = HAL_SPI_TransmitReceive(imu->hspi, tx, rx, len_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[], uint16_t len_tx) {
	HAL_GPIO_WritePin(imu->GPIO_Port, imu->GPIO_Pin, GPIO_PIN_RESET);
	HAL_StatusTypeDef status = HAL_SPI_TransmitReceive_DMA(imu->hspi, tx, rx, len_tx);
	return status;
}

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

void init_icm(ICM45686_HandleTypeDef *imu) {
	uint8_t tx[2], rx[2];
	HAL_StatusTypeDef status;
//	// WHO AM
	tx[0] = ICM45686_WHO_AM_I | 0x80;
	status = read_icm(imu, tx, rx, sizeof(tx));
	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 config
	tx[0] = ICM45686_ACCEL_CONFIG0;
	tx[1] = imu->acc_fs | imu->odr;
	status = write_icm(imu, tx, sizeof(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;
	status = read_icm(imu, tx, rx, sizeof(tx));
	if (status != HAL_OK) {
	  printf("Error reading ACCEL_CONFIG0:%d\n", status);
	} else {
	  printf("ACCEL_CONFIG0 read: 0x%02X \n", rx[1]);
	}

	// gyro config
	tx[0] = ICM45686_GYRO_CONFIG0;
	tx[1] = imu->gyro_fs | imu->odr;
	status = write_icm(imu, tx, sizeof(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;
	status = read_icm(imu, tx, rx, sizeof(tx));
	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;
	status = write_icm(imu, tx, sizeof(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;
	status = read_icm(imu, tx, rx, sizeof(tx));
	if (status != HAL_OK) {
	  printf("Error reading PWR_MGMT0:%d\n", status);
	} else {
	  printf("PWR_MGMT0 read: 0x%02X \n", rx[1]);
	}
}

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

	for (int i = 1; i <= samples; i++) {
		ulTaskNotifyTakeIndexed(ICM_NOTIFY_INT_DRDY, pdTRUE, portMAX_DELAY);
		status = read_icm_dma(imu, tx, raw_data, sizeof(tx));
		if (status != HAL_OK) {
		printf("Calibration read ICM failed: %d\n", status);
		}
		if (ulTaskNotifyTakeIndexed(ICM_NOTIFY_DMA, pdTRUE, pdMS_TO_TICKS(10)) > 0) {
		  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
		}
	}
	// The acc biases is set to 0 or 1 for gravity to avoid messy values without
	// proper multiposition calibration done once per board/sensor
	processed_data[0] = 0.0f; //(float)sensor_data[0] / ((float)samples * imu->acc_ssf);
	processed_data[1] = 0.0f; //(float)sensor_data[1] / ((float)samples * imu->acc_ssf);
	processed_data[2] = 1.0f; //(float)sensor_data[2] / ((float)samples * imu->acc_ssf);
	processed_data[3] = (float)sensor_data[3] / ((float)samples * imu->gyro_ssf);
	processed_data[4] = (float)sensor_data[4] / ((float)samples * imu->gyro_ssf);
	processed_data[5] = (float)sensor_data[5] / ((float)samples * imu->gyro_ssf);
	processed_data[6] = (float)sensor_data[6] / (float)samples;



	imu->imu_bias[0] = processed_data[0];
	imu->imu_bias[1] = processed_data[1];
	imu->imu_bias[2] = processed_data[2];// - 1.0f;
	imu->imu_bias[3] = processed_data[3] * DEG2RAD;
	imu->imu_bias[4] = processed_data[4] * DEG2RAD;
	imu->imu_bias[5] = processed_data[5] * DEG2RAD;
	imu->imu_bias[6] = processed_data[6] / 128.0f + 25.0f;

	imu->calibated = true;

	printf("ICM calibration bias: 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]);
}

void configure_and_enable_int_icm (ICM45686_HandleTypeDef *imu) {
	uint8_t tx[2], rx[2] = {0x00, 0x00};

	// turn off int
	tx[0] = ICM45686_INT1_CONFIG0;
	tx[1] = 0x00;
	HAL_StatusTypeDef status = write_icm(imu, tx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Enabling of IMU INT failed: %d\n", status);
	}

	tx[0] = ICM45686_INT1_CONFIG0 | 0x80;
	tx[1] = 0x00;
	status = read_icm(imu, tx, rx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
	} else {
		printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
	}

	// configure int1 to push-pull, pulse, active high
	tx[0] = ICM45686_INT1_CONFIG2;
	tx[1] = 0x01;
	status = write_icm(imu, tx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Configuration of IMU INT failed: %d\n", status);
	}

	tx[0] = ICM45686_INT1_CONFIG2 | 0x80;
	tx[1] = 0x00;
	status = read_icm(imu, tx, rx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Reading IMU INT1_CONFIG2 failed: %d\n", status);
	} else {
		printf("INT1_CONFIG2 read: 0x%02X\n", rx[1]);
	}

	// enable int
	tx[0] = ICM45686_INT1_CONFIG0;
	tx[1] = 0x04;
	status = write_icm(imu, tx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Enabling of IMU INT failed: %d\n", status);
	}

	tx[0] = ICM45686_INT1_CONFIG0 | 0x80;
	tx[1] = 0x00;
	status = read_icm(imu, tx, rx, sizeof(tx));
	if (status != HAL_OK) {
		printf("Reading IMU INT1_CONFIG0 failed: %d\n", status);
	} else {
		printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
	}
}

// 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_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, reg_data, length) == HAL_OK) {
    	if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, 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;
    if (HAL_I2C_Mem_Write_DMA(&hi2c1, device_addr << 1, reg_addr, I2C_MEMADD_SIZE_8BIT, (uint8_t*)reg_data, length) == HAL_OK) {
    	if (ulTaskNotifyTakeIndexed(BMM_NOTIFY_DMA, 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;

    // 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_400HZ, BMM350_NO_AVERAGING, dev);
    printf("ODR(%d) and AVG(%d): %d\n", BMM350_DATA_RATE_400HZ, BMM350_NO_AVERAGING, rslt);

    return BMM350_OK;
}

int8_t bmm_configure_interrupt (struct bmm350_dev *dev) {
	int8_t rslt;
	rslt = bmm350_enable_interrupt(BMM350_ENABLE_INTERRUPT, dev);
    printf("Interrupt enable: %d \n", rslt);
    rslt = bmm350_configure_interrupt(BMM350_PULSED, BMM350_ACTIVE_HIGH, BMM350_INTR_PUSH_PULL, BMM350_MAP_TO_PIN, dev);
    printf("Configure interrupt: %d \n", rslt);

    return BMM350_OK;
}