#include "KnxWorker.hpp"
#include "esp32_idf_platform.h"
#include "knx_facade.h"
#include "knx/bau07B0.h"
#include "knx/group_object.h"
#include "esp_log.h"
#include <esp_timer.h>

#define TAG "KNXWORKER"
#define MASK_VERSION   0x07B0

// Set to true to enable raw UART debug mode (bypasses KNX library)
#define UART_DEBUG_MODE false

Esp32IdfPlatform knxPlatform(UART_NUM_1); // Use UART_NUM_1, change if needed
Bau07B0 knxBau(knxPlatform);
KnxFacade<Esp32IdfPlatform, Bau07B0> knx(knxBau);

KnxWorker::KnxWorker() {}


void KnxWorker::init() {
     ESP_LOGI(TAG, "INIT");

    knxPlatform.knxUartPins(48, 53); // Set RX=48, TX=53
    knxPlatform.knxUartBaudRate(19200);
    knxPlatform.setupUart();

#if !UART_DEBUG_MODE
    knx.readMemory();
    knx.start();
    ESP_LOGI(TAG, "FINISH");
#else
    ESP_LOGI(TAG, "UART DEBUG MODE - KNX library disabled");
    ESP_LOGI(TAG, "Sending U_STATE_REQ (0x02) to NCN5130...");
    knxPlatform.writeUart(0x02); // U_STATE_REQ command
    ESP_LOGI(TAG, "Waiting for response...");
#endif
}

static uint32_t lastStateReq = 0;

void KnxWorker::toggleProgMode() {
#if !UART_DEBUG_MODE
    knx.toggleProgMode();
#endif
}

void KnxWorker::loop() {
#if UART_DEBUG_MODE
    // Periodically send U_STATE_REQ to test TX direction
    uint32_t now = millis();
    if (now - lastStateReq > 2000) {
        lastStateReq = now;
        ESP_LOGI(TAG, "TX: Sending U_STATE_REQ (0x02)");
        knxPlatform.writeUart(0x02);
    }

    // Raw UART debug - read and print all incoming bytes
    int available = knxPlatform.uartAvailable();
    if (available > 0) {
        ESP_LOGI(TAG, "UART RX: %d bytes available", available);
        char hexbuf[128];
        int pos = 0;
        while (knxPlatform.uartAvailable() > 0 && pos < 120) {
            int byte = knxPlatform.readUart();
            if (byte >= 0) {
                pos += snprintf(hexbuf + pos, sizeof(hexbuf) - pos, "%02X ", byte);
            }
        }
        if (pos > 0) {
            ESP_LOGI(TAG, "Data: %s", hexbuf);
        }
    }
#else
    knx.loop();
#endif
}