/*==========================================================================================================
  Copyright (c) 2021, Quectel Wireless Solutions Co., Ltd. All rights reserved.
  Quectel Wireless Solutions Proprietary and Confidential.
===========================================================================================================*/
/*New version does not need NTP sync*/
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include "ql_application.h"
#include "ql_log.h"
#include "ql_gnss.h"
#include "ql_rtos.h"
#include "ql_ntp.h"
#include "ql_data_call.h"
#include "sockets.h"
#include "netdb.h"
#include "app_inc.h"
#include "../../../../ql-config/config/board_features.h"

// 新增宏定义（原代码中未定义）
#define GNSS_FAST_MODE_HOLD_TIME    (5 * 60 * 1000)   // FAST模式持续5分钟后切SLOW

// Global variables
gnss_mode_t g_gnss_mode = GNSS_MODE_SLOW;       // Default SLOW mode (power save)
gps_location_info_t g_gps_location = {0};       // Global GPS position structure

// ====== SLOW MODE 卫星搜索控制（新逻辑） ======
static volatile int g_gnss_sat_count = 0;           // 当前可见卫星数量（从GSV解析）

static u8 gnss_is_open = 0;

// 新逻辑使用的常量
#define GNSS_SEARCH_INTERVAL       900              // 固定搜索间隔：15分钟 = 900秒
#define GNSS_MAX_SEARCH_TIME       180              // 最大搜索时间：3分钟 = 180秒
#define GNSS_SAT_CHECK_TIMEOUT_60     30               // 60秒卫星检测超时
#define GNSS_SAT_CHECK_TIMEOUT_90     60               // 60秒卫星检测超时

#define GNSS_SAT_MIN_COUNT         4                // 1分钟时要求的最小卫星数

u8 get_gnss_open_status(void)
{
	return gnss_is_open;
}

/*============================================================================
 * NMEA parsing function：From $--GGA sentence extract lat/lon/FS
 * Input: nmea_data - NMEA raw data
 * Output: Return 1 on success, Return 0 on failure
 * 
 * NMEA format: $--GGA,<time>,<Lat>,<N/S>,<Lon>,<E/W>,<FS>,<NoSV>,<HDOP>,<msl>,<M>,<Altref>,<M>,<DiffAge>,<DiffStation>*cs
 * FS (Fix Status): 0=无效, 1=GPS有效, 2=DGPS, 4=RTK固定, 5=RTK浮动, 6=估算
 * Latitude format: DDMM.MMMMM (degree-minute format) -> Convert to degrees
 * Longitude format: DDDMM.MMMMM (degree-minute format) -> Convert to degrees
 *===========================================================================*/
