using Microsoft.VisualStudio.TestTools.UnitTesting;
using NavisworksTransport.Utils.CoordinateSystem;
using System;
using System.Numerics;

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class AlignToGroundMinCrossSectionYawSearcherTests
    {
        // ----- 验证辅助 -----

        private static double Deg(double deg) => deg * Math.PI / 180.0;

        private static double RadToDeg(double rad) => rad * 180.0 / Math.PI;

        /// <summary>
        /// 计算在 q 姿态下物体沿 hostSide/hostUp 的截面投影面积。
        /// 与 Searcher 内部 EvaluateCrossSectionArea 公式一致，用于独立验证搜索结果。
        /// </summary>
        private static double ComputeCrossSectionArea(
            Quaternion q,
            double sizeX, double sizeY, double sizeZ,
            Vector3 hostSide, Vector3 hostUp)
        {
            Vector3 localX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, q));
            Vector3 localY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, q));
            Vector3 localZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, q));
            double width =
                Math.Abs(Vector3.Dot(localX, hostSide)) * sizeX +
                Math.Abs(Vector3.Dot(localY, hostSide)) * sizeY +
                Math.Abs(Vector3.Dot(localZ, hostSide)) * sizeZ;
            double height =
                Math.Abs(Vector3.Dot(localX, hostUp)) * sizeX +
                Math.Abs(Vector3.Dot(localY, hostUp)) * sizeY +
                Math.Abs(Vector3.Dot(localZ, hostUp)) * sizeZ;
            return width * height;
        }

        /// <summary>
        /// 从结果四元数反算绕 hostUp 的 deltaYaw（度）。
        /// deltaQ = inverse(faceDown) * result；其旋转轴应近似 hostUp。
        /// </summary>
        private static double ExtractDeltaYawDegrees(Quaternion faceDown, Quaternion result, Vector3 hostUp)
        {
            Quaternion deltaQ = Quaternion.Normalize(Quaternion.Inverse(faceDown) * result);
            // 提取旋转角度（取绝对值，deltaQ 可能是 q 或 -q，表示同一旋转）
            double wClamped = Math.Min(1.0, Math.Abs(deltaQ.W));
            double angleRad = 2.0 * Math.Acos(wClamped);
            // 轴分量与 hostUp 同向为正，反向为负
            Vector3 axis = new Vector3(deltaQ.X, deltaQ.Y, deltaQ.Z);
            double sign = Vector3.Dot(axis, hostUp) >= 0 ? 1.0 : -1.0;
            return sign * RadToDeg(angleRad);
        }

        // ----- 回归保护：平地 + 路径沿 local X，长方体长边已对齐路径 -----

        [TestMethod]
        public void FlatFace_PathAlongX_LongSideAlreadyAligned_KeepsZeroYaw_ZUp()
        {
            // sizeX=8(长) sizeY=4 sizeZ=2，路径沿 +X，ZUp
            // 贴地=Identity（面已朝下），CAD=Identity
            // yaw=0 时：width(沿Y)=4, height(沿Z)=2, 面积=8（最小）
            // yaw=90° 时：width=8, height=2, 面积=16（更大）
            // 期望搜索结果 ≈ 0°
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.Identity;
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitX, adapter: adapter,
                out _, out _, out _);

            double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
            Assert.AreEqual(0.0, deltaYaw, 1.5,
                $"长边已对齐路径时 yaw 应保持 0°，实际={deltaYaw:F2}°");
        }

        // ----- 关键：平地 + 路径沿 Y，长方体需转 90° 才让长边对齐路径 -----

        [TestMethod]
        public void FlatFace_PathAlongY_SearchFinds90DegToAlignLongSide_ZUp()
        {
            // sizeX=8(长) sizeY=4 sizeZ=2，路径沿 +Y，ZUp
            // yaw=0：local X=+X，width(沿hostSide)=|cross(+Y,+Z)·X|*8 + ... = |+X·+X|*8 + 0 + 0 = 8（宽方向是X），height(沿Z)=2，面积=16
            //   注：pathFwd=+Y, hostUp=+Z, hostSide=cross(+Y,+Z)=+X
            //   yaw=0: localX=+X,localY=+Y,localZ=+Z；width=|X·X|*8+|Y·X|*4+|Z·X|*2=8；height=|X·Z|*8+|Y·Z|*4+|Z·Z|*2=2；面积=16
            // yaw=90°(绕Z)：localX=+Y,localY=-X,localZ=+Z；width=|Y·X|*8+|-X·X|*4+|Z·X|*2=4；height=|Y·Z|*8+|-X·Z|*4+|Z·Z|*2=2；面积=8（最小）
            // 期望搜索结果 ≈ ±90°（两个解面积相同，取其一）
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.Identity;
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitY, adapter: adapter,
                out _, out _, out _);

            double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
            // 接受 +90 或 -90（面积相同），用 mod 180 容差判断
            double mod180 = Math.Abs(deltaYaw) % 180.0;
            Assert.AreEqual(90.0, mod180, 2.0,
                $"路径沿 Y、长边沿 X 时应搜索到 ±90° yaw 使长边对齐路径，实际={deltaYaw:F2}°");

            // 验证结果面积确实最小（=8），优于 yaw=0 的 16
            Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitY, Vector3.UnitZ));
            double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            double zeroYawArea = ComputeCrossSectionArea(faceDown, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            Assert.IsTrue(resultArea < zeroYawArea - 1e-3,
                $"搜索结果面积 {resultArea:F3} 应小于 yaw=0 面积 {zeroYawArea:F3}");
            Assert.AreEqual(8.0, resultArea, 0.1,
                $"最小截面面积应 ≈ 8，实际={resultArea:F3}");
        }

        // ----- YUp 宿主下同样行为 -----

        [TestMethod]
        public void FlatFace_PathAlongY_SearchFinds90Deg_YUp()
        {
            // YUp：hostUp=+Y。sizeX=8(长) sizeY=4 sizeZ=2，路径沿 +Y(=hostUp)？
            // 不对——路径沿 hostUp 是纯垂直，会退化。改路径沿 +Z（YUp 下水平方向之一）
            // YUp 下水平面是 XZ，pathFwd=+Z, hostUp=+Y, hostSide=cross(+Z,+Y)=-X
            // yaw=0: localX=+X,localY=+Y,localZ=+Z
            //   width(沿-X)=|X·-X|*8+|Y·-X|*4+|Z·-X|*2=8；height(沿+Y)=|X·Y|*8+|Y·Y|*4+|Z·Y|*2=4；面积=32
            // yaw=90°(绕+Y): localX=+Z,localY=+Y,localZ=-X
            //   width(沿-X)=|Z·-X|*8+|Y·-X|*4+|-X·-X|*2=2；height(沿+Y)=0+|Y·Y|*4+0=4；面积=8（最小）
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion faceDown = Quaternion.Identity;
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitZ, adapter: adapter,
                out _, out _, out _);

            double deltaYaw = ExtractDeltaYawDegrees(faceDown, q, adapter.HostUpVector3);
            double mod180 = Math.Abs(deltaYaw) % 180.0;
            Assert.AreEqual(90.0, mod180, 2.0,
                $"YUp 下路径沿 Z、长边沿 X 时应搜索到 ±90° yaw，实际={deltaYaw:F2}°");
        }

        // ----- 贴地旋转已固定俯仰，验证搜索仍能找到使截面最小的 yaw -----

        [TestMethod]
        public void TiltedFaceDown_SearchStillFindsMinCrossSection_ZUp()
        {
            // 构造贴地旋转：绕 X 轴翻 30°（模拟贴地后俯仰已固定）
            // 物体 8×4×2，路径沿 +X，ZUp
            // 搜索应在绕 Z 的 yaw 上找到最小截面
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(30.0));
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitX, adapter: adapter,
                out _, out _, out _);

            // 验证：搜索结果的截面面积 <= yaw=0(只用 faceDown) 的面积
            Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitX, Vector3.UnitZ));
            double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            double faceDownOnlyArea = ComputeCrossSectionArea(faceDown, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            Assert.IsTrue(resultArea <= faceDownOnlyArea + 1e-3,
                $"搜索结果面积 {resultArea:F3} 应不大于仅贴地(yaw=0)面积 {faceDownOnlyArea:F3}");
        }

        // ----- CAD 姿态天然歪斜：验证搜索在歪斜 CAD 基础上仍找到最小截面 -----

        [TestMethod]
        public void SkewedCadRotation_SearchFindsMinCrossSection_ZUp()
        {
            // CAD 姿态自带 45° 绕 Z 旋转（天然歪斜），物体 8×4×2，路径沿 +X
            // 搜索应找到补偿 yaw 使截面最小
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.Identity;
            Quaternion cadRot = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)Deg(45.0));

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitX, adapter: adapter,
                out _, out _, out _);

            // 验证：搜索结果面积 <= 不搜索(只用 faceDown) 的面积
            Vector3 hostSide = Vector3.Normalize(Vector3.Cross(Vector3.UnitX, Vector3.UnitZ));
            double resultArea = ComputeCrossSectionArea(q, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            double noYawArea = ComputeCrossSectionArea(faceDown * cadRot, 8.0, 4.0, 2.0, hostSide, Vector3.UnitZ);
            Assert.IsTrue(resultArea <= noYawArea + 1e-3,
                $"CAD 歪斜时搜索结果面积 {resultArea:F3} 应不大于不搜索面积 {noYawArea:F3}");
        }

        // ----- 退化：纯垂直路径（pathFwd 平行 hostUp），保持贴地姿态 -----

        [TestMethod]
        public void VerticalPath_ReturnsFaceDownUnchanged_ZUp()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(20.0));
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: Vector3.UnitZ, adapter: adapter,  // ZUp 下 +Z = hostUp
                out _, out _, out _);

            Assert.AreEqual(faceDown.X, q.X, 1e-6);
            Assert.AreEqual(faceDown.Y, q.Y, 1e-6);
            Assert.AreEqual(faceDown.Z, q.Z, 1e-6);
            Assert.AreEqual(faceDown.W, q.W, 1e-6);
        }

        // ----- 立方体（三轴等长）：yaw 不影响面积，应返回有效结果不崩溃 -----

        [TestMethod]
        public void Cube_AllAxesEqual_ReturnsValidResult_NoCrash()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion faceDown = Quaternion.Identity;
            Quaternion cadRot = Quaternion.Identity;

            Quaternion q = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadRot, sizeX: 4.0, sizeY: 4.0, sizeZ: 4.0,
                hostPathForward: Vector3.UnitX, adapter: adapter,
                out _, out _, out _);

            // 立方体任何 yaw 面积相同，只要返回有效四元数即可
            Assert.AreEqual(1.0, Quaternion.Normalize(q).Length(), 1e-5);
        }

        // ----- ComposeHostCorrection：扣除 CAD yaw + pathYaw，避免重复旋转 -----

        /// <summary>
        /// PathAnimationManager 增量链语义闭环验证：
        ///   finalQ = qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) * cadQ
        ///   应等于 overrideQ * cadQ（贴地时实际施加的最终姿态）
        /// cadQ=Identity 时 currentYaw=0，简化为：
        ///   qup(pathYaw) * correctionHostQ == overrideQ
        /// </summary>
        private static void AssertIncrementalChainClosure(
            Quaternion overrideQ,
            Quaternion cadQ,
            double pathYawRadians,
            HostCoordinateAdapter adapter,
            double tolerance = 1e-3)
        {
            LocalEulerRotationCorrection correction =
                AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
                    overrideQ, cadQ, pathYawRadians, adapter);

            Quaternion correctionHostQ = adapter.CreateHostRotationCorrection(correction);

            // currentYaw 从 cadQ 提取（与 ComposeHostCorrection 内部一致）
            Matrix4x4 cadLinear = Matrix4x4.CreateFromQuaternion(cadQ);
            Vector3 cadFwdHost = new Vector3(cadLinear.M11, cadLinear.M21, cadLinear.M31);
            Vector3 cadFwdCanon = adapter.ToCanonicalVector3(cadFwdHost);
            cadFwdCanon.Z = 0f;
            double currentYaw = cadFwdCanon.LengthSquared() > 1e-9f
                ? Math.Atan2(Vector3.Normalize(cadFwdCanon).Y, Vector3.Normalize(cadFwdCanon).X)
                : 0.0;

            // 增量链：finalQ = qup(pathYaw - currentYaw) * correctionHostQ * cadQ
            Quaternion deltaQ = Quaternion.CreateFromAxisAngle(
                adapter.HostUpVector3, (float)(pathYawRadians - currentYaw));
            Quaternion reconstructedFinal = Quaternion.Normalize(deltaQ * correctionHostQ * cadQ);
            Quaternion expectedFinal = Quaternion.Normalize(overrideQ * cadQ);

            for (int axis = 0; axis < 3; axis++)
            {
                Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
                Vector3 expected = Vector3.Normalize(Vector3.Transform(v, expectedFinal));
                Vector3 actual = Vector3.Normalize(Vector3.Transform(v, reconstructedFinal));
                double dot = Vector3.Dot(expected, actual);
                Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
                    $"axis {axis}: 增量链闭环偏差，dot={dot:F6}");
            }
        }

        [TestMethod]
        public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_ZUp()
        {
            // CAD=Identity(currentYaw=0), overrideQ=纯 pathYaw, correction 应为 0
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            double pathYaw = Deg(30.0);
            Quaternion cadQ = Quaternion.Identity;
            Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);

            LocalEulerRotationCorrection correction =
                AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
                    overrideQ, cadQ, pathYaw, adapter);

            Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
            Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
            Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
        }

        [TestMethod]
        public void ComposeCorrection_CadIdentity_PurePathYaw_ReturnsZeroCorrection_YUp()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            double pathYaw = Deg(-15.62);
            Quaternion cadQ = Quaternion.Identity;
            Quaternion overrideQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);

            LocalEulerRotationCorrection correction =
                AlignToGroundMinCrossSectionYawSearcher.ComposeHostCorrection(
                    overrideQ, cadQ, pathYaw, adapter);

            Assert.AreEqual(0.0, correction.XDegrees, 1e-3, $"X={correction.XDegrees:F4}");
            Assert.AreEqual(0.0, correction.YDegrees, 1e-3, $"Y(up)={correction.YDegrees:F4}");
            Assert.AreEqual(0.0, correction.ZDegrees, 1e-3, $"Z(nonUp)={correction.ZDegrees:F4}");
        }

        [TestMethod]
        public void ComposeCorrection_CadIdentity_FaceDownPlusPathYaw_Closure_YUp()
        {
            // CAD=Identity, overrideQ = pathYawQ * faceDown
            // 增量链闭环验证
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            double pathYaw = Deg(-15.62);
            Quaternion cadQ = Quaternion.Identity;
            Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(15.0));
            Quaternion pathYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)pathYaw);
            Quaternion overrideQ = Quaternion.Normalize(pathYawQ * faceDown);

            AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter);
        }

        [TestMethod]
        public void ComposeCorrection_CadIdentity_SearchedYawPlusFaceDown_Closure_YUp()
        {
            // 模拟真实贴地：CAD=Identity, Search 出 overrideQ, 验证增量链闭环
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion cadQ = Quaternion.Identity;
            Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(25.0));
            Vector3 pathFwd = new Vector3(0.96f, 0.0f, -0.28f);

            Quaternion overrideQ = AlignToGroundMinCrossSectionYawSearcher.Search(
                faceDown, cadQ, sizeX: 8.0, sizeY: 4.0, sizeZ: 2.0,
                hostPathForward: pathFwd, adapter: adapter,
                out _, out _, out _);

            PathTargetFrameResolver.TryResolvePlanarHostYaw(pathFwd, CoordinateSystemType.YUp, out double pathYaw);

            AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
        }

        [TestMethod]
        public void ComposeCorrection_SkewedCad_IncrementalChainClosure_YUp()
        {
            // CAD 姿态天然歪斜（带 yaw），验证增量链仍闭环
            // 这是日志中 25"x25" 的场景：cadQ 自带 -124° yaw
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            // 构造 CAD 姿态：绕 host Y(up) 转 -124° + 一些倾斜
            Quaternion cadQ = Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Deg(-124.0)) *
                Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(10.0)));
            double pathYaw = Deg(-15.62);
            // overrideQ = searchedYawQ * faceDown（贴地搜索结果）
            Quaternion faceDown = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)Deg(20.0));
            Quaternion searchedYawQ = Quaternion.CreateFromAxisAngle(adapter.HostUpVector3, (float)Deg(-57.0));
            Quaternion overrideQ = Quaternion.Normalize(searchedYawQ * faceDown);

            AssertIncrementalChainClosure(overrideQ, cadQ, pathYaw, adapter, tolerance: 2e-3);
        }

        [TestMethod]
        public void HostQuaternionToCanonical_HostUpRotation_MapsToCanonicalZRotation_YUp()
        {
            // YUp: host 绕 Y(up) 转 θ → canonical 绕 Z(up) 转 θ
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            double theta = Deg(37.0);
            Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)theta);

            Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);

            // canonical 下应是绕 Z 转 θ
            Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)theta);
            AssertQuaternionsEqual(expected, canonQ);
        }

        [TestMethod]
        public void HostQuaternionToCanonical_HostXRotation_MapsToCanonicalXRotation_YUp()
        {
            // YUp: host 绕 X(forward) 转 θ → canonical 绕 X(forward) 转 θ（X 轴两坐标系相同）
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            double theta = Deg(42.0);
            Quaternion hostQ = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);

            Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(hostQ, adapter);

            Quaternion expected = Quaternion.CreateFromAxisAngle(Vector3.UnitX, (float)theta);
            AssertQuaternionsEqual(expected, canonQ);
        }

        [TestMethod]
        public void HostQuaternionToCanonical_Identity_StaysIdentity_YUp()
        {
            // Identity 在两坐标系都应是 Identity（相似变换的不动点）
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(
                Quaternion.Identity, adapter);
            AssertQuaternionsEqual(Quaternion.Identity, canonQ);
        }

        [TestMethod]
        public void HostQuaternionToCanonical_ZUp_IsIdentity()
        {
            // ZUp: canonical=host，转换应返回原四元数
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            Quaternion original = Quaternion.CreateFromAxisAngle(
                new Vector3(0.3f, 0.5f, 0.8f), (float)Deg(55.0f));
            original = Quaternion.Normalize(original);

            Quaternion canonQ = AlignToGroundMinCrossSectionYawSearcher.HostQuaternionToCanonical(original, adapter);
            AssertQuaternionsEqual(original, canonQ);
        }

        private static void AssertQuaternionsEqual(Quaternion expected, Quaternion actual, double tolerance = 1e-4)
        {
            // 四元数有双覆盖性（q 和 -q 表示同一旋转），比较作用后的轴
            for (int axis = 0; axis < 3; axis++)
            {
                Vector3 v = axis == 0 ? Vector3.UnitX : (axis == 1 ? Vector3.UnitY : Vector3.UnitZ);
                Vector3 e = Vector3.Normalize(Vector3.Transform(v, expected));
                Vector3 a = Vector3.Normalize(Vector3.Transform(v, actual));
                double dot = Vector3.Dot(e, a);
                Assert.IsTrue(Math.Abs(dot - 1.0) < tolerance,
                    $"axis {axis}: expected≈actual 失败，dot={dot:F6}");
            }
        }
    }
}