using System;
using System.Collections.Generic;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    public sealed class ObjectPassageProjectionOptimizationRequest
    {
        public ObjectPassageProjectionOptimizationRequest(
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 hostPathForward,
            Vector3 hostUp,
            double areaRelativeTieTolerance = 0.002,
            Quaternion? baselineHostRotation = null)
        {
            SizeX = sizeX;
            SizeY = sizeY;
            SizeZ = sizeZ;
            HostPathForward = hostPathForward;
            HostUp = hostUp;
            AreaRelativeTieTolerance = areaRelativeTieTolerance;
            BaselineHostRotation = Quaternion.Normalize(baselineHostRotation ?? Quaternion.Identity);
        }

        public double SizeX { get; }

        public double SizeY { get; }

        public double SizeZ { get; }

        public Vector3 HostPathForward { get; }

        public Vector3 HostUp { get; }

        public double AreaRelativeTieTolerance { get; }

        public Quaternion BaselineHostRotation { get; }

        /// <summary>
        /// 门型偏好：true=宽>高，false=高>宽
        /// </summary>
        public bool RequireWidthLarger { get; set; }

        /// <summary>
        /// 物体下方物流车辆高度（模型单位）
        /// </summary>
        public double VehicleHeightModelUnits { get; set; }
    }

    public sealed class ObjectPassageProjectionOptimizationResult
    {
        private ObjectPassageProjectionOptimizationResult(
            bool success,
            LocalEulerRotationCorrection correction,
            ObjectPassageProjectionScore score,
            string errorMessage,
            double? computedLiftOffsetModelUnits = null,
            double? aabbSx = null, double? aabbSy = null, double? aabbSz = null)
        {
            Success = success;
            Correction = correction;
            Score = score;
            ErrorMessage = errorMessage;
            ComputedLiftOffsetModelUnits = computedLiftOffsetModelUnits;
            AabbSx = aabbSx; AabbSy = aabbSy; AabbSz = aabbSz;
        }

        public bool Success { get; }

        public LocalEulerRotationCorrection Correction { get; }

        public ObjectPassageProjectionScore Score { get; }

        public string ErrorMessage { get; }

        /// <summary>
        /// 自动调整后计算出的上下偏移（模型单位），使物体底面贴合物流车高度。仅地面路径有效。
        /// </summary>
        public double? ComputedLiftOffsetModelUnits { get; }

        /// <summary>
        /// 优化器实测的 AABB 尺寸（模型单位）。
        /// </summary>
        public double? AabbSx { get; }
        public double? AabbSy { get; }
        public double? AabbSz { get; }

        public static ObjectPassageProjectionOptimizationResult CreateSuccess(
            LocalEulerRotationCorrection correction,
            ObjectPassageProjectionScore score,
            double? computedLiftOffsetModelUnits = null,
            double? aabbSx = null, double? aabbSy = null, double? aabbSz = null)
        {
            return new ObjectPassageProjectionOptimizationResult(true, correction, score, null, computedLiftOffsetModelUnits, aabbSx, aabbSy, aabbSz);
        }

        public static ObjectPassageProjectionOptimizationResult CreateFailure(string errorMessage)
        {
            return new ObjectPassageProjectionOptimizationResult(
                false,
                LocalEulerRotationCorrection.Zero,
                ObjectPassageProjectionScore.Invalid,
                errorMessage);
        }
    }

    public struct ObjectPassageProjectionScore
    {
        public static readonly ObjectPassageProjectionScore Invalid =
            new ObjectPassageProjectionScore(double.PositiveInfinity, double.PositiveInfinity);

        public ObjectPassageProjectionScore(double widthAcrossPath, double heightAlongHostUp)
        {
            WidthAcrossPath = widthAcrossPath;
            HeightAlongHostUp = heightAlongHostUp;
            Area = widthAcrossPath * heightAlongHostUp;
        }

        public double WidthAcrossPath { get; }

        public double HeightAlongHostUp { get; }

        public double Area { get; }
    }

    public static class ObjectPassageProjectionOptimizer
    {
        private const double Epsilon = 1e-9;
        private const int QuaternionSampleCount = 256;
        private static readonly int QuaternionRandomSeed = 42;

        public static ObjectPassageProjectionOptimizationResult Optimize(
            ObjectPassageProjectionOptimizationRequest request)
        {
            if (!TryValidateRequest(request, out string errorMessage))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
            }

            return OptimizeWithEvaluator(request, correction => Evaluate(request, correction));
        }

        public static ObjectPassageProjectionOptimizationResult OptimizeWithEvaluator(
            ObjectPassageProjectionOptimizationRequest request,
            Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator)
        {
            if (!TryValidateRequest(request, out string errorMessage))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure(errorMessage);
            }

            if (evaluator == null)
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure("自动调整评分器为空。");
            }

            LocalEulerRotationCorrection bestCorrection = LocalEulerRotationCorrection.Zero;
            // 必须评估 correction=Zero 作为初始候选：当 baseline 已对齐路径时，
            // Zero 就是最优解；若不评估，随机采样必然替换它，refine 也收敛不回 0。
            ObjectPassageProjectionScore bestScore = evaluator(bestCorrection);
            Quaternion bestQuaternion = Quaternion.Identity;

            // S³ 均匀采样四元数 → 转 Euler → 覆盖 SO(3) 无万向节死锁
            foreach (var (correction, quat) in UniformQuaternionCorrections(QuaternionSampleCount, QuaternionRandomSeed))
            {
                ObjectPassageProjectionScore score = evaluator(correction);
                if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance, request.RequireWidthLarger))
                {
                    bestScore = score;
                    bestCorrection = correction;
                    bestQuaternion = quat;
                }
            }

            RefineByQuaternionSearch(request, evaluator, ref bestCorrection, ref bestScore, ref bestQuaternion);

            if (double.IsInfinity(bestScore.Area))
            {
                return ObjectPassageProjectionOptimizationResult.CreateFailure("优化器未找到有效姿态。");
            }

            return ObjectPassageProjectionOptimizationResult.CreateSuccess(bestCorrection, bestScore);
        }

        public static ObjectPassageProjectionScore Evaluate(
            ObjectPassageProjectionOptimizationRequest request,
            LocalEulerRotationCorrection correction)
        {
            if (!TryCreateFrame(request, out Vector3 hostSide, out Vector3 hostUp))
            {
                throw new InvalidOperationException("路径方向退化，无法计算通行截面。");
            }

            Quaternion correctionRotation = correction.CreateQuaternion(Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ);
            Quaternion rotation = Quaternion.Normalize(correctionRotation * request.BaselineHostRotation);
            Vector3 rotatedHostX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, rotation));
            Vector3 rotatedHostY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, rotation));
            Vector3 rotatedHostZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, rotation));

            double width = ProjectExtent(
                request.SizeX,
                request.SizeY,
                request.SizeZ,
                rotatedHostX,
                rotatedHostY,
                rotatedHostZ,
                hostSide);

            double height = ProjectExtent(
                request.SizeX,
                request.SizeY,
                request.SizeZ,
                rotatedHostX,
                rotatedHostY,
                rotatedHostZ,
                hostUp);

            return new ObjectPassageProjectionScore(width, height);
        }

        private static bool TryValidateRequest(
            ObjectPassageProjectionOptimizationRequest request,
            out string errorMessage)
        {
            if (request == null)
            {
                errorMessage = "自动调整请求为空。";
                return false;
            }

            if (request.SizeX <= Epsilon || request.SizeY <= Epsilon || request.SizeZ <= Epsilon)
            {
                errorMessage = "物体尺寸无效。";
                return false;
            }

            if (!TryCreateFrame(request, out _, out _))
            {
                errorMessage = "路径方向退化，无法计算通行截面。";
                return false;
            }

            errorMessage = null;
            return true;
        }

        private static bool TryCreateFrame(
            ObjectPassageProjectionOptimizationRequest request,
            out Vector3 hostSide,
            out Vector3 hostUp)
        {
            hostSide = Vector3.Zero;
            hostUp = Vector3.Zero;

            if (request == null || request.HostUp.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostUp = Vector3.Normalize(request.HostUp);
            Vector3 hostPathForward = request.HostPathForward - hostUp * Vector3.Dot(request.HostPathForward, hostUp);
            if (hostPathForward.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostPathForward = Vector3.Normalize(hostPathForward);
            hostSide = Vector3.Cross(hostPathForward, hostUp);
            if (hostSide.LengthSquared() < Epsilon)
            {
                return false;
            }

            hostSide = Vector3.Normalize(hostSide);
            return true;
        }

        private static bool IsBetter(
            ObjectPassageProjectionScore candidate,
            ObjectPassageProjectionScore currentBest,
            double areaRelativeTieTolerance,
            bool? requireWidthLarger)
        {
            if (double.IsInfinity(currentBest.Area))
            {
                return true;
            }

            double tolerance = Math.Max(0.0, areaRelativeTieTolerance) * Math.Max(1.0, currentBest.Area);
            if (candidate.Area < currentBest.Area - tolerance)
            {
                return true;
            }

            if (Math.Abs(candidate.Area - currentBest.Area) <= tolerance)
            {
                // 面积平局 → 用门型约束选择
                if (requireWidthLarger.HasValue)
                {
                    bool candidateOk = requireWidthLarger.Value
                        ? candidate.WidthAcrossPath >= candidate.HeightAlongHostUp
                        : candidate.HeightAlongHostUp >= candidate.WidthAcrossPath;
                    bool currentOk = requireWidthLarger.Value
                        ? currentBest.WidthAcrossPath >= currentBest.HeightAlongHostUp
                        : currentBest.HeightAlongHostUp >= currentBest.WidthAcrossPath;
                    if (candidateOk && !currentOk) return true;
                    if (!candidateOk && currentOk) return false;
                }
                // 约束相同 → 选更小面积
                if (candidate.HeightAlongHostUp < currentBest.HeightAlongHostUp - Epsilon)
                {
                    return true;
                }

                if (Math.Abs(candidate.HeightAlongHostUp - currentBest.HeightAlongHostUp) <= Epsilon &&
                    candidate.WidthAcrossPath < currentBest.WidthAcrossPath - Epsilon)
                {
                    return true;
                }
            }

            return false;
        }

        private static double ProjectExtent(
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 rotatedHostX,
            Vector3 rotatedHostY,
            Vector3 rotatedHostZ,
            Vector3 targetAxis)
        {
            return Math.Abs(Vector3.Dot(rotatedHostX, targetAxis)) * sizeX +
                   Math.Abs(Vector3.Dot(rotatedHostY, targetAxis)) * sizeY +
                   Math.Abs(Vector3.Dot(rotatedHostZ, targetAxis)) * sizeZ;
        }

        private static IEnumerable<(LocalEulerRotationCorrection euler, Quaternion quat)> UniformQuaternionCorrections(int count, int seed)
        {
            var random = new Random(seed);

            for (int i = 0; i < count; i++)
            {
                double x = NextGaussian(random);
                double y = NextGaussian(random);
                double z = NextGaussian(random);
                double w = NextGaussian(random);

                double norm = Math.Sqrt(x * x + y * y + z * z + w * w);
                x /= norm; y /= norm; z /= norm; w /= norm;

                if (w < 0) { x = -x; y = -y; z = -z; w = -w; }

                var q = new Quaternion((float)x, (float)y, (float)z, (float)w);
                yield return (CanonicalQuaternionToHostEulerCorrection(q), q);
            }
        }

        public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q)
        {
            return CanonicalQuaternionToHostEulerCorrection(q, CoordinateSystemManager.Instance.ResolvedType);
        }

        /// <summary>
        /// 将 canonical 空间的四元数分解为宿主欧拉修正 (X, Y=up, Z=nonUp)。
        /// 接受显式 hostType 参数，避免依赖全局 CoordinateSystemManager 状态，便于单测。
        /// </summary>
        public static LocalEulerRotationCorrection CanonicalQuaternionToHostEulerCorrection(Quaternion q, CoordinateSystemType hostType)
        {
            double qw = q.W, qx = q.X, qy = q.Y, qz = q.Z;

            double sinr_cosp = 2.0 * (qw * qx + qy * qz);
            double cosr_cosp = 1.0 - 2.0 * (qx * qx + qy * qy);
            double roll = Math.Atan2(sinr_cosp, cosr_cosp);

            double sinp = 2.0 * (qw * qy - qz * qx);
            sinp = Math.Max(-1.0, Math.Min(1.0, sinp));
            double pitch = Math.Asin(sinp);

            double siny_cosp = 2.0 * (qw * qz + qx * qy);
            double cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz);
            double yaw_rad = Math.Atan2(siny_cosp, cosy_cosp);

            if (hostType == CoordinateSystemType.ZUp)
            {
                return new LocalEulerRotationCorrection(
                    roll * 180.0 / Math.PI,
                    pitch * 180.0 / Math.PI,
                    yaw_rad * 180.0 / Math.PI);
            }
            // YUp: canonical X→host X, canonical Z→host Y(up), canonical -Y→host Z(nonUp)
            return new LocalEulerRotationCorrection(
                roll * 180.0 / Math.PI,
                yaw_rad * 180.0 / Math.PI,
                -pitch * 180.0 / Math.PI);
        }

        private static double NextGaussian(Random random)
        {
            double u1 = 1.0 - random.NextDouble();
            double u2 = 1.0 - random.NextDouble();
            return Math.Sqrt(-2.0 * Math.Log(u1)) * Math.Sin(2.0 * Math.PI * u2);
        }

        private static void RefineByQuaternionSearch(
            ObjectPassageProjectionOptimizationRequest request,
            Func<LocalEulerRotationCorrection, ObjectPassageProjectionScore> evaluator,
            ref LocalEulerRotationCorrection bestCorrection,
            ref ObjectPassageProjectionScore bestScore,
            ref Quaternion bestQuaternion)
        {
            var axes = new[]
            {
                Vector3.UnitX, Vector3.UnitY, Vector3.UnitZ,
                Vector3.Normalize(new Vector3(1, 1, 0)),
                Vector3.Normalize(new Vector3(1, 0, 1)),
                Vector3.Normalize(new Vector3(0, 1, 1)),
                Vector3.Normalize(new Vector3(1, 1, 1))
            };

            double stepRadians = 22.5 * Math.PI / 180.0;
            while (stepRadians >= 0.125 * Math.PI / 180.0)
            {
                bool improved = false;
                foreach (var axis in axes)
                {
                    for (int sign = -1; sign <= 1; sign += 2)
                    {
                        double angle = sign * stepRadians;
                        var deltaQ = Quaternion.CreateFromAxisAngle(axis, (float)angle);
                        var candidateQ = Quaternion.Normalize(deltaQ * bestQuaternion);
                        var candidateEuler = CanonicalQuaternionToHostEulerCorrection(candidateQ);
                        ObjectPassageProjectionScore score = evaluator(candidateEuler);
                        if (IsBetter(score, bestScore, request.AreaRelativeTieTolerance, request.RequireWidthLarger))
                        {
                            bestCorrection = candidateEuler;
                            bestScore = score;
                            bestQuaternion = candidateQ;
                            improved = true;
                        }
                    }
                }

                if (!improved)
                {
                    stepRadians *= 0.5;
                }
            }
        }
    }
}
