/*
 * FrSky RX 2-way protocol
 */

#ifdef DEBUG

//#define DEBUG_TEST_PART_VERSION
//#define DEBUG_EEPROM_DATA_READ
//#define DEBUG_EEPROM_DATA_WRITE
//#define DEBUG_MISSING_PACKETS
//#define DEBUG_PACKET_DUMP
//#define DEBUG_PACKET_DUMP_TUNE
//#define DEBUG_PACKET_DUMP_BIND
//#define DEBUG_PACKET_DUMP_LIST
//#define DEBUG_PPM_VALUES
//#define DEBUG_ERROR_MESSAGES
//#define DEBUG_MESSAGES
//#define DEBUG_HOP_DATA

#define DEBUG_SINGLE_CHARACTER_MESSAGES

#define ALWAYS_PERFORM_BINDING
#define DISABLE_REAL_EEPROM_WRITE

#endif

#include <stdint.h>

#if defined(__AVR__)
#include <avr/eeprom.h>
#include <avr/wdt.h>
#endif

#include "cc2500.h"
#include "cppm.h"
#include "spi.h"
#include "timer.h"
#include "rx.h"
#include "main.h"

#define CHANNELS 8
#define HOP_DATA_LENGTH 60
#define EEPROM_BASE_ADDRESS 100
#define MISSING_PACKET_DELAY 8999
#define SEEK_CHANNEL_SKIP 13
#define MAX_MISSING_PACKET 20
#define FAILSAFE_MISSING_PACKET 170

#define FAILSAFE_INVALID_VALUE 850
#define FAILSAFE_INVALID_VALUE_MINIMUM 900
#define FAILSAFE_INVALID_VALUE_MAXIMUM 2100

//#define RSSI_OVER_PPM 7
#define RSSI_OFFSET 71
#define RSSI_MIN -103
#define RSSI_MAX -96
#define PPM_MIN 1000
#define PPM_MAX 2000

#define PPM_SCALE 2 / 3

#define FRSKY_PACKET_LENGTH 0x11

#define FREQ_ORIGINAL
//#define FREQUENCY_SCAN

// Original 0x5C 0x76 0x27
#define FREQ_SCAN_START_2 0x5C
#define FREQ_SCAN_START_1 0x76
#define FREQ_SCAN_START_0 0x00

uint8_t ccData[100]; // was 27, led to buffer overruns?!
uint8_t ccLen;
uint8_t packet = 0;
uint8_t channr = 0;
uint8_t missingPackets = 0;
uint8_t hopData[HOP_DATA_LENGTH];
uint8_t listLength;
uint8_t txid[2];
uint8_t counter = 0;
uint8_t failed = 0;
uint16_t frequencyOffsetHack = 0;
uint16_t c[CHANNELS];

#ifdef FREQUENCY_SCAN
uint8_t freq2 = FREQ_SCAN_START_2;
uint8_t freq1 = FREQ_SCAN_START_1;
uint8_t freq0 = FREQ_SCAN_START_0;
#endif

static uint16_t ppmBuffer[CHANNELS];

#ifdef RSSI_OVER_PPM
static int rssi;

static long map(long x, long in_min, long in_max, long out_min, long out_max);
static long constrain(long x, long min, long max);
#endif

static void initialize(uint8_t bind);
static void binding(void);
static void tuning(void);
static void performBind(void);
static void nextChannel(uint8_t skip);
static void writeBindingData(void);

#ifndef ALWAYS_PERFORM_BINDING
static void readBindingData(void);
#endif

void rxInit(void) {
    GDO_dir;

#ifdef DEBUG_MESSAGES
    debugWrite("RX: initializing\n");
#endif

    initialize(1); // binding

#ifdef DEBUG_MESSAGES
    debugWrite("RX: binding\n");
#endif

    binding();

#ifdef DEBUG_MESSAGES
    debugWrite("RX: receiving\n");
#endif

    initialize(0); // data
    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);// 0A - hop
    cc2500WriteReg(CC2500_23_FSCAL3, 0x89); // 23 - 89
    cc2500Strobe(CC2500_SRX);
}

