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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class RailPathPoseHelperTests
    {
        [TestMethod]
        public void NormalizePreferredNormalToHostUpHemisphere_ShouldFlip_WhenPreferredNormalOpposesHostUp()
        {
            Vector3 hostPreferredNormal = new Vector3(-0.1222f, -0.9676f, 0.2211f);
            Vector3 normalized = RailPathPoseHelper.NormalizePreferredNormalToHostUpHemisphere(
                hostPreferredNormal,
                Vector3.UnitY);

            Assert.IsTrue(Vector3.Dot(normalized, Vector3.UnitY) > 0f);
            Assert.AreEqual(0.1222 / 1.0008249, normalized.X, 1e-4);
            Assert.AreEqual(0.9676 / 1.0008249, normalized.Y, 1e-4);
            Assert.AreEqual(-0.2211 / 1.0008249, normalized.Z, 1e-4);
        }

        [TestMethod]
        public void NormalizePreferredNormalToHostUpHemisphere_ShouldKeepDirection_WhenPreferredNormalAlreadyMatchesHostUp()
        {
            Vector3 hostPreferredNormal = new Vector3(0.139f, 0.954f, 0.266f);
            Vector3 normalized = RailPathPoseHelper.NormalizePreferredNormalToHostUpHemisphere(
                hostPreferredNormal,
                Vector3.UnitY);

            Assert.IsTrue(Vector3.Dot(normalized, Vector3.UnitY) > 0f);
            Assert.IsTrue(Vector3.Dot(Vector3.Normalize(hostPreferredNormal), normalized) > 0.9999f);
        }
    }
}