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

namespace NavisworksTransport
{
    /// <summary>
    /// 路径曲线化引擎
    /// 实现基于圆弧过渡（Arc Fillet）的路径曲线化功能
    /// </summary>
    public static class PathCurveEngine
    {
        /// <summary>
        /// 将值限制在指定范围内（.NET Framework 4.8 不支持 Math.Clamp）
        /// </summary>
        private static double Clamp(double value, double min, double max)
        {
            if (value < min) return min;
            if (value > max) return max;
            return value;
        }

        /// <summary>
        /// 计算圆弧切点和轨迹参数
        /// </summary>
        /// <param name="pPrev">前一个控制点</param>
        /// <param name="pCurr">当前控制点</param>
        /// <param name="pNext">下一个控制点</param>
        /// <param name="turnRadius">转向半径（模型单位）</param>
        /// <param name="warningMessage">警告信息（如果有）</param>
        /// <returns>圆弧轨迹数据</returns>
        public static ArcTrajectory CalculateFillet(
            Point3D pPrev,
            Point3D pCurr,
            Point3D pNext,
            double turnRadius,
            out string warningMessage)
        {
            // 初始化警告信息
            warningMessage = null;

            // 1. 计算单位向量
            Vector3D v1 = (pPrev - pCurr).Normalize();
            Vector3D v2 = (pNext - pCurr).Normalize();

            // 2. 计算夹角 α（有向角度，考虑转向方向）
            double cosAlpha = GeometryHelper.DotProduct(
                new Point3D(v1.X, v1.Y, v1.Z),
                new Point3D(v2.X, v2.Y, v2.Z));
            
            // 计算叉乘以确定转向方向
            Point3D cross = GeometryHelper.CrossProduct(
                new Point3D(v1.X, v1.Y, v1.Z),
                new Point3D(v2.X, v2.Y, v2.Z));
            double crossMag = new Vector3D(cross.X, cross.Y, cross.Z).Length;
            
            // 使用Acos计算两个向量之间的夹角（范围 [0, π]）
            // 限制cosAlpha在[-1, 1]范围内以避免数值误差
            double clampedCosAlpha = Math.Max(-1.0, Math.Min(1.0, cosAlpha));
            double angleRad = Math.Acos(clampedCosAlpha);
            
            LogManager.Debug($"[CalculateFillet] 角度计算: cosAlpha={cosAlpha:F4}, clampedCosAlpha={clampedCosAlpha:F4}, angleRad={angleRad:F4}rad ({angleRad * 180.0 / Math.PI:F2}度)");

            // 如果几乎共线，返回无效轨迹
            if (angleRad < 0.01 || angleRad > Math.PI - 0.01)
            {
                return new ArcTrajectory
                {
                    Ts = pCurr,
                    Te = pCurr,
                    ArcCenter = pCurr,
                    RequestedRadius = turnRadius,
                    ActualRadius = 0,
                    DeflectionAngle = angleRad,
                    ArcLength = 0
                };
            }

            // 3. 计算切线长 Lt = R / tan(α/2)
            double Lt = turnRadius / Math.Tan(angleRad / 2.0);

            // 4. 安全截断检查
            double seg1Length = (pPrev - pCurr).Length;
            double seg2Length = (pNext - pCurr).Length;
            double maxAllowedLt = Math.Min(seg1Length, seg2Length) * 0.45;

            double actualRadius = turnRadius;
            bool radiusTruncated = false;
            if (Lt > maxAllowedLt)
            {
                // 记录截断前的参数
                double originalLt = Lt;
                double originalRadius = turnRadius;

                Lt = maxAllowedLt;
                actualRadius = Lt * Math.Tan(angleRad / 2.0);
                radiusTruncated = true;

                // 将模型单位转换为米单位用于日志显示
                double originalRadiusMeters = UnitsConverter.ConvertToMeters(originalRadius);
                double actualRadiusMeters = UnitsConverter.ConvertToMeters(actualRadius);

                // 发出警告（日志）- 显示米单位
                LogManager.Warning($"圆弧半径被安全截断: 配置半径={originalRadiusMeters:F2}米, 实际使用={actualRadiusMeters:F2}米");

                // 设置用户友好的警告信息
                double radiusDiff = originalRadiusMeters - actualRadiusMeters;
                double radiusDiffPercent = (radiusDiff / originalRadiusMeters) * 100.0;
                warningMessage = $"警告：转弯半径被自动缩小了 {radiusDiffPercent:F0}%（从 {originalRadiusMeters:F1}米 减小到 {actualRadiusMeters:F1}米），因为路径段太短。建议：增加路径点之间的距离，或者减小转弯半径设置。";
            }

            // 5. 计算切点
            Point3D ts = pCurr + v1 * Lt;
            Point3D te = pCurr + v2 * Lt;

// 6. 计算圆心 (使用角平分线方向)
            Vector3D bisector = (v1 + v2).Normalize();
            double distToCenter = actualRadius / Math.Sin(angleRad / 2.0);
            Point3D arcCenter = pCurr + bisector * distToCenter;

            // 7. 计算圆弧长度和实际偏转角度
            // 始终使用Ts和Te到圆心的向量之间的实际夹角，确保角度正确
            Vector3D tsToCenter = (ts - arcCenter).Normalize();
            Vector3D teToCenter = (te - arcCenter).Normalize();
            double dotTsTe = GeometryHelper.DotProduct(
                new Point3D(tsToCenter.X, tsToCenter.Y, tsToCenter.Z),
                new Point3D(teToCenter.X, teToCenter.Y, teToCenter.Z));
            
            // 限制在[-1, 1]范围内
            dotTsTe = Math.Max(-1.0, Math.Min(1.0, dotTsTe));
            double actualDeflectionAngle = Math.Acos(dotTsTe);

            LogManager.Debug($"[CalculateFillet] 计算实际偏转角度:");
            LogManager.Debug($"  Ts=({ts.X:F3},{ts.Y:F3},{ts.Z:F3}), Te=({te.X:F3},{te.Y:F3},{te.Z:F3})");
            LogManager.Debug($"  ArcCenter=({arcCenter.X:F3},{arcCenter.Y:F3},{arcCenter.Z:F3})");
            LogManager.Debug($"  Ts到圆心向量: ({tsToCenter.X:F3},{tsToCenter.Y:F3},{tsToCenter.Z:F3})");
            LogManager.Debug($"  Te到圆心向量: ({teToCenter.X:F3},{teToCenter.Y:F3},{teToCenter.Z:F3})");
            LogManager.Debug($"  点积: {dotTsTe:F4}");
            LogManager.Debug($"  实际偏转角度: {actualDeflectionAngle * 180.0 / Math.PI:F2}度");
            LogManager.Debug($"  半径截断: {radiusTruncated}, 请求半径: {UnitsConverter.ConvertToMeters(turnRadius):F2}米, 实际半径: {UnitsConverter.ConvertToMeters(actualRadius):F2}米");

            double arcLength = actualRadius * actualDeflectionAngle;

            return new ArcTrajectory
            {
                Ts = ts,
                Te = te,
                ArcCenter = arcCenter,
                RequestedRadius = turnRadius,
                ActualRadius = actualRadius,
                DeflectionAngle = actualDeflectionAngle,
                ArcLength = arcLength
            };
        }

