using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.Navisworks.Api;

namespace NavisworksTransport.PathPlanning
{
    /// <summary>
    /// 路径优化器
    /// 提供多种路径优化算法，包括简化、平滑、避障等
    /// </summary>
    public class PathOptimizer
    {
        /// <summary>
        /// 共线检测容差（米）
        /// </summary>
        private const double COLLINEAR_TOLERANCE_METERS = 0.1;

        /// <summary>
        /// 重复点检测容差（米）- 用于检测坐标完全相同的点
        /// </summary>
        private const double DUPLICATE_POINT_TOLERANCE_METERS = 0.001;

        /// <summary>
        /// 路径优化配置参数
        /// </summary>
        public class OptimizationConfig
        {
            /// <summary>
            /// 是否启用路径简化（去除共线点）
            /// </summary>
            public bool EnableSimplification { get; set; } = true;

            /// <summary>
            /// 共线检测的容差值（米）
            /// </summary>
            public double CollinearTolerance { get; set; }

            /// <summary>
            /// 是否启用路径平滑（未来扩展）
            /// </summary>
            public bool EnableSmoothing { get; set; }

            /// <summary>
            /// 转弯半径（米，未来扩展）
            /// </summary>
            public double TurningRadius { get; set; }

            /// <summary>
            /// 圆弧分段数（未来扩展）
            /// </summary>
            public int ArcSegments { get; set; }

            /// <summary>
            /// 是否启用碰撞检测验证（未来扩展）
            /// </summary>
            public bool EnableCollisionCheck { get; set; }

            /// <summary>
            /// 物体长度（米，未来扩展）
            /// </summary>
            public double ObjectLength { get; set; }

            /// <summary>
            /// 物体宽度（米，未来扩展）
            /// </summary>
            public double ObjectWidth { get; set; }
        }

        private readonly OptimizationConfig _config;

        /// <summary>
        /// 构造函数
        /// </summary>
        /// <param name="config">优化配置，为null时使用默认配置</param>
        public PathOptimizer(OptimizationConfig config = null)
        {
            _config = config ?? new OptimizationConfig();
        }

        /// <summary>
        /// 优化路径的主入口
        /// </summary>
        /// <param name="originalPath">原始路径</param>
        /// <param name="gridMap">网格地图（用于网格坐标转换）</param>
        /// <returns>优化后的路径</returns>
        public PathRoute OptimizePath(PathRoute originalPath, GridMap gridMap = null)
        {
            if (originalPath == null)
            {
                LogManager.Warning("[路径优化] 原始路径为null");
                return null;
            }

            if (originalPath.Points.Count <= 2)
            {
                LogManager.Info("[路径优化] 路径点数量≤2，无需优化");
                return originalPath;
            }

            try
            {
                var startTime = DateTime.Now;
                var originalCount = originalPath.Points.Count;
                
                LogManager.Info($"[路径优化] 开始优化路径：{originalPath.Name}，原始点数：{originalCount}");

                // 复制路径点进行优化
                var optimizedPoints = new List<PathPoint>(originalPath.Points);

                // 1. 基础简化 - 使用基于网格的算法
                if (_config.EnableSimplification)
                {
                    if (gridMap != null)
                    {
                        optimizedPoints = SimplifyGridBasedPath(optimizedPoints, gridMap);
                        LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
                    }
                    else
                    {
                        optimizedPoints = SimplifyCollinearPoints(optimizedPoints, gridMap);
                        LogManager.Info($"[路径优化] 简化完成，点数：{originalCount} -> {optimizedPoints.Count}");
                    }
                }

                // 2. 未来扩展：路径平滑
                if (_config.EnableSmoothing)
                {
                    LogManager.Info("[路径优化] 路径平滑功能尚未实现");
                    // optimizedPoints = SmoothPath(optimizedPoints);
                }

                // 创建优化后的路径
                var optimizedRoute = CreateOptimizedRoute(originalPath, optimizedPoints);

                var duration = DateTime.Now - startTime;
                LogManager.Info($"[路径优化] 优化完成，耗时：{duration.TotalMilliseconds:F1}ms，优化率：{(1.0 - (double)optimizedPoints.Count / originalCount) * 100:F1}%");

                return optimizedRoute;
            }
            catch (Exception ex)
            {
                LogManager.Error($"[路径优化] 优化过程发生异常：{ex.Message}", ex);
                return originalPath; // 发生异常时返回原始路径
            }
        }

