// Pavo2 Lidar SDK - 雷达数据采集程序
// 功能1：获取单帧雷达数据 + 输出鸟瞰图(ppm)
// 功能2：获取多秒雷达数据，输出 x,y,时间戳(ms) 文件 + 深度图(ppm)

#include <iostream>
#include <fstream>
#include <vector>
#include <chrono>
#include <iomanip>
#include <thread>
#include <mutex>
#include <cmath>
#include <algorithm>
#include <cstring>
#include <cstdlib>
#include <ctime>
#include <direct.h>   // _mkdir
#include <sstream>
#include <unordered_map>

// pavo2_driver.h 内部会引入 winsock2.h，必须在 windows.h 之前
// NOMINMAX: MinGW GCC 在 os_defines.h 中已自动定义，无需手动定义
//#include "pavo2_driver.h"
#include "PAVO2_SDK/include/pavo2_driver.h"
#ifdef _WIN32
#include <windows.h>
#endif

// stb single-header JPEG 编码器
#define STB_IMAGE_WRITE_IMPLEMENTATION
#include "PAVO2_SDK/include/stb_image_write.h"

// ==================== 配置参数 ====================
//const std::string LIDAR_IP   = "192.168.0.170";  // 雷达IP地址（必填）
//const uint16_t    Leida_PORT = 2368;              // 雷达数据端口
const uint32_t    OPEN_TIMEOUT_MS  = 3000;        // 连接超时(ms)
const uint16_t    FRAME_TIMEOUT_MS = 2000;        // 获取帧数据超时(ms)
//driver.pavo2_open(Leida_IP, Leida_PORT, OPEN_TIMEOUT_MS)//0713改
// -------------------配置参数 0713增加------------------//
static std::string Leida_IP      = "192.168.0.170";
static uint16_t    Leida_PORT    = 2368;
static int         Collect_Duration_S  = 6;
static double      Speed_Z    = 20.0;
static bool        Log_Flag    = true;
static double      Z_RANGE_MM   = 50.0;    // Z截面区间长度(mm)
static double      Y_RANGE_MM   = 80.0;    // Y截面区间长度(mm): 只取[Y最小, Y最小+此值]的点参与
static double      Lidar_Hz     = 50.0;    // 雷达帧率(Hz)，用于计算矩形长
static int         Rect_Width_PX = 8;      // 截面矩形宽度(px)
static double      Rect_Length_Ratio = 1.5; // 矩形长倍率: 矩形长=速度/帧率×倍率
static std::string Run_Mode     = "exe";   // exe=直接检测一次, tcp=服务端模式
static uint16_t    Tcp_Port     = 9601;    // TCP模式监听端口
static double      Floor_Height = 1600.0;  // 地板高度(mm): 物品高度 = Floor_Height - Y轴(count计数用)
static double      Count_Base_Tolerance = 50.0;                 // 分层初始冗余 50mm(count分层用)
static double      Count_Additional_Tolerance_Per_Level = 20.0; // 每层增加 20mm 冗余(count分层用)
static int         Point_Dilate = 8;                 // count画layer图: 每个点膨胀 Point_Dilate×Point_Dilate 像素
static std::string Z_Interpolate_Mode = "none";      // count z轴空隙插值模式: none=不插值 / CopyTranslation=同圈复制平移 / AngleBucketInterpolation=角度桶插值

static int  Tuo_Wei    = 2;   // 拖尾滤波0=关, 1=弱, 2=中, 3=强
static int  Qian_Chong = 2;   //前冲滤波0=关, 1=弱, 2=中, 3=强
static int  Yuan_Duan  = 1;   // 远端高反滤波0=关, 1=弱, 2=强
static bool Jin_Duan   = true; // 近端高反识别true开，false关
//----------------------------------------------------//
// 多边形过滤开关（false 则禁用，全部保留）
const bool FILTER_ENABLE = true;

// 多边形过滤区域（顶点坐标，单位: mm）

static std::vector<std::pair<double, double>> Filter_Polygon = {
    {-650, 0}, {-650, 670}, {650, 670}, {650, 0}
};
static int Filter_Poly_N = (int)Filter_Polygon.size();  // 自动获取顶点数  // �Զ���ȡ������

// 当 true 时，所有Y值上移使最小Y≥0（消除负数）
const bool SHIFT_Y_POSITIVE = true;

// Z轴：速度(cm/s)，0=使用时间(ms)，>0=Z轴=速度×时间=移动距离(mm)
//const double Speed_Z = 20.0;  

// 过滤映射到原点(0,0)的无效点（雷达噪点）
// x和y同时接近0时视为原点无效点，丢弃该点
//---------------加载配置文件---------------------------//
// 去除字符串首尾空白
static std::string trim_str(const std::string& s)
{
    size_t b = s.find_first_not_of(" \t\r\n");
    if (b == std::string::npos) return "";
    size_t e = s.find_last_not_of(" \t\r\n");
    return s.substr(b, e - b + 1);
}

// GBK(ANSI) → UTF-8：日志文件按UTF-8写入，避免编辑器打开乱码
// （exe内字符串按 -fexec-charset=GBK 编译，控制台GBK显示正常，写文件需转UTF-8）
static std::string gbk_to_utf8(const std::string& s)
{
#ifdef _WIN32
    if (s.empty()) return s;
    int wlen = MultiByteToWideChar(CP_ACP, 0, s.c_str(), (int)s.size(), nullptr, 0);
    if (wlen <= 0) return s;
    std::wstring wbuf(wlen, L'\0');
    MultiByteToWideChar(CP_ACP, 0, s.c_str(), (int)s.size(), &wbuf[0], wlen);
    int ulen = WideCharToMultiByte(CP_UTF8, 0, wbuf.c_str(), wlen, nullptr, 0, nullptr, nullptr);
    if (ulen <= 0) return s;
    std::string ubuf(ulen, '\0');
    WideCharToMultiByte(CP_UTF8, 0, wbuf.c_str(), wlen, &ubuf[0], ulen, nullptr, nullptr);
    return ubuf;
#else
    return s;
#endif
}

void load_config(const std::string& path = "./config/config")
{
    std::ifstream fin(path);
    if (!fin.is_open()) {
        std::cerr << "[WARN] 未找到 config配置文件" << std::endl;
        exit(-1);
    }

    std::string line;
    while (std::getline(fin, line)) {
        if (line.find("Leida_Ip:") == 0) {
            Leida_IP = line.substr(line.find(':') + 1);
        }
        else if (line.find("Leida_Port:") == 0) {
            Leida_PORT = static_cast<uint16_t>(std::stoi(line.substr(line.find(':') + 1)));
        }
        else if (line.find("Collect_Duration_S:") == 0) {
           Collect_Duration_S = std::stoi(line.substr(line.find(':') + 1));
        }
        else if (line.find("Speed_Z:") == 0) {
            Speed_Z = std::stod(line.substr(line.find(':') + 1));
        }
        else if (line.find("Log_Flag:") == 0) {
            std::string flag = line.substr(line.find(':') + 1);
            Log_Flag = (flag == "YES" || flag == "yes" || flag == "Yes");
        }
        else if (line.find("Z_Range_MM:") == 0) {
            Z_RANGE_MM = std::stod(line.substr(line.find(':') + 1));
        }
        else if (line.find("Y_Range_MM:") == 0) {
            Y_RANGE_MM = std::stod(line.substr(line.find(':') + 1));
        }
        else if (line.find("Lidar_Hz:") == 0) {
            Lidar_Hz = std::stod(line.substr(line.find(':') + 1));
        }
        else if (line.find("Rect_Width_PX:") == 0) {
            Rect_Width_PX = std::stoi(line.substr(line.find(':') + 1));
        }
        else if (line.find("Rect_Length_Ratio:") == 0) {
            Rect_Length_Ratio = std::stod(line.substr(line.find(':') + 1));
        }
        else if (line.find("Run_Mode:") == 0) {
            Run_Mode = trim_str(line.substr(line.find(':') + 1));
        }
        else if (line.find("Tcp_Port:") == 0) {
            Tcp_Port = static_cast<uint16_t>(std::stoi(line.substr(line.find(':') + 1)));
        }
        
        else if (line.find("Filter_Polygon:") == 0) {
            Filter_Polygon.clear();
            std::string data = line.substr(line.find(':') + 1);
            // 格式: {-650,0},{-650,670},{650,670},{650,0}
            size_t pos = 0;
            while (pos < data.size()) {
                size_t lb = data.find('{', pos);
                if (lb == std::string::npos) break;
                size_t rb = data.find('}', lb);
                if (rb == std::string::npos) break;
                std::string pair = data.substr(lb + 1, rb - lb - 1);
                size_t comma = pair.find(',');
                if (comma != std::string::npos) {
                    double x = std::stod(pair.substr(0, comma));
                    double y = std::stod(pair.substr(comma + 1));
                    Filter_Polygon.push_back({x, y});
                }
                pos = rb + 1;
            }
            Filter_Poly_N = (int)Filter_Polygon.size();
        }
        else if (line.find("Tuo_Wei:") == 0) {
				Tuo_Wei = std::stoi(line.substr(line.find(':') + 1));
				if (Tuo_Wei < 0) Tuo_Wei = 0;
				if (Tuo_Wei > 3) Tuo_Wei = 3;
		}
		else if (line.find("Qian_Chong:") == 0) {
				Qian_Chong = std::stoi(line.substr(line.find(':') + 1));
				if (Qian_Chong < 0) Qian_Chong = 0;
				if (Qian_Chong > 3) Qian_Chong = 3;
		}
		else if (line.find("Yuan_Duan:") == 0) {
				Yuan_Duan = std::stoi(line.substr(line.find(':') + 1));
				if (Yuan_Duan < 0) Yuan_Duan = 0;
				if (Yuan_Duan > 2) Yuan_Duan = 2;
		}
		else if (line.find("Jin_Duan:") == 0) {
				std::string val = line.substr(line.find(':') + 1);
				Jin_Duan = (val == "true" || val == "TRUE" || val == "True" ||
							val == "yes" || val == "YES" || val == "Yes");
		}
		else if (line.find("Floor_Height:") == 0) {
				Floor_Height = std::stod(line.substr(line.find(':') + 1));
		}
		else if (line.find("Count_Base_Tolerance:") == 0) {
				Count_Base_Tolerance = std::stod(line.substr(line.find(':') + 1));
				if (Count_Base_Tolerance < 0) Count_Base_Tolerance = 0;
		}
		else if (line.find("Count_Additional_Tolerance_Per_Level:") == 0) {
				Count_Additional_Tolerance_Per_Level = std::stod(line.substr(line.find(':') + 1));
				if (Count_Additional_Tolerance_Per_Level < 0) Count_Additional_Tolerance_Per_Level = 0;
		}
		else if (line.find("Point_Dilate:") == 0) {
				Point_Dilate = std::stoi(line.substr(line.find(':') + 1));
				if (Point_Dilate < 1) Point_Dilate = 1;
				if (Point_Dilate > 64) Point_Dilate = 64;   // 上限保护，防止内存爆炸
		}
		else if (line.find("Z_Interpolate_Mode:") == 0) {
				std::string mode = line.substr(line.find(':') + 1);
				// 去掉首尾空白
				size_t b = mode.find_first_not_of(" \t\r");
				if (b != std::string::npos) mode = mode.substr(b);
				size_t e = mode.find_last_not_of(" \t\r");
				if (e != std::string::npos) mode = mode.substr(0, e + 1);
				if (mode == "CopyTranslation" || mode == "AngleBucketInterpolation")
						Z_Interpolate_Mode = mode;
				else
						Z_Interpolate_Mode = "none";
		}
    
    }
    fin.close();
}
const double ORIGIN_TOL_MM = 0.5;  // mm，判定为原点的容差
inline bool is_origin_point(double x_mm, double y_mm) {
    return std::abs(x_mm) < ORIGIN_TOL_MM && std::abs(y_mm) < ORIGIN_TOL_MM;
}

