//============================================================================
//  Title   : radio_interface
//  Desc    : Nordic radio 802.15.4 interface functions
//  2022-8-5   Glen     Created
//============================================================================
// Documentation on the 802.15.4 driver here:
// https://developer.nordicsemi.com/nRF_Connect_SDK/doc/1.9.1/nrfxlib/nrf_802154/README.html
//
// Sample code used for low level radio functions, rx pool, and how to receive
// are in the SDK here: \nRF_Connect\v2.0.0\nrf\samples\peripheral\802154_phy_test\src
//
//== Includes ================================================================
#include "radio_interface.h"
#include <string.h>
#include <zephyr/kernel.h>
#include <zephyr/sys/atomic.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/sys/printk.h>
#include <hal/nrf_radio.h>
#include <string.h>
#include <stdio.h>
#include "radio.h"
#include "iomapping.h"

//== Definitions and Types ===================================================
//#define NO_TRANSMIT_FCC_VERSION                  1
#define RF_FCS_SIZE                                (2u)
#define POWER_OUTPUT_LEVEL_INTO_AMPLIFIER          0      // dBm
#define POWER_INCREASE_AMPLIFIER                   22     // dBm

typedef uint16_t ptt_pkt_len_t;

struct rf_rx_pkt_s {
	uint8_t *rf_buf; /**< pointer to buffer inside radio driver with a received packet */
	uint8_t *data; /**< pointer to payload */
	ptt_pkt_len_t length; /**< size of payload of received packet */
	int8_t rssi; /**< RSSI */
	uint8_t lqi; /**< LQI */
};

#define RF_WORKQ_STACK_SIZE 2048
#define RF_WORKQ_PRIORITY   5

K_THREAD_STACK_DEFINE(rf_wq_stack, RF_WORKQ_STACK_SIZE);

/* RX queue item (stores the driver buffer pointer until we process+free it) */
struct rx_item {
    sys_snode_t node;
    uint8_t *rf_buf;     /* driver-owned buffer */
    uint8_t *data;       /* &rf_buf[1] */
    uint16_t length;     /* rf_buf[0] - FCS */
    int8_t rssi;
    uint8_t lqi;
};

typedef enum {
    TX_FAIL_NONE = 0,
    TX_FAIL_START,
    TX_FAIL_ON_AIR,
} tx_fail_reason_t;

//== Global Variables ========================================================
static volatile tx_fail_reason_t tx_fail_reason;

static uint8_t failure_reason;
static struct k_work_q rf_wq;
static struct k_work_delayable tx_start_work;

/* Serialize RF operations (similar to ptt_rf lock) */
static atomic_t rf_tx_in_flight;

/* Stage TX frame to guarantee lifetime until TX done */
static uint8_t tx_frame_buf[RADIO_MAX_PAYLOAD_LEN + 1]; /* length byte + payload */

static sys_slist_t rx_list;
static struct k_work rx_work;

/* TX completion work */
static struct k_work tx_done_work;
static struct k_work tx_fail_work;


static volatile bool m_tx_complete = true;
uint8_t m_rssi_lookup[54] = 
{
   1, 2, 5, 9, 13, 18, 23, 27, 32, 37, 43, 48, 53, 58, 63, 68, 
   73, 78, 83, 89, 95, 100, 107, 111, 117, 121, 125, 129, 133, 
   138, 143, 148, 153, 159, 165, 170, 176, 183, 188, 193, 198, 
   203, 207, 212, 216, 221, 225, 228, 233, 239, 245, 250, 253, 254
};

//== Function prototypes =====================================================
uint8_t radio_interface_signal_strength_lookup(int8_t rssi);
static void tx_start_handler(struct k_work *work);
static void rx_work_handler(struct k_work *work);
static void tx_done_handler(struct k_work *work);
static void tx_fail_handler(struct k_work *work);

//============================================================================

//----------------------------------------------------------------------------
// Name  : radio_interface_config
// Desc  : Meant to reset the radio but there isn't an obvious nrf function to
//         that and it might not even make sense for an on-processor radio
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
void radio_interface_config(void)
{
   // Clear the TX in progress flag
   //printk("Config m_tx_complete = true\r\n");
   m_tx_complete = true;
   //nrf_802154_deinit();
   //radio_interface_init();
}

//----------------------------------------------------------------------------
// Name  : radio_interface_receive
// Desc  : Start the radio to receive packets
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
void radio_interface_receive(void)
{
  nrf_802154_receive();
}

//----------------------------------------------------------------------------
// Name  : radio_interface_init
// Desc  : Configure the radio
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
int radio_interface_init(void)
{
   atomic_clear(&rf_tx_in_flight);
   sys_slist_init(&rx_list);

   k_work_queue_start(&rf_wq, rf_wq_stack,
                     K_THREAD_STACK_SIZEOF(rf_wq_stack),
                     RF_WORKQ_PRIORITY, NULL);

   k_work_init(&rx_work, rx_work_handler);
   k_work_init(&tx_done_work, tx_done_handler);
   k_work_init(&tx_fail_work, tx_fail_handler);
   k_work_init(&tx_start_work, tx_start_handler);

  /* nrf radio driver initialization */
   nrf_802154_init();

   uint8_t panId[] = { 0x00, 0x01 };
   nrf_802154_pan_id_set(&panId[0]);
   nrf_802154_channel_set(25);
   nrf_802154_promiscuous_set(false);
   
   return 0;
}

