#include <sys/ioctl.h>
#include <asm/termbits.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <fcntl.h>

#include <iostream>
#include <vector>
#include <cstring>
#include <atomic>
#include <span>




class UDPServer {
private:
    int sockfd;
    sockaddr_in server_addr, client_addr;
    socklen_t client_len;

public:
    UDPServer(uint16_t port) : client_len(sizeof(client_addr)) {
        // Создание UDP сокета
        sockfd = socket(AF_INET, SOCK_DGRAM, 0);
        if (sockfd < 0) {
            throw std::runtime_error("Failed to create socket");
        }

        // Настройка адреса сервера
        memset(&server_addr, 0, sizeof(server_addr));
        server_addr.sin_family = AF_INET;
        server_addr.sin_addr.s_addr = INADDR_ANY;
        server_addr.sin_port = htons(port);

        // Привязка сокета
        if (bind(sockfd, (sockaddr*)&server_addr, sizeof(server_addr)) < 0) {
            close(sockfd);
            throw std::runtime_error("Failed to bind socket");
        }

        std::cout << "UDP server listening on port " << port << std::endl;
    }

    ~UDPServer() {
        if (sockfd >= 0) {
            close(sockfd);
        }
    }

    std::vector<uint16_t> receive() {
        std::vector<uint16_t> data(64);

        ssize_t received = recvfrom(sockfd,
                                   data.data(),
                                   data.size() * sizeof(uint16_t),
                                   MSG_DONTWAIT, // Неблокирующий режим
                                   (sockaddr*)&client_addr,
                                   &client_len);

        if (received > 0) {
            // Выводим информацию о отправителе и данные
            data.resize(received / 2);
            char client_ip[INET_ADDRSTRLEN];
            inet_ntop(AF_INET, &client_addr.sin_addr, client_ip, INET_ADDRSTRLEN);
            // std::cout << "Received data from " << client_ip << ":" << ntohs(client_addr.sin_port) << std::endl;

            // std::cout << "Data: ";
            // for (int i = 0; i < 4 && i < data.size(); ++i) {
            //     std::cout << data[i] << " ";
            // }
            // std::cout << "..." << std::endl;

            return data;
        }

        return {};
    }
};

struct SbusData {
    bool lost_frame = false;
    bool failsafe = false;
    bool ch17 = false, ch18 = false;
    static constexpr size_t NUM_CH = 16;
    static constexpr int16_t SBUS_CH_MIN = 173;
    static constexpr int16_t SBUS_CH_MAX = 1812;
    int16_t ch[NUM_CH];

    /* Message len */
    static constexpr int8_t BUF_LEN_ = 25;
    /* SBUS message defs */
    static constexpr int8_t NUM_SBUS_CH_ = 16;
    static constexpr uint8_t HEADER_ = 0x0F;
    static constexpr uint8_t FOOTER_ = 0x00;
    static constexpr uint8_t FOOTER2_ = 0x04;
    static constexpr uint8_t CH17_MASK_ = 0x01;
    static constexpr uint8_t CH18_MASK_ = 0x02;
    static constexpr uint8_t LOST_FRAME_MASK_ = 0x04;
    static constexpr uint8_t FAILSAFE_MASK_ = 0x08;
    /* Data */
    uint8_t buf_[BUF_LEN_];

    void fillDataBuf() {
        /* Assemble packet */
        buf_[0] = HEADER_;
        buf_[1] = static_cast<uint8_t>((ch[0] & 0x07FF));
        buf_[2] = static_cast<uint8_t>((ch[0] & 0x07FF) >> 8 |
                (ch[1] & 0x07FF) << 3);
        buf_[3] = static_cast<uint8_t>((ch[1] & 0x07FF) >> 5 |
                (ch[2] & 0x07FF) << 6);
        buf_[4] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 2);
        buf_[5] = static_cast<uint8_t>((ch[2] & 0x07FF) >> 10 |
                (ch[3] & 0x07FF) << 1);
        buf_[6] = static_cast<uint8_t>((ch[3] & 0x07FF) >> 7 |
                (ch[4] & 0x07FF) << 4);
        buf_[7] = static_cast<uint8_t>((ch[4] & 0x07FF) >> 4 |
                (ch[5] & 0x07FF) << 7);
        buf_[8] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 1);
        buf_[9] = static_cast<uint8_t>((ch[5] & 0x07FF) >> 9  |
                (ch[6] & 0x07FF) << 2);
        buf_[10] = static_cast<uint8_t>((ch[6] & 0x07FF) >> 6  |
                 (ch[7] & 0x07FF) << 5);
        buf_[11] = static_cast<uint8_t>((ch[7] & 0x07FF) >> 3);
        buf_[12] = static_cast<uint8_t>((ch[8] & 0x07FF));
        buf_[13] = static_cast<uint8_t>((ch[8] & 0x07FF) >> 8 |
                 (ch[9]  & 0x07FF) << 3);
        buf_[14] = static_cast<uint8_t>((ch[9] & 0x07FF) >> 5 |
                 (ch[10] & 0x07FF) << 6);
        buf_[15] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 2);
        buf_[16] = static_cast<uint8_t>((ch[10] & 0x07FF) >> 10 |
                 (ch[11] & 0x07FF) << 1);
        buf_[17] = static_cast<uint8_t>((ch[11] & 0x07FF) >> 7  |
                 (ch[12] & 0x07FF) << 4);
        buf_[18] = static_cast<uint8_t>((ch[12] & 0x07FF) >> 4  |
                 (ch[13] & 0x07FF) << 7);
        buf_[19] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 1);
        buf_[20] = static_cast<uint8_t>((ch[13] & 0x07FF) >> 9  |
                 (ch[14] & 0x07FF) << 2);
        buf_[21] = static_cast<uint8_t>((ch[14] & 0x07FF) >> 6  |
                 (ch[15] & 0x07FF) << 5);
        buf_[22] = static_cast<uint8_t>((ch[15] & 0x07FF) >> 3);
        buf_[23] = 0x00 | (ch17 * CH17_MASK_) | (ch18 * CH18_MASK_) |
                 (failsafe * FAILSAFE_MASK_) |
                 (lost_frame * LOST_FRAME_MASK_);
        buf_[24] = FOOTER_;
    }

};