// ==================== 多边形过滤工具 ====================

// 射线法判断点是否在多边形内（支持凹/凸多边形）
// px, py: 点坐标, poly: 顶点数组（与点坐标相同单位）, n: 顶点数
bool point_in_polygon(double px, double py,
                      const std::vector<std::pair<double, double>>& poly, int n)
{
    bool inside = false;
    for (int i = 0, j = n - 1; i < n; j = i++)
    {
        double xi = poly[i].first, yi = poly[i].second;
        double xj = poly[j].first, yj = poly[j].second;

        if (((yi > py) != (yj > py)) &&
            (px < (xj - xi) * (py - yi) / (yj - yi) + xi))
        {
            inside = !inside;
        }
    }
    return inside;
}

// ==================== 截面切片配置 ====================
// 2D映射图尺寸
const int SLICE_IMG_SIZE = 800;             // 输出图片最大像素尺寸

// ==================== PCD 文件保存 ====================

void save_pcd_ascii(const std::string& filename,
                    const std::vector<double>& xs,
                    const std::vector<double>& ys,
                    const std::vector<double>& zs)
{
    size_t n = xs.size();
    // 一次性构建字符串再写入，避免逐行IO（大量点数时显著提速）
    std::ostringstream oss;
    oss << "# .PCD v0.7 - Point Cloud Data file format\n"
        << "VERSION 0.7\n"
        << "FIELDS x y z\n"
        << "SIZE 4 4 4\n"
        << "TYPE F F F\n"
        << "COUNT 1 1 1\n"
        << "WIDTH " << n << "\n"
        << "HEIGHT 1\n"
        << "VIEWPOINT 0 0 0 1 0 0 0\n"
        << "POINTS " << n << "\n"
        << "DATA ascii\n"
        << std::fixed << std::setprecision(1);
    for (size_t i = 0; i < n; i++) {
        oss << xs[i] << " " << ys[i] << " " << zs[i] << "\n";
    }
    std::string buf = oss.str();
    std::ofstream f(filename, std::ios::binary);
    f.write(buf.data(), (std::streamsize)buf.size());
    f.close();
}



// 保存 JPG 灰度图（灰度转RGB后压缩为JPEG，quality=90）
bool save_jpg_gray(const std::string& filename, int width, int height,
                   const std::vector<uint8_t>& pixels, int quality = 90)
{
    // stbi_write_jpg 需要 RGB 三通道数据
    std::vector<uint8_t> rgb(width * height * 3);
    for (int i = 0; i < width * height; i++) {
        uint8_t v = pixels[i];
        rgb[i * 3 + 0] = v;
        rgb[i * 3 + 1] = v;
        rgb[i * 3 + 2] = v;
    }
    int ret = stbi_write_jpg(filename.c_str(), width, height, 3, rgb.data(), quality);
    return ret != 0;
}

// ==================== 连接雷达 ====================
bool connect_lidar(pavo2::pavo2_driver& driver)
{
    driver.set_conn_protocol(pavo2::PAVO2_PROTO_TYPE::TCP);

    std::cout << "[连接] 正在连接雷达 " << Leida_IP << ":" << Leida_PORT << " ..." << std::endl;

    if (!driver.pavo2_open(Leida_IP, Leida_PORT, OPEN_TIMEOUT_MS))
    {
        std::cerr << "[错误] 连接雷达失败！请检查IP和网络连接。" << std::endl;
        return false;
    }

    std::cout << "[连接] 雷达连接成功！" << std::endl;
    
    driver.enable_tail_filter(Tuo_Wei);
    driver.enable_front_filter(Qian_Chong);
    driver.enable_far_hr_filter(Yuan_Duan);
    driver.enable_near_hr(Jin_Duan);


    std::string sn, pn, sdk_ver, fw_ver;
    if (driver.get_device_sn(sn))
        std::cout << "[信息] 设备SN: " << sn << std::endl;
    if (driver.get_device_pn(pn))
        std::cout << "[信息] 设备PN: " << pn << std::endl;
    if (driver.get_sdk_ver(sdk_ver))
        std::cout << "[信息] SDK版本: " << sdk_ver << std::endl;
    if (driver.get_firmware_ver(fw_ver))
        std::cout << "[信息] 固件版本: " << fw_ver << std::endl;

    if (!driver.enable_data(true))
    {
        std::cerr << "[错误] 开启数据传输失败!" << std::endl;
        return false;
    }

    std::cout << "[信息] 雷达数据传输已开启。" << std::endl;
    return true;
}

// ==================== 功能1：单帧 + 鸟瞰图 ====================
void demo_single_frame(pavo2::pavo2_driver& driver)
{
    std::cout << "\n========== 功能1：获取单帧雷达数据 ==========" << std::endl;

    std::vector<pavo2::pavo_response_pcd_t> points;
    uint32_t timestamp = 0;

    if (!driver.get_scanned_data(points, timestamp, FRAME_TIMEOUT_MS))
    {
        std::cerr << "[错误] 获取单帧数据超时或失败！" << std::endl;
        return;
    }

    std::cout << "[结果] 成功获取一帧数据：" << std::endl;
    std::cout << "  原始点数量: " << points.size() << std::endl;

    if (points.empty()) return;

    // DEBUG: 检查原始坐标范围 + Y轴偏移计算
    double y_offset_single = 0.0;  // mm
    {
        double xmin = 1e9, xmax = -1e9, ymin = 1e9, ymax = -1e9;
        int neg_y = 0, pos_y = 0;
        for (const auto& p : points) {
            double x_mm = p.x * 2.0, y_mm = p.y * 2.0;
            if (x_mm < xmin) xmin = x_mm; if (x_mm > xmax) xmax = x_mm;
            if (y_mm < ymin) ymin = y_mm; if (y_mm > ymax) ymax = y_mm;
            if (y_mm < -0.5) neg_y++; if (y_mm > 0.5) pos_y++;
        }
        std::cout << "[DEBUG单帧] X范围: " << xmin << " ~ " << xmax << " mm" << std::endl;
        std::cout << "[DEBUG单帧] Y范围: " << ymin << " ~ " << ymax << " mm" << std::endl;
        std::cout << "[DEBUG单帧] Y>0点数: " << pos_y << "  Y<0点数: " << neg_y << std::endl;
        if (SHIFT_Y_POSITIVE && ymin < 0) {
            y_offset_single = -ymin;
            std::cout << "[DEBUG单帧] Y轴偏移: +" << y_offset_single << " mm (SHIFT_Y_POSITIVE=true)" << std::endl;
        }
    }

    // --- 鸟瞰图参数 ---
    const int IMG_SIZE = 800;          // 图片像素尺寸
    const double RANGE_MM = 10000.0;   // ±10m 范围
    const double SCALE = IMG_SIZE / (2.0 * RANGE_MM);  // mm转像素

    // 多边形过滤 + 原点过滤
    int filtered_count = 0;
    int origin_skip = 0;
    std::vector<uint8_t> image(IMG_SIZE * IMG_SIZE, 0);

    for (const auto& p : points)
    {
        double x_mm = p.x * 2.0;  // 原始单位0.002m，转mm
        double y_mm = p.y * 2.0 + y_offset_single;  // Y轴上移

        // 跳过原点无效点
        if (is_origin_point(x_mm, y_mm)) {
            origin_skip++;
            continue;
        }

        if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
                    continue;


        filtered_count++;

        // 世界坐标 -> 像素坐标（图片中心为原点，Y轴翻转）
        int px = (int)((x_mm + RANGE_MM) * SCALE);
        int py = (int)((RANGE_MM - y_mm) * SCALE);  // 上方向为Y正

        if (px >= 0 && px < IMG_SIZE && py >= 0 && py < IMG_SIZE)
        {
            // 亮度 = intensity，叠加时取最大值
            int idx = py * IMG_SIZE + px;
            uint8_t val = (uint8_t)(std::min(p.intensity, (uint16_t)255));
            if (val > image[idx]) image[idx] = val;
        }
    }

    // 标记雷达中心点 (小十字)
    int cx = IMG_SIZE / 2, cy = IMG_SIZE / 2;
    for (int dx = -3; dx <= 3; dx++) {
        int idx = cy * IMG_SIZE + (cx + dx);
        if (idx >= 0 && idx < IMG_SIZE * IMG_SIZE) image[idx] = 255;
    }
    for (int dy = -3; dy <= 3; dy++) {
        int idx = (cy + dy) * IMG_SIZE + cx;
        if (idx >= 0 && idx < IMG_SIZE * IMG_SIZE) image[idx] = 255;
    }

    std::string img_file = "single_frame.jpg";
    if (save_jpg_gray(img_file, IMG_SIZE, IMG_SIZE, image))
        std::cout << "[图像] 鸟瞰图已保存: " << img_file
                  << " (" << IMG_SIZE << "x" << IMG_SIZE << ", 范围±" << RANGE_MM/1000 << "m, JPG)" << std::endl;

    // 点云也保存为 txt（仅过滤区域内的点）
    std::ofstream txt("single_frame.txt");
    txt << "# 过滤区域: x∈[-10,10]m, y∈[10,20]m (FILTER_POLY_N=" << Filter_Poly_N << ")\n";
    txt << "# x(mm) y(mm) intensity\n";
    for (const auto& p : points)
    {
        double x_mm = p.x * 2.0;
        double y_mm = p.y * 2.0 + y_offset_single;  // Y轴上移
        if (is_origin_point(x_mm, y_mm)) continue;
        if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
            continue;
        txt << std::fixed << std::setprecision(1)
            << x_mm << " " << y_mm << " " << p.intensity << "\n";
    }
    txt.close();
    std::cout << "[过滤] 区域内点数: " << filtered_count << " / " << points.size() << std::endl;
    if (origin_skip > 0)
        std::cout << "[过滤] 原点无效点跳过: " << origin_skip << " 点" << std::endl;
    std::cout << "[文件] 点云数据已保存: single_frame.txt" << std::endl;
}

