#include <RFM69.h>
#include <RFM69registers.h>
#include <listen.h>
#include <SPI.h>
#include <LowPower.h>	

static volatile uint8_t receivedSize = 0;
static RFM69* pRadio;
static uint8_t maxSize;
static byte* buffer;
static uint8_t node;
static volatile uint8_t burstReceived = false;
static volatile uint16_t stamp;
static uint8_t broadcastId = 255;


void RFM69Listener::setBroadcastId( uint8_t id )
{
	broadcastId = id;
}


uint16_t RFM69Listener::burstStamp()
{
	return stamp;
}

void blink( int delayms );


static void irq() 
{


	burstReceived = true;
	if( receivedSize != 0 ) return;
	
	noInterrupts();
	
    pRadio->select();
    SPI.transfer(REG_FIFO & 0x7F);
    uint8_t len = SPI.transfer(0);
    uint8_t tnode = SPI.transfer(0);

	stamp = SPI.transfer( 0 );
	stamp |= SPI.transfer(0 ) << 8;
	len -= 3;
	

	len = len > maxSize ? maxSize : len; // precaution
	for( uint8_t i = 0; i < len; i++ ) {
		uint8_t b = SPI.transfer( 0 );
		buffer[i] = b;
	}
	
	pRadio->unselect();
	interrupts();
	
	if( tnode == node || tnode == broadcastId ) {
		receivedSize = len;
	}
	

}


static void delayIrq() 
{
	return;
}

void RFM69Listener::delay( RFM69& radio, uint16_t millisInterval ) 
{
	radio.setMode( RF69_MODE_STANDBY );
	while ((radio.readReg(REG_IRQFLAGS1) & RF_IRQFLAGS1_MODEREADY) == 0x00); // wait for ModeReady
	
	detachInterrupt( RF69_IRQ_NUM );
	attachInterrupt( RF69_IRQ_NUM, delayIrq, RISING);
	radio.writeReg( REG_DIOMAPPING1, RF_DIOMAPPING1_DIO0_11 );
	
	radio.writeReg( REG_BITRATEMSB, RF_BITRATEMSB_200000);
	radio.writeReg( REG_BITRATELSB, RF_BITRATELSB_200000);
	radio.writeReg( REG_FDEVMSB, RF_FDEVMSB_100000 );
	radio.writeReg( REG_FDEVLSB, RF_FDEVLSB_100000 );
	radio.writeReg( REG_RXBW, RF_RXBW_DCCFREQ_000 | RF_RXBW_MANT_16 | RF_RXBW_EXP_0 );
	
	uint8_t idleResol;
	uint32_t divisor;
	uint32_t microInterval = millisInterval * 1000L;
	
	if( microInterval > 255 * 4100L ) {
		idleResol = RF_LISTEN1_RESOL_IDLE_262000;
		divisor = 262000;
	}
	else if( microInterval > 255 * 64L ) {
		idleResol = RF_LISTEN1_RESOL_IDLE_4100;
		divisor = 4100;
	}
	else {
		idleResol = RF_LISTEN1_RESOL_IDLE_64;
		divisor = 64;
	}

	radio.writeReg( REG_LISTEN1, RF_LISTEN1_RESOL_RX_64 | idleResol |
					RF_LISTEN1_CRITERIA_RSSI | RF_LISTEN1_END_10 );

	radio.writeReg( REG_LISTEN2, (microInterval + (divisor >> 1 ) ) / divisor );
	radio.writeReg( REG_LISTEN3, 4 );

    radio.writeReg( REG_RSSITHRESH, 255 );
	radio.writeReg( REG_RXTIMEOUT2, 1 );
	
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_STANDBY  );
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_STANDBY | RF_OPMODE_LISTEN_ON  );
	
	LowPower.powerDown( SLEEP_FOREVER, ADC_OFF, BOD_OFF );
	LowPower.powerDown( SLEEP_FOREVER, ADC_OFF, BOD_OFF );
	LowPower.powerDown( SLEEP_FOREVER, ADC_OFF, BOD_OFF );

	endListening( radio );
}


