#include "SoftUARTManager.h"
#include "esp_log.h"
#include "driver/uart.h"
#include "driver/gpio.h"
#include <cstring>

#define TAG "SoftUART"

SoftUARTManager::SoftUARTManager(uint8_t num_channels) {
    if (num_channels == 0 || num_channels > SUART_NUM_CHANNELS) {
        ESP_LOGE(TAG, "Invalid num_channels %d (must be 1-%d)", num_channels, SUART_NUM_CHANNELS);
        this->num_channels = 0;
        return;
    }

    this->num_channels = num_channels;

    memset(hw, 0, sizeof(hw));
    memset(ch_status, SUART_CHANNEL_NOT_READY, sizeof(ch_status));

    for (uint8_t i = 0; i < num_channels; i++) {
        if (init_hw_channel(i) == ESP_OK) {
            ch_status[i] = SUART_CHANNEL_READY;
        } else {
            ESP_LOGE(TAG, "Failed to init HW UART channel %d", i);
        }
    }

    ESP_LOGI(TAG, "SoftUARTManager ready: %d HW UART channel(s)", num_channels);
}

SoftUARTManager::~SoftUARTManager() {
    for (uint8_t i = 0; i < num_channels; i++) deinit_hw_channel(i);
}

esp_err_t SoftUARTManager::init_hw_channel(uint8_t idx) {
    uart_port_t port = HW_PORT[idx];
    hw[idx].port    = port;
    hw[idx].tx_gpio = HW_TX[idx];
    hw[idx].rx_gpio = HW_RX[idx];

    const uart_config_t cfg = {
        .baud_rate  = SUART_BAUD_RATE,
        .data_bits  = UART_DATA_8_BITS,
        .parity     = UART_PARITY_DISABLE,
        .stop_bits  = UART_STOP_BITS_1,
        .flow_ctrl  = UART_HW_FLOWCTRL_DISABLE,
        .rx_flow_ctrl_thresh = 0,
        .source_clk = UART_SCLK_DEFAULT,
    };

    esp_err_t res = uart_param_config(port, &cfg);
    if (res != ESP_OK) { ESP_LOGE(TAG, "HW[%d] uart_param_config: %s", idx, esp_err_to_name(res)); return res; }

    res = uart_set_pin(port, (int)HW_TX[idx], (int)HW_RX[idx], UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    if (res != ESP_OK) { ESP_LOGE(TAG, "HW[%d] uart_set_pin: %s", idx, esp_err_to_name(res)); return res; }

    res = uart_driver_install(port, SUART_RX_BUF_SIZE * 2, SUART_RX_BUF_SIZE * 2, 0, nullptr, 0);
    if (res != ESP_OK) { ESP_LOGE(TAG, "HW[%d] uart_driver_install: %s", idx, esp_err_to_name(res)); return res; }

    hw[idx].tx_done_sem = xSemaphoreCreateBinary();
    if (hw[idx].tx_done_sem == nullptr) return ESP_ERR_NO_MEM;

    hw[idx].status = SUART_CHANNEL_READY;
    ESP_LOGI(TAG, "HW channel %d ready (UART%d TX=GPIO%d RX=GPIO%d)", idx, (int)port, (int)HW_TX[idx], (int)HW_RX[idx]);
    return ESP_OK;
}

void SoftUARTManager::deinit_hw_channel(uint8_t idx) {
    if (hw[idx].tx_done_sem) { vSemaphoreDelete(hw[idx].tx_done_sem); hw[idx].tx_done_sem = nullptr; }
    if (hw[idx].status != SUART_CHANNEL_NOT_READY) uart_driver_delete(hw[idx].port);
    hw[idx].status = SUART_CHANNEL_NOT_READY;
}

esp_err_t SoftUARTManager::send(uint8_t* data, size_t size, void* /*config*/, uint8_t channel_num) {
    if (channel_num >= num_channels || data == nullptr || size == 0) return ESP_ERR_INVALID_ARG;
    if (ch_status[channel_num] == SUART_CHANNEL_NOT_READY) return ESP_ERR_INVALID_STATE;
    return send_hw(channel_num, data, size);
}

esp_err_t SoftUARTManager::receive(uint8_t* recv_buf, size_t size, size_t* output_size, uint8_t channel_num) {
    if (channel_num >= num_channels || recv_buf == nullptr || output_size == nullptr) return ESP_FAIL;
    if (ch_status[channel_num] != SUART_CHANNEL_LISTENING) return ESP_FAIL;
    return receive_hw(channel_num, recv_buf, size, output_size);
}

esp_err_t SoftUARTManager::start_receiving(uint8_t channel_num) {
    if (channel_num >= num_channels) return ESP_FAIL;
    if (ch_status[channel_num] == SUART_CHANNEL_NOT_READY) return ESP_ERR_INVALID_STATE;
    if (ch_status[channel_num] == SUART_CHANNEL_LISTENING) return ESP_OK;

    uart_flush_input(hw[channel_num].port);
    ch_status[channel_num] = SUART_CHANNEL_LISTENING;
    return ESP_OK;
}

esp_err_t SoftUARTManager::wait_until_send_complete(uint8_t channel_num) {
    if (channel_num >= num_channels) return ESP_FAIL;

    SemaphoreHandle_t sem = hw[channel_num].tx_done_sem;
    if (sem == nullptr) return ESP_FAIL;

    if (xSemaphoreTake(sem, SUART_TX_DONE_WAIT_TICKS) != pdTRUE) {
        ESP_LOGE(TAG, "wait_until_send_complete: timeout on channel %d", channel_num);
        return ESP_FAIL;
    }

    uart_wait_tx_done(hw[channel_num].port, SUART_TX_DONE_WAIT_TICKS);
    return ESP_OK;
}

esp_err_t SoftUARTManager::send_hw(uint8_t idx, uint8_t* data, size_t size) {
    uart_wait_tx_done(hw[idx].port, SUART_TX_DONE_WAIT_TICKS);
    int written = uart_write_bytes(hw[idx].port, reinterpret_cast<const char*>(data), size);
    if (written < 0 || static_cast<size_t>(written) != size) {
        ESP_LOGE(TAG, "HW[%d] uart_write_bytes returned %d", idx, written);
        return ESP_FAIL;
    }
    xSemaphoreGive(hw[idx].tx_done_sem);
    return ESP_OK;
}

esp_err_t SoftUARTManager::receive_hw(uint8_t idx, uint8_t* buf, size_t size, size_t* out) {
    uart_port_t port = hw[idx].port;

    // wait for at least a minimum complete frame
    const size_t MIN_FRAME = 10;
    size_t available = 0;
    if (uart_get_buffered_data_len(port, &available) != ESP_OK || available < MIN_FRAME) {
        return ESP_FAIL;
    }

    //scan for the preamble byte (0xAB)
    uint8_t preamble = 0;
    int discard_count = 0;
    while (true) {
        int n = uart_read_bytes(port, &preamble, 1, pdMS_TO_TICKS(2));
        if (n < 1) return ESP_FAIL;

        if (preamble == 0xAB) break;

        discard_count++;
        if (discard_count > (int)SUART_RX_BUF_SIZE) {
            ESP_LOGW(TAG, "HW[%d] no preamble after %d bytes — flushing", idx, discard_count);
            uart_flush_input(port);
            return ESP_FAIL;
        }
    }
    if (discard_count > 0) {
        ESP_LOGD(TAG, "HW[%d] skipped %d noise byte(s) before preamble", idx, discard_count);
    }
    buf[0] = preamble;

    // read the remaining 7 header bytes
    int peeked = uart_read_bytes(port, buf + 1, 7, pdMS_TO_TICKS(20));
    if (peeked < 7) {
        ESP_LOGE(TAG, "HW[%d] timeout reading header (got %d of 7)", idx, peeked);
        return ESP_FAIL;
    }

    // determine frame type and total length
    bool is_control = (buf[5] & 0x80) != 0;
    uint16_t len_field = (uint16_t)buf[6] | ((uint16_t)buf[7] << 8);

    size_t total_frame_len;
    if (is_control) {
        total_frame_len = 8 + len_field + 2;
    } else {
        uint8_t hdr2[4];
        int n2 = uart_read_bytes(port, hdr2, 4, pdMS_TO_TICKS(20));
        if (n2 < 4) {
            ESP_LOGE(TAG, "HW[%d] timeout reading generic header tail", idx);
            return ESP_FAIL;
        }
        memcpy(buf + 8, hdr2, 4);
        uint16_t data_len = (uint16_t)hdr2[2] | ((uint16_t)hdr2[3] << 8);
        total_frame_len = 12 + data_len + 2;
    }

    if (total_frame_len > size || total_frame_len < 10 || total_frame_len > 256) {
        ESP_LOGW(TAG, "HW[%d] unreasonable frame length %d — draining", idx, (int)total_frame_len);
        // Drain remaining bytes of this bad frame so the next preamble scan
        // doesn't find false 0xAB matches inside the body.
        size_t hdr_consumed = is_control ? 8 : 12;
        size_t to_drain = 0;
        if (total_frame_len > hdr_consumed && total_frame_len <= 256) {
            to_drain = total_frame_len - hdr_consumed;
        } else {
            size_t avail = 0;
            uart_get_buffered_data_len(port, &avail);
            to_drain = avail;
        }
        if (to_drain > 0) {
            uint8_t drain_buf[128];
            while (to_drain > 0) {
                size_t chunk = (to_drain > sizeof(drain_buf)) ? sizeof(drain_buf) : to_drain;
                int got = uart_read_bytes(port, drain_buf, chunk, pdMS_TO_TICKS(5));
                if (got <= 0) break;
                to_drain -= (size_t)got;
            }
        }
        return ESP_FAIL;
    }

    // read the rest of the frame
    size_t already_read = is_control ? 8 : 12;
    size_t remaining = total_frame_len - already_read;
    if (remaining > 0) {
        int nr = uart_read_bytes(port, buf + already_read, remaining, pdMS_TO_TICKS(100));
        if (nr < (int)remaining) {
            ESP_LOGE(TAG, "HW[%d] incomplete frame: got %d of %d remaining bytes",
                     idx, nr, (int)remaining);
            return ESP_FAIL;
        }
    }

    *out = total_frame_len;
    return ESP_OK;
}