#include "gps_buffer.h"
#include <pthread.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <sys/socket.h>
#include <netdb.h>
#include <math.h>
#include <errno.h> // 确保包含 errno

// 全局共享变量
static gps_video_sample_t g_current_gps_data = {0.0, 0.0};
static volatile int g_running = 0;
static pthread_t g_gps_thread;
static pthread_mutex_t g_gps_mutex = PTHREAD_MUTEX_INITIALIZER;

static double normalize_coordinate(double coordinate) {
    return round(coordinate * 1000000.0) / 1000000.0;
}

static void store_gps(double latitude, double longitude) {
    pthread_mutex_lock(&g_gps_mutex);
    g_current_gps_data.latitude = normalize_coordinate(latitude);
    g_current_gps_data.longitude = normalize_coordinate(longitude);
    pthread_mutex_unlock(&g_gps_mutex);
}

static void clear_gps(void) {
    pthread_mutex_lock(&g_gps_mutex);
    g_current_gps_data.latitude = 0.0;
    g_current_gps_data.longitude = 0.0;
    pthread_mutex_unlock(&g_gps_mutex);
}

static gps_video_sample_t load_gps(void) {
    gps_video_sample_t sample;

    pthread_mutex_lock(&g_gps_mutex);
    sample = g_current_gps_data;
    pthread_mutex_unlock(&g_gps_mutex);
    return sample;
}

static void gps_sleep_before_retry(void) {
    int retry_ms = 1000;
    int step_ms = 100;
    int elapsed_ms = 0;

    while (g_running && elapsed_ms < retry_ms) {
        usleep((useconds_t) step_ms * 1000U);
        elapsed_ms += step_ms;
    }
}

// 将经纬度规范化为 double，保留 6 位小数。
static int normalize_gps(double latitude, double longitude, gps_video_sample_t* sample) {
    if (!isfinite(latitude) || !isfinite(longitude)) {
        return -1;
    }
    // 过滤掉 0,0 这种无效坐标
    if (fabs(latitude) < 1e-6 && fabs(longitude) < 1e-6) {
        return -1;
    }

    if (sample == NULL) {
        return -1;
    }

    sample->latitude = normalize_coordinate(latitude);
    sample->longitude = normalize_coordinate(longitude);
    return 0;
}

// =================================================================
// 以下是借鉴 gps_parse.c 实现的底层解析函数
// =================================================================

// 1. 辅助函数：在 JSON 字符串中查找键对应的值的起始位置
static const char* find_json_value(const char* json, const char* key) {
    char pattern[64];
    int written;
    const char* position;

    if (json == NULL || key == NULL) return NULL;

    // 构建搜索模式: "key":
    written = snprintf(pattern, sizeof(pattern), "\"%s\":", key);
    if (written < 0 || (size_t)written >= sizeof(pattern)) {
        return NULL;
    }

    position = strstr(json, pattern);
    if (position == NULL) {
        return NULL;
    }

    // 跳过 "key":
    position += written;

    // 跳过可能存在的空格
    while (*position == ' ' || *position == '\t') {
        position++;
    }

    return position;
}

// 2. 解析函数：从 JSON 字符串中提取 Double 类型的值
static int json_extract_double(const char* json, const char* key, double* value) {
    const char* position;
    char* endptr = NULL;
    double parsed;

    position = find_json_value(json, key);
    if (position == NULL) {
        return 0; // 键不存在
    }

    // 确保当前位置是数字或负号
    if (*position != '-' && !(*position >= '0' && *position <= '9')) {
        return 0;
    }

    // 重置 errno 以检测错误
    errno = 0;
    parsed = strtod(position, &endptr);

    // 检查转换是否成功
    if (errno != 0 || endptr == position || !isfinite(parsed)) {
        return 0;
    }

    *value = parsed;
    return 1;
}

// 3. 解析函数：从 JSON 字符串中提取 Int 类型的值
static int json_extract_int(const char* json, const char* key, int* value) {
    double dval;
    if (json_extract_double(json, key, &dval)) {
        *value = (int)dval;
        return 1;
    }
    return 0;
}

// 4. 检查是否为 TPV (定位数据) 包
static int is_tpv_class(const char* json) {
    char class_buf[32] = {0};
    const char* pos = find_json_value(json, "class");
    if (pos == NULL || *pos != '"') return 0;
    
    // 简单提取 class 的值 (TPV/SKY/DEVICES)
    sscanf(pos, "\"%31[^\"]\"", class_buf);
    return (strcmp(class_buf, "TPV") == 0);
}

