using Flyshot.Core.Domain;
using Microsoft.Extensions.Logging;

namespace Flyshot.Core.Planning;

/// <summary>
/// 在 ICSP 外层包裹补中点策略，实现 self-adapt-icsp 行为。
///
/// 为什么需要这层？
/// ---
/// 逆向分析已经指出：原系统里普通 icsp 若仍有段 scale > 1，不会直接返回未收敛结果；
/// 配置中还明确存在 adapt_icsp_try_num。本层把"超限段统一插入中点后再重规划"的逻辑显式落地，
/// 补上 demo 缺失的失败恢复路径。
///
/// 补点策略：
/// ---
/// 对当前所有 scale > 1 + tolerance 的段统一插入关节空间中点，然后把新路点集交给 ICSPPlanner
/// 重新规划。这种"先把明显病灶都降一档，再整体重规划"的策略比逐段拆分更稳定，
/// 也更符合服务端 adapt_icsp_try_num 的意图。
/// </summary>
public sealed class SelfAdaptIcspPlanner
{
    /// <summary>
    /// 判定段是否超限的数值容差，过滤浮点噪声。
    /// </summary>
    public const double ScaleTolerance = 5e-4;

    private readonly ICspPlanner _innerPlanner;
    private readonly ILogger<SelfAdaptIcspPlanner>? _logger;

    /// <summary>
    /// 初始化 SelfAdaptIcspPlanner。
    /// </summary>
    /// <param name="logger">日志记录器；允许 null。</param>
    public SelfAdaptIcspPlanner(ILogger<SelfAdaptIcspPlanner>? logger = null)
    {
        _innerPlanner = new ICspPlanner(enforceFinalScale: false, logger: null);
        _logger = logger;
    }

    /// <summary>
    /// 执行自适应 ICSP 规划，允许在超限段插入中点后重试。
    /// </summary>
    /// <param name="request">轨迹规划请求。</param>
    /// <param name="adaptIcspTryNum">最大补点重试次数（默认 5）。</param>
    /// <returns>规划后的轨迹结果。</returns>
    /// <exception cref="InvalidOperationException">超过最大重试次数仍未收敛时抛出。</exception>
    public PlannedTrajectory Plan(TrajectoryRequest request, int adaptIcspTryNum = 5)
    {
        ArgumentNullException.ThrowIfNull(request);

        var currentWaypoints = request.Program.Waypoints.ToArray();
        var currentShotFlags = request.Program.ShotFlags.ToArray();
        var currentOffsets = request.Program.OffsetValues.ToArray();
        var currentAddrs = request.Program.AddressGroups.ToArray();
        int originalWaypointCount = currentWaypoints.Length;

        if (originalWaypointCount < 4)
        {
            throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
        }

        _logger?.LogInformation(
            "SelfAdaptICSP 规划开始: 名称={Name}, 原始路点数={WaypointCount}, 最大补点次数={MaxAttempts}",
            request.Program.Name, originalWaypointCount, adaptIcspTryNum);

        var currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
        var currentRequest = new TrajectoryRequest(
            robot: request.Robot,
            program: currentProgram,
            method: PlanningMethod.SelfAdaptIcsp,
            moveToStart: request.MoveToStart,
            saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
            useCache: request.UseCache);

        PlannedTrajectory? lastTrajectory = null;
        int maxAttempts = Math.Max(0, adaptIcspTryNum);

        for (int attempt = 0; attempt <= maxAttempts; attempt++)
        {
            var trajectory = _innerPlanner.Plan(currentRequest);
            lastTrajectory = trajectory;

            var badSegments = new List<int>();
            for (int seg = 0; seg < trajectory.SegmentScales.Count; seg++)
            {
                if (trajectory.SegmentScales[seg] > 1.0 + ScaleTolerance)
                {
                    badSegments.Add(seg);
                }
            }

            if (badSegments.Count == 0)
            {
                _logger?.LogInformation(
                    "SelfAdaptICSP 规划完成: 名称={Name}, 补点轮数={Attempts}, 最终路点数={WaypointCount}, 迭代次数={Iterations}, 总时长={Duration:F4}s",
                    request.Program.Name, attempt, currentWaypoints.Length, trajectory.Iterations, trajectory.WaypointTimes[^1]);
                // 所有段都满足约束，收敛成功。返回包含补中点后路点的轨迹。
                return new PlannedTrajectory(
                    robot: trajectory.Robot,
                    originalProgram: request.Program,
                    plannedWaypoints: currentWaypoints,
                    waypointTimes: trajectory.WaypointTimes,
                    segmentDurations: trajectory.SegmentDurations,
                    segmentScales: trajectory.SegmentScales,
                    method: PlanningMethod.SelfAdaptIcsp,
                    iterations: trajectory.Iterations,
                    threshold: trajectory.Threshold);
            }

            _logger?.LogWarning(
                "SelfAdaptICSP 第 {Attempt} 轮存在超限段: 超限段数={BadCount}, 段索引=[{Segments}], 最大缩放={MaxScale:F4}",
                attempt, badSegments.Count, string.Join(", ", badSegments), trajectory.SegmentScales.Max());

            if (attempt >= maxAttempts)
            {
                break;
            }

            // 对超限段插入中点，并同步扩展 shot 元数据。
            int waypointCountBefore = currentWaypoints.Length;
            (currentWaypoints, currentShotFlags, currentOffsets, currentAddrs) =
                InsertSegmentMidpoints(currentWaypoints, currentShotFlags, currentOffsets, currentAddrs, badSegments);

            _logger?.LogDebug(
                "SelfAdaptICSP 补中点: 路点数 {Before} -> {After}, 插入段=[{Segments}]",
                waypointCountBefore, currentWaypoints.Length, string.Join(", ", badSegments));

            currentProgram = BuildProgram(request.Program.Name, currentWaypoints, currentShotFlags, currentOffsets, currentAddrs);
            currentRequest = new TrajectoryRequest(
                robot: request.Robot,
                program: currentProgram,
                method: PlanningMethod.SelfAdaptIcsp,
                moveToStart: request.MoveToStart,
                saveTrajectoryArtifacts: request.SaveTrajectoryArtifacts,
                useCache: request.UseCache);
        }

        double maxScale = lastTrajectory?.SegmentScales.Max() ?? double.NaN;
        _logger?.LogError(
            "SelfAdaptICSP 规划失败: 名称={Name}, 在 {Attempts} 轮补点后仍未收敛, 最大段缩放因子={MaxScale:F6}",
            request.Program.Name, adaptIcspTryNum, maxScale);
        throw new InvalidOperationException(
            $"self-adapt ICSP 在 {adaptIcspTryNum} 轮补点后仍未收敛，最大段缩放因子={maxScale:F6}。");
    }