//----------------------------------------------------------------------------
// Name  : radio_interface_channel_set
// Desc  : Send the channel to the radio driver
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
void radio_interface_channel_set(uint8_t channel)
{
   nrf_802154_channel_set(channel);
}

//----------------------------------------------------------------------------
// Name  : nrf_802154_received_raw
// Desc  : Received a packet from the nrf radio. Save off needed data and call 
//         rf_process_rx_packets to handle it
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
/* Simple fixed pool to avoid malloc in callback */
#define RX_ITEM_POOL_N 16
static struct rx_item rx_pool[RX_ITEM_POOL_N];

static struct rx_item *rx_item_alloc(void)
{
    for (int i = 0; i < RX_ITEM_POOL_N; i++) 
    {
        if (rx_pool[i].rf_buf == NULL) 
        {
            return &rx_pool[i];
        }
    }
    return NULL;
}

static void rx_item_free(struct rx_item *it)
{
    it->rf_buf = NULL;
    it->data = NULL;
    it->length = 0;
    it->rssi = 0;
    it->lqi = 0;
}

void nrf_802154_received_raw(uint8_t *data, int8_t power, uint8_t lqi)
{
    if (data == NULL) 
    {
        return;
    }

    struct rx_item *it = rx_item_alloc();
    if (!it) 
    {
        /* No space: drop (phy_test does this too) */
        printk("FAILURE - RX buffer overrun\r\n");
        nrf_802154_buffer_free_raw(data);
        return;
    }

    it->rf_buf = data;
    it->data = &data[1];
    it->length = data[0] - RF_FCS_SIZE;

    /* RSSI conversion */
    it->rssi = radio_interface_signal_strength_lookup(power);
    it->lqi = lqi;

    /* enqueue and schedule work */
    sys_slist_append(&rx_list, &it->node);
    k_work_submit_to_queue(&rf_wq, &rx_work);
}

static void rx_work_handler(struct k_work *work)
{
    while (1) 
    {
        sys_snode_t *n = sys_slist_get(&rx_list);
        if (!n) {
            break;
        }

        struct rx_item *it = CONTAINER_OF(n, struct rx_item, node);

        radio_interrupt(true, false, it->data, it->length, it->rssi, it->lqi, 0);

        nrf_802154_buffer_free_raw(it->rf_buf);
        rx_item_free(it);
    }
}

//----------------------------------------------------------------------------
// Name  : nrf_802154_transmitted_raw
// Desc  : Override the WEAK defined library "tx done" function so that I can
//         add a call to the radio_interrupt in radio.c for status tracking
//         If ACK was requested for the transmitted frame, this function is 
//         called after a proper ACK is received. If ACK was not requested, 
//         this function is called just after transmission has ended.
// Ins   : none
// Outs  : none
//----------------------------------------------------------------------------
void nrf_802154_transmitted_raw(uint8_t *p_frame,
                               const nrf_802154_transmit_done_metadata_t *p_metadata)
{
    (void)p_frame;

    uint8_t *p_ack = p_metadata->data.transmitted.p_ack;
    if (p_ack != NULL) {
        /* You can either free here (as you do now), or defer like phy_test.
           Freeing here is usually OK, but deferring is more consistent. */
        nrf_802154_buffer_free_raw(p_ack);
    }

    k_work_submit_to_queue(&rf_wq, &tx_done_work);
}

//----------------------------------------------------------------------------
// Name  : nrf_802154_transmit_failed
// Desc  : Notifies that a frame was not transmitted due to a busy channel.
// Ins   : p_frame      Pointer to a buffer that contains PHR and PSDU of the frame that was not transmitted
// Ins   : error        Reason of the failure.
// Ins   : p_metadata   Pointer to a metadata structure describing frame passed in @p p_frame.
// Outs  : none
//----------------------------------------------------------------------------
void nrf_802154_transmit_failed(uint8_t *p_frame, nrf_802154_tx_error_t error,
                                const nrf_802154_transmit_done_metadata_t *p_metadata)
{
    (void)p_frame;
    (void)error;
    (void)p_metadata;

    printk("Failure: TX_ON_AIR_FAIL: error=%d\r\n", error);
    tx_fail_reason = TX_FAIL_ON_AIR;
    k_work_submit_to_queue(&rf_wq, &tx_fail_work);
}