static void initialize(uint8_t bind) {
    cc2500ResetChip();
    cc2500WriteReg(CC2500_02_IOCFG0,   0x01); // RX complete interrupt(GDO0)
    cc2500WriteReg(CC2500_17_MCSM1,    0x0F);
    cc2500WriteReg(CC2500_18_MCSM0,    0x18);
    cc2500WriteReg(CC2500_06_PKTLEN,   0x19); // Leave room for appended status bytes
    cc2500WriteReg(CC2500_08_PKTCTRL0, 0x05);
    cc2500WriteReg(CC2500_3E_PATABLE,  0xFF);
    cc2500WriteReg(CC2500_0B_FSCTRL1,  0x08);
    cc2500WriteReg(CC2500_0C_FSCTRL0,  0x00);

#ifdef FREQ_ORIGINAL
    /*
     * According to the internet, these are the default FrSky
     * FREQ values (0x5C7627) which should equal 2.404GHz.
     * The datasheet mentions Fcarrier = Fxosc * FREQ / 2^16
     * Therefore, FrSky seems to be using 26MHz too.
     */
    cc2500WriteReg(CC2500_0D_FREQ2,    0x5C);
    cc2500WriteReg(CC2500_0E_FREQ1,    0x76);
    cc2500WriteReg(CC2500_0F_FREQ0,    0x27);
#else
    cc2500WriteReg(CC2500_0D_FREQ2,    FREQ_SCAN_START_2);
    cc2500WriteReg(CC2500_0E_FREQ1,    FREQ_SCAN_START_1);
    cc2500WriteReg(CC2500_0F_FREQ0,    FREQ_SCAN_START_0);
#endif

    cc2500WriteReg(CC2500_10_MDMCFG4,  0xAA);
    cc2500WriteReg(CC2500_11_MDMCFG3,  0x39);
    cc2500WriteReg(CC2500_12_MDMCFG2,  0x11);
    cc2500WriteReg(CC2500_13_MDMCFG1,  0x23);
    cc2500WriteReg(CC2500_14_MDMCFG0,  0x7A);
    cc2500WriteReg(CC2500_15_DEVIATN,  0x42);
    cc2500WriteReg(CC2500_19_FOCCFG,   0x16);
    cc2500WriteReg(CC2500_1A_BSCFG,    0x6C);
    cc2500WriteReg(CC2500_1B_AGCCTRL2, 0x03);
    cc2500WriteReg(CC2500_1C_AGCCTRL1, 0x40);
    cc2500WriteReg(CC2500_1D_AGCCTRL0, 0x91);
    cc2500WriteReg(CC2500_21_FREND1,   0x56);
    cc2500WriteReg(CC2500_22_FREND0,   0x10);
    cc2500WriteReg(CC2500_23_FSCAL3,   0xA9);
    cc2500WriteReg(CC2500_24_FSCAL2,   0x05);
    cc2500WriteReg(CC2500_25_FSCAL1,   0x00);
    cc2500WriteReg(CC2500_26_FSCAL0,   0x11);
    //cc2500WriteReg(CC2500_29_FSTEST,   0x59);
    //cc2500WriteReg(CC2500_2C_TEST2,    0x88);
    //cc2500WriteReg(CC2500_2D_TEST1,    0x31);
    //cc2500WriteReg(CC2500_2E_TEST0,    0x0B);
    //cc2500WriteReg(CC2500_03_FIFOTHR,  0x0F); // 0x07?

    cc2500WriteReg(CC2500_09_ADDR, bind ? 0x03 : txid[0]);

#ifdef DEBUG_MESSAGES
    debugWrite("CC2500: Entering IDLE mode...\n");
#endif

    cc2500Strobe(CC2500_SIDLE); // Go to idle...

#ifdef DEBUG_TEST_PART_VERSION
    uint8_t part = cc2500ReadReg(CC2500_30_PARTNUM);
    uint8_t version = cc2500ReadReg(CC2500_31_VERSION);

    debugWrite("CC2500: Part Number: 0x");
    debugHex(part);
    debugWrite("\nCC2500: Version: 0x");
    debugHex(version);
    debugWrite("\n");

    if ((part != 0x80) || (version != 0x03)) {
        debugWrite("INVALID CC2500 STATUS! IT MAY BE DEAD!\n");
    }
#endif

    // hack: Append status, filter by address, auto-flush on bad crc, PQT=0
    cc2500WriteReg(CC2500_07_PKTCTRL1, 0x0D);

    //cc2500WriteReg(CC2500_0C_FSCTRL0, 0); // Frequency offset
    cc2500WriteReg(CC2500_0C_FSCTRL0, bind ? 0x00 : frequencyOffsetHack);

    cc2500WriteReg(CC2500_0A_CHANNR, 0x00);
}

