/*
 Protocol by Dennis Cabell, 2017
 KE8FZX
  
 To use this software, you must adhere to the license terms described below, and assume all responsibility for the use
 of the software.  The user is responsible for all consequences or damage that may result from using this software.
 The user is responsible for ensuring that the hardware used to run this software complies with local regulations and that 
 any radio signal generated or received from use of this software is legal for that user to generate.  The author(s) of this software 
 assume no liability whatsoever.  The author(s) of this software is not responsible for legal or civil consequences of 
 using this software, including, but not limited to, any damages cause by lost control of a vehicle using this software.  
 If this software is copied or modified, this disclaimer must accompany all copies.
 
 This project is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.

Multiprotocol is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.

 You should have received a copy of the GNU General Public License
 along with Multiprotocol.  If not, see <http://www.gnu.org/licenses/>.
 */
 
// The Receiver for this protocol is available at: https://github.com/soligen2010/RC_RX_CABELL_V3_FHSS

#if defined(CABELL_NRF24L01_INO)

#include "iface_nrf24l01.h"

#define CABELL_BIND_COUNT		2000		// At least 2000 so that if TX toggles the serial bind flag then bind mode is never exited
#define CABELL_PACKET_PERIOD	3000		// Do not set too low or else next packet may not be finished transmitting before the channel is changed next time around

#define CABELL_NUM_CHANNELS		16			// The maximum number of RC channels that can be sent in one packet
#define CABELL_MIN_CHANNELS		4			// The minimum number of channels that must be included in a packet, the number of channels cannot be reduced any further than this
#define CABELL_PAYLOAD_BYTES	24			// 12 bits per value * 16 channels

#define CABELL_RADIO_CHANNELS			9	// This is 1/5 of the total number of radio channels used for FHSS
#define CABELL_RADIO_MIN_CHANNEL_NUM	3	// Channel 0 is right on the boarder of allowed frequency range, so move up to avoid bleeding over
#define CABELL_TELEMETRY_PACKET_LENGTH	4

#define CABELL_BIND_RADIO_ADDR	0xA4B7C123F7LL

#define CABELL_OPTION_MASK_CHANNEL_REDUCTION		0x0F
#define CABELL_OPTION_MASK_RECIEVER_OUTPUT_MODE		0x30
#define CABELL_OPTION_SHIFT_RECIEVER_OUTPUT_MODE	4
#define CABELL_OPTION_MASK_MAX_POWER_OVERRIDE		0x40

typedef struct
{
   enum RxMode_t : uint8_t
   {   // Note bit 8 is used to indicate if the packet is the first of 2 on the channel.  Mask out this bit before using the enum
         normal                 = 0,
         bind                   = 1,
         setFailSafe            = 2,
         normalWithTelemetry    = 3,
         telemetryResponse      = 4,
         unBind                 = 127
   } RxMode;
   uint8_t reserved = 0;
   uint8_t option;
                          /*   mask 0x0F    : Channel reduction.  The number of channels to not send (subtracted from the 16 max channels) at least 4 are always sent
                           *   mask 0x30>>4 : Receiver output mode
                           *                  0 (00) = Single PPM on individual pins for each channel 
                           *                  1 (01) = SUM PPM on channel 1 pin
                           *                  2 (10) = Future use.  Reserved for SBUS output
                           *                  3 (11) = Unused
                           *   mask 0x40>>6   Contains max power override flag for Multi-protocol TX module. Also sent to RX
                           *   mask 0x80>>7   Unused 
                           */  
   uint8_t modelNum;
   uint8_t checkSum_LSB; 
   uint8_t checkSum_MSB; 
   uint8_t payloadValue [CABELL_PAYLOAD_BYTES] = {0}; //12 bits per channel value, unsigned
} CABELL_RxTxPacket_t;   

