#include "RsLidarDevice.h"
#include <cmath>
#include <cstdio>
#include <chrono>

// WSAStartup 引用计数
static std::atomic<int> g_sockRefCount{0};

// ============================================================
// SDK ErrCode → 统计桶映射
// SDK error_code.hpp: MSOPTIMEOUT=0x40 WRONGMSOPLEN=0x42 PKTBUFOVERFLOW=0x48 CLOUDOVERFLOW=0x49
// 桶顺序与日志列对齐：0=msopto 1=pktof 2=cldof 3=wlen 4=other
// ============================================================
size_t CRsLidarDevice::errCodeToBucket(int errCode)
{
    switch (errCode)
    {
        case 0x40: return 0;  // MSOPTIMEOUT
        case 0x48: return 1;  // PKTBUFOVERFLOW
        case 0x49: return 2;  // CLOUDOVERFLOW
        case 0x42: return 3;  // WRONGMSOPLEN
        default:   return 4;  // other
    }
}

// ============================================================
// 工厂方法
// ============================================================
int IRsLidarDevice::CreateObject(IRsLidarDevice** ppDevice)
{
    if (!ppDevice) return -1;
    CRsLidarDevice* p = new CRsLidarDevice();
    *ppDevice = p;
    return 0;
}

// ============================================================
// 构造 / 析构
// ============================================================
CRsLidarDevice::CRsLidarDevice()
    : m_pDriver(std::make_unique<LidarDriver<SdkCloudMsg>>())
{
}

CRsLidarDevice::~CRsLidarDevice()
{
    Stop();
    CloseDevice();
}

// ============================================================
// InitDevice
// ============================================================
int CRsLidarDevice::InitDevice()
{
#ifdef _WIN32
    if (g_sockRefCount == 0)
    {
        WSADATA wsaData;
        if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0)
        {
            return -1;
        }
    }
    g_sockRefCount++;
#endif
    return 0;
}

// ============================================================
// 类型转换: RsLidarConfig → RSDriverParam
// ============================================================
RSDriverParam CRsLidarDevice::toDriverParam(const RsLidarConfig& config)
{
    RSDriverParam param;
    param.lidar_type               = strToLidarType(config.lidarType);
    param.input_type               = InputType::ONLINE_LIDAR;
    param.frame_id                 = config.frameId;
    param.input_param.host_address = config.hostAddress;
    param.input_param.msop_port    = config.msopPort;
    param.input_param.difop_port   = config.difopPort;
    param.input_param.imu_port     = config.imuPort;
    param.decoder_param.min_distance  = config.minDistance;
    param.decoder_param.max_distance  = config.maxDistance;
    param.decoder_param.dense_points  = config.densePoints;
    param.decoder_param.use_lidar_clock = config.useLidarClock;
    param.decoder_param.ts_first_point  = config.tsFirstPoint;
    param.decoder_param.wait_for_difop  = config.waitForDifop;
    param.decoder_param.start_angle     = config.startAngle;
    param.decoder_param.end_angle       = config.endAngle;
    param.decoder_param.split_angle     = config.splitAngle;

    param.input_param.socket_recv_buf   = (config.socketRecvBufBytes > 0)
                                          ? config.socketRecvBufBytes
                                          : 33554432;  // 32MB，低配电脑防内核缓冲溢出

    return param;
}

// ============================================================
// OpenDevice
// ============================================================
int CRsLidarDevice::OpenDevice(const RsLidarConfig& config)
{
    if (!m_pDriver)
    {
        return -1;
    }

    CloseDevice();

    m_param = toDriverParam(config);

    m_pDriver->regPointCloudCallback(
        [this]() -> SdkCloudPtr { return this->getFreeCloud(); },
        [this](SdkCloudPtr msg)  { this->putStuffedCloud(msg); }
    );

    m_pDriver->regPacketCallback(
        [this](const Packet& pkt) { this->onPacket(pkt); }
    );

    m_pDriver->regExceptionCallback(
        [this](const Error& code) { this->onException(code); }
    );

    if (!m_pDriver->init(m_param))
    {
        return -2;
    }

    m_bOpened = true;
    return 0;
}

