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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class CanonicalPlanarPoseBuilderTests
    {
        [TestMethod]
        public void StraightForward_ZUpConvention_ShouldKeepLocalZAsWorldUp()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            AssertColumn(linear, 0, 1, 0, 0);
            AssertColumn(linear, 1, 0, 1, 0);
            AssertColumn(linear, 2, 0, 0, 1);
        }

        [TestMethod]
        public void StraightForward_YUpConvention_ShouldMapLocalYToWorldUp()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(5, 0, 0),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.YUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            AssertColumn(linear, 0, 1, 0, 0);
            AssertColumn(linear, 1, 0, 0, 1);
            AssertColumn(linear, 2, 0, -1, 0);
        }

        [TestMethod]
        public void ForwardWithVerticalComponent_ShouldProjectToHorizontalPlane()
        {
            bool ok = CanonicalPlanarPoseBuilder.TryCreateQuaternionFromForward(
                new Vector3(10, 0, 3),
                Vector3.UnitZ,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out Quaternion rotation);

            Assert.IsTrue(ok);
            Matrix4x4 linear = Matrix4x4.CreateFromQuaternion(rotation);
            Assert.AreEqual(0.0, linear.M31, 1e-6);
            Assert.AreEqual(0.0, linear.M32, 1e-6);
            Assert.AreEqual(1.0, linear.M33, 1e-6);
        }

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