using System;
using System.Numerics;

namespace NavisworksTransport.Utils.CoordinateSystem
{
    /// <summary>
    /// 根据模型局部轴和世界目标 forward/up 构造姿态。
    /// </summary>
    public static class LocalAxisPoseBuilder
    {
        public static Quaternion CreateQuaternionFromLocalAxes(
            Vector3 localForward,
            Vector3 localUp,
            Vector3 worldForward,
            Vector3 worldUp)
        {
            Vector3 normalizedLocalForward = Vector3.Normalize(localForward);
            Vector3 normalizedLocalUp = Vector3.Normalize(localUp);
            Vector3 normalizedWorldForward = Vector3.Normalize(worldForward);
            Vector3 normalizedWorldUp = Vector3.Normalize(worldUp);

            Vector3 localRemaining = Vector3.Normalize(Vector3.Cross(normalizedLocalForward, normalizedLocalUp));
            Vector3 worldRemaining = Vector3.Normalize(Vector3.Cross(normalizedWorldForward, normalizedWorldUp));

            Vector3 worldX = MapOriginalLocalAxis(Vector3.UnitX, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);
            Vector3 worldY = MapOriginalLocalAxis(Vector3.UnitY, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);
            Vector3 worldZ = MapOriginalLocalAxis(Vector3.UnitZ, normalizedLocalForward, normalizedLocalUp, localRemaining, normalizedWorldForward, normalizedWorldUp, worldRemaining);

            Matrix4x4 linear = new Matrix4x4(
                worldX.X, worldY.X, worldZ.X, 0f,
                worldX.Y, worldY.Y, worldZ.Y, 0f,
                worldX.Z, worldY.Z, worldZ.Z, 0f,
                0f, 0f, 0f, 1f);
            return Quaternion.CreateFromRotationMatrix(linear);
        }

        public static void ApplyLocalPreRotation(
            ModelAxisConvention convention,
            Quaternion correctionQuaternion,
            out Vector3 correctedLocalForward,
            out Vector3 correctedLocalUp)
        {
            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)
            {
                correctedLocalForward = convention.ForwardUnitVector;
                correctedLocalUp = convention.UpUnitVector;
                return;
            }

            correctedLocalForward = Vector3.Normalize(Vector3.Transform(convention.ForwardUnitVector, correctionQuaternion));
            correctedLocalUp = Vector3.Normalize(Vector3.Transform(convention.UpUnitVector, correctionQuaternion));
        }

        private static Vector3 MapOriginalLocalAxis(
            Vector3 originalAxis,
            Vector3 localForward,
            Vector3 localUp,
            Vector3 localRemaining,
            Vector3 worldForward,
            Vector3 worldUp,
            Vector3 worldRemaining)
        {
            float forwardWeight = Vector3.Dot(originalAxis, localForward);
            float upWeight = Vector3.Dot(originalAxis, localUp);
            float remainingWeight = Vector3.Dot(originalAxis, localRemaining);

            return Vector3.Normalize(
                forwardWeight * worldForward +
                upWeight * worldUp +
                remainingWeight * worldRemaining);
        }
    }
}