// 一次检测的结果（供TCP返回）
struct DetectResult
{
    double start_fill_pct = -1.0;   // 起点截面白色占比(%)
    double end_fill_pct   = -1.0;   // 终点截面白色占比(%)
    double start_y_max    = 0.0;    // 起点截面Y最大值(mm)
    double start_y_med    = 0.0;    // 起点截面Y中位数(mm)
    double end_y_max      = 0.0;    // 终点截面Y最大值(mm)
    double end_y_med      = 0.0;    // 终点截面Y中位数(mm)
    bool   valid          = false;  // 是否有有效结果
};

// ==================== 功能2：多秒采集 + 深度图 ====================
void demo_collect_multi(pavo2::pavo2_driver& driver, DetectResult* result = nullptr)
{
    std::cout << "\n========== 功能2：采集 " << Collect_Duration_S
              << " 秒雷达数据 ==========" << std::endl;

    // --- 创建输出目录(data/当前时间 年月日HH-MM-SS) + 日志文件 ---
    time_t rawtime = std::time(nullptr);
    struct tm* timeinfo = std::localtime(&rawtime);
    char dir_buf[32];
    //std::strftime(dir_buf, sizeof(dir_buf), "%H-%M-%S", timeinfo);
	std::strftime(dir_buf, sizeof(dir_buf), "data/%Y%m%d/%H%M%S", timeinfo); //文件放到data下面好管理
	std::string output_dir(dir_buf);
	mkdir("data");
	mkdir(output_dir.substr(0, output_dir.rfind('/')).c_str());	
    _mkdir(output_dir.c_str());
	//增加日志使能
	std::ofstream log_file;
	if (Log_Flag) {
		log_file.open(output_dir + "/log.txt");
	}
    //std::ofstream log_file(output_dir + "/log.txt");
    auto log_msg = [&log_file](const std::string& s) {
        std::cout << s;
        if (log_file.is_open()) log_file << gbk_to_utf8(s);
    };
    log_msg("[目录] 输出目录: " + output_dir + "/\n");

    struct PointRecord
    {
        double x_mm;
        double y_mm;
        double angle_deg;   // 角度(度)
        uint16_t distance;  // 原始距离(unit=2mm)
        double z_val;       // Z轴值: 距离(mm) 或 时间(ms)，取决于Speed_Z
    };

    // 帧数据（用于深度图）
    struct FrameData
    {
        uint32_t elapsed_ms;
        std::vector<uint16_t> dist_by_angle;  // 每个角度bin的距离
        std::vector<uint8_t>  intensity_by_angle;
    };

    std::vector<PointRecord> all_points;
    std::vector<FrameData> all_frames;
    size_t origin_skip_total = 0;

    // 清空SDK缓冲帧：enable_data后等待稳定期间，网络线程会把完整扫描帧缓存进队列，
    // 若不清空，采集开始时会瞬间连续取出这些旧帧，它们的Z值几乎都为0，全部堆在Z起点。
    int drained = 0;
    {
        pavo2::pavo_packet_info_scan_t tmp_scan;
        while (driver.get_scanned_data(tmp_scan, 5))   // 5ms超时，空队列立即结束
            drained++;
    }
    if (drained > 0)
        std::cout << "[采集] 已清空缓冲旧帧 " << drained << " 帧" << std::endl;

    auto start_time = std::chrono::steady_clock::now();
    int frame_count = 0;
    size_t total_points = 0;

    // 深度图参数
    const int DEPTH_ANGLE_BINS = 720;  // 0.5度一个bin

    std::cout << "[采集] 开始采集，将持续 " << Collect_Duration_S << " 秒..." << std::endl;
    if (Speed_Z > 0)
        std::cout << "[采集] z轴 = 距离(mm), 速度=" << Speed_Z << " cm/s" << std::endl;
    else
        std::cout << "[采集] z轴 = 从采集开始的系统毫秒时间" << std::endl;

    while (true)
    {
        auto now = std::chrono::steady_clock::now();
        auto elapsed_s = std::chrono::duration_cast<std::chrono::seconds>(now - start_time).count();
        auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time).count();

        if (elapsed_s >= Collect_Duration_S)
        {
            std::cout << "[采集] " << Collect_Duration_S << " 秒到，停止采集。" << std::endl;
            break;
        }

        // 获取原始scan数据（带角度），便于做深度图
        pavo2::pavo_packet_info_scan_t scan_info;
        if (driver.get_scanned_data(scan_info, FRAME_TIMEOUT_MS))
        {
            frame_count++;
            // Z轴：距离模式 或 时间模式
            double z_val = (Speed_Z > 0)
                ? (Speed_Z * 10.0 * elapsed_ms / 1000.0)  // cm/s → mm/s, × ms = mm
                : (double)elapsed_ms;

            // 构建本帧的角度-距离直方图
            FrameData frame;
            frame.elapsed_ms = (uint32_t)elapsed_ms;
            frame.dist_by_angle.assign(DEPTH_ANGLE_BINS, 0);
            frame.intensity_by_angle.assign(DEPTH_ANGLE_BINS, 0);

            for (const auto& s : scan_info.vec_buff)
            {
                double angle_deg = s.angle / 100.0;  // SDK角度单位是0.01度
                // 雷达0°指向-Y方向(第3/4象限交界)，旋转-90°映射：
                //   cos(A-90°)=sin(A), sin(A-90°)=-cos(A)
                //   0°→(0,-dist)向下, 90°→(+dist,0)向右, 180°→(0,+dist)向上, 270°→(-dist,0)向左
                double angle_rad = angle_deg * 3.14159265 / 180.0;
                double x_mm =  s.distance * sin(angle_rad) * 2.0;
                double y_mm = -s.distance * cos(angle_rad) * 2.0;

                // 跳过原点无效点
                if (is_origin_point(x_mm, y_mm)) {
                    origin_skip_total++;
                    continue;
                }

                // 多边形过滤：只保留区域内的点（FILTER_POLY_N=0 则不过滤）
                if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N))
                    continue;

                all_points.push_back({x_mm, y_mm, angle_deg, s.distance, z_val});

                // 角度 -> bin
                int bin = (int)(angle_deg / 360.0 * DEPTH_ANGLE_BINS + 0.5) % DEPTH_ANGLE_BINS;
                if (s.distance > frame.dist_by_angle[bin])
                    frame.dist_by_angle[bin] = s.distance;
                if (s.intensity > frame.intensity_by_angle[bin])
                    frame.intensity_by_angle[bin] = (uint8_t)(std::min(s.intensity, (uint16_t)255));
            }

            all_frames.push_back(frame);
            total_points += scan_info.vec_buff.size();

            if (frame_count % 50 == 0)
            {
                std::cout << "  已采集 " << elapsed_s << "s, 帧数=" << frame_count
                          << ", 总点数=" << total_points << std::endl;
            }
        }
    }

    std::cout << "[采集] 采集完成！" << std::endl;
    std::cout << "  总帧数: " << frame_count << std::endl;
    std::cout << "  总点数: " << total_points << std::endl;
    if (origin_skip_total > 0)
        std::cout << "  原点无效点跳过: " << origin_skip_total << " 点" << std::endl;

    // ==========> 保存多边形过滤后的原始PCD（Y偏移之前）<==========
    if (!all_points.empty()) {
        std::vector<double> raw_xs, raw_ys, raw_zs;
        raw_xs.reserve(all_points.size());
        raw_ys.reserve(all_points.size());
        raw_zs.reserve(all_points.size());
        for (const auto& pt : all_points) {
            raw_xs.push_back(pt.x_mm);
            raw_ys.push_back(pt.y_mm);
            raw_zs.push_back(pt.z_val);
        }
        save_pcd_ascii(output_dir + "/lidar_data.pcd", raw_xs, raw_ys, raw_zs);
        std::cout << "[文件] 多边形过滤后原始点云已保存: lidar_data.pcd ("
                  << all_points.size() << " 点, Y未偏移)" << std::endl;
    }

    // Y轴偏移
    if (!all_points.empty()) {
        double ymin = 1e9;
        for (const auto& pt : all_points) {
            if (pt.y_mm < ymin) ymin = pt.y_mm;
        }
        if (SHIFT_Y_POSITIVE && ymin < 0) {
            double y_offset = -ymin;
            for (auto& pt : all_points) {
                pt.y_mm += y_offset;
            }
            std::cout << "[信息] Y轴偏移: +" << y_offset << " mm" << std::endl;
        }
    }

    // ==========> 保存Y偏移后的PCD <==========
    if (!all_points.empty()) {
        std::vector<double> shifted_xs, shifted_ys, shifted_zs;
        shifted_xs.reserve(all_points.size());
        shifted_ys.reserve(all_points.size());
        shifted_zs.reserve(all_points.size());
        for (const auto& pt : all_points) {
            shifted_xs.push_back(pt.x_mm);
            shifted_ys.push_back(pt.y_mm);
            shifted_zs.push_back(pt.z_val);
        }
        save_pcd_ascii(output_dir + "/lidar_data_shifted.pcd", shifted_xs, shifted_ys, shifted_zs);
        std::cout << "[文件] Y偏移后点云已保存: lidar_data_shifted.pcd ("
                  << all_points.size() << " 点, Y已偏移)" << std::endl;
    }

    // ============ 截面切片：Y轴切片 → X-Z平面映射 (起点+终点各一张) ============
    if (all_points.empty()) {
		if (Log_Flag && log_file.is_open())//0713改
        log_file << gbk_to_utf8("无点云数据，跳过截面切片。") << std::endl;
        log_file.close();
        return;
    }

    // Step 1: Y范围（全图）
    double y_min = all_points[0].y_mm;
    double y_max = all_points[0].y_mm;
    for (const auto& pt : all_points) {
        if (pt.y_mm < y_min) y_min = pt.y_mm;
        if (pt.y_mm > y_max) y_max = pt.y_mm;
    }
    std::cout << "\n[截面] Y全图范围: " << y_min << " ~ " << y_max << " mm" << std::endl;

    // Step 2: Y截面区间 = [Y最小, Y最小+Y_Range_MM]，只取该区间内的点参与
    double y_slice_max = y_min + Y_RANGE_MM;
    std::vector<const PointRecord*> y_filtered;
    y_filtered.reserve(all_points.size());
    for (const auto& pt : all_points) {
        if (pt.y_mm >= y_min && pt.y_mm <= y_slice_max)
            y_filtered.push_back(&pt);
    }
    std::cout << "[截面] Y截面区间: " << y_min << " ~ " << y_slice_max
              << " mm (长度=" << Y_RANGE_MM << "mm, 配置Y_Range_MM)" << std::endl;
    std::cout << "[截面] 参与点数: " << y_filtered.size() << " / " << all_points.size() << std::endl;
    if (y_filtered.empty()) {
        std::cout << "[截面] Y截面区间内无点，跳过截面生成。" << std::endl;
        return;
    }

    // Step 3: 计算全局X范围和Z范围
    //   X范围: 用全图所有点的X最大~X最小（图像高度方向，必须全图范围）
    //   Z范围: 用Y过滤后点的Z最大~Z最小（定义起终点截面区间用）
    double gx_min = all_points[0].x_mm, gx_max = all_points[0].x_mm;
    for (const auto& pt : all_points) {
        if (pt.x_mm < gx_min) gx_min = pt.x_mm;
        if (pt.x_mm > gx_max) gx_max = pt.x_mm;
    }
    double gz_min = y_filtered[0]->z_val, gz_max = y_filtered[0]->z_val;
    for (const auto* pt : y_filtered) {
        if (pt->z_val < gz_min) gz_min = pt->z_val;
        if (pt->z_val > gz_max) gz_max = pt->z_val;
    }
    const char* z_unit = (Speed_Z > 0) ? "mm" : "ms";
    std::cout << "[截面] 全局X范围(全图点云): " << gx_min << " ~ " << gx_max << " mm (图像高度方向)" << std::endl;
    std::cout << "[截面] 全局Z范围: " << gz_min << " ~ " << gz_max << " " << z_unit << std::endl;
    std::cout << "[截面] 每张图高度由全图 X最大~X最小 范围决定(不再固定800px)" << std::endl;

    // Step 4: 两个Z区间
    double z_first_start = gz_min;
    double z_first_end   = gz_min + Z_RANGE_MM;
    double z_last_end    = gz_max;
    double z_last_start  = gz_max - Z_RANGE_MM;
    if (z_first_end  > gz_max) z_first_end  = gz_max;
    if (z_last_start < gz_min) z_last_start = gz_min;

    // ====== 通用：Y+Z过滤 → X-Z平面二值映射 ======
    auto make_xz_slice = [&](double z0, double z1, const std::string& label,
                              const std::string& out_pcd, const std::string& out_jpg,
                              double* out_fill_pct = nullptr,
                              int* out_w = nullptr, int* out_h = nullptr,
                              double* out_y_max = nullptr, double* out_y_med = nullptr,
                              std::vector<uint8_t>* out_img = nullptr,
                              bool right_align = false)
    {
        std::ostringstream oss;
        oss << "\n[截面-" << label << "] Z区间: " << z0 << " ~ " << z1 << " " << z_unit << "\n";
        log_msg(oss.str());

        // 收集切片点（Y已过滤 + Z区间过滤）
        std::vector<double> slice_xs, slice_ys, slice_zs;
        for (const auto* pt : y_filtered) {
            if (pt->z_val >= z0 && pt->z_val <= z1) {
                slice_xs.push_back(pt->x_mm);
                slice_ys.push_back(pt->y_mm);
                slice_zs.push_back(pt->z_val);
            }
        }
        oss.str(""); oss.clear();
        oss << "[截面-" << label << "] 切片点数: " << slice_xs.size() << "\n";
        log_msg(oss.str());

        if (slice_xs.empty()) {
            oss.str(""); oss.clear();
            oss << "[截面-" << label << "] 无数据，跳过。\n";
            log_msg(oss.str());
            return;
        }

        // 图像高度方向采用全图 X 范围（gx_min~gx_max），高度=该范围/比例（不再固定800px）
        double sx_min = gx_min, sx_max = gx_max;
        double sx_range = sx_max - sx_min;
        if (sx_range < 1.0) sx_range = 1.0;
        // 图像尺寸：宽=Z_Range_MM(px)，高=全图X范围(px)，1px=1mm，不做比例缩放
        const int img_w = std::max(1, (int)std::ceil(Z_RANGE_MM));   // 宽 = Z_Range_MM
        const double img_scale = 1.0;                                 // 1px = 1mm
        int img_h = std::max(1, (int)std::ceil(sx_range));           // 高 = 全图X范围
        if (out_w) *out_w = img_w;
        if (out_h) *out_h = img_h;

        oss.str(""); oss.clear();
        oss << "[截面-" << label << "] 高度方向X范围(全图): " << gx_min << " ~ " << gx_max
            << " mm\n";
        log_msg(oss.str());
        oss.str(""); oss.clear();
        oss << "[截面-" << label << "] 图像尺寸: " << img_w << "x" << img_h
            << " px (1px=1mm: 宽=Z_Range_MM=" << Z_RANGE_MM
            << ", 高=全图X范围=" << sx_range << ")\n";
        log_msg(oss.str());

        // Y统计: 最大值 + 中位数
        double y_max_slice = *std::max_element(slice_ys.begin(), slice_ys.end());
        std::vector<double> ys_sorted = slice_ys;
        std::sort(ys_sorted.begin(), ys_sorted.end());
        double y_med = (ys_sorted.size() % 2 == 1)
            ? ys_sorted[ys_sorted.size() / 2]
            : (ys_sorted[ys_sorted.size() / 2 - 1] + ys_sorted[ys_sorted.size() / 2]) / 2.0;
        oss.str(""); oss.clear();
        oss << "[截面-" << label << "] Y统计: 最大值=" << std::fixed << std::setprecision(1)
            << y_max_slice << " mm, 中位数=" << y_med << " mm\n";
        log_msg(oss.str());
        if (out_y_max) *out_y_max = y_max_slice;
        if (out_y_med) *out_y_med = y_med;

        // 保存 PCD
        save_pcd_ascii(out_pcd, slice_xs, slice_ys, slice_zs);
        oss.str(""); oss.clear();
        oss << "[文件] " << out_pcd << " (" << slice_xs.size() << " 点)\n";
        log_msg(oss.str());

        // X-Z平面映射：矩形渲染
        // 横轴=Z(运动方向, 左近→右远), 纵轴=X(X越大越靠上)
        // 每个点作为矩形的左边中心点，矩形向右延伸(长=速度/帧率=每帧移动距离)，宽=Rect_Width_PX
        std::vector<uint8_t> slice_img(img_w * img_h, 0);

        // 矩形长(mm) = 运行速度(mm/s) / 雷达帧率(Hz) × 倍率(默认1.5)
        double rect_len_mm = 10.0;   // 时间模式(无速度)默认10mm
        if (Speed_Z > 0)
            rect_len_mm = Speed_Z * 10.0 / Lidar_Hz * Rect_Length_Ratio;
        int len_px = std::max(1, (int)(rect_len_mm / img_scale));
        int half_w = Rect_Width_PX / 2;
        if (half_w < 1) half_w = 1;

        // 水平方向定位：start左对齐(矩形紧贴左边缘)，end右对齐(矩形紧贴右边缘)
        // 切片Z区间长度==图像宽度(1px=1mm)，矩形铺满整图，左右不留黑边
        double z_ref = z0;
        int x_offset = 0;  // start：最左点的矩形左边缘=0
        if (right_align && !slice_zs.empty()) {
            z_ref = *std::max_element(slice_zs.begin(), slice_zs.end());
            x_offset = img_w - len_px;  // end：最右点的矩形右边缘=图宽-1
        }

        for (size_t i = 0; i < slice_xs.size(); i++) {
            int cx = right_align
                ? (int)(x_offset - (z_ref - slice_zs[i]) / img_scale)  // 右对齐：右侧中间
                : (int)((slice_zs[i] - z0) / img_scale + x_offset);    // 左对齐：左侧中间
            int cy = (int)((sx_max - slice_xs[i]) / img_scale);  // 矩形左边中心点y

            // 以 (cx, cy) 为左边中心画矩形: 向右延伸 len_px，上下各 half_w
            for (int dx = 0; dx < len_px; dx++) {
                for (int dy = -half_w; dy <= half_w; dy++) {
                    int px = cx + dx;
                    int py = cy + dy;
                    if (px >= 0 && px < img_w && py >= 0 && py < img_h) {
                        slice_img[py * img_w + px] = 255;
                    }
                }
            }
        }

        // 统计矩形覆盖像素（白色）的占比
        int filled = 0;
        for (int i = 0; i < img_w * img_h; i++)
            if (slice_img[i] > 0) filled++;
        double fill_pct = (img_w * img_h > 0)
            ? filled * 100.0 / (img_w * img_h) : 0.0;
        if (out_fill_pct) *out_fill_pct = fill_pct;

        // 白色(所有矩形)占全图比例日志：无论图片保存成功与否都输出
        oss.str(""); oss.clear();
        oss << "[截面-" << label << "] 白色占比: " << filled << "/"
            << (img_w * img_h) << " 像素 ("
            << std::fixed << std::setprecision(2) << fill_pct << "%)\n";
        log_msg(oss.str());

        if (save_jpg_gray(out_jpg, img_w, img_h, slice_img)) {
            oss.str(""); oss.clear();
            oss << "[图像] " << out_jpg << " (" << img_w << "x" << img_h
                << ", Z=" << z0 << "~" << z1 << " " << z_unit
                << ", X=" << sx_min << "~" << sx_max << "mm"
                << ", 矩形长=" << rect_len_mm << "mm(" << len_px << "px)"
                << ", 矩形宽=" << Rect_Width_PX << "px"
                << ", 对齐=" << (right_align ? "右" : "左") << ")\n";
            log_msg(oss.str());
        }

        if (out_img) *out_img = slice_img;  // 回传渲染图，用于合并对比图
    };

    double start_fill_pct = -1.0, end_fill_pct = -1.0;
    int start_w = 0, start_h = 0, end_w = 0, end_h = 0;
    double start_y_max = 0.0, start_y_med = 0.0, end_y_max = 0.0, end_y_med = 0.0;
    std::vector<uint8_t> start_img, end_img;

    // 起点截面
    make_xz_slice(z_first_start, z_first_end, "起点",
                  output_dir + "/lidar_slice_start.pcd",
                  output_dir + "/lidar_slice_start.jpg",
                  &start_fill_pct, &start_w, &start_h,
                  &start_y_max, &start_y_med, &start_img,
                  false);  // 起点：左对齐，在左侧

    // 终点截面（结束前 Z_RANGE_MM）
    if (z_last_start < z_last_end) {
        make_xz_slice(z_last_start, z_last_end, "终点",
                      output_dir + "/lidar_slice_end.pcd",
                      output_dir + "/lidar_slice_end.jpg",
                      &end_fill_pct, &end_w, &end_h,
                      &end_y_max, &end_y_med, &end_img,
                      true);   // 终点：右对齐，在右侧
    } else {
        std::cout << "\n[截面-终点] Z总长不足" << Z_RANGE_MM << "mm，跳过终点截面。" << std::endl;
    }

    // ====== 合并对比图：start在左半区，end在右半区 ======
    if (!start_img.empty() && !end_img.empty() && start_w == end_w && start_h == end_h) {
        int comb_w = start_w * 2;
        int comb_h = start_h;
        std::vector<uint8_t> combined(comb_w * comb_h, 0);
        for (int y = 0; y < comb_h; y++) {
            std::memcpy(&combined[y * comb_w], &start_img[y * start_w], start_w);
            std::memcpy(&combined[y * comb_w + start_w], &end_img[y * start_w], start_w);
            combined[y * comb_w + start_w] = 128;  // 中间分隔线(灰)
        }
        if (save_jpg_gray(output_dir + "/lidar_slice_combined.jpg", comb_w, comb_h, combined)) {
            std::cout << "[图像] " << output_dir << "/lidar_slice_combined.jpg ("
                      << comb_w << "x" << comb_h << ", 左=start 右=end)" << std::endl;
        }
    }

    // 回传检测结果（供TCP返回）
    if (result) {
        result->valid = true;
        result->start_fill_pct = start_fill_pct;
        result->end_fill_pct   = end_fill_pct;
        result->start_y_max    = start_y_max;
        result->start_y_med    = start_y_med;
        result->end_y_max      = end_y_max;
        result->end_y_med      = end_y_med;
    }

    // --- 写入日志摘要 ---
    {
        std::ostringstream sum;
        sum << "\n========== 采集摘要 ==========" << std::endl;
        sum << "采集时长: " << Collect_Duration_S << " s" << std::endl;
        sum << "总帧数: " << frame_count << std::endl;
        sum << "总点数: " << total_points << std::endl;
        sum << "过滤后点数: " << all_points.size() << std::endl;
        sum << "原点无效点跳过: " << origin_skip_total << std::endl;
        sum << "Y范围(全部点参与): " << y_min << " ~ " << y_max << " mm" << std::endl;
        sum << "参与点数: " << y_filtered.size() << std::endl;
        sum << "Z截面区间长度: " << Z_RANGE_MM << " mm" << std::endl;
        sum << "雷达帧率: " << Lidar_Hz << " Hz" << std::endl;
        sum << "矩形宽度: " << Rect_Width_PX << " px" << std::endl;
        sum << "全局X范围: " << gx_min << " ~ " << gx_max << " mm" << std::endl;
        sum << "全局Z范围: " << gz_min << " ~ " << gz_max << " " << z_unit << std::endl;
        if (start_w > 0)
            sum << "起点截面图像尺寸: " << start_w << "x" << start_h << " px" << std::endl;
        if (end_w > 0)
            sum << "终点截面图像尺寸: " << end_w << "x" << end_h << " px" << std::endl;
        if (start_fill_pct >= 0.0)
            sum << "起点截面白色占比: " << std::fixed << std::setprecision(2)
                << start_fill_pct << "%" << std::endl;
        if (end_fill_pct >= 0.0)
            sum << "终点截面白色占比: " << std::fixed << std::setprecision(2)
                << end_fill_pct << "%" << std::endl;
        sum << "输出文件:" << std::endl;
        sum << "  " << output_dir << "/lidar_data.pcd" << std::endl;
        sum << "  " << output_dir << "/lidar_data_shifted.pcd" << std::endl;
        sum << "  " << output_dir << "/lidar_slice_start.pcd" << std::endl;
        sum << "  " << output_dir << "/lidar_slice_start.jpg" << std::endl;
        sum << "  " << output_dir << "/lidar_slice_end.pcd" << std::endl;
        sum << "  " << output_dir << "/lidar_slice_end.jpg" << std::endl;
        sum << "==============================" << std::endl;
        if (log_file.is_open()) log_file << gbk_to_utf8(sum.str());
    }
    log_file.close();

    std::cout << "\n[截面] 完成。" << std::endl;
}

