#include <iostream>
#include <string>
#include <vector>
#include <cstring>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <signal.h>
#include <errno.h>
#include <sys/time.h>
#include <sys/select.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 int process_received_data(uint8_t *data, uint16_t size);

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

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

/*
 * 初始化ComHS的fastdds
 *
 * @domainid: 域ID
 * @appname: 应用程序名称
 */
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);
	}
}

/*
 * 通信硬件服务化遥控发布
 *
 * @data: 要发布的数据
 * @dest: 目标对象
 * @len: 数据长度
 */
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);
	}
}

/*
 * 通信硬件服务化遥测发布
 *
 * @data: 要发布的数据
 * @dest: 目标对象
 * @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命令回调函数
 *
 * 将接收到的遥控指令通过send_S_COMM_Cmder()函数发送给串口
 */
void command_callback(string src, string dest, string type,
		      string reserve1, string reserve2,
		      vector<uint8_t> &data)
{
	if (S_COMM_ON_OFF != 1) {
		cerr << "S_COMM not available" << endl;
		return;
	}

	if (data.empty()) {
		cerr << "No command data provided" << endl;
		return;
	}

	/* 直接发送给通信机 */
	uint8_t *cmd_data = data.data();
	uint16_t cmd_len = data.size();

	/* 发送控制指令 */
	send_S_COMM_Cmder(cmd_data, cmd_len);
}

/*
 * ComHS遥测回调函数
 *
 * 1、当接收到的为命令类型，即为取遥测指令时，触发取遥测
 * 2、当接收到的为遥测类型，即为需要下行的遥测时，将遥测下行
 */
void telemetry_callback(string src, string dest, string type,
			string reserve1, string reserve2,
			vector<uint8_t> &data)
{
	if (S_COMM_ON_OFF != 1) {
		cerr << "S_COMM not available" << endl;
		return;
	}

	if (data.empty()) {
		cerr << "No telemetry data provided" << endl;
		return;
	}

    if (type == "command" ) {
        if (data.size() == 1 && data[0] == 0xFF) {
            Get_S_COMM_UP_CMD();
        }
    } else if (type == "telemetry") {
	    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));
	    } else {
		    cerr << "Telemetry data too small" << endl;
	    }
    }
}

/*
 * 串口初始化
 *
 * @dev_telec: 遥控串口设备路径
 * @dev_telem: 遥测串口设备路径
 * @baudrate: 波特率
 *
 * 返回值: 成功返回0，失败返回-1
 */
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 << "S_COMM UART initialized: telec=" << dev_telec
	     << " telem=" << dev_telem << " baud=" << baudrate << endl;
	return 0;
}

/*
 * S遥测串口发送
 *
 * 将遥测数据进行封装，然后发送到通信机串口
 *
 * @cmd: 命令码
 * @len: 数据长度
 * @nums: 数据指针
 */
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("S_COMM write failed");
		}
	}
}

/*
 * 超时读取
 *
 * @timeout_ms: 超时时间（毫秒）
 * @return: 读取到的字节数，-1表示超时或错误
 */
int scomm_read_with_select(int timeout_ms)
{
    if (fd_telecontrol < 0) {
        cerr << "S_COMM telecontrol port not opened" << endl;
        return -1;
    }
    
    fd_set read_fds;
    struct timeval tv;
    uint8_t buffer[512];
    int total_read = 0;
    
    // 设置超时时间
    tv.tv_sec = timeout_ms / 1000;
    tv.tv_usec = (timeout_ms % 1000) * 1000;
    
    // 清空并设置文件描述符集合
    FD_ZERO(&read_fds);
    FD_SET(fd_telecontrol, &read_fds);
    
    // 使用select等待数据到达
    int ret = select(fd_telecontrol + 1, &read_fds, NULL, NULL, &tv);
    
    if (ret > 0) {
        // 有数据可读
        if (FD_ISSET(fd_telecontrol, &read_fds)) {
            // 读取所有可用数据
            while (total_read < (int)sizeof(buffer)) {
                int bytes = read(fd_telecontrol, buffer + total_read,
                                sizeof(buffer) - total_read);
                if (bytes > 0) {
                    total_read += bytes;
                    
                    // 尝试处理数据
                    if (process_received_data(buffer, total_read) == 0) {
                        return total_read;
                    }
                } else if (bytes < 0) {
                    if (errno == EAGAIN || errno == EWOULDBLOCK) {
                        break;
                    } else {
                        perror("S_COMM read error");
                        return -1;
                    }
                } else {
                    break;
                }
            }
            
            return total_read;
        }
    } else if (ret == 0) {
        return -1;  // 超时
    } else {
        perror("select failed");
        return -1;
    }
    
    return total_read;
}

