/**
 * @file rcsignal.c
 * @brief RC PWM/SBUS signal reading implementation with runtime mode selection
 */

#include "rcsignal.h"
#include "config.h"

#include "driver/gpio.h"
#include "driver/uart.h"
#include "esp_timer.h"
#include "esp_log.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

#include <string.h>

static const char *TAG = "RCSIGNAL";

// SBUS protocol constants
#define SBUS_FRAME_SIZE 25
#define SBUS_HEADER 0x0F
#define SBUS_FOOTER 0x00
#define SBUS_FOOTER2 0x04 // Alternative footer
#define SBUS_BAUDRATE 100000
#define SBUS_CH_MIN 172
#define SBUS_CH_CENTER 992
#define SBUS_CH_MAX 1811
#define UART_NUM UART_NUM_1
#define UART_BUF_SIZE 256

// PWM mode constants
#define PULSE_THRESHOLD_US 1500
#define SIGNAL_TIMEOUT_MS 100
static struct
{
    int8_t gpio_pin;
    bool use_sbus_mode;           // Runtime mode selection
    uint8_t sbus_trigger_channel; // SBUS trigger channel
    uint16_t sbus_threshold_low;  // SBUS low threshold
    uint16_t sbus_threshold_high; // SBUS high threshold

    // SBUS state
    volatile uint16_t channels[SBUS_NUM_CHANNELS];
    volatile int64_t last_frame_time;
    uint8_t rx_buffer[SBUS_FRAME_SIZE];

    // PWM state
    volatile uint32_t pulse_width_us;
    volatile int64_t last_edge_time;
    volatile int64_t pulse_start_time;
    volatile bool last_level;

    // Common state
    volatile bool signal_active;
    volatile bool pull_detected;
    uint8_t current_mode;
    rcsignal_mode_change_callback_t callback;
    bool initialized;
    TaskHandle_t monitor_task;
} rcsignal = {
    .gpio_pin = -1,
    .use_sbus_mode = false,
    .sbus_trigger_channel = 3,
    .sbus_threshold_low = 800,
    .sbus_threshold_high = 1100,
    .channels = {0},
    .last_frame_time = 0,
    .rx_buffer = {0},
    .pulse_width_us = 0,
    .last_edge_time = 0,
    .pulse_start_time = 0,
    .last_level = false,
    .signal_active = false,
    .pull_detected = false,
    .current_mode = 0,
    .callback = NULL,
    .initialized = false,
    .monitor_task = NULL,
};

// PWM Mode: GPIO ISR handler
static void IRAM_ATTR gpio_isr_handler(void *arg)
{
    int64_t now = esp_timer_get_time();
    bool level = gpio_get_level(rcsignal.gpio_pin);

    if (level && !rcsignal.last_level)
    {
        // Rising edge - start of pulse
        rcsignal.pulse_start_time = now;
    }
    else if (!level && rcsignal.last_level)
    {
        // Falling edge - end of pulse
        if (rcsignal.pulse_start_time > 0)
        {
            rcsignal.pulse_width_us = (uint32_t)(now - rcsignal.pulse_start_time);
            rcsignal.last_edge_time = now;
            rcsignal.signal_active = true;
        }
    }

    rcsignal.last_level = level;
}

// SBUS Mode: Parse SBUS frame
static bool parse_sbus_frame(const uint8_t *frame, uint16_t *channels)
{
    // Check header and footer
    if (frame[0] != SBUS_HEADER || (frame[24] != SBUS_FOOTER && frame[24] != SBUS_FOOTER2))
    {
        return false;
    }

    // SBUS uses 11 bits per channel, packed into bytes
    channels[0] = ((frame[1] | frame[2] << 8) & 0x07FF);
    channels[1] = ((frame[2] >> 3 | frame[3] << 5) & 0x07FF);
    channels[2] = ((frame[3] >> 6 | frame[4] << 2 | frame[5] << 10) & 0x07FF);
    channels[3] = ((frame[5] >> 1 | frame[6] << 7) & 0x07FF);
    channels[4] = ((frame[6] >> 4 | frame[7] << 4) & 0x07FF);
    channels[5] = ((frame[7] >> 7 | frame[8] << 1 | frame[9] << 9) & 0x07FF);
    channels[6] = ((frame[9] >> 2 | frame[10] << 6) & 0x07FF);
    channels[7] = ((frame[10] >> 5 | frame[11] << 3) & 0x07FF);
    channels[8] = ((frame[12] | frame[13] << 8) & 0x07FF);
    channels[9] = ((frame[13] >> 3 | frame[14] << 5) & 0x07FF);
    channels[10] = ((frame[14] >> 6 | frame[15] << 2 | frame[16] << 10) & 0x07FF);
    channels[11] = ((frame[16] >> 1 | frame[17] << 7) & 0x07FF);
    channels[12] = ((frame[17] >> 4 | frame[18] << 4) & 0x07FF);
    channels[13] = ((frame[18] >> 7 | frame[19] << 1 | frame[20] << 9) & 0x07FF);
    channels[14] = ((frame[20] >> 2 | frame[21] << 6) & 0x07FF);
    channels[15] = ((frame[21] >> 5 | frame[22] << 3) & 0x07FF);

    return true;
}