//-----------------------------------------------------------------------------------------
static uint8_t __attribute__((unused)) CABELL_getNextChannel (uint8_t seqArray[], uint8_t seqArraySize, uint8_t prevChannel)
{
	/* Possible channels are in 5 bands, each band comprised of seqArraySize channels
	* seqArray contains seqArraySize elements in the relative order in which we should progress through the band 
	* 
	* Each time the channel is changes, bands change in a way so that the next channel will be in a
	* different non-adjacent band. Both the band changes and the index in seqArray is incremented.
	*/
	prevChannel -= CABELL_RADIO_MIN_CHANNEL_NUM;				// Subtract CABELL_RADIO_MIN_CHANNEL_NUM because it was added to the return value
	if(prevChannel>(seqArraySize * 5))
		prevChannel=seqArraySize * 5;							// Constrain the values just in case something bogus was sent in.

	uint8_t currBand = prevChannel / seqArraySize;             
	uint8_t nextBand = (currBand + 3) % 5;

	uint8_t prevChannalSeqArrayValue = prevChannel % seqArraySize;
	uint8_t prevChannalSeqArrayPosition = 0;
	for (int x = 0; x < seqArraySize; x++)
	{	// Find the position of the previous channel in the array
		if (seqArray[x] == prevChannalSeqArrayValue)
			prevChannalSeqArrayPosition = x;
	}
	uint8_t nextChannalSeqArrayPosition = prevChannalSeqArrayPosition + 1;
	if (nextChannalSeqArrayPosition >= seqArraySize)
		nextChannalSeqArrayPosition = 0;

	return (seqArraySize * nextBand) + seqArray[nextChannalSeqArrayPosition] + CABELL_RADIO_MIN_CHANNEL_NUM;	// Add CABELL_RADIO_MIN_CHANNEL_NUM so we dont use channel 0 as it may bleed below 2.400 GHz
}

//-----------------------------------------------------------------------------------------
#if defined CABELL_HUB_TELEMETRY 
static void __attribute__((unused)) CABELL_get_telemetry()
{
	// calculate TX rssi based on past 250 expected telemetry packets.  Cannot use full second count because telemetry_counter is not large enough
	state++;
	if (state > 250)
	{
		TX_RSSI = telemetry_counter;
		telemetry_counter = 0;
		state = 0;
		telemetry_lost=0;
	}

	// Process incoming telemetry packet of it was received
	if (NRF24L01_ReadReg(NRF24L01_07_STATUS) & _BV(NRF24L01_07_RX_DR)) 
	{ // data received from model
		NRF24L01_ReadPayload(packet, CABELL_TELEMETRY_PACKET_LENGTH);
		if ((packet[0] & 0x7F) == CABELL_RxTxPacket_t::telemetryResponse)	// ignore high order bit in compare because it toggles with each packet
		{
			RX_RSSI = packet[1];	// Packet rate 0 to 255 where 255 is 100% packet rate
			v_lipo1 = packet[2];	// Directly from analog input of receiver, but reduced to 8-bit depth (0 to 255).  Scaling depends on the input to the analog pin of the receiver.
			v_lipo2 = packet[3];	// Directly from analog input of receiver, but reduced to 8-bit depth (0 to 255).  Scaling depends on the input to the analog pin of the receiver.
			telemetry_counter++;      
			if(telemetry_lost==0)
				telemetry_link=1;
		}
	}
	else
	{
		// If no telemetry packet was received then delay by the typical telemetry packet processing time
		// This is done to try to keep the sendPacket process timing more consistent. Since the SPI payload read takes some time
		delayMicroseconds(50);
	}
	NRF24L01_SetTxRxMode(TX_EN);  
	NRF24L01_FlushRx(); 
}
#endif

