#include <xradio.h>
#ifdef AVR
#include <avr/wdt.h>
#include <avr/sleep.h>
#endif
#include <math.h>

// This is a replacement mechanism for listen mode. It enables a sleeping node to be
// woken up by a signal.
// It uses the PLLLock irq generated by the radio's listen mode with a
// RSSIThresh of 0 to work around all known listen mode bugs.
// Every LISTEN_IDLE_PERIOD the radio wakes up, switches off listen mode and detects RSSI
// For 2 bits.

// If RSSI detected it probes every LISTEN_PROBE_INTERVAL whether RSSI is still detected.
// Once it's not it goes into RX for LISTEN_RX_TIMEOUT and tries to receive a packet.

// The wakeup agent sends preamble for LISTEN_PROBE_PERIOD to ensure the sleeper sees RSSI.
// Then it is silent for LISTEN_SILENCE_AFTER_PREAMBLE to ensure the sleeper sees the gap and
// afterwards it sends the packet.

// Total RX time for the sleeper after being woken up is on average:
// LISTEN_PROBE_PERIOD / 2 / LISTEN_PROBE_INTERVAL * LISTEN_TIME_TO_PROBE +
// LISTEN_PROBE_INTERVAL + PROFILE_TIMEOUT.

// We want to minimize this by finding the right LISTEN_PROBE_INTERVAL, so set the derivative
// to zero:

// -LISTEN_PROBE_PERIOD / ( 4 * LISTEN_PROBE_INTERVAL^2 ) * LISTEN_TIME_TO_PROBE + 1 = 0
// 4 * LISTEN_PROBE_INTERVAL^2 = LISTEN_PROBE_PERIOD * LISTEN_TIME_TO_PROBE
// LISTEN_PROBE_INTERVAL = sqrt( LISTEN_PROBE_PERIOD * LISTEN_TIME_TO_PROBE / 4 ) 

// Now LISTEN_TIME_TO_PROBE = 150us(PLL_LOCK) + 20us(Tana) + 5bit(~Tcf) + ~2bit(Tddc) + 2bit( RSSI )

// This gives us enough info to calculate LISTEN_PROBE_INTERVAL (see defines below).

// The profiles used to detect RSSI and for the actual packet
// reception can be set independently from each other. Two use cases:
// at 200kbit packet reception at full power is unreliable without
// AGC. So 200kbit RSSI + 55kbit RX allows for full power wakeups with
// reliable packet reception.  Secondly this can be used to wakeup
// using a profile at which link quality is not good enough to RX.


// this is the bitrate which is used to send preamble and wake the sleeper
#define LISTEN_B             200000
// this is the bitrate for the data packet that follows wakeup
#define LISTEN_RX_B          55555


#define GENEROUS( x ) INTDIV( (x) * 11U, 10U )
#define TIGHT( x )    INTDIV( (x) * 10U, 11U )
// #define CAT2(x, y ) x##y
// #define CAT( x, y ) CAT2( x, y )


#define LISTEN_PROFILE       CAT( SX1231_PROFILE_, LISTEN_B)
#define LISTEN_BITRATE       CAT( LISTEN_B, UL)
#define LISTEN_RX_BITRATE    CAT( LISTEN_RX_B, UL)
#define LISTEN_RX_PROFILE    CAT( SX1231_PROFILE_, LISTEN_RX_B)
#define LISTEN_PERIOD        350000UL
#define LISTEN_IDLE_PERIOD   TIGHT( LISTEN_PERIOD )
#define LISTEN_PROBE_PERIOD  GENEROUS( LISTEN_PERIOD )
#define LISTEN_SILENCE_AFTER_PREAMBLE GENEROUS(LISTEN_PROBE_INTERVAL + MICROS_BEFORE_RX_READY( MICROS_PER_BYTE( LISTEN_RX_BITRATE )) + 2000)
#define LISTEN_RX_PERIOD     ROUGH_STEPS_TO_MICROS( STEPS_TO_PAYLOADDONE( MICROS_PER_BYTE( LISTEN_RX_BITRATE )))
#define LISTEN_RX_TIMEOUT    GENEROUS(LISTEN_SILENCE_AFTER_PREAMBLE + ROUGH_STEPS_TO_MICROS( STEPS_TO_PAYLOADDONE( MICROS_PER_BYTE( LISTEN_RX_BITRATE ))))
#define LISTEN_RSSI_THRESH   80
#define LISTEN_TIME_TO_PROBE (150 + 20 + DIV_ROUND_CLOSEST( MICROS_PER_BYTE(LISTEN_BITRATE) * (5UL + 2 + 2 ), 8 ))
#define LISTEN_PROBE_INTERVAL  ((uint32_t)sqrt( LISTEN_PROBE_PERIOD * LISTEN_TIME_TO_PROBE / 4.0 ))
#define LISTEN_RSSI_DETECT_TIME GENEROUS( (( 4U * 1000000UL ) / LISTEN_BITRATE ) )
	