static int parse_gnmea_coordinates(const char *nmea_data, double *latitude, double *longitude, int *fix_status)
{
    char buffer[512];
    char *token;
    char *saveptr;
    int field_index = 0;
    char lat_str[32] = {0};
    char lat_dir[4] = {0};
    char lon_str[32] = {0};
    char lon_dir[4] = {0};
    char fs_str[8] = {0};

    if (nmea_data == NULL || latitude == NULL || longitude == NULL || fix_status == NULL) {
        return 0;
    }

    // Copy to modifiable buffer
    strncpy(buffer, nmea_data, sizeof(buffer) - 1);
    buffer[sizeof(buffer) - 1] = '\0';

    // Check if GGA sentence
    if (strstr(buffer, "GGA") == NULL) {
        return 0;
    }

    // Use strtok_r to split fields
    token = strtok_r(buffer, ",", &saveptr);

    while (token != NULL) {
        switch (field_index) {
            case 2:  // Latitude field (DDMM.MMMMM format)
                strncpy(lat_str, token, sizeof(lat_str) - 1);
                break;
            case 3:  // Latitude direction (N/S)
                strncpy(lat_dir, token, sizeof(lat_dir) - 1);
                break;
            case 4:  // Longitude field (DDDMM.MMMMM format)
                strncpy(lon_str, token, sizeof(lon_str) - 1);
                break;
            case 5:  // Longitude direction (E/W)
                strncpy(lon_dir, token, sizeof(lon_dir) - 1);
                break;
            case 6:  // Fix Status (FS): 0=无效, 1=GPS, 2=DGPS, 4=RTK固定, 5=RTK浮动, 6=估算
                strncpy(fs_str, token, sizeof(fs_str) - 1);
                break;
            default:
                break;
        }
        field_index++;
        token = strtok_r(NULL, ",", &saveptr);
    }

    // Parse Fix Status: 0=invalid, others=valid
    if (strlen(fs_str) == 0) {
        *fix_status = 0;  // No FS field, treat as invalid
    } else {
        *fix_status = atoi(fs_str);
    }

    // Check if valid data obtained (FS must be non-zero for valid position)
    if (strlen(lat_str) == 0 || strlen(lon_str) == 0 || *fix_status == 0) {
        return 0;
    }

    // Parse Latitude: DDMM.MMMMM -> degree
    // Example: 2311.71298 -> 23 + 11.71298/60 = 23.1952163
    {
        double lat_deg = 0, lat_min = 0;
        int lat_deg_int = 0;

        // Extract degree part (first 2 digits for latitude)
        lat_deg_int = (int)(atof(lat_str) / 100);
        lat_min = atof(lat_str) - (lat_deg_int * 100);
        *latitude = lat_deg_int + (lat_min / 60.0);

        // South latitude is negative
        if (lat_dir[0] == 'S' || lat_dir[0] == 's') {
            *latitude = -(*latitude);
        }
    }

    // Parse Longitude: DDDMM.MMMMM -> degree
    // Example: 11328.63118 -> 113 + 28.63118/60 = 113.4771863
    {
        double lon_deg = 0, lon_min = 0;
        int lon_deg_int = 0;

        // Extract degree part (first 3 digits for longitude)
        lon_deg_int = (int)(atof(lon_str) / 100);
        lon_min = atof(lon_str) - (lon_deg_int * 100);
        *longitude = lon_deg_int + (lon_min / 60.0);

        // West longitude is negative
        if (lon_dir[0] == 'W' || lon_dir[0] == 'w') {
            *longitude = -(*longitude);
        }
    }

    return 1;  // parse success
}


/*============================================================================
 * Parse GSV sentence to get total satellites in view
 * GSV format: $xxGSV,<total_msgs>,<msg_num>,<total_sats>,<sat1_prn>,<elev>,<azim>,<cn0>,...*cs
 * Field 3 (index 3): Total number of satellites in view
 * Return: satellite count on success, -1 on failure
 *===========================================================================*/
static int parse_gsv_sat_count(const char *nmea_data)
{
    char buffer[512];
    char *token;
    char *saveptr;
    int field_index = 0;
    int sat_count = -1;

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

    strncpy(buffer, nmea_data, sizeof(buffer) - 1);
    buffer[sizeof(buffer) - 1] = '\0';

    // Check if GSV sentence
    if (strstr(buffer, "GSV") == NULL) {
        return -1;
    }

    token = strtok_r(buffer, ",", &saveptr);
    while (token != NULL) {
        if (field_index == 3) {  // Total satellites in view
            sat_count = atoi(token);
            break;
        }
        field_index++;
        token = strtok_r(NULL, ",", &saveptr);
    }

    return sat_count;
}


/*============================================================================
 * GNSS callback function：Receive NMEA data, parse and save to global structure
 * 
 * Fix Status (FS)判断逻辑：
 *   FS = 0 : 无效定位（No Fix）
 *   FS = 1 : GPS定位有效（SPS）
 *   FS = 2 : DGPS定位有效
 *   FS = 4 : RTK固定解
 *   FS = 5 : RTK浮点解
 *   FS = 6 : 估算（Dead Reckoning）
 *   FS >= 1 均为有效定位
 *===========================================================================*/