//-----------------------------------------------------------------------------------------
static void __attribute__((unused)) CABELL_send_packet(uint8_t bindMode)
{  
	#if defined CABELL_HUB_TELEMETRY  
		if (!bindMode && (sub_protocol == CABELL_V3_TELEMETRY))		// check for incoming packet and switch radio back to TX mode if we were listening for telemetry      
			CABELL_get_telemetry();
	#endif

	CABELL_RxTxPacket_t TxPacket;

	uint8_t channelReduction = constrain((option & CABELL_OPTION_MASK_CHANNEL_REDUCTION),0,CABELL_NUM_CHANNELS-CABELL_MIN_CHANNELS);	// Max 12 - cannot reduce below 4 channels
	if (bindMode)
		channelReduction = 0;	// Send full packet to bind as higher channels will contain bind info

	uint8_t packetSize = sizeof(TxPacket) - ((((channelReduction - (channelReduction%2))/ 2)) * 3);		// reduce 3 bytes per 2 channels, but not last channel if it is odd
	uint8_t maxPayloadValueIndex = sizeof(TxPacket.payloadValue) - (sizeof(TxPacket) - packetSize);

	if ((sub_protocol == CABELL_UNBIND) && !bindMode)
	{
		TxPacket.RxMode = CABELL_RxTxPacket_t::unBind;
		TxPacket.option = option;
	}
	else
	{
		if (sub_protocol == CABELL_SET_FAIL_SAFE && !bindMode)
			TxPacket.RxMode = CABELL_RxTxPacket_t::setFailSafe;
		else
		{
			if (bindMode)
				TxPacket.RxMode = CABELL_RxTxPacket_t::bind;        
			else
			{
				switch (sub_protocol)
				{
					case CABELL_V3_TELEMETRY:
						TxPacket.RxMode = CABELL_RxTxPacket_t::normalWithTelemetry;
						break;
					default:
						TxPacket.RxMode = CABELL_RxTxPacket_t::normal;  
						break;
				}      
			}
		}
		TxPacket.option = (bindMode) ? (option & (~CABELL_OPTION_MASK_CHANNEL_REDUCTION)) : option;		//remove channel reduction if in bind mode
	}
 
	rf_ch_num = CABELL_getNextChannel (hopping_frequency,CABELL_RADIO_CHANNELS, rf_ch_num);
	TxPacket.reserved = rf_ch_num & 0x3F;
	TxPacket.modelNum = RX_num;
	uint16_t checkSum = TxPacket.modelNum + TxPacket.option + TxPacket.RxMode  + TxPacket.reserved;		// Start Calculate checksum

	int adjusted_x;
	int payloadIndex = 0;
	uint16_t holdValue;

	for (int x = 0;(x < CABELL_NUM_CHANNELS - channelReduction); x++)
	{
		switch (x)
		{
			case 0	: adjusted_x = ELEVATOR;	break;
			case 1	: adjusted_x = AILERON;		break;
			case 2	: adjusted_x = RUDDER;		break;
			case 3	: adjusted_x = THROTTLE;	break;
			default	: adjusted_x = x;			break;
		}
		holdValue = convert_channel_16b_limit(adjusted_x,1000,2000);				// valid channel values are 1000 to 2000
		if (bindMode)
		{
			switch (adjusted_x)
			{
				case THROTTLE	: holdValue = 1000;					break;      // always set throttle to off when binding for safety
				//tx address sent for bind
				case 11			: holdValue = 1000 + rx_tx_addr[0];	break;
				case 12			: holdValue = 1000 + rx_tx_addr[1];	break;
				case 13			: holdValue = 1000 + rx_tx_addr[2];	break;
				case 14			: holdValue = 1000 + rx_tx_addr[3];	break;
				case 15			: holdValue = 1000 + rx_tx_addr[4];	break;
			}
		}

		// use 12 bits per value
		if (x % 2)
		{ //output channel number is ODD
			holdValue = holdValue<<4;
			payloadIndex--;     
		}
		else
			holdValue &= 0x0FFF;
		TxPacket.payloadValue[payloadIndex] |=  (uint8_t)(holdValue & 0x00FF);
		payloadIndex++;
		TxPacket.payloadValue[payloadIndex] |=  (uint8_t)((holdValue>>8) & 0x00FF);   
		payloadIndex++;
	}

	for(int x = 0; x < maxPayloadValueIndex ; x++)
		checkSum += TxPacket.payloadValue[x];  // Finish Calculate checksum 

	TxPacket.checkSum_MSB = checkSum >> 8;
	TxPacket.checkSum_LSB = checkSum & 0x00FF;

	// Set channel for next transmission
	NRF24L01_WriteReg(NRF24L01_05_RF_CH,rf_ch_num); 

	//NRF24L01_FlushTx();   //just in case things got hung up
	//NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);

	uint8_t* p = reinterpret_cast<uint8_t*>(&TxPacket.RxMode);
	*p &= 0x7F;                  // Make sure 8th bit is clear
	*p |= (packet_count++)<<7;   // This causes the 8th bit of the first byte to toggle with each xmit so consecutive payloads are not identical.
	// This is a work around for a reported bug in clone NRF24L01 chips that mis-took this case for a re-transmit of the same packet.

	NRF24L01_SetPower();
	NRF24L01_WritePayload((uint8_t*)&TxPacket, packetSize);

	#if defined CABELL_HUB_TELEMETRY 
		if (!bindMode && (sub_protocol == CABELL_V3_TELEMETRY))
		{ // switch radio to rx as soon as packet is sent  
			// calculate transmit time based on packet size and data rate of 1MB per sec
			// This is done because polling the status register during xmit caused issues.
			// bits = packst_size * 8  +  73 bits overhead
			// at 250 Kbs per sec, one bit is 4 uS
			// then add 140 uS which is 130 uS to begin the xmit and 10 uS fudge factor
			delayMicroseconds(((((unsigned long)packetSize * 8ul)  +  73ul) * 4ul) + 140ul)   ;
			packet_period = CABELL_PACKET_PERIOD + (constrain(((int16_t)(CABELL_NUM_CHANNELS - channelReduction) - (int16_t)6 ),(int16_t)0 ,(int16_t)10 ) * (int16_t)100);  // increase packet period by 100 us for each channel over 6
			NRF24L01_WriteReg(NRF24L01_00_CONFIG, 0x0F);  // RX mode with 16 bit CRC
		}
		else 
	#endif
			packet_period = CABELL_PACKET_PERIOD;   // Standard packet period when not in telemetry mode.
}

