#include "core/PointCloudProcessor.h"
#include <QDebug>
#include <vector>

PointCloudProcessor::PointCloudProcessor(QObject *parent)
    : QObject(parent)
    , m_fx(1432.8957f)
    , m_fy(1432.6590f)
    , m_cx(637.5117f)
    , m_cy(521.8720f)
    , m_zScale(0.2f)
    , m_imageWidth(1224)
    , m_imageHeight(1024)
    , m_totalPoints(1224 * 1024)
    , m_platform(nullptr)
    , m_device(nullptr)
    , m_context(nullptr)
    , m_queue(nullptr)
    , m_program(nullptr)
    , m_kernel(nullptr)
    , m_depthBuffer(nullptr)
    , m_xyzBuffer(nullptr)
    , m_clInitialized(false)
{
}

PointCloudProcessor::~PointCloudProcessor()
{
    cleanupOpenCL();
}

void PointCloudProcessor::setCameraIntrinsics(float fx, float fy, float cx, float cy)
{
    m_fx = fx;
    m_fy = fy;
    m_cx = cx;
    m_cy = cy;
}

void PointCloudProcessor::setZScaleFactor(float scale)
{
    m_zScale = scale;
}

bool PointCloudProcessor::initializeOpenCL()
{
    if (m_clInitialized) {
        return true;
    }

    cl_int err;
    qDebug() << "[OpenCL] Starting initialization...";

    // 1. 获取平台
    err = clGetPlatformIDs(1, &m_platform, nullptr);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to get OpenCL platform");
        return false;
    }

    // 2. 获取设备（优先GPU）
    err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_GPU, 1, &m_device, nullptr);
    if (err != CL_SUCCESS) {
        qDebug() << "[OpenCL] GPU not available, using CPU";
        err = clGetDeviceIDs(m_platform, CL_DEVICE_TYPE_CPU, 1, &m_device, nullptr);
        if (err != CL_SUCCESS) {
            emit errorOccurred("Failed to get OpenCL device");
            return false;
        }
    }

    // 3. 创建上下文
    m_context = clCreateContext(nullptr, 1, &m_device, nullptr, nullptr, &err);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create OpenCL context");
        return false;
    }

    // 4. 创建命令队列
#if CL_TARGET_OPENCL_VERSION >= 200
    cl_queue_properties props[] = { CL_QUEUE_ON_DEVICE_DEFAULT, 0 };
    m_queue = clCreateCommandQueueWithProperties(m_context, m_device, props, &err);
#else
    m_queue = clCreateCommandQueue(m_context, m_device, 0, &err);
#endif
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create command queue");
        cleanupOpenCL();
        return false;
    }

    qDebug() << "[OpenCL] Platform, device, context, and queue created successfully";

    // 5. 创建并编译OpenCL程序
    const char* kernelSource =
        "__kernel void compute_xyz(__global const float* depth, "
        "__global float* xyz, int width, int height, "
        "float inv_fx, float inv_fy, float cx, float cy, float z_scale) { "
        "int idx = get_global_id(0); "
        "if (idx >= width * height) return; "
        "int y = idx / width; "
        "int x = idx % width; "
        "float z = depth[idx] * z_scale; "
        // 完全平面的圆柱投影：X和Y直接使用像素坐标，缩放到合适的范围
        "xyz[idx*3] = (x - cx) * 2.0f; "  // X坐标，缩放系数2.0
        "xyz[idx*3+1] = -(y - cy) * 2.0f; "  // Y坐标取反，修正上下颠倒
        "xyz[idx*3+2] = z; "
        "}";

    m_program = clCreateProgramWithSource(m_context, 1, &kernelSource, nullptr, &err);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create OpenCL program");
        cleanupOpenCL();
        return false;
    }

    err = clBuildProgram(m_program, 1, &m_device, nullptr, nullptr, nullptr);
    if (err != CL_SUCCESS) {
        char buildLog[4096];
        clGetProgramBuildInfo(m_program, m_device, CL_PROGRAM_BUILD_LOG, sizeof(buildLog), buildLog, nullptr);
        emit errorOccurred(QString("Failed to build OpenCL program: %1").arg(buildLog));
        cleanupOpenCL();
        return false;
    }

    qDebug() << "[OpenCL] Program compiled successfully";

    // 6. 创建kernel
    m_kernel = clCreateKernel(m_program, "compute_xyz", &err);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create OpenCL kernel");
        cleanupOpenCL();
        return false;
    }

    // 7. 创建缓冲区
    m_depthBuffer = clCreateBuffer(m_context, CL_MEM_READ_ONLY,
                                   sizeof(float) * m_totalPoints, nullptr, &err);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create depth buffer");
        cleanupOpenCL();
        return false;
    }

    m_xyzBuffer = clCreateBuffer(m_context, CL_MEM_WRITE_ONLY,
                                 sizeof(float) * m_totalPoints * 3, nullptr, &err);
    if (err != CL_SUCCESS) {
        emit errorOccurred("Failed to create XYZ buffer");
        cleanupOpenCL();
        return false;
    }

    m_clInitialized = true;
    qDebug() << "[OpenCL] Initialization complete";
    return true;
}

