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

namespace NavisworksTransport.UnitTests.CoordinateSystem
{
    [TestClass]
    public class FragmentRepresentativePoseHelperTests
    {
        [TestMethod]
        public void ShouldPreserveAxes_ForObservedYUpRealObjectFragmentBasis()
        {
            Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f);
            Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f);

            double[] fragmentMatrix =
            {
                axisX.X, axisX.Y, axisX.Z, 0.0,
                axisY.X, axisY.Y, axisY.Z, 0.0,
                axisZ.X, axisZ.Y, axisZ.Z, 0.0,
                0.0,     0.0,     0.0,     1.0
            };

            bool ok = FragmentRepresentativePoseHelper.TryExtractRotationFromFragmentMatrix(
                fragmentMatrix,
                out Quaternion rotation);

            Assert.IsTrue(ok);
            AssertVector(Vector3.Transform(Vector3.UnitX, rotation), axisX, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitY, rotation), axisY, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitZ, rotation), axisZ, 1e-4);
        }

        [TestMethod]
        public void RepresentativeFrame_ShouldExposeSameAxes_AsRepresentativeRotation()
        {
            Vector3 axisX = new Vector3(1.0f, 0.0f, 0.0f);
            Vector3 axisY = new Vector3(0.0f, 0.0f, 1.0f);
            Vector3 axisZ = new Vector3(0.0f, -1.0f, 0.0f);

            double[] fragmentMatrix =
            {
                axisX.X, axisX.Y, axisX.Z, 0.0,
                axisY.X, axisY.Y, axisY.Z, 0.0,
                axisZ.X, axisZ.Y, axisZ.Z, 0.0,
                0.0,     0.0,     0.0,     1.0
            };

            bool ok = FragmentRepresentativePoseHelper.TryGetRepresentativeFrame(
                new[] { fragmentMatrix },
                out FragmentRepresentativePoseHelper.RepresentativeFrame frame);

            Assert.IsTrue(ok);
            AssertVector(frame.AxisX, axisX, 1e-4);
            AssertVector(frame.AxisY, axisY, 1e-4);
            AssertVector(frame.AxisZ, axisZ, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitX, frame.Rotation), axisX, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitY, frame.Rotation), axisY, 1e-4);
            AssertVector(Vector3.Transform(Vector3.UnitZ, frame.Rotation), axisZ, 1e-4);
        }

        private static void AssertVector(Vector3 actual, Vector3 expected, double tolerance)
        {
            Assert.AreEqual(expected.X, actual.X, tolerance, "X 分量");
            Assert.AreEqual(expected.Y, actual.Y, tolerance, "Y 分量");
            Assert.AreEqual(expected.Z, actual.Z, tolerance, "Z 分量");
        }
    }
}
