#include "nrf24l01.h"

#define SPI_CHECK_ENABLED_RESP(SPIx, val)   if (!((SPIx)->CR1 & SPI_CR1_SPE)) {return (val);}
#define SPI_WAIT_TX(SPIx)                   while ((SPIx->SR & SPI_FLAG_TXE) == 0 || (SPIx->SR & SPI_FLAG_BSY))
#define SPI_WAIT_RX(SPIx)                   while ((SPIx->SR & SPI_FLAG_RXNE) == 0 || (SPIx->SR & SPI_FLAG_BSY))
#define SPI_CHECK_ENABLED(SPIx)             if (!((SPIx)->CR1 & SPI_CR1_SPE)) {return;}
#define SPI_CHECK_ENABLED_RESP(SPIx, val)   if (!((SPIx)->CR1 & SPI_CR1_SPE)) {return (val);}


static __INLINE uint8_t SPI_Send(SPI_TypeDef* SPIx, uint8_t data) {
    /* Check if SPI is enabled */
    SPI_CHECK_ENABLED_RESP(SPIx, 0);

    /* Wait for previous transmissions to complete if DMA TX enabled for SPI */
    SPI_WAIT_TX(SPIx);

    /* Fill output buffer with data */
    *(__IO uint8_t *)&SPIx->DR = data;

    /* Wait for transmission to complete */
    SPI_WAIT_RX(SPIx);

    /* Return data from buffer */
    return SPIx->DR;
}

void SPI_ReadMulti(SPI_TypeDef* SPIx, uint8_t* dataIn, uint8_t dummy, uint32_t count) {
	/* Check if SPI is enabled */
	SPI_CHECK_ENABLED(SPIx);

	while (count--) {
		/* Wait busy */
		SPI_WAIT_TX(SPIx);

		/* Fill output buffer with data */
		*(__IO uint8_t *)&SPIx->DR = dummy;

		/* Wait for SPI to end everything */
		SPI_WAIT_RX(SPIx);

		/* Save data to buffer */
		*dataIn++ = *(__IO uint8_t *)&SPIx->DR;
	}
}

void SPI_WriteMulti(SPI_TypeDef* SPIx, uint8_t* dataOut, uint32_t count) {
	/* Check if SPI is enabled */
	SPI_CHECK_ENABLED(SPIx);

	while (count--) {
		/* Wait busy */
		SPI_WAIT_TX(SPIx);

		/* Fill output buffer with data */
		*(__IO uint8_t *)&SPIx->DR = *dataOut++;

		/* Wait for SPI to end everything */
		SPI_WAIT_RX(SPIx);

		/* Read data register */
		(void)*(__IO uint16_t *)&SPIx->DR;
	}
}


void SPI_SendMulti(SPI_TypeDef* SPIx, uint8_t* dataOut, uint8_t* dataIn, uint32_t count) {
	/* Check if SPI is enabled */
	SPI_CHECK_ENABLED(SPIx);

	while (count--) {
		/* Wait busy */
		SPI_WAIT_TX(SPIx);

		/* Fill output buffer with data */
		*(__IO uint8_t *)&SPIx->DR = *dataOut++;

		/* Wait for SPI to end everything */
		SPI_WAIT_RX(SPIx);

		/* Read data register */
		*dataIn++ = *(__IO uint8_t *)&SPIx->DR;
	}
}
//--------------------------------------------------------------------------------

Nrf24L01::Nrf24L01(uint8_t channel, SPI_HandleTypeDef *spi, GPIO_TypeDef *port_cs, uint16_t pin_cs, GPIO_TypeDef *port_ce, uint16_t pin_ce){
	_spi = spi;
	_cs_port = port_cs;
	_cs_pin = pin_cs;
	_ce_port = port_ce;
	_ce_pin = pin_ce;
	_channel = channel;
	_payload_size = 32;

	init();
}



void Nrf24L01::TM_NRF24L01_WriteBit(uint8_t reg, uint8_t bit, uint8_t value) {
	uint8_t tmp;
	/* Read register */
	tmp = TM_NRF24L01_ReadRegister(reg);
	/* Make operation */
	if (value) {
		tmp |= 1 << bit;
	} else {
		tmp &= ~(1 << bit);
	}
	/* Write back */
	TM_NRF24L01_WriteRegister(reg, tmp);
}

uint8_t Nrf24L01::TM_NRF24L01_ReadBit(uint8_t reg, uint8_t bit) {
	uint8_t tmp;
	tmp = TM_NRF24L01_ReadRegister(reg);
	if (!NRF24L01_CHECK_BIT(tmp, bit)) {
		return 0;
	}
	return 1;
}