#ifdef AVR

static bool abortL = false;

void abortListen()
{
    abortL = true;
}

static volatile int8_t irqHits;

static void irq() 
{
	irqHits--;
}

static void rdelay_impl( uint32_t us )
{
	uint8_t idleResol;
	uint32_t divisor;
	
	if( us > 255 * 4100UL ) {
		idleResol = SX1231_LISTEN1_RESOL_IDLE_262000;
		divisor = 262000;
	}
	else if( us > 255 * 64L ) {
		idleResol = SX1231_LISTEN1_RESOL_IDLE_4100;
		divisor = 4100;
	}
	else {
		idleResol = SX1231_LISTEN1_RESOL_IDLE_64;
		divisor = 64;
	}

	uint8_t period = (us + (divisor >> 1 ) ) / divisor;
	
	SX1231Driver::writeReg( SX1231_REG_LISTEN1, SX1231_LISTEN1_RESOL_RX_64 | idleResol |
							SX1231_LISTEN1_CRITERIA_RSSI | SX1231_LISTEN1_END_00 );
	SX1231Driver::writeReg( SX1231_REG_LISTEN2, period );
	
	SX1231Driver::setMode( SX1231_OPMODE_SEQUENCER_ON | SX1231_OPMODE_STANDBY | SX1231_OPMODE_LISTEN_ON  );
		
	irqHits = 3;
	while( irqHits > 0 && !abortL) {
		
#if RADIO_ENABLE_LOG && defined( AVR )
		Serial.flush();
#endif
		cli();
		wdt_disable();
		
		ADCSRA &= ~(1 << ADEN);
        rtc_notify_sleep();
//        set_sleep_mode( SLEEP_MODE_PWR_DOWN );
		set_sleep_mode( SLEEP_MODE_PWR_SAVE );
		
		sleep_enable();
#if !DEBUG_CONSOLE_EN
		sleep_bod_disable();
#endif        
		sei();
		sleep_cpu();
		sleep_disable();
		ADCSRA |= (1 << ADEN);
		sei();
	}
}

void rdelay( uint32_t us, uint8_t targetMode ) 
{
	SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
	attachInterrupt( RADIO_IRQ_NUM, irq, RISING );
	uint8_t oldThresh = SX1231Driver::readReg( SX1231_REG_RSSITHRESH );
	uint8_t oldMapping = SX1231Driver::readReg( SX1231_REG_DIOMAPPING1 );
	SX1231Driver::writeReg( SX1231_REG_DIOMAPPING1, SX1231_DIOMAPPING1_DIO0_11 );
	SX1231Driver::writeReg( SX1231_REG_RSSITHRESH, 0 );
	SX1231Driver::writeReg( SX1231_REG_LISTEN3, 2 );

	const uint32_t maxDelay = 255UL * 262000UL;
	while( us > maxDelay ) {
		rdelay_impl( maxDelay );
		us -= maxDelay;
	}
	rdelay_impl( us );
	detachInterrupt( RADIO_IRQ_NUM );
	XRadio<SX1231Driver>::attachInterrupt();
	SX1231Driver::setMode(
		SX1231_OPMODE_SEQUENCER_ON | SX1231_OPMODE_LISTENABORT | targetMode );
	SX1231Driver::setMode(
		SX1231_OPMODE_SEQUENCER_ON | targetMode );
	SX1231Driver::writeReg( SX1231_REG_RSSITHRESH, oldThresh );
	SX1231Driver::writeReg( SX1231_REG_DIOMAPPING1, oldMapping );
 }