static void monitor_task(void *arg)
{
    if (rcsignal.use_sbus_mode)
    {
        // SBUS mode
        while (1)
        {
            // Read SBUS data from UART
            int len = uart_read_bytes(UART_NUM, rcsignal.rx_buffer, SBUS_FRAME_SIZE, pdMS_TO_TICKS(20));

            if (len == SBUS_FRAME_SIZE)
            {
                uint16_t temp_channels[SBUS_NUM_CHANNELS];

                if (parse_sbus_frame(rcsignal.rx_buffer, temp_channels))
                {
                    // Copy parsed channels
                    for (int i = 0; i < SBUS_NUM_CHANNELS; i++)
                    {
                        rcsignal.channels[i] = temp_channels[i];
                    }

                    rcsignal.last_frame_time = esp_timer_get_time();
                    rcsignal.signal_active = true;

                    // Check trigger channel for mode change
                    uint16_t ch_value = rcsignal.channels[rcsignal.sbus_trigger_channel];

                    // Detect pull low
                    if (ch_value < rcsignal.sbus_threshold_low)
                    {
                        rcsignal.pull_detected = true;
                    }

                    // Detect rising edge (pull high after low)
                    if (ch_value > rcsignal.sbus_threshold_high && rcsignal.pull_detected)
                    {
                        rcsignal.pull_detected = false;

                        if (rcsignal.callback)
                        {
                            rcsignal.callback();
                        }
                    }
                }
            }

            // Check for signal timeout
            int64_t now = esp_timer_get_time();
            if (rcsignal.signal_active && (now - rcsignal.last_frame_time) > (SIGNAL_TIMEOUT_MS * 1000))
            {
                rcsignal.signal_active = false;
                memset((void *)rcsignal.channels, 0, sizeof(rcsignal.channels));
            }

            vTaskDelay(pdMS_TO_TICKS(5));
        }
    }
    else
    {
        // PWM mode
        uint32_t last_pulse_width = 0;

        while (1)
        {
            vTaskDelay(pdMS_TO_TICKS(10));

            // Check for signal timeout
            int64_t now = esp_timer_get_time();
            if (rcsignal.signal_active && (now - rcsignal.last_edge_time) > (SIGNAL_TIMEOUT_MS * 1000))
            {
                rcsignal.signal_active = false;
                rcsignal.pulse_width_us = 0;
            }

            // Detect mode change (rising edge on PWM signal > 1500us)
            if (rcsignal.pulse_width_us != last_pulse_width)
            {
                last_pulse_width = rcsignal.pulse_width_us;

                if (rcsignal.pulse_width_us < PULSE_THRESHOLD_US)
                {
                    rcsignal.pull_detected = true;
                }

                if (rcsignal.pulse_width_us > PULSE_THRESHOLD_US && rcsignal.pull_detected)
                {
                    // Mode change detected
                    rcsignal.pull_detected = false;

                    if (rcsignal.callback)
                    {
                        rcsignal.callback();
                    }
                }
            }
        }
    }
}

esp_err_t rcsignal_init(const config_t *config)
{
    if (!config || config->rc_signal_pin < 0)
    {
        ESP_LOGI(TAG, "RC signal disabled (no pin configured)");
        return ESP_OK;
    }

    // Store configuration
    rcsignal.gpio_pin = config->rc_signal_pin;
    rcsignal.use_sbus_mode = config->use_sbus_mode;
    rcsignal.sbus_trigger_channel = config->sbus_trigger_channel;
    rcsignal.sbus_threshold_low = config->sbus_threshold_low;
    rcsignal.sbus_threshold_high = config->sbus_threshold_high;

    if (rcsignal.use_sbus_mode)
    {
        // SBUS Mode: Configure UART with inverted RX
        ESP_LOGI(TAG, "Initializing SBUS mode on GPIO%d", rcsignal.gpio_pin);
        ESP_LOGI(TAG, "  Trigger channel: CH%d", rcsignal.sbus_trigger_channel + 1);
        ESP_LOGI(TAG, "  Thresholds: %d / %d", rcsignal.sbus_threshold_low, rcsignal.sbus_threshold_high);

        uart_config_t uart_config = {
            .baud_rate = SBUS_BAUDRATE,
            .data_bits = UART_DATA_8_BITS,
            .parity = UART_PARITY_EVEN,
            .stop_bits = UART_STOP_BITS_2,
            .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
            .source_clk = UART_SCLK_APB,
        };

        ESP_ERROR_CHECK(uart_param_config(UART_NUM, &uart_config));
        ESP_ERROR_CHECK(uart_set_pin(UART_NUM, UART_PIN_NO_CHANGE, rcsignal.gpio_pin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE));
        ESP_ERROR_CHECK(uart_driver_install(UART_NUM, UART_BUF_SIZE * 2, 0, 0, NULL, 0));

        // Set inverted RX for FrSky receivers (they output inverted SBUS)
        ESP_ERROR_CHECK(uart_set_line_inverse(UART_NUM, UART_SIGNAL_RXD_INV));

        ESP_LOGI(TAG, "SBUS UART configured with inverted RX");
    }
    else
    {
        // PWM Mode: Configure GPIO with interrupts
        ESP_LOGI(TAG, "Initializing PWM mode on GPIO%d", rcsignal.gpio_pin);

        gpio_config_t io_conf = {
            .pin_bit_mask = (1ULL << rcsignal.gpio_pin),
            .mode = GPIO_MODE_INPUT,
            .pull_up_en = GPIO_PULLUP_ENABLE,
            .pull_down_en = GPIO_PULLDOWN_DISABLE,
            .intr_type = GPIO_INTR_ANYEDGE,
        };
        ESP_ERROR_CHECK(gpio_config(&io_conf));

        // Add ISR handler (ISR service must be installed by caller)
        ESP_ERROR_CHECK(gpio_isr_handler_add(rcsignal.gpio_pin, gpio_isr_handler, NULL));
    }

    // Create monitor task
    BaseType_t ret = xTaskCreate(monitor_task, "rcsignal_monitor", 2048, NULL, 5, &rcsignal.monitor_task);
    if (ret != pdPASS)
    {
        if (rcsignal.use_sbus_mode)
        {
            uart_driver_delete(UART_NUM);
        }
        else
        {
            gpio_isr_handler_remove(rcsignal.gpio_pin);
        }
        return ESP_FAIL;
    }

    rcsignal.initialized = true;
    ESP_LOGI(TAG, "RC signal initialized successfully");

    return ESP_OK;
}

