using System;
using System.Collections.Generic;
using System.Linq;
using System.Numerics;
using Autodesk.Navisworks.Api;
using ComApi = Autodesk.Navisworks.Api.Interop.ComApi;
using ComApiBridge = Autodesk.Navisworks.Api.ComApi.ComApiBridge;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// Fragment 代表姿态辅助工具。
    /// 
    /// 语义：
    /// - 从 fragment 的 world 矩阵中提取旋转
    /// - 对多个 fragment 旋转做同半球加权平均
    /// - 返回一个可作为真实物体参考位姿的代表旋转
    /// </summary>
    public static class FragmentRepresentativePoseHelper
    {
        private const float AxisEpsilon = 1e-6f;

        public readonly struct RepresentativeFrame
        {
            public Vector3 AxisX { get; }
            public Vector3 AxisY { get; }
            public Vector3 AxisZ { get; }
            public Quaternion Rotation { get; }

            public RepresentativeFrame(Vector3 axisX, Vector3 axisY, Vector3 axisZ, Quaternion rotation)
            {
                AxisX = axisX;
                AxisY = axisY;
                AxisZ = axisZ;
                Rotation = rotation;
            }
        }

        /// <summary>
        /// 从 ModelItem 的 fragment 几何中提取代表旋转。
        /// </summary>
        public static bool TryGetRepresentativeRotation(ModelItem item, out Quaternion representativeRotation)
        {
            representativeRotation = Quaternion.Identity;

            if (item == null)
            {
                return false;
            }

            try
            {
                var selection = ComApiBridge.ToInwOpSelection(new ModelItemCollection { item });
                var fragmentInfos = GeometryHelper.GetAllFragments(selection);
                return TryGetRepresentativeRotation(
                    fragmentInfos.Select(fragmentInfo => fragmentInfo.TransformMatrix),
                    out representativeRotation);
            }
            catch (Exception ex)
            {
                LogManager.Warning($"[fragment代表姿态] 从ModelItem提取失败: {ex.Message}");
                return false;
            }
        }

        /// <summary>
        /// 从 fragment 矩阵集合中提取代表旋转。
        /// </summary>
        public static bool TryGetRepresentativeRotation(
            IEnumerable<double[]> fragmentMatrices,
            out Quaternion representativeRotation)
        {
            representativeRotation = Quaternion.Identity;

            if (fragmentMatrices == null)
            {
                return false;
            }

            var fragmentRotations = new List<Quaternion>();
            foreach (double[] fragmentMatrix in fragmentMatrices)
            {
                if (TryExtractRotationFromFragmentMatrix(fragmentMatrix, out Quaternion fragmentRotation))
                {
                    fragmentRotations.Add(fragmentRotation);
                }
            }

            return TryGetRepresentativeRotation(fragmentRotations, out representativeRotation);
        }

        public static bool TryGetRepresentativeFrame(
            IEnumerable<double[]> fragmentMatrices,
            out RepresentativeFrame representativeFrame)
        {
            representativeFrame = default;

            if (fragmentMatrices == null)
            {
                return false;
            }

            var rotations = new List<Quaternion>();
            foreach (double[] fragmentMatrix in fragmentMatrices)
            {
                if (TryExtractRotationFromFragmentMatrix(fragmentMatrix, out Quaternion fragmentRotation))
                {
                    rotations.Add(fragmentRotation);
                }
            }

            if (!TryGetRepresentativeRotation(rotations, out Quaternion representativeRotation))
            {
                return false;
            }

            Vector3 axisX = Vector3.Normalize(Vector3.Transform(Vector3.UnitX, representativeRotation));
            Vector3 axisY = Vector3.Normalize(Vector3.Transform(Vector3.UnitY, representativeRotation));
            Vector3 axisZ = Vector3.Normalize(Vector3.Transform(Vector3.UnitZ, representativeRotation));
            representativeFrame = new RepresentativeFrame(axisX, axisY, axisZ, representativeRotation);
            return true;
        }

        public static bool TryInterpretRepresentativeFrame(
            RepresentativeFrame rawFrame,
            string fragmentDefaultUpAxis,
            string hostUpAxis,
            out RepresentativeFrame interpretedFrame)
        {
            interpretedFrame = default;

            bool hostIsYUp = string.Equals(hostUpAxis, "Y", StringComparison.OrdinalIgnoreCase);
            bool fragmentIsYUp = string.Equals(fragmentDefaultUpAxis, "Y", StringComparison.OrdinalIgnoreCase);

            Vector3 semanticAxisX = rawFrame.AxisX;
            Vector3 semanticAxisY;
            Vector3 semanticAxisZ;

            if (hostIsYUp == fragmentIsYUp)
            {
                semanticAxisY = rawFrame.AxisY;
                semanticAxisZ = rawFrame.AxisZ;
            }
            else if (hostIsYUp)
            {
                // fragment 按 ZUp 解释，宿主按 YUp 解释：X 保持，Y 取原始 Z，Z 由右手系重建。
                semanticAxisY = rawFrame.AxisZ;
                semanticAxisZ = Vector3.Normalize(Vector3.Cross(semanticAxisX, semanticAxisY));
            }
            else
            {
                // fragment 按 YUp 解释，宿主按 ZUp 解释：X 保持，Z 取原始 Y，Y 由右手系重建。
                semanticAxisZ = rawFrame.AxisY;
                semanticAxisY = Vector3.Normalize(Vector3.Cross(semanticAxisZ, semanticAxisX));
            }

            if (!TryNormalize(semanticAxisX, out semanticAxisX) ||
                !TryNormalize(semanticAxisY, out semanticAxisY) ||
                !TryNormalize(semanticAxisZ, out semanticAxisZ))
            {
                return false;
            }

            // 用解释后的三轴重新正交化，保证右手系稳定。
            Vector3 axisX = semanticAxisX;
            Vector3 axisY = semanticAxisY - Vector3.Dot(semanticAxisY, axisX) * axisX;
            if (!TryNormalize(axisY, out axisY))
            {
                return false;
            }

            Vector3 axisZ = Vector3.Cross(axisX, axisY);
            if (!TryNormalize(axisZ, out axisZ))
            {
                return false;
            }

            if (hostIsYUp && Vector3.Dot(axisY, semanticAxisY) < 0.0f)
            {
                axisY = -axisY;
                axisZ = -axisZ;
            }
            else if (!hostIsYUp && Vector3.Dot(axisZ, semanticAxisZ) < 0.0f)
            {
                axisY = -axisY;
                axisZ = -axisZ;
            }

            Quaternion rotation = Quaternion.Normalize(CreateQuaternionFromBasis(axisX, axisY, axisZ));
            interpretedFrame = new RepresentativeFrame(axisX, axisY, axisZ, rotation);
            return true;
        }

        /// <summary>
        /// 从 fragment 旋转集合中提取代表旋转。
        /// </summary>
        public static bool TryGetRepresentativeRotation(
            IEnumerable<Quaternion> fragmentRotations,
            out Quaternion representativeRotation)
        {
            representativeRotation = Quaternion.Identity;

            if (fragmentRotations == null)
            {
                return false;
            }

            var rotations = fragmentRotations
                .Where(rotation => TryNormalize(rotation, out _))
                .Select(rotation => Quaternion.Normalize(rotation))
                .ToList();

            if (rotations.Count == 0)
            {
                return false;
            }

            Quaternion seed = rotations[0];
            Vector4 sum = Vector4.Zero;
            foreach (Quaternion rotation in rotations)
            {
                Quaternion aligned = Quaternion.Dot(seed, rotation) < 0.0f
                    ? new Quaternion(-rotation.X, -rotation.Y, -rotation.Z, -rotation.W)
                    : rotation;

                sum += new Vector4(aligned.X, aligned.Y, aligned.Z, aligned.W);
            }

            if (sum.LengthSquared() < AxisEpsilon)
            {
                return false;
            }

            representativeRotation = Quaternion.Normalize(new Quaternion(sum.X, sum.Y, sum.Z, sum.W));
            return true;
        }

        /// <summary>
        /// 从 fragment 的 4x4 world matrix 中提取旋转。
        /// 仅使用旋转部分，忽略平移。
        /// </summary>
        public static bool TryExtractRotationFromFragmentMatrix(double[] fragmentMatrix, out Quaternion rotation)
        {
            rotation = Quaternion.Identity;

            if (fragmentMatrix == null || fragmentMatrix.Length != 16)
            {
                return false;
            }

            Vector3 axisX = new Vector3(
                (float)fragmentMatrix[0],
                (float)fragmentMatrix[1],
                (float)fragmentMatrix[2]);
            Vector3 axisY = new Vector3(
                (float)fragmentMatrix[4],
                (float)fragmentMatrix[5],
                (float)fragmentMatrix[6]);
            Vector3 axisZ = new Vector3(
                (float)fragmentMatrix[8],
                (float)fragmentMatrix[9],
                (float)fragmentMatrix[10]);

            if (!TryNormalize(axisX, out axisX) ||
                !TryNormalize(axisY, out axisY) ||
                !TryNormalize(axisZ, out axisZ))
            {
                return false;
            }

            // 先将三个轴正交化，避免 fragment 矩阵里存在轻微数值误差或尺度干扰。
            Vector3 orthogonalY = axisY - Vector3.Dot(axisY, axisX) * axisX;
            if (!TryNormalize(orthogonalY, out orthogonalY))
            {
                orthogonalY = Vector3.Normalize(Vector3.Cross(axisZ, axisX));
            }

            Vector3 orthogonalZ = Vector3.Cross(axisX, orthogonalY);
            if (!TryNormalize(orthogonalZ, out orthogonalZ))
            {
                return false;
            }

            orthogonalY = Vector3.Normalize(Vector3.Cross(orthogonalZ, axisX));

            rotation = Quaternion.Normalize(CreateQuaternionFromBasis(axisX, orthogonalY, orthogonalZ));
            return true;
        }

        private static Quaternion CreateQuaternionFromBasis(Vector3 axisX, Vector3 axisY, Vector3 axisZ)
        {
            float m00 = axisX.X;
            float m01 = axisY.X;
            float m02 = axisZ.X;
            float m10 = axisX.Y;
            float m11 = axisY.Y;
            float m12 = axisZ.Y;
            float m20 = axisX.Z;
            float m21 = axisY.Z;
            float m22 = axisZ.Z;

            float trace = m00 + m11 + m22;
            float qx;
            float qy;
            float qz;
            float qw;

            if (trace > 0.0f)
            {
                float s = (float)Math.Sqrt(trace + 1.0f) * 2.0f;
                qw = 0.25f * s;
                qx = (m21 - m12) / s;
                qy = (m02 - m20) / s;
                qz = (m10 - m01) / s;
            }
            else if (m00 > m11 && m00 > m22)
            {
                float s = (float)Math.Sqrt(1.0f + m00 - m11 - m22) * 2.0f;
                qw = (m21 - m12) / s;
                qx = 0.25f * s;
                qy = (m01 + m10) / s;
                qz = (m02 + m20) / s;
            }
            else if (m11 > m22)
            {
                float s = (float)Math.Sqrt(1.0f + m11 - m00 - m22) * 2.0f;
                qw = (m02 - m20) / s;
                qx = (m01 + m10) / s;
                qy = 0.25f * s;
                qz = (m12 + m21) / s;
            }
            else
            {
                float s = (float)Math.Sqrt(1.0f + m22 - m00 - m11) * 2.0f;
                qw = (m10 - m01) / s;
                qx = (m02 + m20) / s;
                qy = (m12 + m21) / s;
                qz = 0.25f * s;
            }

            return new Quaternion(qx, qy, qz, qw);
        }

        private static bool TryNormalize(Quaternion value, out Quaternion normalized)
        {
            float lengthSquared = value.X * value.X + value.Y * value.Y + value.Z * value.Z + value.W * value.W;
            if (lengthSquared < AxisEpsilon)
            {
                normalized = Quaternion.Identity;
                return false;
            }

            normalized = Quaternion.Normalize(value);
            return true;
        }

        private static bool TryNormalize(Vector3 value, out Vector3 normalized)
        {
            if (value.LengthSquared() < AxisEpsilon)
            {
                normalized = Vector3.Zero;
                return false;
            }

            normalized = Vector3.Normalize(value);
            return true;
        }
    }
}