static void binding(void) {
#ifndef ALWAYS_PERFORM_BINDING
    readBindingData();
    if ((txid[0] != 0xff) || (txid[1] != 0xff)) {
        // valid binding data found
        debugWrite("RX: found data in EEPROM!\n");
        return;
    }
#endif

#ifdef DEBUG_MESSAGES
    debugWrite("RX: no stored data, tuning...\n");
#endif

    // No valid txid, forcing bind
    tuning();

    //frequencyOffsetHack = 0xC8; // for test
    cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);

#ifndef DISABLE_REAL_EEPROM_WRITE
    eeprom_write_byte(EEPROM_BASE_ADDRESS + 101, frequencyOffsetHack);
#endif

#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("RX: Write EEPROM Frequency Offset Hack: ");
    debugUnsigned8(frequencyOffsetHack);
    debugWrite("\n");
#endif

#ifdef DEBUG_MESSAGES
    debugWrite("RX: tuned, binding...\n");
#endif

    performBind();
}

static void tuning() {
    cc2500Strobe(CC2500_SRX); // enter in rx mode

    uint16_t frequencyOffsetTimer = 0;
    while (1) {
        frequencyOffsetTimer++;
        if (frequencyOffsetHack >= 250) {
            frequencyOffsetHack = 0;

#ifdef FREQUENCY_SCAN
            if (freq0 < 255) {
                freq0++;
            } else {
                freq0 = 0;
                if (freq1 < 255) {
                    freq1++;
                } else {
                    freq1 = 0;
                    if (freq2 < 255) {
                        freq2++;
                    } else {
                        freq2 = 0;
                    }
                }
            }

            debugWrite("0x");
            debugHex(freq2);
            debugHex(freq1);
            debugHex(freq0);
            debugWrite("\n");

            cc2500WriteReg(CC2500_0D_FREQ2, freq2);
            cc2500WriteReg(CC2500_0E_FREQ1, freq1);
            cc2500WriteReg(CC2500_0F_FREQ0, freq0);
#endif

        }

        // TODO use timer here?
        if (frequencyOffsetTimer > 3000) {
            frequencyOffsetTimer = 0;
            //cc2500WriteReg(CC2500_0C_FSCTRL0, frequencyOffsetHack);
            //frequencyOffsetHack += 10;
            //cc2500Strobe(CC2500_SRX); // enter in rx mode
        }

        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
            if (ccLen) {
                cc2500ReadFifo(ccData, ccLen);

#ifdef DEBUG_PACKET_DUMP_TUNE
                debugWrite("RX: Tune Packet Dump (");
                debugUnsigned8(ccLen);
                debugWrite("):\n");
                for (uint8_t i = 0; i < ccLen; i++) {
                    debugWrite("0x");
                    debugHex(ccData[i]);
                    if (i < (ccLen - 1)) {
                        debugWrite(" ");
                    } else {
                        debugWrite("\n");
                    }
                }
#endif

                if ((ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)
                        && (ccData[2] == 0x01)
                        && (ccData[5] == 0x00)) {
#ifdef DEBUG_MESSAGES
                    debugWrite("RX: Tune: finished at: ");
                    debugUnsigned8(frequencyOffsetHack);
                    debugWrite("!\n");
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    debugChar('t');
#endif
                    break;
                } else {
#ifdef DEBUG_ERROR_MESSAGES
                    if (!(ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)) {
                        debugWrite("RX: Tune: invalid CRC!\n");
                    }
                    if (ccData[2] != 0x01) {
                        debugWrite("RX: Tune: invalid data[2]: ");
                        debugUnsigned8(ccData[2]);
                        debugWrite(" != ");
                        debugUnsigned8(0x01);
                        debugWrite("\n");
                    }
                    if (ccData[5] != 0x00) {
                        debugWrite("RX: Tune: invalid data[5]: ");
                        debugUnsigned8(ccData[5]);
                        debugWrite(" != ");
                        debugUnsigned8(0x00);
                        debugWrite("\n");
                    }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    debugChar('u');
#endif
                }
            }
        }

#if defined(__AVR__)
        wdt_reset();
#endif

#ifdef DEBUG_UART_MENU
        uartMenu();
#endif
    }
}

