#include <iostream>
#include <string>
#include <vector>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <pthread.h>
#include <sys/ioctl.h>
#include <signal.h>
#include <errno.h>

#include "ComHS.h"
#include "SimMsg.h"

using namespace std;

// 全局变量定义
static int fd_telecontrol = -1; // 遥控串口  通信硬件服务化发送给通信机
static int fd_telemetry = -1;   // 遥测串口  通信机发送给通信硬件服务化

S_Comm_telemetry_data_t S_TELE;   // 维护遥测数据缓存，供状态查询使用
uint8_t cmderPadding[1] = {0xA5}; // 用于填充指令
uint8_t S_COMM_ON_OFF = 0;

uint8_t scommHeaderErrCnt = 0;        // 帧头错误
uint8_t scommResIDErrCnt = 0;         // 应答帧ID错误
uint8_t scommCheckErrCnt = 0;         // 帧校验错误
uint8_t scommUartResetCnt = 0;        // 无应答串口复位计数
uint8_t sendCmderCnt = 0;             // 向通信机发送控制指令计数
uint8_t sendCmderSuccessCnt = 0;      // 通信机控制指令执行成功应答计数
uint8_t sendCmderErrCnt = 0;          // 通信机控制指令错误应答计数
uint8_t sendTelemCnt = 0;             // 向通信机发送遥测帧计数
uint8_t sendTelemSuccessCnt = 0;      // 接收通信机遥测帧应答计数
uint8_t sendGetSelfTelemCmderCnt = 0; // 发送查询通信机工程遥测计数

unsigned char S_Telec1_Data[512] = {0}; // 遥控
unsigned char S_Telem_Data[512] = {0};  // 遥测

// ComHS相关全局变量
SimMsg* ComHS_part = nullptr;
string servername = "Com_Hardware_Service";
string topic_name_cmd = "Command";
string topic_name_tlm = "Telemetry";
const char* fastdds_dest = "Com_Service";

// 运行控制
static volatile int g_running = 1;
static pthread_t telemetry_thread, monitor_thread;

// 信号处理函数
static void signal_handler(int signum)
{
    g_running = 0;
}

// ComHS日志函数
void ComHSWriteLog(const string &msg)
{
    cout << msg << endl;
}

// 初始化ComHS的fastdds
void ComHS_init(uint8_t domainid, string appname)
{
    vector<string> parameters;
    string expression = "dest = '" + servername + "'";
    
    if (nullptr == ComHS_part)
    {
        ComHS_part = new SimMsg(domainid, 3000, appname, ComHSWriteLog);
        ComHS_part->create_pub(topic_name_cmd);
        ComHS_part->create_pub(topic_name_tlm);
        
        ComHS_part->create_sub(topic_name_cmd, command_callback, expression, parameters);
        ComHS_part->create_sub(topic_name_tlm, telemetry_callback, expression, parameters);
        
    }
}

// 通信硬件服务化遥控发布
void ComHS_command_Pub(uint8_t* data, string dest, uint16_t len)
{
    if (ComHS_part)
    {
        ComHS_part->publish(topic_name_cmd, "Com_Hardware_Service", dest, "command", data, len);
    }
}

// 通信硬件服务化遥测发布
void ComHS_telemetry_Pub(uint8_t* data, string dest, uint16_t len)
{
    if (ComHS_part)
    {
        ComHS_part->publish(topic_name_tlm, "Com_Hardware_Service", dest, "telemetry", data, len);
    }
}

// ComHS命令回调函数
void command_callback(string src, string dest, string type,
                      string reserve1, string reserve2,
                      vector<uint8_t>& data)
{
    
    if (S_COMM_ON_OFF != 1)
    {
        cout << "[ERROR] S_COMM is not available" << endl;
        return;
    }
    
    if (data.empty())
    {
        cout << "[ERROR] No command data provided" << endl;
        return;
    }
    
    // 直接发送给通信机
    uint8_t *cmd_data = data.data();
    uint16_t cmd_len = data.size();
    
    cout << "[INFO] Sending command to S_COMM: " << data.size() << " bytes" << endl;
    
    // 发送控制指令
    send_S_COMM_Cmder(cmd_data, cmd_len);
}

// ComHS遥测回调函数
void telemetry_callback(string src, string dest, string type,
                      string reserve1, string reserve2,
                      vector<uint8_t>& data)
{
    
    if (S_COMM_ON_OFF != 1)
    {
        cout << "[ERROR] S_COMM is not available" << endl;
        return;
    }
    
    if (data.empty())
    {
        cout << "[ERROR] No telemetry data provided" << endl;
        return;
    }
    
    // 解析为Multi_EPDU_packet_t结构体
    if (data.size() >= sizeof(Multi_EPDU_packet_t))
    {
        Multi_EPDU_packet_t *down_pkt = (Multi_EPDU_packet_t *)data.data();
        
        // 通过通信机发送下行遥测
        Send_Telemetry_From_S((uint8_t *)down_pkt, sizeof(Multi_EPDU_packet_t));
        cout << "[INFO] Downlink telemetry sent via S_COMM" << endl;
    }
    else
    {
        cout << "[ERROR] Telemetry data too small: " << data.size() 
             << " bytes, need at least " << sizeof(Multi_EPDU_packet_t) << " bytes" << endl;
    }
}

