#pragma once

#include "CoordinateTypes.h"
#include "SG_baseDataType.h"

#include <array>

struct HandEyeExtrinsic;
struct RobotPose6D;

// 砂轮盘孔 / 砂轮盘架子姿态计算工具集。
//
// 解决两类问题：
//   1) 当主轴接近世界 Z 时，原 Y = Z_world x 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 matrix16[16]);

// 按 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 x 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);

// ============ 完整流水线（DetectDiscHole / DetectDiscRack 共用）============

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

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

// ============ 砂轮盘孔定位专用工具 ============

// 从 SSG_pointPose 的三轴向量 (pose_x/y/z) 构造旋转矩阵，经手眼变换流水线提取欧拉角。
// 用于砂轮盘架子检测（算法直接给出完整姿态三元组）。
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);

// 从单一法向量 normDir 构造完整姿态（Z 轴 = normDir，X/Y 自动补全），
// 经手眼变换流水线提取欧拉角。
// 用于砂轮盘孔检测（算法只给出孔中心和法向量）。
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);

} // namespace PoseAxesBuilder