using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 在“默认局部轴约定”下解析 Rail 路径应使用的局部轴约定。
    ///
    /// 语义（0.18.2 起，真实物体姿态链不依赖 fragment）：
    /// - 真实物体没有资产坐标系，姿态链使用默认局部轴（forward=+X，up=宿主 up）
    ///   作为基线，用户通过“调整物体”手工指定最终朝向。
    /// - 本解析器只负责回答：在这组默认局部轴中，哪个局部 forward 轴最接近当前路径 forward；
    ///   up 轴始终采用当前宿主语义对应的轴族（YUp => +Y，ZUp => +Z）。
    /// - 这样轨道沿 Y 时自动用 +Y 作 forward，避免 90° 基线偏差，减少用户调整量。
    /// </summary>
    public static class RealObjectRailAxisConventionResolver
    {
        private const float AxisEpsilon = 1e-6f;

        public static bool TryResolve(
            Vector3 defaultAxisX,
            Vector3 defaultAxisY,
            Vector3 defaultAxisZ,
            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(defaultAxisX, out Vector3 axisX) ||
                !TryNormalize(defaultAxisY, out Vector3 axisY) ||
                !TryNormalize(defaultAxisZ, 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;
            }
        }
    }
}