// 串口初始化
int scomm_uart_init(const char *dev_telec, const char *dev_telem, int baudrate)
{
    // 初始化遥控串口
    fd_telecontrol = open(dev_telec, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_telecontrol < 0)
    {
        perror("open telecontrol serial failed");
        return -1;
    }

    // 设置为非阻塞模式
    fcntl(fd_telecontrol, F_SETFL, 0);

    // 初始化遥测串口
    fd_telemetry = open(dev_telem, O_RDWR | O_NOCTTY | O_NDELAY);
    if (fd_telemetry < 0)
    {
        perror("open telemetry serial failed");
        close(fd_telecontrol);
        fd_telecontrol = -1;
        return -1;
    }

    // 设置为非阻塞模式
    fcntl(fd_telemetry, F_SETFL, 0);

    // 设置两个串口参数
    int fds[] = {fd_telecontrol, fd_telemetry};
    for (int i = 0; i < 2; i++)
    {
        struct termios tty;
        if (tcgetattr(fds[i], &tty) != 0)
        {
            perror("tcgetattr failed");
            continue;
        }

        // 设置波特率
        speed_t speed;
        switch(baudrate) {
            case 9600: speed = B9600; break;
            case 19200: speed = B19200; break;
            case 38400: speed = B38400; break;
            case 57600: speed = B57600; break;
            case 115200: speed = B115200; break;
            default: speed = B115200; break;
        }
        
        cfsetospeed(&tty, speed);
        cfsetispeed(&tty, speed);

        // 8N1: 8位数据位，无奇偶校验，1位停止位
        tty.c_cflag &= ~PARENB;  // 无奇偶校验
        tty.c_cflag &= ~CSTOPB;  // 1位停止位
        tty.c_cflag &= ~CSIZE;
        tty.c_cflag |= CS8;      // 8位数据位
        tty.c_cflag |= CREAD | CLOCAL;  // 启用接收器，忽略调制解调器控制线

        // 关闭流控
        tty.c_cflag &= ~CRTSCTS;

        // 设置输入模式
        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);  // 非规范模式
        tty.c_iflag &= ~(IXON | IXOFF | IXANY);          // 关闭软件流控
        tty.c_iflag &= ~(INLCR | ICRNL | IGNCR | IUCLC); // 关闭输入转换

        // 设置输出模式
        tty.c_oflag &= ~OPOST;  // 原始输出
        tty.c_oflag &= ~ONLCR;  // 不转换换行

        // 设置超时和最小读取字符数
        tty.c_cc[VMIN] = 0;     // 非阻塞
        tty.c_cc[VTIME] = 5;    // 0.5秒超时

        if (tcsetattr(fds[i], TCSANOW, &tty) != 0)
        {
            perror("tcsetattr failed");
        }
        
        // 清空缓冲区
        tcflush(fds[i], TCIOFLUSH);
    }

    cout << "[OK] S_COMM UART init telec=" << dev_telec 
         << " telem=" << dev_telem << " baud=" << baudrate << endl;
    return 0;
}

// S遥测串口发送
void S_UART_TELEMETRY_SEND(uint8_t cmd, uint16_t len, uint8_t *nums)
{
    uint8_t tele_check = 0;
    uint16_t i;
    memset(S_Telem_Data, 0, 512);

    S_Telem_Data[0] = 0xEB;
    S_Telem_Data[1] = 0x90;
    S_Telem_Data[2] = cmd;
    S_Telem_Data[3] = (unsigned char)len >> 8;
    S_Telem_Data[4] = (unsigned char)len;

    for (i = 0; i < len; i++)
    {
        S_Telem_Data[i + 5] = *(nums + i);
    }

    for (i = 2; i < (len + 5); i++)
    {
        tele_check += S_Telem_Data[i];
    }

    S_Telem_Data[len + 5] = tele_check;

    if (fd_telemetry >= 0)
    {
        int written = write(fd_telemetry, S_Telem_Data, (len + 6));
        if (written < 0)
        {
            perror("[ERROR] S_COMM write failed");
        }
        else
        {
            printf("[INFO] S_COMM sent command: 0x%02X, len: %d, bytes: %d\n", cmd, len, written);
        }
    }
}