// ==================== 计数模式（count） ====================
// TCP收到 {"count":"start","box":{"l":行mm,"w":宽mm,"h":层高mm,"maxPerLayer":每层最大个数},"task_id":"xxx"} 开始扫描
// TCP收到 {"count":"stop"} 停止扫描并返回计数结果
// 高度计算: 物品高度 = Floor_Height - Y轴(mm)，参照Java: 地板值减去当前点的y轴值为高度
struct CountPoint
{
    double x_mm;
    double y_mm;
    double z_val;   // Z轴: 距离(mm)或时间(ms)，用于面积计算的第二维度
};

struct CountParams
{
    double l = 0.0;            // 行(长) mm
    double w = 0.0;            // 宽 mm
    double h = 0.0;            // 层高 mm
    int    maxPerLayer = 0;    // 每层最大个数
    std::string task_id;
};

static std::mutex              count_mtx;
static bool                    count_running = false;
static bool                    count_stop    = false;
static std::thread             count_thread;
static std::vector<CountPoint> count_points;
static CountParams             count_params;
static std::chrono::steady_clock::time_point count_start_tp;

// 后台采集线程：count start 后持续扫描，count stop 后退出
// start 后先丢弃SDK队列中积压的旧扫描数据(雷达一直转, start之前已缓存多圈数据)，
// 确保只统计 start 之后新扫描到的数据，否则积压的旧包会在图像开头z方向挤成一段密集点。
// z轴: 用本机时钟 elapsed_ms (相对清空积压后开始计时的时刻)。
static void count_collect_loop(pavo2::pavo2_driver* driver)
{
    // 排空积压：把SDK缓存中 start 之前的数据包全部取出丢弃
    pavo2::pavo_packet_info_scan_t flush_tmp;
    int flushed = 0;
    {
        auto flush_begin = std::chrono::steady_clock::now();
        while (true)
        {
            {
                std::lock_guard<std::mutex> lk(count_mtx);
                if (count_stop) return;   // 排空期间被stop，直接退出
            }
            if (driver->get_scans_available() <= 0) break;
            if (!driver->get_scanned_data(flush_tmp, 0)) break;   // 0ms = 立即返回，不阻塞
            flushed++;
            // 防御：雷达持续产生数据，最多排空500ms，避免卡死
            if (std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::steady_clock::now() - flush_begin).count() > 500)
                break;
        }
    }
    if (flushed > 0)
        std::cout << "[count] 已丢弃积压扫描包 " << flushed << " 个" << std::endl;

    {
        std::lock_guard<std::mutex> lk(count_mtx);
        if (count_stop) return;
        count_start_tp = std::chrono::steady_clock::now();   // 排空完成才开始计时
    }

    while (true)
    {
        pavo2::pavo_packet_info_scan_t scan_info;
        if (driver->get_scanned_data(scan_info, 100))   // 100ms超时轮询，可及时响应stop
        {
            auto now = std::chrono::steady_clock::now();
            auto elapsed_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - count_start_tp).count();
            double z_val = (Speed_Z > 0)
                ? (Speed_Z * 10.0 * elapsed_ms / 1000.0)   // cm/s × 10 = mm/s, × s = mm
                : (double)elapsed_ms;

            std::lock_guard<std::mutex> lk(count_mtx);
            if (count_stop) break;
            for (const auto& s : scan_info.vec_buff)
            {
                double angle_deg = s.angle / 100.0;
                double angle_rad = angle_deg * 3.14159265 / 180.0;
                double x_mm = s.distance * sin(angle_rad) * 2.0;
                double y_mm = -s.distance * cos(angle_rad) * 2.0;
                if (is_origin_point(x_mm, y_mm)) continue;
                if (FILTER_ENABLE && !point_in_polygon(x_mm, y_mm, Filter_Polygon, Filter_Poly_N)) continue;
                count_points.push_back({x_mm, y_mm, z_val});
            }
        }
        else
        {
            std::lock_guard<std::mutex> lk(count_mtx);
            if (count_stop) break;
        }
    }
}

