#include "dw1000_hal.h"

#include "FreeRTOS.h"
#include "task.h"
#include "cmsis_os.h"
#include "stm32f4xx_hal.h"
#include "Trace.h"
#include "deca_regs.h"

#include "spi.h"

SPI_HandleTypeDef *hspi;

uint8_t read3[7];
static void (*spiCallback)(int state, void *data, uint16_t len);
static void *spiCallbackData;
static SemaphoreHandle_t spiSema = NULL;	//semaphore for SPI access

void dw1000Hal_reset(void)
{
  GPIO_InitTypeDef GPIO_InitStruct;
  /*Configure GPIO pin : PD7 */
  GPIO_InitStruct.Pin = GPIO_PIN_7;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Speed = GPIO_SPEED_LOW;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
  HAL_GPIO_WritePin(GPIOD, GPIO_PIN_7, GPIO_PIN_RESET);
  GPIO_InitStruct.Pin = GPIO_PIN_7;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
}

void dw1000Hal_chipSelect(void)
{
	HAL_GPIO_WritePin(DW1000HAL_SS_GPIO, DW1000HAL_SS_PIN, GPIO_PIN_RESET);
}

void dw1000Hal_chipDeselect(void)
{
	HAL_GPIO_WritePin(DW1000HAL_SS_GPIO, DW1000HAL_SS_PIN, GPIO_PIN_SET);
}

int dw1000Hal_readSubRegister(uint8_t regID, uint16_t offset, uint8_t *dest, uint16_t len)
{
	uint8_t dummy[3];
	int ret;
	portENTER_CRITICAL();
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, ( TickType_t ) DW1000HAL_SPI_TIMEOUT) == pdTRUE)) {
		if (len <= 127) {
			dummy[0] = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
			dummy[0] |= 0x40; // subregister offset follows in second byte
			dummy[1] = offset & 0x7f; // set first bit 0 ( no third byte present)
			dw1000Hal_chipSelect();
			ret = HAL_SPI_Transmit(hspi, dummy, 2, DW1000HAL_SPI_TIMEOUT);
		} else {
			dummy[0] = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
			dummy[0] |= 0x40; // subregister offset follows in second byte
			dummy[1] = 0x80 | (offset & 0x7f); // set first bit 1 ( third byte present)
			dummy[2] = offset >> 7;
			dw1000Hal_chipSelect();
			ret = HAL_SPI_Transmit(hspi, dummy, 3, DW1000HAL_SPI_TIMEOUT);
		}
		if (ret == HAL_OK) {
			ret = HAL_SPI_Receive(hspi, dest, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY);
		xSemaphoreGive(spiSema);
	} else {
		ret = HAL_LOCKED;
	}
	portEXIT_CRITICAL();
	return ret;
}

int dw1000Hal_writeSubRegister(uint8_t regID, uint16_t offset, uint8_t *src, uint16_t len)
{
	uint8_t dummy[3];
	int ret;
	portENTER_CRITICAL();
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, ( TickType_t ) DW1000HAL_SPI_TIMEOUT) == pdTRUE)) {
		if (len <= 127) {
			dummy[0] = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
			dummy[0] |= 0xC0; //write access, subregister offset follows in second byte
			dummy[1] = offset & 0x7f; // set first bit 0 ( no third byte present)
			dw1000Hal_chipSelect();
			ret = HAL_SPI_Transmit(hspi, dummy, 2, DW1000HAL_SPI_TIMEOUT);

		} else {
			dummy[0] = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
			dummy[0] |= 0xC0; //write access, subregister offset follows in second byte
			dummy[1] = 0x80 | (offset & 0x7f); // set first bit 1 ( third byte present)
			dummy[2] = offset >> 7;
			dw1000Hal_chipSelect();
			ret = HAL_SPI_Transmit(hspi, dummy, 3, DW1000HAL_SPI_TIMEOUT);
		}
		if (ret == HAL_OK) {
			ret = HAL_SPI_Transmit(hspi, src, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY);
		xSemaphoreGive(spiSema);
	} else {
		ret = HAL_LOCKED;
	}
	portEXIT_CRITICAL();
	return ret;
}