uint8_t Nrf24L01::TM_NRF24L01_ReadRegister(uint8_t reg) {
	uint8_t value;
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	SPI_Send(_spi->Instance, NRF24L01_READ_REGISTER_MASK(reg));
	value = SPI_Send(_spi->Instance, NRF24L01_NOP_MASK);
	//NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);

	return value;
}

void Nrf24L01::TM_NRF24L01_ReadRegisterMulti(uint8_t reg, uint8_t* data, uint8_t count) {
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	SPI_Send(_spi->Instance, NRF24L01_READ_REGISTER_MASK(reg));
	SPI_ReadMulti(_spi->Instance, data, NRF24L01_NOP_MASK, count);
//	NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);
}

void Nrf24L01::TM_NRF24L01_WriteRegister(uint8_t reg, uint8_t value) {
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	SPI_Send(_spi->Instance, NRF24L01_WRITE_REGISTER_MASK(reg));
	SPI_Send(_spi->Instance, value);
//	NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);
}

void Nrf24L01::TM_NRF24L01_WriteRegisterMulti(uint8_t reg, uint8_t *data, uint8_t count) {
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	SPI_Send(_spi->Instance, NRF24L01_WRITE_REGISTER_MASK(reg));
	SPI_WriteMulti(_spi->Instance, data, count);
//	NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);
}


void Nrf24L01::softwareReset(void) {
	uint8_t data[5];

	TM_NRF24L01_WriteRegister(NRF24L01_REG_CONFIG, 		NRF24L01_REG_DEFAULT_VAL_CONFIG);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_EN_AA,		NRF24L01_REG_DEFAULT_VAL_EN_AA);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_EN_RXADDR, 	NRF24L01_REG_DEFAULT_VAL_EN_RXADDR);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_SETUP_AW, 	NRF24L01_REG_DEFAULT_VAL_SETUP_AW);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_SETUP_RETR, 	NRF24L01_REG_DEFAULT_VAL_SETUP_RETR);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RF_CH, 		NRF24L01_REG_DEFAULT_VAL_RF_CH);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RF_SETUP, 	NRF24L01_REG_DEFAULT_VAL_RF_SETUP);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_STATUS, 		NRF24L01_REG_DEFAULT_VAL_STATUS);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_OBSERVE_TX, 	NRF24L01_REG_DEFAULT_VAL_OBSERVE_TX);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RPD, 		NRF24L01_REG_DEFAULT_VAL_RPD);

	//P0
	data[0] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P0_0;
	data[1] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P0_1;
	data[2] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P0_2;
	data[3] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P0_3;
	data[4] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P0_4;
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_RX_ADDR_P0, data, 5);

	//P1
	data[0] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P1_0;
	data[1] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P1_1;
	data[2] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P1_2;
	data[3] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P1_3;
	data[4] = NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P1_4;
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_RX_ADDR_P1, data, 5);

	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_ADDR_P2, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P2);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_ADDR_P3, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P3);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_ADDR_P4, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P4);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_ADDR_P5, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P5);

	//TX
	data[0] = NRF24L01_REG_DEFAULT_VAL_TX_ADDR_0;
	data[1] = NRF24L01_REG_DEFAULT_VAL_TX_ADDR_1;
	data[2] = NRF24L01_REG_DEFAULT_VAL_TX_ADDR_2;
	data[3] = NRF24L01_REG_DEFAULT_VAL_TX_ADDR_3;
	data[4] = NRF24L01_REG_DEFAULT_VAL_TX_ADDR_4;
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_TX_ADDR, data, 5);

	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P0, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P0);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P1, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P1);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P2, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P2);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P3, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P3);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P4, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P4);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P5, 	NRF24L01_REG_DEFAULT_VAL_RX_PW_P5);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_FIFO_STATUS, NRF24L01_REG_DEFAULT_VAL_FIFO_STATUS);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_DYNPD, 		NRF24L01_REG_DEFAULT_VAL_DYNPD);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_FEATURE, 	NRF24L01_REG_DEFAULT_VAL_FEATURE);
}

uint8_t Nrf24L01::RxFifoEmpty(void) {
	uint8_t reg = TM_NRF24L01_ReadRegister(NRF24L01_REG_FIFO_STATUS);
	return NRF24L01_CHECK_BIT(reg, NRF24L01_RX_EMPTY);
}

// ----------------------- PUBLIC METHODS -------------------------------------