// 简易JSON字段提取（仅用于内部命令解析）
static bool json_get_string(const std::string& json, const std::string& key, std::string& out)
{
    std::string pat = "\"" + key + "\"";
    size_t pos = json.find(pat);
    if (pos == std::string::npos) return false;
    size_t colon = json.find(':', pos + pat.size());
    if (colon == std::string::npos) return false;
    size_t p = colon + 1;
    while (p < json.size() && (json[p] == ' ' || json[p] == '\t')) p++;
    if (p < json.size() && json[p] == '"')
    {
        size_t q = json.find('"', p + 1);
        if (q == std::string::npos) return false;
        out = json.substr(p + 1, q - p - 1);
        return true;
    }
    return false;
}

static bool json_get_number(const std::string& json, const std::string& key, double& out)
{
    std::string pat = "\"" + key + "\"";
    size_t pos = json.find(pat);
    if (pos == std::string::npos) return false;
    size_t colon = json.find(':', pos + pat.size());
    if (colon == std::string::npos) return false;
    size_t p = colon + 1;
    while (p < json.size() && (json[p] == ' ' || json[p] == '\t')) p++;
    size_t end = p;
    while (end < json.size() && (std::isdigit((unsigned char)json[end]) || json[end] == '.' ||
           json[end] == '-' || json[end] == '+' || json[end] == 'e' || json[end] == 'E')) end++;
    if (end == p) return false;
    try { out = std::stod(json.substr(p, end - p)); return true; }
    catch (...) { return false; }
}

