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 首选法向统一到宿主 up 半球，避免新建装配路径把“轨上侧”写反。
        /// </summary>
        public static Vector3 NormalizePreferredNormalToHostUpHemisphere(Vector3 hostPreferredNormal, Vector3 hostUpVector)
        {
            if (hostPreferredNormal.LengthSquared() < 1e-9f)
            {
                throw new ArgumentException("Rail 首选法向长度过小，无法归一化。", nameof(hostPreferredNormal));
            }

            if (hostUpVector.LengthSquared() < 1e-9f)
            {
                throw new ArgumentException("宿主 up 向量长度过小，无法归一化。", nameof(hostUpVector));
            }

            Vector3 normalizedPreferredNormal = Vector3.Normalize(hostPreferredNormal);
            Vector3 normalizedHostUp = Vector3.Normalize(hostUpVector);
            if (Vector3.Dot(normalizedPreferredNormal, normalizedHostUp) < 0f)
            {
                normalizedPreferredNormal = -normalizedPreferredNormal;
            }

            return normalizedPreferredNormal;
        }

        /// <summary>
        /// 计算构件底面中心相对于 Rail 参考点的 Z 偏移（模型单位）。
        /// </summary>
        public static double GetBottomZOffset(PathRoute route, double objectHeight)
        {
            if (route == null || route.PathType != PathType.Rail)
            {
                return 0.0;
            }

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

            return 0.0;
        }

        /// <summary>
        /// 计算通行空间中心相对于 Rail 参考点的 Z 偏移（模型单位）。
        /// 路径参考点位于构件顶面时，通行空间中心位于参考点下方半高；
        /// 路径参考点位于构件底面时，通行空间中心位于参考点上方半高。
        /// </summary>
        public static double GetObjectSpaceCenterZOffset(PathRoute route, double objectSpaceHeight)
        {
            if (route == null || route.PathType != PathType.Rail)
            {
                return 0.0;
            }

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

            return 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);
            if (!TryCreateCanonicalLocalFrame(route, previousPoint, referencePoint, nextPoint, out RailLocalFrame frame))
            {
                var canonicalCenterPointForRailFallback = new Point3D(
                    canonicalReferencePointForRail.X,
                    canonicalReferencePointForRail.Y,
                    canonicalReferencePointForRail.Z + centerOffset);
                return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRailFallback);
            }

            Vector3 trackedCenter = CanonicalRailOffsetResolver.ResolveTrackedCenter(
                route,
                ToNumerics(canonicalReferencePointForRail),
                frame,
                objectSpaceHeight);
            var canonicalCenterPointForRail = new Point3D(trackedCenter.X, trackedCenter.Y, trackedCenter.Z);
            return adapterForRail.FromCanonicalPoint(canonicalCenterPointForRail);
        }

        internal static Vector3D CalculatePreservedTrackedCenterOffset(
            PathRoute route,
            Point3D actualTrackedCenter,
            Point3D referencePoint,
            Point3D previousPoint,
            Point3D nextPoint,
            double objectSpaceHeight)
        {
            Point3D semanticTrackedCenter = ResolveObjectSpaceCenterPosition(
                route,
                referencePoint,
                previousPoint,
                nextPoint,
                objectSpaceHeight);

            return new Vector3D(
                actualTrackedCenter.X - semanticTrackedCenter.X,
                actualTrackedCenter.Y - semanticTrackedCenter.Y,
                actualTrackedCenter.Z - semanticTrackedCenter.Z);
        }

        internal static Vector3 CalculatePreservedTrackedCenterOffset(Vector3 actualTrackedCenter, Vector3 semanticTrackedCenter)
        {
            return actualTrackedCenter - semanticTrackedCenter;
        }

        internal static Point3D ResolvePreservedTrackedCenterPosition(
            PathRoute route,
            Point3D referencePoint,
            Point3D previousPoint,
            Point3D nextPoint,
            double objectSpaceHeight,
            Vector3D preservedOffset)
        {
            Point3D semanticTrackedCenter = ResolveObjectSpaceCenterPosition(
                route,
                referencePoint,
                previousPoint,
                nextPoint,
                objectSpaceHeight);

            return new Point3D(
                semanticTrackedCenter.X + preservedOffset.X,
                semanticTrackedCenter.Y + preservedOffset.Y,
                semanticTrackedCenter.Z + preservedOffset.Z);
        }

        internal static Vector3 ResolvePreservedTrackedCenterPosition(Vector3 semanticTrackedCenter, Vector3 preservedOffset)
        {
            return semanticTrackedCenter + preservedOffset;
        }

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

        public static bool TryCreateRailLinearTransform(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out Matrix3 linearTransform)
        {
            return TryCreateRailLinearTransform(
                route,
                previousPoint,
                currentPoint,
                nextPoint,
                ModelAxisConvention.CreateRailAssetConvention(),
                out linearTransform);
        }

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

        public static bool TryCreateRailLinearTransform(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            double localUpRotationDegrees,
            out Matrix3 linearTransform)
        {
            return TryCreateRailLinearTransform(
                route,
                previousPoint,
                currentPoint,
                nextPoint,
                axisConvention,
                LocalEulerRotationCorrection.FromConventionUpDegrees(axisConvention, localUpRotationDegrees),
                out linearTransform);
        }

        public static bool TryCreateRailLinearTransform(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            LocalEulerRotationCorrection correction,
            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.TryCreateLocalFrame(
                canonicalPreviousPoint,
                canonicalCurrentPoint,
                canonicalNextPoint,
                HostCoordinateAdapter.CanonicalUpVector3,
                ResolvePreferredNormal(route),
                out RailLocalFrame frame))
            {
                return false;
            }

            var canonicalForward = frame.Forward;
            var canonicalLateral = frame.Lateral;
            var canonicalUp = frame.Normal;
            Quaternion correctionQuaternion = adapter.CreateCanonicalRotationCorrection(correction);
            Quaternion canonicalRotation = CanonicalRailPoseBuilder.TryCreateQuaternion(
                canonicalPreviousPoint,
                canonicalCurrentPoint,
                canonicalNextPoint,
                HostCoordinateAdapter.CanonicalUpVector3,
                axisConvention,
                ResolvePreferredNormal(route),
                correctionQuaternion,
                out var correctedRotation)
                ? correctedRotation
                : 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.Debug(
                $"[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}, 宿主轴旋转={correction}");
            return true;
        }

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

        public static bool TryCreateRailRotation(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out Rotation3D rotation)
        {
            return TryCreateRailRotation(
                route,
                previousPoint,
                currentPoint,
                nextPoint,
                ModelAxisConvention.CreateRailAssetConvention(),
                out rotation);
        }

        public static bool TryCreateRailRotation(
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            out Rotation3D rotation)
        {
            return TryCreateRailRotation(
                null,
                previousPoint,
                currentPoint,
                nextPoint,
                axisConvention,
                out rotation);
        }

        public static bool TryCreateRailRotation(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            out Rotation3D rotation)
        {
            return TryCreateRailRotation(
                route,
                previousPoint,
                currentPoint,
                nextPoint,
                axisConvention,
                0.0,
                out rotation);
        }

        public static bool TryCreateRailRotation(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            ModelAxisConvention axisConvention,
            double localUpRotationDegrees,
            out Rotation3D rotation)
        {
            return TryCreateRailRotation(
                route,
                previousPoint,
                currentPoint,
                nextPoint,
                axisConvention,
                LocalEulerRotationCorrection.FromConventionUpDegrees(axisConvention, localUpRotationDegrees),
                out rotation);
        }

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

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

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

            LogManager.Debug(
                $"[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.Debug(
                $"[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 baseOffset = PathRoute.IsTopReferenceFaceForMountMode(route.RailMountMode)
                ? -objectHeight
                : 0.0;
            return baseOffset + route.RailNormalOffset;
        }

        private static double GetObjectSpaceCenterOffsetMagnitude(PathRoute route, double objectSpaceHeight)
        {
            double baseOffset = PathRoute.IsTopReferenceFaceForMountMode(route.RailMountMode)
                ? -objectSpaceHeight / 2.0
                : objectSpaceHeight / 2.0;
            return baseOffset + route.RailNormalOffset;
        }

        private static Vector3D ResolveRailNormal(Point3D previousPoint, Point3D currentPoint, Point3D nextPoint)
        {
            if (TryCreateCanonicalLocalFrame(null, previousPoint, currentPoint, nextPoint, out RailLocalFrame frame))
            {
                return ToNavVector(frame.Normal);
            }

            return HostCoordinateAdapter.CanonicalUp;
        }

        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 bool TryCreateCanonicalLocalFrame(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out RailLocalFrame frame)
        {
            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            return CanonicalRailPoseBuilder.TryCreateLocalFrame(
                ToNumerics(adapter.ToCanonicalPoint(previousPoint)),
                ToNumerics(adapter.ToCanonicalPoint(currentPoint)),
                ToNumerics(adapter.ToCanonicalPoint(nextPoint)),
                HostCoordinateAdapter.CanonicalUpVector3,
                ResolvePreferredNormal(route),
                out frame);
        }

        /// <summary>
        /// 获取 Rail 路径在宿主坐标系中的目标框架。
        /// 统一供真实物体姿态解释层之后的路径目标框架消费。
        /// </summary>
        public static bool TryCreateHostLocalFrame(
            PathRoute route,
            Point3D previousPoint,
            Point3D currentPoint,
            Point3D nextPoint,
            out RailLocalFrame hostFrame)
        {
            hostFrame = null;

            if (!TryCreateCanonicalLocalFrame(route, previousPoint, currentPoint, nextPoint, out var canonicalFrame))
            {
                return false;
            }

            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            hostFrame = new RailLocalFrame(
                adapter.FromCanonicalVector3(canonicalFrame.Forward),
                adapter.FromCanonicalVector3(canonicalFrame.Lateral),
                adapter.FromCanonicalVector3(canonicalFrame.Normal));
            return true;
        }

        private static Vector3? ResolvePreferredNormal(PathRoute route)
        {
            if (route?.RailPreferredNormal == null)
            {
                return null;
            }

            var preferredNormal = new Vector3(
                (float)route.RailPreferredNormal.X,
                (float)route.RailPreferredNormal.Y,
                (float)route.RailPreferredNormal.Z);

            if (preferredNormal.LengthSquared() < 1e-9f)
            {
                return null;
            }

            var adapter = CoordinateSystemManager.Instance.CreateHostAdapter();
            Vector3 canonicalPreferredNormal = adapter.ToCanonicalVector3(preferredNormal);
            if (canonicalPreferredNormal.LengthSquared() < 1e-9f)
            {
                return null;
            }

            return Vector3.Normalize(canonicalPreferredNormal);
        }

        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);
        }
    }
}