        /// <summary>
        /// 基于网格坐标的路径简化算法
        /// </summary>
        /// <param name="points">原始路径点列表</param>
        /// <param name="gridMap">网格地图</param>
        /// <returns>简化后的路径点列表</returns>
        private List<PathPoint> SimplifyGridBasedPath(List<PathPoint> points, GridMap gridMap)
        {
            if (points == null || points.Count < 3)
                return points;

            // 转换为网格坐标
            var worldPath = points.Select(p => p.Position).ToList();
            var gridPath = worldPath.Select(p => gridMap.WorldToGrid(p)).ToList();

            // 步骤1：先去除重复点，但保护限速边界和多层高度
            var dedupedGridPath = new List<GridPoint2D>();
            var dedupedPoints = new List<PathPoint>();
            for (int i = 0; i < gridPath.Count; i++)
            {
                // 🔥 关键修复：即使网格坐标相同，如果限速不同或高度不同也不能去重
                // 2D场景：hasHeightChange为false，不影响原有逻辑
                // 3D场景：保留同网格不同高度的点
                bool hasGridChange = i == 0 || !gridPath[i].Equals(gridPath[i-1]);
                bool hasSpeedChange = i > 0 && HasSpeedLimitChange(points[i-1], points[i]);
                bool hasHeightChange = i > 0 && HasHeightChange(points[i], points[i - 1], gridMap);

                bool shouldKeep = hasGridChange || hasSpeedChange || hasHeightChange;

                if (shouldKeep)
                {
                    dedupedGridPath.Add(gridPath[i]);
                    dedupedPoints.Add(points[i]);
                }
            }
            LogManager.Info($"[路径优化] 去重完成：{gridPath.Count} -> {dedupedPoints.Count} 个点");

            // 步骤2：基于去重后的路径进行方向优化
            // 🔥 关键修复：不使用网格坐标，直接使用索引来跟踪保留的点
            // 这样可以避免同一网格坐标的不同高度点被混淆
            var simplifiedIndices = new List<int> { 0 }; // 保留起点索引

            // 获取初始方向（归一化）
            int initialDx = dedupedGridPath[1].X - dedupedGridPath[0].X;
            int initialDy = dedupedGridPath[1].Y - dedupedGridPath[0].Y;
            GridPoint2D prevDirection = new GridPoint2D(
                initialDx == 0 ? 0 : Math.Sign(initialDx),
                initialDy == 0 ? 0 : Math.Sign(initialDy)
            );

            // 简化路径：只保留转弯点、高度变化点和限速变化点
            bool forceKeepNext = false;
            for (int i = 2; i < dedupedGridPath.Count; i++)
            {
                // 计算当前线段的位移
                int dx = dedupedGridPath[i].X - dedupedGridPath[i-1].X;
                int dy = dedupedGridPath[i].Y - dedupedGridPath[i-1].Y;

                // 归一化为单位方向向量 (-1, 0, 1)
                var currentDirection = new GridPoint2D(
                    dx == 0 ? 0 : Math.Sign(dx),
                    dy == 0 ? 0 : Math.Sign(dy)
                );

                // 检查高度变化（2D场景下始终为false，不影响原有逻辑）
                bool hasHeightChange = HasHeightChange(
                    dedupedPoints[i-2],
                    dedupedPoints[i-1],
                    dedupedPoints[i],
                    gridMap
                );

                // 检查限速变化
                bool hasSpeedLimitChange = HasSpeedLimitChange(
                    dedupedPoints[i-2],
                    dedupedPoints[i-1],
                    dedupedPoints[i]
                );

                // 保留转折点、高度变化点或限速变化点
                bool shouldKeep = !currentDirection.Equals(prevDirection) || hasHeightChange || hasSpeedLimitChange || forceKeepNext;

                if (shouldKeep)
                {
                    simplifiedIndices.Add(i - 1); // 保留索引而不是网格坐标
                    prevDirection = currentDirection;
                }

                // 🔥 如果当前检测到高度变化，下一个点也必须保留
                forceKeepNext = hasHeightChange;
            }

            // 添加终点索引
            simplifiedIndices.Add(dedupedGridPath.Count - 1);

            LogManager.Info($"[网格路径优化] 简化完成：{dedupedGridPath.Count} -> {simplifiedIndices.Count} 个点");

            // 步骤3：根据索引提取PathPoint
            var simplifiedPoints = new List<PathPoint>();
            foreach (int index in simplifiedIndices)
            {
                simplifiedPoints.Add(dedupedPoints[index]);
            }

            // 打印优化后的路径点详情
            LogManager.Info($"[优化后路径详情] 共 {simplifiedPoints.Count} 个点：");
            for (int i = 0; i < simplifiedPoints.Count; i++)
            {
                var pt = simplifiedPoints[i];
                LogManager.Debug($"  [优化后点{i}] 位置=({pt.Position.X:F2}, {pt.Position.Y:F2}, Z={pt.Position.Z:F2}), 限速={pt.SpeedLimit:F1}");
            }

            return simplifiedPoints;
        }