// 参照Java drawPointsAndCalculateArea：点集 → 图像(8x8点)
// → 统计非白像素 → 换算实际面积
// 宽: 由 Filter_Polygon 多边形 X 最大最小值构成；高: 由 z 最大值构成(Z为运动方向,从0开始)
// 绘制平面取 (x_mm, z_val)：X为宽度方向，Z为运动方向
// 返回白底图像(0=白,255=黑点)，便于保存为JPG；area_out返回换算面积；out_w/out_h返回实际图像尺寸
static std::vector<uint8_t> points_to_image(const std::vector<CountPoint>& points,
                                            double* area_out = nullptr,
                                            int* out_w = nullptr,
                                            int* out_h = nullptr)
{
    const int MIN_DIM = 8;     // 最小尺寸保护
    const int MAX_DIM = 20000; // 单边最大尺寸保护(防止z过大导致内存爆炸)
    if (area_out) *area_out = 0.0;
    if (out_w) *out_w = 0;
    if (out_h) *out_h = 0;
    if (points.empty()) return std::vector<uint8_t>((size_t)MIN_DIM * MIN_DIM, 0);

    // 宽: Filter_Polygon 多边形 X 最大最小值直接构成 (1mm=1px)
    double polyXMin = 0.0, polyXMax = 0.0;
    if (Filter_Poly_N > 0)
    {
        polyXMin = Filter_Polygon[0].first;
        polyXMax = Filter_Polygon[0].first;
        for (int i = 1; i < Filter_Poly_N; i++)
        {
            polyXMin = std::min(polyXMin, Filter_Polygon[i].first);
            polyXMax = std::max(polyXMax, Filter_Polygon[i].first);
        }
    }
    double spanX = polyXMax - polyXMin;
    if (spanX <= 0.0) spanX = 1.0;

    // 高: z 最大值直接构成 (Z=运动方向, 从0开始递增, 1单位=1px)
    double maxZ = 0.0;
    for (const auto& p : points) maxZ = std::max(maxZ, p.z_val);
    if (maxZ <= 0.0) maxZ = 1.0;

    int imageWidth  = (int)std::lround(spanX);
    int imageHeight = (int)std::lround(maxZ);
    if (imageWidth  < MIN_DIM) imageWidth  = MIN_DIM;
    if (imageHeight < MIN_DIM) imageHeight = MIN_DIM;
    if (imageWidth  > MAX_DIM) imageWidth  = MAX_DIM;
    if (imageHeight > MAX_DIM) imageHeight = MAX_DIM;
    if (out_w) *out_w = imageWidth;
    if (out_h) *out_h = imageHeight;
    std::vector<uint8_t> image((size_t)imageWidth * imageHeight, 0);   // 0=白底

    // X坐标相对多边形左边界偏移到0；Z从0开始
    double scaleX = (double)imageWidth / spanX;   // ≈1: 宽直接由多边形X跨度构成
    double scaleY = (double)imageHeight / maxZ;   // ≈1: 高直接由z最大值构成
    double xOffset = -polyXMin;

    // 优化：Step1 先按1x1网格标记点位置（重复点只写一次），
    //      Step2 再对标记位置膨胀 Point_Dilate×Point_Dilate（与逐点画 Point_Dilate×Point_Dilate 数学等价，写次数大幅减少）
    std::vector<uint32_t> marks;
    marks.reserve(points.size() / 2);
    for (const auto& p : points)
    {
        int px = (int)((p.x_mm + xOffset) * scaleX);
        int py = (int)(p.z_val * scaleY);
        if (px < 0 || px >= imageWidth || py < 0 || py >= imageHeight) continue;
        size_t idx = (size_t)py * imageWidth + px;
        if (image[idx] == 0)          // 未标记过才记录
        {
            image[idx] = 1;
            marks.push_back((uint32_t)idx);
        }
    }
    for (uint32_t idx : marks)        // 膨胀 Point_Dilate×Point_Dilate
    {
        int px = (int)(idx % imageWidth);
        int py = (int)(idx / imageWidth);
        for (int dy = 0; dy < Point_Dilate; dy++)
            for (int dx = 0; dx < Point_Dilate; dx++)
            {
                int ix = px + dx, iy = py + dy;
                if (ix < imageWidth && iy < imageHeight)
                    image[(size_t)iy * imageWidth + ix] = 255;   // 255=黑点
            }
    }
    int totalPixels = 0;
    for (size_t i = 0; i < image.size(); i++)
        if (image[i] != 0) totalPixels++;

    double pixelToAreaRatio = spanX * maxZ / (double)(imageWidth * imageHeight);
    if (area_out) *area_out = totalPixels * pixelToAreaRatio;
    return image;
}

static double draw_points_calc_area(const std::vector<CountPoint>& points)
{
    double area = 0.0;
    points_to_image(points, &area);
    return area;
}

// ==================== z轴空隙插值 ====================
// 雷达某段时间无数据(断流/瞬间丢包)时, z方向会出现空白带。
// 原理: 同一scan包(一圈)的所有点 z 相同, 正常圈间隔近似恒定(取相邻圈z差的中位数);
//       若某两个相邻圈的 z 差远大于正常间隔 → 判定为空隙, 用前/后圈数据补出虚拟圈。
// Z_Interpolate_Mode:
//   none                      = 不插值
//   CopyTranslation           = 空隙内按正常间隔分层, 复制"前一圈"的点平移填充
//   AngleBucketInterpolation  = 空隙内以前/后两圈做"角度桶"插值生成虚拟圈(平滑过渡)
// 返回新增点数
static int z_gap_interpolate(std::vector<CountPoint>& points)
{
    if (points.empty() || Z_Interpolate_Mode == "none")
        return 0;

    // 正常相邻两条(圈)的 z 间隔: 速度模式 = 每圈行进距离(mm); 时间模式 = 每圈耗时(ms)
    double normal_gap = (Speed_Z > 0)
        ? (Speed_Z * 10.0 / Lidar_Hz)   // cm/s ×10 = mm/s, ÷Hz = mm/圈
        : (1000.0 / Lidar_Hz);          // ms/圈
    if (normal_gap <= 0) return 0;

    // 空隙判定: 本条与上一条 z 差 > 2×正常间隔 视为断流空隙
    const double gap_threshold = 2.0 * normal_gap;
    // 条件1: 前后两条 Y 平均值差超过30mm 不插(可能跨层/换物体)
    const double max_y_mean_diff = 30.0;
    // 一个空隙最多补的行数
    const int MAX_FILL_LEVELS = 10;

    // 1. 按 z 升序排序
    std::sort(points.begin(), points.end(),
              [](const CountPoint& a, const CountPoint& b) { return a.z_val < b.z_val; });

    // 2. 聚类成"圈"(同一scan包的点 z 完全相同, 用区间 [begin,end) 索引 points)
    struct Circle { double z; size_t begin; size_t end; };
    std::vector<Circle> circles;
    {
        size_t i = 0;
        while (i < points.size())
        {
            size_t j = i + 1;
            while (j < points.size() && points[j].z_val - points[i].z_val < 1e-6) ++j;
            circles.push_back(Circle());
            circles.back().z = points[i].z_val;
            circles.back().begin = i;
            circles.back().end = j;
            i = j;
        }
    }
    if (circles.size() < 2)
        return 0;

    auto circle_mean_y = [&](const Circle& c) -> double
    {
        double s = 0;
        for (size_t p = c.begin; p < c.end; ++p) s += points[p].y_mm;
        return s / (double)(c.end - c.begin);
    };

    int added = 0;
    // 3. 逐空隙补行
    for (size_t i = 1; i < circles.size(); ++i)
    {
        double gap = circles[i].z - circles[i - 1].z;
        if (gap <= gap_threshold)      // 未超过2×正常间隔, 不算空隙
            continue;

        const Circle& cA = circles[i - 1];   // 上一条
        const Circle& cB = circles[i];       // 本条

        // 条件1: 前后两条的Y平均值几乎相当(差≤30mm)才插值
        if (std::fabs(circle_mean_y(cA) - circle_mean_y(cB)) > max_y_mean_diff)
            continue;

        // 条件2: 生成行的 X 范围 = 这两条的 X 最大和最小
        double x_lo = points[cA.begin].x_mm, x_hi = points[cA.begin].x_mm;
        for (size_t p = cA.begin; p < cA.end; ++p)
        {
            if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
            if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
        }
        for (size_t p = cB.begin; p < cB.end; ++p)
        {
            if (points[p].x_mm < x_lo) x_lo = points[p].x_mm;
            if (points[p].x_mm > x_hi) x_hi = points[p].x_mm;
        }

        // 空隙内补行数: 按正常间隔容纳行数-1(空隙两端已有), 最多10行
        int n = (int)(gap / normal_gap + 0.5) - 1;
        if (n < 1) continue;
        if (n > MAX_FILL_LEVELS) n = MAX_FILL_LEVELS;

        if (Z_Interpolate_Mode == "CopyTranslation")
        {
            // 复制"上一条"的点, z 均匀平移到空隙内的各补行
            for (int k = 1; k <= n; ++k)
            {
                double zk = cA.z + gap * (double)k / (n + 1);   // 均匀分布
                for (size_t p = cA.begin; p < cA.end; ++p)
                {
                    const CountPoint& src = points[p];
                    if (src.x_mm < x_lo || src.x_mm > x_hi) continue;   // X范围约束
                    CountPoint np = src;
                    np.z_val = zk;
                    points.push_back(np);
                    ++added;
                }
            }
        }
        else if (Z_Interpolate_Mode == "AngleBucketInterpolation")
        {
            // 角度桶插值: 每桶用前A/后B平均点线性插值(平滑过渡), 同样受Y平均/X范围条件约束
            struct Buck { double sx, sy; int cnt; Buck() : sx(0), sy(0), cnt(0) {} };
            std::vector<Buck> bA(360), bB(360);
            auto fill = [&](const Circle& c, std::vector<Buck>& out)
            {
                for (size_t p = c.begin; p < c.end; ++p)
                {
                    double deg = std::atan2(points[p].x_mm, -points[p].y_mm) * 57.29577951308232;
                    if (deg < 0) deg += 360.0;
                    int b = (int)(deg + 0.5);
                    if (b >= 360) b = 0;
                    out[b].sx += points[p].x_mm;
                    out[b].sy += points[p].y_mm;
                    out[b].cnt++;
                }
            };
            fill(cA, bA);
            fill(cB, bB);
            for (int k = 1; k <= n; ++k)
            {
                double zk = cA.z + gap * (double)k / (n + 1);   // 均匀分布
                double t = (zk - cA.z) / gap;                    // 0~1
                for (int b = 0; b < 360; ++b)
                {
                    if (bA[b].cnt <= 0 || bB[b].cnt <= 0) continue;
                    double ax = bA[b].sx / bA[b].cnt, ay = bA[b].sy / bA[b].cnt;
                    double bx = bB[b].sx / bB[b].cnt, by = bB[b].sy / bB[b].cnt;
                    double nx = ax + (bx - ax) * t;
                    double ny = ay + (by - ay) * t;
                    if (nx < x_lo || nx > x_hi) continue;        // X范围约束
                    CountPoint np;
                    np.x_mm = nx;
                    np.y_mm = ny;
                    np.z_val = zk;
                    points.push_back(np);
                    ++added;
                }
            }
        }
    }

    if (added > 0)
        std::cout << "[count] z空隙插值: 模式=" << Z_Interpolate_Mode
                  << ", 正常间隔=" << normal_gap
                  << ", 判定阈值=2×间隔=" << gap_threshold
                  << ", 新增" << added << "点" << std::endl;
    return added;
}

