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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class PathTargetFrameResolverTests
    {
        [TestMethod]
        public void YUp_PlanarFrame_ShouldUseHostYAsUp()
        {
            bool ok = PathTargetFrameResolver.TryCreatePlanarHostFrame(
                new Vector3(2.0f, 0.0f, 1.0f),
                CoordinateSystemType.YUp,
                out PathTargetFrame frame);

            Assert.IsTrue(ok);
            AssertVector(frame.Up, 0.0, 1.0, 0.0);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
            Assert.AreEqual(1.0, frame.Side.Length(), 1e-6);
        }

        [TestMethod]
        public void ZUp_PlanarFrame_ShouldUseHostZAsUp()
        {
            bool ok = PathTargetFrameResolver.TryCreatePlanarHostFrame(
                new Vector3(2.0f, 1.0f, 0.0f),
                CoordinateSystemType.ZUp,
                out PathTargetFrame frame);

            Assert.IsTrue(ok);
            AssertVector(frame.Up, 0.0, 0.0, 1.0);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
            Assert.AreEqual(1.0, frame.Side.Length(), 1e-6);
        }

        [TestMethod]
        public void Hoisting_StartForward_ShouldUseHorizontalSegmentInsteadOfInitialLift()
        {
            var pathPoints = new List<Vector3>
            {
                new Vector3(-105.992f, -28.615f, 77.043f),
                new Vector3(-105.992f, 16.230f, 77.043f),
                new Vector3(-229.870f, 16.230f, -32.982f)
            };

            bool ok = PathTargetFrameResolver.TryResolvePlanarStartHostForward(
                NavisworksTransport.PathType.Hoisting,
                pathPoints,
                out Vector3 hostForward);

            Assert.IsTrue(ok);
            AssertVector(Vector3.Normalize(hostForward), -0.7477, 0.0, -0.6640, 1e-4);
        }

        [TestMethod]
        public void Hoisting_StartFrame_ShouldUseHorizontalSegmentAndKeepHostUp()
        {
            var pathPoints = new List<Vector3>
            {
                new Vector3(-105.992f, -28.615f, 77.043f),
                new Vector3(-105.992f, 16.230f, 77.043f),
                new Vector3(-229.870f, 16.230f, -32.982f)
            };

            bool ok = PathTargetFrameResolver.TryCreatePlanarStartHostFrame(
                NavisworksTransport.PathType.Hoisting,
                pathPoints,
                CoordinateSystemType.YUp,
                out PathTargetFrame frame);

            Assert.IsTrue(ok);
            AssertVector(frame.Up, 0.0, 1.0, 0.0);
            Assert.AreEqual(0.0, Vector3.Dot(frame.Forward, frame.Up), 1e-6);
        }

        [TestMethod]
        public void Hoisting_StartForward_ShouldUseEditedRealPathHorizontalSegment_FromDebugLog()
        {
            var pathPoints = new List<Vector3>
            {
                new Vector3(-197.436f, 14.833f, 29.413f),
                new Vector3(-197.436f, 31.240f, 29.413f),
                new Vector3(-213.650f, 31.240f, 29.413f)
            };

            bool ok = PathTargetFrameResolver.TryResolvePlanarStartHostForward(
                NavisworksTransport.PathType.Hoisting,
                pathPoints,
                out Vector3 hostForward);

            Assert.IsTrue(ok);
            AssertVector(Vector3.Normalize(hostForward), -1.0, 0.0, 0.0, 1e-6);
        }

        [TestMethod]
        public void YUp_PlanarYaw_ShouldUseHostXZPlane_NotHostXY()
        {
            bool ok1 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
                new Vector3(1.0f, 0.0f, 0.0f),
                CoordinateSystemType.YUp,
                out double yaw1);
            bool ok2 = PathTargetFrameResolver.TryResolvePlanarHostYaw(
                new Vector3(0.0f, 0.0f, 1.0f),
                CoordinateSystemType.YUp,
                out double yaw2);

            Assert.IsTrue(ok1);
            Assert.IsTrue(ok2);
            Assert.AreEqual(0.0, yaw1, 1e-6);
            Assert.AreEqual(-System.Math.PI / 2.0, yaw2, 1e-6);
        }

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