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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class HoistingRealObjectPoseHelperTests
    {
        [TestMethod]
        public void ShouldKeepActualUpAxis_WhenAligningHoistingForward()
        {
            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
            Vector3 hostUp = Vector3.UnitY;
            Vector3 targetForward = -Vector3.UnitX;

            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
                actualRotation,
                targetForward,
                hostUp,
                out Quaternion resultRotation,
                out LocalAxisDirection selectedDirection,
                out Vector3 selectedAxisLocal,
                out Vector3 projectedForward);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
            AssertVector(projectedForward, 1.0, 0.0, 0.0);

            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);

            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
        }

        [TestMethod]
        public void ShouldChooseHorizontalAxisFromActualPose_ForHoisting()
        {
            Quaternion actualRotation = Quaternion.Normalize(new Quaternion(0.70710677f, 0.0f, 0.0f, 0.70710677f));
            Vector3 hostUp = Vector3.UnitY;
            Vector3 targetForward = Vector3.UnitX;

            bool ok = HoistingRealObjectPoseHelper.TryCreateRotationFromActualPose(
                actualRotation,
                targetForward,
                hostUp,
                out Quaternion resultRotation,
                out LocalAxisDirection selectedDirection,
                out Vector3 selectedAxisLocal,
                out Vector3 projectedForward);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, selectedDirection);
            AssertVector(selectedAxisLocal, 1.0, 0.0, 0.0);
            AssertVector(projectedForward, 1.0, 0.0, 0.0);

            Vector3 transformedUp = Vector3.Normalize(Vector3.Transform(-Vector3.UnitZ, resultRotation));
            AssertVector(transformedUp, 0.0, 1.0, 0.0, 1e-4);

            Vector3 transformedForward = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, resultRotation));
            AssertVector(transformedForward, 1.0, 0.0, 0.0, 1e-4);
        }

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