#include "pg0403_serial.h"
#include "app_device_common.h"
#include "drivers_common.h"
#include "modules_common.h"

// 预设指令
static uint8_t start_continuous[] = {0x01, 0x10, 0x00, 0x3B, 0x00, 0x01, 0x02, 0x00, 0x04, 0xA3, 0x18};
static uint8_t stop_modbus[] = {0x01, 0x10, 0x00, 0x3B, 0x00, 0x01, 0x02, 0x00, 0x00, 0xA2, 0xDB};

static uint16_t s_x_local[MAX_TAGS];
static uint16_t s_y_local[MAX_TAGS];
static int s_tag_id[MAX_TAGS]; // 假设 ID 是 int 类型

// 为每个可能的标签ID分配一个滤波器
static TagFilter s_filters[MAX_TAGS]; 

// 初始化滤波器数据的函数 (在 main 开始调用一次)
void init_filters() {
    for (int i = 0; i < MAX_TAGS; i++) {
        s_filters[i].initialized = 0;
        s_filters[i].kx.estimate = 0;
        s_filters[i].kx.error_cov = 1.0f;
        s_filters[i].ky.estimate = 0;
        s_filters[i].ky.error_cov = 1.0f;
    }
}
/**
 * 执行单次卡尔曼滤波更新
 * @param k: 滤波器状态指针
 * @param measurement: 当前传感器读到的原始坐标
 * @return: 滤波后的坐标
 */
float run_kalman(Kalman1D *k, float measurement) {
    // 1. 预测阶段 (Prediction)
    // 假设物体上一刻的位置就是下一刻的位置 (静止模型)，加上过程噪声 Q
    k->error_cov = k->error_cov + KALMAN_Q;

    // 2. 更新阶段 (Update)
    // 计算卡尔曼增益 (Kalman Gain)
    // 增益越大，越相信传感器数据；增益越小，越相信预测值
    float gain = k->error_cov / (k->error_cov + KALMAN_R);

    // 更新估计值
    k->estimate = k->estimate + gain * (measurement - k->estimate);

    // 更新误差协方差
    k->error_cov = (1.0f - gain) * k->error_cov;

    return k->estimate;
}



// 辅助函数：将整数波特率转换为 termios 定义的 speed_t
static speed_t get_baud_rate(int baud_rate) {
    switch (baud_rate) {
        case 9600: return B9600;
        case 19200: return B19200;
        case 38400: return B38400;
        case 57600: return B57600;
        case 115200: return B115200;
        case 230400: return B230400;
        case 460800: return B460800;
        case 921600: return B921600;
        default: return B9600; // 默认
    }
}

/**
 * 打开串口
 * 对应原 C++: bool SerialPort::open(speed_t baud_rate)
 */
static int serial_open(SerialPort *sp, const char *port_name, int baud_rate) {
    if (sp->is_open) {
        fprintf(stderr, "Warning: Port %s is already open.\n", sp->port_name);
        return 1; // 视为成功
    }

    // 保存端口名
    strncpy(sp->port_name, port_name, sizeof(sp->port_name) - 1);

    // 打开串口
    // O_RDWR: 读写模式
    // O_NOCTTY: 不作为控制终端
    // O_NDELAY: 非阻塞打开（防止DCD线不在线时在此阻塞）
    sp->fd = open(sp->port_name, O_RDWR | O_NOCTTY | O_NDELAY);
    if (sp->fd == -1) {
        fprintf(stderr, "Error: Failed to open serial port %s - %s\n", 
                sp->port_name, strerror(errno));
        return 0; // 失败
    }
    // 方法1：保持非阻塞标志
    //fcntl(sp->fd, F_SETFL, O_NONBLOCK);

    // 恢复为阻塞模式（对应原代码中的 fcntl(fd_, F_SETFL, 0)）
    fcntl(sp->fd, F_SETFL, 0);

    struct termios options;
    if (tcgetattr(sp->fd, &options) != 0) {
        fprintf(stderr, "Error: tcgetattr failed - %s\n", strerror(errno));
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }

    // 设置波特率
    speed_t speed = get_baud_rate(baud_rate);
    cfsetispeed(&options, speed);
    cfsetospeed(&options, speed);

    // 设置控制标志 (8N1 配置)
    options.c_cflag &= ~PARENB;   // 无校验
    options.c_cflag &= ~CSTOPB;   // 1位停止位
    options.c_cflag &= ~CSIZE;    // 清除数据位掩码
    options.c_cflag |= CS8;       // 8位数据位
    
    // 启用接收器并设置本地模式
    options.c_cflag |= (CLOCAL | CREAD);

    // 设置输入标志 (禁用软件流控)
    options.c_iflag &= ~(IXON | IXOFF | IXANY);

    // 设置本地标志 (Raw模式: 无规范模式，无回显，无信号)
    options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);

    // 设置输出标志 (Raw模式)
    options.c_oflag &= ~OPOST;

    // 设置控制字符
    // VMIN=0, VTIME=0: 纯非阻塞轮询读取（原代码逻辑）
    // 这意味着 read() 会立即返回，如果没有数据则返回0
    options.c_cc[VMIN] = 0;
    options.c_cc[VTIME] = 0;

    // 应用设置
    if (tcsetattr(sp->fd, TCSANOW, &options) != 0) {
        fprintf(stderr, "Error: tcsetattr failed - %s\n", strerror(errno));
        close(sp->fd);
        sp->fd = -1;
        return 0;
    }

    // 清空缓冲区
    tcflush(sp->fd, TCIOFLUSH);
    
    sp->is_open = 1;
   my_zlog_info("Serial port %s opened successfully.", sp->port_name);
    return 1; // 成功
}

