using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    public static class HoistingRealObjectPoseHelper
    {
        public static Quaternion CreateRotationFromPlanarBasePose(
            Quaternion baseRotation,
            double baseYawRadians,
            double targetYawRadians,
            Vector3 hostUp)
        {
            Vector3 normalizedHostUp = NormalizeSafe(hostUp);
            if (normalizedHostUp.LengthSquared() < 1e-6f)
            {
                return Quaternion.Normalize(baseRotation);
            }

            float deltaYawRadians = NormalizeRadians(targetYawRadians - baseYawRadians);
            Quaternion deltaRotation = Quaternion.CreateFromAxisAngle(normalizedHostUp, deltaYawRadians);
            return Quaternion.Normalize(deltaRotation * baseRotation);
        }

        public static bool TryCreateRotationFromActualPose(
            Quaternion actualRotation,
            Vector3 targetForward,
            Vector3 hostUp,
            out Quaternion resultRotation,
            out LocalAxisDirection selectedForwardAxisDirection,
            out Vector3 selectedForwardAxisLocal,
            out Vector3 projectedForward)
        {
            resultRotation = Quaternion.Identity;
            selectedForwardAxisDirection = LocalAxisDirection.PositiveX;
            selectedForwardAxisLocal = Vector3.UnitX;
            projectedForward = Vector3.Zero;

            if (actualRotation.LengthSquared() < 1e-6f)
            {
                return false;
            }

            Vector3 normalizedHostUp = NormalizeSafe(hostUp);
            if (normalizedHostUp.LengthSquared() < 1e-6f)
            {
                return false;
            }

            Vector3 normalizedTargetForward = NormalizeOnPlane(targetForward, normalizedHostUp);
            if (normalizedTargetForward.LengthSquared() < 1e-6f)
            {
                return false;
            }

            Quaternion normalizedActualRotation = Quaternion.Normalize(actualRotation);
            Vector3[] localAxes =
            {
                Vector3.UnitX,
                Vector3.UnitY,
                Vector3.UnitZ
            };
            LocalAxisDirection[] directions =
            {
                LocalAxisDirection.PositiveX,
                LocalAxisDirection.PositiveY,
                LocalAxisDirection.PositiveZ
            };

            float bestScore = float.NegativeInfinity;
            Vector3 bestProjectedAxis = Vector3.Zero;

            for (int i = 0; i < localAxes.Length; i++)
            {
                Vector3 worldAxis = Vector3.Transform(localAxes[i], normalizedActualRotation);
                float upAlignment = Math.Abs(Vector3.Dot(Vector3.Normalize(worldAxis), normalizedHostUp));
                if (upAlignment > 0.9f)
                {
                    continue;
                }

                Vector3 projectedAxis = NormalizeOnPlane(worldAxis, normalizedHostUp);
                if (projectedAxis.LengthSquared() < 1e-6f)
                {
                    continue;
                }

                float score = Math.Abs(Vector3.Dot(projectedAxis, normalizedTargetForward));
                if (score > bestScore)
                {
                    bestScore = score;
                    bestProjectedAxis = projectedAxis;
                    selectedForwardAxisDirection = directions[i];
                    selectedForwardAxisLocal = localAxes[i];
                }
            }

            if (bestScore == float.NegativeInfinity)
            {
                return false;
            }

            Vector3 resolvedForward = Vector3.Dot(bestProjectedAxis, normalizedTargetForward) >= 0.0f
                ? normalizedTargetForward
                : -normalizedTargetForward;

            projectedForward = resolvedForward;
            float signedAngle = SignedAngleAroundAxis(bestProjectedAxis, resolvedForward, normalizedHostUp);
            Quaternion deltaRotation = Quaternion.CreateFromAxisAngle(normalizedHostUp, signedAngle);
            resultRotation = Quaternion.Normalize(deltaRotation * normalizedActualRotation);
            return true;
        }

        private static Vector3 NormalizeOnPlane(Vector3 vector, Vector3 planeNormal)
        {
            Vector3 planar = vector - Vector3.Dot(vector, planeNormal) * planeNormal;
            return NormalizeSafe(planar);
        }

        private static Vector3 NormalizeSafe(Vector3 vector)
        {
            if (vector.LengthSquared() < 1e-6f)
            {
                return Vector3.Zero;
            }

            return Vector3.Normalize(vector);
        }

        private static float SignedAngleAroundAxis(Vector3 from, Vector3 to, Vector3 axis)
        {
            float dot = Vector3.Dot(from, to);
            if (dot > 1.0f)
            {
                dot = 1.0f;
            }
            else if (dot < -1.0f)
            {
                dot = -1.0f;
            }

            float angle = (float)Math.Acos(dot);
            float sign = Math.Sign(Vector3.Dot(axis, Vector3.Cross(from, to)));
            if (Math.Abs(sign) < 1e-6f)
            {
                sign = 1.0f;
            }

            return angle * sign;
        }

        private static float NormalizeRadians(double angleRadians)
        {
            while (angleRadians > Math.PI)
            {
                angleRadians -= 2.0 * Math.PI;
            }

            while (angleRadians < -Math.PI)
            {
                angleRadians += 2.0 * Math.PI;
            }

            return (float)angleRadians;
        }
    }
}
