#include "PointCloudImageUtils.h"
#include <QPainter>
#include <cmath>
#include <algorithm>
#include <limits>
#include "VrLog.h"

#include "VrTimeUtils.h"

#ifndef PI
#define PI 3.14159265358979323846
#endif

QImage PointCloudImageUtils::GeneratePointCloudImage(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                                     const std::vector<std::vector<SVzNL3DPoint>>& detectionResults,
                                                     int imageWidth, int imageHeight)
{
    if (scanLines.empty() || imageWidth <= 0 || imageHeight <= 0) {
        LOG_WARNING("Invalid input parameters: scanLines.size()=%d, imageWidth=%d, imageHeight=%d\n", scanLines.size(), imageWidth, imageHeight);
        return QImage();
    }

    // 快速计算点云范围
    double xMin, xMax, yMin, yMax;
    if (!CalculateRangeFast(scanLines, xMin, xMax, yMin, yMax)) {
        LOG_WARNING("No valid points found in scan lines\n");
        return QImage();
    }

    // 检查范围是否有效
    if (xMax <= xMin || yMax <= yMin) {
        LOG_WARNING("Invalid point cloud range: xRange=%.2f, yRange=%.2f\n", xMax - xMin, yMax - yMin);
        return QImage();
    }

    // 创建图像
    QImage image(imageWidth, imageHeight, QImage::Format_RGB888);
    if (image.isNull()) {
        LOG_ERROR("Failed to create QImage with size %dx%d\n", imageWidth, imageHeight);
        return QImage();
    }

    image.fill(Qt::black);

    // 直接绘制到图像数据，避免QPainter开销
    DrawPointCloudDirect(image, scanLines, xMin, xMax, yMin, yMax, imageWidth, imageHeight);

    // 绘制检测结果（如果需要）
    if (!detectionResults.empty()) {
        QPainter painter(&image);
        DrawLapWeldResults(painter, detectionResults, xMin, xMax, yMin, yMax, imageWidth, imageHeight);
    }
    return image;
}

bool PointCloudImageUtils::CalculateRangeFast(const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                              double& xMin, double& xMax,
                                              double& yMin, double& yMax)
{
    // 使用局部变量减少内存写入
    double localXMin = std::numeric_limits<double>::max();
    double localXMax = std::numeric_limits<double>::lowest();
    double localYMin = std::numeric_limits<double>::max();
    double localYMax = std::numeric_limits<double>::lowest();

    bool hasValidPoints = false;

    // 使用const引用减少拷贝开销
    for (const auto& scanLine : scanLines) {
        const SVzNL3DPosition* points = scanLine.data();
        const size_t count = scanLine.size();

        // 使用指针遍历，减少边界检查
        for (size_t i = 0; i < count; ++i) {
            const auto& point = points[i];

            // ARM兼容的Z值检查 - 避免位操作
            if (point.pt3D.z < 1e-4) continue;

            hasValidPoints = true;

            // 使用局部变量缓存坐标值
            const double x = point.pt3D.x;
            const double y = point.pt3D.y;

            // 分支预测友好的比较 - 使用条件运算符
            localXMin = (x < localXMin) ? x : localXMin;
            localXMax = (x > localXMax) ? x : localXMax;
            localYMin = (y < localYMin) ? y : localYMin;
            localYMax = (y > localYMax) ? y : localYMax;
        }
    }

    // 最后一次性写入结果
    xMin = localXMin;
    xMax = localXMax;
    yMin = localYMin;
    yMax = localYMax;

    return hasValidPoints;
}

