/**
 * @file rf_task.c
 * @brief RF task - NRF24L01 communication
 */
#include <stdio.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_log.h"
#include "app_main.h"

static const char *TAG = "rf";

/* NRF24L01 protocol constants (matching AliceRX_STC) */
#define FRAME_ID_CHANNELS      0xA0
#define FRAME_ID_TELEMETRY     0xB0
#define RF_PAYLOAD_SIZE        32
#define RF_CHANNEL_DEFAULT     100
#define RF_DATA_RATE           RF_1MBPS
#define RF_TX_POWER            RF_PA_MAX

/* RF frame structure */
typedef struct __attribute__((packed)) {
    uint8_t frame_id;       /* 0xA0 = FRAME_ID_CHANNELS */
    uint8_t seq;            /* Sequence number */
    uint8_t model_id;       /* Model ID */
    uint8_t flags;          /* Flags */
    uint8_t sbus_data[22];  /* 16 channels × 11bit = 22 bytes little-endian bitstream */
    uint8_t reserved[6];    /* Reserved */
} rf_payload_t;

/* Telemetry data from receiver */
typedef struct __attribute__((packed)) {
    uint8_t frame_id;       /* 0xB0 = FRAME_ID_TELEMETRY */
    uint8_t seq;            /* Sequence number */
    uint16_t battery_mv;    /* Battery voltage (big-endian, mV) */
    uint8_t reserved[28];   /* Reserved */
} rf_telemetry_t;

/* RF state */
static uint8_t rf_seq = 0;
static uint8_t rf_model_id = 0;

void rf_send_channels(adc_data_t *adc_data)
{
    rf_payload_t payload;
    memset(&payload, 0, sizeof(payload));
    
    payload.frame_id = FRAME_ID_CHANNELS;
    payload.seq = rf_seq++;
    payload.model_id = rf_model_id;
    
    /* Convert 8 ADC channels to SBUS-like 11-bit format */
    /* Map 12-bit ADC (0-4095) to SBUS range (200-1844) */
    uint16_t sbus_channels[16] = {0};
    for (int i = 0; i < 8; i++) {
        uint32_t val = adc_data->channels[i];
        /* Map 0-4095 to 200-1844 */
        sbus_channels[i] = 200 + (val * 1644 / 4096);
        if (sbus_channels[i] > 1844) sbus_channels[i] = 1844;
    }
    /* Center unused channels */
    for (int i = 8; i < 16; i++) {
        sbus_channels[i] = 1024;
    }
    
    /* Pack 16 × 11bit into 22 bytes little-endian */
    for (int i = 0; i < 16; i++) {
        uint16_t ch = sbus_channels[i];
        int byte_idx = (i * 11) / 8;
        int bit_off = (i * 11) % 8;
        payload.sbus_data[byte_idx] |= (ch << bit_off) & 0xFF;
        payload.sbus_data[byte_idx + 1] |= (ch >> (8 - bit_off)) & 0xFF;
        if (bit_off > 5) {
            payload.sbus_data[byte_idx + 2] |= (ch >> (16 - bit_off)) & 0xFF;
        }
    }
    
    /* TODO: Send via NRF24L01 hardware */
    ESP_LOGD(TAG, "RF TX seq=%d ch1=%d", payload.seq, sbus_channels[0]);
}

void rf_task(void *pvParameters)
{
    ESP_LOGI(TAG, "RF task started");
    
    /* TODO: Initialize NRF24L01 hardware */
    /* nrf24_init(); */
    /* nrf24_set_channel(100); */
    /* nrf24_set_data_rate(RF_DATA_RATE); */
    /* nrf24_set_power(RF_TX_POWER); */
    
    while (1) {
        /* Read latest ADC data */
        adc_data_t adc_data;
        if (adc_data_queue && xQueuePeek(adc_data_queue, &adc_data, 0)) {
            /* Send channel data over RF */
            rf_send_channels(&adc_data);
        }
        
        /* TODO: Check for ACK/telemetry from receiver */
        /* rf_telemetry_t telemetry; */
        /* if (nrf24_read_ack(&telemetry)) { */
        /*     linked = 1; */
        /*     lost_count = 0; */
        /* } else { */
        /*     if (++lost_count > 50) linked = 0; */
        /* } */
        
        vTaskDelay(pdMS_TO_TICKS(20));  /* 50Hz RF update rate */
    }
}