// ============================================================
// CloseDevice
// ============================================================
int CRsLidarDevice::CloseDevice()
{
    if (m_bRunning)
    {
        Stop();
    }

    m_bOpened = false;

    m_freeQueue.clear();
    m_stuffedQueue.clear();

#ifdef _WIN32
    if (--g_sockRefCount == 0)
    {
        WSACleanup();
    }
#endif

    return 0;
}

bool CRsLidarDevice::IsOpened() const
{
    return m_bOpened;
}

// ============================================================
// Start / Stop
// ============================================================
int CRsLidarDevice::Start()
{
    if (!m_bOpened || !m_pDriver)
    {
        return -1;
    }

    if (m_bRunning)
    {
        return 0;
    }

    // 重置诊断计数（每次 Start 开始新会话统计）
    m_droppedFrameCount.store(0, std::memory_order_relaxed);
    m_totalPushCount.store(0, std::memory_order_relaxed);
    for (auto& c : m_exceptionCounts) c.store(0, std::memory_order_relaxed);
    m_cbMaxUs.store(0, std::memory_order_relaxed);
    m_cbAccumUs.store(0, std::memory_order_relaxed);
    m_cbCount.store(0, std::memory_order_relaxed);
    m_qPeak.store(0, std::memory_order_relaxed);

    // 清空可能残留的预热缓冲（防止 Stop→Start 循环累积膨胀）
    m_freeQueue.clear();
    m_stuffedQueue.clear();

    for (int i = 0; i < 32; ++i)
    {
        m_freeQueue.push(std::make_shared<SdkCloudMsg>());
    }

    m_bStopProcess = false;
    m_pDriver->start();

    m_bRunning = true;
    m_processThread = std::thread(&CRsLidarDevice::processCloudThread, this);

    return 0;
}

int CRsLidarDevice::Stop()
{
    if (!m_bRunning)
    {
        return 0;
    }

    m_bStopProcess = true;

    if (m_pDriver)
    {
        m_pDriver->stop();
    }

    if (m_processThread.joinable())
    {
        m_processThread.join();
    }

    m_bRunning = false;
    return 0;
}

bool CRsLidarDevice::IsRunning() const
{
    return m_bRunning;
}

// ============================================================
// 回调注册
// ============================================================
int CRsLidarDevice::SetPointCloudCallback(PointCloudCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_pointCloudCallback = std::move(callback);
    return 0;
}

int CRsLidarDevice::SetRawCloudCallback(RawCloudCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_rawCloudCallback = std::move(callback);
    return 0;
}

int CRsLidarDevice::SetPacketCallback(PacketCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_packetCallback = std::move(callback);
    return 0;
}

int CRsLidarDevice::SetExceptionCallback(ExceptionCallback callback)
{
    std::lock_guard<std::mutex> lock(m_callbackMutex);
    m_exceptionCallback = std::move(callback);
    return 0;
}

std::string CRsLidarDevice::GetVersion()
{
    return getDriverVersion();
}

// ============================================================
// 内部：5s 窗口诊断日志输出 + 字段重置（仅当距上次输出 ≥5s 时实际输出）
// 在 processCloudThread 内部调用，因此对窗口字段的 exchange 是单写者操作
// ============================================================
void CRsLidarDevice::emitDiagnosticLogIfDue(std::chrono::steady_clock::time_point& lastLogTime)
{
    auto now = std::chrono::steady_clock::now();
    if (std::chrono::duration_cast<std::chrono::seconds>(now - lastLogTime).count() < 5)
    {
        return;
    }

    uint64_t cbCnt = m_cbCount.exchange(0);
    uint64_t cbMax = m_cbMaxUs.exchange(0);
    uint64_t cbAcc = m_cbAccumUs.exchange(0);
    uint64_t cbAvg = (cbCnt > 0) ? (cbAcc / cbCnt) : 0;
    uint64_t e0 = m_exceptionCounts[0].exchange(0);
    uint64_t e1 = m_exceptionCounts[1].exchange(0);
    uint64_t e2 = m_exceptionCounts[2].exchange(0);
    uint64_t e3 = m_exceptionCounts[3].exchange(0);
    uint64_t e4 = m_exceptionCounts[4].exchange(0);
    size_t   qPeakSnap = m_qPeak.load();

    fprintf(stderr,
        "[RsLidarDevice] qd=%zu qpeak=%zu drop=%llu push=%llu | "
        "cb_us max=%llu avg=%llu n=%llu | "
        "exc msopto=%llu pktof=%llu cldof=%llu wlen=%llu other=%llu\n",
        static_cast<size_t>(m_stuffedQueue.size()),
        qPeakSnap,
        static_cast<unsigned long long>(m_droppedFrameCount.load()),
        static_cast<unsigned long long>(m_totalPushCount.load()),
        static_cast<unsigned long long>(cbMax),
        static_cast<unsigned long long>(cbAvg),
        static_cast<unsigned long long>(cbCnt),
        static_cast<unsigned long long>(e0),
        static_cast<unsigned long long>(e1),
        static_cast<unsigned long long>(e2),
        static_cast<unsigned long long>(e3),
        static_cast<unsigned long long>(e4));
    lastLogTime = now;
}