void quec_gnss_callback(char *data, int len)
{
	int i;
	char nmea_buffer[512];
	int fix_status = 0;

	// 打印原始 NMEA 数据
	for (i = 0; i < len; i++) {
		printf("%c", data[i]);
	}

	// 在 data 中定位 GGA 句子的起止位置
	// GGA 格式: $xxGGA,...,*CS\r\n
	char *gga_start = NULL;
	char *gga_end = NULL;

	for (i = 0; i < len - 5; i++) {
		// 找 "$xxGGA" (兼容 GPGGA, GBGGA, GNGGA 等)
		if (data[i] == '$' && 
			data[i + 3] == 'G' && 
			data[i + 4] == 'G' && 
			data[i + 5] == 'A') {

			gga_start = &data[i];

			// 从 GGA 起始位置往后找行尾 (\r\n 或 \n)
			int j;
			for (j = i; j < len; j++) {
				if (data[j] == '\r' || data[j] == '\n') {
					break;
				}
			}
			gga_end = &data[j];  // 指向 \r 或 \n 的位置

			break; // 只取第一条 GGA
		}
	}

	// 找到 GGA 句子且长度合法
	if (gga_start != NULL && gga_end != NULL && gga_end > gga_start) {
		int gga_len = gga_end - gga_start;

		if (gga_len > 0 && gga_len < sizeof(nmea_buffer)) {
			// 拷贝 GGA 句子到缓冲区
			memcpy(nmea_buffer, gga_start, gga_len);
			nmea_buffer[gga_len] = '\0';

			// 保存原始 NMEA 到全局结构
			int raw_len = (gga_len < sizeof(g_gps_location.raw_nmea) - 1) ? 
						  gga_len : sizeof(g_gps_location.raw_nmea) - 1;
			memcpy(g_gps_location.raw_nmea, nmea_buffer, raw_len);
			g_gps_location.raw_nmea[raw_len] = '\0';

			// 解析坐标和定位状态
			double lat = 0, lon = 0;
			if (parse_gnmea_coordinates(nmea_buffer, &lat, &lon, &fix_status)) {
				g_gps_location.latitude = lat;
				g_gps_location.longitude = lon;
				// FS = 0 无效, FS >= 1 有效
				g_gps_location.is_valid = (fix_status >= 1) ? 1 : 0;
			} else {
				g_gps_location.is_valid = 0;
				printf("\n[GNSS Parse failed] FS=%d, %s\n", 
					   fix_status,
					   (fix_status == 0) ? "No Fix (Invalid)" : "Format error");
			}
		}
	}

	// ====== 解析 GSV 获取卫星数量 ======
	// GSV 格式: $xxGSV,<total_msgs>,<msg_num>,<total_sats>,...*cs
	char *gsv_start = NULL;
	char *gsv_end = NULL;

	for (i = 0; i < len - 5; i++) {
		if (data[i] == '$' && 
			data[i + 3] == 'G' && 
			data[i + 4] == 'S' && 
			data[i + 5] == 'V') {

			gsv_start = &data[i];

			int j;
			for (j = i; j < len; j++) {
				if (data[j] == '\r' || data[j] == '\n') {
					break;
				}
			}
			gsv_end = &data[j];

			break; // 只取第一条 GSV
		}
	}

	if (gsv_start != NULL && gsv_end != NULL && gsv_end > gsv_start) {
		int gsv_len = gsv_end - gsv_start;
		if (gsv_len > 0 && gsv_len < sizeof(nmea_buffer)) {
			memcpy(nmea_buffer, gsv_start, gsv_len);
			nmea_buffer[gsv_len] = '\0';

			int sat_count = parse_gsv_sat_count(nmea_buffer);
			if (sat_count >= 0) {
				g_gnss_sat_count = sat_count;
			}
		}
	}
}


/*============================================================================
 * Set GNSS positioning mode
 * Parameter: mode - GNSS_MODE_FAST(Fast30s) or GNSS_MODE_SLOW(Slow5min)
 *===========================================================================*/
void set_gnss_mode(gnss_mode_t mode)
{
#if !NFC_GNSS_BOARD_HAS_GNSS
    (void)mode;
    return; /* No GNSS task exists to leave FAST mode and allow sleep. */
#else
    g_gnss_mode = mode;
    printf("[GNSSMode] Set to %s Mode\n", 
           (mode == GNSS_MODE_FAST) ? "Fast (30s)" : "Slow (5min)");
#endif
}

gnss_mode_t get_gnss_mode(void)
{
	return g_gnss_mode;
}

/*============================================================================
 * Get current positioning interval (seconds) - 保留但不再使用
 *===========================================================================*/