static bool detectRssi() 
{
	while( SX1231Driver::readReg( SX1231_REG_OPMODE ) != SX1231_OPMODE_RECEIVER );
	SX1231Driver::waitModeReady();
	delayMicroseconds( LISTEN_RSSI_DETECT_TIME);
	
	bool rssi = SX1231Driver::readReg( SX1231_REG_IRQFLAGS1 ) & SX1231_IRQFLAGS1_RSSI;
	SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
	return rssi;
}

uint8_t listen( void* buf, uint8_t maxlen ) 
{
	uint8_t oldProfile = SX1231Driver::getProfile();
	uint8_t oldThresh = SX1231Driver::readReg( SX1231_REG_RSSITHRESH );
	SX1231Driver::writeReg( SX1231_REG_RSSITHRESH, LISTEN_RSSI_THRESH );
	logFlushFlash();
	while( true ) {
		SX1231Driver::setProfile( LISTEN_PROFILE );
		rdelay( LISTEN_IDLE_PERIOD, SX1231_OPMODE_RECEIVER );
        if( abortL ) {
            abortL = false;
            return 0;
        }
		if( detectRssi() ) {
			logValue( P("rssi") );
			SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
			const uint16_t probeDelay = LISTEN_PROBE_INTERVAL;
			for( uint16_t i = 0; i < LISTEN_PROBE_PERIOD / LISTEN_PROBE_INTERVAL; i++ ) {
				rdelay( probeDelay, SX1231_OPMODE_RECEIVER );
				if( !detectRssi() ) {
					SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
					SX1231Driver::writeReg( SX1231_REG_RSSITHRESH, oldThresh );
					SX1231Driver::setProfile( LISTEN_RX_PROFILE );		
					XRadio<SX1231Driver>::setMode( SX1231_OPMODE_RECEIVER );
					uint8_t len = 0;
					
					logValue( P("gap") );
					logValue( i );
					uint32_t m = micros();
#if RADIO_ENABLE_HOPPING
					const uint8_t mlen = maxlen + sizeof( FHopHeader );
					uint8_t b[ mlen ];
#else
					uint8_t* b = (uint8_t*)buf;
					const uint8_t mlen = maxlen;
#endif
					
					while( micros() - m < LISTEN_RX_TIMEOUT ) {
						len = Radio<SX1231Driver>::receive( b, mlen );
						if( len ) {
							uint8_t totallen = Radio<SX1231Driver>::getTotalLength();
							// logValue( totallen );
							// logValue( len );
							SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
							SX1231Driver::setProfile( oldProfile );
#if RADIO_ENABLE_HOPPING
							FHopHeader header;
							memcpy( &header, b, sizeof( header ) );
							memcpy( buf, b + sizeof( header ), len - sizeof( header ) );
							logValue( header.remoteSteps );
							XRadio<SX1231Driver>::setSynchronized(
								Radio<SX1231Driver>::getSender(), header, totallen  );
#endif							
							
							return len;
						}
					}		
					if( !len) {
						SX1231Driver::writeReg( SX1231_REG_RSSITHRESH, LISTEN_RSSI_THRESH );
						logValue( "no packet" );
						SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
						delaySteps( ROUGH_MILLIS_TO_STEPS( 5000UL ) );
					}
					SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
					break;
				}
			}
		}
		logFlushFlash();
	}
}

#endif