/**
 * 发送数据
 * C语言中通常使用 write
 */
static int serial_send(SerialPort *sp, uint8_t *data, int len) {
    if (!sp->is_open) {
        my_zlog_error("Error: Port not open.");
        return -1;
    }

    int bytes_written = write(sp->fd, data, len);
    if (bytes_written < 0) {
        my_zlog_error("Error: Write failed - %s", strerror(errno));
    }
    return bytes_written;
}

/**
 * 关闭串口
 * 对应原 C++: void SerialPort::close()
 */
static void serial_close(SerialPort *sp) {
    // 原代码中有 stopListening(); C语言中如果没有线程需要手动停止，这里省略
    
    if (sp->is_open) {
        sp->is_open = 0;
        close(sp->fd);
        sp->fd = -1;
        my_zlog_info("Serial port %s closed.\n", sp->port_name);
    }
}

// 初始化结构体
static void serial_init(SerialPort *sp) {
    sp->fd = -1;
    sp->is_open = 0;
    memset(sp->port_name, 0, sizeof(sp->port_name));
}

static SerialPort s_pg_serial;

int pg0403_serial_init_send(){
    serial_init(&s_pg_serial);
    // 1. 设置端口号
    // 香橙派/树莓派的USB转串口通常是 /dev/ttyUSB0 或 /dev/ttyACM0
    // 板载UART通常是 /dev/ttyS1 或 /dev/ttyS3 (取决于具体型号和DT配置)
    const char *dev_path = "/dev/ttyUSB0"; 
    if (!serial_open(&s_pg_serial, dev_path, 115200)) {
        return -1;
    }
    serial_send(&s_pg_serial, start_continuous, sizeof(start_continuous));
    my_zlog_info("串口发送成功，长度:%d",sizeof(start_continuous));
    return 0;

}


// ==========================================
// 3. 核心处理函数 (对应 onSerialMessageReceived)
// ==========================================
/**
 * 解析并处理接收到的数据
 * @param buffer: 接收数据的缓冲区
 * @param len: 接收到的数据长度
 */
