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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RealObjectRailAxisConventionResolverTests
    {
        [TestMethod]
        public void YUp_InterpretedReferencePose_ShouldChoosePositiveXAndPositiveYForRail()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, -1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.8987f, 0.0f, 0.4386f));

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.YUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out Vector3 selectedForwardWorldAxis);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveY, convention.UpAxis);
            AssertVector(selectedForwardWorldAxis, -1.0, 0.0, 0.0);
        }

        [TestMethod]
        public void ZUp_InterpretedReferencePose_ShouldChoosePositiveXAndPositiveZForRail()
        {
            Vector3 referenceAxisX = new Vector3(-1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.9f, 0.3f, 0.0f));

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.ZUp,
                out ModelAxisConvention convention,
                out LocalAxisDirection selectedForwardAxis,
                out _);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveX, convention.ForwardAxis);
            Assert.AreEqual(LocalAxisDirection.PositiveZ, convention.UpAxis);
        }

        [TestMethod]
        public void YUp_ShouldNotSelectYAxisFamilyAsForwardCandidate()
        {
            Vector3 referenceAxisX = new Vector3(1.0f, 0.0f, 0.0f);
            Vector3 referenceAxisY = new Vector3(0.0f, 1.0f, 0.0f);
            Vector3 referenceAxisZ = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 desiredForward = Vector3.Normalize(new Vector3(0.0f, 1.0f, 0.01f));

            bool ok = RealObjectRailAxisConventionResolver.TryResolve(
                referenceAxisX,
                referenceAxisY,
                referenceAxisZ,
                desiredForward,
                CoordinateSystemType.YUp,
                out _,
                out LocalAxisDirection selectedForwardAxis,
                out _);

            Assert.IsTrue(ok);
            Assert.AreNotEqual(LocalAxisDirection.PositiveY, selectedForwardAxis);
            Assert.AreNotEqual(LocalAxisDirection.NegativeY, selectedForwardAxis);
        }

        private static void AssertVector(Vector3 actual, double x, double y, double z, double tolerance = 1e-6)
        {
            Assert.AreEqual(x, actual.X, tolerance);
            Assert.AreEqual(y, actual.Y, tolerance);
            Assert.AreEqual(z, actual.Z, tolerance);
        }
    }
}