void PointCloudImageUtils::DrawPointCloudDirect(QImage& image,
                                               const std::vector<std::vector<SVzNL3DPosition>>& scanLines,
                                               double xMin, double xMax, double yMin, double yMax,
                                               int imageWidth, int imageHeight)
{
    if (scanLines.empty()) return;

    // 预计算缩放因子和偏移量
    const double xScale = imageWidth / (xMax - xMin);
    const double yScale = imageHeight / (yMax - yMin);

    // 验证图像格式是否为RGB888
    if (image.format() != QImage::Format_RGB888) {
        LOG_WARNING("Image format is not RGB888, converting...\n");
        image = image.convertToFormat(QImage::Format_RGB888);
    }

    // 直接访问图像数据
    uchar* imageData = image.bits();
    if (!imageData) {
        LOG_ERROR("Failed to get image data pointer\n");
        return;
    }

    const int bytesPerLine = image.bytesPerLine();
    const int maxX = imageWidth - 1;
    const int maxY = imageHeight - 1;

    // 预计算常量
    constexpr uchar grayValue = 150;

    for (const auto& scanLine : scanLines) {
        const SVzNL3DPosition* points = scanLine.data();
        const size_t count = scanLine.size();

        // 使用指针遍历减少边界检查
        for (size_t i = 0; i < count; ++i) {
            const auto& point = points[i];

            // ARM兼容的Z值检查 - 避免位操作
            if (point.pt3D.z < 1e-4) continue;

            // 快速坐标转换
            const int px = static_cast<int>((point.pt3D.x - xMin) * xScale);
            const int py = static_cast<int>((point.pt3D.y - yMin) * yScale);

            // 快速边界检查 - 使用无符号比较
            if (static_cast<unsigned>(px) <= static_cast<unsigned>(maxX) &&
                static_cast<unsigned>(py) <= static_cast<unsigned>(maxY)) {

                // ARM安全的像素写入 - 确保内存对齐
                const size_t pixelOffset = static_cast<size_t>(py) * bytesPerLine + static_cast<size_t>(px) * 3;
                if (pixelOffset + 2 < static_cast<size_t>(image.sizeInBytes())) {
                    uchar* pixel = imageData + pixelOffset;
                    pixel[0] = grayValue; // R
                    pixel[1] = grayValue; // G
                    pixel[2] = grayValue; // B
                }
            }
        }
    }
}

void PointCloudImageUtils::DrawLapWeldResults(QPainter& painter,
                                             const std::vector<std::vector<SVzNL3DPoint>>& weldResults,
                                             double xMin, double xMax, double yMin, double yMax,
                                             int imageWidth, int imageHeight)
{
    if (weldResults.empty()) return;

    double xScale = (xMax - xMin) / imageWidth;
    double yScale = (yMax - yMin) / imageHeight;

    // 使用不同颜色绘制每条焊缝
    QColor weldColors[] = {
        QColor(255, 0, 0),     // 红色
        QColor(0, 255, 0),     // 绿色
        QColor(0, 0, 255),     // 蓝色
        QColor(255, 255, 0),   // 黄色
        QColor(255, 0, 255),   // 紫色
        QColor(0, 255, 255),   // 青色
        QColor(255, 128, 0),   // 橙色
        QColor(128, 255, 0)    // 浅绿色
    };
    int numColors = sizeof(weldColors) / sizeof(weldColors[0]);

    for (size_t i = 0; i < weldResults.size(); i++) {
        const auto& weldLine = weldResults[i];
        if (weldLine.empty()) continue;

        QColor weldColor = weldColors[i % numColors];
        painter.setPen(QPen(weldColor, 3));

        // 绘制焊缝线段
        for (size_t j = 1; j < weldLine.size(); j++) {
            int px1 = (int)((weldLine[j-1].x - xMin) / xScale);
            int py1 = (int)((weldLine[j-1].y - yMin) / yScale);
            int px2 = (int)((weldLine[j].x - xMin) / xScale);
            int py2 = (int)((weldLine[j].y - yMin) / yScale);

            if (px1 >= 0 && px1 < imageWidth && py1 >= 0 && py1 < imageHeight &&
                px2 >= 0 && px2 < imageWidth && py2 >= 0 && py2 < imageHeight) {
                painter.drawLine(px1, py1, px2, py2);
            }
        }

        // 在起点和终点绘制标记
        if (!weldLine.empty()) {
            // 起点标记 - 圆形
            int startX = (int)((weldLine[0].x - xMin) / xScale);
            int startY = (int)((weldLine[0].y - yMin) / yScale);
            if (startX >= 0 && startX < imageWidth && startY >= 0 && startY < imageHeight) {
                painter.setPen(QPen(weldColor, 2));
                painter.setBrush(QBrush(weldColor));
                painter.drawEllipse(startX - 5, startY - 5, 10, 10);
            }

            // 终点标记 - 方形
            int endX = (int)((weldLine.back().x - xMin) / xScale);
            int endY = (int)((weldLine.back().y - yMin) / yScale);
            if (endX >= 0 && endX < imageWidth && endY >= 0 && endY < imageHeight) {
                painter.setPen(QPen(weldColor, 2));
                painter.setBrush(QBrush(weldColor));
                painter.drawRect(endX - 4, endY - 4, 8, 8);
            }
        }
    }
}