// 向S测控发送控制指令
void send_S_COMM_Cmder(uint8_t *cmd, uint16_t len)
{
    sendCmderCnt++;

    uint8_t kc = 0;
    uint8_t cmd_buff[64] = {0};

    cmd_buff[0] = 0x11;
    cmd_buff[1] = 0x1A;
    cmd_buff[2] = 0xC0;
    cmd_buff[3] = 0x00;

    cmd_buff[4] = (uint8_t)(len + 1) >> 8; // 根据高层协议来,实际上是+2减1
    cmd_buff[5] = (uint8_t)len + 1;

    for (kc = 0; kc < len + 2; kc++)
        cmd_buff[kc + 6] = cmd[kc];

    S_UART_TELEMETRY_SEND(TELECONTROL_CMD, len + 8, cmd_buff);
}

// 获取S测控常规遥测
void Get_S_COMM_Telemetry_Data(void)
{
    if (S_COMM_ON_OFF == 1)
    {
        S_UART_TELEMETRY_SEND(CON_TELEMETRY, 1, cmderPadding);
        printf("[INFO] S_COMM query telemetry sent\n");
        if (sendGetSelfTelemCmderCnt++ >= 10)
        {
            sendGetSelfTelemCmderCnt = 0;
            printf("[WARN] S_COMM telemetry no response, reset UART\n");
        }
    }
}

// 获取上行遥控指令
void Get_S_COMM_UP_CMD(void)
{
    if (S_COMM_ON_OFF == 1)
    {
        S_UART_TELEMETRY_SEND(QUERY_TELECMD, 1, cmderPadding);
        printf("[INFO] S_COMM query uplink command sent\n");
    }
}

// 通过S测控发送下行遥测
void Send_Telemetry_From_S(uint8_t *tele, uint16_t len)
{
    sendTelemCnt++;
    S_UART_TELEMETRY_SEND(SEND_TELEMETRY, len, tele);
    printf("[INFO] S_COMM send telemetry from S, len: %d\n", len);
}

// 接收数据处理函数
int process_received_data(uint8_t *data, uint16_t size)
{
    if (size < 6)
    {
        printf("[ERROR] S_COMM received data too short: %d bytes\n", size);
        return -1;
    }

    uint16_t i = 0;
    uint8_t cmd = 0;
    uint16_t frame_len = 0;
    uint8_t checksum0 = 0, checksum1 = 0;

    memcpy(S_Telec1_Data, data, size);

    if ((S_Telec1_Data[0] == 0xEB) && (S_Telec1_Data[1] == 0x90))
    {
        cmd = S_Telec1_Data[2];
        frame_len = (uint16_t)(S_Telec1_Data[3] << 8) | (S_Telec1_Data[4]);
        checksum0 = S_Telec1_Data[frame_len + 5];

        // 检查帧长度是否合理
        if (frame_len > 500)
        {
            printf("[ERROR] S_COMM frame length too large: %d\n", frame_len);
            return -1;
        }

        // 检查实际接收到的数据是否足够
        if (size < (frame_len + 6))
        {
            printf("[ERROR] S_COMM incomplete frame: need %d bytes, got %d\n", 
                   frame_len + 6, size);
            return -1;
        }

        // 计算校验和
        checksum1 = 0;
        for (i = 2; i < (frame_len + 5); i++)
        {
            checksum1 += S_Telec1_Data[i];
        }

        if (checksum1 != checksum0)
        {
            scommCheckErrCnt++;
            printf("[ERROR] S_COMM checksum error: calc=0x%02X recv=0x%02X\n", checksum1, checksum0);
            return -1;
        }

        // 根据命令类型处理
        switch (cmd)
        {
        case 0xF0: // 常规遥测数据
            printf("[INFO] S_COMM telemetry data received, frame_len=%d\n", frame_len);
            if (size >= (13 + frame_len - 10))
            {
                // 维护遥测数据缓存
                memcpy((uint8_t *)&S_TELE, (uint8_t *)&S_Telec1_Data[13], frame_len - 10);
                if (sendGetSelfTelemCmderCnt > 0)
                    sendGetSelfTelemCmderCnt--;
                
                // 通过ComHS发布遥测数据
                if (frame_len - 10 > 0)
                {
                    ComHS_telemetry_Pub((uint8_t *)&S_TELE, fastdds_dest, sizeof(S_Comm_telemetry_data_t));
                    printf("[INFO] S_COMM telemetry published via ComHS\n");
                }
            }
            break;

        case 0xE1: // 遥控指令应答
            if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x8C))
            {
                sendCmderSuccessCnt++;
                printf("[INFO] S_COMM command success\n");
            }
            else
            {
                sendCmderErrCnt++;
                printf("[ERROR] S_COMM command error\n");
            }
            break;

        case 0xB4: // 上行遥控指令
            printf("[INFO] S_COMM uplink command received, frame_len=%d\n", frame_len);
            if (size >= 9 && frame_len >= 4)
            {
                uint16_t total_data_len = frame_len;
        
                if (total_data_len > 4)
                {
                    ComHS_command_Pub(&S_Telec1_Data[5], fastdds_dest, total_data_len);
                    printf("[INFO] S_COMM uplink command published, len=%d bytes\n", total_data_len);
                }
                else
                {
                    printf("[INFO] No actual data in uplink command\n");
                }
            }
            break;

        case 0xA5: // 遥测确认
            if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x50))
            {
                sendTelemSuccessCnt = 1;
                printf("[INFO] S_COMM telemetry sent successfully\n");
            }
            break;

        default:
            scommResIDErrCnt++;
            printf("[ERROR] S_COMM unknown command ID: 0x%02X\n", cmd);
            return -1;
        }

        return 0; // 处理成功
    }
    else
    {
        scommHeaderErrCnt++;
        printf("[ERROR] S_COMM frame header error: 0x%02X 0x%02X\n", 
               S_Telec1_Data[0], S_Telec1_Data[1]);
        return -1;
    }
}