    /// <summary>
    /// 用当前路点集和元数据构造 FlyshotProgram。
    /// </summary>
    private static FlyshotProgram BuildProgram(
        string name,
        JointWaypoint[] waypoints,
        bool[] shotFlags,
        int[] offsets,
        IoAddressGroup[] addrs)
    {
        return new FlyshotProgram(
            name: name,
            waypoints: waypoints,
            shotFlags: shotFlags,
            offsetValues: offsets,
            addressGroups: addrs);
    }

    /// <summary>
    /// 对超限段统一插入关节空间中点，并同步扩展 shot 元数据。
    /// 新插入的路点默认 shotFlag=false、offset=0、addr=空。
    /// </summary>
    private static (JointWaypoint[] waypoints, bool[] shotFlags, int[] offsets, IoAddressGroup[] addrs)
        InsertSegmentMidpoints(
            JointWaypoint[] waypoints,
            bool[] shotFlags,
            int[] offsets,
            IoAddressGroup[] addrs,
            List<int> badSegments)
    {
        if (badSegments.Count == 0)
        {
            return (waypoints, shotFlags, offsets, addrs);
        }

        var badSet = new HashSet<int>(badSegments);
        var newWaypoints = new List<JointWaypoint>(waypoints.Length + badSegments.Count);
        var newShotFlags = new List<bool>(waypoints.Length + badSegments.Count);
        var newOffsets = new List<int>(waypoints.Length + badSegments.Count);
        var newAddrs = new List<IoAddressGroup>(waypoints.Length + badSegments.Count);

        newWaypoints.Add(waypoints[0]);
        newShotFlags.Add(shotFlags[0]);
        newOffsets.Add(offsets[0]);
        newAddrs.Add(addrs[0]);

        for (int seg = 0; seg < waypoints.Length - 1; seg++)
        {
            if (badSet.Contains(seg))
            {
                var mid = new JointWaypoint(
                    waypoints[seg].Positions.Zip(waypoints[seg + 1].Positions, (a, b) => (a + b) / 2.0));
                newWaypoints.Add(mid);
                newShotFlags.Add(false);
                newOffsets.Add(0);
                newAddrs.Add(new IoAddressGroup(Array.Empty<int>()));
            }

            newWaypoints.Add(waypoints[seg + 1]);
            newShotFlags.Add(shotFlags[seg + 1]);
            newOffsets.Add(offsets[seg + 1]);
            newAddrs.Add(addrs[seg + 1]);
        }

        return (newWaypoints.ToArray(), newShotFlags.ToArray(), newOffsets.ToArray(), newAddrs.ToArray());
    }
}
