// lidar_utils.cpp - 雷达连接与扫描采集实现
#include "lidar_utils.h"
#include "config.h"
#include "filter.h"
#include <cmath>
#include <iostream>

// ==================== 雷达连接 ====================
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;
}

// ==================== 扫描辅助 ====================
int drain_stale_scans(pavo2::pavo2_driver& driver, int max_ms)
{
    pavo2::pavo_packet_info_scan_t tmp;
    int flushed = 0;
    auto flush_begin = std::chrono::steady_clock::now();
    while (true)
    {
        if (driver.get_scans_available() <= 0) break;
        if (!driver.get_scanned_data(tmp, 0)) break;   // 0ms = 立即返回，不阻塞
        flushed++;
        if (std::chrono::duration_cast<std::chrono::milliseconds>(
                std::chrono::steady_clock::now() - flush_begin).count() > max_ms)
            break;
    }
    return flushed;
}

double z_for_elapsed(double elapsed_ms)
{
    return (Speed_Z > 0)
        ? (Speed_Z * 10.0 * elapsed_ms / 1000.0)   // cm/s ×10 = mm/s, ×秒 = mm
        : (double)elapsed_ms;
}

size_t scan_convert_points(const pavo2::pavo_packet_info_scan_t& scan_info,
                           double z_val, std::vector<CountPoint>& out)
{
    size_t before = out.size();
    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 (!xy_filter_allowed(x_mm, y_mm)) continue;
        out.push_back({x_mm, y_mm, z_val});
    }
    return out.size() - before;
}

// ==================== 扫描会话 ====================
bool ScanSession::start(pavo2::pavo2_driver& driver)
{
    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (running_) return false;
        running_ = true;
        stop_req_ = false;
    }
    try
    {
        th_ = std::thread(&ScanSession::collect_loop, this, &driver);
        return true;
    }
    catch (...)
    {
        std::lock_guard<std::mutex> lk(mtx_);
        running_ = false;
        stop_req_ = false;
        return false;
    }
}

size_t ScanSession::stop()
{
    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (!running_) return points_.size();
        stop_req_ = true;
    }
    if (th_.joinable()) th_.join();
    std::lock_guard<std::mutex> lk(mtx_);
    running_ = false;
    return points_.size();
}

std::vector<CountPoint> ScanSession::snapshot() const
{
    std::lock_guard<std::mutex> lk(mtx_);
    return points_;
}

bool ScanSession::running() const
{
    std::lock_guard<std::mutex> lk(mtx_);
    return running_;
}

void ScanSession::reset()
{
    std::lock_guard<std::mutex> lk(mtx_);
    points_.clear();
    running_ = false;
    stop_req_ = false;
}

void ScanSession::collect_loop(pavo2::pavo2_driver* driver)
{
    // 排空积压：丢弃 SDK 缓存中 start 之前雷达已转出的旧数据包。
    // 否则旧包会在图像开头 z 方向挤成一段密集点；排空完成才开始计时。
    int flushed = drain_stale_scans(*driver);

    {
        std::lock_guard<std::mutex> lk(mtx_);
        if (stop_req_) return;   // 排空期间被 stop，直接退出
        start_tp_ = std::chrono::steady_clock::now();   // 排空完成才开始计时
    }
    if (flushed > 0)
        std::cout << "[lidar] 已丢弃积压扫描包 " << flushed << " 个" << std::endl;

    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();
            double elapsed_ms = (double)std::chrono::duration_cast<std::chrono::milliseconds>(
                                    now - start_tp_).count();
            double z_val = z_for_elapsed(elapsed_ms);

            std::lock_guard<std::mutex> lk(mtx_);
            if (stop_req_) break;
            scan_convert_points(scan_info, z_val, points_);
        }
        else
        {
            std::lock_guard<std::mutex> lk(mtx_);
            if (stop_req_) break;
        }
    }
}