uint8_t Nrf24L01::init() {
	/* Initialize CE and CSN pins */
	/* CNS pin */
	//TM_GPIO_Init(NRF24L01_CSN_PORT, NRF24L01_CSN_PIN, TM_GPIO_Mode_OUT, TM_GPIO_OType_PP, TM_GPIO_PuPd_UP, TM_GPIO_Speed_Low);

	/* CE pin */
	//TM_GPIO_Init(NRF24L01_CE_PORT, NRF24L01_CE_PIN, TM_GPIO_Mode_OUT, TM_GPIO_OType_PP, TM_GPIO_PuPd_UP, TM_GPIO_Speed_Low);

	/* CSN high = disable SPI */
	PIN_HIGH(_cs_port, _cs_pin);

	/* CE low = disable TX/RX */
	PIN_LOW(_ce_port, _ce_pin);



	/* Max payload is 32bytes */
	if (_payload_size > 32) {
		_payload_size = 32;
	}

	/* Fill structure */
	nrf_config.Channel = !_channel; /* Set channel to some different value for TM_NRF24L01_SetChannel() function */
	nrf_config.PayloadSize = _payload_size;
	nrf_config.OutPwr = TM_NRF24L01_OutputPower_0dBm;
	nrf_config.DataRate = TM_NRF24L01_DataRate_2M;

	/* Reset nRF24L01+ to power on registers values */
	softwareReset();

	/* Channel select */
	SetChannel(_channel);

	/* Set pipeline to max possible 32 bytes */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P0, nrf_config.PayloadSize); // Auto-ACK pipe
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P1, nrf_config.PayloadSize); // Data payload pipe
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P2, nrf_config.PayloadSize);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P3, nrf_config.PayloadSize);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P4, nrf_config.PayloadSize);
	TM_NRF24L01_WriteRegister(NRF24L01_REG_RX_PW_P5, nrf_config.PayloadSize);

	/* Set RF settings (2mbps, output power) */
	SetRF(nrf_config.DataRate, nrf_config.OutPwr);

	/* Config register */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_CONFIG, NRF24L01_CONFIG);

	/* Enable auto-acknowledgment for all pipes */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_EN_AA, 0x3F);

	/* Enable RX addresses */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_EN_RXADDR, 0x3F);

	/* Auto retransmit delay: 1000 (4x250) us and Up to 15 retransmit trials */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_SETUP_RETR, 0x4F);

	/* Dynamic length configurations: No dynamic length */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_DYNPD, (0 << NRF24L01_DPL_P0) | (0 << NRF24L01_DPL_P1) | (0 << NRF24L01_DPL_P2) | (0 << NRF24L01_DPL_P3) | (0 << NRF24L01_DPL_P4) | (0 << NRF24L01_DPL_P5));

	/* Clear FIFOs */
	NRF24L01_FLUSH_TX(_spi->Instance, _cs_port, _cs_pin);
	NRF24L01_FLUSH_RX(_spi->Instance, _cs_port, _cs_pin);

	/* Clear interrupts */
	Clear_Interrupts();

	/* Go to RX mode */
	PowerUpRx();

	/* Return OK */
	return 1;
}

void Nrf24L01::SetMyAddress(uint8_t *adr) {
//	NRF24L01_CE_LOW;
	PIN_LOW(_ce_port, _ce_pin);
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_RX_ADDR_P1, adr, 5);
//	NRF24L01_CE_HIGH;
	PIN_HIGH(_ce_port, _ce_pin);
}

void Nrf24L01::SetTxAddress(uint8_t *adr) {
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_RX_ADDR_P0, adr, 5);
	TM_NRF24L01_WriteRegisterMulti(NRF24L01_REG_TX_ADDR, adr, 5);
}


uint8_t Nrf24L01::GetStatus(void) {
	uint8_t status;

//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	/* First received byte is always status register */
	status = SPI_Send(_spi->Instance, NRF24L01_NOP_MASK);
	/* Pull up chip select */
//	NRF24L01_CSN_HIGH;
	PIN_LOW(_cs_port, _cs_pin);

	return status;
}

Transmit_Status_t Nrf24L01::GetTransmissionStatus(void) {
	uint8_t status = GetStatus();
	if (NRF24L01_CHECK_BIT(status, NRF24L01_TX_DS)) {
		/* Successfully sent */
		return TM_NRF24L01_Transmit_Status_Ok;
	} else if (NRF24L01_CHECK_BIT(status, NRF24L01_MAX_RT)) {
		/* Message lost */
		return TM_NRF24L01_Transmit_Status_Lost;
	}

	/* Still sending */
	return TM_NRF24L01_Transmit_Status_Sending;
}


uint8_t Nrf24L01::GetRetransmissionsCount(void) {
	/* Low 4 bits */
	return TM_NRF24L01_ReadRegister(NRF24L01_REG_OBSERVE_TX) & 0x0F;
}

void Nrf24L01::PowerUpTx(void) {
	Clear_Interrupts();
	TM_NRF24L01_WriteRegister(NRF24L01_REG_CONFIG, NRF24L01_CONFIG | (0 << NRF24L01_PRIM_RX) | (1 << NRF24L01_PWR_UP));
}

