#include "PoseAxesBuilder.h"

#include "CoordinateTransform.h"
#include "DetectPresenter.h"           // for HandEyeExtrinsic
#include "JiuruiWorkpiecePoseTCPProtocol.h"   // for RobotPose6D

#include <cmath>

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

namespace {

constexpr double kVectorEpsilon = 1e-8;
constexpr double kRotationEpsilon = 1e-9;

double DotProduct(const CTVec3D& a, const CTVec3D& b)
{
    return a.x * b.x + a.y * b.y + a.z * b.z;
}

CTVec3D CrossProduct(const CTVec3D& a, const CTVec3D& b)
{
    return CTVec3D(a.y * b.z - a.z * b.y,
                   a.z * b.x - a.x * b.z,
                   a.x * b.y - a.y * b.x);
}

CTVec3D ToCTVec3D(const SVzNL3DPoint& point)
{
    return CTVec3D(point.x, point.y, point.z);
}

double WrapDegreesTo180(double deg)
{
    while (deg > 180.0) {
        deg -= 360.0;
    }
    while (deg <= -180.0) {
        deg += 360.0;
    }
    return deg;
}

// 对 Tait-Bryan 欧拉角做等价归一化，使 pitch 属于 [-90 度, 90 度]。
// 依据：(alpha, beta, gamma) 等价于 (alpha +/- 180 度, 180 度 - beta, gamma +/- 180 度) 以及 (alpha +/- 180 度, -180 度 - beta, gamma +/- 180 度)，
// 适用于 CTEulerOrder 中所有 12 种 Tait-Bryan 顺序（内旋/外旋一致）。
void NormalizePitchRange(double& rollDeg, double& pitchDeg, double& yawDeg)
{
    pitchDeg = WrapDegreesTo180(pitchDeg);
    if (pitchDeg > 90.0) {
        pitchDeg = 180.0 - pitchDeg;
        rollDeg += 180.0;
        yawDeg += 180.0;
    } else if (pitchDeg < -90.0) {
        pitchDeg = -180.0 - pitchDeg;
        rollDeg += 180.0;
        yawDeg += 180.0;
    }
    rollDeg = WrapDegreesTo180(rollDeg);
    yawDeg = WrapDegreesTo180(yawDeg);
}

// 万向锁消歧：当 |pitch| 接近 90 度 时，Rz(yaw).Ry(pitch).Rx(roll) 的分解退化成一个自由度。
// 严格展开可得：
//   pitch = +90 度  ->  R 只依赖 (roll - yaw)；任意满足 roll - yaw = const 的组合都表示同一 R
//   pitch = -90 度  ->  R 只依赖 (roll + yaw)；任意满足 roll + yaw = const 的组合都表示同一 R
// 本函数把 yaw 锚定到 refYaw 附近，把剩余自由度放到 roll，让不同帧的输出稳定落在
// 参考（通常是机器人法兰的 rx/rz）附近。阈值覆盖 80 度 - 85 度：
//   |pitch| < 80 度：完全不改（保留真实分解）
//   |pitch| >= 85 度：完全锁到参考（对 R 影响最小）
//   80 度 <= |pitch| < 85 度：线性插值过渡
void ResolveGimbalAmbiguity(double& rollDeg, double pitchDeg, double& yawDeg,
                            double refRollDeg, double refYawDeg)
{
    constexpr double kSoftThresholdDeg = 80.0;
    constexpr double kHardThresholdDeg = 85.0;
    const double absPitch = std::abs(pitchDeg);
    if (absPitch < kSoftThresholdDeg) {
        return;
    }

    // 不变量方向与 pitch 符号的关系（由 Rz.Ry.Rx 展开式导出）：
    //   pitch > 0 -> inv = roll - yaw
    //   pitch < 0 -> inv = roll + yaw
    const bool positivePitch = (pitchDeg >= 0.0);
    const double sum    = positivePitch ? (rollDeg - yawDeg)       : (rollDeg + yawDeg);
    const double refSum = positivePitch ? (refRollDeg - refYawDeg) : (refRollDeg + refYawDeg);

    double adjustedSum = sum;
    while (adjustedSum - refSum >  180.0) { adjustedSum -= 360.0; }
    while (adjustedSum - refSum < -180.0) { adjustedSum += 360.0; }

    const double fullYaw  = WrapDegreesTo180(refYawDeg);
    const double fullRoll = WrapDegreesTo180(positivePitch ? (adjustedSum + fullYaw)
                                                            : (adjustedSum - fullYaw));

    if (absPitch >= kHardThresholdDeg) {
        rollDeg = fullRoll;
        yawDeg  = fullYaw;
        return;
    }

    const double alpha = (absPitch - kSoftThresholdDeg) /
                         (kHardThresholdDeg - kSoftThresholdDeg);
    auto lerpAngle = [alpha](double from, double to) {
        const double diff = WrapDegreesTo180(to - from);
        return WrapDegreesTo180(from + alpha * diff);
    };
    rollDeg = lerpAngle(rollDeg, fullRoll);
    yawDeg  = lerpAngle(yawDeg,  fullYaw);
}

// 从单一法向量 Z 轴构造完整正交三元组（X/Y 自动补全）。
// 用于 NormalDirToPoseAngles：算法只给出孔的法向量，需要补全姿态。
bool BuildFrameFromZAxis(const CTVec3D& zAxisInput, std::array<CTVec3D, 3>& axes)
{
    const CTVec3D zAxis = PoseAxesBuilder::NormalizeVector(zAxisInput);
    if (!PoseAxesBuilder::IsValidVector(zAxis)) {
        return false;
    }

    const CTVec3D worldY(0.0, 1.0, 0.0);
    const CTVec3D worldX(1.0, 0.0, 0.0);

    // 尝试 Z x worldY 得到 X 轴
    CTVec3D xAxis = PoseAxesBuilder::NormalizeVector(CrossProduct(zAxis, worldY));
    if (!PoseAxesBuilder::IsValidVector(xAxis)) {
        // Z 轴与 worldY 近共线，改用 worldX
        xAxis = PoseAxesBuilder::NormalizeVector(CrossProduct(zAxis, worldX));
    }
    if (!PoseAxesBuilder::IsValidVector(xAxis)) {
        return false;
    }

    const CTVec3D yAxis = PoseAxesBuilder::NormalizeVector(CrossProduct(zAxis, xAxis));
    if (!PoseAxesBuilder::IsValidVector(yAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

// 公共手眼变换构建：从 clibMatrix + robotPose + extrinsic 产出一个 eyeInHandTransform，
// 同时返回 order / rxDeg / rzDeg 供后续欧拉提取和万向锁消歧使用。
void BuildEyeInHandTransform(const double matrix16[16],
                             const RobotPose6D& robotPose,
                             const HandEyeExtrinsic& extrinsic,
                             int poseOutputOrder,
                             CTHomogeneousMatrix& eyeInHandTransform,
                             CTEulerOrder& order,
                             double& rxDeg,
                             double& rzDeg)
{
    const CTHomogeneousMatrix handEyeMatrix = PoseAxesBuilder::BuildHandEyeMatrix(matrix16);
    order = static_cast<CTEulerOrder>(extrinsic.eulerOrder);

    double ryDeg = 0.0;
    PoseAxesBuilder::ResolveRobotPoseAnglesDegrees(robotPose, poseOutputOrder, rxDeg, ryDeg, rzDeg);
    const CTRobotPose flangePose =
        CTRobotPose::fromDegrees(robotPose.x, robotPose.y, robotPose.z, rxDeg, ryDeg, rzDeg);
    eyeInHandTransform =
        CCoordinateTransform::sixAxisEyeInHandBuildTransform(flangePose, order, handEyeMatrix);
}

// 填充输出位姿：将 Eye 系点经手眼变换后加上 offset，封装欧拉角。
void FillOutputPose(const CTVec3D& eyePoint,
                    const CTHomogeneousMatrix& eyeInHandTransform,
                    const HandEyeExtrinsic& extrinsic,
                    const PoseAxesBuilder::PoseAngles& angles,
                    double& outX,
                    double& outY,
                    double& outZ,
                    double& outRoll,
                    double& outPitch,
                    double& outYaw)
{
    const CTVec3D robotPoint = eyeInHandTransform.transformPoint(eyePoint);

    outX = robotPoint.x + extrinsic.offsetX;
    outY = robotPoint.y + extrinsic.offsetY;
    outZ = robotPoint.z + extrinsic.offsetZ;

    outRoll = angles.rollDeg;
    outPitch = angles.pitchDeg;
    outYaw = angles.yawDeg;
}

}  // namespace

namespace PoseAxesBuilder
{

CTVec3D NormalizeVector(const CTVec3D& v)
{
    const double length = v.norm();
    if (length < kVectorEpsilon) {
        return CTVec3D();
    }
    return v * (1.0 / length);
}

bool IsValidVector(const CTVec3D& v)
{
    return v.norm() >= kVectorEpsilon;
}

CTHomogeneousMatrix BuildHandEyeMatrix(const double matrix16[16])
{
    CTHomogeneousMatrix handEyeMatrix;
    for (int row = 0; row < 4; ++row) {
        for (int col = 0; col < 4; ++col) {
            handEyeMatrix.at(row, col) = matrix16[row * 4 + col];
        }
    }
    return handEyeMatrix;
}

void ResolveRobotPoseAnglesDegrees(const RobotPose6D& robotPose,
                                   int poseOutputOrder,
                                   double& rxDeg,
                                   double& ryDeg,
                                   double& rzDeg)
{
    switch (poseOutputOrder) {
    case 1:
        rxDeg = robotPose.a;
        ryDeg = robotPose.b;
        rzDeg = robotPose.c;
        break;
    case 2:
        rxDeg = robotPose.b;
        ryDeg = robotPose.a;
        rzDeg = robotPose.c;
        break;
    case 3:
        rxDeg = robotPose.c;
        ryDeg = robotPose.a;
        rzDeg = robotPose.b;
        break;
    case 4:
        rxDeg = robotPose.b;
        ryDeg = robotPose.c;
        rzDeg = robotPose.a;
        break;
    case 5:
        rxDeg = robotPose.c;
        ryDeg = robotPose.b;
        rzDeg = robotPose.a;
        break;
    case 0:
    default:
        rxDeg = robotPose.a;
        ryDeg = robotPose.b;
        rzDeg = robotPose.c;
        break;
    }
}

bool BuildAnchoredFrame(const CTVec3D& primary,
                        const CTVec3D& referenceY,
                        std::array<CTVec3D, 3>& axes,
                        double minPerpendicularity)
{
    const CTVec3D xAxis = NormalizeVector(primary);
    if (!IsValidVector(xAxis)) {
        return false;
    }
    const CTVec3D refY = NormalizeVector(referenceY);
    if (!IsValidVector(refY)) {
        return false;
    }

    // Gram-Schmidt：refY 投影到与 X 垂直的平面。
    // |yRaw| = sin(refY 与 X 的夹角)。小于阈值则两者近共线、副轴方向无意义。
    const CTVec3D yRaw = refY - xAxis * DotProduct(refY, xAxis);
    if (yRaw.norm() < minPerpendicularity) {
        return false;
    }

    const CTVec3D yAxis = NormalizeVector(yRaw);
    const CTVec3D zAxis = NormalizeVector(CrossProduct(xAxis, yAxis));
    if (!IsValidVector(zAxis)) {
        return false;
    }

    axes = {xAxis, yAxis, zAxis};
    return true;
}

void ApplyAxesRotation(std::array<CTVec3D, 3>& axes,
                       double rotXDeg,
                       double rotYDeg,
                       double rotZDeg)
{
    if (std::fabs(rotXDeg) < kRotationEpsilon &&
        std::fabs(rotYDeg) < kRotationEpsilon &&
        std::fabs(rotZDeg) < kRotationEpsilon) {
        return;
    }

    const double rx = rotXDeg * M_PI / 180.0;
    const double ry = rotYDeg * M_PI / 180.0;
    const double rz = rotZDeg * M_PI / 180.0;

    const double cx = std::cos(rx), sx = std::sin(rx);
    const double cy = std::cos(ry), sy = std::sin(ry);
    const double cz = std::cos(rz), sz = std::sin(rz);

    // R = Rx(rx) * Ry(ry) * Rz(rz)；对工具三轴（列向量）右乘 R，等价于在工具自身坐标系
    // 内按内旋 XYZ 顺序施加补偿旋转。
    double R[3][3];
    R[0][0] = cy * cz;
    R[0][1] = -cy * sz;
    R[0][2] = sy;
    R[1][0] = sx * sy * cz + cx * sz;
    R[1][1] = -sx * sy * sz + cx * cz;
    R[1][2] = -sx * cy;
    R[2][0] = -cx * sy * cz + sx * sz;
    R[2][1] = cx * sy * sz + sx * cz;
    R[2][2] = cx * cy;

    const CTVec3D oldX = axes[0];
    const CTVec3D oldY = axes[1];
    const CTVec3D oldZ = axes[2];

    axes[0] = oldX * R[0][0] + oldY * R[1][0] + oldZ * R[2][0];
    axes[1] = oldX * R[0][1] + oldY * R[1][1] + oldZ * R[2][1];
    axes[2] = oldX * R[0][2] + oldY * R[1][2] + oldZ * R[2][2];
}

bool TransformAxes(const std::array<CTVec3D, 3>& srcAxes,
                   const CTHomogeneousMatrix& transform,
                   std::array<CTVec3D, 3>& dstAxes)
{
    for (size_t i = 0; i < srcAxes.size(); ++i) {
        dstAxes[i] = NormalizeVector(transform.transformVector(srcAxes[i]));
        if (!IsValidVector(dstAxes[i])) {
            return false;
        }
    }
    return true;
}

CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& axes)
{
    CTRotationMatrix rotation;
    rotation.at(0, 0) = axes[0].x;
    rotation.at(0, 1) = axes[1].x;
    rotation.at(0, 2) = axes[2].x;
    rotation.at(1, 0) = axes[0].y;
    rotation.at(1, 1) = axes[1].y;
    rotation.at(1, 2) = axes[2].y;
    rotation.at(2, 0) = axes[0].z;
    rotation.at(2, 1) = axes[1].z;
    rotation.at(2, 2) = axes[2].z;
    return rotation;
}

void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation,
                                            CTEulerOrder order,
                                            double& rollDeg,
                                            double& pitchDeg,
                                            double& yawDeg)
{
    const CTEulerAngles euler = CCoordinateTransform::rotationMatrixToEuler(rotation, order);
    euler.toDegrees(rollDeg, pitchDeg, yawDeg);
    NormalizePitchRange(rollDeg, pitchDeg, yawDeg);
}

bool ComputeRobotPoseAngles(const std::array<CTVec3D, 3>& eyeAxes,
                            const CTHomogeneousMatrix& eyeInHandTransform,
                            const HandEyeExtrinsic& extrinsic,
                            CTEulerOrder eulerOrder,
                            double refRollDeg,
                            double refYawDeg,
                            PoseAngles& outAngles)
{
    std::array<CTVec3D, 3> eyeAxesWorking = eyeAxes;

    // 1) Eye 系内补偿
    ApplyAxesRotation(eyeAxesWorking, extrinsic.rotX, extrinsic.rotY, extrinsic.rotZ);

    // 2) Eye -> Robot 变换
    std::array<CTVec3D, 3> robotAxes;
    if (!TransformAxes(eyeAxesWorking, eyeInHandTransform, robotAxes)) {
        return false;
    }

    // 3) Robot 系内补偿
    ApplyAxesRotation(robotAxes, extrinsic.outRotX, extrinsic.outRotY, extrinsic.outRotZ);

    // 4) 提欧拉角 + 万向锁消歧
    const CTRotationMatrix robotRotation = BuildRotationMatrix(robotAxes);
    RotationMatrixToConfiguredEulerDegrees(robotRotation, eulerOrder,
                                           outAngles.rollDeg,
                                           outAngles.pitchDeg,
                                           outAngles.yawDeg);
    ResolveGimbalAmbiguity(outAngles.rollDeg, outAngles.pitchDeg, outAngles.yawDeg,
                           refRollDeg, refYawDeg);
    return true;
}

// ============ 工件孔定位专用工具 ============

bool PointPoseToEuler(const SSG_pointPose& pointPose,
                      const double handEyeMatrix[16],
                      const HandEyeExtrinsic& extrinsic,
                      const RobotPose6D& robotPose,
                      double& outX,
                      double& outY,
                      double& outZ,
                      double& outRoll,
                      double& outPitch,
                      double& outYaw,
                      int poseOutputOrder)
{
    // 从 SSG_pointPose 的三轴向量构造 Eye 系三元组
    std::array<CTVec3D, 3> eyeAxes;
    const CTVec3D eyeXAxis = NormalizeVector(ToCTVec3D(pointPose.pose_x));
    const CTVec3D eyeYAxis = NormalizeVector(ToCTVec3D(pointPose.pose_y));
    if (!BuildAnchoredFrame(eyeXAxis, eyeYAxis, eyeAxes)) {
        return false;
    }

    // 构建手眼变换
    CTHomogeneousMatrix eyeInHandTransform;
    CTEulerOrder order = CTEulerOrder::sZYX;
    double rxDeg = 0.0;
    double rzDeg = 0.0;
    BuildEyeInHandTransform(handEyeMatrix, robotPose, extrinsic, poseOutputOrder,
                            eyeInHandTransform, order, rxDeg, rzDeg);

    // 计算欧拉角
    PoseAngles angles;
    if (!ComputeRobotPoseAngles(eyeAxes, eyeInHandTransform, extrinsic, order, rxDeg, rzDeg, angles)) {
        return false;
    }

    // 填充输出
    const CTVec3D eyePoint = ToCTVec3D(pointPose.point);
    FillOutputPose(eyePoint, eyeInHandTransform, extrinsic, angles,
                   outX, outY, outZ, outRoll, outPitch, outYaw);
    return true;
}

bool NormalDirToPoseAngles(const SVzNL3DPoint& center,
                           const SVzNL3DPoint& normDir,
                           const double handEyeMatrix[16],
                           const HandEyeExtrinsic& extrinsic,
                           const RobotPose6D& robotPose,
                           double& outX,
                           double& outY,
                           double& outZ,
                           double& outRoll,
                           double& outPitch,
                           double& outYaw,
                           int poseOutputOrder)
{
    // 从单一法向量构造 Eye 系三元组（Z = normDir，X/Y 自动补全）
    std::array<CTVec3D, 3> eyeAxes;
    if (!BuildFrameFromZAxis(ToCTVec3D(normDir), eyeAxes)) {
        return false;
    }

    // 构建手眼变换
    CTHomogeneousMatrix eyeInHandTransform;
    CTEulerOrder order = CTEulerOrder::sZYX;
    double rxDeg = 0.0;
    double rzDeg = 0.0;
    BuildEyeInHandTransform(handEyeMatrix, robotPose, extrinsic, poseOutputOrder,
                            eyeInHandTransform, order, rxDeg, rzDeg);

    // 计算欧拉角
    PoseAngles angles;
    if (!ComputeRobotPoseAngles(eyeAxes, eyeInHandTransform, extrinsic, order, rxDeg, rzDeg, angles)) {
        return false;
    }

    // 填充输出
    const CTVec3D eyePoint = ToCTVec3D(center);
    FillOutputPose(eyePoint, eyeInHandTransform, extrinsic, angles,
                   outX, outY, outZ, outRoll, outPitch, outYaw);
    return true;
}

}  // namespace PoseAxesBuilder