static void performBind(void) {
    cc2500Strobe(CC2500_SRX); // enter in rx mode

    while (1) {
        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
            if (ccLen) {
                cc2500ReadFifo(ccData, ccLen);

#ifdef DEBUG_PACKET_DUMP_BIND
                debugWrite("RX: Bind Packet Dump (");
                debugUnsigned8(ccLen);
                debugWrite("):\n");
                for (uint8_t i = 0; i < ccLen; i++) {
                    debugWrite("0x");
                    debugHex(ccData[i]);
                    if (i < (ccLen - 1)) {
                        debugWrite(" ");
                    } else {
                        debugWrite("\n");
                    }
                }
#endif

                if ((ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)
                        && (ccData[2] == 0x01)
                        && (ccData[5] == 0x00)) {
                    txid[0] = ccData[3];
                    txid[1] = ccData[4];
#ifdef DEBUG_MESSAGES
                    debugWrite("RX: Bind: got txid: 0x");
                    debugHex(txid[0]);
                    debugWrite(" 0x");
                    debugHex(txid[1]);
                    debugWrite("\n");
#endif
#ifdef DEBUG_HOP_DATA
                    debugWrite("RX: Bind: got hop list:\n");
#endif
                    for (uint8_t n = 0; n < 5; n++) {
                        hopData[ccData[5] + n] = ccData[6 + n];
#ifdef DEBUG_HOP_DATA
                        debugUnsigned8(ccData[5] + n);
                        debugWrite(": ");
                        debugUnsigned8(ccData[6 + n]);
                        debugWrite("\n");
#endif
                    }
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    debugChar('b');
#endif
                    break;
                } else {
#ifdef DEBUG_ERROR_MESSAGES
                    if (!(ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)) {
                        debugWrite("RX: Bind: invalid CRC!\n");
                    }
                    if (ccData[2] != 0x01) {
                        debugWrite("RX: Bind: invalid data[2]: ");
                        debugUnsigned8(ccData[2]);
                        debugWrite(" != ");
                        debugUnsigned8(0x01);
                        debugWrite("\n");
                    }
                    if (ccData[5] != 0x00) {
                        debugWrite("RX: Bind: invalid data[5]: ");
                        debugUnsigned8(ccData[5]);
                        debugWrite(" != ");
                        debugUnsigned8(0x00);
                        debugWrite("\n");
                    }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    debugChar('c');
#endif
                }
            }
        }

#if defined(__AVR__)
        wdt_reset();
#endif

#ifdef DEBUG_UART_MENU
        uartMenu();
#endif
    }

#ifdef DEBUG_MESSAGES
    debugWrite("RX: got hop data, reading list...\n");
#endif

    listLength = 0;
    uint8_t eol = 0;
    for (uint8_t bindIdx = 0x05; bindIdx <= 120; bindIdx += 5) {
        while (1) {
            if (GDO_1) {
                ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
                if (ccLen) {
                    cc2500ReadFifo(ccData, ccLen);

#ifdef DEBUG_PACKET_DUMP_LIST
                    debugWrite("RX: List Packet Dump (");
                    debugUnsigned8(ccLen);
                    debugWrite("):\n");
                    for (uint8_t i = 0; i < ccLen; i++) {
                        debugWrite("0x");
                        debugHex(ccData[i]);
                        if (i < (ccLen - 1)) {
                            debugWrite(" ");
                        } else {
                            debugWrite("\n");
                        }
                    }
#endif

                    if ((ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)
                            && (ccData[2] == 0x01)
                            && (ccData[3] == txid[0])
                            && (ccData[4] == txid[1])
                            && (ccData[5] == bindIdx)) {
#ifdef DEBUG_HOP_DATA
                        debugWrite("RX: List: got hop list:\n");
#endif
                        for (uint8_t n = 0; n < 5; n++) {
                            //if (ccData[6 + n] == ccData[ccLen - 3]) {
                              if (ccData[6 + n] <= 3) {
#ifdef DEBUG_HOP_DATA
                                  debugWrite("End of List!\n");
#endif
                                eol = 1;
                                listLength = ccData[5] + n;
                                break;
                            }
                            hopData[ccData[5] + n] = ccData[6 + n];
#ifdef DEBUG_HOP_DATA
                            debugUnsigned8(ccData[5] + n);
                            debugWrite(": ");
                            debugUnsigned8(ccData[6 + n]);
                            debugWrite("\n");
#endif
                        }
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                        debugChar('l');
#endif
                        break;
                    } else {
#ifdef DEBUG_ERROR_MESSAGES
                        if (!(ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM)) {
                            debugWrite("RX: List: invalid CRC!\n");
                        }
                        if (ccData[2] != 0x01) {
                            debugWrite("RX: List: invalid data[2]: ");
                            debugUnsigned8(ccData[2]);
                            debugWrite(" != ");
                            debugUnsigned8(0x01);
                            debugWrite("\n");
                        }
                        if (ccData[3] != txid[0]) {
                            debugWrite("RX: List: invalid data[3]: ");
                            debugUnsigned8(ccData[3]);
                            debugWrite(" != ");
                            debugUnsigned8(txid[0]);
                            debugWrite("\n");
                        }
                        if (ccData[4] != txid[1]) {
                            debugWrite("RX: List: invalid data[4]: ");
                            debugUnsigned8(ccData[4]);
                            debugWrite(" != ");
                            debugUnsigned8(txid[1]);
                            debugWrite("\n");
                        }
                        if (ccData[5] != bindIdx) {
                            debugWrite("RX: List: invalid data[5]: ");
                            debugUnsigned8(ccData[5]);
                            debugWrite(" != ");
                            debugUnsigned8(bindIdx);
                            debugWrite("\n");
                        }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                        debugChar('m');
#endif
                    }
                }
            }

#if defined(__AVR__)
            wdt_reset();
#endif

#ifdef DEBUG_UART_MENU
            uartMenu();
#endif
        }

        if (eol) {
            break; // End of list found, stop!
        }
    }

