using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 在内部 Canonical Space 中，为地面/吊装这类“法向取项目 up”的路径构造姿态。
    /// 这一层只处理纯数学的前进方向和上方向，不依赖 Navisworks API。
    /// </summary>
    public static class CanonicalPlanarPoseBuilder
    {
        private const float ForwardEpsilon = 1e-6f;

        public static bool TryCreateQuaternionFromPoints(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            out Quaternion rotation)
        {
            rotation = Quaternion.Identity;

            Vector3 tangent = nextPoint - previousPoint;
            if (tangent.LengthSquared() < ForwardEpsilon)
            {
                tangent = nextPoint - currentPoint;
            }

            return TryCreateQuaternionFromForward(tangent, canonicalUp, convention, out rotation);
        }

        public static bool TryCreateQuaternionFromForward(
            Vector3 desiredForward,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            out Quaternion rotation)
        {
            return TryCreateQuaternionFromForward(
                desiredForward,
                canonicalUp,
                convention,
                LocalEulerRotationCorrection.Zero,
                out rotation);
        }

        public static bool TryCreateQuaternionFromForward(
            Vector3 desiredForward,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            LocalEulerRotationCorrection correction,
            out Quaternion rotation)
        {
            return TryCreateQuaternionFromForward(
                desiredForward,
                canonicalUp,
                convention,
                correction.CreateQuaternion(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ),
                out rotation);
        }

        public static bool TryCreateQuaternionFromForward(
            Vector3 desiredForward,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            Quaternion correctionQuaternion,
            out Quaternion rotation)
        {
            if (convention == null)
            {
                throw new ArgumentNullException(nameof(convention));
            }

            rotation = Quaternion.Identity;

            Vector3 projectedForward = desiredForward - Vector3.Dot(desiredForward, canonicalUp) * canonicalUp;
            if (projectedForward.LengthSquared() < ForwardEpsilon)
            {
                return false;
            }

            Vector3 normalizedForward = Vector3.Normalize(projectedForward);
            Vector3 normalizedUp = Vector3.Normalize(canonicalUp);

            if (Math.Abs(correctionQuaternion.X) < 1e-9f &&
                Math.Abs(correctionQuaternion.Y) < 1e-9f &&
                Math.Abs(correctionQuaternion.Z) < 1e-9f &&
                Math.Abs(correctionQuaternion.W - 1.0f) < 1e-9f)
            {
                rotation = convention.CreateQuaternion(normalizedForward, normalizedUp);
                return true;
            }

            LocalAxisPoseBuilder.ApplyLocalPreRotation(
                convention,
                correctionQuaternion,
                out var correctedLocalForward,
                out var correctedLocalUp);
            rotation = LocalAxisPoseBuilder.CreateQuaternionFromLocalAxes(
                correctedLocalForward,
                correctedLocalUp,
                normalizedForward,
                normalizedUp);
            return true;
        }

        public static bool TryCreateQuaternionFromForwardWithWorldCorrection(
            Vector3 desiredForward,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            Quaternion worldCorrectionQuaternion,
            out Quaternion rotation)
        {
            rotation = Quaternion.Identity;

            if (!TryCreateQuaternionFromForward(
                    desiredForward,
                    canonicalUp,
                    convention,
                    out var baselineRotation))
            {
                return false;
            }

            if (Math.Abs(worldCorrectionQuaternion.X) < 1e-9f &&
                Math.Abs(worldCorrectionQuaternion.Y) < 1e-9f &&
                Math.Abs(worldCorrectionQuaternion.Z) < 1e-9f &&
                Math.Abs(worldCorrectionQuaternion.W - 1.0f) < 1e-9f)
            {
                rotation = baselineRotation;
                return true;
            }

            rotation = Quaternion.Normalize(worldCorrectionQuaternion * baselineRotation);
            return true;
        }
    }
}
