#include <iostream>
#include <iomanip>
#include <thread>
#include <chrono>
#include <atomic>
#include <mutex>
#include <string>
#include <ctime>
#include <sstream>
#include <vector>
#include <memory>
#include <cstring>
#include <cmath>
#include <limits>
#define NOMINMAX
#include <windows.h>
#include "IRsLidarDevice.h"
#include "ICloudShow.h"

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

/// 全局控制标志
static std::atomic<bool> g_bExit{false};

/// 统计信息
static std::atomic<uint64_t> g_nFrameCount{0};
static std::atomic<uint64_t> g_nTotalPoints{0};
static std::chrono::steady_clock::time_point g_startTime;

/// 当前帧追踪
static std::atomic<int> g_nFrameBeginCount{0};

/// 控制台输出锁
static std::mutex g_consoleMutex;
static bool g_bVerbose = false;
static bool g_bDisplay = true;
static std::string g_saveDir;
static std::atomic<int> g_nSaveSeq{0};
static std::unique_ptr<ICloudShow> g_pCloudShow;

static void PrintLog(const std::string& message)
{
    std::lock_guard<std::mutex> lock(g_consoleMutex);
    auto now = std::time(nullptr);
    char timeStr[20];
    std::strftime(timeStr, sizeof(timeStr), "%H:%M:%S", std::localtime(&now));
    std::cout << "[" << timeStr << "] " << message << std::endl;
}

/// 每收到一个原始包触发 — 通过 is_frame_begin 感知扫描起始
static void OnPacket(const RsPacketInfo& pkt)
{
    if (pkt.is_frame_begin)
    {
        g_nFrameBeginCount++;

        if (g_bVerbose)
        {
            std::ostringstream oss;
            oss << ">>> is_frame_begin=1"
                << " | pkt_seq=" << pkt.seq
                << " | 第 " << g_nFrameBeginCount << " 圈";
            PrintLog(oss.str());
        }
    }
}

/// 每收到一帧完整点云触发 — 可在此做后处理
static void OnPointCloud(const RsPointCloudData& data)
{
    if (data.pointCount == 0) return;

    g_nFrameCount++;
    g_nTotalPoints += data.pointCount;

    if (g_bVerbose)
    {
        const auto& first = data.points[0];
        const auto& last  = data.points[data.pointCount - 1];

        std::ostringstream oss;
        oss << "  └─ 完整帧 #" << data.seq
            << " | " << data.pointCount << "点"
            << " | ts=" << std::fixed << std::setprecision(3) << data.timestamp
            << " | (" << first.x << "," << first.y << "," << first.z << ")"
            << "→(" << last.x << "," << last.y << "," << last.z << ")";
        PrintLog(oss.str());
    }

    // 转换为 PCL 格式 — RsPointXYZI(packed 13B) 与 pcl::PointXYZI(20B) 布局不同，需逐字段拷贝
    // NaN→0 和 m→mm 已在 RsLidarDevice 层处理
    auto pclCloud = std::make_shared<pcl::PointCloud<pcl::PointXYZI>>();
    pclCloud->height   = data.height;
    pclCloud->width    = data.width;
    pclCloud->is_dense = data.isDense;
    pclCloud->points.resize(data.pointCount);
    const float nan = std::numeric_limits<float>::quiet_NaN();
    for (size_t i = 0; i < data.pointCount; ++i)
    {
        float x = data.points[i].x;
        float y = data.points[i].y;
        float z = data.points[i].z;
        if (std::abs(x) < 0.005f && std::abs(y) < 0.005f && std::abs(z) < 0.005f)
        {
            pclCloud->points[i].x = nan;
            pclCloud->points[i].y = nan;
            pclCloud->points[i].z = nan;
            pclCloud->is_dense = false;
        }
        else
        {
            pclCloud->points[i].x = x;
            pclCloud->points[i].y = y;
            pclCloud->points[i].z = z;
        }
        pclCloud->points[i].intensity = static_cast<float>(data.points[i].intensity);
    }

    // 持续保存到目录（--save dir）
    if (!g_saveDir.empty() && g_pCloudShow)
    {
        int seq = ++g_nSaveSeq;
        std::ostringstream oss;
        oss << g_saveDir << "/LaserData_" << seq << ".txt";
        PrintLog("开始存储: " + oss.str());
        g_pCloudShow->SaveToTxt(oss.str(), pclCloud);
        PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast<int>(data.pointCount)) + "点");
    }

    // 单次保存（PCL 窗口按键 S/s 触发）
    if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsSaveRequested())
    {
        g_pCloudShow->ClearSaveRequest();
        int seq = ++g_nSaveSeq;
        std::string dir = g_saveDir.empty() ? "." : g_saveDir;
        std::ostringstream oss;
        oss << dir << "/LaserData_" << seq << ".txt";
        PrintLog("开始存储: " + oss.str());
        g_pCloudShow->SaveToTxt(oss.str(), pclCloud);
        PrintLog("存储完成: " + oss.str() + " | " + std::to_string(static_cast<int>(data.pointCount)) + "点");
    }

    // 推送点云到显示窗口
    if (g_bDisplay && g_pCloudShow)
    {
        g_pCloudShow->UpdateCloud(pclCloud);
    }
}

