using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 根据“已解释为当前宿主语义”的真实物体参考姿态，解析 Rail 路径应使用的局部轴约定。
    ///
    /// 语义：
    /// - 真实物体没有资产坐标系，但 fragment 参考姿态经过 Fragment默认Up 解释后，
    ///   可以得到“当前宿主语义下”的真实参考三轴。
    /// - Rail 与 Ground/Hoisting 的统一点在于：都先解释对象姿态，再进入路径框架。
    /// - Rail 的特殊性只在路径框架：forward 来自 rail 切向，up 来自 rail 法向。
    /// - 本解析器只负责回答：对这个真实物体来说，哪个局部 forward 轴最接近当前路径 forward；
    ///   up 轴始终采用当前宿主语义对应的轴族（YUp => +Y，ZUp => +Z）。
    /// </summary>
    public static class RealObjectRailAxisConventionResolver
    {
        private const float AxisEpsilon = 1e-6f;

        public static bool TryResolve(
            Vector3 referenceAxisX,
            Vector3 referenceAxisY,
            Vector3 referenceAxisZ,
            LocalAxisDirection hostUpLocalAxis,
            Vector3 desiredForward,
            CoordinateSystemType hostType,
            out ModelAxisConvention convention,
            out LocalAxisDirection selectedForwardAxis,
            out Vector3 selectedForwardWorldAxis,
            out LocalAxisDirection selectedUpAxis,
            out Vector3 selectedUpWorldAxis)
        {
            convention = null;
            selectedForwardAxis = LocalAxisDirection.PositiveX;
            selectedForwardWorldAxis = Vector3.Zero;
            selectedUpAxis = hostType == CoordinateSystemType.YUp
                ? LocalAxisDirection.PositiveY
                : LocalAxisDirection.PositiveZ;
            selectedUpWorldAxis = Vector3.Zero;

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

            if (!TryNormalize(referenceAxisX, out Vector3 axisX) ||
                !TryNormalize(referenceAxisY, out Vector3 axisY) ||
                !TryNormalize(referenceAxisZ, out Vector3 axisZ))
            {
                return false;
            }

            if (!TryResolveWorldAxisForDirection(hostUpLocalAxis, axisX, axisY, axisZ, out var fixedUpWorldAxis))
            {
                return false;
            }

            Candidate bestForward = Candidate.Invalid;
            foreach (Candidate candidate in EnumerateForwardCandidates(axisX, axisY, axisZ, hostUpLocalAxis))
            {
                float score = Vector3.Dot(candidate.WorldAxis, normalizedForward);
                if (bestForward.IsInvalid || score > bestForward.Score)
                {
                    bestForward = new Candidate(candidate.Axis, candidate.WorldAxis, score);
                }
            }

            if (bestForward.IsInvalid)
            {
                return false;
            }

            convention = new ModelAxisConvention(bestForward.Axis, hostUpLocalAxis);
            selectedForwardAxis = bestForward.Axis;
            selectedForwardWorldAxis = bestForward.WorldAxis;
            selectedUpAxis = hostUpLocalAxis;
            selectedUpWorldAxis = fixedUpWorldAxis;
            return true;
        }

        private static Candidate[] EnumerateForwardCandidates(
            Vector3 axisX,
            Vector3 axisY,
            Vector3 axisZ,
            LocalAxisDirection selectedUpAxis)
        {
            int upAxisIndex = GetAxisFamilyIndex(selectedUpAxis);
            return Array.FindAll(
                EnumerateAxisCandidates(axisX, axisY, axisZ),
                candidate => GetAxisFamilyIndex(candidate.Axis) != upAxisIndex);
        }

        private static Candidate[] EnumerateAxisCandidates(
            Vector3 axisX,
            Vector3 axisY,
            Vector3 axisZ)
        {
            return new[]
            {
                new Candidate(LocalAxisDirection.PositiveX, axisX, 0f),
                new Candidate(LocalAxisDirection.NegativeX, -axisX, 0f),
                new Candidate(LocalAxisDirection.PositiveY, axisY, 0f),
                new Candidate(LocalAxisDirection.NegativeY, -axisY, 0f),
                new Candidate(LocalAxisDirection.PositiveZ, axisZ, 0f),
                new Candidate(LocalAxisDirection.NegativeZ, -axisZ, 0f)
            };
        }

        private static bool TryResolveWorldAxisForDirection(
            LocalAxisDirection axis,
            Vector3 axisX,
            Vector3 axisY,
            Vector3 axisZ,
            out Vector3 worldAxis)
        {
            switch (axis)
            {
                case LocalAxisDirection.PositiveX:
                    worldAxis = axisX;
                    return true;
                case LocalAxisDirection.NegativeX:
                    worldAxis = -axisX;
                    return true;
                case LocalAxisDirection.PositiveY:
                    worldAxis = axisY;
                    return true;
                case LocalAxisDirection.NegativeY:
                    worldAxis = -axisY;
                    return true;
                case LocalAxisDirection.PositiveZ:
                    worldAxis = axisZ;
                    return true;
                case LocalAxisDirection.NegativeZ:
                    worldAxis = -axisZ;
                    return true;
                default:
                    worldAxis = Vector3.Zero;
                    return false;
            }
        }

        private static int GetAxisFamilyIndex(LocalAxisDirection axis)
        {
            switch (axis)
            {
                case LocalAxisDirection.PositiveX:
                case LocalAxisDirection.NegativeX:
                    return 0;
                case LocalAxisDirection.PositiveY:
                case LocalAxisDirection.NegativeY:
                    return 1;
                case LocalAxisDirection.PositiveZ:
                case LocalAxisDirection.NegativeZ:
                    return 2;
                default:
                    throw new ArgumentOutOfRangeException(nameof(axis), axis, null);
            }
        }

        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 readonly struct Candidate
        {
            public static Candidate Invalid => new Candidate(LocalAxisDirection.PositiveX, Vector3.Zero, float.MinValue);

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

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

            public Candidate(LocalAxisDirection axis, Vector3 worldAxis, float score)
            {
                Axis = axis;
                WorldAxis = worldAxis;
                Score = score;
            }
        }
    }
}