#ifdef DEBUG_MESSAGES
    debugWrite("RX: binding finished!\n");
#endif

    writeBindingData();
    cc2500Strobe(CC2500_SIDLE); // Back to idle
}

static void nextChannel(uint8_t skip) {
    channr += skip;
    if (channr >= listLength) {
        channr -= listLength;
    }

    cc2500WriteReg(CC2500_0A_CHANNR, hopData[channr]);
    cc2500WriteReg(CC2500_23_FSCAL3, 0x89);
}

#ifndef ALWAYS_PERFORM_BINDING
static void readBindingData() {
#ifdef DEBUG_EEPROM_DATA_READ
    debugWrite("RX: Read EEPROM TXID: ");
#endif
    for (uint8_t i = 0; i < 2; i++) {
        txid[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + i);
#ifdef DEBUG_EEPROM_DATA_READ
        debugWrite("0x");
        debugHex(txid[i]);
        debugWrite(" ");
#endif
    }
#ifdef DEBUG_EEPROM_DATA_READ
    debugWrite("\n");
#endif

#ifdef DEBUG_EEPROM_DATA_READ
    debugWrite("RX: Read EEPROM Hop Data (");
    debugUnsigned8(HOP_DATA_LENGTH);
    debugWrite(")\n");
#endif
    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
        hopData[i] = eeprom_read_byte(EEPROM_BASE_ADDRESS + 10 + i);
#ifdef DEBUG_EEPROM_DATA_READ
        debugWrite("0x");
        debugHex(hopData[i]);
        debugWrite(" ");
#endif
    }
#ifdef DEBUG_EEPROM_DATA_READ
    debugWrite("\n");
#endif

    listLength = eeprom_read_byte(EEPROM_BASE_ADDRESS + 100);
    frequencyOffsetHack = eeprom_read_byte(EEPROM_BASE_ADDRESS + 101);

#ifdef DEBUG_EEPROM_DATA_READ
    debugWrite("RX: Read EEPROM List Length: ");
    debugUnsigned8(listLength);
    debugWrite("\nRX: Read EEPROM Frequency Offset Hack: ");
    debugUnsigned8(frequencyOffsetHack);
    debugWrite("\n");
#endif
}
#endif

