using System;
using System.Numerics;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    public static class ObjectSpaceOrientationHelper
    {
        public static bool TryCalculateAxes(
            Vector3 segmentDirection,
            Vector3 upReference,
            out (Vector3 right, Vector3 up) axes,
            Vector3? horizontalDirection = null)
        {
            double segmentLength = Math.Sqrt(
                segmentDirection.X * segmentDirection.X +
                segmentDirection.Y * segmentDirection.Y +
                segmentDirection.Z * segmentDirection.Z);

            if (segmentLength < 1e-9)
            {
                axes = (Vector3.Zero, Vector3.Zero);
                return false;
            }

            axes = CalculateAxes(segmentDirection, upReference, horizontalDirection);
            return true;
        }

        public static (Vector3D right, Vector3D up) CalculateAxes(
            Point3D startPoint,
            Point3D endPoint,
            Vector3D upReference,
            Vector3D horizontalDirection = null)
        {
            var segmentDirection = new Vector3(
                (float)(endPoint.X - startPoint.X),
                (float)(endPoint.Y - startPoint.Y),
                (float)(endPoint.Z - startPoint.Z));

            var (right, up) = CalculateAxes(
                segmentDirection,
                ToNumerics(upReference),
                horizontalDirection == null ? (Vector3?)null : ToNumerics(horizontalDirection));

            return (ToNavisworks(right), ToNavisworks(up));
        }

        public static (Vector3 right, Vector3 up) CalculateAxes(
            Vector3 segmentDirection,
            Vector3 upReference,
            Vector3? horizontalDirection = null)
        {
            double segmentLength = Math.Sqrt(
                segmentDirection.X * segmentDirection.X +
                segmentDirection.Y * segmentDirection.Y +
                segmentDirection.Z * segmentDirection.Z);

            if (segmentLength < 1e-9)
            {
                throw new InvalidOperationException("路径段长度过小，无法计算通行空间方向。");
            }

            var normalizedDirection = Vector3.Normalize(segmentDirection);
            var normalizedUpReference = Normalize(upReference);
            var right = Cross(normalizedDirection, normalizedUpReference);

            double parallelToUp = Math.Abs(Vector3.Dot(normalizedDirection, normalizedUpReference));
            if (parallelToUp > 0.9)
            {
                if (horizontalDirection.HasValue)
                {
                    right = Cross(horizontalDirection.Value, normalizedUpReference);
                }
                else
                {
                    right = Math.Abs(normalizedUpReference.X) < 0.9f
                        ? Normalize(Cross(normalizedUpReference, new Vector3(1f, 0f, 0f)))
                        : Normalize(Cross(normalizedUpReference, new Vector3(0f, 1f, 0f)));
                }
            }

            right = Normalize(right);
            var up = Normalize(Cross(normalizedDirection, right));
            return (right, up);
        }

        private static Vector3 Cross(Vector3 a, Vector3 b)
        {
            return Vector3.Cross(a, b);
        }

        private static Vector3 Normalize(Vector3 vector)
        {
            if (vector.LengthSquared() < 1e-9f)
            {
                return Vector3.Zero;
            }

            return Vector3.Normalize(vector);
        }

        private static Vector3 ToNumerics(Vector3D vector)
        {
            return new Vector3((float)vector.X, (float)vector.Y, (float)vector.Z);
        }

        private static Vector3D ToNavisworks(Vector3 vector)
        {
            return new Vector3D(vector.X, vector.Y, vector.Z);
        }
    }
}
