/*
 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/>.
 */

#if defined(KN_NRF24L01_INO)

#include "iface_nrf24l01.h"

#define KN_BIND_COUNT 1000 // for KN 2sec every 2ms - 1000 packets
// Timeout for callback in uSec, 2ms=2000us for KN
#define KN_PACKET_PERIOD 2000
#define KN_PACKET_CHKTIME  100 // Time to wait for packet to be sent (no ACK, so very short)

//#define PAYLOADSIZE 16
#define NFREQCHANNELS 4
#define KN_TXID_SIZE 4


enum {
	KN_FLAG_DR     = 0x01, // Dual Rate
	KN_FLAG_TH     = 0x02, // Throttle Hold
	KN_FLAG_IDLEUP = 0x04, // Idle up
	KN_FLAG_RES1   = 0x08,
	KN_FLAG_RES2   = 0x10,
	KN_FLAG_RES3   = 0x20,
	KN_FLAG_GYRO3  = 0x40, // 00 - 6G mode, 01 - 3G mode
	KN_FLAG_GYROR  = 0x80  // Always 0 so far
};

//
enum {
	KN_INIT2 = 0,
	KN_INIT2_NO_BIND,
	KN_BIND,
	KN_DATA
};

/*enum {
	USE1MBPS_NO  = 0,
	USE1MBPS_YES = 1,
};*/

// 2-bytes CRC
#define CRC_CONFIG (BV(NRF24L01_00_EN_CRC) | BV(NRF24L01_00_CRCO))

void kn_init()
{
	NRF24L01_Initialize();

	NRF24L01_WriteReg(NRF24L01_00_CONFIG, CRC_CONFIG); 
	NRF24L01_WriteReg(NRF24L01_01_EN_AA, 0x00);      // No Auto Acknoledgement
	NRF24L01_WriteReg(NRF24L01_02_EN_RXADDR, 0x01);  // Enable data pipe 0
	NRF24L01_WriteReg(NRF24L01_03_SETUP_AW, 0x03);   // 5-byte RX/TX address
	NRF24L01_WriteReg(NRF24L01_04_SETUP_RETR, 0);    // Disable retransmit
	NRF24L01_SetPower();
	NRF24L01_WriteReg(NRF24L01_07_STATUS, 0x70);     // Clear data ready, data sent, and retransmit
	NRF24L01_WriteReg(NRF24L01_11_RX_PW_P0, 0x20);   // bytes of data payload for pipe 0


    NRF24L01_Activate(0x73);                         // Activate feature register
	NRF24L01_WriteReg(NRF24L01_1C_DYNPD, 1); // Dynamic payload for data pipe 0
	// Enable: Dynamic Payload Length, Payload with ACK , W_TX_PAYLOAD_NOACK
	NRF24L01_WriteReg(NRF24L01_1D_FEATURE, BV(NRF2401_1D_EN_DPL) | BV(NRF2401_1D_EN_ACK_PAY) | BV(NRF2401_1D_EN_DYN_ACK));
    NRF24L01_Activate(0x73);
}

uint16_t kn_init2()
{
	NRF24L01_FlushTx();
	NRF24L01_FlushRx();
	packet_sent = 0;
	packet_count = 0;
	hopping_frequency_no = 0;

	// Turn radio power on
    NRF24L01_SetTxRxMode(TX_EN);
	NRF24L01_WriteReg(NRF24L01_00_CONFIG, CRC_CONFIG | BV(NRF24L01_00_PWR_UP));
	return 150;
}

void set_tx_for_bind()
{
	NRF24L01_WriteReg(NRF24L01_05_RF_CH, 83);
	NRF24L01_SetBitrate(NRF24L01_BR_1M); // 1Mbps for binding
	NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, (uint8_t *) "KNDZK", 5);
}

void set_tx_for_data()
{
	NRF24L01_WriteRegisterMulti(NRF24L01_10_TX_ADDR, rx_tx_addr, 5);
}

void kn_calc_fh_channels(uint32_t seed)
{
	uint8_t idx = 0;
	uint32_t rnd = seed;
	while (idx < NFREQCHANNELS) {
		uint8_t i;
		rnd = rnd * 0x0019660D + 0x3C6EF35F; // Randomization

		// Use least-significant byte. 73 is prime, so channels 76..77 are unused
		uint8_t next_ch = ((rnd >> 8) % 73) + 2;
		// Keep the distance 2 between the channels - either odd or even
		if (((next_ch ^ seed) & 0x01 )== 0)
			continue;
		// Check that it's not duplicate and spread uniformly
		for (i = 0; i < idx; i++) {
			if(hopping_frequency[i] == next_ch)
			break;
		}
		if (i != idx)
			continue;
		hopping_frequency[idx++] = next_ch;
	}
}

void kn_initialize_tx_id()
{
	rx_tx_addr[4] = 'K';
	kn_calc_fh_channels(MProtocol_id);
}