static void writeBindingData() {
#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("RX: Write EEPROM TXID: ");
#endif
    for (uint8_t i = 0; i < 2; i++) {
#ifndef DISABLE_REAL_EEPROM_WRITE
        eeprom_write_byte(EEPROM_BASE_ADDRESS + i, txid[i]);
#endif
#ifdef DEBUG_EEPROM_DATA_WRITE
        debugWrite("0x");
        debugHex(txid[i]);
        debugWrite(" ");
#endif
    }
#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("\n");
#endif

#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("RX: Write EEPROM Hop Data (");
    debugUnsigned8(HOP_DATA_LENGTH);
    debugWrite(")\n");
#endif
    for (uint8_t i = 0; i < HOP_DATA_LENGTH; i++) {
#ifndef DISABLE_REAL_EEPROM_WRITE
        eeprom_write_byte(EEPROM_BASE_ADDRESS + 10 + i, hopData[i]);
#endif
#ifdef DEBUG_EEPROM_DATA_WRITE
        debugWrite("0x");
        debugHex(hopData[i]);
        debugWrite(" ");
#endif
    }
#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("\n");
#endif

#ifndef DISABLE_REAL_EEPROM_WRITE
    eeprom_write_byte(EEPROM_BASE_ADDRESS + 100, listLength);
#endif
#ifdef DEBUG_EEPROM_DATA_WRITE
    debugWrite("RX: Write EEPROM List Length: ");
    debugUnsigned8(listLength);
    debugWrite("\n");
#endif
}

