using System;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.Utils
{
    /// <summary>
    /// ModelItem变换辅助工具
    /// 提供临时移动和恢复物体位置的通用方法
    /// </summary>
    public static class ModelItemTransformHelper
    {
        /// <summary>
        /// 临时移动物体到指定位置
        /// </summary>
        /// <param name="item">要移动的物体</param>
        /// <param name="targetPosition">目标位置（物体中心）</param>
        /// <returns>用于恢复的偏移向量</returns>
        public static Vector3D MoveItemToPosition(ModelItem item, Point3D targetPosition)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            // 计算当前中心位置
            var currentBounds = item.BoundingBox();
            var currentPos = new Point3D(
                (currentBounds.Min.X + currentBounds.Max.X) / 2,
                (currentBounds.Min.Y + currentBounds.Max.Y) / 2,
                (currentBounds.Min.Z + currentBounds.Max.Z) / 2
            );

            // 计算偏移
            var offset = new Vector3D(
                targetPosition.X - currentPos.X,
                targetPosition.Y - currentPos.Y,
                targetPosition.Z - currentPos.Z
            );

            // 应用变换
            var transform = Transform3D.CreateTranslation(offset);
            doc.Models.OverridePermanentTransform(modelItems, transform, false);

            return offset;
        }

        /// <summary>
        /// 从Transform3D中提取Yaw角度（绕Z轴旋转，单位：弧度）
        /// </summary>
        /// <param name="transform">变换矩阵</param>
        /// <returns>Yaw角度（弧度）</returns>
        public static double GetYawFromTransform(Transform3D transform)
        {
            if (transform == null) return 0.0;

            try
            {
                var components = transform.Factor();
                return GetYawFromRotation(components.Rotation);
            }
            catch (Exception)
            {
                return 0.0;
            }
        }

        /// <summary>
        /// 从 Rotation3D 中提取 XY 平面的 yaw 角（弧度）。
        /// </summary>
        public static double GetYawFromRotation(Rotation3D rotation)
        {
            if (rotation == null)
            {
                return 0.0;
            }

            try
            {
                var rotationResult = rotation.ToAxisAndAngle();
                double angle = rotationResult.Angle;
                double ux = rotationResult.Axis.X;
                double uy = rotationResult.Axis.Y;
                double uz = rotationResult.Axis.Z;

                double cos = Math.Cos(angle);
                double sin = Math.Sin(angle);

                double vx = cos + ux * ux * (1 - cos);
                double vy = uz * sin + ux * uy * (1 - cos);

                return Math.Atan2(vy, vx);
            }
            catch (Exception)
            {
                return 0.0;
            }
        }

        /// <summary>
        /// 获取变换后局部 X 轴在世界坐标中的方向，通常可作为物体主朝向。
        /// </summary>
        public static Vector3D GetForwardDirectionFromTransform(Transform3D transform)
        {
            return GetAxisDirectionFromTransform(transform, 0, new Vector3D(1, 0, 0));
        }

        /// <summary>
        /// 获取变换后局部 Z 轴在世界坐标中的方向，可作为构件顶/底面法向。
        /// </summary>
        public static Vector3D GetUpDirectionFromTransform(Transform3D transform)
        {
            return GetAxisDirectionFromTransform(transform, 2, new Vector3D(0, 0, 1));
        }

        /// <summary>
        /// 根据世界轴对齐包围盒和物体变换，估算物体在局部坐标系中的尺寸。
        /// 适用于刚性箱体类对象，用于顶/底面对接点推导。
        /// </summary>
        public static Vector3D EstimateLocalBoxSize(BoundingBox3D bounds, Transform3D transform)
        {
            double halfXWorld = Math.Max(0.0, (bounds.Max.X - bounds.Min.X) / 2.0);
            double halfYWorld = Math.Max(0.0, (bounds.Max.Y - bounds.Min.Y) / 2.0);
            double halfZWorld = Math.Max(0.0, (bounds.Max.Z - bounds.Min.Z) / 2.0);

            if (transform == null)
            {
                return new Vector3D(halfXWorld * 2.0, halfYWorld * 2.0, halfZWorld * 2.0);
            }

            try
            {
                var forward = GetForwardDirectionFromTransform(transform);
                var localY = GetAxisDirectionFromTransform(transform, 1, new Vector3D(0, 1, 0));
                var up = GetUpDirectionFromTransform(transform);

                double[,] matrix =
                {
                    { Math.Abs(forward.X), Math.Abs(localY.X), Math.Abs(up.X) },
                    { Math.Abs(forward.Y), Math.Abs(localY.Y), Math.Abs(up.Y) },
                    { Math.Abs(forward.Z), Math.Abs(localY.Z), Math.Abs(up.Z) }
                };

                double[] worldHalfExtents = { halfXWorld, halfYWorld, halfZWorld };
                double[] localHalfExtents = SolveLinearSystem3x3(matrix, worldHalfExtents);

                return new Vector3D(
                    Math.Max(0.0, localHalfExtents[0] * 2.0),
                    Math.Max(0.0, localHalfExtents[1] * 2.0),
                    Math.Max(0.0, localHalfExtents[2] * 2.0));
            }
            catch (Exception)
            {
                return new Vector3D(halfXWorld * 2.0, halfYWorld * 2.0, halfZWorld * 2.0);
            }
        }

        /// <summary>
        /// 根据箱体局部尺寸和世界原点方向，推导“背离球心”的真实长轴方向。
        /// 先选局部最长轴，再用箱体中心相对世界原点的方向决定正负。
        /// </summary>
        public static Vector3D GetLongestAxisDirectionAwayFromOrigin(BoundingBox3D bounds, Transform3D transform)
        {
            Vector3D localSize = EstimateLocalBoxSize(bounds, transform);

            int longestAxisIndex = 0;
            double longestAxisLength = localSize.X;
            if (localSize.Y > longestAxisLength)
            {
                longestAxisIndex = 1;
                longestAxisLength = localSize.Y;
            }

            if (localSize.Z > longestAxisLength)
            {
                longestAxisIndex = 2;
            }

            Vector3D axisDirection = GetAxisDirectionFromTransform(
                transform,
                longestAxisIndex,
                longestAxisIndex == 0 ? new Vector3D(1, 0, 0) :
                longestAxisIndex == 1 ? new Vector3D(0, 1, 0) :
                                        new Vector3D(0, 0, 1));

            Vector3D awayFromOrigin = new Vector3D(bounds.Center.X, bounds.Center.Y, bounds.Center.Z);
            double awayLengthSquared = awayFromOrigin.X * awayFromOrigin.X + awayFromOrigin.Y * awayFromOrigin.Y + awayFromOrigin.Z * awayFromOrigin.Z;
            if (awayLengthSquared < 1e-9)
            {
                return axisDirection;
            }

            double dot = axisDirection.X * awayFromOrigin.X + axisDirection.Y * awayFromOrigin.Y + axisDirection.Z * awayFromOrigin.Z;
            if (dot < 0.0)
            {
                return new Vector3D(-axisDirection.X, -axisDirection.Y, -axisDirection.Z);
            }

            return axisDirection;
        }

        private static Vector3D GetAxisDirectionFromTransform(Transform3D transform, int axisIndex, Vector3D fallback)
        {
            if (transform == null)
            {
                return fallback;
            }

            try
            {
                var linear = transform.Linear;
                var direction = new Vector3D(
                    linear.Get(0, axisIndex),
                    linear.Get(1, axisIndex),
                    linear.Get(2, axisIndex));

                double lengthSquared = direction.X * direction.X + direction.Y * direction.Y + direction.Z * direction.Z;
                if (lengthSquared < 1e-9)
                {
                    return fallback;
                }

                double length = Math.Sqrt(lengthSquared);
                return new Vector3D(direction.X / length, direction.Y / length, direction.Z / length);
            }
            catch (Exception)
            {
                return fallback;
            }
        }

        private static double[] SolveLinearSystem3x3(double[,] matrix, double[] values)
        {
            double determinant =
                matrix[0, 0] * (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) -
                matrix[0, 1] * (matrix[1, 0] * matrix[2, 2] - matrix[1, 2] * matrix[2, 0]) +
                matrix[0, 2] * (matrix[1, 0] * matrix[2, 1] - matrix[1, 1] * matrix[2, 0]);

            if (Math.Abs(determinant) < 1e-9)
            {
                return new[] { values[0], values[1], values[2] };
            }

            double inverseDeterminant = 1.0 / determinant;
            double[,] inverse =
            {
                {
                    (matrix[1, 1] * matrix[2, 2] - matrix[1, 2] * matrix[2, 1]) * inverseDeterminant,
                    (matrix[0, 2] * matrix[2, 1] - matrix[0, 1] * matrix[2, 2]) * inverseDeterminant,
                    (matrix[0, 1] * matrix[1, 2] - matrix[0, 2] * matrix[1, 1]) * inverseDeterminant
                },
                {
                    (matrix[1, 2] * matrix[2, 0] - matrix[1, 0] * matrix[2, 2]) * inverseDeterminant,
                    (matrix[0, 0] * matrix[2, 2] - matrix[0, 2] * matrix[2, 0]) * inverseDeterminant,
                    (matrix[0, 2] * matrix[1, 0] - matrix[0, 0] * matrix[1, 2]) * inverseDeterminant
                },
                {
                    (matrix[1, 0] * matrix[2, 1] - matrix[1, 1] * matrix[2, 0]) * inverseDeterminant,
                    (matrix[0, 1] * matrix[2, 0] - matrix[0, 0] * matrix[2, 1]) * inverseDeterminant,
                    (matrix[0, 0] * matrix[1, 1] - matrix[0, 1] * matrix[1, 0]) * inverseDeterminant
                }
            };

            return new[]
            {
                inverse[0, 0] * values[0] + inverse[0, 1] * values[1] + inverse[0, 2] * values[2],
                inverse[1, 0] * values[0] + inverse[1, 1] * values[1] + inverse[1, 2] * values[2],
                inverse[2, 0] * values[0] + inverse[2, 1] * values[1] + inverse[2, 2] * values[2]
            };
        }

        /// <summary>
        /// 恢复物体到原位置
        /// </summary>
        /// <param name="item">要恢复的物体</param>
        /// <param name="originalOffset">原始移动偏移向量</param>
        public static void RestoreItemPosition(ModelItem item, Vector3D originalOffset)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            // 反向偏移
            var restoreOffset = new Vector3D(-originalOffset.X, -originalOffset.Y, -originalOffset.Z);
            var restoreTransform = Transform3D.CreateTranslation(restoreOffset);
            doc.Models.OverridePermanentTransform(modelItems, restoreTransform, false);
        }

        /// <summary>
        /// 将物体移动到指定位置和朝向（先回到CAD原始位置，再移动）
        /// 适用于碰撞位置还原等场景
        /// </summary>
        /// <param name="item">要移动的物体</param>
        /// <param name="targetPosition">目标位置（地面位置，即包围盒底面）</param>
        /// <param name="targetYaw">目标朝向（弧度，绕Z轴）</param>
        public static void MoveItemToPositionAndYaw(ModelItem item, Point3D targetPosition, double targetYaw)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            // 🔥 关键：先回到CAD原始位置，确保从已知状态开始计算
            doc.Models.ResetPermanentTransform(modelItems);

            // 获取CAD原始状态
            var originalBounds = item.BoundingBox();
            var originalGroundPos = new Point3D(
                originalBounds.Center.X,
                originalBounds.Center.Y,
                originalBounds.Min.Z
            );
            var originalYaw = GetYawFromTransform(item.Transform);

            // 计算从CAD原始位置到目标位置的增量
            var deltaPos = new Vector3D(
                targetPosition.X - originalGroundPos.X,
                targetPosition.Y - originalGroundPos.Y,
                targetPosition.Z - originalGroundPos.Z
            );
            double deltaYaw = targetYaw - originalYaw;

            // 应用增量变换
            Transform3D transform;
            if (Math.Abs(deltaYaw) > 0.001)
            {
                // 有旋转：需要补偿绕原点旋转带来的位置偏移
                double cos = Math.Cos(deltaYaw);
                double sin = Math.Sin(deltaYaw);
                double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin;
                double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos;

                var compensatedTranslation = new Vector3D(
                    targetPosition.X - rotatedX,
                    targetPosition.Y - rotatedY,
                    deltaPos.Z
                );

                var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
                var components = identity.Factor();
                components.Rotation = new Rotation3D(new UnitVector3D(0, 0, 1), deltaYaw);
                components.Translation = compensatedTranslation;
                transform = components.Combine();
            }
            else
            {
                // 纯平移
                transform = Transform3D.CreateTranslation(deltaPos);
            }

            doc.Models.OverridePermanentTransform(modelItems, transform, false);
        }

        /// <summary>
        /// 将物体移动到指定位置和完整三维朝向（先回到CAD原始位置，再移动）
        /// </summary>
        public static void MoveItemToPositionAndRotation(ModelItem item, Point3D targetPosition, Rotation3D targetRotation)
        {
            ApplyAbsoluteTransform(item, targetPosition, targetRotation, preserveCurrentScale: false);
        }

        /// <summary>
        /// 将物体移动到指定位置和完整三维朝向，同时保留当前缩放。
        /// </summary>
        public static void MoveItemToPositionAndRotationWithCurrentScale(ModelItem item, Point3D targetPosition, Rotation3D targetRotation)
        {
            ApplyAbsoluteTransform(item, targetPosition, targetRotation, preserveCurrentScale: true);
        }

        /// <summary>
        /// 将物体移动到指定中心点和完整三维朝向，同时保留当前缩放。
        /// 适用于参考杆、辅助几何等以几何中心作为定位基准的场景。
        /// </summary>
        public static void MoveItemToCenterAndRotationWithCurrentScale(ModelItem item, Point3D targetCenter, Rotation3D targetRotation)
        {
            if (item == null)
            {
                throw new ArgumentNullException(nameof(item));
            }

            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            var currentComponents = item.Transform.Factor();
            Vector3D currentScale = currentComponents.Scale;

            doc.Models.ResetPermanentTransform(modelItems);

            var originalBounds = item.BoundingBox();
            Point3D originalCenter = originalBounds.Center;

            var rotationTransform = new Transform3D(targetRotation);
            var linear = rotationTransform.Linear;

            var rotatedCenter = new Point3D(
                linear.Get(0, 0) * originalCenter.X + linear.Get(0, 1) * originalCenter.Y + linear.Get(0, 2) * originalCenter.Z,
                linear.Get(1, 0) * originalCenter.X + linear.Get(1, 1) * originalCenter.Y + linear.Get(1, 2) * originalCenter.Z,
                linear.Get(2, 0) * originalCenter.X + linear.Get(2, 1) * originalCenter.Y + linear.Get(2, 2) * originalCenter.Z);

            var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
            var components = identity.Factor();
            components.Scale = currentScale;
            components.Rotation = targetRotation;
            components.Translation = new Vector3D(
                targetCenter.X - rotatedCenter.X,
                targetCenter.Y - rotatedCenter.Y,
                targetCenter.Z - rotatedCenter.Z);

            doc.Models.OverridePermanentTransform(modelItems, components.Combine(), false);
        }

        /// <summary>
        /// 将物体移动到指定中心点和完整三维朝向。
        /// 仅应用位置和旋转，不保留当前缩放。
        /// 适用于缩放已经在 Model 层完成的参考杆等场景。
        /// </summary>
        public static void MoveItemToCenterAndRotation(ModelItem item, Point3D targetCenter, Rotation3D targetRotation)
        {
            if (item == null)
            {
                throw new ArgumentNullException(nameof(item));
            }

            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            doc.Models.ResetPermanentTransform(modelItems);

            var originalBounds = item.BoundingBox();
            Point3D originalCenter = originalBounds.Center;

            var rotationTransform = new Transform3D(targetRotation);
            var linear = rotationTransform.Linear;

            var rotatedCenter = new Point3D(
                linear.Get(0, 0) * originalCenter.X + linear.Get(0, 1) * originalCenter.Y + linear.Get(0, 2) * originalCenter.Z,
                linear.Get(1, 0) * originalCenter.X + linear.Get(1, 1) * originalCenter.Y + linear.Get(1, 2) * originalCenter.Z,
                linear.Get(2, 0) * originalCenter.X + linear.Get(2, 1) * originalCenter.Y + linear.Get(2, 2) * originalCenter.Z);

            var translation = new Vector3D(
                targetCenter.X - rotatedCenter.X,
                targetCenter.Y - rotatedCenter.Y,
                targetCenter.Z - rotatedCenter.Z);

            doc.Models.OverridePermanentTransform(modelItems, new Transform3D(targetRotation, translation), false);
        }

        /// <summary>
        /// 将物体的局部正 X 端面中心对齐到目标点，并保留当前缩放。
        /// 适用于单位立方体/单位圆柱体沿 +X 定义长度方向的参考杆资源。
        /// </summary>
        public static void MoveItemPositiveXEndToPointAndRotationWithCurrentScale(ModelItem item, Point3D targetPoint, Rotation3D targetRotation)
        {
            if (item == null)
            {
                throw new ArgumentNullException(nameof(item));
            }

            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            var currentComponents = item.Transform.Factor();
            Vector3D currentScale = currentComponents.Scale;

            doc.Models.ResetPermanentTransform(modelItems);

            var originalBounds = item.BoundingBox();
            Point3D originalPositiveXEndCenter = new Point3D(
                originalBounds.Max.X,
                originalBounds.Center.Y,
                originalBounds.Center.Z);

            var rotationTransform = new Transform3D(targetRotation);
            var linear = rotationTransform.Linear;

            var rotatedPositiveXEndCenter = new Point3D(
                linear.Get(0, 0) * originalPositiveXEndCenter.X + linear.Get(0, 1) * originalPositiveXEndCenter.Y + linear.Get(0, 2) * originalPositiveXEndCenter.Z,
                linear.Get(1, 0) * originalPositiveXEndCenter.X + linear.Get(1, 1) * originalPositiveXEndCenter.Y + linear.Get(1, 2) * originalPositiveXEndCenter.Z,
                linear.Get(2, 0) * originalPositiveXEndCenter.X + linear.Get(2, 1) * originalPositiveXEndCenter.Y + linear.Get(2, 2) * originalPositiveXEndCenter.Z);

            var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
            var components = identity.Factor();
            components.Scale = currentScale;
            components.Rotation = targetRotation;
            components.Translation = new Vector3D(
                targetPoint.X - rotatedPositiveXEndCenter.X,
                targetPoint.Y - rotatedPositiveXEndCenter.Y,
                targetPoint.Z - rotatedPositiveXEndCenter.Z);

            doc.Models.OverridePermanentTransform(modelItems, components.Combine(), false);
        }

        /// <summary>
        /// 将物体的局部正 X 端面中心对齐到目标点。
        /// 仅应用位置和旋转，不保留当前缩放。
        /// 适用于缩放已经在 Model 层完成、Item 层只负责定位的场景。
        /// </summary>
        public static void MoveItemPositiveXEndToPointAndRotation(ModelItem item, Point3D targetPoint, Rotation3D targetRotation)
        {
            if (item == null)
            {
                throw new ArgumentNullException(nameof(item));
            }

            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            doc.Models.ResetPermanentTransform(modelItems);

            var originalBounds = item.BoundingBox();
            Point3D originalPositiveXEndCenter = new Point3D(
                originalBounds.Max.X,
                originalBounds.Center.Y,
                originalBounds.Center.Z);

            var rotationTransform = new Transform3D(targetRotation);
            var linear = rotationTransform.Linear;

            var rotatedPositiveXEndCenter = new Point3D(
                linear.Get(0, 0) * originalPositiveXEndCenter.X + linear.Get(0, 1) * originalPositiveXEndCenter.Y + linear.Get(0, 2) * originalPositiveXEndCenter.Z,
                linear.Get(1, 0) * originalPositiveXEndCenter.X + linear.Get(1, 1) * originalPositiveXEndCenter.Y + linear.Get(1, 2) * originalPositiveXEndCenter.Z,
                linear.Get(2, 0) * originalPositiveXEndCenter.X + linear.Get(2, 1) * originalPositiveXEndCenter.Y + linear.Get(2, 2) * originalPositiveXEndCenter.Z);

            var translation = new Vector3D(
                targetPoint.X - rotatedPositiveXEndCenter.X,
                targetPoint.Y - rotatedPositiveXEndCenter.Y,
                targetPoint.Z - rotatedPositiveXEndCenter.Z);

            doc.Models.OverridePermanentTransform(modelItems, new Transform3D(targetRotation, translation), false);
        }

        private static void ApplyAbsoluteTransform(ModelItem item, Point3D targetPosition, Rotation3D targetRotation, bool preserveCurrentScale)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };
            Vector3D currentScale = new Vector3D(1, 1, 1);

            if (preserveCurrentScale)
            {
                var currentComponents = item.Transform.Factor();
                currentScale = currentComponents.Scale;
            }

            doc.Models.ResetPermanentTransform(modelItems);

            var originalBounds = item.BoundingBox();
            var originalGroundPos = new Point3D(
                originalBounds.Center.X,
                originalBounds.Center.Y,
                originalBounds.Min.Z
            );

            var rotationTransform = new Transform3D(targetRotation);
            var linear = rotationTransform.Linear;

            var rotatedGroundPos = new Point3D(
                linear.Get(0, 0) * originalGroundPos.X + linear.Get(0, 1) * originalGroundPos.Y + linear.Get(0, 2) * originalGroundPos.Z,
                linear.Get(1, 0) * originalGroundPos.X + linear.Get(1, 1) * originalGroundPos.Y + linear.Get(1, 2) * originalGroundPos.Z,
                linear.Get(2, 0) * originalGroundPos.X + linear.Get(2, 1) * originalGroundPos.Y + linear.Get(2, 2) * originalGroundPos.Z
            );

            var translation = new Vector3D(
                targetPosition.X - rotatedGroundPos.X,
                targetPosition.Y - rotatedGroundPos.Y,
                targetPosition.Z - rotatedGroundPos.Z
            );

            Transform3D transform;
            if (preserveCurrentScale)
            {
                var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
                var components = identity.Factor();
                components.Scale = currentScale;
                components.Rotation = targetRotation;
                components.Translation = translation;
                transform = components.Combine();
            }
            else
            {
                transform = new Transform3D(targetRotation, translation);
            }

            doc.Models.OverridePermanentTransform(modelItems, transform, false);
        }

        /// <summary>
        /// 将物体移动到指定位置和朝向，同时保持缩放比例
        /// 专为虚拟物体设计，避免缩放被覆盖
        /// </summary>
        /// <param name="item">要移动的物体</param>
        /// <param name="targetPosition">目标位置（地面位置）</param>
        /// <param name="targetYaw">目标朝向（弧度）</param>
        /// <param name="scaleX">X方向缩放</param>
        /// <param name="scaleY">Y方向缩放</param>
        /// <param name="scaleZ">Z方向缩放</param>
        public static void MoveItemToPositionAndYawWithScale(ModelItem item, Point3D targetPosition, double targetYaw,
            double scaleX, double scaleY, double scaleZ)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            // 重置到CAD原始状态
            doc.Models.ResetPermanentTransform(modelItems);

            // 获取CAD原始状态
            var originalBounds = item.BoundingBox();
            var originalGroundPos = new Point3D(
                originalBounds.Center.X,
                originalBounds.Center.Y,
                originalBounds.Min.Z
            );
            var originalYaw = GetYawFromTransform(item.Transform);

            // 计算从CAD原始位置到目标位置的增量
            var deltaPos = new Vector3D(
                targetPosition.X - originalGroundPos.X,
                targetPosition.Y - originalGroundPos.Y,
                targetPosition.Z - originalGroundPos.Z
            );
            double deltaYaw = targetYaw - originalYaw;

            // 构建变换组件
            var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
            var components = identity.Factor();

            // 应用缩放
            components.Scale = new Vector3D(scaleX, scaleY, scaleZ);

            // 应用旋转
            if (Math.Abs(deltaYaw) > 0.001)
            {
                components.Rotation = new Rotation3D(new UnitVector3D(0, 0, 1), deltaYaw);
            }

            // 计算平移（考虑旋转带来的位置偏移）
            if (Math.Abs(deltaYaw) > 0.001)
            {
                double cos = Math.Cos(deltaYaw);
                double sin = Math.Sin(deltaYaw);
                double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin;
                double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos;

                components.Translation = new Vector3D(
                    targetPosition.X - rotatedX,
                    targetPosition.Y - rotatedY,
                    deltaPos.Z
                );
            }
            else
            {
                components.Translation = deltaPos;
            }

            // 应用组合变换
            Transform3D transform = components.Combine();
            doc.Models.OverridePermanentTransform(modelItems, transform, false);
        }

        /// <summary>
        /// 物体状态快照，用于保存和恢复
        /// </summary>
        public class ObjectStateSnapshot
        {
            public Point3D Position { get; set; }
            public double YawRadians { get; set; }
            public Transform3D Transform { get; set; }
        }

        /// <summary>
        /// 保存物体当前状态
        /// 注意：YawRadians 通过当前位置计算，因为 Transform 返回的是CAD原始值
        /// </summary>
        public static ObjectStateSnapshot SaveObjectState(ModelItem item, double? currentYaw = null)
        {
            if (item == null) return null;

            var bounds = item.BoundingBox();
            
            // 🔥 关键：如果提供了当前朝向（如 PathAnimationManager._currentYaw），使用它
            // 否则从 Transform 获取（但这可能不是实际朝向）
            double yaw;
            if (currentYaw.HasValue)
            {
                yaw = currentYaw.Value;
            }
            else
            {
                // 尝试通过包围盒方向计算（如果物体有几何特征）
                yaw = GetYawFromTransform(item.Transform);
                LogManager.Debug($"[SaveObjectState] 未提供当前朝向，使用Transform: {yaw * 180 / Math.PI:F2}°");
            }
            
            return new ObjectStateSnapshot
            {
                Position = new Point3D(bounds.Center.X, bounds.Center.Y, bounds.Min.Z),
                YawRadians = yaw,
                Transform = item.Transform
            };
        }

        /// <summary>
        /// 从状态快照恢复物体位置和朝向（标准版本，会重置缩放）
        /// </summary>
        public static void RestoreObjectState(ModelItem item, ObjectStateSnapshot state)
        {
            RestoreObjectStateInternal(item, state, preserveScale: false);
        }
        
        /// <summary>
        /// 从状态快照恢复物体位置和朝向（保留缩放版本，用于虚拟物体）
        /// </summary>
        public static void RestoreObjectStateWithScale(ModelItem item, ObjectStateSnapshot state)
        {
            RestoreObjectStateInternal(item, state, preserveScale: true);
        }
        
        /// <summary>
        /// 从状态快照恢复物体位置的内部实现
        /// </summary>
        private static void RestoreObjectStateInternal(ModelItem item, ObjectStateSnapshot state, bool preserveScale)
        {
            if (item == null || state == null) return;
            
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };
            
            // 1. 先回到CAD原始位置
            doc.Models.ResetPermanentTransform(modelItems);
            
            // 2. 从CAD原始位置移动到保存的位置
            // 获取CAD原始状态
            var originalBounds = item.BoundingBox();
            var originalGroundPos = new Point3D(
                originalBounds.Center.X,
                originalBounds.Center.Y,
                originalBounds.Min.Z
            );
            var originalYaw = GetYawFromTransform(item.Transform);
            
            // 计算到保存位置的增量
            var deltaPos = new Vector3D(
                state.Position.X - originalGroundPos.X,
                state.Position.Y - originalGroundPos.Y,
                state.Position.Z - originalGroundPos.Z
            );
            double deltaYaw = state.YawRadians - originalYaw;
            
            // 应用增量变换
            Transform3D transform;
            if (Math.Abs(deltaYaw) > 0.001)
            {
                double cos = Math.Cos(deltaYaw);
                double sin = Math.Sin(deltaYaw);
                double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin;
                double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos;

                var compensatedTranslation = new Vector3D(
                    state.Position.X - rotatedX,
                    state.Position.Y - rotatedY,
                    deltaPos.Z
                );

                var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
                var components = identity.Factor();
                components.Rotation = new Rotation3D(new UnitVector3D(0, 0, 1), deltaYaw);
                components.Translation = compensatedTranslation;
                transform = components.Combine();
            }
            else
            {
                transform = Transform3D.CreateTranslation(deltaPos);
            }
            
            doc.Models.OverridePermanentTransform(modelItems, transform, false);
        }
        
        /// <summary>
        /// 将物体移动到指定位置和朝向，同时保持当前缩放（专为虚拟物体设计）
        /// </summary>
        public static void MoveItemToPositionAndYawWithCurrentScale(ModelItem item, Point3D targetPosition, double targetYaw)
        {
            var doc = Application.ActiveDocument;
            var modelItems = new ModelItemCollection { item };

            // 获取当前变换中的缩放
            var currentTransform = item.Transform;
            var currentComponents = currentTransform.Factor();
            var currentScale = currentComponents.Scale;

            // 重置到CAD原始状态
            doc.Models.ResetPermanentTransform(modelItems);

            // 获取CAD原始状态
            var originalBounds = item.BoundingBox();
            var originalGroundPos = new Point3D(
                originalBounds.Center.X,
                originalBounds.Center.Y,
                originalBounds.Min.Z
            );
            var originalYaw = GetYawFromTransform(item.Transform);

            // 计算从CAD原始位置到目标位置的增量
            var deltaPos = new Vector3D(
                targetPosition.X - originalGroundPos.X,
                targetPosition.Y - originalGroundPos.Y,
                targetPosition.Z - originalGroundPos.Z
            );
            double deltaYaw = targetYaw - originalYaw;

            // 构建变换组件，保留原有缩放
            var identity = Transform3D.CreateTranslation(new Vector3D(0, 0, 0));
            var components = identity.Factor();
            components.Scale = currentScale;  // 保留当前缩放
            
            if (Math.Abs(deltaYaw) > 0.001)
            {
                components.Rotation = new Rotation3D(new UnitVector3D(0, 0, 1), deltaYaw);
            }
            
            // 计算平移（考虑旋转带来的位置偏移）
            if (Math.Abs(deltaYaw) > 0.001)
            {
                double cos = Math.Cos(deltaYaw);
                double sin = Math.Sin(deltaYaw);
                double rotatedX = originalGroundPos.X * cos - originalGroundPos.Y * sin;
                double rotatedY = originalGroundPos.X * sin + originalGroundPos.Y * cos;

                components.Translation = new Vector3D(
                    targetPosition.X - rotatedX,
                    targetPosition.Y - rotatedY,
                    deltaPos.Z
                );
            }
            else
            {
                components.Translation = deltaPos;
            }

            // 应用组合变换
            Transform3D transform = components.Combine();
            doc.Models.OverridePermanentTransform(modelItems, transform, false);
        }
    }
}