        /// <summary>
        /// 简化共线点（去除直线上的冗余点）
        /// </summary>
        /// <param name="points">原始路径点列表</param>
        /// <returns>简化后的路径点列表</returns>
        private List<PathPoint> SimplifyCollinearPoints(List<PathPoint> points, GridMap gridMap = null)
        {
            if (points.Count <= 2) return points;

            var simplified = new List<PathPoint>();
            simplified.Add(points[0]); // 添加起点

            int removedCount = 0;

            // 遍历中间点，只保留转折点和限速边界点
            for (int i = 1; i < points.Count - 1; i++)
            {
                var prevPoint = points[i - 1];
                var currPoint = points[i];
                var nextPoint = points[i + 1];

                // 检查是否在同一直线上
                bool isCollinear = IsCollinear(prevPoint.Position, currPoint.Position, nextPoint.Position, gridMap);

                // 🔥 关键修复：检查限速变化，即使共线也不能移除限速边界点
                bool hasSpeedLimitChange = HasSpeedLimitChange(prevPoint, currPoint, nextPoint);

                if (!isCollinear || hasSpeedLimitChange)
                {
                    // 这是一个转折点或限速边界点，需要保留
                    simplified.Add(currPoint);

                    if (hasSpeedLimitChange)
                    {
                        LogManager.Debug($"[共线简化] 保留限速边界点 {i}: {prevPoint.SpeedLimit} -> {currPoint.SpeedLimit} -> {nextPoint.SpeedLimit}");
                    }
                }
                else
                {
                    // 这是直线上的冗余点且无限速变化，跳过
                    removedCount++;
                }
            }

            simplified.Add(points[points.Count - 1]); // 添加终点

            LogManager.Info($"[共线简化] 简化完成，移除了 {removedCount} 个冗余点");
            return simplified;
        }

        /// <summary>
        /// 检查两个路径点之间是否有显著的高度变化
        /// 用于路径优化时判断是否需要保留中间点
        /// </summary>
        /// <param name="point1">第一个路径点</param>
        /// <param name="point2">第二个路径点</param>
        /// <returns>如果有高度变化返回true，需要保留中间点</returns>
        private bool HasHeightChange(PathPoint point1, PathPoint point2, GridMap gridMap = null)
        {
            const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差
            double elevation1 = GetElevation(point1.Position, gridMap);
            double elevation2 = GetElevation(point2.Position, gridMap);
            return Math.Abs(elevation1 - elevation2) > heightTolerance;
        }