/*
 * 发送指令并等待响应
 * 
 * @cmd: 命令码
 * @len: 数据长度
 * @nums: 数据指针
 * @timeout_ms: 等待响应超时时间
 * @return: 响应数据长度，-1表示失败
 */
int S_UART_SEND_AND_WAIT(uint8_t cmd, uint16_t len, uint8_t *nums, int timeout_ms)
{
    if (S_COMM_ON_OFF != 1) {
        cerr << "S_COMM is not available" << endl;
        return -1;
    }
    
    if (fd_telemetry < 0 || fd_telecontrol < 0) {
        cerr << "Serial ports not initialized" << endl;
        return -1;
    }
    
    // 1. 先清空输入缓冲区
    tcflush(fd_telecontrol, TCIFLUSH);
    
    // 2. 发送指令
    S_UART_TELEMETRY_SEND(cmd, len, nums);
    
    // 3. 等待响应
    return scomm_read_with_select(timeout_ms);
}

/*
 * 向S测控发送控制指令
 *
 * 将遥控指令封装，然后发送到通信机串口
 * 
 * @cmd: 指令数据
 * @len: 指令长度
 */
void send_S_COMM_Cmder(uint8_t *cmd, uint16_t len)
{
    if (S_COMM_ON_OFF != 1) {
        cerr << "S_COMM is not available" << endl;
        return;
    }
    
    sendCmderCnt++;
    
    // 构造命令数据
    uint8_t cmd_buff[64] = {0};
    uint8_t kc = 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;
    cmd_buff[5] = (uint8_t)len + 1;
    
    for (kc = 0; kc < len + 2; kc++) {
        cmd_buff[kc + 6] = cmd[kc];
    }
    
    // 发送并等待响应
    int result = S_UART_SEND_AND_WAIT(TELECONTROL_CMD, len + 8, cmd_buff, 100);
    
    if (result > 0) {
        sendCmderSuccessCnt++;
    } else {
        sendCmderErrCnt++;
        cerr << "Command timeout (100ms)" << endl;
    }
}

/*
 * 获取S测控常规遥测（同步版本，使用select超时）
 * 
 * 向通信机串口写入常规遥测查询的指令，进行遥测的查询
 * 
 */
void Get_S_COMM_Telemetry_Data(void)
{
    if (S_COMM_ON_OFF != 1) return;
    
    int result = S_UART_SEND_AND_WAIT(CON_TELEMETRY, 1, cmderPadding, 100);
    
    if (result > 0) {
        sendGetSelfTelemCmderCnt = 0;
    } else {
        sendGetSelfTelemCmderCnt++;
        if (sendGetSelfTelemCmderCnt >= 3) {
            cerr << "Multiple telemetry queries failed" << endl;
        }
    }
}

/*
 * 获取上行遥控指令（同步版本，使用select超时）
 * 
 * 向通信机串口写入获取上行遥控指令，进行上行遥控查询
 * 
 */
void Get_S_COMM_UP_CMD(void)
{
    if (S_COMM_ON_OFF != 1) return;
    
    int result = S_UART_SEND_AND_WAIT(QUERY_TELECMD, 1, cmderPadding, 100);
    
    if (result < 0) {
        cerr << "Uplink command query timeout" << endl;
    }
}

/*
 * 通过S测控发送下行遥测（可选等待确认）
 *
 * @tele: 遥测数据
 * @len: 数据长度
 */
void Send_Telemetry_From_S(uint8_t *tele, uint16_t len)
{
    if (S_COMM_ON_OFF != 1) return;
    
    sendTelemCnt++;
    
    S_UART_TELEMETRY_SEND(SEND_TELEMETRY, len, tele);
    scomm_read_with_select(100);
}