//-----------------------------------------------------------------------------------------
static void __attribute__((unused)) CABELL_getChannelSequence (uint8_t outArray[], uint8_t numChannels, uint64_t permutation)
{
	/* This procedure initializes an array with the sequence progression of channels.
	* This is not the actual channels itself, but the sequence base to be used within bands of
	* channels.
	*
	* There are numChannels! permutations for arranging the channels
	* one of these permutations will be calculated based on the permutation input
	* permutation should be between 1 and numChannels! but the routine will constrain it
	* if these bounds are exceeded.  Typically the radio's unique TX ID should be used.
	*
	* The maximum numChannels is 20.  Anything larger than this will cause the uint64_t
	* variables to overflow, yielding unknown results (possibly infinite loop?).  Therefor
	* this routine constrains the value.
	*/
	uint8_t i;   //iterator counts numChannels
	uint32_t indexOfNextSequenceValue;
	uint32_t numChannelsFactorial=1;
	uint32_t perm32 ;
	uint8_t  sequenceValue;

	numChannels = constrain(numChannels,1,9);

	for (i = 1; i <= numChannels;i++)
	{
		numChannelsFactorial *= i;					//  Calculate n!
		outArray[i-1] = i-1;						//  Initialize array with the sequence
	}

	perm32 = permutation >> 8 ;						// Shift 40 bit input to 32 bit
	perm32 = (perm32 % numChannelsFactorial);		// permutation must be between 1 and n! or this algorithm will infinite loop
	perm32 <<= 8 ;									// Shift back 8 bits
	perm32 += permutation & 0x00FF ;				// Tack on least 8 bits
	perm32 = (perm32 % numChannelsFactorial) + 1;	// permutation must be between 1 and n! or this algorithm will infinite loop

	//Rearrange the array elements based on the permutation selected
	for (i=0, perm32--; i<numChannels; i++ )
	{
		numChannelsFactorial /= numChannels-i;
		indexOfNextSequenceValue = i+(perm32/numChannelsFactorial);
		perm32 %= numChannelsFactorial;

		//Copy the value in the selected array position
		sequenceValue = outArray[indexOfNextSequenceValue];

		//Shift the unused elements in the array to make room to move in the one just selected
		for( ; indexOfNextSequenceValue > i; indexOfNextSequenceValue--)
			outArray[indexOfNextSequenceValue] = outArray[indexOfNextSequenceValue-1];

		// Copy the selected value into it's new array slot
		outArray[i] = sequenceValue;
	}
}

