using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 在内部 Canonical Space 中构造 Rail 姿态。
    /// 这一层只处理纯数学切向/法向/局部轴约定，不依赖 Navisworks API。
    /// </summary>
    public static class CanonicalRailPoseBuilder
    {
        private const float TangentEpsilon = 1e-6f;

        public static bool TryCreateLocalFrame(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            out RailLocalFrame frame)
        {
            return TryCreateLocalFrame(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                null,
                out frame);
        }

        public static bool TryCreateLocalFrame(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            Vector3? preferredNormal,
            out RailLocalFrame frame)
        {
            frame = null;

            if (!TryCreateBasis(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                preferredNormal,
                out var forward,
                out var lateral,
                out var up))
            {
                return false;
            }

            frame = new RailLocalFrame(forward, lateral, up);
            return true;
        }

        public static bool TryCreateBasis(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            out Vector3 forward,
            out Vector3 lateral,
            out Vector3 up)
        {
            return TryCreateBasis(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                null,
                out forward,
                out lateral,
                out up);
        }

        public static bool TryCreateBasis(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            Vector3? preferredNormal,
            out Vector3 forward,
            out Vector3 lateral,
            out Vector3 up)
        {
            forward = default;
            lateral = default;
            up = default;

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

            if (tangent.LengthSquared() < TangentEpsilon)
            {
                return false;
            }

            forward = Vector3.Normalize(tangent);
            Vector3 referenceUp = canonicalUp;
            if (preferredNormal.HasValue && preferredNormal.Value.LengthSquared() >= TangentEpsilon)
            {
                referenceUp = Vector3.Normalize(preferredNormal.Value);
            }

            Vector3 projectedUp = referenceUp - Vector3.Dot(referenceUp, forward) * forward;
            if (projectedUp.LengthSquared() < TangentEpsilon)
            {
                projectedUp = Math.Abs(forward.Z) < 0.9f
                    ? Vector3.UnitZ
                    : Vector3.UnitY;
                projectedUp = projectedUp - Vector3.Dot(projectedUp, forward) * forward;
            }

            if (projectedUp.LengthSquared() < TangentEpsilon)
            {
                return false;
            }

            up = Vector3.Normalize(projectedUp);
            lateral = Vector3.Normalize(Vector3.Cross(up, forward));
            up = Vector3.Normalize(Vector3.Cross(forward, lateral));
            return true;
        }

        public static bool TryCreateQuaternion(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            out Quaternion rotation)
        {
            return TryCreateQuaternion(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                convention,
                null,
                0.0,
                out rotation);
        }

        public static bool TryCreateQuaternion(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            double localUpRotationDegrees,
            out Quaternion rotation)
        {
            return TryCreateQuaternion(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                convention,
                null,
                Quaternion.CreateFromAxisAngle(Vector3.Normalize(convention.UpUnitVector), (float)(localUpRotationDegrees * Math.PI / 180.0)),
                out rotation);
        }

        public static bool TryCreateQuaternion(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            LocalEulerRotationCorrection correction,
            out Quaternion rotation)
        {
            return TryCreateQuaternion(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                convention,
                null,
                correction.CreateQuaternion(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ),
                out rotation);
        }

        public static bool TryCreateQuaternion(
            Vector3 previousPoint,
            Vector3 currentPoint,
            Vector3 nextPoint,
            Vector3 canonicalUp,
            ModelAxisConvention convention,
            Vector3? preferredNormal,
            double localUpRotationDegrees,
            out Quaternion rotation)
        {
            return TryCreateQuaternion(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                convention,
                preferredNormal,
                Quaternion.CreateFromAxisAngle(Vector3.Normalize(convention.UpUnitVector), (float)(localUpRotationDegrees * Math.PI / 180.0)),
                out rotation);
        }

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

            if (convention == null)
            {
                throw new ArgumentNullException(nameof(convention));
            }

            if (!TryCreateBasis(
                previousPoint,
                currentPoint,
                nextPoint,
                canonicalUp,
                preferredNormal,
                out var forward,
                out _,
                out var up))
            {
                return false;
            }

            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(forward, up);
                return true;
            }

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