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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class VirtualGroundPoseCharacterizationTests
    {
        [TestMethod]
        public void YUp_GroundVirtualPose_WithYUpVirtualAsset_ShouldUseHostYAsUp()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var convention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);

            Vector3 hostForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
            Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward);

            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                canonicalForward,
                HostCoordinateAdapter.CanonicalUpVector3,
                convention,
                out Quaternion canonicalRotation);

            Assert.IsTrue(ok);

            Matrix4x4 hostLinear = adapter.FromCanonicalLinearTransform(
                Matrix4x4.CreateFromQuaternion(canonicalRotation));

            AssertColumn(hostLinear, 0, -0.8987, 0.0000, 0.4386);
            AssertColumn(hostLinear, 1, 0.0000, 1.0000, 0.0000);
            AssertColumn(hostLinear, 2, -0.4386, 0.0000, -0.8987);
        }

        [TestMethod]
        public void YUp_GroundVirtualPose_WithYUpVirtualAsset_ShouldMatchHostYUpDefaultPlanarConvention()
        {
            var adapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            Vector3 hostForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));
            Vector3 canonicalForward = adapter.ToCanonicalVector3(hostForward);

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                canonicalForward,
                HostCoordinateAdapter.CanonicalUpVector3,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp),
                out Quaternion assetRotation));

            Assert.IsTrue(CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                canonicalForward,
                HostCoordinateAdapter.CanonicalUpVector3,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp),
                out Quaternion hostDefaultRotation));

            Matrix4x4 assetHostLinear = adapter.FromCanonicalLinearTransform(
                Matrix4x4.CreateFromQuaternion(assetRotation));
            Matrix4x4 hostDefaultLinear = adapter.FromCanonicalLinearTransform(
                Matrix4x4.CreateFromQuaternion(hostDefaultRotation));

            Assert.AreEqual(1.0, assetHostLinear.M22, 1e-3, "YUp 虚拟物体资源应把 local Y 对到宿主 up。");
            Assert.AreEqual(hostDefaultLinear.M11, assetHostLinear.M11, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M21, assetHostLinear.M21, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M31, assetHostLinear.M31, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M12, assetHostLinear.M12, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M22, assetHostLinear.M22, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M32, assetHostLinear.M32, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M13, assetHostLinear.M13, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M23, assetHostLinear.M23, 1e-3);
            Assert.AreEqual(hostDefaultLinear.M33, assetHostLinear.M33, 1e-3);
        }

        [TestMethod]
        public void VirtualObjectReferencePose_ShouldMatchSelectedVirtualAssetConvention()
        {
            var yUpAdapter = new HostCoordinateAdapter(CoordinateSystemType.YUp);
            var zUpAdapter = new HostCoordinateAdapter(CoordinateSystemType.ZUp);
            var yUpConvention = ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp);
            var zUpConvention = ModelAxisConvention.CreateVirtualObjectAssetConvention();

            Matrix4x4 yUpLinear = Matrix4x4.CreateFromQuaternion(
                yUpConvention.CreateQuaternion(new Vector3(1, 0, 0), yUpAdapter.HostUpVector3));
            Matrix4x4 zUpLinear = Matrix4x4.CreateFromQuaternion(
                zUpConvention.CreateQuaternion(new Vector3(1, 0, 0), zUpAdapter.HostUpVector3));

            AssertColumn(yUpLinear, 0, 1.0, 0.0, 0.0);
            AssertColumn(yUpLinear, 1, 0.0, 1.0, 0.0);
            AssertColumn(yUpLinear, 2, 0.0, 0.0, 1.0);

            AssertColumn(zUpLinear, 0, 1.0, 0.0, 0.0);
            AssertColumn(zUpLinear, 1, 0.0, 1.0, 0.0);
            AssertColumn(zUpLinear, 2, 0.0, 0.0, 1.0);
        }

        private static void AssertColumn(Matrix4x4 matrix, int column, double x, double y, double z)
        {
            switch (column)
            {
                case 0:
                    Assert.AreEqual(x, matrix.M11, 1e-3);
                    Assert.AreEqual(y, matrix.M21, 1e-3);
                    Assert.AreEqual(z, matrix.M31, 1e-3);
                    break;
                case 1:
                    Assert.AreEqual(x, matrix.M12, 1e-3);
                    Assert.AreEqual(y, matrix.M22, 1e-3);
                    Assert.AreEqual(z, matrix.M32, 1e-3);
                    break;
                case 2:
                    Assert.AreEqual(x, matrix.M13, 1e-3);
                    Assert.AreEqual(y, matrix.M23, 1e-3);
                    Assert.AreEqual(z, matrix.M33, 1e-3);
                    break;
                default:
                    Assert.Fail("Only first 3 columns are valid.");
                    break;
            }
        }
    }
}