//-----------------------------------------------------------------------------------------
static void __attribute__((unused)) CABELL_setAddress()
{
	uint64_t CABELL_addr;

	//  Serial.print("NORM ID: ");Serial.print((uint32_t)(CABELL_normal_addr>>32)); Serial.print("    ");Serial.println((uint32_t)((CABELL_normal_addr<<32)>>32));

	if (IS_BIND_DONE)
	{
		CABELL_addr = (((uint64_t)rx_tx_addr[0]) << 32) + 
			(((uint64_t)rx_tx_addr[1]) << 24) + 
			(((uint64_t)rx_tx_addr[2]) << 16) + 
			(((uint64_t)rx_tx_addr[3]) << 8) + 
			(((uint64_t)rx_tx_addr[4]));					// Address to use after binding
	}
	else
		CABELL_addr = CABELL_BIND_RADIO_ADDR;				// Static addr for binding

	CABELL_getChannelSequence(hopping_frequency,CABELL_RADIO_CHANNELS,CABELL_addr);		// Get the sequence for hopping through channels
	rf_ch_num = CABELL_RADIO_MIN_CHANNEL_NUM;				// Initialize the channel sequence

	packet_count=0;  

	uint64_t CABELL_Telemetry_addr = ~CABELL_addr;			// Invert bits for reading so that telemetry packets have a different address.

	NRF24L01_WriteRegisterMulti(NRF24L01_0A_RX_ADDR_P0, reinterpret_cast<uint8_t*>(&CABELL_Telemetry_addr), 5);
	NRF24L01_WriteRegisterMulti(NRF24L01_0B_RX_ADDR_P1, reinterpret_cast<uint8_t*>(&CABELL_Telemetry_addr), 5);
	NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR,    reinterpret_cast<uint8_t*>(&CABELL_addr), 5);
}

//-----------------------------------------------------------------------------------------
static void __attribute__((unused)) CABELL_RF_init()
{
	NRF24L01_Initialize();

	NRF24L01_SetBitrate(NRF24L01_BR_250K);				// slower data rate gives better range/reliability
	CABELL_setAddress();
	NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20);		// 32 byte packet length
	NRF24L01_WriteReg(NRF24L01_12_RX_PW_P1, 0x20);		// 32 byte packet length
	NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 0x3F);			// Enable dynamic payload length on all pipes
	NRF24L01_WriteReg(NRF24L01_1D_FEATURE, 0x04);		// Enable dynamic Payload Length

	NRF24L01_SetTxRxMode(TX_EN);						// Clear data ready, data sent, retransmit and enable CRC 16bits, ready for TX
}

//-----------------------------------------------------------------------------------------
uint16_t CABELL_callback()
{
	if (IS_BIND_DONE)
	{
		CABELL_send_packet(0);  // packet_period is set/adjusted in CABELL_send_packet
		#ifdef MULTI_SYNC
			telemetry_set_input_sync(packet_period);
		#endif
		return packet_period;
	}
	else if (bind_counter == 0)
	{
		BIND_DONE;
		CABELL_RF_init();   // non-bind address 
	}
	else
	{
		CABELL_send_packet(1);
		bind_counter--;
	}
	return CABELL_PACKET_PERIOD;
}

//-----------------------------------------------------------------------------------------
void CABELL_init(void)
{
	if (IS_BIND_DONE)
		bind_counter = 0;
	else  
		bind_counter = CABELL_BIND_COUNT;
	CABELL_RF_init();

	packet_period = CABELL_PACKET_PERIOD;
}

#endif