using System;
using System.Numerics;
using Autodesk.Navisworks.Api;
using NavisworksTransport.Utils.CoordinateSystem;

namespace NavisworksTransport.Utils
{
    /// <summary>
    /// Rail 路径姿态辅助工具。
    /// LegacySuspensionPoint 模式下保持世界 Z 偏移；
    /// RailCenterLine 模式下按路径切线推导局部法向，支持斜轨的轨上/轨下偏移。
    /// </summary>
    public static class RailPathPoseHelper
    {
        private const double TangentEpsilon = 1e-9;
        private static bool _rotationConstructorConventionLogged;

        /// <summary>
        /// 获取 Rail 参考点到构件对接点的有符号偏移（模型单位）。
        /// LegacySuspensionPoint 模式下，路径点本身就是构件对接点，因此偏移为 0。
        /// </summary>
        public static double GetAnchorOffset(PathRoute route)
        {
            if (route == null || route.PathType != PathType.Rail)
            {
                return 0.0;
            }

            if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
            {
                return 0.0;
            }

            return route.RailMountMode == RailMountMode.OverRail
                ? route.RailReferenceToAnchorOffset
                : -route.RailReferenceToAnchorOffset;
        }

        /// <summary>
        /// 计算构件底面中心相对于 Rail 参考点的 Z 偏移（模型单位）。
        /// </summary>
        public static double GetBottomZOffset(PathRoute route, double objectHeight)
        {
            double anchorOffset = GetAnchorOffset(route);

            if (route == null || route.PathType != PathType.Rail)
            {
                return 0.0;
            }

            if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode))
            {
                return anchorOffset - objectHeight;
            }