        /// <summary>
        /// Rodrigues旋转公式 - 绕任意轴旋转点
        /// </summary>
        /// <param name="point">要旋转的点</param>
        /// <param name="center">旋转中心</param>
        /// <param name="axis">旋转轴（单位向量）</param>
        /// <param name="angle">旋转角度（弧度）</param>
        /// <returns>旋转后的点</returns>
        // RotatePointAroundAxis方法已移到GeometryHelper工具类中

        /// <summary>
        /// 圆弧采样为离散点序列
        /// </summary>
        /// <param name="trajectory">圆弧轨迹</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>采样点列表</returns>
        public static List<Point3D> SampleArc(
            ArcTrajectory trajectory,
            double samplingStep)
        {
            var points = new List<Point3D>();

            // 如果圆弧长度很小，直接返回起止点
            if (trajectory.ArcLength < 0.001)
            {
                points.Add(trajectory.Ts);
                points.Add(trajectory.Te);
                return points;
            }

            // 打印角度信息
            double angleDegrees = trajectory.DeflectionAngle * 180.0 / Math.PI;
            LogManager.Debug($"圆弧采样: 角度={angleDegrees:F2}度, 弧度={trajectory.DeflectionAngle:F4}, 半径={trajectory.ActualRadius:F2}m, 长度={trajectory.ArcLength:F2}m");
            LogManager.Debug($"  Ts=({trajectory.Ts.X:F3},{trajectory.Ts.Y:F3},{trajectory.Ts.Z:F3}), Te=({trajectory.Te.X:F3},{trajectory.Te.Y:F3},{trajectory.Te.Z:F3})");

            // 根据采样步长计算采样点数量
            int sampleCount = Math.Max(2, (int)Math.Ceiling(trajectory.ArcLength / samplingStep));

            // 计算起始和结束向量
            Vector3D startVec = (trajectory.Ts - trajectory.ArcCenter).Normalize();
            Vector3D endVec = (trajectory.Te - trajectory.ArcCenter).Normalize();

            // 计算旋转轴（叉乘）
            Point3D cross = GeometryHelper.CrossProduct(
                new Point3D(startVec.X, startVec.Y, startVec.Z),
                new Point3D(endVec.X, endVec.Y, endVec.Z));
            Vector3D rotationAxis = new Vector3D(cross.X, cross.Y, cross.Z).Normalize();

            LogManager.Debug($"  旋转轴=({rotationAxis.X:F6},{rotationAxis.Y:F6},{rotationAxis.Z:F6})");

            // 计算有向旋转角度（使用 Atan2 而不是 Acos，以保留方向信息）
            double dot = GeometryHelper.DotProduct(
                new Point3D(startVec.X, startVec.Y, startVec.Z),
                new Point3D(endVec.X, endVec.Y, endVec.Z));
            double crossMag = new Vector3D(cross.X, cross.Y, cross.Z).Length;

            // 使用 Atan2 计算有向角度，确保旋转方向正确
            double signedAngle = Math.Atan2(crossMag, dot);
            LogManager.Debug($"  重新计算的有向角度: {signedAngle:F4}rad ({signedAngle * 180.0 / Math.PI:F2}度)");
            LogManager.Debug($"  使用DeflectionAngle: {trajectory.DeflectionAngle:F4}rad ({trajectory.DeflectionAngle * 180.0 / Math.PI:F2}度)");

            // 使用trajectory.DeflectionAngle而不是重新计算的角度
            signedAngle = trajectory.DeflectionAngle;

            // 等角度插值
            for (int i = 0; i <= sampleCount; i++)
            {
                double t = i / (double)sampleCount;
                double theta = t * signedAngle;

                // Rodrigues旋转公式
                Point3D sampledPoint = GeometryHelper.RotatePointAroundAxis(
                    trajectory.Ts,
                    trajectory.ArcCenter,
                    rotationAxis,
                    theta
                );

                if (i == 0 || i == sampleCount)
                {
                    LogManager.Debug($"  采样点[{i}/{sampleCount}]: t={t:F4}, theta={theta:F4}rad, 坐标=({sampledPoint.X:F3},{sampledPoint.Y:F3},{sampledPoint.Z:F3})");
                }

                points.Add(sampledPoint);
            }

            // 验证最后一个点是否接近Te
            if (points.Count > 0)
            {
                var lastPoint = points.Last();
                double distanceToTe = (lastPoint - trajectory.Te).Length;
                LogManager.Debug($"  终点验证: 目标Te=({trajectory.Te.X:F3},{trajectory.Te.Y:F3},{trajectory.Te.Z:F3}), 实际终点=({lastPoint.X:F3},{lastPoint.Y:F3},{lastPoint.Z:F3}), 误差={distanceToTe:F6}m");
                if (distanceToTe > 0.001)
                {
                    LogManager.Warning($"  圆弧采样终点误差: {distanceToTe:F6}m");
                }
            }

            return points;
        }