//----------------------------------------------------------------------------
// Name  : radio_interface_transmit_packet
// Desc  : Send a packet out
// Ins   : A pointer to the data to send
// Outs  : none
//----------------------------------------------------------------------------
void radio_interface_transmit_packet(uint8_t *txdata)
{
#ifndef NO_TRANSMIT_FCC_VERSION
    if (txdata == NULL) {
        return;
    }

    /* Prevent overlapping TX attempts */
    if (atomic_cas(&rf_tx_in_flight, 0, 1) == false) {
        /* Already busy */
        printk("FAILURE - TX request while busy\r\n");
        return;
    }

    /* Set power */
    nrf_802154_tx_power_set(POWER_OUTPUT_LEVEL_INTO_AMPLIFIER + POWER_INCREASE_AMPLIFIER);

    /* Stage TX buffer */
    uint16_t total_len = txdata[0] + 1;  /* length byte + PSDU */
    if (total_len > sizeof(tx_frame_buf)) 
    {
        atomic_clear(&rf_tx_in_flight);
        printk("FAILURE - TX frame too large\r\n");
        return;
    }

    memcpy(tx_frame_buf, txdata, total_len);

    /* Start TX in rf workqueue context */
    m_tx_complete = false;

    //k_work_submit_to_queue(&rf_wq, &tx_start_work);
    k_work_schedule_for_queue(&rf_wq, &tx_start_work, K_MSEC(1));
#endif
}

//----------------------------------------------------------------------------
// Name  : radio_interface_get_tx_busy
// Desc  : Returns 1 if the radio has not yet completed its transmit (including receiving the hardware ACK if requested)
// Ins   : none
// Outs  : 1 for busy, 0 for not busy
//----------------------------------------------------------------------------
uint8_t radio_interface_get_tx_busy(void)
{
   if (m_tx_complete == false)
   {
      return 1;
   }
   else
   {
      return 0;
   }
}

//----------------------------------------------------------------------------
// Name  : radio_interface_signal_strength_lookup
// Desc  : Looks up the RSSI value needed by this app from the RSSI/dBm value
// Ins   : traditional RSSI in dBm
// Outs  : converted RSSI value per the lookup table in the MRF24J20 datasheet lookup table 3-8.
//----------------------------------------------------------------------------
uint8_t radio_interface_signal_strength_lookup(int8_t rssi)
{
   // After lookup, RSSI will be a value from 255 (great) to 0 (bad)
   // An RSSI of -90dBm or below is 0
   // An RSSI of -35dBm or above is 255
   // In-between lookup table
   uint8_t signal_strength_lookup;
   if (rssi <= -90 )
   {
      signal_strength_lookup = 0;
   }
   else if (rssi >= -35 )
   {
      signal_strength_lookup = 255;
   }
   else
   {  // It's somewhere in the middle. Use a lookup table
      signal_strength_lookup = m_rssi_lookup[rssi + 89];
   }
   return signal_strength_lookup;
}

// This function is called when we start to send an ACK. Just using it temporarily for amplifier control debug
//void nrf_802154_tx_ack_started(const uint8_t * p_data)
//{
//   iomapping_set_tp2(0);      
//}

static void tx_start_handler(struct k_work *work)
{
    bool ok;

    /* NOTE: we are already logically "locked" here via rf_tx_in_flight */
    const nrf_802154_transmit_metadata_t metadata = {
        .frame_props = NRF_802154_TRANSMITTED_FRAME_PROPS_DEFAULT_INIT,
        .cca = true,  /* set based on what you want */
    };

//#warning DEBUG PRINT
//printk("TX FCF bytes: %02x %02x\r\n", tx_frame_buf[1], tx_frame_buf[2]);
//uint8_t len = tx_frame_buf[0];
//uint16_t fcf = (uint16_t)tx_frame_buf[1] | ((uint16_t)tx_frame_buf[2] << 8);
//uint8_t seq = tx_frame_buf[3];
//bool ackreq = (fcf & (1u << 5)) != 0;   // 802.15.4 ACK request bit is bit 5 of FCF
//printk("TX: len=%u fcf=0x%04x seq=%u ackreq(bit)=%u\r\n", len, fcf, seq, ackreq);


    ok = nrf_802154_transmit_raw(tx_frame_buf, &metadata);

    if (!ok) {
        /* Could not even start TX: complete as failure */
        tx_fail_reason = TX_FAIL_START;
        k_work_submit_to_queue(&rf_wq, &tx_fail_work);
    }
}


static void tx_done_handler(struct k_work *work)
{
    radio_interrupt(false, true, NULL, 0, 0, 0, 0);
    m_tx_complete = true;
    atomic_clear(&rf_tx_in_flight);
}

static void tx_fail_handler(struct k_work *work)
{
    radio_interrupt(false, true, NULL, 0, 0, 0, 1);
    m_tx_complete = true;
    atomic_clear(&rf_tx_in_flight);
    if (tx_fail_reason == TX_FAIL_START)
    {
      printk("\r\nFAILURE - Failed to start transmit\r\n");
    }
    else if (tx_fail_reason == TX_FAIL_ON_AIR)
    {
      //printk("\r\nFAILURE - Transmit failed on-air\r\n");      
    }
}
#if(0)
void nrf_802154_tx_ack_started(const uint8_t *p_data)
{
    printk("AUTOACK started\r\n");
}

void nrf_802154_tx_ack_finished(const uint8_t *p_data)
{
    printk("AUTOACK finished\r\n");
}
#endif