// ============================================================
// 内部：点云处理线程
// ============================================================
void CRsLidarDevice::processCloudThread()
{
    auto lastLogTime = std::chrono::steady_clock::now();
    while (!m_bStopProcess)
    {
        SdkCloudPtr sdkCloud = m_stuffedQueue.popWait(500000);
        if (!sdkCloud || sdkCloud->points.empty())
        {
            // 即使无帧也要按周期打印诊断（确认线程活着 + 异常计数继续流动）
            emitDiagnosticLogIfDue(lastLogTime);
            continue;
        }

        // 每 5 秒打印队列深度、丢帧、回调耗时、异常分类（窗口式重置）
        emitDiagnosticLogIfDue(lastLogTime);

        // 优先使用原始回调（零拷贝路径，用户自行在一次遍历中完成转换）
        RawCloudCallback rawCb;
        {
            std::lock_guard<std::mutex> lock(m_callbackMutex);
            rawCb = m_rawCloudCallback;
        }

        auto cbStart = std::chrono::steady_clock::now();

        if (rawCb)
        {
            RsFrameMeta meta;
            meta.seq       = sdkCloud->seq;
            meta.timestamp = sdkCloud->timestamp;
            meta.height    = sdkCloud->height;
            meta.width     = sdkCloud->width;
            meta.isDense   = sdkCloud->is_dense;
            meta.frameId   = sdkCloud->frame_id;

            const auto& src = sdkCloud->points;
            rawCb(reinterpret_cast<const RsPointXYZI*>(src.data()),
                  static_cast<uint32_t>(src.size()), meta);
        }
        else
        {
            PointCloudCallback cb;
            {
                std::lock_guard<std::mutex> lock(m_callbackMutex);
                cb = m_pointCloudCallback;
            }

            if (cb)
            {
                // 转换并清洗数据: NaN→0, m→mm
                const auto& src = sdkCloud->points;
                std::vector<RsPointXYZI> transformed;
                transformed.reserve(src.size());
                for (const auto& s : src)
                {
                    RsPointXYZI p;
                    p.x = (std::isnan(s.x) ? 0.0f : s.x) * 1000.0f;
                    p.y = (std::isnan(s.y) ? 0.0f : s.y) * 1000.0f;
                    p.z = (std::isnan(s.z) ? 0.0f : s.z) * 1000.0f;
                    p.intensity = s.intensity;
                    transformed.push_back(p);
                }

                RsPointCloudData data;
                data.seq        = sdkCloud->seq;
                data.timestamp  = sdkCloud->timestamp;
                data.height     = sdkCloud->height;
                data.width      = sdkCloud->width;
                data.isDense    = sdkCloud->is_dense;
                data.frameId    = sdkCloud->frame_id;
                data.pointCount = static_cast<uint32_t>(transformed.size());
                data.points     = transformed.data();

                cb(data);
            }
        }

        // 累计回调耗时
        auto cbEnd = std::chrono::steady_clock::now();
        uint64_t us = static_cast<uint64_t>(
            std::chrono::duration_cast<std::chrono::microseconds>(cbEnd - cbStart).count());
        m_cbAccumUs.fetch_add(us, std::memory_order_relaxed);
        m_cbCount.fetch_add(1, std::memory_order_relaxed);
        // CAS 更新 max
        uint64_t prevMax = m_cbMaxUs.load(std::memory_order_relaxed);
        while (us > prevMax &&
               !m_cbMaxUs.compare_exchange_weak(prevMax, us, std::memory_order_relaxed))
        {
            // prevMax 已被 compare_exchange_weak 自动更新，循环再试
        }

        m_freeQueue.push(sdkCloud);
    }
}