// 参照Java getLongitudinalType：按层统计总数
// 高度 = Floor_Height - Y轴; level = round(高度/层高);
// 冗余: 第1层50mm, 每高一层+20mm; 从最高层(5)往下找有效层, 该层面积/(行×宽) 得该层个数
// detail: 可选输出每层点数/面积详情（供日志使用）
// top_layer_img/level/area: 可选输出被计数层(最高有效层)的图像/层号/面积
static int calc_item_count(const std::vector<CountPoint>& points, const CountParams& box,
                           std::string* detail = nullptr,
                           std::vector<uint8_t>* top_layer_img = nullptr,
                           int* top_layer_img_w = nullptr,
                           int* top_layer_img_h = nullptr,
                           int* top_layer_level = nullptr,
                           double* top_layer_area = nullptr)
{
    if (points.empty() || box.l <= 0 || box.w <= 0 || box.h <= 0 || box.maxPerLayer <= 0)
        return 0;

    std::unordered_map<int, std::vector<CountPoint>> layer_map;
    for (const auto& p : points)
    {
        if (FILTER_ENABLE && !point_in_polygon(p.x_mm, p.y_mm, Filter_Polygon, Filter_Poly_N))
            continue;
        double currentHeight = Floor_Height - p.y_mm;  // 高度 = 地板值 - Y轴
        if (currentHeight < 0) continue;
        int level = (int)std::round(currentHeight / box.h);
        double tolerance = Count_Base_Tolerance + level * Count_Additional_Tolerance_Per_Level;
        if (std::abs(currentHeight - level * box.h) <= tolerance)
            layer_map[level].push_back(p);
    }

    // 每层面积/图像只算一次，详情与结果共用，避免重复构建图像
    std::ostringstream oss;
    std::unordered_map<int, double> layer_area;
    std::unordered_map<int, std::vector<uint8_t>> layer_img;
    std::unordered_map<int, std::pair<int, int>> layer_img_size;
    for (int h = 5; h > 0; h--)
    {
        auto it = layer_map.find(h);
        if (it == layer_map.end()) continue;
        oss << "  层" << h << ": 点数=" << it->second.size();
        // 高度范围: 该层算法对应的理论高度区间(即生成该层count_layer截取的高度范围) - 每层都显示
        double tol = Count_Base_Tolerance + h * Count_Additional_Tolerance_Per_Level;
        double h_lo = h * box.h - tol;
        double h_hi = h * box.h + tol;
        oss << ", 高度范围（" << std::setprecision(0) << h_lo << "," << h_hi << "）";
        if (it->second.size() > 1000)
        {
            double area = 0.0;
            int img_w = 0, img_h = 0;
            std::vector<uint8_t> img = points_to_image(it->second, &area, &img_w, &img_h);
            layer_area[h] = area;
            layer_img[h] = std::move(img);
            layer_img_size[h] = std::make_pair(img_w, img_h);
            double item_cnt = area / (box.l * box.w);   // 未取整的个数(面积/单件面积)
            oss << ", 面积=" << std::fixed << std::setprecision(1) << area << "mm^2"
                << ", 个数" << std::setprecision(2) << item_cnt << "个";
        }
        oss << "\n";
    }

    int layersCount = box.maxPerLayer;   // 每层最大个数
    int result = 0;
    int top_lv = 0;
    double top_area = 0.0;
    for (int h = 5; h > 0; h--)          // 最多5层，从顶层往下找
    {
        auto it = layer_map.find(h);
        if (it != layer_map.end() && it->second.size() > 1000)
        {
            double area = layer_area[h];
            if (area > 0)
            {
                int i = (int)std::min(std::round(area / (box.l * box.w)), (double)layersCount);
                if (i > 0)
                {
                    result = layersCount * (h - 1) + i;
                    top_lv = h;
                    top_area = area;
                    break;
                }
            }
        }
    }
    if (detail) *detail = oss.str();
    // 输出被计数层(最高有效层)的图像/尺寸/层号/面积
    if (top_layer_img && top_lv > 0)
    {
        auto it = layer_img.find(top_lv);
        if (it != layer_img.end()) *top_layer_img = it->second;
        auto sit = layer_img_size.find(top_lv);
        if (sit != layer_img_size.end())
        {
            if (top_layer_img_w) *top_layer_img_w = sit->second.first;
            if (top_layer_img_h) *top_layer_img_h = sit->second.second;
        }
    }
    if (top_layer_level) *top_layer_level = top_lv;
    if (top_layer_area) *top_layer_area = top_area;
    return result;
}

// ==================== TCP服务端模式 ====================
// 收到 {"check":1} 触发一次检测（demo_collect_multi）
void handle_tcp_client(SOCKET client, pavo2::pavo2_driver* driver)
{
    static std::mutex check_mutex;   // 串行化检测，避免并发

    char buf[1024];
    std::string recv_buf;
    while (true)
    {
        int n = recv(client, buf, sizeof(buf) - 1, 0);
        if (n <= 0) break;   // 连接关闭或出错
        buf[n] = '\0';
        recv_buf.append(buf, n);

        // 按行处理消息
        size_t pos;
        while ((pos = recv_buf.find('\n')) != std::string::npos)
        {
            std::string line = recv_buf.substr(0, pos);
            recv_buf.erase(0, pos + 1);

            // 去空白后匹配命令: {"check":1}=触发检测, {"config":1}=重读配置
            std::string cleaned;
            for (char c : line)
                if (c != ' ' && c != '\t' && c != '\r' && c != '\n') cleaned += c;

            if (cleaned == "{\"config\":1}")
            {
                std::cout << "[TCP] 收到配置重载命令，重新读取 config/config ..." << std::endl;
                load_config();
                std::cout << "[TCP] 配置已重载: 采集=" << Collect_Duration_S << "s, 速度=" << Speed_Z
                          << "cm/s, Z区间=" << Z_RANGE_MM << "mm, 帧率=" << Lidar_Hz
                          << "Hz, 矩形宽=" << Rect_Width_PX << "px, 模式=" << Run_Mode
                          << ", 端口=" << Tcp_Port << std::endl;
                const char* ack = "{\"code\":200,\"message\":\"config reloaded\"}\n";
                send(client, ack, (int)std::strlen(ack), 0);
            }
            else if (cleaned == "{\"check\":1}")
            {
                std::lock_guard<std::mutex> lk(check_mutex);
                std::cout << "[TCP] 收到检测命令: " << line << "，开始检测..." << std::endl;
                const char* ack = "{\"code\":200,\"message\":\"check received, detecting...\"}\n";
                send(client, ack, (int)std::strlen(ack), 0);

                DetectResult res;
                demo_collect_multi(*driver, &res);

                // 返回检测完成 + start/end 白色占比 + Y最大/中位数
                std::ostringstream resp;
                resp << "{\"code\":200,\"message\":\"detect done\"";
                if (res.valid) {
                    resp << ",\"start\":{\"fill_pct\":"
                         << std::fixed << std::setprecision(2) << res.start_fill_pct
                         << ",\"y_max\":" << std::setprecision(1) << res.start_y_max
                         << ",\"y_median\":" << res.start_y_med << "}";
                    resp << ",\"end\":{\"fill_pct\":"
                         << std::fixed << std::setprecision(2) << res.end_fill_pct
                         << ",\"y_max\":" << std::setprecision(1) << res.end_y_max
                         << ",\"y_median\":" << res.end_y_med << "}";
                }
                resp << "}\n";
                std::string resp_str = resp.str();
                send(client, resp_str.c_str(), (int)resp_str.size(), 0);
                std::cout << "[TCP] 检测完成，已返回结果。" << std::endl;
            }
            else if (cleaned.find("\"count\"") != std::string::npos)
            {
                // count 计数命令: {"count":"start","box":{"l":行mm,"w":宽mm,"h":层高mm,"maxPerLayer":每层最大个数},"task_id":"xxx"}
                //               {"count":"stop"}
                std::string count_cmd;
                if (!json_get_string(cleaned, "count", count_cmd))
                {
                    const char* ack = "{\"code\":400,\"message\":\"count parse error\"}\n";
                    send(client, ack, (int)std::strlen(ack), 0);
                    continue;
                }

                if (count_cmd == "start")
                {
                    CountParams params;
                    json_get_string(cleaned, "task_id", params.task_id);
                    json_get_number(cleaned, "l", params.l);
                    json_get_number(cleaned, "w", params.w);
                    json_get_number(cleaned, "h", params.h);
                    double d = 0.0;
                    if (json_get_number(cleaned, "maxPerLayer", d) || json_get_number(cleaned, "max", d))
                        params.maxPerLayer = (int)d;

                    {
                        std::lock_guard<std::mutex> lk(count_mtx);
                        if (count_running)
                        {
                            const char* ack = "{\"code\":400,\"message\":\"count already running\"}\n";
                            send(client, ack, (int)std::strlen(ack), 0);
                            continue;
                        }
                        count_params = params;
                        count_points.clear();
                        count_stop = false;
                        count_running = true;
                        count_thread = std::thread(count_collect_loop, driver);
                    }

                    std::cout << "[TCP] 收到count:start, task_id=" << params.task_id
                              << ", box(l=" << params.l << ",w=" << params.w
                              << ",h=" << params.h << ",maxPerLayer=" << params.maxPerLayer
                              << ")，开始扫描..." << std::endl;
                    std::ostringstream resp;
                    resp << "{\"code\":200,\"message\":\"count start\"";
                    if (!params.task_id.empty())
                        resp << ",\"task_id\":\"" << params.task_id << "\"";
                    resp << "}\n";
                    std::string resp_str = resp.str();
                    send(client, resp_str.c_str(), (int)resp_str.size(), 0);
                }
                else if (count_cmd == "stop")
                {
                    std::string task_id;
                    json_get_string(cleaned, "task_id", task_id);

                    // 只有真正 start 过的 stop 才有效，否则返回 0
                    bool was_running = false;
                    {
                        std::lock_guard<std::mutex> lk(count_mtx);
                        if (count_running)
                        {
                            count_stop = true;
                            count_running = false;
                            was_running = true;
                        }
                    }
                    if (count_thread.joinable()) count_thread.join();

                    int total = 0;
                    size_t pt_cnt = 0;
                    std::string layer_detail;
                    CountParams params_for_log;   // 本次任务的box参数（日志用）
                    std::vector<CountPoint> saved_points;
                    std::vector<uint8_t> top_img;      // 被计数层的图像
                    int    top_lv = 0;
                    int    top_img_w = 0, top_img_h = 0;
                    double top_area = 0.0;
                    {
                        std::lock_guard<std::mutex> lk(count_mtx);
                        saved_points = count_points;
                        z_gap_interpolate(saved_points);   // z轴空隙插值(none时不处理)
                        pt_cnt = saved_points.size();
                        params_for_log = count_params;
                        if (was_running && pt_cnt > 0 && count_params.l > 0 &&
                            count_params.w > 0 && count_params.h > 0 && count_params.maxPerLayer > 0)
                            total = calc_item_count(saved_points, count_params, &layer_detail,
                                                    &top_img, &top_img_w, &top_img_h,
                                                    &top_lv, &top_area);
                        // 清空遗留数据/参数，避免未start的stop复用旧数据重复计数
                        count_points.clear();
                        count_params = CountParams();
                    }

                    std::cout << "[TCP] 收到count:stop, task_id=" << task_id
                              << ", 采集点数=" << pt_cnt << ", 计数结果=" << total << std::endl;

                    // 先返回结果，PCD/图片/日志放到后台线程保存，不阻塞响应
                    std::ostringstream resp;
                    resp << "{\"count\":" << total;
                    if (!task_id.empty())
                        resp << ",\"task_id\":\"" << task_id << "\"";
                    resp << "}\n";
                    std::string resp_str = resp.str();
                    send(client, resp_str.c_str(), (int)resp_str.size(), 0);

                    if (was_running && pt_cnt > 0)
                    {
                        // C++11 兼容：先 move 到局部变量，再按值捕获
                        std::vector<CountPoint> bg_points = std::move(saved_points);
                        std::vector<uint8_t> bg_img = std::move(top_img);
                        std::thread([bg_points, bg_img, params_for_log, layer_detail, task_id,
                                     total, pt_cnt, top_lv, top_area,
                                     top_img_w, top_img_h]() {
                            // 目录: data/日期/时间
                            time_t rawtime = std::time(nullptr);
                            struct tm* timeinfo = std::localtime(&rawtime);
                            char dir_buf[32];
                            std::strftime(dir_buf, sizeof(dir_buf), "data/%Y%m%d/%H%M%S", timeinfo);
                            std::string out_dir(dir_buf);
                            mkdir("data");
                            mkdir(out_dir.substr(0, out_dir.rfind('/')).c_str());
                            _mkdir(out_dir.c_str());

                            // PCD：y 换成高度(Floor_Height - y_mm)
                            std::vector<double> xs, ys, zs;
                            xs.reserve(pt_cnt); ys.reserve(pt_cnt); zs.reserve(pt_cnt);
                            for (const auto& p : bg_points)
                            {
                                xs.push_back(p.x_mm);
                                ys.push_back(Floor_Height - p.y_mm);
                                zs.push_back(p.z_val);
                            }
                            save_pcd_ascii(out_dir + "/count_data.pcd", xs, ys, zs);
                            std::cout << "[count] 点云已保存: " << out_dir << "/count_data.pcd ("
                                      << pt_cnt << " 点, Y=高度)" << std::endl;

                            // 图片1：全点X-Z平面图(白底黑点, 尺寸按实际X/Z跨度比例)
                            double all_area = 0.0;
                            int all_w = 0, all_h = 0;
                            std::vector<uint8_t> all_img = points_to_image(bg_points, &all_area, &all_w, &all_h);
                            if (save_jpg_gray(out_dir + "/count_points.jpg", all_w, all_h, all_img))
                                std::cout << "[count] 点云图已保存: " << out_dir << "/count_points.jpg ("
                                          << all_w << "x" << all_h << ", X-Z平面, 面积≈"
                                          << std::fixed << std::setprecision(1)
                                          << all_area << "mm^2)" << std::endl;

                            // 图片2：被计数层(最高有效层)图像（Java drawPointsAndCalculateArea语义）
                            if (top_lv > 0 && !bg_img.empty() && top_img_w > 0 && top_img_h > 0)
                            {
                                if (save_jpg_gray(out_dir + "/count_layer.jpg", top_img_w, top_img_h, bg_img))
                                    std::cout << "[count] 有效层图已保存: " << out_dir << "/count_layer.jpg ("
                                              << top_img_w << "x" << top_img_h << ", 层" << top_lv
                                              << ", 面积≈" << std::fixed
                                              << std::setprecision(1) << top_area << "mm^2)" << std::endl;
                            }

                            // 日志
                            if (Log_Flag)
                            {
                                std::ofstream lf(out_dir + "/log.txt");
                                if (lf.is_open())
                                {
                                    std::ostringstream sum;
                                    sum << "==============================\n";
                                    sum << "[count] task_id: " << task_id << "\n";
                                    sum << "[count] box: l=" << params_for_log.l
                                        << ", w=" << params_for_log.w
                                        << ", h=" << params_for_log.h
                                        << ", maxPerLayer=" << params_for_log.maxPerLayer << "\n";
                                    sum << "[count] Floor_Height: " << Floor_Height << " mm\n";
                                    sum << "[count] 采集点数: " << pt_cnt << "\n";
                                    sum << "[count] 计数层: 层" << top_lv
                                        << " (面积=" << std::fixed << std::setprecision(1)
                                        << top_area << "mm^2)\n";
                                    sum << "[count] 分层情况:\n" << layer_detail;
                                    sum << "[count] 计数结果: " << total << "\n";
                                    sum << "[count] 输出文件:\n";
                                    sum << "  " << out_dir << "/count_data.pcd\n";
                                    sum << "  " << out_dir << "/count_points.jpg (" << all_w << "x" << all_h << ")\n";
                                    if (top_lv > 0 && !bg_img.empty() && top_img_w > 0 && top_img_h > 0)
                                        sum << "  " << out_dir << "/count_layer.jpg ("
                                            << top_img_w << "x" << top_img_h << ")\n";
                                    sum << "==============================\n";
                                    lf << gbk_to_utf8(sum.str());
                                    lf.close();
                                }
                            }
                            std::cout << "[count] 输出目录: " << out_dir << std::endl;
                        }).detach();
                    }
                }
                else
                {
                    const char* ack = "{\"code\":400,\"message\":\"unknown count command\"}\n";
                    send(client, ack, (int)std::strlen(ack), 0);
                }
            }
        }
    }
    closesocket(client);
}

