#include "DetectionOutputConverter.h"
#include "IYTireHolePoseStatus.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::ConvertTireHoleResult(
    const std::vector<TireHoleInfo>& tireHoleInfoList,
    bool success,
    int errorCode,
    const QString& message,
    int cameraIndex)
{
    ProtocolDetectionOutput output;
    output.success = success;
    output.errorCode = errorCode;
    output.message = message;
    output.cameraIndex = cameraIndex;

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

    return output;
}

QJsonObject DetectionOutputConverter::TireHoleOutputToJson(const TireHoleDetectOutput& 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;
    obj["HoleR"] = output.holeR;
    return obj;
}

QJsonObject DetectionOutputConverter::ToJson(const ProtocolDetectionOutput& output)
{
    QJsonObject response;
    response["MessageType"] = "TireHoleResult";

    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.tireHoleOutputs) {
        arr.append(TireHoleOutputToJson(h));
    }
    data["Count"] = static_cast<int>(output.tireHoleOutputs.size());
    data["TireHoles"] = arr;
    response["Data"] = data;
    response["Timestamp"] = output.timestamp;
    return response;
}