int dw1000Hal_readRegister(uint8_t regID, uint8_t *dest, uint16_t len)
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	int ret;
	portENTER_CRITICAL();
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, 10) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Receive(hspi, dest, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY) {
		}
		xSemaphoreGive(spiSema);
	} else {
		ret = HAL_LOCKED;
		trace_printf("nb");
	}
	portEXIT_CRITICAL();
	return ret;
}

int dw1000Hal_readRegisterFromIsr(uint8_t regID, uint8_t *dest, uint16_t len)
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	int ret;
	if ((spiSema != NULL) && (xSemaphoreTakeFromISR(spiSema, pdFALSE) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Receive(hspi, dest, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY) {
		}
		xSemaphoreGiveFromISR(spiSema,pdFALSE);
	} else {
		ret = HAL_LOCKED;
		trace_printf("ib");
	}
	return ret;
}
int dw1000Hal_writeRegister(uint8_t regID, uint8_t *src, uint16_t len)
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	dummy |= 0x80; 		 //set first byte 1 (write access)
	int ret;
	portENTER_CRITICAL();
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, 10) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Transmit(hspi, src, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY) {
		}
		xSemaphoreGive(spiSema);
	} else {
		ret = HAL_LOCKED;
		trace_printf("na");
	}
	portEXIT_CRITICAL();
	return ret;
}
int dw1000Hal_writeRegisterFromIsr(uint8_t regID, uint8_t *src, uint16_t len)
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	dummy |= 0x80; 		 //set first byte 1 (write access)
	int ret;
	if ((spiSema != NULL) && (xSemaphoreTakeFromISR(spiSema, pdFALSE) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Transmit(hspi, src, len, DW1000HAL_SPI_TIMEOUT);
		}
		dw1000Hal_chipDeselect();
		while (hspi->State != HAL_SPI_STATE_READY) {
		}
		xSemaphoreGiveFromISR(spiSema,pdFALSE);
	} else {
		ret = HAL_LOCKED;
		trace_printf("ia");
	}
	return ret;
}
int dw1000Hal_readDmaRegister(uint8_t regID, uint8_t *dest, uint16_t len, void (*callback)(int state, void *data, uint16_t len))
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	spiCallback = callback;
	spiCallbackData = dest;
	int ret;
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, ( TickType_t ) DW1000HAL_SPI_TIMEOUT) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Receive_DMA(hspi, dest, len);
		} else {
			xSemaphoreGive(spiSema);
			dw1000Hal_chipDeselect();
		}
	} else {
		ret = HAL_LOCKED;
	}
	return ret;
}

int dw1000Hal_writeDmaRegister(uint8_t regID, uint8_t *src, uint16_t len, void (*callback)(int state, void *data, uint16_t len))
{
	uint8_t dummy;
	dummy = regID & 0x3f; //set the first two bit 0 (read access, no subregister)
	dummy |= 0x80; 		 //set first byte 1 (write access)
	spiCallback = callback;
	spiCallbackData = src;
	int ret;
	if ((spiSema != NULL) && (xSemaphoreTake(spiSema, ( TickType_t ) DW1000HAL_SPI_TIMEOUT) == pdTRUE)) {
		dw1000Hal_chipSelect();
		ret = HAL_SPI_Transmit(hspi, &dummy, 1, DW1000HAL_SPI_TIMEOUT);
		if (ret == HAL_OK) {
			ret = HAL_SPI_Transmit_DMA(hspi, src, len);
		} else {
			xSemaphoreGive(spiSema);
			dw1000Hal_chipDeselect();
		}
	} else {
		ret = HAL_LOCKED;
	}
	return ret;
}