// 读取通信机响应数据
int read_scomm_response(void)
{
    if (fd_telecontrol < 0)
        return -1;

    uint8_t buffer[512];
    int total_read = 0;
    
    // 非阻塞读取所有可用数据
    while (g_running)
    {
        int bytes = read(fd_telecontrol, buffer + total_read, sizeof(buffer) - total_read);
        if (bytes > 0)
        {
            total_read += bytes;
            printf("[INFO] S_COMM received %d bytes response, total: %d\n", bytes, total_read);
        }
        else if (bytes < 0)
        {
            if (errno == EAGAIN || errno == EWOULDBLOCK)
            {
                // 无数据可读，退出循环
                break;
            }
            else
            {
                perror("[ERROR] S_COMM read error");
                break;
            }
        }
        else
        {
            // read返回0，通常表示文件结束
            break;
        }
        
        // 检查是否读取了足够的数据
        if (total_read >= sizeof(buffer))
        {
            printf("[WARN] S_COMM buffer full\n");
            break;
        }
        usleep(1000);
    }
    
    if (total_read > 0)
    {
        // 处理接收到的数据
        process_received_data(buffer, total_read);
    }
    
    return total_read;
}

// 定时查询遥测数据线程
static void *telemetry_query_thread(void *arg)
{
    while (g_running)
    {
        if (S_COMM_ON_OFF == 1)
        {
            // 定时查询遥测数据
            Get_S_COMM_UP_CMD();
        }
        
        // 每2秒查询一次
        sleep(2);
    }
    
    return NULL;
}

// 监听串口数据线程
static void *serial_monitor_thread(void *arg)
{
    while (g_running)
    {
        if (S_COMM_ON_OFF == 1 && fd_telecontrol >= 0)
        {
            // 读取串口响应
            read_scomm_response();
        }
        
        // 短暂延时
        usleep(10000); // 10ms
    }
    
    return NULL;
}

// 启动SCOMM服务
void start_scomm_service(const char *dev_telec, const char *dev_telem, int baudrate)
{
    // 设置信号处理
    signal(SIGINT, signal_handler);
    signal(SIGTERM, signal_handler);
    
    // 初始化串口
    if (scomm_uart_init(dev_telec, dev_telem, baudrate) < 0)
    {
        S_COMM_ON_OFF = 0;
        cerr << "[WARN] S_COMM UART init failed. Service will start but communication will fail." << endl;
    }
    else
    {
        S_COMM_ON_OFF = 1;
    }
    
    // 初始化ComHS通信
    ComHS_init(0, "S_Comm_Hardware_Service");
    
    // 创建线程
    if (pthread_create(&telemetry_thread, NULL, telemetry_query_thread, NULL) != 0)
    {
        perror("[ERROR] Failed to create telemetry query thread");
    }
    
    if (pthread_create(&monitor_thread, NULL, serial_monitor_thread, NULL) != 0)
    {
        perror("[ERROR] Failed to create serial monitor thread");
    }
}

// 停止SCOMM服务
void stop_scomm_service(void)
{
    g_running = 0;
    
    // 等待线程结束
    if (telemetry_thread)
    {
        pthread_join(telemetry_thread, NULL);
    }
    if (monitor_thread)
    {
        pthread_join(monitor_thread, NULL);
    }
    
    // 关闭串口
    if (fd_telecontrol >= 0)
    {
        close(fd_telecontrol);
        fd_telecontrol = -1;
    }
    if (fd_telemetry >= 0)
    {
        close(fd_telemetry);
        fd_telemetry = -1;
    }
    
    // 清理ComHS资源
    if (ComHS_part)
    {
        delete ComHS_part;
        ComHS_part = nullptr;
    }
    
    cout << "[INFO] S_COMM Hardware Service stopped" << endl;
}