#include <iostream>
#include <vector>
#include <thread>
#include <port/poller.h>
#include <port/uart.h>
#include <port/udp.h>
#include <crsf.h>

#include "port/poller.h"
#include "port/uart.h"
#include "port/udp.h"

static constexpr uint16_t UDP_PORT = 1067;

int main(int argc, char* argv[]) {
    if (argc < 3 || argc > 4) {
        std::cerr << "Usage: " << argv[0]
                  << " serial_port remote.ip.addr.4 [control packets frequency]\n";
        return 1;
    }

    const std::string serialPort = argv[1];
    const std::string remoteIp = argv[2];
    const int controlFreq = (argc == 4) ? std::atoi(argv[3]) : 50;

    // UART speed fixed at 400k per your earlier specification
    constexpr int UART_BAUD = 400000;

    std::cout << "Opening UART: " << serialPort << " @ " << UART_BAUD << "\n";
    std::cout << "Remote CRSF UDP endpoint: " << remoteIp << ":14550\n";
    std::cout << "Control packets frequency: " << controlFreq << " Hz\n";

    poller::PollWrapper poller;

    auto uart = std::make_shared<drivers::UartDriver>(serialPort, UART_BAUD);
    auto udp  = std::make_shared<drivers::UdpDriver>(UDP_PORT);

    poller.objects.push_back(uart);
    poller.objects.push_back(udp);

    crsf::CrsfParser parser;
    // CrsfTxProcessor processor(udp, uart, remoteIp, UDP_PORT, controlFreq);

    std::vector<uint8_t> tmpBuffer;

    auto lastTick = std::chrono::steady_clock::now();

    while (true) {
        poller.loop(1000);

        if (uart->isPollHup()) {
            std::cerr << "UART disconnected!\n";
            break;
        }
        if (udp->isPollHup()) {
            std::cerr << "UDP socket error!\n";
            break;
        }

        bool eventReceived = false;

        // UART input
        if (uart->isPollIn()) {
            eventReceived = true;
            size_t read = uart->readChunk(tmpBuffer);
            if (read > 0) {
                parser.parseBytes(tmpBuffer);
            }
        }

        while (true) {
            auto pkt = parser.pullPacket();
            if (pkt == nullptr) {
                break;
            }
            pkt->writeToBuffer(tmpBuffer);
            udp->sendTo(std::span<const uint8_t>(tmpBuffer), remoteIp, UDP_PORT);
        }

        if (!eventReceived) {
            std::cout << "[W]: No actions received in last 1s!" << std::endl;
        }
    }

    return 0;
}