#define LGVAR( x )								\
	logValue( #x ": " );					\
	logValue( x )							


void wakeListener( uint8_t target, const void* buf, uint8_t len ) 
{
	// LGVAR( LISTEN_BITRATE );
	// LGVAR( LISTEN_PERIOD );
	// LGVAR( LISTEN_IDLE_PERIOD );
	// LGVAR( LISTEN_PROBE_PERIOD );
	// LGVAR( LISTEN_PROBE_INTERVAL );
	// LGVAR( LISTEN_SILENCE_AFTER_PREAMBLE );
	// LGVAR( LISTEN_RX_TIMEOUT );
	// LGVAR( LISTEN_TIME_TO_PROBE );
	// LGVAR( LISTEN_RX_PERIOD );
	// LGVAR( LISTEN_RSSI_THRESH );
	// LGVAR( LISTEN_RSSI_DETECT_TIME );		
	
	logFlushFlash();
	
	int8_t pwr = SX1231Driver::getPowerLeveldBm();
	SX1231Driver::setMaxPower();
	uint8_t oldProfile = SX1231Driver::getProfile();
	SX1231Driver::setProfile( LISTEN_PROFILE );
	SX1231Driver::setMode( SX1231_OPMODE_TRANSMITTER );
	delaySteps( MILLIS_TO_STEPS(INTDIV( LISTEN_PERIOD, 1000 )));
	SX1231Driver::setMode( SX1231_OPMODE_STANDBY );
	uint32_t m = micros();
	while( micros() - m < LISTEN_SILENCE_AFTER_PREAMBLE )
		optimistic_yield( 10000 );
	XRadio<SX1231Driver>::pauseMetaPackets( true );
	SX1231Driver::setProfile( LISTEN_RX_PROFILE );
	
#if RADIO_ENABLE_HOPPING
	uint8_t b[ sizeof( FHopHeader ) + len ];
	FHopHeader header;
	header.remoteSteps = getSteps();
	logValue( header.remoteSteps );
	memcpy( b, &header, sizeof( header ) );
	memcpy( b + sizeof( header ), buf, len );
	
	Radio<SX1231Driver>::send( target, b, len + sizeof( header ));
#else
	Radio<SX1231Driver>::send( target, buf, len);
#endif
	
	
	SX1231Driver::waitForTansmitComplete( );
	
	XRadio<SX1231Driver>::pauseMetaPackets( false );
	SX1231Driver::setProfile( oldProfile );
	SX1231Driver::setPowerLeveldBm( pwr );
	logFlushFlash();
}

//#define TEST

#ifdef TEST

XRadio<SX1231Driver> radio;

#define PRVAR( x )								\
	Serial.print( #x ": " );					\
	Serial.println( x )							


#ifdef LISTENER

void setup()
{
	Serial.begin( 500000 );
	Serial.println( "Begin Listener" );
	PRVAR( LISTEN_BITRATE );
	PRVAR( LISTEN_PERIOD );
	PRVAR( LISTEN_IDLE_PERIOD );
	PRVAR( LISTEN_PROBE_PERIOD );
	PRVAR( LISTEN_PROBE_INTERVAL );
	PRVAR( LISTEN_SILENCE_AFTER_PREAMBLE );
	PRVAR( LISTEN_RX_TIMEOUT );
	PRVAR( LISTEN_TIME_TO_PROBE );
	PRVAR( LISTEN_RX_PERIOD );
	PRVAR( LISTEN_RSSI_THRESH );
	PRVAR( LISTEN_RSSI_DETECT_TIME );

	wdt_disable();
	radio.initialize( 5, 120, RADIO_868MHZ, false );
	logEnableAll( false );
	while( true ) {
		char buf[61];
		listen( buf, 61 );
		Serial.println( buf );
	}
}
#endif


#ifdef SENDER

void setup()
{
	Serial.begin( 500000 );
	Serial.println( "Begin Sender" );

	PRVAR( LISTEN_BITRATE );
	PRVAR( LISTEN_PERIOD );
	PRVAR( LISTEN_IDLE_PERIOD );
	PRVAR( LISTEN_PROBE_PERIOD );
	PRVAR( LISTEN_PROBE_INTERVAL );
	PRVAR( LISTEN_SILENCE_AFTER_PREAMBLE );
	PRVAR( LISTEN_RX_TIMEOUT );
	PRVAR( LISTEN_TIME_TO_PROBE );
	PRVAR( LISTEN_RX_PERIOD );
	PRVAR( LISTEN_RSSI_THRESH );
	PRVAR( LISTEN_RSSI_DETECT_TIME );


	wdt_disable();
	radio.initialize( 1, 100, RADIO_868MHZ, true );
	radio.setKey( "Flume_EncryptKey" );
	
//	logEnableAll( false );
//	radio.setPowerLeveldBm(  );
	radio.setMaxPower();
	radio.setAutoGain( 3, false );
	
	uint16_t i = 0;
	while( true ) {
		wakeListener( 3, "ping", 5 );
		Serial.print( "Sent ping " );
		Serial.println( i++ );
		logFlushFlash();
		Serial.flush();
		rdelay( 1000000, SX1231_OPMODE_STANDBY );
	}
}

#endif

void loop()
{
}


#endif