static int get_gnss_interval(void)
{
    return 180; // 仅保留，新逻辑已固定15分钟
}

/*============================================================================
 * Get global GPS position info
 * Return: Pointer to global structure
 *===========================================================================*/
gps_location_info_t* get_gps_location_info(void)
{
    return &g_gps_location;
}

/*============================================================================
 * Print current GPS position info (For debug)
 *===========================================================================*/
void print_gps_location_info(void)
{
    if (g_gps_location.is_valid) {
        printf("[GPSinfo] Latitude: %.8f, Longitude: %.8f\n", 
               g_gps_location.latitude, g_gps_location.longitude);
        printf("[GPSinfo] rawNMEA: %s\n", g_gps_location.raw_nmea);
    } else {
        printf("[GPSinfo] No valid position data\n");
    }
}


/*============================================================================
 * GNSS test task main function
 * 
 * FAST MODE: GPS持续开启，每45秒上报一次定位，持续5分钟后自动切到SLOW
 * SLOW MODE: 固定每15分钟搜索一次，搜索逻辑：
 *   - 开启GPS后，30秒内检测是否搜到卫星（通过GSV解析sat_count）
 *   - 若30秒内卫星数为0，立即关闭GPS，等待15分钟
 *   - 若30秒内搜到卫星，继续等待至1分钟，检查卫星数量是否 >= 3
 *   - 若1分钟时卫星数 < 3，立即关闭GPS，等待15分钟
 *   - 若卫星数 >= 3，继续定位，但总搜索时间最长3分钟
 *   - 3分钟后关闭GPS，无论是否获得有效定位，等待15分钟
 *===========================================================================*/