            return anchorOffset;
        }

        /// <summary>
        /// 计算通行空间中心相对于 Rail 参考点的 Z 偏移（模型单位）。
        /// 顶面对接时，通行空间中心位于对接点下方半高；
        /// 底面对接时，通行空间中心位于对接点上方半高。
        /// </summary>
        public static double GetObjectSpaceCenterZOffset(PathRoute route, double objectSpaceHeight)
        {
            double anchorOffset = GetAnchorOffset(route);

            if (route == null || route.PathType != PathType.Rail)
            {
                return 0.0;
            }

            if (PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode))
            {
                return anchorOffset - objectSpaceHeight / 2.0;
            }

            return anchorOffset + objectSpaceHeight / 2.0;
        }

        /// <summary>
        /// 根据 Rail 路径参考点计算构件底面中心位置。
        /// </summary>
        public static Point3D ResolveBottomPosition(PathRoute route, Point3D referencePoint, double objectHeight)
        {
            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
            var canonicalUp = HostCoordinateAdapter.CanonicalUp;
            double bottomOffset = GetBottomOffsetMagnitude(route, objectHeight);

            var canonicalBottomPoint = new Point3D(
                canonicalReferencePoint.X + canonicalUp.X * bottomOffset,
                canonicalReferencePoint.Y + canonicalUp.Y * bottomOffset,
                canonicalReferencePoint.Z + canonicalUp.Z * bottomOffset);

            return adapter.FromCanonicalPoint(canonicalBottomPoint);
        }

        /// <summary>
        /// 根据 Rail 路径参考点和相邻点计算构件底面中心位置。
        /// RailCenterLine 模式下沿轨道局部法向偏移，适用于斜轨。
        /// </summary>
        public static Point3D ResolveBottomPosition(
            PathRoute route,
            Point3D referencePoint,
            Point3D previousPoint,
            Point3D nextPoint,
            double objectHeight)
        {
            if (route == null || route.PathType != PathType.Rail)
            {
                return referencePoint;
            }

            if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
            {
                return ResolveBottomPosition(route, referencePoint, objectHeight);
            }

            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
            var normal = ResolveRailNormal(previousPoint, referencePoint, nextPoint);
            double bottomOffset = GetBottomOffsetMagnitude(route, objectHeight);

            var canonicalBottomPoint = new Point3D(
                canonicalReferencePoint.X + normal.X * bottomOffset,
                canonicalReferencePoint.Y + normal.Y * bottomOffset,
                canonicalReferencePoint.Z + normal.Z * bottomOffset);

            return adapter.FromCanonicalPoint(canonicalBottomPoint);
        }

        /// <summary>
        /// 根据 Rail 路径参考点和相邻点计算通行空间中心位置。
        /// </summary>
        public static Point3D ResolveObjectSpaceCenterPosition(
            PathRoute route,
            Point3D referencePoint,
            Point3D previousPoint,
            Point3D nextPoint,
            double objectSpaceHeight)
        {
            if (route == null || route.PathType != PathType.Rail)
            {
                return referencePoint;
            }

            double centerOffset = GetObjectSpaceCenterOffsetMagnitude(route, objectSpaceHeight);

            if (route.RailPathDefinitionMode == RailPathDefinitionMode.LegacySuspensionPoint)
            {
                var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
                var canonicalReferencePoint = adapter.ToCanonicalPoint(referencePoint);
                var canonicalCenterPoint = new Point3D(
                    canonicalReferencePoint.X,
                    canonicalReferencePoint.Y,
                    canonicalReferencePoint.Z + centerOffset);
                return adapter.FromCanonicalPoint(canonicalCenterPoint);
            }

            var adapterForRail = CoordinateSystemManager.Instance.CreateHostAdapter();
            var canonicalReferencePointForRail = adapterForRail.ToCanonicalPoint(referencePoint);
            var normal = ResolveRailNormal(previousPoint, referencePoint, nextPoint);
            var canonicalCenterPointForRail = new Point3D(
                canonicalReferencePointForRail.X + normal.X * centerOffset,
                canonicalReferencePointForRail.Y + normal.Y * centerOffset,
                canonicalReferencePointForRail.Z + normal.Z * centerOffset);
            return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRail);
        }

        /// <summary>
        /// 根据轨道路径切向和法向创建 Rail 构件完整三维姿态。
        /// 约定：构件本地 X 轴为前进方向，本地 Z 轴为上方向。
        /// 返回值是可直接用于 Transform3D 的线性矩阵。
        /// </summary>
        public static bool TryCreateRailLinearTransform(
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out Matrix3 linearTransform)
        {
            return TryCreateRailLinearTransform(
                previousPoint,
                currentPoint,
                nextPoint,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out linearTransform);
        }

        /// <summary>
        /// 根据轨道路径切向和法向创建 Rail 构件完整三维姿态。
        /// 通过模型局部轴约定显式指定本地哪个轴代表 forward/up。
        /// </summary>
        public static bool TryCreateRailLinearTransform(
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            out Matrix3 linearTransform)
        {
            linearTransform = null;

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

            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            var canonicalPreviousPoint = ToNumerics(adapter.ToCanonicalPoint(previousPoint));
            var canonicalCurrentPoint = ToNumerics(adapter.ToCanonicalPoint(currentPoint));
            var canonicalNextPoint = ToNumerics(adapter.ToCanonicalPoint(nextPoint));

            if (!CanonicalRailPoseBuilder.TryCreateBasis(
                canonicalPreviousPoint,
                canonicalCurrentPoint,
                canonicalNextPoint,
                HostCoordinateAdapter.CanonicalUpVector3,
                out var canonicalForward,
                out var canonicalLateral,
                out var canonicalUp))
            {
                return false;
            }

            Quaternion canonicalRotation = axisConvention.CreateQuaternion(canonicalForward, canonicalUp);
            Matrix4x4 canonicalLinear = Matrix4x4.CreateFromQuaternion(canonicalRotation);

            var hostXAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M11, canonicalLinear.M21, canonicalLinear.M31))));
            var hostYAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M12, canonicalLinear.M22, canonicalLinear.M32))));
            var hostZAxis = Normalize(adapter.FromCanonicalVector(ToNavVector(new Vector3(canonicalLinear.M13, canonicalLinear.M23, canonicalLinear.M33))));

            // Navisworks 在 Transform3D(Matrix3, ...) 中按列读取局部基向量：
            // 第 1 列 = 本地 X 轴在世界中的方向
            // 第 2 列 = 本地 Y 轴在世界中的方向
            // 第 3 列 = 本地 Z 轴在世界中的方向
            linearTransform = new Matrix3(
                hostXAxis.X, hostYAxis.X, hostZAxis.X,
                hostXAxis.Y, hostYAxis.Y, hostZAxis.Y,
                hostXAxis.Z, hostYAxis.Z, hostZAxis.Z);

            LogManager.Info(
                $"[Rail姿态] Canonical切向=({canonicalForward.X:F4},{canonicalForward.Y:F4},{canonicalForward.Z:F4}), " +
                $"Canonical侧向=({canonicalLateral.X:F4},{canonicalLateral.Y:F4},{canonicalLateral.Z:F4}), " +
                $"Canonical法向=({canonicalUp.X:F4},{canonicalUp.Y:F4},{canonicalUp.Z:F4}), " +
                $"Host法向=({hostZAxis.X:F4},{hostZAxis.Y:F4},{hostZAxis.Z:F4}), " +
                $"模型Forward={axisConvention.ForwardAxis}, 模型Up={axisConvention.UpAxis}");
            return true;
        }

        /// <summary>
        /// 根据轨道路径切向和法向创建 Rail 构件完整三维旋转。
        /// 约定：构件本地 X 轴为前进方向，本地 Z 轴为上方向。
        /// </summary>
        public static bool TryCreateRailRotation(
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out Rotation3D rotation)
        {
            return TryCreateRailRotation(
                previousPoint,
                currentPoint,
                nextPoint,
                ModelAxisConvention.CreateDefaultForHost(CoordinateSystemType.ZUp),
                out rotation);
        }

        public static bool TryCreateRailRotation(
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            out Rotation3D rotation)
        {
            rotation = Rotation3D.Identity;
            LogRotationConstructorConventionOnce();

            if (!TryCreateRailLinearTransform(previousPoint, currentPoint, nextPoint, axisConvention, out var linearTransform))
            {
                return false;
            }

            rotation = CreateRotationFromLinearTransform(linearTransform);
            var verifiedLinear = new Transform3D(rotation).Linear;

            LogManager.Info(
                $"[Rail旋转验证] X=({verifiedLinear.Get(0, 0):F4},{verifiedLinear.Get(1, 0):F4},{verifiedLinear.Get(2, 0):F4}), " +
                $"Y=({verifiedLinear.Get(0, 1):F4},{verifiedLinear.Get(1, 1):F4},{verifiedLinear.Get(2, 1):F4}), " +
                $"Z=({verifiedLinear.Get(0, 2):F4},{verifiedLinear.Get(1, 2):F4},{verifiedLinear.Get(2, 2):F4})");
            return true;
        }

        private static Rotation3D CreateRotationFromLinearTransform(Matrix3 linearTransform)
        {
            double m00 = linearTransform.Get(0, 0);
            double m01 = linearTransform.Get(0, 1);
            double m02 = linearTransform.Get(0, 2);
            double m10 = linearTransform.Get(1, 0);
            double m11 = linearTransform.Get(1, 1);
            double m12 = linearTransform.Get(1, 2);
            double m20 = linearTransform.Get(2, 0);
            double m21 = linearTransform.Get(2, 1);
            double m22 = linearTransform.Get(2, 2);

            double qx;
            double qy;
            double qz;
            double qw;

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

            var rotation = new Rotation3D(qx, qy, qz, qw);
            double error = CalculateLinearError(new Transform3D(rotation).Linear, linearTransform);
            LogManager.Info(
                $"[Rail旋转验证] quaternion候选=({qx:F4},{qy:F4},{qz:F4},{qw:F4}), 误差={error:F6}");
            return rotation;
        }

        private static void LogRotationConstructorConventionOnce()
        {
            if (_rotationConstructorConventionLogged)
            {
                return;
            }

            _rotationConstructorConventionLogged = true;

            LogKnownAxisRotation("X90", new UnitVector3D(1, 0, 0), Math.PI / 2.0);
            LogKnownAxisRotation("Y90", new UnitVector3D(0, 1, 0), Math.PI / 2.0);
            LogKnownAxisRotation("Z90", new UnitVector3D(0, 0, 1), Math.PI / 2.0);
        }

        private static void LogKnownAxisRotation(string name, UnitVector3D axis, double angle)
        {
            Rotation3D axisAngleRotation = new Rotation3D(axis, angle);
            Rotation3D reconstructed = new Rotation3D(
                axisAngleRotation.A,
                axisAngleRotation.B,
                axisAngleRotation.C,
                axisAngleRotation.D);
            Matrix3 linear = new Transform3D(reconstructed).Linear;

            LogManager.Info(
                $"[Rotation构造测试] {name}: " +
                $"A={axisAngleRotation.A:F6}, B={axisAngleRotation.B:F6}, C={axisAngleRotation.C:F6}, D={axisAngleRotation.D:F6}, " +
                $"LinearX=({linear.Get(0, 0):F4},{linear.Get(1, 0):F4},{linear.Get(2, 0):F4}), " +
                $"LinearY=({linear.Get(0, 1):F4},{linear.Get(1, 1):F4},{linear.Get(2, 1):F4}), " +
                $"LinearZ=({linear.Get(0, 2):F4},{linear.Get(1, 2):F4},{linear.Get(2, 2):F4})");
        }

        private static double CalculateLinearError(Matrix3 actual, Matrix3 expected)
        {
            double error = 0.0;
            for (int row = 0; row < 3; row++)
            {
                for (int column = 0; column < 3; column++)
                {
                    double delta = actual.Get(row, column) - expected.Get(row, column);
                    error += delta * delta;
                }
            }

            return Math.Sqrt(error);
        }

        private static double GetBottomOffsetMagnitude(PathRoute route, double objectHeight)
        {
            double anchorOffset = GetAnchorOffset(route);
            return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)
                ? anchorOffset - objectHeight
                : anchorOffset;
        }

        private static double GetObjectSpaceCenterOffsetMagnitude(PathRoute route, double objectSpaceHeight)
        {
            double anchorOffset = GetAnchorOffset(route);
            return PathRoute.IsTopPayloadAnchorForMountMode(route.RailMountMode)
                ? anchorOffset - objectSpaceHeight / 2.0
                : anchorOffset + objectSpaceHeight / 2.0;
        }

        private static Vector3D ResolveRailNormal(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint)
        {
            var tangent = ResolveTangent(previousPoint, currentPoint, nextPoint);
            double tangentLengthSquared = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;

            if (tangentLengthSquared < TangentEpsilon)
            {
                return HostCoordinateAdapter.CanonicalUp;
            }

            tangent = new Vector3D(
                tangent.X / Math.Sqrt(tangentLengthSquared),
                tangent.Y / Math.Sqrt(tangentLengthSquared),
                tangent.Z / Math.Sqrt(tangentLengthSquared));

            var worldUp = HostCoordinateAdapter.CanonicalUp;
            double projection = worldUp.X * tangent.X + worldUp.Y * tangent.Y + worldUp.Z * tangent.Z;
            var normal = new Vector3D(
                worldUp.X - projection * tangent.X,
                worldUp.Y - projection * tangent.Y,
                worldUp.Z - projection * tangent.Z);

            double normalLengthSquared = normal.X * normal.X + normal.Y * normal.Y + normal.Z * normal.Z;
            if (normalLengthSquared < TangentEpsilon)
            {
                return HostCoordinateAdapter.CanonicalUp;
            }

            double normalLength = Math.Sqrt(normalLengthSquared);
            return new Vector3D(
                normal.X / normalLength,
                normal.Y / normalLength,
                normal.Z / normalLength);
        }

        private static Vector3D ResolveTangent(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint)
        {
            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            var canonicalPreviousPoint = adapter.ToCanonicalPoint(previousPoint);
            var canonicalCurrentPoint = adapter.ToCanonicalPoint(currentPoint);
            var canonicalNextPoint = adapter.ToCanonicalPoint(nextPoint);

            var tangent = new Vector3D(
                canonicalNextPoint.X - canonicalPreviousPoint.X,
                canonicalNextPoint.Y - canonicalPreviousPoint.Y,
                canonicalNextPoint.Z - canonicalPreviousPoint.Z);

            double tangentLengthSquared = tangent.X * tangent.X + tangent.Y * tangent.Y + tangent.Z * tangent.Z;
            if (tangentLengthSquared < TangentEpsilon)
            {
                tangent = new Vector3D(
                    canonicalNextPoint.X - canonicalCurrentPoint.X,
                    canonicalNextPoint.Y - canonicalCurrentPoint.Y,
                    canonicalNextPoint.Z - canonicalCurrentPoint.Z);
            }

            return tangent;
        }

        private static Vector3D Normalize(Vector3D vector)
        {
            double lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;
            if (lengthSquared < TangentEpsilon)
            {
                return new Vector3D(0, 0, 0);
            }

            double length = Math.Sqrt(lengthSquared);
            return new Vector3D(vector.X / length, vector.Y / length, vector.Z / length);
        }

        private static Vector3D Cross(Vector3D a, Vector3D b)
        {
            return new Vector3D(
                a.Y * b.Z - a.Z * b.Y,
                a.Z * b.X - a.X * b.Z,
                a.X * b.Y - a.Y * b.X);
        }

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

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