static void OnException(const RsExceptionInfo& info)
{
    PrintLog("[异常 " + std::to_string(info.code) + "] " + info.message);
}

static void StatsLoop()
{
    while (!g_bExit)
    {
        std::this_thread::sleep_for(std::chrono::seconds(5));

        auto now = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::seconds>(now - g_startTime).count();
        if (elapsed == 0) elapsed = 1;

        std::ostringstream oss;
        oss << "== " << elapsed << "s"
            << " | 帧=" << g_nFrameCount
            << " | 帧起始=" << g_nFrameBeginCount
            << " | fps=" << std::fixed << std::setprecision(1)
            << (static_cast<double>(g_nFrameCount) / elapsed)
            << " | pps=" << std::fixed << std::setprecision(0)
            << (static_cast<double>(g_nTotalPoints) / elapsed);
        PrintLog(oss.str());
    }
}

static void PrintUsage(const char* prog)
{
    std::cout << "用法: " << prog << " [选项]" << std::endl;
    std::cout << std::endl;
    std::cout << "选项:" << std::endl;
    std::cout << "  --host <IP>        雷达主机地址 (默认: 0.0.0.0)" << std::endl;
    std::cout << "  --msop <port>      MSOP 端口 (默认: 6699)" << std::endl;
    std::cout << "  --difop <port>     DIFOP 端口 (默认: 7788)" << std::endl;
    std::cout << "  --type <name>      雷达型号 (默认: RSAIRY)" << std::endl;
    std::cout << "  --time <seconds>   运行时长 (默认: 30, 0=持续)" << std::endl;
    std::cout << "  --verbose          逐帧打印扫描生命周期" << std::endl;
    std::cout << "  --display          实时点云显示（非阻塞，PCL窗口 S=保存 Q=退出）" << std::endl;
    std::cout << "  --save <dir>       保存点云到指定目录（frame_<seq>.txt）" << std::endl;
    std::cout << std::endl;
    std::cout << "雷达型号: RS16 RS32 RSBP RSAIRY RSHELIOS RS128 RS80 RS48 RSM1 RSM2 RSM3 等" << std::endl;
    std::cout << std::endl;
    std::cout << "示例:" << std::endl;
    std::cout << "  " << prog << " --host 192.168.1.100 --type RSHELIOS --time 60 --verbose" << std::endl;
    std::cout << "  " << prog << " --host 192.168.1.100 --type RSAIRY --display --save ./data --time 120" << std::endl;
}

