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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RailPreservedPoseTests
    {
        [TestMethod]
        public void ResolvePreservedPoseRotation_ShouldPreferActualGeometryRotationForRealObjects()
        {
            Rotation3D fallbackRotation = new Rotation3D(0.0, 0.0, 0.0, 1.0);
            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);

            Rotation3D resolved = PathAnimationManager.ResolvePreservedPoseRotation(
                preferActualGeometryRotation: true,
                hasActualGeometryRotation: true,
                actualGeometryRotation: actualGeometryRotation,
                fallbackRotation: fallbackRotation);

            Assert.AreEqual(actualGeometryRotation.A, resolved.A, 1e-9);
            Assert.AreEqual(actualGeometryRotation.B, resolved.B, 1e-9);
            Assert.AreEqual(actualGeometryRotation.C, resolved.C, 1e-9);
            Assert.AreEqual(actualGeometryRotation.D, resolved.D, 1e-9);
        }

        [TestMethod]
        public void ResolvePreservedPoseRotation_ShouldFallbackWhenActualGeometryRotationUnavailable()
        {
            Rotation3D fallbackRotation = new Rotation3D(0.0, 0.0, 0.38268343, 0.92387953);
            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);

            Rotation3D resolved = PathAnimationManager.ResolvePreservedPoseRotation(
                preferActualGeometryRotation: true,
                hasActualGeometryRotation: false,
                actualGeometryRotation: actualGeometryRotation,
                fallbackRotation: fallbackRotation);

            Assert.AreEqual(fallbackRotation.A, resolved.A, 1e-9);
            Assert.AreEqual(fallbackRotation.B, resolved.B, 1e-9);
            Assert.AreEqual(fallbackRotation.C, resolved.C, 1e-9);
            Assert.AreEqual(fallbackRotation.D, resolved.D, 1e-9);
        }

        [TestMethod]
        public void ShouldPreservePathRotationForFrames_ShouldEnableHoistingWhenTranslationModeHasLockedRotation()
        {
            bool shouldPreserve = PathAnimationManager.ShouldPreservePathRotationForFrames(
                PathType.Hoisting,
                ObjectStartPlacementMode.PreserveInitialPose,
                hasPreservedRotation: true);

            Assert.IsTrue(shouldPreserve);
        }

        [TestMethod]
        public void ShouldPreservePathRotationForFrames_ShouldEnableRailWhenTranslationModeHasLockedRotation()
        {
            bool shouldPreserve = PathAnimationManager.ShouldPreservePathRotationForFrames(
                PathType.Rail,
                ObjectStartPlacementMode.PreserveInitialPose,
                hasPreservedRotation: true);

            Assert.IsTrue(shouldPreserve);
        }

        [TestMethod]
        public void ShouldPreservePathRotationForFrames_ShouldDisableGroundEvenInTranslationMode()
        {
            bool shouldPreserve = PathAnimationManager.ShouldPreservePathRotationForFrames(
                PathType.Ground,
                ObjectStartPlacementMode.PreserveInitialPose,
                hasPreservedRotation: true);

            Assert.IsFalse(shouldPreserve);
        }

        [TestMethod]
        public void ShouldPreservePathRotationForFrames_ShouldDisableWhenLockedRotationMissing()
        {
            bool shouldPreserve = PathAnimationManager.ShouldPreservePathRotationForFrames(
                PathType.Hoisting,
                ObjectStartPlacementMode.PreserveInitialPose,
                hasPreservedRotation: false);

            Assert.IsFalse(shouldPreserve);
        }

        [TestMethod]
        public void ShouldAllowFragmentPlanarFallback_ShouldDisableHoisting()
        {
            Assert.IsFalse(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Hoisting));
            Assert.IsFalse(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Ground));
            Assert.IsTrue(PathAnimationManager.ShouldAllowFragmentPlanarFallback(PathType.Rail));
        }

        [TestMethod]
        public void ShouldUseReferenceBasedRealObjectPlanarPose_ShouldDisableGroundAndHoisting()
        {
            Assert.IsFalse(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Ground));
            Assert.IsFalse(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Hoisting));
            Assert.IsTrue(PathAnimationManager.ShouldUseReferenceBasedRealObjectPlanarPose(PathType.Rail));
        }

        [TestMethod]
        public void TryResolveDisplayedPlanarYawFromRotation_ShouldResolveYUpFromDisplayedXAxis()
        {
            Rotation3D rotation = new Rotation3D(
                0.0,
                System.Math.Sin(-System.Math.PI / 4.0),
                0.0,
                System.Math.Cos(-System.Math.PI / 4.0));

            bool ok = PathAnimationManager.TryResolveDisplayedPlanarYawFromRotation(
                rotation,
                CoordinateSystemType.YUp,
                out double yawRadians);

            Assert.IsTrue(ok);
            Assert.AreEqual(-System.Math.PI / 2.0, yawRadians, 1e-6);
        }

        [TestMethod]
        public void ShouldUsePlanarRealObjectPureIncrementFrames_ShouldEnableOnlyForPlanarRealObjects()
        {
            Assert.IsTrue(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Ground, true));
            Assert.IsTrue(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Hoisting, true));
            Assert.IsFalse(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Ground, false));
            Assert.IsFalse(PathAnimationManager.ShouldUsePlanarRealObjectPureIncrementFrames(PathType.Rail, true));
        }

        [TestMethod]
        public void ResolveAnimationFramePlaybackYawRadians_ShouldPreferPlanarHostYawWithHostCorrection()
        {
            var frame = new AnimationFrame
            {
                YawRadians = 0.25,
                PlanarHostYawRadians = 0.75,
                HasCustomRotation = true
            };

            double resolved = PathAnimationManager.ResolveAnimationFramePlaybackYawRadians(
                frame,
                new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
                CoordinateSystemType.YUp);

            Assert.AreEqual(0.75 + 20.0 * System.Math.PI / 180.0, resolved, 1e-9);
        }

        [TestMethod]
        public void ApplyRecordedPlanarRealObjectFramePose_ShouldPreserveFullRotation_ForGroundCollisionLogCase()
        {
            var frame = new AnimationFrame();
            double yawRadians = 115.85 * System.Math.PI / 180.0;

            var hostLinear = new Matrix4x4(
                -0.4360f, 0.0000f, -0.8999f, 0.0f,
                -0.6364f, 0.7071f, 0.3083f, 0.0f,
                 0.6364f, 0.7071f, -0.3083f, 0.0f,
                 0.0f,    0.0f,    0.0f,    1.0f);
            Quaternion hostQuaternion = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(hostLinear));
            var rotation = new Rotation3D(hostQuaternion.X, hostQuaternion.Y, hostQuaternion.Z, hostQuaternion.W);

            PathAnimationManager.ApplyRecordedPlanarRealObjectFramePose(frame, yawRadians, rotation);

            Assert.IsTrue(frame.PlanarHostYawRadians.HasValue);
            Assert.AreEqual(yawRadians, frame.PlanarHostYawRadians.Value, 1e-9);
            Assert.IsTrue(frame.HasCustomRotation);

            Matrix3 linear = new Transform3D(frame.Rotation).Linear;
            Assert.AreEqual(-0.4360, linear.Get(0, 0), 5e-4);
            Assert.AreEqual(-0.6364, linear.Get(1, 0), 5e-4);
            Assert.AreEqual(0.6364, linear.Get(2, 0), 5e-4);
            Assert.AreEqual(0.0000, linear.Get(0, 1), 5e-4);
            Assert.AreEqual(0.7071, linear.Get(1, 1), 5e-4);
            Assert.AreEqual(0.7071, linear.Get(2, 1), 5e-4);
            Assert.AreEqual(-0.8999, linear.Get(0, 2), 5e-4);
            Assert.AreEqual(0.3083, linear.Get(1, 2), 5e-4);
            Assert.AreEqual(-0.3083, linear.Get(2, 2), 5e-4);
        }

        [TestMethod]
        public void TryResolveGroundHostPlanarYawFromFramePoints_ShouldResolveYUpHostYaw()
        {
            bool ok = PathAnimationManager.TryResolveGroundHostPlanarYawFromFramePoints(
                new Point3D(0, 0, 0),
                new Point3D(1, 0, 0),
                new Point3D(1, 0, -1),
                CoordinateSystemType.YUp,
                out double yawRadians);

            Assert.IsTrue(ok);
            Assert.AreEqual(-System.Math.PI / 4.0, yawRadians, 1e-6);
        }

        [TestMethod]
        public void ResolvePlanarHostUpCorrectionRadians_ShouldUseYDegreesForYUp()
        {
            double radians = PathAnimationManager.ResolvePlanarHostUpCorrectionRadians(
                new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
                CoordinateSystemType.YUp);

            Assert.AreEqual(20.0 * System.Math.PI / 180.0, radians, 1e-9);
        }

        [TestMethod]
        public void ResolvePlanarHostUpCorrectionRadians_ShouldUseZDegreesForZUp()
        {
            double radians = PathAnimationManager.ResolvePlanarHostUpCorrectionRadians(
                new LocalEulerRotationCorrection(10.0, 20.0, 30.0),
                CoordinateSystemType.ZUp);

            Assert.AreEqual(30.0 * System.Math.PI / 180.0, radians, 1e-9);
        }

        [TestMethod]
        public void ResolveCurrentRotationBaseline_ShouldPreferActualGeometryForHoisting()
        {
            Rotation3D trackedRotation = new Rotation3D(0.0, 0.0, 0.0, 1.0);
            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);

            Rotation3D resolved = PathAnimationManager.ResolveCurrentRotationBaseline(
                PathType.Hoisting,
                isRealObjectMode: true,
                hasTrackedRotation: true,
                trackedRotation: trackedRotation,
                hasReferenceRotation: false,
                referenceRotation: Rotation3D.Identity,
                transformRotation: Rotation3D.Identity,
                hasActualGeometryRotation: true,
                actualGeometryRotation: actualGeometryRotation);

            Assert.AreEqual(actualGeometryRotation.A, resolved.A, 1e-9);
            Assert.AreEqual(actualGeometryRotation.B, resolved.B, 1e-9);
            Assert.AreEqual(actualGeometryRotation.C, resolved.C, 1e-9);
            Assert.AreEqual(actualGeometryRotation.D, resolved.D, 1e-9);
        }

        [TestMethod]
        public void ResolveCurrentRotationBaseline_ShouldKeepTrackedRotationForGround()
        {
            Rotation3D trackedRotation = new Rotation3D(0.0, 0.0, 0.38268343, 0.92387953);
            Rotation3D actualGeometryRotation = new Rotation3D(0.0, 0.70710678, 0.0, 0.70710678);

            Rotation3D resolved = PathAnimationManager.ResolveCurrentRotationBaseline(
                PathType.Ground,
                isRealObjectMode: true,
                hasTrackedRotation: true,
                trackedRotation: trackedRotation,
                hasReferenceRotation: false,
                referenceRotation: Rotation3D.Identity,
                transformRotation: Rotation3D.Identity,
                hasActualGeometryRotation: true,
                actualGeometryRotation: actualGeometryRotation);

            Assert.AreEqual(trackedRotation.A, resolved.A, 1e-9);
            Assert.AreEqual(trackedRotation.B, resolved.B, 1e-9);
            Assert.AreEqual(trackedRotation.C, resolved.C, 1e-9);
            Assert.AreEqual(trackedRotation.D, resolved.D, 1e-9);
        }

        [TestMethod]
        public void ShouldUseRealObjectOverrideRotation_ShouldEnableForRealObjects()
        {
            Assert.IsTrue(PathAnimationManager.ShouldUseRealObjectOverrideRotation(true));
            Assert.IsFalse(PathAnimationManager.ShouldUseRealObjectOverrideRotation(false));
        }

        [TestMethod]
        public void ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition_ShouldEnableOnlyForGroundRealObjects()
        {
            Assert.IsTrue(PathAnimationManager.ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
                PathType.Ground,
                isRealObjectMode: true));
            Assert.IsFalse(PathAnimationManager.ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
                PathType.Ground,
                isRealObjectMode: false));
            Assert.IsFalse(PathAnimationManager.ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
                PathType.Hoisting,
                isRealObjectMode: true));
            Assert.IsFalse(PathAnimationManager.ShouldUseOriginalBoundingBoxCenterForBusinessTrackedPosition(
                PathType.Rail,
                isRealObjectMode: true));
        }

    }
}