void rxReceivePacket() {
    time_t time = timerGet();

#if defined(DEBUG_MISSING_PACKETS) || defined(DEBUG_SINGLE_CHARACTER_MESSAGES)
    static uint8_t packetMissingNotification = 0;
#endif

    if (missingPackets > FAILSAFE_MISSING_PACKET) {
        failed = 1;
        missingPackets = 0;
    }

    while (1) {
        if ((timerGet() - time) > MISSING_PACKET_DELAY) {
            missingPackets++;
            cc2500Strobe(CC2500_SIDLE);
            if (missingPackets > MAX_MISSING_PACKET) {
                nextChannel(SEEK_CHANNEL_SKIP);
                counter++;

                if (counter > (MAX_MISSING_PACKET << 1)) {
#ifdef DEBUG_MISSING_PACKETS
                    if (packetMissingNotification < 1) {
                        debugWrite("RX: missing packet notification!\n");
                        packetMissingNotification = 1;
                    }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    if (packetMissingNotification < 1) {
                        debugChar('1');
                        packetMissingNotification = 1;
                    }
#endif
                }

                if (counter == (MAX_MISSING_PACKET << 2)) {
#ifdef DEBUG_MISSING_PACKETS
                    if (packetMissingNotification < 2) {
                        debugWrite("RX: missing packet notification 2!\n");
                        packetMissingNotification = 2;
                    }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    if (packetMissingNotification < 2) {
                        debugChar('2');
                        packetMissingNotification = 2;
                    }
#endif
                    counter = 0;
                }
            } else {
                nextChannel(1);
            }
            break;
        }

        if (GDO_1) {
            ccLen = cc2500ReadReg(CC2500_3B_RXBYTES) & 0x7F;
            if (ccLen > 20) {
#ifdef DEBUG_ERROR_MESSAGES
                debugWrite("RX: packet too long: ");
                debugUnsigned8(ccLen);
                debugWrite("\n");
#endif
                ccLen = 20;
            }
            if (ccLen) {
                cc2500ReadFifo((uint8_t *)ccData, ccLen);
                if (ccData[ccLen - 1] & CC2500_LQI_CRC_OK_BM) { // Only if correct CRC

#ifdef DEBUG_PACKET_DUMP
                    debugWrite("RX: Packet Dump (");
                    debugUnsigned8(ccLen);
                    debugWrite("):\n");
                    for (uint8_t i = 0; i < ccLen; i++) {
                        debugWrite("0x");
                        debugHex(ccData[i]);
                        if (i < (ccLen - 1)) {
                            debugWrite(" ");
                        } else {
                            debugWrite("\n");
                        }
                    }
#endif

                    missingPackets = 0;
                    if ((ccData[0] == FRSKY_PACKET_LENGTH) // Correct length
                            && (ccData[1] == txid[0]) // Correct txid
                            && (ccData[2] == txid[1])) {
#if defined(DEBUG_MISSING_PACKETS) || defined(DEBUG_SINGLE_CHARACTER_MESSAGES)
                        packetMissingNotification = 0;
#endif

                        packet = 1;

#ifdef RSSI_OVER_PPM
                        int rssi_dec = cc2500ReadReg(CC2500_34_RSSI);
                        if (rssi_dec < 128) {
                            rssi = ((rssi_dec / 2) - RSSI_OFFSET) & 0x7f;
                        } else {
                            rssi = (((rssi_dec - 256) / 2)) - RSSI_OFFSET;
                        }
                        rssi = constrain(rssi, RSSI_MIN, RSSI_MAX);
#endif

                        cc2500Strobe(CC2500_SIDLE);
                        nextChannel(1);
                        failed = 0;
                        break;
                    } else {
#ifdef DEBUG_ERROR_MESSAGES
                        if (ccData[0] != FRSKY_PACKET_LENGTH) {
                            debugWrite("RX: invalid packet length: ");
                            debugUnsigned8(ccData[0]);
                            debugWrite(" != ");
                            debugUnsigned8(FRSKY_PACKET_LENGTH);
                            debugWrite("\n");
                        }
                        if (ccData[1] != txid[0]) {
                            debugWrite("RX: invalid txid[0]: ");
                            debugUnsigned8(ccData[1]);
                            debugWrite(" != ");
                            debugUnsigned8(txid[0]);
                            debugWrite("\n");
                        }
                        if (ccData[2] == txid[1]) {
                            debugWrite("RX: invalid txid[1]: ");
                            debugUnsigned8(ccData[2]);
                            debugWrite(" != ");
                            debugUnsigned8(txid[1]);
                            debugWrite("\n");
                        }
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                        debugChar('r');
#endif
                    }
                } else {
#ifdef DEBUG_ERROR_MESSAGES
                    debugWrite("RX: invalid CRC!\n");
#endif
#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
                    debugChar('q');
#endif
                }
            }
        }

#if defined(__AVR__)
        wdt_reset();
#endif

#ifdef DEBUG_UART_MENU
        uartMenu();
#endif
    }

    if (packet != 0) {
        packet = 0;
        c[0] = (uint16_t)(ccData[10] & 0x0F) << 8 | ccData[6];
        c[1] = (uint16_t)(ccData[10] & 0xF0) << 4 | ccData[7];
        c[2] = (uint16_t)(ccData[11] & 0x0F) << 8 | ccData[8];
        c[3] = (uint16_t)(ccData[11] & 0xF0) << 4 | ccData[9];
        c[4] = (uint16_t)(ccData[16] & 0x0F) << 8 | ccData[12];
        c[5] = (uint16_t)(ccData[16] & 0xF0) << 4 | ccData[13];
        c[6] = (uint16_t)(ccData[17] & 0x0F) << 8 | ccData[14];
        c[7] = (uint16_t)(ccData[17] & 0xF0) << 4 | ccData[15];

        for (int i = 0; i < CHANNELS; i++) {
            ppmBuffer[i] = (uint16_t)((uint32_t)(c[i]) * PPM_SCALE);
            if (ppmBuffer[i] < FAILSAFE_INVALID_VALUE_MINIMUM) {
#ifdef FAILSAFE_INVALID_VALUE
                ppmBuffer[i] = FAILSAFE_INVALID_VALUE;
#else
                ppmBuffer[i] = FAILSAFE_INVALID_VALUE_MINIMUM;
#endif
            }

            if (ppmBuffer[i] > FAILSAFE_INVALID_VALUE_MAXIMUM) {
#ifdef FAILSAFE_INVALID_VALUE
                ppmBuffer[i] = FAILSAFE_INVALID_VALUE;
#else
                ppmBuffer[i] = FAILSAFE_INVALID_VALUE_MAXIMUM;
#endif
            }
        }

#ifdef RSSI_OVER_PPM
        ppmBuffer[RSSI_OVER_PPM] = map(rssi, RSSI_MIN, RSSI_MAX, PPM_MIN, PPM_MAX);
#endif

#ifdef DEBUG_PPM_VALUES
        debugWrite("RX: got packet @ ");
        debugUnsigned64(timerGet() / 1000000);
        debugWrite("s:\n");
        for (uint8_t i = 0; i < CHANNELS; i++) {
            debugUnsigned8(i);
            debugWrite(": ");
            debugUnsigned16(ppmBuffer[i]);
            debugWrite(" - ");
            debugUnsigned16(c[i]);
            debugWrite("\n");
        }
#endif

#ifdef DEBUG_SINGLE_CHARACTER_MESSAGES
        debugChar('p');
#endif

        cppmCopy(ppmBuffer);
    }

    cc2500Strobe(CC2500_SRX);
}

#ifdef RSSI_OVER_PPM
static long map(long x, long in_min, long in_max, long out_min, long out_max) {
    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

static long constrain(long x, long min, long max) {
    if (x < min) {
        x = min;
    }
    if (x > max) {
        x = max;
    }
    return x;
}
#endif