void RFM69Listener::startListening(
	RFM69& radio, void* buf, uint8_t size, uint16_t millisInterval, bool highspeed )
{

	maxSize = size;
	buffer = (byte*)buf;

	pRadio = &radio;
	node = radio.getAddress();
	
	clearBuffer();

	radio.setMode( RF69_MODE_STANDBY );
	while ((radio.readReg(REG_IRQFLAGS1) & RF_IRQFLAGS1_MODEREADY) == 0x00); // wait for ModeReady

	detachInterrupt( RF69_IRQ_NUM );
	attachInterrupt( RF69_IRQ_NUM, irq, RISING);
	
	radio.writeReg( REG_DIOMAPPING1, RF_DIOMAPPING1_DIO0_01 );

	radio.writeReg( REG_FRFMSB, RF_FRFMSB_433 + 1);
	radio.writeReg( REG_FRFMID, RF_FRFMID_433 );
	radio.writeReg( REG_FRFLSB, RF_FRFLSB_433 );
	

	if( highspeed ) {
		radio.writeReg( REG_BITRATEMSB, RF_BITRATEMSB_200000);
		radio.writeReg( REG_BITRATELSB, RF_BITRATELSB_200000);
		radio.writeReg( REG_FDEVMSB, RF_FDEVMSB_100000 );
		radio.writeReg( REG_FDEVLSB, RF_FDEVLSB_100000 );
		radio.writeReg( REG_RXBW, RF_RXBW_DCCFREQ_000 | RF_RXBW_MANT_16 | RF_RXBW_EXP_0 );
	}
	
	radio.writeReg( REG_PACKETCONFIG2, RF_PACKET2_RXRESTARTDELAY_NONE | RF_PACKET2_AUTORXRESTART_ON | RF_PACKET2_AES_OFF );

	radio.writeReg( REG_PACKETCONFIG1, RF_PACKET1_FORMAT_VARIABLE | RF_PACKET1_DCFREE_WHITENING | RF_PACKET1_CRC_ON | RF_PACKET1_CRCAUTOCLEAR_ON  );
	radio.writeReg( REG_SYNCVALUE1, 0x5A );
	radio.writeReg( REG_SYNCVALUE2, 0x5A );

	uint8_t idleResol;
	uint32_t divisor;
	uint32_t microInterval = millisInterval * 1000L;

	if( microInterval > 255 * 4100L ) {
		idleResol = RF_LISTEN1_RESOL_IDLE_262000;
		divisor = 262000;
	}
	else if( microInterval > 255 * 64L ) {
		idleResol = RF_LISTEN1_RESOL_IDLE_4100;
		divisor = 4100;
	}
	else {
		idleResol = RF_LISTEN1_RESOL_IDLE_64;
		divisor = 64;
	}

	radio.writeReg( REG_LISTEN1, RF_LISTEN1_RESOL_RX_64 | idleResol |
					RF_LISTEN1_CRITERIA_RSSI | RF_LISTEN1_END_10 );

	radio.writeReg( REG_LISTEN2, (microInterval + (divisor >> 1 ) ) / divisor );

	if( highspeed ) 
		radio.writeReg( REG_LISTEN3, 4 );
	else
		radio.writeReg( REG_LISTEN3, 6 );


    radio.writeReg( REG_RSSITHRESH, 180 );
	radio.writeReg( REG_RXTIMEOUT2, 75 );
	
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_STANDBY  );
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_STANDBY | RF_OPMODE_LISTEN_ON  );
}

uint8_t RFM69Listener::receivedBytes()
{
	return receivedSize;
}

uint8_t RFM69Listener::wokenByBurst() 
{
	return burstReceived;
}

void RFM69Listener::clearBuffer()
{
	receivedSize = 0;
	burstReceived = false;
	stamp = 0;
}

void RFM69Listener::endListening( RFM69& radio )
{
	detachInterrupt( RF69_IRQ_NUM );
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_LISTENABORT | RF_OPMODE_STANDBY );
	radio.writeReg( REG_OPMODE, RF_OPMODE_SEQUENCER_ON | RF_OPMODE_STANDBY );
 	radio.writeReg( REG_RXTIMEOUT2, 0 );
	radio.setMode( RF69_MODE_STANDBY );
	while ((radio.readReg(REG_IRQFLAGS1) & RF_IRQFLAGS1_MODEREADY) == 0x00); // wait for ModeReady
}

void RFM69Listener::send( RFM69& radio, uint8_t targetNode, void* buffer, uint8_t size, uint16_t boostPeriod, bool highspeed )
{
	detachInterrupt( RF69_IRQ_NUM );
	radio.encrypt( 0 );
	radio.setMode( RF69_MODE_STANDBY );

	radio.writeReg( REG_PACKETCONFIG1, RF_PACKET1_FORMAT_VARIABLE | RF_PACKET1_DCFREE_WHITENING | RF_PACKET1_CRC_ON | RF_PACKET1_CRCAUTOCLEAR_ON  );
	radio.writeReg( REG_SYNCVALUE1, 0x5A );
	radio.writeReg( REG_SYNCVALUE2, 0x5A );

	radio.writeReg( REG_FRFMSB, RF_FRFMSB_433 + 1 );
	radio.writeReg( REG_FRFMID, RF_FRFMID_433  );
	radio.writeReg( REG_FRFLSB, RF_FRFLSB_433 );

	if( highspeed ) {
		radio.writeReg( REG_BITRATEMSB, RF_BITRATEMSB_200000);
		radio.writeReg( REG_BITRATELSB, RF_BITRATELSB_200000);
		radio.writeReg( REG_FDEVMSB, RF_FDEVMSB_100000 );
		radio.writeReg( REG_FDEVLSB, RF_FDEVLSB_100000 );
		radio.writeReg( REG_RXBW, RF_RXBW_DCCFREQ_000 | RF_RXBW_MANT_16 | RF_RXBW_EXP_0 );


	}

	
	
	radio.setMode( RF69_MODE_TX );
	
	uint32_t time = millis();
	uint32_t m;
	while( (m = millis()) - time <= boostPeriod ) {

		noInterrupts();
		// write to FIFO
		radio.select();
		SPI.transfer(REG_FIFO | 0x80);

		SPI.transfer( size + 3);

		SPI.transfer( targetNode );

		uint16_t diff = m - time;
		SPI.transfer( diff & 0xff );
		SPI.transfer( (diff & 0xff00) >> 8 );

		for (uint8_t i = 0; i < size; i++)
		  	SPI.transfer(((uint8_t*) buffer)[i]);
		radio.unselect();
		interrupts();
		while ((radio.readReg(REG_IRQFLAGS2) & RF_IRQFLAGS2_FIFONOTEMPTY) != 0x00);
	}
	
	radio.setMode( RF69_MODE_STANDBY );
}