class SerialPort {
private:
    int fd;

public:
    SerialPort() : fd(-1) {}

    bool open(const std::string& port_path) {
        // Открытие последовательного порта
        fd = ::open(port_path.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
        if (fd < 0) {
            std::cerr << "Failed to open serial port " << port_path << ": " << std::strerror(errno) << std::endl;
            return false;
        }
        struct termios2 tio{};
        ioctl(fd, TCGETS2, &tio);
        // 8bit
        tio.c_cflag &= ~CSIZE;
        tio.c_cflag |= CS8;
        // even
        tio.c_cflag &= ~(PARODD | CMSPAR);
        tio.c_cflag |= PARENB;
        // 2 stop bits
        tio.c_cflag |= CSTOPB;
        // baud rate
        tio.c_ispeed = 100000;
        tio.c_ospeed = 100000;
        // other
        tio.c_iflag |= (INPCK|IGNBRK|IGNCR|ISTRIP);
        tio.c_cflag &= ~CBAUD;
        tio.c_cflag |= (BOTHER|CREAD|CLOCAL);

        if (ioctl(fd, TCSETS2, &tio) != 0) {
            std::cerr << "Failed to set termios2 attributes: " << std::strerror(errno) << std::endl;
            close(fd);
            return false;
        }

        std::cout << "Serial port " << port_path << " opened and configured: 100000 baud, 8E2" << std::endl;
        return true;
    }

    ~SerialPort() {
        _close();
    }

    void _close() {
        if (fd >= 0) {
            ::close(fd);
            fd = -1;
        }
    }

    // Метод для получения файлового дескриптора порта
    int getDescriptor() const {
        return fd;
    }

    // Метод для записи данных в порт
    bool write(std::span<const uint8_t> data) {
        if (fd < 0) return false;

        ssize_t written = ::write(fd, data.data(), data.size());
        if (written < 0) {
            std::cerr << "Failed to write to serial port" << std::endl;
            return false;
        }

        // Принудительная отправка данных
        // tcdrain(fd);
        return true;
    }
};

int main(int argc, char* argv[]) {
    // Парсим аргументы командной строки
    std::string serial_port = "/dev/ttyUSB0";
    if (argc > 1) {
        serial_port = argv[1];
    }

    try {
        // Создание UDP сервера
        UDPServer udp_server(1066);

        // Открытие последовательного порта
        SerialPort serial;
        if (!serial.open(serial_port)) {
            return 1;
        }

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

        SbusData sb{};

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

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

                for (int i = 0; i < data.size() && i < SbusData::NUM_CH; ++i) {
                    auto item = static_cast<double>(data[i]);
                    item -= 1000.0;
                    item = std::min(item, 1000.0);
                    item = std::max(item, 0.0);
                    item *= (SbusData::SBUS_CH_MAX - SbusData::SBUS_CH_MIN) / 1000.0;
                    item += SbusData::SBUS_CH_MIN;

                    sb.ch[i] = static_cast<int16_t>(item);
                    if (sb.ch[i] < SbusData::SBUS_CH_MIN) {
                        sb.ch[i] = SbusData::SBUS_CH_MIN; // минимальное число
                    } else if (sb.ch[i] > SbusData::SBUS_CH_MAX) {
                        sb.ch[i] = SbusData::SBUS_CH_MAX; // максимальное число
                    }
                }
                sb.fillDataBuf();
                if (!serial.write(sb.buf_)) {
                    break;
                }

                // Выводим статистику каждые 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;
}