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

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

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

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

            // 验证：搜索结果的截面面积 <= 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);

            // 验证：搜索结果面积 <= 不搜索(只用 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

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

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