#include "DetectionOutputConverter.h"
#include "IYDiscHolePoseStatus.h"
#include <cmath>
#include <QJsonArray>
#include <QJsonObject>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

// 从法向量计算欧拉角（pitch/yaw 由法向量方向确定，roll 设为 0）
void DetectionOutputConverter::NormalDirToEuler(double nx, double ny, double nz,
                                                  double& roll, double& pitch, double& yaw)
{
    roll = 0.0;

    const double xyLen = std::sqrt(nx * nx + ny * ny);
    // pitch 由 z 分量与水平面夹角确定
    pitch = std::atan2(-nz, xyLen) * 180.0 / M_PI;
    // yaw 由 x/y 分量确定
    yaw = std::atan2(ny, nx) * 180.0 / M_PI;
}

ProtocolDetectionOutput DetectionOutputConverter::ConvertDiscHoleResult(
    const std::vector<DiscHoleInfo>& discHoleInfoList,
    bool success,
    int errorCode,
    const QString& message,
    int cameraIndex)
{
    ProtocolDetectionOutput output;
    output.type = DETECTION_TYPE_DISC_HOLE;
    output.success = success;
    output.errorCode = errorCode;
    output.message = message;
    output.cameraIndex = cameraIndex;

    for (const auto& hole : discHoleInfoList) {
        DiscHoleDetectOutput holeOutput;
        holeOutput.x = hole.centerX;
        holeOutput.y = hole.centerY;
        holeOutput.z = hole.centerZ;
        NormalDirToEuler(hole.normDirX, hole.normDirY, hole.normDirZ,
                         holeOutput.roll, holeOutput.pitch, holeOutput.yaw);
        output.discHoleOutputs.push_back(holeOutput);
    }

    return output;
}

ProtocolDetectionOutput DetectionOutputConverter::ConvertDiscRackResult(
    const std::vector<DiscRackInfo>& discRackInfoList,
    bool success,
    int errorCode,
    const QString& message,
    int cameraIndex)
{
    ProtocolDetectionOutput output;
    output.type = DETECTION_TYPE_DISC_RACK;
    output.success = success;
    output.errorCode = errorCode;
    output.message = message;
    output.cameraIndex = cameraIndex;

    for (const auto& rack : discRackInfoList) {
        DiscRackDetectOutput rackOutput;
        rackOutput.x = rack.centerX;
        rackOutput.y = rack.centerY;
        rackOutput.z = rack.centerZ;
        rackOutput.roll = rack.roll;
        rackOutput.pitch = rack.pitch;
        rackOutput.yaw = rack.yaw;
        output.discRackOutputs.push_back(rackOutput);
    }

    return output;
}

QJsonObject DetectionOutputConverter::DiscHoleOutputToJson(const DiscHoleDetectOutput& output)
{
    QJsonObject obj;
    obj["X"] = output.x;
    obj["Y"] = output.y;
    obj["Z"] = output.z;
    obj["Roll"] = output.roll;
    obj["Pitch"] = output.pitch;
    obj["Yaw"] = output.yaw;
    return obj;
}

QJsonObject DetectionOutputConverter::DiscRackOutputToJson(const DiscRackDetectOutput& output)
{
    QJsonObject obj;
    obj["X"] = output.x;
    obj["Y"] = output.y;
    obj["Z"] = output.z;
    obj["Roll"] = output.roll;
    obj["Pitch"] = output.pitch;
    obj["Yaw"] = output.yaw;
    return obj;
}

QJsonObject DetectionOutputConverter::ToJson(const ProtocolDetectionOutput& output)
{
    QJsonObject response;
    if (output.type == DETECTION_TYPE_DISC_RACK) {
        response["MessageType"] = "DiscRackResult";

        QJsonObject data;
        data["Success"] = output.success;
        data["ErrorCode"] = output.errorCode;
        data["Message"] = output.message;
        data["CameraIndex"] = output.cameraIndex;

        QJsonArray arr;
        for (const auto& r : output.discRackOutputs) {
            arr.append(DiscRackOutputToJson(r));
        }
        data["Count"] = static_cast<int>(output.discRackOutputs.size());
        data["DiscRacks"] = arr;
        response["Data"] = data;
    } else {
        response["MessageType"] = "DiscHoleResult";

        QJsonObject data;
        data["Success"] = output.success;
        data["ErrorCode"] = output.errorCode;
        data["Message"] = output.message;
        data["CameraIndex"] = output.cameraIndex;

        QJsonArray arr;
        for (const auto& h : output.discHoleOutputs) {
            arr.append(DiscHoleOutputToJson(h));
        }
        data["Count"] = static_cast<int>(output.discHoleOutputs.size());
        data["DiscHoles"] = arr;
        response["Data"] = data;
    }
    response["Timestamp"] = output.timestamp;
    return response;
}