static void process_serial_data(const uint8_t *buffer, int len) {
    // 1. 基础长度检查
    if (len < 17) {
        return;
    }

    // 2. 校验特定标志位 (对应 message[10] == 0x01)
    if (buffer[10] == 0x01) {
        int i = (int)buffer[8]; // 获取索引

        // 3. 边界检查 (i > 0 && i < size)
        if (i >= 0 && i < MAX_TAGS) {
            
            // 4. 数据提取与合并 (高位在前，大端模式)
            // 对应: (static_cast<uint16_t>(message[13]) << 8) | message[14]
            uint16_t raw_x = ((uint16_t)buffer[13] << 8) | buffer[14];
            uint16_t raw_y = ((uint16_t)buffer[15] << 8) | buffer[16];


            s_x_local[i] = raw_x; // +0.5 为了四舍五入
            s_y_local[i] = raw_y;
            // // 3. 执行卡尔曼滤波
            // if (s_filters[i].initialized == 0) {
            //     // 如果是第一次收到这个标签的数据，直接初始化为测量值
            //     // 否则滤波需要很长时间才能从 0 爬升到 实际坐标
            //     s_filters[i].kx.estimate = (float)raw_x;
            //     s_filters[i].ky.estimate = (float)raw_y;
            //     s_filters[i].kx.error_cov = 1.0f;
            //     s_filters[i].ky.error_cov = 1.0f;
            //     s_filters[i].initialized = 1;

            //     s_x_local[i] = raw_x;
            //     s_y_local[i] = raw_y;
            // } else {
            //     // 后续数据进入滤波逻辑
            //     float filtered_x = run_kalman(&s_filters[i].kx, (float)raw_x);
            //     float filtered_y = run_kalman(&s_filters[i].ky, (float)raw_y);

            //     // 4. 将滤波结果转回整数存入全局变量
            //     s_x_local[i] = (uint16_t)(filtered_x + 0.5f); // +0.5 为了四舍五入
            //     s_y_local[i] = (uint16_t)(filtered_y + 0.5f);
            // }

            s_tag_id[i]=i;
            //my_zlog_debug("串口数据更新 -> 标签ID: %d, X: %d, Y: %d", s_tag_id[i], s_x_local[i], s_y_local[i]);
                
        }
    }
}


int pg0403_serial_init_close(){
    
    serial_send(&s_pg_serial, stop_modbus, sizeof(stop_modbus));
    serial_close(&s_pg_serial);
    return 0; 
}

// ==========================================
// 新增：构建并处理 JSON 数据的函数
// ==========================================
void pg0403_serial_gpssend_mqtt_json(int id, uint16_t x, uint16_t y) {

    cJSON *root = cJSON_CreateObject();
    if (root == NULL) {
        return;
    }
    cJSON *body = cJSON_CreateObject();
    cJSON *head = cJSON_CreateObject();

    cJSON_AddNumberToObject(head, "message_type", 1);

    cJSON_AddNumberToObject(body, "id", id);
    cJSON_AddNumberToObject(body, "resX", x);
    cJSON_AddNumberToObject(body, "resY", y);



    cJSON_AddItemToObject(root, "body", body);
    cJSON_AddItemToObject(root, "head",head);

    char *json_str = cJSON_PrintUnformatted(root);

    if (json_str == NULL) {
        cJSON_Delete(root);
        return;
    }
    
    my_zlog_info("[MQTT Payload]: %s", json_str);


    char mqtt_topic_pg[256];
    sprintf(mqtt_topic_pg,"positioning/%s",mqtt_topic_pure_number());

    for(int i=0;i<g_mqtt_cam_config_t->mqtt_count;i++){
        mosquitto_publish(g_clients_t[i].mosq, NULL, mqtt_topic_pg ,strlen(json_str), json_str, 0, false); 

        }

    free(json_str);     
    cJSON_Delete(root); 
}

void pg0403_all_serial_send(){
    for(int i=0;i<MAX_TAGS;i++){
        if(s_x_local[i]!=0) pg0403_serial_gpssend_mqtt_json(s_tag_id[i], s_x_local[i], s_y_local[i]);
    }
}

int pg0403_serial_run(){
    uint8_t rx_buffer[1024];
    int total_bytes = 0;

    my_zlog_info("设备为串口打开设备");
    init_filters();
    if(pg0403_serial_init_send()==0){
        my_zlog_info("串口启动成功");
    }else {
        my_zlog_info("串口启动失败");
        pg0403_serial_init_close();
        return -1;
    }  
    while(1){
        static int send_mqtt_count=0;
        int n = read(s_pg_serial.fd, rx_buffer, sizeof(rx_buffer));
        process_serial_data(rx_buffer, n);

        if(send_mqtt_count>50){
            pg0403_all_serial_send();
            send_mqtt_count=0;
        }
        delay_ms(10);
        send_mqtt_count++;
    }
    pg0403_serial_init_close();
    return 0;
    
}