void PointCloudProcessor::processDepthData(const QByteArray &depthData, uint32_t blockId)
{
    if (!m_clInitialized) {
        if (!initializeOpenCL()) {
            return;
        }
    }

    // 验证数据大小
    if (depthData.size() != m_totalPoints * sizeof(int16_t)) {
        qDebug() << "[PointCloud] Invalid depth data size:" << depthData.size()
                 << "expected:" << (m_totalPoints * sizeof(int16_t));
        return;
    }

    // 转换int16_t到float
    const int16_t* depthShort = reinterpret_cast<const int16_t*>(depthData.constData());
    std::vector<float> depthFloat(m_totalPoints);
    for (size_t i = 0; i < m_totalPoints; i++) {
        depthFloat[i] = static_cast<float>(depthShort[i]);
    }

    // 注释掉频繁的日志输出
    // qDebug() << "[PointCloud] Processing block" << blockId << "with" << m_totalPoints << "points";

    // 上传深度数据到GPU
    cl_int err = clEnqueueWriteBuffer(m_queue, m_depthBuffer, CL_TRUE, 0,
                                      sizeof(float) * m_totalPoints, depthFloat.data(),
                                      0, nullptr, nullptr);
    if (err != CL_SUCCESS) {
        emit errorOccurred(QString("Failed to upload depth data: %1").arg(err));
        return;
    }

    // 设置kernel参数
    float inv_fx = 1.0f / m_fx;
    float inv_fy = 1.0f / m_fy;
    int width = m_imageWidth;
    int height = m_imageHeight;

    clSetKernelArg(m_kernel, 0, sizeof(cl_mem), &m_depthBuffer);
    clSetKernelArg(m_kernel, 1, sizeof(cl_mem), &m_xyzBuffer);
    clSetKernelArg(m_kernel, 2, sizeof(int), &width);
    clSetKernelArg(m_kernel, 3, sizeof(int), &height);
    clSetKernelArg(m_kernel, 4, sizeof(float), &inv_fx);
    clSetKernelArg(m_kernel, 5, sizeof(float), &inv_fy);
    clSetKernelArg(m_kernel, 6, sizeof(float), &m_cx);
    clSetKernelArg(m_kernel, 7, sizeof(float), &m_cy);
    clSetKernelArg(m_kernel, 8, sizeof(float), &m_zScale);

    // 执行kernel
    size_t local = 256;
    size_t global = ((m_totalPoints + local - 1) / local) * local;
    err = clEnqueueNDRangeKernel(m_queue, m_kernel, 1, nullptr,
                                 &global, &local, 0, nullptr, nullptr);
    if (err != CL_SUCCESS) {
        emit errorOccurred(QString("Failed to execute kernel: %1").arg(err));
        return;
    }

    clFinish(m_queue);

    // 读取结果
    std::vector<float> xyz(m_totalPoints * 3);
    err = clEnqueueReadBuffer(m_queue, m_xyzBuffer, CL_TRUE, 0,
                              sizeof(float) * m_totalPoints * 3, xyz.data(),
                              0, nullptr, nullptr);
    if (err != CL_SUCCESS) {
        emit errorOccurred(QString("Failed to read XYZ data: %1").arg(err));
        return;
    }

    // 创建PCL点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    cloud->width = m_imageWidth;
    cloud->height = m_imageHeight;
    cloud->is_dense = false;
    cloud->points.resize(m_totalPoints);

    // 填充点云数据（过滤全零点）
    for (size_t i = 0; i < m_totalPoints; i++) {
        cloud->points[i].x = xyz[i * 3];
        cloud->points[i].y = xyz[i * 3 + 1];
        cloud->points[i].z = xyz[i * 3 + 2];
    }

    // 注释掉频繁的日志输出
    // qDebug() << "[PointCloud] Block" << blockId << "processed successfully";
    emit pointCloudReady(cloud, blockId);
}

void PointCloudProcessor::cleanupOpenCL()
{
    if (m_depthBuffer) {
        clReleaseMemObject(m_depthBuffer);
        m_depthBuffer = nullptr;
    }
    if (m_xyzBuffer) {
        clReleaseMemObject(m_xyzBuffer);
        m_xyzBuffer = nullptr;
    }
    if (m_kernel) {
        clReleaseKernel(m_kernel);
        m_kernel = nullptr;
    }
    if (m_program) {
        clReleaseProgram(m_program);
        m_program = nullptr;
    }
    if (m_queue) {
        clReleaseCommandQueue(m_queue);
        m_queue = nullptr;
    }
    if (m_context) {
        clReleaseContext(m_context);
        m_context = nullptr;
    }
    m_clInitialized = false;
    qDebug() << "[OpenCL] Cleanup complete";
}