// ============================================================
// 内部：空闲/就绪队列回调（rs_driver 内部线程）
// ============================================================
CRsLidarDevice::SdkCloudPtr CRsLidarDevice::getFreeCloud()
{
    auto msg = m_freeQueue.pop();
    if (msg) return msg;
    return std::make_shared<SdkCloudMsg>();
}

void CRsLidarDevice::putStuffedCloud(SdkCloudPtr msg)
{
    m_totalPushCount++;
    size_t depth = m_stuffedQueue.push(msg);
    if (depth == 0)
    {
        m_droppedFrameCount++;
        // 丢帧时把 SdkCloudMsg 回收到 freeQueue，复用其 points vector 容量
        // (单帧最多 reserve MAX_POINT_CLOUD_SIZE × 13B ≈ 22MB，避免反复分配)
        m_freeQueue.push(msg);
    }
    else
    {
        // CAS 采样队列峰值（Open 以来）
        size_t prevPeak = m_qPeak.load(std::memory_order_relaxed);
        while (depth > prevPeak &&
               !m_qPeak.compare_exchange_weak(prevPeak, depth, std::memory_order_relaxed))
        {
            // prevPeak 已被自动更新，循环重试
        }
    }
}

// ============================================================
// 内部：Packet → RsPacketInfo
// ============================================================
void CRsLidarDevice::onPacket(const Packet& pkt)
{
    PacketCallback cb;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        cb = m_packetCallback;
    }

    if (cb)
    {
        RsPacketInfo info;
        info.timestamp      = pkt.timestamp;
        info.seq            = pkt.seq;
        info.is_difop       = pkt.is_difop;
        info.is_frame_begin = pkt.is_frame_begin;
        info.frameId        = pkt.frame_id;
        info.dataSize       = static_cast<uint32_t>(pkt.buf_.size());
        cb(info);
    }
}

// ============================================================
// 监控接口
// ============================================================
uint64_t CRsLidarDevice::GetDroppedFrameCount() const
{
    return m_droppedFrameCount.load();
}

size_t CRsLidarDevice::GetStuffedQueueDepth() const
{
    return m_stuffedQueue.size();
}

uint64_t CRsLidarDevice::GetExceptionCount(int errCode) const
{
    size_t bucket = errCodeToBucket(errCode);
    return m_exceptionCounts[bucket].load(std::memory_order_relaxed);
}

RsCallbackStats CRsLidarDevice::GetCallbackLatencyStats() const
{
    RsCallbackStats stats;
    stats.count = m_cbCount.load(std::memory_order_relaxed);
    stats.maxUs = m_cbMaxUs.load(std::memory_order_relaxed);
    uint64_t acc = m_cbAccumUs.load(std::memory_order_relaxed);
    stats.avgUs = (stats.count > 0) ? (acc / stats.count) : 0;
    return stats;
}

size_t CRsLidarDevice::GetStuffedQueuePeak() const
{
    return m_qPeak.load(std::memory_order_relaxed);
}

// ============================================================
// 内部：Error → RsExceptionInfo（按 ErrCode 分桶计数后转发）
// ============================================================
void CRsLidarDevice::onException(const Error& code)
{
    // 分桶累加（用于诊断日志和 GetExceptionCount 查询）
    size_t bucket = errCodeToBucket(static_cast<int>(code.error_code));
    m_exceptionCounts[bucket].fetch_add(1, std::memory_order_relaxed);

    ExceptionCallback cb;
    {
        std::lock_guard<std::mutex> lock(m_callbackMutex);
        cb = m_exceptionCallback;
    }

    if (cb)
    {
        RsExceptionInfo info;
        info.code    = static_cast<int>(code.error_code);
        info.message = code.toString();
        cb(info);
    }
}