void rcsignal_deinit(void)
{
    if (!rcsignal.initialized)
        return;

    if (rcsignal.monitor_task)
    {
        vTaskDelete(rcsignal.monitor_task);
        rcsignal.monitor_task = NULL;
    }

    if (rcsignal.use_sbus_mode)
    {
        uart_driver_delete(UART_NUM);
    }
    else
    {
        if (rcsignal.gpio_pin >= 0)
        {
            gpio_isr_handler_remove(rcsignal.gpio_pin);
        }
    }

    rcsignal.initialized = false;
}

void rcsignal_register_callback(rcsignal_mode_change_callback_t callback)
{
    rcsignal.callback = callback;
}

uint32_t rcsignal_get_pulse_width(void)
{
    if (rcsignal.use_sbus_mode)
    {
        // In SBUS mode, return trigger channel value mapped to microseconds
        // SBUS: 172-1811 -> PWM: ~1000-2000us
        if (rcsignal.signal_active)
        {
            uint16_t ch_val = rcsignal.channels[rcsignal.sbus_trigger_channel];
            return 1000 + ((ch_val - SBUS_CH_MIN) * 1000) / (SBUS_CH_MAX - SBUS_CH_MIN);
        }
        return 0;
    }
    else
    {
        return rcsignal.pulse_width_us;
    }
}

bool rcsignal_is_active(void)
{
    return rcsignal.signal_active;
}

uint8_t rcsignal_get_current_mode(void)
{
    return rcsignal.current_mode;
}

uint16_t rcsignal_get_sbus_channel(uint8_t channel)
{
    if (!rcsignal.use_sbus_mode || channel >= SBUS_NUM_CHANNELS)
    {
        return 0;
    }
    return rcsignal.channels[channel];
}

void rcsignal_debug_print_channels(void)
{
    if (!rcsignal.use_sbus_mode)
    {
        ESP_LOGW(TAG, "Not in SBUS mode");
        return;
    }

    if (!rcsignal.signal_active)
    {
        ESP_LOGW(TAG, "No SBUS signal active");
        return;
    }

    ESP_LOGI(TAG, "SBUS Channels:");
    ESP_LOGI(TAG, "  CH1: %4d  CH2: %4d  CH3: %4d  CH4: %4d",
             rcsignal.channels[0], rcsignal.channels[1],
             rcsignal.channels[2], rcsignal.channels[3]);
    ESP_LOGI(TAG, "  CH5: %4d  CH6: %4d  CH7: %4d  CH8: %4d",
             rcsignal.channels[4], rcsignal.channels[5],
             rcsignal.channels[6], rcsignal.channels[7]);
    ESP_LOGI(TAG, "  CH9: %4d CH10: %4d CH11: %4d CH12: %4d",
             rcsignal.channels[8], rcsignal.channels[9],
             rcsignal.channels[10], rcsignal.channels[11]);
    ESP_LOGI(TAG, " CH13: %4d CH14: %4d CH15: %4d CH16: %4d",
             rcsignal.channels[12], rcsignal.channels[13],
             rcsignal.channels[14], rcsignal.channels[15]);

    // Highlight the trigger channel
    ESP_LOGI(TAG, "Trigger channel (CH%d): %d", rcsignal.sbus_trigger_channel + 1,
             rcsignal.channels[rcsignal.sbus_trigger_channel]);
}