/***** Includes *****/
/* Standard C Libraries */
#include <stdlib.h>
#include <unistd.h>
#include <string.h>

/* TI Drivers */
#include <ti/drivers/rf/RF.h>
#include <ti/drivers/GPIO.h>
#include <ti/drivers/UART2.h>

/* Driverlib Header files */
#include DeviceFamily_constructPath(driverlib/rf_prop_mailbox.h)

/* Board Header files */
#include "ti_drivers_config.h"
#include "RFQueue.h"
#include <ti_radio_config.h>

/***** Defines *****/
#define DATA_ENTRY_HEADER_SIZE 8
#define MAX_LENGTH             30
#define NUM_DATA_ENTRIES       2
#define NUM_APPENDED_BYTES     2

//#define PAYLOAD_LENGTH         7
#define PAYLOAD_LENGTH         6
#define PACKET_INTERVAL        500000

// RSSI offset correction - adjusted to match SmartRF consistent reading
// SmartRF shows consistent -47dBm, so we need to match that
//#define RSSI_OFFSET            32  // Increased to get closer to -47dBm target
#define RSSI_OFFSET            31  // Increased to get closer to -47dBm target
// RSSI averaging for stable readings like SmartRF
#define RSSI_SAMPLES           4
static int8_t rssiBuffer[RSSI_SAMPLES];
static uint8_t rssiIndex = 0;
static bool rssiBufferFull = false;

/***** Global Variables *****/
static RF_Object rfObject;
static RF_Handle rfHandle;

/* RX Data Entry Buffer */
#if defined(__TI_COMPILER_VERSION__)
#pragma DATA_ALIGN(rxDataEntryBuffer, 4);
static uint8_t rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES, MAX_LENGTH, NUM_APPENDED_BYTES)];
#elif defined(__IAR_SYSTEMS_ICC__)
#pragma data_alignment = 4
static uint8_t rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES, MAX_LENGTH, NUM_APPENDED_BYTES)];
#elif defined(__GNUC__)
static uint8_t rxDataEntryBuffer[RF_QUEUE_DATA_ENTRY_BUFFER_SIZE(NUM_DATA_ENTRIES, MAX_LENGTH, NUM_APPENDED_BYTES)] __attribute__((aligned(4)));
#else
#error This compiler is not supported.
#endif

static dataQueue_t dataQueue;
static rfc_dataEntryGeneral_t* currentDataEntry;
static uint8_t packetLength;
static uint8_t* packetDataPointer;

static uint8_t rxPacket[MAX_LENGTH + NUM_APPENDED_BYTES - 1];
static uint8_t txPacket[MAX_LENGTH + NUM_APPENDED_BYTES - 1];

static UART2_Handle uart;

/* RSSI tracking - corrected approach */
static rfc_propRxOutput_t rxStatistics_prop;
static int8_t currentRssi = 0;

/***** Function Prototypes *****/
static void rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
static int8_t calculateRssi(int8_t rawRssi);

/***** RSSI Calculation Function *****/
static int8_t calculateRssi(int8_t rawRssi)
{
    // Add to rolling average buffer
    rssiBuffer[rssiIndex] = rawRssi;
    rssiIndex = (rssiIndex + 1) % RSSI_SAMPLES;
    if (rssiIndex == 0) rssiBufferFull = true;

    // Calculate average if we have enough samples
    if (rssiBufferFull) {
        int16_t sum = 0;
        for (int i = 0; i < RSSI_SAMPLES; i++) {
            sum += rssiBuffer[i];
        }
        rawRssi = (int8_t)(sum / RSSI_SAMPLES);
    }

    // Apply offset correction
    int8_t correctedRssi = rawRssi + RSSI_OFFSET;

    return correctedRssi;
}