/*
 * 接收数据处理函数
 *
 * @data: 接收到的数据
 * @size: 数据大小
 *
 * 返回值: 0-成功处理, -1-处理失败, -2-需要更多数据
 */
static int process_received_data(uint8_t *data, uint16_t size)
{
    uint16_t i = 0;
    uint8_t cmd = 0;
    uint16_t frame_len = 0;
    uint8_t checksum0 = 0, checksum1 = 0;

    if (size < 2) {
        return -2;
    }

    // 查找有效的帧头
    int header_pos = -1;
    for (i = 0; i < size - 1; i++) {
        if (data[i] == 0xEB && data[i+1] == 0x90) {
            header_pos = i;
            break;
        }
    }
    
    if (header_pos < 0) {
        if (size > 10) {
            scommHeaderErrCnt++;
            cerr << "Frame header not found" << endl;
            return -1;
        }
        return -2;
    }
    
    if (header_pos > 0) {
        memmove(S_Telec1_Data, data + header_pos, size - header_pos);
        size -= header_pos;
    } else {
        memcpy(S_Telec1_Data, data, size);
    }
    
    if (size < 6) {
        return -2;
    }

    cmd = S_Telec1_Data[2];
    frame_len = (uint16_t)(S_Telec1_Data[3] << 8) | (S_Telec1_Data[4]);
    
    if (size < (frame_len + 6)) {
        return -2;
    }

    /* 检查帧长度是否合理 */
    if (frame_len > 500) {
        cerr << "Frame length too large: " << frame_len << endl;
        return -1;
    }

    checksum0 = S_Telec1_Data[frame_len + 5];

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

    if (checksum1 != checksum0) {
        scommCheckErrCnt++;
        cerr << "Checksum error: calc=0x" << hex << (int)checksum1 
             << " recv=0x" << hex << (int)checksum0 << endl;
        return -1;
    }

    /* 根据命令类型处理 */
    switch (cmd) {
    case 0xF0:  /* 常规遥测数据 */
        if (size >= (13 + frame_len - 10)) {
            memcpy((uint8_t *)&S_TELE, (uint8_t *)&S_Telec1_Data[13],
                   frame_len - 10);
            if (sendGetSelfTelemCmderCnt > 0) {
                sendGetSelfTelemCmderCnt--;
            }

            if (frame_len - 10 > 0) {
                ComHS_telemetry_Pub((uint8_t *)&S_TELE, fastdds_dest,
                                    sizeof(S_Comm_telemetry_data_t));
            }
        }
        break;

    case 0xE1:  /* 遥控指令应答 */
        if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x8C)) {
            sendCmderSuccessCnt++;
        } else {
            sendCmderErrCnt++;
            cerr << "Command execution failed" << endl;
        }
        break;

    case 0xB4:  /* 上行遥控指令 */
        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);
            }
        }
        break;

    case 0xA5:  /* 遥测确认 */
        if ((S_Telec1_Data[5] == 0xAA) && (S_Telec1_Data[6] == 0x50)) {
            sendTelemSuccessCnt = 1;
        }
        break;

    default:
        scommResIDErrCnt++;
        cerr << "Unknown command ID: 0x" << hex << (int)cmd << endl;
        return -1;
    }

    return 0;
}

/*
 * 启动SCOMM服务（同步模式，使用select超时）
 *
 * @dev_telec: 遥控串口设备
 * @dev_telem: 遥测串口设备
 * @baudrate: 波特率
 */
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 << "S_COMM UART init failed" << endl;
        return;
    }

    S_COMM_ON_OFF = 1;
    ComHS_init(0, "S_Comm_Hardware_Service");
}

/*
 * 停止SCOMM服务
 */
void stop_scomm_service(void)
{
    g_running = 0;

    if (fd_telecontrol >= 0) {
        close(fd_telecontrol);
        fd_telecontrol = -1;
    }
    if (fd_telemetry >= 0) {
        close(fd_telemetry);
        fd_telemetry = -1;
    }

    if (ComHS_part) {
        delete ComHS_part;
        ComHS_part = nullptr;
    }
}