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

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

static void AxialDirToEuler(double axialDirX, double axialDirY, double axialDirZ,
                             double rotateAngle,
                             double& roll, double& pitch, double& yaw)
{
    roll = rotateAngle;

    const double xyLen = std::sqrt(axialDirX * axialDirX + axialDirY * axialDirY);
    pitch = std::atan2(-axialDirZ, xyLen) * 180.0 / M_PI;
    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(
    const std::vector<ScrewPosition>& positions,
    int cameraIndex,
    bool success,
    int errorCode,
    const QString& message)
{
    ProtocolDetectionOutput output;
    output.type = DETECTION_TYPE_TOOL_DISK;
    output.success = success;
    output.errorCode = errorCode;
    output.message = message;
    output.cameraIndex = cameraIndex;

    for (const auto& pos : positions) {
        ToolDiskDetectOutput toolOutput;
        toolOutput.x = pos.x;
        toolOutput.y = pos.y;
        toolOutput.z = pos.z;
        toolOutput.roll = pos.roll;
        toolOutput.pitch = pos.pitch;
        toolOutput.yaw = pos.yaw;
        output.toolDiskOutputs.push_back(toolOutput);
    }

    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);
}