void vTaskDW1000HAL(void *pvParameters)
{
	hspi = &hspi1;
	spiSema = xSemaphoreCreateMutex(); //creating semaphore for SPI access

	TickType_t xNextWakeTime;
	xNextWakeTime = xTaskGetTickCount();

	//Configure the dw1000
	dw1000Hal_reset();
	uint32_t sys_mask = 0;
	dw1000Hal_readRegister(SYS_MASK_ID,&sys_mask,SYS_MASK_LEN);
	sys_mask |= ( SYS_MASK_MTXFRS | SYS_MASK_MRXDFR | SYS_MASK_MCPLLLL); /* Mask transmit frame sent event	*//* Mask receiver data frame ready event	*/
	//trace_printf("sys_mask %x\n",sys_mask);
	dw1000Hal_writeRegister(SYS_MASK_ID,&sys_mask,SYS_MASK_LEN);
	//set TX LED
	uint32_t gpio = 0x00001000UL;
	dw1000Hal_writeSubRegister(GPIO_CTRL_ID,GPIO_MODE_OFFSET,(uint8_t*)&gpio,GPIO_MODE_LEN);
	uint32_t led = 0;
	dw1000Hal_readSubRegister(PMSC_ID,PMSC_LEDC_OFFSET,(uint8_t*)&led,PMSC_LEDC_LEN);
	led |= PMSC_LEDC_BLNKEN;
	dw1000Hal_writeSubRegister(PMSC_ID,PMSC_LEDC_OFFSET,(uint8_t*)&led,PMSC_LEDC_LEN);
	//uint8_t event_clear[SYS_STATUS_LEN];
	//memset(event_clear, 0xff, SYS_STATUS_LEN);
	//dw1000Hal_writeRegister(SYS_STATUS_ID, &event_clear, SYS_STATUS_LEN);


	uint8_t led_values = 0;

	while(1){
		vTaskDelayUntil(&xNextWakeTime, 1000/portTICK_PERIOD_MS);
	}
	vTaskDelete( NULL );
}

/***********************************************************
 * Callback functions
 */
void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
{
	dw1000Hal_chipDeselect();
	xSemaphoreGiveFromISR(spiSema, pdFALSE);
	if (spiCallback != NULL){
		spiCallback(HAL_ERROR, 0, hspi->TxXferSize);
	}

}

void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{
	dw1000Hal_chipDeselect();
	xSemaphoreGiveFromISR(spiSema, pdFALSE);
	if (spiCallback != NULL){
		spiCallback(HAL_ERROR, 0, hspi->TxXferSize);
	}
}

void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
{
	dw1000Hal_chipDeselect();
	xSemaphoreGiveFromISR(spiSema, pdFALSE);
	if (spiCallback != NULL){
		spiCallback(HAL_ERROR, 0, hspi->TxXferSize);
	}
}

void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi)
{
	dw1000Hal_chipDeselect();
	xSemaphoreGiveFromISR(spiSema, pdFALSE);
	if (spiCallback != NULL){
		spiCallback(HAL_ERROR, 0, hspi->TxXferSize);
	}
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if (GPIO_PIN_8 == GPIO_Pin){
		uint64_t event = 0;
		uint64_t event_clear = 0;
		dw1000Hal_readRegisterFromIsr(SYS_STATUS_ID, (uint8_t*)&event, SYS_STATUS_LEN);
		if (event & SYS_STATUS_TXFRS){ // Frame sent
			event_clear |= SYS_STATUS_TXFRS ; //clear interrupt

		}

		if (event & SYS_STATUS_RXDFR){ // Frame sent
			event_clear |= SYS_STATUS_RXDFR; //clear interrupt
			uint32_t sys_ctrl = 0;
			dw1000Hal_readRegisterFromIsr(SYS_CTRL_ID, (uint8_t*)&sys_ctrl, SYS_CTRL_LEN); // switch to rx mode
			sys_ctrl |= SYS_CTRL_RXENAB;
			dw1000Hal_writeRegisterFromIsr(SYS_CTRL_ID, (uint8_t*)&sys_ctrl, SYS_CTRL_LEN);

		}
		if (event & SYS_STATUS_CLKPLL_LL){ // Frame sent
			event_clear |= SYS_STATUS_CLKPLL_LL;
			if (event & SYS_STATUS_CPLOCK){
				event_clear |= SYS_STATUS_CPLOCK;
				//trace_printf("fuu\n");
			}
		}
		dw1000Hal_writeRegisterFromIsr(SYS_STATUS_ID, (uint8_t*)&event_clear, SYS_STATUS_LEN);
	}
}