void run_tcp_server(pavo2::pavo2_driver& driver)
{
    std::cout << "\n========== TCP服务端模式 ==========" << std::endl;
    std::cout << "[TCP] 监听端口: " << Tcp_Port << std::endl;

    SOCKET listen_sock = socket(AF_INET, SOCK_STREAM, 0);
    if (listen_sock == INVALID_SOCKET)
    {
        std::cerr << "[错误] 创建监听socket失败!" << std::endl;
        return;
    }

    sockaddr_in addr;
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = INADDR_ANY;
    addr.sin_port = htons(Tcp_Port);
    if (bind(listen_sock, (sockaddr*)&addr, sizeof(addr)) == SOCKET_ERROR)
    {
        std::cerr << "[错误] 绑定端口 " << Tcp_Port << " 失败!" << std::endl;
        closesocket(listen_sock);
        return;
    }
    if (listen(listen_sock, 5) == SOCKET_ERROR)
    {
        std::cerr << "[错误] 监听失败!" << std::endl;
        closesocket(listen_sock);
        return;
    }

    std::cout << "[TCP] 初始化完成，等待检测命令 {\"check\":1} ..." << std::endl;
    while (true)
    {
        SOCKET client = accept(listen_sock, nullptr, nullptr);
        if (client == INVALID_SOCKET)
        {
            std::cout << "[TCP] accept失败，重试..." << std::endl;
            continue;
        }
        std::thread(handle_tcp_client, client, &driver).detach();
    }
}

// ==================== 主函数 ====================
int main()
{
#ifdef _WIN32
    //SetConsoleOutputCP(CP_UTF8);  20260714临时去掉，应对乱码

    WSADATA wsa_data;
    if (WSAStartup(MAKEWORD(2, 2), &wsa_data) != 0)
    {
        std::cerr << "[错误] WSAStartup 初始化失败！" << std::endl;
        return -1;
    }
#endif
    load_config(); //加载配置文件0713增加
	
    std::cout << "===============config配置内容===============" << std::endl;
    std::cout << "   雷达IP: " << Leida_IP << std::endl;
    std::cout << "   端口: " << Leida_PORT << std::endl;
    std::cout << "   采集时间: " << Collect_Duration_S << " s" << std::endl;
    std::cout << "   运行速度: " << Speed_Z << " cm/s" << std::endl;
    std::cout << "   Z截面区间: " << Z_RANGE_MM << " mm" << std::endl;
    std::cout << "   Y截面区间: " << Y_RANGE_MM << " mm (从Y最小开始)" << std::endl;
    std::cout << "   雷达帧率: " << Lidar_Hz << " Hz" << std::endl;
    std::cout << "   矩形宽度: " << Rect_Width_PX << " px" << std::endl;
    std::cout << "   矩形长倍率: " << Rect_Length_Ratio << std::endl;
    std::cout << "   运行模式: " << Run_Mode << " (tcp端口: " << Tcp_Port << ")" << std::endl;
    std::cout << "   日志使能: " << (Log_Flag ? "YES" : "NO") << std::endl;
    std::cout << "   地板高度(Floor_Height): " << Floor_Height << " mm (物品高度=Floor_Height-Y轴)" << std::endl;
    std::cout << "========================================" << std::endl;
	
    std::cout << "========================================" << std::endl;
    std::cout << "   Pavo2 Lidar 雷达数据采集程序" << std::endl;
    std::cout << "   SDK版本: " << SDK_VER << std::endl;
    std::cout << "========================================" << std::endl;

    pavo2::pavo2_driver driver;

    if (!connect_lidar(driver))
    {
        std::cerr << "\n[退出] 连接雷达失败，程序退出。" << std::endl;
        std::cout << "请检查：" << std::endl;
        std::cout << "  1. 雷达IP地址是否正确（当前: " << Leida_IP << "）" << std::endl;
        std::cout << "  2. 本机与雷达的网络是否连通" << std::endl;
        std::cout << "  3. 防火墙是否放行端口 " << Leida_PORT << std::endl;
#ifdef _WIN32
        WSACleanup();
#endif
        return -1;
    }

    std::cout << "\n[等待] 等待雷达数据稳定（2秒）..." << std::endl;
    std::this_thread::sleep_for(std::chrono::seconds(2));

    // --- 功能1：单帧 + 鸟瞰图 (已关闭) ---
    // demo_single_frame(driver);

    // --- 运行模式: exe=直接检测一次; tcp=服务端模式，收到 {"check":1} 触发检测 ---
    if (Run_Mode == "tcp")
    {
        run_tcp_server(driver);
    }
    else
    {
        // --- 功能2：多秒采集 + 深度图 ---
        demo_collect_multi(driver);
    }

    std::cout << "\n[关闭] 正在断开雷达连接..." << std::endl;
    driver.pavo2_close();

#ifdef _WIN32
    WSACleanup();
#endif

    std::cout << "[完成] 程序运行结束。" << std::endl;
    return 0;
}