#pragma once

#include "CoordinateTypes.h"
#include <array>

struct HandEyeExtrinsic;
struct RobotPose6D;

// 螺杆 / 工具盘姿态计算工具集。
//
// 解决两类问题：
//   1) 当主轴接近世界 Z 时，原 Y = Z_world × X 构造的副轴对 X 的微小抖动敏感（叉乘量级
//      退化到 sin(夹角)），会被放大成 ~10° 的方向摆动。BuildAnchoredFrame 用「参考方向
//      投影到 X 垂直平面」的 Gram-Schmidt 方式锚定 Y 轴，对主轴抖动鲁棒。
//   2) 「Eye 三轴 → Eye 补偿 → Eye→Robot 变换 → Robot 补偿 → 提欧拉 → 万向锁消歧」
//      的流水线在螺杆和工具盘场景重复出现，封装为 ComputeRobotPoseAngles 单次调用。
namespace PoseAxesBuilder
{

// ============ 基础数值工具 ============

CTVec3D NormalizeVector(const CTVec3D& v);
bool IsValidVector(const CTVec3D& v);

// ============ 手眼标定相关 ============

// 从 16 元素行优先数组构建手眼标定矩阵。
// 约定：clibMatrix 表达 T_flange_camera（即 Eye 系下的点经其变换得到 Flange 系下的点），
// 与 CCoordinateTransform::sixAxisEyeInHandBuildTransform 入参一致。
CTHomogeneousMatrix BuildHandEyeMatrix(const double clibMatrix[16]);

// 把法兰系下的局部方向（例如 (0,1,0) = 法兰 Y 轴）转换为 Eye 系下的方向向量。
// 用于得到稳定的 Y 锚点：法兰姿态固定时其轴在 Eye 系下的方向也稳定。
// 内部走 handEyeMatrix.rotation^T，仅旋转、不平移。
CTVec3D FlangeAxisToEye(const CTHomogeneousMatrix& handEyeMatrix,
                        const CTVec3D& flangeLocalAxis);

// 按 poseOutputOrder 把 RobotPose6D.a/b/c 重排成真实绕轴角度 rx/ry/rz（度）。
void ResolveRobotPoseAnglesDegrees(const RobotPose6D& robotPose,
                                   int poseOutputOrder,
                                   double& rxDeg,
                                   double& ryDeg,
                                   double& rzDeg);

// ============ 锚点式正交三元组构建 ============

// 给定主轴 primary 和参考方向 referenceY，构建正交三元组：
//     X = primary（归一化）
//     Y = (referenceY - (referenceY·X) X) 归一化       （Gram-Schmidt 投影）
//     Z = X × Y（归一化）
//
// 典型用法：
//   - 螺杆：primary = 螺杆轴向, referenceY = FlangeAxisToEye(handEye, (0,1,0))
//   - 工具盘：primary = 算法 X 轴, referenceY = 算法 Y 轴（已正交时退化为恒等）
//
// 退化判定：投影后 |Y_proj| < minPerpendicularity（即 primary 与 referenceY 近共线），
// 此时副轴无法稳定确定，返回 false；调用方应换用另一参考方向或跳过该目标。
bool BuildAnchoredFrame(const CTVec3D& primary,
                        const CTVec3D& referenceY,
                        std::array<CTVec3D, 3>& axes,
                        double minPerpendicularity = 0.1);

// ============ 三元组运算 ============

// 在三轴自身坐标系内施加 R = Rx(rotX)·Ry(rotY)·Rz(rotZ)（角度）。
// 物理含义：把"算法工具系"二次旋转到"机器人期望 TCP 系"或远离万向锁的位姿。
void ApplyAxesRotation(std::array<CTVec3D, 3>& axes,
                       double rotXDeg, double rotYDeg, double rotZDeg);

// 把 srcAxes 经齐次变换的旋转部分搬到 dstAxes（不平移），并归一化。
// 任一轴归一化失败返回 false。
bool TransformAxes(const std::array<CTVec3D, 3>& srcAxes,
                   const CTHomogeneousMatrix& transform,
                   std::array<CTVec3D, 3>& dstAxes);

// 把三轴当列向量组装成 3x3 旋转矩阵。
CTRotationMatrix BuildRotationMatrix(const std::array<CTVec3D, 3>& axes);

// ============ 欧拉提取与消歧 ============

// 旋转矩阵 → 欧拉角（按 order），自动把 pitch 归一化到 [-90°, 90°]。
void RotationMatrixToConfiguredEulerDegrees(const CTRotationMatrix& rotation,
                                            CTEulerOrder order,
                                            double& rollDeg,
                                            double& pitchDeg,
                                            double& yawDeg);

// 万向锁附近（|pitch| 接近 90°）把 yaw/roll 锚定到 (refYaw/refRoll) 周围，让多帧输出
// 稳定落在机器人当前法兰姿态附近。|pitch| < 80° 时不做任何修改。
void ResolveGimbalAmbiguity(double& rollDeg,
                            double pitchDeg,
                            double& yawDeg,
                            double refRollDeg,
                            double refYawDeg);

// ============ 完整流水线（DetectScrew / DetectToolDisk 共用）============

struct PoseAngles
{
    double rollDeg = 0.0;
    double pitchDeg = 0.0;
    double yawDeg = 0.0;
};

// 中间状态调试信息；不需要可传 nullptr。
struct PoseDebugInfo
{
    PoseAngles eyeEulerBefore;              // ApplyAxesRotation(rotX/Y/Z) 之前的 Eye 系欧拉角
    PoseAngles eyeEulerAfter;               // 同上之后的 Eye 系欧拉角
    PoseAngles robotEulerBeforeOutRot;      // Eye→Robot 变换后、ApplyAxesRotation(outRotX/Y/Z) 之前的 Robot 系欧拉角
                                            // —— 标定 outRot 时把它粘进 CloudView「姿态补偿」的"当前"栏
    CTRotationMatrix eyeRotationAfter;      // Eye 补偿后的旋转矩阵
    CTRotationMatrix robotRotation;         // 最终 Robot 系旋转矩阵
};

// Eye 系三轴 → Robot 系欧拉角完整流水线：
//   1) Eye 系内补偿：ApplyAxesRotation(extrinsic.rotX/Y/Z)
//   2) Eye → Robot 变换：TransformAxes(eyeInHandTransform)
//   3) Robot 系内补偿：ApplyAxesRotation(extrinsic.outRotX/Y/Z)
//   4) 提欧拉角 + 万向锁消歧（refRollDeg / refYawDeg 通常取机器人当前法兰姿态）
//
// 返回 false：变换链中任一步退化（向量长度为零等）。
bool ComputeRobotPoseAngles(const std::array<CTVec3D, 3>& eyeAxes,
                            const CTHomogeneousMatrix& eyeInHandTransform,
                            const HandEyeExtrinsic& extrinsic,
                            CTEulerOrder eulerOrder,
                            double refRollDeg,
                            double refYawDeg,
                            PoseAngles& outAngles,
                            PoseDebugInfo* outDebugInfo = nullptr);

} // namespace PoseAxesBuilder