void Nrf24L01::SetChannel(uint8_t channel) {
	if (channel <= 125 && channel != nrf_config.Channel) {
		/* Store new channel setting */
		nrf_config.Channel = channel;
		/* Write channel */
		TM_NRF24L01_WriteRegister(NRF24L01_REG_RF_CH, channel);
	}
}

void Nrf24L01::SetRF(TM_NRF24L01_DataRate_t DataRate, TM_NRF24L01_OutputPower_t OutPwr) {
	uint8_t tmp = 0;
	nrf_config.DataRate = DataRate;
	nrf_config.OutPwr = OutPwr;

	if (DataRate == TM_NRF24L01_DataRate_2M) {
		tmp |= 1 << NRF24L01_RF_DR_HIGH;
	} else if (DataRate == TM_NRF24L01_DataRate_250k) {
		tmp |= 1 << NRF24L01_RF_DR_LOW;
	}
	/* If 1Mbps, all bits set to 0 */

	if (OutPwr == TM_NRF24L01_OutputPower_0dBm) {
		tmp |= 3 << NRF24L01_RF_PWR;
	} else if (OutPwr == TM_NRF24L01_OutputPower_M6dBm) {
		tmp |= 2 << NRF24L01_RF_PWR;
	} else if (OutPwr == TM_NRF24L01_OutputPower_M12dBm) {
		tmp |= 1 << NRF24L01_RF_PWR;
	}

	TM_NRF24L01_WriteRegister(NRF24L01_REG_RF_SETUP, tmp);
}

uint8_t Nrf24L01::Read_Interrupts(TM_NRF24L01_IRQ_t* IRQ) {
	IRQ->Status = GetStatus();
	return IRQ->Status;
}

void Nrf24L01::Clear_Interrupts(void) {
	TM_NRF24L01_WriteRegister(0x07, 0x70);
}

void Nrf24L01::PowerUpRx(void) {
	/* Disable RX/TX mode */
//	NRF24L01_CE_LOW;
	PIN_LOW(_ce_port, _ce_pin);
	/* Clear RX buffer */
	NRF24L01_FLUSH_RX(_spi->Instance, _cs_port, _cs_pin);
	/* Clear interrupts */
	Clear_Interrupts();
	/* Setup RX mode */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_CONFIG, NRF24L01_CONFIG | 1 << NRF24L01_PWR_UP | 1 << NRF24L01_PRIM_RX);
	/* Start listening */
//	NRF24L01_CE_HIGH;
	PIN_HIGH(_ce_port, _ce_pin);
}


void Nrf24L01::PowerDown(void) {
//	NRF24L01_CE_LOW;
	PIN_LOW(_ce_port, _ce_pin);
	TM_NRF24L01_WriteBit(NRF24L01_REG_CONFIG, NRF24L01_PWR_UP, 0);
}



void Nrf24L01::Transmit(uint8_t *data) {
	uint8_t count = nrf_config.PayloadSize;

	/* Chip enable put to low, disable it */
//	NRF24L01_CE_LOW;
	PIN_LOW(_ce_port, _ce_pin);

	/* Go to power up tx mode */
	PowerUpTx();

	/* Clear TX FIFO from NRF24L01+ */
	NRF24L01_FLUSH_TX(_spi->Instance, _cs_port, _cs_pin);

	/* Send payload to nRF24L01+ */
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	/* Send write payload command */
	SPI_Send(_spi->Instance, NRF24L01_W_TX_PAYLOAD_MASK);
	/* Fill payload with data*/
	SPI_WriteMulti(_spi->Instance, data, count);
	/* Disable SPI */
//	NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);

	/* Send data! */
//	NRF24L01_CE_HIGH;
	PIN_HIGH(_ce_port, _ce_pin);
}

void Nrf24L01::GetData(uint8_t* data) {
	/* Pull down chip select */
//	NRF24L01_CSN_LOW;
	PIN_LOW(_cs_port, _cs_pin);
	/* Send read payload command*/
	SPI_Send(_spi->Instance, NRF24L01_R_RX_PAYLOAD_MASK);
	/* Read payload */
	SPI_SendMulti(_spi->Instance, data, data, nrf_config.PayloadSize);
	/* Pull up chip select */
//	NRF24L01_CSN_HIGH;
	PIN_HIGH(_cs_port, _cs_pin);

	/* Reset status register, clear RX_DR interrupt flag */
	TM_NRF24L01_WriteRegister(NRF24L01_REG_STATUS, (1 << NRF24L01_RX_DR));
}

uint8_t Nrf24L01::DataReady(void) {
	uint8_t status = GetStatus();

	if (NRF24L01_CHECK_BIT(status, NRF24L01_RX_DR)) {
		return 1;
	}
	return !RxFifoEmpty();
}