        /// <summary>
        /// 构建直线边
        /// </summary>
        /// <param name="p1">起始点</param>
        /// <param name="p2">结束点</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>路径边</returns>
        private static PathEdge BuildStraightEdge(
            PathPoint p1,
            PathPoint p2,
            double samplingStep)
        {
            var edge = new PathEdge
            {
                StartPointId = p1.Id,
                EndPointId = p2.Id,
                SegmentType = PathSegmentType.Straight,
                PhysicalLength = (p1.Position - p2.Position).Length
            };

            // 采样直线段
            int sampleCount = Math.Max(2, (int)Math.Ceiling(edge.PhysicalLength / samplingStep));
            edge.SampledPoints = new List<Point3D>();

            for (int i = 0; i <= sampleCount; i++)
            {
                double t = i / (double)sampleCount;
                Point3D sampledPoint = p1.Position + (p2.Position - p1.Position) * t;
                edge.SampledPoints.Add(sampledPoint);
            }

            return edge;
        }

        /// <summary>
        /// 构建包含圆弧的边
        /// </summary>
        /// <param name="p1">起始点</param>
        /// <param name="p2">结束点</param>
        /// <param name="arcTrajectory">圆弧轨迹</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>路径边</returns>
        private static PathEdge BuildEdgeWithArc(
            PathPoint p1,
            PathPoint p2,
            ArcTrajectory arcTrajectory,
            double samplingStep)
        {
            var edge = new PathEdge
            {
                StartPointId = p1.Id,
                EndPointId = p2.Id,
                SegmentType = PathSegmentType.Arc,
                Trajectory = arcTrajectory,
                PhysicalLength = arcTrajectory.ArcLength
            };

            // 采样圆弧段
            edge.SampledPoints = SampleArc(arcTrajectory, samplingStep);

            return edge;
        }

