#include <iostream>
#include <vector>
#include <atomic>
#include <port/udp.h>

#include "port/uart.h"

static constexpr uint16_t UDP_PORT = 1067;

int main(int argc, char* argv[]) {
    std::string serial_port = "/dev/ttyUSB0";
    if (argc > 1) {
        serial_port = argv[1];
    }

    try {
        // Создание UDP сервера
        drivers::UdpDriver udp_server(UDP_PORT);
        drivers::UartDriver uart(serial_port, 416666);

        std::cout << "Ready to receive UDP packets and forward to serial port" << std::endl;
        std::cout << "Press Ctrl+C to exit" << std::endl;

        int packet_count = 0;
        while (true) {
            // Прием UDP пакета
            std::vector<uint16_t> data = udp_server.recvPacket();

            if (!data.empty()) {
                packet_count++;

                for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
                    auto item = static_cast<int16_t>(data[i]);
                    sb.ch[i] = static_cast<int16_t>((item - 1000.0) * 2);
                    if (sb.ch[i] < 50) {
                        sb.ch[i] = 50; // минимальное число
                    } else if (sb.ch[i] > 1900) {
                        sb.ch[i] = 1900; // максимальное число
                    }
                }
                sb.fillDataBuf();
                serial.write(sb.buf_);

                // Выводим статистику каждые 100 пакетов
                if (packet_count % 100 == 0) {
                    std::cout << "Received " << packet_count << " packets total" << std::endl;
                }
            }
        }

    } catch (const std::exception& e) {
        std::cerr << "Error: " << e.what() << std::endl;
        return 1;
    }

    std::cout << "Exiting..." << std::endl;
    return 0;
}