        /// <summary>
        /// 检查三个点之间是否有高度变化（用于判断是否可以跳过中间点）
        /// </summary>
        /// <param name="p1">第一个点的3D坐标</param>
        /// <param name="p2">中间点的3D坐标</param>
        /// <param name="p3">第三个点的3D坐标</param>
        /// <returns>如果有高度变化返回true，不能跳过中间点</returns>
        private bool HasHeightChange(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null)
        {
            const double heightTolerance = 1e-6; // 仅覆盖浮点数精度误差

            // 检查任意两点间是否有高度变化
            double elevation1 = GetElevation(p1, gridMap);
            double elevation2 = GetElevation(p2, gridMap);
            double elevation3 = GetElevation(p3, gridMap);

            if (Math.Abs(elevation1 - elevation2) > heightTolerance ||
                Math.Abs(elevation2 - elevation3) > heightTolerance)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// 检查三个点之间是否有高度变化（PathPoint版本）
        /// </summary>
        /// <param name="point1">第一个路径点</param>
        /// <param name="point2">中间路径点</param>
        /// <param name="point3">第三个路径点</param>
        /// <returns>如果有高度变化返回true，不能跳过中间点</returns>
        private bool HasHeightChange(PathPoint point1, PathPoint point2, PathPoint point3, GridMap gridMap = null)
        {
            return HasHeightChange(point1.Position, point2.Position, point3.Position, gridMap);
        }

        /// <summary>
        /// 检查两个路径点之间是否有限速变化
        /// 用于路径优化时判断是否需要保留中间点
        /// </summary>
        /// <param name="point1">第一个路径点</param>
        /// <param name="point2">第二个路径点</param>
        /// <returns>如果有限速变化返回true，需要保留中间点</returns>
        private bool HasSpeedLimitChange(PathPoint point1, PathPoint point2)
        {
            const double speedTolerance = 1e-6; // 仅覆盖浮点数精度误差
            return Math.Abs(point1.SpeedLimit - point2.SpeedLimit) > speedTolerance;
        }

        /// <summary>
        /// 检查三个点之间是否有限速变化（用于判断是否可以跳过中间点）
        /// </summary>
        /// <param name="point1">第一个路径点</param>
        /// <param name="point2">中间路径点</param>
        /// <param name="point3">第三个路径点</param>
        /// <returns>如果有限速变化返回true，不能跳过中间点</returns>
        private bool HasSpeedLimitChange(PathPoint point1, PathPoint point2, PathPoint point3)
        {
            const double speedTolerance = 1e-6; // 仅覆盖浮点数精度误差

            // 检查任意两点间是否有限速变化
            if (Math.Abs(point1.SpeedLimit - point2.SpeedLimit) > speedTolerance ||
                Math.Abs(point2.SpeedLimit - point3.SpeedLimit) > speedTolerance)
            {
                return true;
            }
            return false;
        }

        /// <summary>
        /// 检查三点是否真正共线（只有当三点都在同一条直线上且方向一致时才返回true）
        /// 🔥 关键修复：严格检查两个线段的方向是否一致，防止在转折点处错误合并
        /// </summary>
        /// <param name="p1">第一个点</param>
        /// <param name="p2">第二个点（中间点）</param>
        /// <param name="p3">第三个点</param>
        /// <returns>如果三点在同一直线上且方向一致返回true</returns>
        private bool IsCollinear(Point3D p1, Point3D p2, Point3D p3, GridMap gridMap = null)
        {
            const double tolerance = COLLINEAR_TOLERANCE_METERS; // 容差值，单位：模型单位

            // 🔥 修复：使用统一的高度检查函数
            if (HasHeightChange(p1, p2, p3, gridMap))
            {
                LogManager.Debug($"[共线检测] ❌ 拒绝Z方向变化：({p1.X:F3},{p1.Y:F3},{p1.Z:F3}) -> ({p2.X:F3},{p2.Y:F3},{p2.Z:F3}) -> ({p3.X:F3},{p3.Y:F3},{p3.Z:F3})");
                return false;
            }

            double dx12;
            double dy12;
            double dx23;
            double dy23;

            if (gridMap != null)
            {
                var g1 = gridMap.WorldToGrid(p1);
                var g2 = gridMap.WorldToGrid(p2);
                var g3 = gridMap.WorldToGrid(p3);

                dx12 = g2.X - g1.X;
                dy12 = g2.Y - g1.Y;
                dx23 = g3.X - g2.X;
                dy23 = g3.Y - g2.Y;
            }
            else
            {
                // 回退到历史 ZUp 语义，仅用于未提供网格地图的旧调用链。
                dx12 = p2.X - p1.X;
                dy12 = p2.Y - p1.Y;
                dx23 = p3.X - p2.X;
                dy23 = p3.Y - p2.Y;
            }

            // 🔥 修复：使用更严格的容差检查真正的重复点（坐标完全相同）
            const double duplicatePointTolerance = DUPLICATE_POINT_TOLERANCE_METERS; // 用很小的值检查真正的重复点
            bool isSegment1Zero = Math.Abs(dx12) < duplicatePointTolerance && Math.Abs(dy12) < duplicatePointTolerance;
            bool isSegment2Zero = Math.Abs(dx23) < duplicatePointTolerance && Math.Abs(dy23) < duplicatePointTolerance;
            
            if (isSegment1Zero || isSegment2Zero)
            {
                // 🔥 在真正重复点时才认为共线
                return true;  
            }

            // 🔥 关键修复：严格检查两个线段是否都是水平或都是垂直
            bool isSegment1Horizontal = Math.Abs(dy12) < tolerance && Math.Abs(dx12) > tolerance;
            bool isSegment1Vertical = Math.Abs(dx12) < tolerance && Math.Abs(dy12) > tolerance;
            
            bool isSegment2Horizontal = Math.Abs(dy23) < tolerance && Math.Abs(dx23) > tolerance;
            bool isSegment2Vertical = Math.Abs(dx23) < tolerance && Math.Abs(dy23) > tolerance;

            // 🔥 核心逻辑：两个线段必须都是同一种类型（都水平或都垂直）
            if (isSegment1Horizontal && isSegment2Horizontal)
            {
                // 检查水平方向是否一致（同向或反向都可以）
                return (dx12 * dx23 > 0) || Math.Abs(dx12) < tolerance || Math.Abs(dx23) < tolerance;
            }
            else if (isSegment1Vertical && isSegment2Vertical)
            {
                // 检查垂直方向是否一致（同向或反向都可以）
                return (dy12 * dy23 > 0) || Math.Abs(dy12) < tolerance || Math.Abs(dy23) < tolerance;
            }
            else
            {
                // 🔥 关键：不同类型的线段（一个水平一个垂直，或包含斜线）一律拒绝
                string seg1Type = isSegment1Horizontal ? "水平" : (isSegment1Vertical ? "垂直" : "斜线");
                string seg2Type = isSegment2Horizontal ? "水平" : (isSegment2Vertical ? "垂直" : "斜线");
                return false;
            }
        }

        private static double GetElevation(Point3D point, GridMap gridMap)
        {
            return gridMap?.GetWorldElevation(point) ?? point.Z;
        }

        /// <summary>
        /// 创建优化后的路径对象
        /// </summary>
        /// <param name="originalPath">原始路径</param>
        /// <param name="optimizedPoints">优化后的路径点</param>
        /// <returns>优化后的路径对象</returns>
        private PathRoute CreateOptimizedRoute(PathRoute originalPath, List<PathPoint> optimizedPoints)
        {
            var optimizedRoute = new PathRoute($"{originalPath.Name}_优化")
            {
                Id = originalPath.Id,
                IsComplete = originalPath.IsComplete,
                OriginalEndPoint = originalPath.OriginalEndPoint,
                ActualEndPoint = originalPath.ActualEndPoint,
                CompletionPercentage = originalPath.CompletionPercentage
            };

            // 重新分配索引和更新点类型
            for (int i = 0; i < optimizedPoints.Count; i++)
            {
                var point = optimizedPoints[i];
                point.Index = i;

                // 重新确定点类型
                if (i == 0)
                    point.Type = PathPointType.StartPoint;
                else if (i == optimizedPoints.Count - 1)
                    point.Type = PathPointType.EndPoint;
                else
                    point.Type = PathPointType.WayPoint;

                optimizedRoute.Points.Add(point);
            }

            // 重新计算路径数据（但不保存到数据库，由调用方决定）
            optimizedRoute.RecalculateRoute("路径优化");

            return optimizedRoute;
        }

        #region 未来扩展方法（暂时保留接口）

        /// <summary>
        /// 路径平滑处理（未来实现）
        /// </summary>
        /// <param name="points">路径点列表</param>
        /// <returns>平滑后的路径点列表</returns>
        private List<PathPoint> SmoothPath(List<PathPoint> points)
        {
            // TODO: 实现贝塞尔曲线或圆弧平滑
            LogManager.Info("[路径平滑] 功能尚未实现");
            return points;
        }

        /// <summary>
        /// 在转角处添加圆弧（未来实现）
        /// </summary>
        /// <param name="points">路径点列表</param>
        /// <returns>添加圆弧后的路径点列表</returns>
        private List<PathPoint> AddTurningArcs(List<PathPoint> points)
        {
            // TODO: 在转角处添加圆弧
            LogManager.Info("[圆弧转弯] 功能尚未实现");
            return points;
        }

        #endregion
    }
}