        /// <summary>
        /// 从两个点构建直线边
        /// </summary>
        /// <param name="p1">起始点</param>
        /// <param name="p2">结束点</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>路径边</returns>
        private static PathEdge BuildStraightEdgeFromPoints(
            Point3D p1,
            Point3D p2,
            double samplingStep)
        {
            var edge = new PathEdge
            {
                StartPointId = "",
                EndPointId = "",
                SegmentType = PathSegmentType.Straight,
                PhysicalLength = (p1 - p2).Length
            };

            // 采样直线段
            int sampleCount = Math.Max(2, (int)Math.Ceiling(edge.PhysicalLength / samplingStep));
            edge.SampledPoints = new List<Point3D>();

            for (int i = 0; i <= sampleCount; i++)
            {
                double t = i / (double)sampleCount;
                Point3D sampledPoint = p1 + (p2 - p1) * t;
                edge.SampledPoints.Add(sampledPoint);
            }

            return edge;
        }

        /// <summary>
        /// 从圆弧轨迹构建圆弧边
        /// </summary>
        /// <param name="arcTrajectory">圆弧轨迹</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>路径边</returns>
        private static PathEdge BuildEdgeWithArcFromTrajectory(
            ArcTrajectory arcTrajectory,
            double samplingStep)
        {
            var edge = new PathEdge
            {
                StartPointId = "",
                EndPointId = "",
                SegmentType = PathSegmentType.Arc,
                Trajectory = arcTrajectory,
                PhysicalLength = arcTrajectory.ArcLength
            };

            // 采样圆弧段
            edge.SampledPoints = SampleArc(arcTrajectory, samplingStep);

            return edge;
        }

        /// <summary>
        /// 生成采样点序列（用于延迟加载）
        /// </summary>
        /// <param name="edge">路径边</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <returns>采样点列表</returns>
        public static List<Point3D> GenerateSampledPoints(PathEdge edge, double samplingStep)
        {
            if (edge.SegmentType == PathSegmentType.Straight)
            {
                // 直线段采样
                int sampleCount = Math.Max(2, (int)Math.Ceiling(edge.PhysicalLength / samplingStep));
                var points = new List<Point3D>();

                for (int i = 0; i <= sampleCount; i++)
                {
                    double t = i / (double)sampleCount;
                    Point3D p1 = edge.Trajectory?.Ts ?? edge.Trajectory?.Te ?? new Point3D();
                    Point3D p2 = edge.Trajectory?.Te ?? new Point3D();
                    Point3D sampledPoint = p1 + (p2 - p1) * t;
                    points.Add(sampledPoint);
                }

                return points;
            }
            else if (edge.SegmentType == PathSegmentType.Arc && edge.Trajectory != null)
            {
                // 圆弧段采样
                return SampleArc(edge.Trajectory, samplingStep);
            }

            return new List<Point3D>();
        }

