using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 真实物体在地面/吊装平面路径下的起点姿态求解器。
    ///
    /// 语义：
    /// - 输入和输出都保持在宿主坐标系语义下
    /// - 真实物体没有资产坐标系
    /// - 从原始参考位姿的 6 个候选轴里，选择与路径前进方向最接近的那个
    /// - 先求“原始前进轴 -> 路径前进方向”的旋转差，再作用到原始参考位姿上
    /// </summary>
    public static class RealObjectPlanarPoseSolver
    {
        private const float AxisEpsilon = 1e-6f;

        public static bool TryCreatePlanarPoseFromReferencePose(
            Quaternion referenceRotation,
            Vector3 desiredForward,
            Vector3 desiredUp,
            out RealObjectPlanarPoseSolution solution)
        {
            return TryCreatePlanarPoseFromReferencePose(
                referenceRotation,
                desiredForward,
                desiredUp,
                null,
                out solution);
        }

        public static bool TryCreatePlanarPoseFromReferencePose(
            Quaternion referenceRotation,
            Vector3 desiredForward,
            Vector3 desiredUp,
            LocalAxisDirection? fixedReferenceAxis,
            out RealObjectPlanarPoseSolution solution)
        {
            solution = null;

            if (!TryNormalize(referenceRotation, out Quaternion normalizedReferenceRotation))
            {
                return false;
            }

            if (!TryNormalize(desiredUp, out Vector3 normalizedUp))
            {
                return false;
            }

            if (!TryNormalize(desiredForward, out Vector3 normalizedForward))
            {
                return false;
            }

            CandidateAxis bestCandidate = fixedReferenceAxis.HasValue
                ? CreateCandidateAxisFromFixedDirection(fixedReferenceAxis.Value, normalizedReferenceRotation)
                : SelectBestCandidateAxis(
                    normalizedReferenceRotation,
                    normalizedForward);

            if (bestCandidate.IsInvalid)
            {
                return false;
            }

            Quaternion rotationDelta = CreateShortestArcQuaternion(
                bestCandidate.WorldAxis,
                normalizedForward,
                normalizedUp);

            Quaternion baselineRotation = Quaternion.Normalize(rotationDelta * normalizedReferenceRotation);
            solution = new RealObjectPlanarPoseSolution(
                normalizedForward,
                normalizedUp,
                bestCandidate.LocalAxisDirection,
                bestCandidate.LocalAxis,
                bestCandidate.WorldAxis,
                rotationDelta,
                baselineRotation);
            return true;
        }

        private static CandidateAxis SelectBestCandidateAxis(
            Quaternion referenceRotation,
            Vector3 desiredForward)
        {
            CandidateAxis bestCandidate = CandidateAxis.Invalid;

            foreach (var candidate in EnumerateCandidateAxes(referenceRotation))
            {
                float score = Vector3.Dot(candidate.WorldAxis, desiredForward);
                if (bestCandidate.IsInvalid || score > bestCandidate.Score)
                {
                    bestCandidate = new CandidateAxis(
                        candidate.LocalAxisDirection,
                        candidate.LocalAxis,
                        candidate.WorldAxis,
                        score);
                }
            }

            return bestCandidate;
        }

        private static CandidateAxis CreateCandidateAxisFromFixedDirection(
            LocalAxisDirection direction,
            Quaternion referenceRotation)
        {
            Vector3 localAxis = GetAxisVector(direction);
            Vector3 worldAxis = Vector3.Normalize(Vector3.Transform(localAxis, referenceRotation));
            return new CandidateAxis(direction, localAxis, worldAxis, 0f);
        }

        private static CandidateAxis[] EnumerateCandidateAxes(Quaternion referenceRotation)
        {
            return new[]
            {
                CreateCandidateAxis(LocalAxisDirection.PositiveX, Vector3.UnitX, referenceRotation),
                CreateCandidateAxis(LocalAxisDirection.NegativeX, -Vector3.UnitX, referenceRotation),
                CreateCandidateAxis(LocalAxisDirection.PositiveY, Vector3.UnitY, referenceRotation),
                CreateCandidateAxis(LocalAxisDirection.NegativeY, -Vector3.UnitY, referenceRotation),
                CreateCandidateAxis(LocalAxisDirection.PositiveZ, Vector3.UnitZ, referenceRotation),
                CreateCandidateAxis(LocalAxisDirection.NegativeZ, -Vector3.UnitZ, referenceRotation)
            };
        }

        private static CandidateAxis CreateCandidateAxis(
            LocalAxisDirection direction,
            Vector3 localAxis,
            Quaternion referenceRotation)
        {
            Vector3 worldAxis = Vector3.Normalize(Vector3.Transform(localAxis, referenceRotation));
            return new CandidateAxis(direction, localAxis, worldAxis, float.MinValue);
        }

        private static Vector3 GetAxisVector(LocalAxisDirection direction)
        {
            switch (direction)
            {
                case LocalAxisDirection.PositiveX:
                    return Vector3.UnitX;
                case LocalAxisDirection.NegativeX:
                    return -Vector3.UnitX;
                case LocalAxisDirection.PositiveY:
                    return Vector3.UnitY;
                case LocalAxisDirection.NegativeY:
                    return -Vector3.UnitY;
                case LocalAxisDirection.PositiveZ:
                    return Vector3.UnitZ;
                case LocalAxisDirection.NegativeZ:
                    return -Vector3.UnitZ;
                default:
                    throw new ArgumentOutOfRangeException(nameof(direction), direction, null);
            }
        }

        private static Quaternion CreateShortestArcQuaternion(
            Vector3 from,
            Vector3 to,
            Vector3 desiredUp)
        {
            Vector3 normalizedFrom = Vector3.Normalize(from);
            Vector3 normalizedTo = Vector3.Normalize(to);

            float dot = Vector3.Dot(normalizedFrom, normalizedTo);
            dot = Math.Max(-1.0f, Math.Min(1.0f, dot));

            if (dot > 1.0f - 1e-6f)
            {
                return Quaternion.Identity;
            }

            if (dot < -1.0f + 1e-6f)
            {
                Vector3 axis = Vector3.Cross(normalizedFrom, desiredUp);
                if (axis.LengthSquared() < AxisEpsilon)
                {
                    axis = Vector3.Cross(normalizedFrom, GetAnyOrthogonalVector(normalizedFrom));
                }

                axis = Vector3.Normalize(axis);
                return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(axis, (float)Math.PI));
            }

            Vector3 cross = Vector3.Cross(normalizedFrom, normalizedTo);
            if (cross.LengthSquared() < AxisEpsilon)
            {
                return Quaternion.Identity;
            }

            cross = Vector3.Normalize(cross);
            double angle = Math.Acos(dot);
            return Quaternion.Normalize(Quaternion.CreateFromAxisAngle(cross, (float)angle));
        }

        private static Vector3 GetAnyOrthogonalVector(Vector3 source)
        {
            if (Math.Abs(source.X) < 0.9f)
            {
                return Vector3.UnitX;
            }

            if (Math.Abs(source.Y) < 0.9f)
            {
                return Vector3.UnitY;
            }

            return Vector3.UnitZ;
        }

        private static bool TryNormalize(Quaternion value, out Quaternion normalized)
        {
            float lengthSquared = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
            if (lengthSquared < AxisEpsilon)
            {
                normalized = Quaternion.Identity;
                return false;
            }

            normalized = Quaternion.Normalize(value);
            return true;
        }

        private static bool TryNormalize(Vector3 value, out Vector3 normalized)
        {
            if (value.LengthSquared() < AxisEpsilon)
            {
                normalized = Vector3.Zero;
                return false;
            }

            normalized = Vector3.Normalize(value);
            return true;
        }

        private struct CandidateAxis
        {
            public static CandidateAxis Invalid => new CandidateAxis(LocalAxisDirection.PositiveX, Vector3.Zero, Vector3.Zero, float.MinValue);

            public LocalAxisDirection LocalAxisDirection { get; }
            public Vector3 LocalAxis { get; }
            public Vector3 WorldAxis { get; }
            public float Score { get; }

            public bool IsInvalid => Score == float.MinValue;

            public CandidateAxis(LocalAxisDirection localAxisDirection, Vector3 localAxis, Vector3 worldAxis, float score)
            {
                LocalAxisDirection = localAxisDirection;
                LocalAxis = localAxis;
                WorldAxis = worldAxis;
                Score = score;
            }
        }
    }

    /// <summary>
    /// 真实物体平面起点姿态求解结果。
    /// </summary>
    public sealed class RealObjectPlanarPoseSolution
    {
        public Vector3 ProjectedForward { get; }
        public Vector3 DesiredUp { get; }
        public LocalAxisDirection SelectedReferenceAxisDirection { get; }
        public Vector3 SelectedReferenceAxisLocal { get; }
        public Vector3 SelectedReferenceAxisWorld { get; }
        public Quaternion RotationDelta { get; }
        public Quaternion BaselineRotation { get; }

        public RealObjectPlanarPoseSolution(
            Vector3 projectedForward,
            Vector3 desiredUp,
            LocalAxisDirection selectedReferenceAxisDirection,
            Vector3 selectedReferenceAxisLocal,
            Vector3 selectedReferenceAxisWorld,
            Quaternion rotationDelta,
            Quaternion baselineRotation)
        {
            ProjectedForward = projectedForward;
            DesiredUp = desiredUp;
            SelectedReferenceAxisDirection = selectedReferenceAxisDirection;
            SelectedReferenceAxisLocal = selectedReferenceAxisLocal;
            SelectedReferenceAxisWorld = selectedReferenceAxisWorld;
            RotationDelta = rotationDelta;
            BaselineRotation = baselineRotation;
        }
    }
}
