#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);}
*/


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

	init();
}

uint8_t Nrf24L01::init() {
	//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);
	//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 */
	writeRegister(NRF24L01_REG_RX_PW_P0, nrf_config.PayloadSize); // Auto-ACK pipe
	writeRegister(NRF24L01_REG_RX_PW_P1, nrf_config.PayloadSize); // Data payload pipe
	writeRegister(NRF24L01_REG_RX_PW_P2, nrf_config.PayloadSize);
	writeRegister(NRF24L01_REG_RX_PW_P3, nrf_config.PayloadSize);
	writeRegister(NRF24L01_REG_RX_PW_P4, nrf_config.PayloadSize);
	writeRegister(NRF24L01_REG_RX_PW_P5, nrf_config.PayloadSize);

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

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

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

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

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

	/* Dynamic length configurations: No dynamic length */
	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(_nrfManager, _cs_port, _cs_pin);
	NRF24L01_FLUSH_RX(_nrfManager, _cs_port, _cs_pin);

	/* Clear interrupts */
	Clear_Interrupts();

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

	/* Return OK */
	return 1;
}


void Nrf24L01::writeBit(uint8_t reg, uint8_t bit, uint8_t value) {
	uint8_t tmp;
	tmp = readRegister(reg);
	if (value) {
		tmp |= 1 << bit;
	} else {
		tmp &= ~(1 << bit);
	}
	writeRegister(reg, tmp);
}

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

uint8_t Nrf24L01::readRegister(uint8_t reg) {
	uint8_t value;

	PIN_LOW(_cs_port, _cs_pin);

	_nrfManager->SPI_Send(NRF24L01_READ_REGISTER_MASK(reg));
	value = _nrfManager->SPI_Send(NRF24L01_NOP_MASK);

	PIN_HIGH(_cs_port, _cs_pin);

	return value;
}

void Nrf24L01::readRegisterMulti(uint8_t reg, uint8_t* data, uint8_t count) {
	PIN_LOW(_cs_port, _cs_pin);

	_nrfManager->SPI_Send(NRF24L01_READ_REGISTER_MASK(reg));
	_nrfManager->SPI_ReadMulti(data, NRF24L01_NOP_MASK, count);

	PIN_HIGH(_cs_port, _cs_pin);
}

void Nrf24L01::writeRegister(uint8_t reg, uint8_t value) {
	PIN_LOW(_cs_port, _cs_pin);
	_nrfManager->SPI_Send(NRF24L01_WRITE_REGISTER_MASK(reg));
	_nrfManager->SPI_Send(value);
	PIN_HIGH(_cs_port, _cs_pin);
}

void Nrf24L01::writeRegisterMulti(uint8_t reg, uint8_t *data, uint8_t count) {
	PIN_LOW(_cs_port, _cs_pin);

	_nrfManager->SPI_Send(NRF24L01_WRITE_REGISTER_MASK(reg));
	_nrfManager->SPI_WriteMulti(data, count);

	PIN_HIGH(_cs_port, _cs_pin);
}


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

	writeRegister(NRF24L01_REG_CONFIG, 		NRF24L01_REG_DEFAULT_VAL_CONFIG);
	writeRegister(NRF24L01_REG_EN_AA,		NRF24L01_REG_DEFAULT_VAL_EN_AA);
	writeRegister(NRF24L01_REG_EN_RXADDR, 	NRF24L01_REG_DEFAULT_VAL_EN_RXADDR);
	writeRegister(NRF24L01_REG_SETUP_AW, 	NRF24L01_REG_DEFAULT_VAL_SETUP_AW);
	writeRegister(NRF24L01_REG_SETUP_RETR, 	NRF24L01_REG_DEFAULT_VAL_SETUP_RETR);
	writeRegister(NRF24L01_REG_RF_CH, 		NRF24L01_REG_DEFAULT_VAL_RF_CH);
	writeRegister(NRF24L01_REG_RF_SETUP, 	NRF24L01_REG_DEFAULT_VAL_RF_SETUP);
	writeRegister(NRF24L01_REG_STATUS, 		NRF24L01_REG_DEFAULT_VAL_STATUS);
	writeRegister(NRF24L01_REG_OBSERVE_TX, 	NRF24L01_REG_DEFAULT_VAL_OBSERVE_TX);
	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;
	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;
	writeRegisterMulti(NRF24L01_REG_RX_ADDR_P1, data, 5);

	writeRegister(NRF24L01_REG_RX_ADDR_P2, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P2);
	writeRegister(NRF24L01_REG_RX_ADDR_P3, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P3);
	writeRegister(NRF24L01_REG_RX_ADDR_P4, 	NRF24L01_REG_DEFAULT_VAL_RX_ADDR_P4);
	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;
	writeRegisterMulti(NRF24L01_REG_TX_ADDR, data, 5);

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

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

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

void Nrf24L01::SetMyAddress(uint8_t *adr) {
	PIN_LOW(_ce_port, _ce_pin);

	writeRegisterMulti(NRF24L01_REG_RX_ADDR_P1, adr, 5);

	PIN_HIGH(_ce_port, _ce_pin);
}

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


uint8_t Nrf24L01::GetStatus(void) {
	uint8_t status;
	PIN_LOW(_cs_port, _cs_pin);

	/* First received byte is always status register */
	status = _nrfManager->SPI_Send(NRF24L01_NOP_MASK);

	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 readRegister(NRF24L01_REG_OBSERVE_TX) & 0x0F;
}

void Nrf24L01::PowerUpTx(void) {
	Clear_Interrupts();
	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 */
		writeRegister(NRF24L01_REG_RF_CH, channel);
	}
}

void Nrf24L01::SetRF(NRF_DataRate_t DataRate, NRF_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;
	}

	writeRegister(NRF24L01_REG_RF_SETUP, tmp);
}

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

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

void Nrf24L01::PowerUpRx(void) {
	/* Disable RX/TX mode */
	PIN_LOW(_ce_port, _ce_pin);
	NRF24L01_FLUSH_RX(_nrfManager, _cs_port, _cs_pin);
	Clear_Interrupts();
	/* Setup RX mode */
	writeRegister(NRF24L01_REG_CONFIG, NRF24L01_CONFIG | 1 << NRF24L01_PWR_UP | 1 << NRF24L01_PRIM_RX);
	/* Start listening */
	PIN_HIGH(_ce_port, _ce_pin);
}


void Nrf24L01::PowerDown(void) {
	PIN_LOW(_ce_port, _ce_pin);
	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 */
	PIN_LOW(_ce_port, _ce_pin);

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

	/* Clear TX FIFO from NRF24L01+ */
	NRF24L01_FLUSH_TX(_nrfManager, _cs_port, _cs_pin);

	/* Send payload to nRF24L01+ */
	PIN_LOW(_cs_port, _cs_pin);
	/* Send write payload command */
	_nrfManager->SPI_Send(NRF24L01_W_TX_PAYLOAD_MASK);
	/* Fill payload with data*/
	_nrfManager->SPI_WriteMulti(data, count);

	PIN_HIGH(_cs_port, _cs_pin);

	PIN_HIGH(_ce_port, _ce_pin);
}

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

	/* Reset status register, clear RX_DR interrupt flag */
	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();
}