/***** Main Thread *****/
void *mainThread(void *arg0)
{
    RF_Params rfParams;
    RF_Params_init(&rfParams);

    UART2_Params uartParams;
    UART2_Params_init(&uartParams);
    uartParams.baudRate = 115200;

    uart = UART2_open(CONFIG_UART2_0, &uartParams);
    UART2_write(uart, "\r\nUART OPEN - RSSI Corrected Version\r\n", strlen("\r\nUART OPEN - RSSI Corrected Version\r\n"), NULL);

    GPIO_setConfig(CONFIG_GPIO_RLED, GPIO_CFG_OUT_STD | GPIO_CFG_OUT_LOW);
    GPIO_setConfig(CONFIG_GPIO_GLED, GPIO_CFG_OUT_STD | GPIO_CFG_OUT_LOW);
    GPIO_write(CONFIG_GPIO_RLED, CONFIG_GPIO_LED_OFF);
    GPIO_write(CONFIG_GPIO_GLED, CONFIG_GPIO_LED_OFF);

    if (RFQueue_defineQueue(&dataQueue, rxDataEntryBuffer, sizeof(rxDataEntryBuffer),
                            NUM_DATA_ENTRIES, MAX_LENGTH + NUM_APPENDED_BYTES)) {
        while (1); // RF Queue allocation failed
    }

    /* Set up RX command */
    RF_cmdPropRx.pQueue = &dataQueue;
    RF_cmdPropRx.rxConf.bAutoFlushIgnored = 1;
    RF_cmdPropRx.rxConf.bAutoFlushCrcErr = 1;
    RF_cmdPropRx.maxPktLen = MAX_LENGTH;
    RF_cmdPropRx.pOutput = (uint8_t*)&rxStatistics_prop;

    /* Set up TX command */
    RF_cmdPropTx.pktLen = PAYLOAD_LENGTH;
    RF_cmdPropTx.pPkt = txPacket;
    RF_cmdPropTx.startTrigger.triggerType = TRIG_NOW;

#if defined(DeviceFamily_CC26X0R2)
    rfHandle = RF_open(&rfObject, &RF_prop,
                       (RF_RadioSetup*)&RF_cmdPropRadioSetup, &rfParams);
#else
    rfHandle = RF_open(&rfObject, &RF_prop,
                       (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);
#endif

    RF_postCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);

    while (1) {
        RF_EventMask rxTermination = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropRx,
                                               RF_PriorityNormal, &rxCallback, RF_EventRxEntryDone);

        if (rxTermination & RF_EventLastCmdDone) {
            // RX done - RSSI is now available and processed in callback
        } else {
            // Handle timeout/abort if needed
        }

        /* Prepare TX packet in response */
        txPacket[2] = 0xf0;
        txPacket[3] = 0x03;
        txPacket[4] = rxPacket[4];  // Echo received data
        txPacket[5] = 0xfe;
        //txPacket[5] = (uint8_t)(currentRssi);   // Embed corrected RSSI value
       // txPacket[6] = 0xfe;

        RF_EventMask txTermination = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx,
                                               RF_PriorityNormal, NULL, 0);
        if (txTermination & RF_EventLastCmdDone) {
            GPIO_toggle(CONFIG_GPIO_GLED); // TX successful
        }

        usleep(PACKET_INTERVAL);
    }
}

/***** RX Callback - WITH RSSI AVERAGING *****/
static void rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    if (e & RF_EventRxEntryDone) {
        GPIO_toggle(CONFIG_GPIO_RLED); // RX success

        currentDataEntry = RFQueue_getDataEntry();
        packetLength = *(uint8_t*)(&currentDataEntry->data);
        packetDataPointer = (uint8_t*)(&currentDataEntry->data + 1);
        memcpy(rxPacket, packetDataPointer, packetLength);

        // Get raw RSSI from statistics (most reliable)
        int8_t rawRssi = rxStatistics_prop.lastRssi;
        // Calculate instantaneous corrected RSSI (matches SmartRF)
               int8_t instantRssi = rawRssi + RSSI_OFFSET;

               // Calculate averaged RSSI for stability
               int8_t averagedRssi = calculateRssi(rawRssi);
        // Calculate averaged and corrected RSSI
        currentRssi = calculateRssi(rawRssi);

        RFQueue_nextEntry();

        // Print both instantaneous and averaged RSSI
        char rssiStr[80];
        snprintf(rssiStr, sizeof(rssiStr), "Raw: %d, Instant: %d, Averaged: %d dBm\r\n",
                rawRssi, rawRssi + RSSI_OFFSET, currentRssi);
        UART2_write(uart, rssiStr, strlen(rssiStr), NULL);
    }
}