#define PACKET_COUNT_SHIFT 5
#define RF_CHANNEL_SHIFT 2
void kn_send_packet(uint8_t bind)
{
	uint8_t rf_ch;
	if (bind) {
		rf_ch = 83;
		packet[0]  = 'K';
		packet[1]  = 'N';
		packet[2]  = 'D';
		packet[3]  = 'Z';
		packet[4]  = rx_tx_addr[0];
		packet[5]  = rx_tx_addr[1];
		packet[6]  = rx_tx_addr[2];
		packet[7]  = rx_tx_addr[3];
		packet[8]  = hopping_frequency[0];
		packet[9]  = hopping_frequency[1];
		packet[10] = hopping_frequency[2];
		packet[11] = hopping_frequency[3];
		packet[12] = 0x00;
		packet[13] = 0x00;
		packet[14] = 0x00;
		packet[15] = 0x01; //mode_bitrate == USE1MBPS_YES ? 0x01 : 0x00;
	} else {
		rf_ch = hopping_frequency[hopping_frequency_no];

		// Each packet is repeated 4 times on the same channel
		// We're not strictly repeating them, rather we
		// send new packet on the same frequency, so the
		// receiver gets the freshest command. As receiver
		// hops to a new frequency as soon as valid packet
		// received it does not matter that the packet is
		// not the same one repeated twice - nobody checks this

		// NB! packet_count overflow is handled and used in
		// callback.
		if (++packet_count == 4)
			hopping_frequency_no = (hopping_frequency_no + 1) & 0x03;

		uint16_t kn_throttle, kn_rudder, kn_elevator, kn_aileron;
		kn_throttle = convert_channel_10b(THROTTLE);
		kn_aileron  = convert_channel_10b(AILERON);
		kn_elevator = convert_channel_10b(ELEVATOR);
		kn_rudder   = convert_channel_10b(RUDDER);
		packet[0]  = (kn_throttle >> 8) & 0xFF;
		packet[1]  = kn_throttle & 0xFF;
		packet[2]  = (kn_aileron >> 8) & 0xFF;
		packet[3]  = kn_aileron  & 0xFF;
		packet[4]  = (kn_elevator >> 8) & 0xFF;
		packet[5]  = kn_elevator & 0xFF;
		packet[6]  = (kn_rudder >> 8) & 0xFF;
		packet[7]  = kn_rudder & 0xFF;
		// Trims, middle is 0x64 (100) 0-200
		packet[8]  = 0x64; // T
		packet[9]  = 0x64; // A
		packet[10] = 0x64; // E
		packet[11] = 0x64; // R

		if (Servo_data[AUX1] > PPM_SWITCH)
			flags |= KN_FLAG_DR;
		else
			flags=0;
		if (Servo_data[AUX2] > PPM_SWITCH)
			flags |= KN_FLAG_TH;
		if (Servo_data[AUX3] > PPM_SWITCH)
			flags |= KN_FLAG_IDLEUP;
		if (Servo_data[AUX4] > PPM_SWITCH)
			flags |= KN_FLAG_GYRO3;

		packet[12] = flags;

		packet[13] = (packet_count << PACKET_COUNT_SHIFT) | (hopping_frequency_no << RF_CHANNEL_SHIFT);

		packet[14] = 0x00;
		packet[15] = 0x00;
	}

	packet_sent = 0;
	NRF24L01_WriteReg(NRF24L01_05_RF_CH, rf_ch);
	NRF24L01_FlushTx();
	NRF24L01_WritePayload(packet, 16);
	//++total_packets;
	packet_sent = 1;

	if (! hopping_frequency_no) {
		//Keep transmit power updated
		NRF24L01_SetPower();
	}
}

uint16_t kn_callback()
{
	uint16_t timeout = KN_PACKET_PERIOD;
	switch (phase)
	{
		case KN_INIT2:
			bind_counter = KN_BIND_COUNT;
			timeout = kn_init2();
			phase = KN_BIND;
			set_tx_for_bind();
			break;
		case KN_INIT2_NO_BIND:
			timeout = kn_init2();
			phase = KN_DATA;
			set_tx_for_data();
			break;
		case KN_BIND:
			if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
				return KN_PACKET_CHKTIME;
			kn_send_packet(1);
			if (--bind_counter == 0) {
				phase = KN_DATA;
				set_tx_for_data();
				BIND_DONE;			
			}
			break;
		case KN_DATA:
			if (packet_count == 4)
				packet_count = 0;
			else {
				if (packet_sent && NRF24L01_packet_ack() != PKT_ACKED)
					return KN_PACKET_CHKTIME;
				kn_send_packet(0);
			}
			break;
	}
	return timeout;
}

uint16_t initKN(){	
	//total_packets = 0;
	//mode_bitrate=USE1MBPS_YES;
	kn_init();
	phase = IS_AUTOBIND_FLAG_on ? KN_INIT2 : KN_INIT2_NO_BIND;
	kn_initialize_tx_id();

	// Call callback in 50ms
	return 50000;
}

#endif