#include "DetectionOutputConverter.h"
#include "IYScrewPositionStatus.h"
#include <QDateTime>
#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

/**
 * @brief 将轴向方向向量转换为欧拉角（Roll, Pitch, Yaw）
 *
 * 轴向方向向量表示螺杆的朝向，转换为：
 * - Pitch: 绕Y轴旋转角（俯仰角），由Z分量和XY平面分量计算
 * - Yaw:   绕Z轴旋转角（偏航角），由X和Y分量计算
 * - Roll:  使用算法输出的旋转角度 rotateAngle
 */
static void AxialDirToEuler(double axialDirX, double axialDirY, double axialDirZ,
                             double rotateAngle,
                             double& roll, double& pitch, double& yaw)
{
    // Roll 直接使用算法的旋转角度
    roll = rotateAngle;

    // Pitch = atan2(-z, sqrt(x^2 + y^2))  俯仰角
    double xyLen = std::sqrt(axialDirX * axialDirX + axialDirY * axialDirY);
    pitch = std::atan2(-axialDirZ, xyLen) * 180.0 / M_PI;

    // Yaw = atan2(y, x)  偏航角
    yaw = std::atan2(axialDirY, axialDirX) * 180.0 / M_PI;
}

ProtocolDetectionOutput DetectionOutputConverter::ConvertScrewResult(
    const std::vector<ScrewInfo>& screwInfoList,
    int cameraIndex,
    bool success,
    int errorCode,
    const QString& message)
{
    ProtocolDetectionOutput output;
    output.type = DETECTION_TYPE_SCREW;
    output.success = success;
    output.errorCode = errorCode;
    output.message = message;
    output.cameraIndex = cameraIndex;

    for (const auto& screw : screwInfoList) {
        ScrewDetectOutput screwOutput;
        screwOutput.x = screw.centerX;
        screwOutput.y = screw.centerY;
        screwOutput.z = screw.centerZ;

        // 轴向方向转欧拉角
        AxialDirToEuler(screw.axialDirX, screw.axialDirY, screw.axialDirZ,
                        screw.rotateAngle,
                        screwOutput.roll, screwOutput.pitch, screwOutput.yaw);

        output.screwOutputs.push_back(screwOutput);
    }

    return output;
}

ProtocolDetectionOutput DetectionOutputConverter::ConvertToolDiskResult(int cameraIndex)
{
    ProtocolDetectionOutput output;
    output.type = DETECTION_TYPE_TOOL_DISK;
    output.success = false;
    output.errorCode = -100;
    output.message = "工具盘检测算法接口尚未实现";
    output.cameraIndex = cameraIndex;
    return output;
}

QJsonObject DetectionOutputConverter::ScrewOutputToJson(const ProtocolDetectionOutput& output)
{
    QJsonObject response;
    response["MessageType"] = "ScrewResult";
    response["Timestamp"] = output.timestamp;

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

    QJsonArray screwArray;
    for (const auto& screw : output.screwOutputs) {
        QJsonObject screwObj;
        screwObj["X"] = screw.x;
        screwObj["Y"] = screw.y;
        screwObj["Z"] = screw.z;
        screwObj["Roll"] = screw.roll;
        screwObj["Pitch"] = screw.pitch;
        screwObj["Yaw"] = screw.yaw;
        screwArray.append(screwObj);
    }

    data["Count"] = static_cast<int>(output.screwOutputs.size());
    data["Screws"] = screwArray;

    response["Data"] = data;
    return response;
}

QJsonObject DetectionOutputConverter::ToolDiskOutputToJson(const ProtocolDetectionOutput& output)
{
    QJsonObject response;
    response["MessageType"] = "ToolResult";
    response["Timestamp"] = output.timestamp;

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

    QJsonArray toolDiskArray;
    for (const auto& toolDisk : output.toolDiskOutputs) {
        QJsonObject obj;
        obj["X"] = toolDisk.x;
        obj["Y"] = toolDisk.y;
        obj["Z"] = toolDisk.z;
        obj["Roll"] = toolDisk.roll;
        obj["Pitch"] = toolDisk.pitch;
        obj["Yaw"] = toolDisk.yaw;
        toolDiskArray.append(obj);
    }

    data["Count"] = static_cast<int>(output.toolDiskOutputs.size());
    data["ToolDisks"] = toolDiskArray;

    response["Data"] = data;
    return response;
}

QJsonObject DetectionOutputConverter::ToJson(const ProtocolDetectionOutput& output)
{
    if (output.type == DETECTION_TYPE_TOOL_DISK) {
        return ToolDiskOutputToJson(output);
    }
    return ScrewOutputToJson(output);
}