// =================================================================
// 后台线程函数：负责连接 gpsd 并更新全局变量
// =================================================================
void* gps_update_thread(void* arg) {
    const char* host = (const char*)arg;
    const char* gpsd_host = (host != NULL && host[0] != '\0') ? host : "127.0.0.1";

    while (g_running) {
        int sockfd = -1;
        struct addrinfo hints;
        struct addrinfo *res = NULL;
        struct addrinfo *rp = NULL;
        int s;
        char buffer[4096];
        size_t offset = 0;

        // 1. 解析地址并连接 gpsd (默认端口 2947)
        memset(&hints, 0, sizeof(hints));
        hints.ai_family = AF_UNSPEC;    // 兼容 IPv4/IPv6
        hints.ai_socktype = SOCK_STREAM;

        s = getaddrinfo(gpsd_host, "2947", &hints, &res);
        if (s != 0) {
            fprintf(stderr, "GPS线程: 解析 gpsd 地址失败 %s:2947: %s\n", gpsd_host, gai_strerror(s));
            gps_sleep_before_retry();
            continue;
        }

        // 尝试连接每一个解析出来的地址
        for (rp = res; rp != NULL; rp = rp->ai_next) {
            sockfd = socket(rp->ai_family, rp->ai_socktype, rp->ai_protocol);
            if (sockfd == -1) {
                continue;
            }

            if (connect(sockfd, rp->ai_addr, rp->ai_addrlen) != -1) {
                break;
            }
            close(sockfd);
            sockfd = -1;
        }

        freeaddrinfo(res);

        if (sockfd < 0) {
            fprintf(stderr, "GPS线程: 无法连接到 %s:2947，1 秒后重试\n", gpsd_host);
            gps_sleep_before_retry();
            continue;
        }

        printf("GPS线程: 已连接到 gpsd %s\n", gpsd_host);

        // 2. 发送 WATCH 命令，开启 JSON 流
        {
            const char* watch_cmd = "?WATCH={\"enable\":true,\"json\":true};\n";

            if (send(sockfd, watch_cmd, strlen(watch_cmd), 0) < 0) {
                perror("GPS线程: 发送 WATCH 命令失败");
                close(sockfd);
                gps_sleep_before_retry();
                continue;
            }
        }

        // 3. 主循环：读取并解析数据流
        // 注意：gpsd 数据是以 \n 结尾的，不能直接用固定长度 recv
        while (g_running) {
            ssize_t len = recv(sockfd, buffer + offset, sizeof(buffer) - 1 - offset, 0);

            if (len <= 0) {
                break;
            }

            offset += (size_t) len;
            buffer[offset] = '\0'; // 确保字符串结束

            // 查找换行符 \n，因为一条完整的 JSON 消息以 \n 结尾
            char* start = buffer;
            char* end;

            while ((end = memchr(start, '\n', (buffer + offset) - start)) != NULL) {
                *end = '\0'; // 临时截断，形成独立字符串

                // --- 核心解析逻辑 ---
                // 1. 检查是否为 TPV 数据包
                if (is_tpv_class(start)) {
                    double lat = 0.0;
                    double lon = 0.0;
                    int mode = 0;
                    int has_fix = 0;

                    // 2. 提取定位模式 (mode: 1=无定位, 2=2D, 3=3D)
                    if (json_extract_int(start, "mode", &mode)) {
                        has_fix = (mode >= 2);
                    }

                    // 3. 如果有定位，提取经纬度
                    if (has_fix) {
                        int got_lat = json_extract_double(start, "lat", &lat);
                        int got_lon = json_extract_double(start, "lon", &lon);

                        if (got_lat && got_lon) {
                            gps_video_sample_t sample;

                            // 4. 更新全局共享变量，使用 double 直接携带经纬度。
                            if (normalize_gps(lat, lon, &sample) == 0) {
                                store_gps(sample.latitude, sample.longitude);
                            }
                            // 调试：取消注释可查看实时经纬度
                            // printf("更新GPS: lat=%.6f, lon=%.6f\n", lat, lon);
                        }
                    }
                    // 如果无定位，这里不操作，保持上一次的有效值
                }
                // --- 解析结束 ---

                // 移动指针到下一条消息
                start = end + 1;
            }

            // 处理完所有完整消息后，将剩余未处理的数据移到缓冲区头部
            if (start < buffer + offset) {
                size_t remaining = (size_t) ((buffer + offset) - start);
                memmove(buffer, start, remaining);
                offset = remaining;
            } else {
                offset = 0; // 缓冲区已清空
            }
        }

        close(sockfd);
        if (g_running) {
            fprintf(stderr, "GPS线程: 连接断开，1 秒后重连...\n");
            gps_sleep_before_retry();
        }
    }

    return NULL;
}

// =================================================================
// 接口函数实现
// =================================================================
gps_video_sample_t get_latest_gps_for_video(void) {
    return load_gps();
}

int gps_buffer_init(const char* host) {
    if (g_running) return 0;
    
    g_running = 1;
    clear_gps();
    pthread_attr_t attr;
    pthread_attr_init(&attr);
    pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
    // 创建后台线程
    if (pthread_create(&g_gps_thread, &attr, gps_update_thread, (void*)host) != 0) {
        g_running = 0;
        pthread_attr_destroy(&attr); // 清理属性
        perror("无法创建 GPS 线程");
        return -1;
    }
    pthread_attr_destroy(&attr); // 清理属性
    return 0;
}

void gps_buffer_cleanup(void) {
    g_running = 0;
    // 等待线程结束

    usleep(10000); // 等待 100ms 让后台线程有机会处理退出标志
}


//gcc main.c video_pipeline_run.c gps_buffer.c -lpthread -lm -o my_app 请确保在编译命令中链接 pthread 和 m (math) 库