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

namespace NavisworksTransport.UnitTests.Core
{
    [TestClass]
    public class FreePathTests
    {
        // === PathType 枚举 ===

        [TestMethod]
        public void PathType_Free_ShouldHaveValue3()
        {
            Assert.AreEqual(3, (int)PathType.Free);
        }

        [TestMethod]
        public void PathType_Free_GetDisplayName_ShouldReturnFree()
        {
            Assert.AreEqual("自由", PathType.Free.GetDisplayName());
        }

        // === 自由路径旋转姿态（方案1：forward=切线, up=世界up正交化） ===

        [TestMethod]
        public void FreePathRotation_StraightHorizontal_ShouldAlignForwardToX_UpToWorldUp()
        {
            // ZUp 约定，水平直路：forward=+X，up=+Z（世界up）
            bool ok = CanonicalRailPoseBuilder.TryCreateQuaternion(
                new Vector3(0, 0, 0),
                new Vector3(1, 0, 0),
                new Vector3(2, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                null,  // preferredNormal=null → 使用世界up
                Quaternion.Identity,
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            // 局部 X → 世界 X（forward=切线）
            AssertColumn(linear, 0, 1, 0, 0);
            // 局部 Z → 世界 Z（up=世界up）
            AssertColumn(linear, 2, 0, 0, 1);
        }

        [TestMethod]
        public void FreePathRotation_SlopedPath_ShouldProjectForwardToHorizontal_UpStaysWorldUp()
        {
            // 斜路径（沿X上升）：物体只绕垂直轴转 yaw，forward 投影到水平面，up 恒为世界Z（不侧滚）
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(2, 0, 1),  // 切线（含垂直分量）
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                LocalEulerRotationCorrection.Zero,
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            // forward = 切线水平投影 = (1,0,0) → 局部X映射到世界X
            AssertColumn(linear, 0, 1, 0, 0);
            // up = 世界Z（不侧滚，关键：斜路径也不倾斜）
            AssertColumn(linear, 2, 0, 0, 1);
        }

        [TestMethod]
        public void FreePathRotation_PureVerticalPath_ShouldFail_NoHorizontalDirection()
        {
            // 纯垂直路径无水平分量，无法确定 yaw，应返回 false（保持当前姿态）
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(0, 0, 2),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                LocalEulerRotationCorrection.Zero,
                out _);

            Assert.IsFalse(ok);
        }

        // === 自由路径几何中心对齐语义 ===

        [TestMethod]
        public void FreePath_CenterAlignment_ShouldNotApplyHeightOffset()
        {
            // 自由路径：路径点即物体中心，ResolveGroundTrackedCenter 的半高偏移不应用于自由路径
            // 这里验证 CanonicalTrackedPositionResolver 的语义不影响自由路径
            var resolver = CanonicalTrackedPositionResolver.ResolveGroundTrackedCenter(
                new Vector3(5, 5, 5),
                Vector3.UnitZ,
                2.0);
            // Ground 语义：中心上移半高
            Assert.AreEqual(6.0, resolver.Z, 1e-6);
        }

        private static void AssertColumn(Matrix4x4 m, int col, float x, float y, float z)
        {
            switch (col)
            {
                case 0:
                    Assert.AreEqual(x, m.M11, 1e-5f);
                    Assert.AreEqual(y, m.M21, 1e-5f);
                    Assert.AreEqual(z, m.M31, 1e-5f);
                    break;
                case 1:
                    Assert.AreEqual(x, m.M12, 1e-5f);
                    Assert.AreEqual(y, m.M22, 1e-5f);
                    Assert.AreEqual(z, m.M32, 1e-5f);
                    break;
                case 2:
                    Assert.AreEqual(x, m.M13, 1e-5f);
                    Assert.AreEqual(y, m.M23, 1e-5f);
                    Assert.AreEqual(z, m.M33, 1e-5f);
                    break;
                default:
                    Assert.Fail("无效列");
                    break;
            }
        }
    }
}