static void quec_gnss_test(void * argv)
{

    u32 fast_mode_start_time = 0;
    u32 slow_mode_start_time = 0;
    u32 slow_next_wake_time = 0;    // SLOW模式下下次唤醒时间

    // 初始化 GNSS 配置
    ql_gnss_cfgsys_set(1);          // BDS
    ql_gnss_cfgmsg_set(31);         // GGA output
    ql_gnss_agps_cfg(AGPS_GNSS_ENABLE, 0);
    ql_gnss_apflash_cfg(APFLASH_GNSS_ENABLE);
    ql_set_gnss_nmea_mode(PACKET_MODE);

    while (1)
    {
        if (g_gnss_mode == GNSS_MODE_FAST)
        {
            /* ========== FAST MODE ========== */
            if (fast_mode_start_time == 0) {
                fast_mode_start_time = hal_os_up_time_ms();
                printf("[GNSS] Enter FAST mode\n");
            }

            if (!gnss_is_open) {
				gnss_is_open = 1;
				app_set_app_operation(1);
                ql_gnss_open(quec_gnss_callback);
                printf("[GNSS] FAST: GPS OPEN\n");
            }

            // 5分钟超时切SLOW
            if (hal_os_up_time_ms() - fast_mode_start_time >= GNSS_FAST_MODE_HOLD_TIME) {
                printf("[GNSS] FAST 5min timeout -> SLOW\n");
                g_gnss_mode = GNSS_MODE_SLOW;
                fast_mode_start_time = 0;
                slow_mode_start_time = 0;
                slow_next_wake_time = 0;
				
                gnss_is_open = 0;
                printf("[GNSS] SLOW: GPS CLOSED ret:%d\n", ql_gnss_close());
            }
        }
        else
        {
            /* ========== SLOW MODE ========== */
            u32 now = hal_os_up_time_ms();

            // 首次进入或重置后初始化
            if (slow_mode_start_time == 0) {
                slow_mode_start_time = now;
                slow_next_wake_time = now;          // 立即尝试第一次定位
                printf("[GNSS] Enter SLOW mode (fixed 15min interval)\n");
            }

            // 检查是否到达唤醒时间
            if (now < slow_next_wake_time) {
                printf("[GNSS] SLOW: next wake in %d s\n",
                       (slow_next_wake_time - now) / 1000);
            }
            else if (!gnss_is_open) {
                /* ===== 开始新一轮搜索 ===== */
                u32 search_start_time = now;
                g_gnss_sat_count = 0;
                g_gps_location.is_valid = 0;

				gnss_is_open = 1;
				app_set_app_operation(1);
                ql_gnss_open(quec_gnss_callback);

                printf("[GNSS] SLOW: GPS OPEN, search start\n");

                /* ---- 第一阶段：60秒卫星检测 ---- */
                int sat_found = 0;
                int elapsed = 0;
                while (elapsed < GNSS_SAT_CHECK_TIMEOUT_60) {
                    ql_rtos_task_sleep_s(1);
                    elapsed++;
                    if (g_gnss_sat_count > 1) {
                        sat_found = 1;
                        printf("[GNSS] SLOW: Sat found! count=%d, after %ds\n",
                               g_gnss_sat_count, elapsed);
                        break;
                    }
                }

                if (!sat_found) {
                    // 60秒无卫星 → 立即关闭，等待15分钟
                    printf("[GNSS] SLOW: No satellite in 30s, stop ret:%d.\n", ql_gnss_close());
                    gnss_is_open = 0;
                    slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
                    printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
                    continue;
                }

                /* ---- 第二阶段：等待至90分钟，检查卫星数量是否 >= 3 ---- */
                // 此时已过 elapsed 秒 (<=30)，再等待至总时间达到60秒
                while (elapsed < GNSS_SAT_CHECK_TIMEOUT_90) {
                    ql_rtos_task_sleep_s(1);
                    elapsed++;
                }

                // 1.5分钟时刻检查卫星数
                int sat_count_now = g_gnss_sat_count;
                printf("[GNSS] SLOW: 1min check, satellite count = %d\n", sat_count_now);
                if (sat_count_now < GNSS_SAT_MIN_COUNT) {
                    printf("[GNSS] SLOW: Sat count < %d, stop search ret:%d.\n", GNSS_SAT_MIN_COUNT,ql_gnss_close());
                    gnss_is_open = 0;
                    slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
                    printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
                    continue;
                }

                /* ---- 第三阶段：卫星足够，继续定位，但总时间不超过3分钟 ---- */
                printf("[GNSS] SLOW: Sat count OK, continue for fix (max %ds total)\n",
                       GNSS_MAX_SEARCH_TIME);

                // 继续等待直到总时间达到 GNSS_MAX_SEARCH_TIME (180秒)
                while (elapsed < GNSS_MAX_SEARCH_TIME) {
                    ql_rtos_task_sleep_s(1);
                    elapsed++;

                    // 如果获得有效定位，打印提示（但继续等待至3分钟）
                    if (g_gps_location.is_valid) {
                        printf("[GNSS] SLOW: Fix obtained at %ds!\n", elapsed);
                        print_gps_location_info();
                        // 若希望在定位成功后立即关闭，可在此 break，但此处按“最长3分钟”处理
                    }
                }

                // 3分钟时间到，关闭GPS
                printf("[GNSS] SLOW: Max search time reached, stop.\n");
                if (g_gps_location.is_valid) {
                    printf("[GNSS] SLOW: Final valid position:\n");
                    print_gps_location_info();
                } else {
                    printf("[GNSS] SLOW: No valid fix within 3min.\n");
                }

                printf("[GNSS] SLOW: GPS CLOSED ret:%d",ql_gnss_close());
                gnss_is_open = 0;
                g_gps_location.is_valid = 0;   // 重置状态

                // 设置下次唤醒时间为 15分钟 后
                slow_next_wake_time = search_start_time + (GNSS_SEARCH_INTERVAL * 1000);
                printf("[GNSS] SLOW: GPS CLOSED, next search at +%ds\n", GNSS_SEARCH_INTERVAL);
            }
            // else: GPS正在运行中（理论上不会到这里）
        }

        // 统一10秒sleep，避免任务空转
        ql_rtos_task_sleep_s(5);
    }
}

void app_gnss_init(void)
{
#if !NFC_GNSS_BOARD_HAS_GNSS
    printf("[GNSS] Legacy board: positioning disabled\n");
    return;
#else
    ql_task_t gnssTaskRef = NULL;
    ql_rtos_task_create(&gnssTaskRef, 1024*4, 100, "gnss_Task_Ref", quec_gnss_test, NULL);
#endif
}