#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\n", HAL_I2C_GetError(hi2c));
        vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
        portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
    }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
	// was macht das?
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;

	if (GPIO_Pin == GPIO_IMU_INT_Pin) {
		 vTaskNotifyGiveFromISR(xIMU_Task, &xHigherPriorityTaskWoken);
//		 printf("Triggered ICM45686 EXTI\n");
	}

	if (GPIO_Pin == GPIO_MAGN_INT_Pin) {
		 vTaskNotifyGiveFromISR(xMagn_Task, &xHigherPriorityTaskWoken);
//		 printf("Triggered BMM350 EXTI\n");
	}
	// was macht das?
	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) {
	// TODO: Add settings as enum to choose from
	// FIFO
	// Interrupt
	/* Interrupt functionality is configured via the Interrupt Configuration register. Items that are configurable include the
		interrupt pins configuration, the interrupt latching and clearing method, and triggers for the interrupt. Items that
		can trigger an interrupt are (1) Clock generator locked to new reference oscillator (used when switching clock
		sources); (2) new data is available to be read (from the FIFO and Data registers); (3) accelerometer event
		interrupts; (4) FIFO watermark; (5) FIFO overflow. The interrupt status can be read from the Interrupt Status
		register.
		- Configure interrupt pin on icm
		- configure interrupt pin on bmm
		- -> push/pull or open drain, latch, polarity
		- everything stays as is only that a interrupt callback gets added, that when called wakes up the
		corresponding task for the sensor
		*/
	// 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, 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
	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, 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;
//	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, 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
	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, 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;
//	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, 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;
//	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, 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;
//	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, sizeof(tx));
	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, sizeof(tx));
		  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]);

}

void enable_interrupt_icm(ICM45686_HandleTypeDef *imu) {
	uint8_t tx[2], rx[2] = {0x00, 0x00};
	tx[0] = ICM45686_INT1_CONFIG0;
	tx[1] = 0x04;
	printf("Enabling IMU INT pin\n");

	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 INT configuration0 failed: %d\n", status);
	} else {
		printf("INT1_CONFIG0 read: 0x%02X\n", rx[1]);
	}
}

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

	tx[0] = ICM45686_INT1_CONFIG2;
	tx[1] = 0x01;
	printf("Configuring IMU INT\n");
	HAL_StatusTypeDef 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 INT configuration2 failed: %d\n", status);
	} else {
		printf("INT1_CONFIG2 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(&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);

//    // configure interrupt
//    rslt = bmm350_enable_interrupt(BMM350_ENABLE_INTERRUPT, dev);
//    printf("Interrupt enable: %d", rslt);
//    rslt = bmm350_configure_interrupt(BMM350_LATCHED, BMM350_ACTIVE_HIGH, BMM350_INTR_OPEN_DRAIN, BMM350_MAP_TO_PIN, dev);
//    printf("Configure interrupt: %d", 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;
}