using System;
using System.Numerics;
using NavisworksTransport.Utils;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 贴合地面后的最小截面 yaw 搜索器。
    /// 在贴地旋转(把用户选定的面旋到 -hostUp)基础上，绕宿主 Up 轴搜索一个 yaw，
    /// 使物体在"垂直路径方向 × 沿宿主 Up"截面上的投影面积最小。
    /// 与 ObjectPassageProjectionOptimizer 同目标，但只在 yaw 单自由度上搜索——
    /// 贴地旋转已固定俯仰/翻滚，只剩绕垂直轴的旋转自由度。
    /// 适用于 CAD 姿态天然歪斜、无法靠"假设 local +X 是长轴"对齐路径的物体。
    /// </summary>
    public static class AlignToGroundMinCrossSectionYawSearcher
    {
        private const double CoarseStepDegrees = 5.0;
        private const double FineStepDegrees = 1.0;
        private const float HorizontalEpsilon = 1e-8f;

        /// <summary>
        /// 在贴地旋转基础上绕宿主 Up 搜索 yaw，使物体截面投影面积最小。
        /// </summary>
        /// <param name="faceDownRotation">贴地旋转(把面法线旋到 -hostUp)，宿主世界空间。</param>
        /// <param name="cadRotation">CAD/复位姿态旋转，宿主世界空间。</param>
        /// <param name="sizeX">物体 local +X 方向尺寸(模型单位)。</param>
        /// <param name="sizeY">物体 local +Y 方向尺寸(模型单位)。</param>
        /// <param name="sizeZ">物体 local +Z 方向尺寸(模型单位)。</param>
        /// <param name="hostPathForward">路径起点方向(宿主世界)。</param>
        /// <param name="adapter">宿主坐标系适配器。</param>
        /// <param name="bestWidthAcrossPath">最佳 yaw 下垂直路径方向的宽度(模型单位)。</param>
        /// <param name="bestHeightAlongUp">最佳 yaw 下沿宿主 Up 的高度(模型单位)。</param>
        /// <param name="bestForwardAlongPath">最佳 yaw 下沿路径方向的长度(模型单位)。</param>
        /// <returns>合成后的宿主世界四元数(已归一化)：yawQ * faceDownRotation。</returns>
        public static Quaternion Search(
            Quaternion faceDownRotation,
            Quaternion cadRotation,
            double sizeX,
            double sizeY,
            double sizeZ,
            Vector3 hostPathForward,
            HostCoordinateAdapter adapter,
            out double bestWidthAcrossPath,
            out double bestHeightAlongUp,
            out double bestForwardAlongPath)
        {
            bestWidthAcrossPath = 0.0;
            bestHeightAlongUp = 0.0;
            bestForwardAlongPath = 0.0;

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

            Vector3 hostUp = adapter.HostUpVector3;

            // 路径方向投影到水平面，构造 hostSide(垂直路径方向，水平)
            Vector3 pathFwdHorizontal = hostPathForward - hostUp * Vector3.Dot(hostPathForward, hostUp);
            if (pathFwdHorizontal.LengthSquared() < HorizontalEpsilon)
            {
                // 纯垂直路径：无水平方向，yaw 无意义，保持贴地姿态
                return faceDownRotation;
            }
            pathFwdHorizontal = Vector3.Normalize(pathFwdHorizontal);
            Vector3 hostSide = Vector3.Normalize(Vector3.Cross(pathFwdHorizontal, hostUp));

            // 物体最终姿态 = yawQ * faceDown * cad
            // local 轴在宿主世界的方向 = totalRotation * UnitX/Y/Z
            // 在该姿态下，截面 = 沿 hostSide 的宽度 × 沿 hostUp 的高度
            double bestYawRad = 0.0;
            double bestArea = double.MaxValue;

            // 粗搜 [0, 360°)，步长 5°
            for (double deg = 0.0; deg < 360.0; deg += CoarseStepDegrees)
            {
                double yawRad = deg * Math.PI / 180.0;
                double area = EvaluateCrossSectionArea(
                    faceDownRotation, cadRotation, yawRad,
                    pathFwdHorizontal, hostSide, hostUp, sizeX, sizeY, sizeZ,
                    out double width, out double height, out double forward);
                if (area < bestArea)
                {
                    bestArea = area;
                    bestYawRad = yawRad;
                    bestWidthAcrossPath = width;
                    bestHeightAlongUp = height;
                    bestForwardAlongPath = forward;
                }
            }

            // 细搜 [best - step, best + step]，步长 1°
            double loDeg = (bestYawRad * 180.0 / Math.PI) - CoarseStepDegrees;
            double hiDeg = (bestYawRad * 180.0 / Math.PI) + CoarseStepDegrees;
            for (double deg = loDeg; deg <= hiDeg; deg += FineStepDegrees)
            {
                double yawRad = deg * Math.PI / 180.0;
                double area = EvaluateCrossSectionArea(
                    faceDownRotation, cadRotation, yawRad,
                    pathFwdHorizontal, hostSide, hostUp, sizeX, sizeY, sizeZ,
                    out double width, out double height, out double forward);
                if (area < bestArea)
                {
                    bestArea = area;
                    bestYawRad = yawRad;
                    bestWidthAcrossPath = width;
                    bestHeightAlongUp = height;
                    bestForwardAlongPath = forward;
                }
            }

            return Quaternion.Normalize(
                Quaternion.CreateFromAxisAngle(hostUp, (float)bestYawRad) * faceDownRotation);
        }

        /// <summary>
        /// 计算给定 deltaYaw 下物体截面投影面积。
        /// 复用 ObjectPassageProjectionOptimizer.ProjectExtent 的 AABB 投影公式：
        ///   width   = Σ |localAxis · hostSide|   * sizeAxis  (垂直路径方向宽度)
        ///   height  = Σ |localAxis · hostUp|     * sizeAxis  (沿宿主 Up 高度)
        ///   forward = Σ |localAxis · pathForward| * sizeAxis  (沿路径方向长度)
        ///   area    = width * height
        /// </summary>
        private static double EvaluateCrossSectionArea(
            Quaternion faceDownRotation,
            Quaternion cadRotation,
            double deltaYawRadians,
            Vector3 pathForward,
            Vector3 hostSide,
            Vector3 hostUp,
            double sizeX,
            double sizeY,
            double sizeZ,
            out double widthAcrossPath,
            out double heightAlongUp,
            out double forwardAlongPath)
        {
            Quaternion yawQ = Quaternion.CreateFromAxisAngle(hostUp, (float)deltaYawRadians);
            Quaternion total = Quaternion.Normalize(yawQ * faceDownRotation * cadRotation);

            Vector3 localX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, total));
            Vector3 localY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, total));
            Vector3 localZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, total));

            widthAcrossPath =
                Math.Abs(Vector3.Dot(localX, hostSide)) * sizeX +
                Math.Abs(Vector3.Dot(localY, hostSide)) * sizeY +
                Math.Abs(Vector3.Dot(localZ, hostSide)) * sizeZ;
            heightAlongUp =
                Math.Abs(Vector3.Dot(localX, hostUp)) * sizeX +
                Math.Abs(Vector3.Dot(localY, hostUp)) * sizeY +
                Math.Abs(Vector3.Dot(localZ, hostUp)) * sizeZ;
            forwardAlongPath =
                Math.Abs(Vector3.Dot(localX, pathForward)) * sizeX +
                Math.Abs(Vector3.Dot(localY, pathForward)) * sizeY +
                Math.Abs(Vector3.Dot(localZ, pathForward)) * sizeZ;

            return widthAcrossPath * heightAlongUp;
        }

        /// <summary>
        /// 把贴地搜索后的绝对宿主姿态(overrideQ)转成"相对于路径 baseline 的 host 欧拉修正"，
        /// 与自动调整返回的 correction 语义一致，供动画系统在 baseline 之上叠加。
        ///
        /// 关键：PathAnimationManager 增量链从 CAD 姿态出发：
        ///   finalQ = qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) * cadQ
        /// 贴地时 finalQ = overrideQ * cadQ，所以：
        ///   qup(pathYaw + YDeg - currentYaw) * qnonUp(ZDeg) * qx(XDeg) = overrideQ
        /// 而 CreateHostRotationCorrection(correction) = qup(YDeg) * qnonUp(ZDeg) * qx(XDeg)，所以：
        ///   qup(pathYaw - currentYaw) * CreateHostRotationCorrection(correction) = overrideQ
        ///   => correctionHostQ = qup(currentYaw - pathYaw) * overrideQ
        ///
        /// currentYaw 是 CAD 姿态的 canonical yaw，从 cadQ 提取（与 GetYawFromRotation 一致）。
        public static LocalEulerRotationCorrection ComposeHostCorrection(
            Quaternion absoluteHostQ,
            Quaternion cadRotation,
            double pathYawRadians,
            HostCoordinateAdapter adapter)
        {
            if (adapter == null)
            {
                throw new ArgumentNullException(nameof(adapter));
            }

            double currentYaw = ExtractCanonicalYaw(cadRotation, adapter);
            Quaternion preRotation = Quaternion.CreateFromAxisAngle(
                adapter.HostUpVector3, (float)(currentYaw - pathYawRadians));
            Quaternion correctionHostQ = Quaternion.Normalize(preRotation * absoluteHostQ);

            Quaternion correctionCanonQ = HostQuaternionToCanonical(correctionHostQ, adapter);
            return ObjectPassageProjectionOptimizer.CanonicalQuaternionToHostEulerCorrection(
                correctionCanonQ, adapter.HostType);
        }

        /// <summary>
        /// 从宿主空间四元数提取 canonical yaw（Atan2(canonicalForward.Y, canonicalForward.X)）。
        /// 与 ModelItemTransformHelper.GetYawFromRotation 一致：取 forward 轴(X列)，转 canonical，投影 XY 平面。
        /// </summary>
        private static double ExtractCanonicalYaw(Quaternion hostQ, HostCoordinateAdapter adapter)
        {
            Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);
            Vector3 hostForward = new Vector3(hostLinear.M11, hostLinear.M21, hostLinear.M31);
            Vector3 canonFwd = adapter.ToCanonicalVector3(hostForward);
            canonFwd.Z = 0f;
            if (canonFwd.LengthSquared() < 1e-9f)
            {
                return 0.0;
            }
            canonFwd = Vector3.Normalize(canonFwd);
            return Math.Atan2(canonFwd.Y, canonFwd.X);
        }

        /// <summary>
        /// 宿主空间四元数 → canonical 空间四元数。
        /// 旋转的坐标系转换必须用相似变换 R_canon = M⁻¹ · R_host · M
        /// （M = canonical→host 坐标变换矩阵，正交故 M⁻¹ = Mᵀ）。
        /// 不能用列向量映射——那只在"基向量定义"场景成立，会破坏 Identity 不变性。
        /// </summary>
        public static Quaternion HostQuaternionToCanonical(Quaternion hostQ, HostCoordinateAdapter adapter)
        {
            Matrix4x4 hostLinear = Matrix4x4.CreateFromQuaternion(hostQ);

            // M: canonical→host 坐标变换（列 = canonical 基在 host 中的表示）
            Vector3 mColX = adapter.FromCanonicalVector3(Vector3.UnitX);
            Vector3 mColY = adapter.FromCanonicalVector3(Vector3.UnitY);
            Vector3 mColZ = adapter.FromCanonicalVector3(Vector3.UnitZ);
            Matrix4x4 m = new Matrix4x4(
                mColX.X, mColY.X, mColZ.X, 0f,
                mColX.Y, mColY.Y, mColZ.Y, 0f,
                mColX.Z, mColY.Z, mColZ.Z, 0f,
                0f, 0f, 0f, 1f);
            // M 正交，M⁻¹ = Mᵀ
            Matrix4x4 mInv = Matrix4x4.Transpose(m);
            // R_canon = M⁻¹ · R_host · M
            Matrix4x4 canonLinear = Matrix4x4.Multiply(Matrix4x4.Multiply(mInv, hostLinear), m);
            return Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(canonLinear));
        }
    }
}