        /// <summary>
        /// 对路径应用曲线化处理
        /// </summary>
        /// <param name="route">待处理的路径</param>
        /// <param name="samplingStep">采样步长（米）</param>
        /// <param name="warningMessages">警告信息列表（如果有）</param>
        public static void ApplyCurvatureToRoute(PathRoute route, double samplingStep, out List<string> warningMessages)
        {
            if (route == null)
                throw new ArgumentNullException(nameof(route));

            warningMessages = new List<string>();
            route.Edges.Clear();
            var sortedPoints = route.Points.OrderBy(p => p.Index).ToList();

            if (sortedPoints.Count < 2)
            {
                route.IsCurved = false;
                return;
            }

            if (sortedPoints.Count < 3)
            {
                // 只有2个点，直接生成一条直线边
                var edge = BuildStraightEdge(sortedPoints[0], sortedPoints[1], samplingStep);
                route.Edges.Add(edge);
                route.IsCurved = true;
                return;
            }

            // 第一步：计算所有转折点的圆弧轨迹和切点
            var arcTrajectories = new Dictionary<int, ArcTrajectory>();

            for (int i = 1; i < sortedPoints.Count - 1; i++)
            {
                var prevPoint = sortedPoints[i - 1];
                var currPoint = sortedPoints[i];
                var nextPoint = sortedPoints[i + 1];

                // 获取转向半径（优先使用自定义值）
                double radius = currPoint.CustomTurnRadius ?? route.TurnRadius;

                // 计算圆弧轨迹
                string warning;
                var arcTraj = CalculateFillet(prevPoint.Position, currPoint.Position, nextPoint.Position, radius, out warning);
                arcTrajectories[i] = arcTraj;

                // 收集警告信息
                if (!string.IsNullOrEmpty(warning))
                {
                    warningMessages.Add(warning);
                }
            }

            // 第二步：构建边集合
            // 从起点开始
            Point3D currentPos = sortedPoints[0].Position;

            // 处理每个转折点
            for (int i = 1; i < sortedPoints.Count - 1; i++)
            {
                var arcTraj = arcTrajectories[i];

                // 1. 当前位置 -> 进入切点（直线段）
                var edge1 = BuildStraightEdgeFromPoints(currentPos, arcTraj.Ts, samplingStep);
                route.Edges.Add(edge1);

                // 2. 进入切点 -> 退出切点（圆弧段）
                var edge2 = BuildEdgeWithArcFromTrajectory(arcTraj, samplingStep);
                route.Edges.Add(edge2);

                // 更新当前位置为退出切点
                currentPos = arcTraj.Te;
            }

            // 3. 最后一个退出切点 -> 终点（直线段）
            var lastEdge = BuildStraightEdgeFromPoints(currentPos, sortedPoints[sortedPoints.Count - 1].Position, samplingStep);
            route.Edges.Add(lastEdge);

            route.IsCurved = true;
        }

        /// <summary>
        /// 【已废弃】重新计算路径总长度
        /// 
        /// TotalLength 现在是计算属性，自动从 Edges/Points 实时计算，无需手动调用此方法。
        /// 保留此方法用于兼容，但实际不执行任何操作。
        /// </summary>
        /// <param name="route">路径</param>
        [Obsolete("TotalLength 是计算属性，自动实时计算，无需调用此方法。")]
        public static void RecalculateRouteLength(PathRoute route)
        {
            // 不再设置 route.TotalLength，因为它现在是只读计算属性
            // 长度会自动从 Edges 或 Points 计算
        }
    }
}