int main(int argc, char* argv[])
{
    SetConsoleOutputCP(CP_UTF8);

    RsLidarConfig config;
    int runSeconds = 0;

    for (int i = 1; i < argc; ++i)
    {
        std::string arg = argv[i];
        if (arg == "--help" || arg == "-h")
        {
            PrintUsage(argv[0]);
            return 0;
        }
        else if (arg == "--host" && i + 1 < argc)
            config.hostAddress = argv[++i];
        else if (arg == "--msop" && i + 1 < argc)
            config.msopPort = static_cast<uint16_t>(std::stoi(argv[++i]));
        else if (arg == "--difop" && i + 1 < argc)
            config.difopPort = static_cast<uint16_t>(std::stoi(argv[++i]));
        else if (arg == "--type" && i + 1 < argc)
            config.lidarType = argv[++i];
        else if (arg == "--time" && i + 1 < argc)
            runSeconds = std::stoi(argv[++i]);
        else if (arg == "--verbose")
            g_bVerbose = true;
        else if (arg == "--display")
            g_bDisplay = true;
        else if (arg == "--save" && i + 1 < argc)
            g_saveDir = argv[++i];
    }

    std::cout << "============================================" << std::endl;
    std::cout << "  RsLidarDevice 测试" << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "雷达: " << config.lidarType << std::endl;
    std::cout << "地址: " << config.hostAddress << std::endl;
    std::cout << "端口: MSOP=" << config.msopPort << " DIFOP=" << config.difopPort << std::endl;
    std::cout << "时长: " << (runSeconds > 0 ? std::to_string(runSeconds) + "s" : "持续") << std::endl;
    std::cout << "详细: " << (g_bVerbose ? "ON" : "OFF") << std::endl;
    std::cout << "============================================" << std::endl;

    // 1. 创建
    IRsLidarDevice* pDevice = nullptr;
    if (IRsLidarDevice::CreateObject(&pDevice) != 0 || !pDevice)
    {
        std::cerr << "创建设备失败" << std::endl;
        return -1;
    }
    PrintLog("设备实例创建完成");
    PrintLog("驱动版本: " + pDevice->GetVersion());

    // CloudShow 实例
    g_pCloudShow = ICloudShow::Create();

    // 2. 初始化
    if (pDevice->InitDevice() != 0)
    {
        std::cerr << "SDK 初始化失败" << std::endl;
        delete pDevice;
        return -1;
    }
    PrintLog("SDK 初始化完成");

    // 3. 打开
    if (pDevice->OpenDevice(config) != 0)
    {
        std::cerr << "打开设备失败" << std::endl;
        pDevice->CloseDevice();
        delete pDevice;
        return -1;
    }
    PrintLog("设备打开完成");

    // 4. 注册回调
    pDevice->SetPacketCallback(OnPacket);
    pDevice->SetPointCloudCallback(OnPointCloud);
    pDevice->SetExceptionCallback(OnException);

    // 5. 启动
    if (pDevice->Start() != 0)
    {
        std::cerr << "启动失败" << std::endl;
        pDevice->CloseDevice();
        delete pDevice;
        return -1;
    }
    g_startTime = std::chrono::steady_clock::now();
    PrintLog("数据采集已启动");

    // 6. 统计线程
    std::thread statsThread(StatsLoop);

    // 7. 启动显示窗口（主线程创建 viewer + 驱动渲染）
    if (g_bDisplay && g_pCloudShow)
    {
        if (g_pCloudShow->Start("LiDAR PointCloud") == 0)
        {
            PrintLog("PCL 窗口已打开 — S=保存下一帧  Q=退出");
        }
        else
        {
            PrintLog("PCL 窗口创建失败，继续无显示运行");
            g_bDisplay = false;
        }
    }

    // 8. 主循环：驱动 PCL 渲染 + 轮询键盘 + 超时
    auto deadline = (runSeconds > 0)
        ? std::chrono::steady_clock::now() + std::chrono::seconds(runSeconds)
        : std::chrono::steady_clock::time_point::max();

    while (!g_bExit)
    {
        if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsRunning())
        {
            if (g_pCloudShow->SpinOnce(10) < 0)
            {
                PrintLog("PCL 窗口已关闭");
                break;
            }
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }

        if (g_bDisplay && g_pCloudShow && g_pCloudShow->IsQuitRequested())
        {
            PrintLog("用户退出 (Q)");
            break;
        }

        if (std::chrono::steady_clock::now() >= deadline)
            break;
    }

    // 9. 停止
    g_bExit = true;
    pDevice->Stop();
    if (statsThread.joinable()) statsThread.join();

    auto endTime = std::chrono::steady_clock::now();
    auto totalElapsed = std::chrono::duration_cast<std::chrono::seconds>(endTime - g_startTime).count();
    if (totalElapsed == 0) totalElapsed = 1;

    std::cout << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "  最终统计" << std::endl;
    std::cout << "============================================" << std::endl;
    std::cout << "运行: " << totalElapsed << "s" << std::endl;
    std::cout << "is_frame_begin: " << g_nFrameBeginCount << " 次" << std::endl;
    std::cout << "完整帧: " << g_nFrameCount << std::endl;
    std::cout << "总点数: " << g_nTotalPoints << std::endl;
    std::cout << "帧率: " << std::fixed << std::setprecision(2)
              << (static_cast<double>(g_nFrameCount) / totalElapsed) << " fps" << std::endl;
    std::cout << "点率: " << std::fixed << std::setprecision(0)
              << (static_cast<double>(g_nTotalPoints) / totalElapsed) << " pts/s" << std::endl;
    std::cout << "============================================" << std::endl;

    pDevice->CloseDevice();
    delete pDevice;

    if (g_pCloudShow)
    {
        g_pCloudShow->Stop();
        g_pCloudShow.reset();
    }

    return 0;
}