#include "radio_ctrl/uart_handler.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <termios.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>
#include <stdint.h>
#include <iostream>

#ifndef BOTHER
#define BOTHER 0010000
#endif

struct termios2
{
    tcflag_t c_iflag;
    tcflag_t c_oflag;
    tcflag_t c_cflag;
    tcflag_t c_lflag;
    cc_t c_line;
    cc_t c_cc[19];
    speed_t c_ispeed;
    speed_t c_ospeed;
};

#define TCGETS2 _IOR('T', 0x2A, struct termios2)
#define TCSETS2 _IOW('T', 0x2B, struct termios2)

// 构造函数
UartHandler::UartHandler(const std::string &device, int baudrate)
    : serial_device(device), baudrate(baudrate), fd(-1), failsafe_status(SBUS_SIGNAL_OK)
{
    std::fill(std::begin(sbus_channels), std::end(sbus_channels), 0); // 初始化通道数据
}

// 析构函数，关闭串口
UartHandler::~UartHandler()
{
    stop_reading();
    if (fd != -1)
    {
        close(fd);
    }
}

bool UartHandler::open_serial()
{
    fd = open(serial_device.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK);
    if (fd < 0)
    {
        perror("open");
        return false;
    }

    struct termios2 tio = {};
    if (ioctl(fd, TCGETS2, &tio) != 0)
    {
        perror("ioctl TCGETS2");
        close(fd);
        fd = -1;
        return false;
    }

    tio.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR |
                     IGNCR | ICRNL | IXON);
    tio.c_iflag |= (INPCK | IGNPAR);
    tio.c_oflag &= ~OPOST;
    tio.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
    tio.c_cflag &= ~(CSIZE | CRTSCTS | PARODD | CBAUD);
    tio.c_cflag |= (CS8 | CSTOPB | CLOCAL | PARENB | BOTHER | CREAD);

    tio.c_ispeed = baudrate;
    tio.c_ospeed = baudrate;
    tio.c_cc[VMIN] = 1;
    tio.c_cc[VTIME] = 0;

    if (ioctl(fd, TCSETS2, &tio) != 0)
    {
        perror("ioctl TCSETS2");
        close(fd);
        fd = -1;
        return false;
    }

    std::cout << "Serial port " << serial_device << " opened and configured.\n";
    return true;
}

// 启动串口数据读取线程
void UartHandler::start_reading()
{
    reading = true;
    read_thread = std::thread(&UartHandler::read_loop, this);
}

// 停止串口数据读取线程
void UartHandler::stop_reading()
{
    reading = false;
    if (read_thread.joinable())
    {
        read_thread.join();
    }
}

void UartHandler::read_loop()
{
    uint8_t buf[512];
    while (reading)
    {
        int n = read(fd, buf, sizeof(buf));
        if (n > 0)
        {
            parse_data(buf, n);
        }
        else if (n == 0)
        {
            usleep(1000);
        }
        else
        {
            if (errno == EAGAIN)
            {
                usleep(1000);
            }
            else
            {
                perror("read");
                break;
            }
        }
    }
}

void UartHandler::print_hex(uint8_t *buf, int len)
{
    for (int i = 0; i < len; ++i)
    {
        printf("%02X ", buf[i]);
    }
    printf("\n");
}

void UartHandler::parse_data(uint8_t *buf, int len)
{
    // 收到一帧完整的25字节数据
    if (len >= kSbusFrameLength)
    {
        memcpy(sbus_data, buf, kSbusFrameLength);
        int status = sbus_parse();

        // 打印通道值
        // std::cout << "Parsed Channels:\n";
        // for (int i = 0; i < 8; ++i)
        // {
        //     std::cout << "CH" << i + 1 << ": " << sbus_channels[i] << " ";
        //     if ((i + 1) % 8 == 0)
        //         std::cout << "\n"; // 每8个换行
        // }

        if (status == SBUS_SIGNAL_LOST)
        {
            std::cout << "[Warning] SBUS Signal Lost!\n";
        }
        else if (status == SBUS_SIGNAL_FAILSAFE)
        {
            std::cout << "[Warning] SBUS Failsafe!\n";
        }
    }
    else
    {
        std::cerr << "Received data too short to parse.\n";
        failsafe_status = SBUS_SIGNAL_LOST;
    }
}

int UartHandler::sbus_parse()
{
    sbus_channels[0] = ((sbus_data[1] | sbus_data[2] << 8) & 0x07FF);
    sbus_channels[1] = ((sbus_data[2] >> 3 | sbus_data[3] << 5) & 0x07FF);
    sbus_channels[2] = ((sbus_data[3] >> 6 | sbus_data[4] << 2 | sbus_data[5] << 10) & 0x07FF);
    sbus_channels[3] = ((sbus_data[5] >> 1 | sbus_data[6] << 7) & 0x07FF);
    sbus_channels[4] = ((sbus_data[6] >> 4 | sbus_data[7] << 4) & 0x07FF);
    sbus_channels[5] = ((sbus_data[7] >> 7 | sbus_data[8] << 1 | sbus_data[9] << 9) & 0x07FF);
    sbus_channels[6] = ((sbus_data[9] >> 2 | sbus_data[10] << 6) & 0x07FF);
    sbus_channels[7] = ((sbus_data[10] >> 5 | sbus_data[11] << 3) & 0x07FF);
    sbus_channels[8] = ((sbus_data[12] | sbus_data[13] << 8) & 0x07FF);
    sbus_channels[9] = ((sbus_data[13] >> 3 | sbus_data[14] << 5) & 0x07FF);
    sbus_channels[10] = ((sbus_data[14] >> 6 | sbus_data[15] << 2 | sbus_data[16] << 10) & 0x07FF);
    sbus_channels[11] = ((sbus_data[16] >> 1 | sbus_data[17] << 7) & 0x07FF);
    sbus_channels[12] = ((sbus_data[17] >> 4 | sbus_data[18] << 4) & 0x07FF);
    sbus_channels[13] = ((sbus_data[18] >> 7 | sbus_data[19] << 1 | sbus_data[20] << 9) & 0x07FF);
    sbus_channels[14] = ((sbus_data[20] >> 2 | sbus_data[21] << 6) & 0x07FF);
    sbus_channels[15] = ((sbus_data[21] >> 5 | sbus_data[22] << 3) & 0x07FF);

    // 安全检测
    failsafe_status = SBUS_SIGNAL_OK;
    if (sbus_data[23] & (1 << 2))
    {
        failsafe_status = SBUS_SIGNAL_LOST;
    }
    else if (sbus_data[23] & (1 << 3))
    {
        failsafe_status = SBUS_SIGNAL_FAILSAFE;
    }
    return failsafe_status;
}

int UartHandler::get_channel_value(int channel)
{
    if (channel >= 0 && channel < 8)
    {
        return sbus_channels[channel]; // 返回指定通道的值
    }
    return 0; // 如果无效的通道，返回默认值
}

bool UartHandler::get_data_safe()
{
    if (failsafe_status == SBUS_SIGNAL_OK)
        return true;
    else
        return false;
}