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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RealObjectPlanarPoseSolverTests
    {
        [TestMethod]
        public void ShouldChooseClosestCandidateAxis_FromRotatedReferencePose()
        {
            Quaternion referenceRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, (float)(Math.PI / 6.0));
            Vector3 desiredForward = new Vector3(1.0f, 0.2f, 0.1f);
            Vector3 desiredUp = Vector3.UnitZ;

            bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForward,
                desiredUp,
                out RealObjectPlanarPoseSolution solution);

            Assert.IsTrue(ok);
            AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);

            Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
            AssertVector(transformedSelectedAxis, desiredForward.X / desiredForward.Length(), desiredForward.Y / desiredForward.Length(), desiredForward.Z / desiredForward.Length(), 1e-4);
        }

        [TestMethod]
        public void ShouldPreferNegativeAxisWhenItMatchesForwardBest()
        {
            bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                Quaternion.Identity,
                new Vector3(-0.1f, -1.0f, 0.05f),
                Vector3.UnitZ,
                out RealObjectPlanarPoseSolution solution);

            Assert.IsTrue(ok);
            AssertVector(solution.SelectedReferenceAxisLocal, 0.0, -1.0, 0.0);
        }

        [TestMethod]
        public void ShouldRespectDesiredUpPlane_WhenDesiredForwardHasVerticalComponent()
        {
            Vector3 desiredForward = Vector3.Normalize(new Vector3(1.0f, 1.0f, 1.0f));

            bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                Quaternion.Identity,
                desiredForward,
                Vector3.UnitY,
                out RealObjectPlanarPoseSolution solution);

            Assert.IsTrue(ok);
            Vector3 transformedSelectedAxis = Vector3.Normalize(Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
            AssertVector(transformedSelectedAxis, desiredForward.X, desiredForward.Y, desiredForward.Z, 1e-4);
        }

        [TestMethod]
        public void ShouldPreserveReferenceOrthogonalityByApplyingSingleRotationDelta()
        {
            Quaternion referenceRotation = Quaternion.Normalize(
                Quaternion.CreateFromYawPitchRoll(
                    (float)(Math.PI / 7.0),
                    (float)(Math.PI / 9.0),
                    (float)(Math.PI / 11.0)));

            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.3f, 0.8f, 0.52f));

            bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForward,
                Vector3.UnitZ,
                out RealObjectPlanarPoseSolution solution);

            Assert.IsTrue(ok);

            Vector3 transformedX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, solution.BaselineRotation));
            Vector3 transformedY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, solution.BaselineRotation));
            Vector3 transformedZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, solution.BaselineRotation));

            Assert.AreEqual(1.0, transformedX.Length(), 1e-4);
            Assert.AreEqual(1.0, transformedY.Length(), 1e-4);
            Assert.AreEqual(1.0, transformedZ.Length(), 1e-4);
            Assert.AreEqual(0.0, Vector3.Dot(transformedX, transformedY), 1e-4);
            Assert.AreEqual(0.0, Vector3.Dot(transformedY, transformedZ), 1e-4);
            Assert.AreEqual(0.0, Vector3.Dot(transformedZ, transformedX), 1e-4);
        }

        [TestMethod]
        public void FixedReferenceAxis_ShouldKeepAxisFamilyAcrossTurn()
        {
            Quaternion referenceRotation = Quaternion.CreateFromAxisAngle(Vector3.UnitY, (float)Math.PI);
            Vector3 desiredForwardStart = Vector3.Normalize(new Vector3(-0.95f, 0.0f, 0.31f));
            Vector3 desiredForwardTurn = Vector3.Normalize(new Vector3(0.22f, 0.0f, 0.97f));
            Vector3 desiredUp = Vector3.UnitY;

            bool startOk = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForwardStart,
                desiredUp,
                out RealObjectPlanarPoseSolution startSolution);

            Assert.IsTrue(startOk);

            bool turnOk = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForwardTurn,
                desiredUp,
                startSolution.SelectedReferenceAxisDirection,
                out RealObjectPlanarPoseSolution turnSolution);

            Assert.IsTrue(turnOk);
            Assert.AreEqual(startSolution.SelectedReferenceAxisDirection, turnSolution.SelectedReferenceAxisDirection);

            Vector3 transformedSelectedAxis = Vector3.Normalize(
                Vector3.Transform(turnSolution.SelectedReferenceAxisLocal, turnSolution.BaselineRotation));
            AssertVector(
                transformedSelectedAxis,
                desiredForwardTurn.X,
                desiredForwardTurn.Y,
                desiredForwardTurn.Z,
                1e-4);
        }

        [TestMethod]
        public void FixedPositiveX_ShouldNotSwitchToZ_WhenPathLeansTowardZ()
        {
            Quaternion referenceRotation = Quaternion.Identity;
            Vector3 desiredForward = Vector3.Normalize(new Vector3(-0.690f, 0.0f, 0.724f));
            Vector3 desiredUp = Vector3.UnitY;

            bool ok = RealObjectPlanarPoseSolver.TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForward,
                desiredUp,
                LocalAxisDirection.PositiveX,
                out RealObjectPlanarPoseSolution solution);

            Assert.IsTrue(ok);
            Assert.AreEqual(LocalAxisDirection.PositiveX, solution.SelectedReferenceAxisDirection);
            AssertVector(solution.SelectedReferenceAxisLocal, 1.0, 0.0, 0.0);

            Vector3 transformedSelectedAxis = Vector3.Normalize(
                Vector3.Transform(solution.SelectedReferenceAxisLocal, solution.BaselineRotation));
            AssertVector(
                transformedSelectedAxis,
                desiredForward.X,
                desiredForward.Y,
                desiredForward.Z,
                1e-4);
        }

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