leida_simi/lidar_utils.h

// lidar_utils.h - 雷达连接与扫描采集工具
#ifndef LIDAR_UTILS_H
#define LIDAR_UTILS_H

#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <chrono>
#include "PAVO2_SDK/include/pavo2_driver.h"
#include "common.h"

// ==================== 雷达连接 ====================
// 连接雷达(TCP)，配置滤波并开启数据传输；失败返回 false。
// 注意：Windows 下需在 WSAStartup 之后调用。
bool connect_lidar(pavo2::pavo2_driver& driver);

// ==================== 扫描辅助 ====================
// 排空 SDK 队列中积压的旧扫描包（采集 start 之前雷达已缓存的数据）。
// 防御性：最多排空 max_ms 毫秒（默认500），避免雷达持续产数导致卡死。
// 返回丢弃包数。
int drain_stale_scans(pavo2::pavo2_driver& driver, int max_ms = 500);

// z轴值换算（时间→z）：
//  - cfg.scan.z_speed_type == "configuration" 且 speed.txt 有效：按速度剖面(时间→距离)变速插值，
//    超过表末尾时间按最后两条的平均速度外推延伸；
//  - 否则按原逻辑：cfg.scan.z_speed>0 时为移动距离(mm)=cfg.scan.z_speed(cm/s)*10*秒；
//    cfg.scan.z_speed=0 时 z 直接取 elapsed_ms（时间模式）。
double z_for_elapsed(double elapsed_ms);

// ==================== 变速速度剖面 (speed.txt) ====================
// speed.txt 每行两个数字: "时间(ms) 距离(mm)"（# 开头为注释，空行忽略），时间须递增。
// 当 cfg.scan.z_speed_type == "configuration" 时程序使用该表把采集时间换算为 z(行进距离, mm)，
// 从而实现变速运动识别；未找到有效表时自动回退使用 cfg.scan.z_speed 匀速。
// 默认路径 ./config/speed.txt（与 config/config 同目录）。
// 加载剖面（成功后内部替换当前剖面）。返回是否有效(≥2行)。
bool load_speed_profile(const std::string& path = "./config/speed.txt");

// 按当前 cfg.scan.z_speed_type 重载：configuration → 重新读 speed.txt；
// uniformSpeed → 清空剖面（不使用表）。配置重载(load_config)后应调用一次。
void reload_speed_profile();

// 当前剖面是否有效(≥2行，配置为 configuration 时 z 按表换算)。
bool speed_profile_loaded();

// 剖面平均速度(mm/s)；无效或非 distance 数据时返回 0。
double speed_profile_avg_velocity_mm_s();

// 当前 z 轴是否为距离(mm)：configuration 且剖面有效=true；
// uniformSpeed 时为 cfg.scan.z_speed>0。
bool z_is_distance_mode();

// ConfigureSpeed 标定采集：采集 seconds 秒，逐帧记录(时间, 过滤后Y最大值)，
// 平滑(抑制与总体趋势反向>5mm的抖动)后写入 ./config/speed.txt。
// 返回写入行数；失败返回 <0 并在 err 中给出原因。
// 注意：调用前应确保没有其他线程同时 get_scanned_data（与 ScanSession 互斥使用）。
int collect_speed_profile(pavo2::pavo2_driver& driver, double seconds,
                          std::string& err);

// 将一帧扫描包 vec_buff 中的点做 XY 过滤（原点噪点 + 过滤多边形）后追加到 out。
// z 由调用方给出（通常为 z_for_elapsed(elapsed_ms)）。
// 返回新增点数。
size_t scan_convert_points(const pavo2::pavo_packet_info_scan_t& scan_info,
                           double z_val,
                           std::vector<CountPoint>& out);

// ==================== 扫描会话 ====================
// 单次后台采集会话：reset() → start() 开启采集线程 → stop() 请求停止并等待线程退出。
// count/check 统一使用同一会话，保证同一时刻只有一个采集线程访问雷达。
// 线程安全；仅允许空闲时 reset()。
class ScanSession
{
public:
    // 启动后台采集线程。内部先排空 SDK 积压旧包，排空完成才开始 z 计时。
    // 已在运行或线程创建失败时返回 false。
    bool start(pavo2::pavo2_driver& driver);

    // 请求停止并等待采集线程退出，返回当前已采集点数。
    // 会话未运行（未 start 或已 stop）时仅返回现存点数，不做其他事。
    size_t stop();

    // 拷贝当前点集快照（线程安全）。
    std::vector<CountPoint> snapshot() const;

    // 是否正在采集运行中。
    bool running() const;

    // 复位（仅空闲时调用）：清空上一次点集并复位状态，供下一次 start 使用。
    void reset();

    // 后台采集线程执行体。
    void collect_loop(pavo2::pavo2_driver* driver);

private:
    mutable std::mutex mtx_;
    bool running_  = false;
    bool stop_req_ = false;
    std::thread th_;
    std::vector<CountPoint> points_;
    std::chrono::steady_clock::time_point start_tp_;
};

#endif // LIDAR_UTILS_H