FlyShotHost/src/Flyshot.Core.Planning/ICspPlanner.cs

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

namespace Flyshot.Core.Planning;

/// <summary>
/// 基于 CubicSpline + 逐段时间缩放迭代的 ICSP 规划器。
///
/// 算法核心逻辑（与逆向文档一致）：
/// 1. 初始段时长取关节空间欧氏距离；
/// 2. 每轮用当前时长构造 CubicSpline，解析求每段 1/2/3 阶导峰值；
/// 3. 按速度一次方、加速度平方根、jerk 立方根缩放时长；
/// 4. 以 sum(|scale_i - 1|) 为收敛指标，保存历史最优结果；
/// 5. 最终用最优时长构造 CubicSpline 并输出时间轴。
/// </summary>
public sealed class ICspPlanner
{
    /// <summary>
    /// 默认收敛阈值，对应原实现中的 1e-3。
    /// </summary>
    public const double DefaultThreshold = 1e-3;

    /// <summary>
    /// 默认最大迭代轮数，对应原实现中的 1000。
    /// </summary>
    public const int DefaultMaxIterations = 1000;

    /// <summary>
    /// 默认最终 scale 容差。当前 C# spline 与旧系统对齐样本存在约 1% 内的数值余量。
    /// </summary>
    public const double DefaultFinalScaleTolerance = 1e-2;

    private readonly double _threshold;
    private readonly int _maxIterations;
    private readonly bool _enforceFinalScale;
    private readonly double _finalScaleTolerance;
    private readonly ILogger<ICspPlanner>? _logger;

    /// <summary>
    /// 初始化 ICSP 规划器。
    /// </summary>
    /// <param name="threshold">收敛阈值。</param>
    /// <param name="maxIterations">最大迭代轮数。</param>
    /// <param name="enforceFinalScale">是否在最终最优 scale 仍大于 1.0 时抛出失败。</param>
    /// <param name="finalScaleTolerance">最终 scale 判定容差。</param>
    /// <param name="logger">日志记录器；允许 null，供无日志场景使用。</param>
    public ICspPlanner(
        double threshold = DefaultThreshold,
        int maxIterations = DefaultMaxIterations,
        bool enforceFinalScale = true,
        double finalScaleTolerance = DefaultFinalScaleTolerance,
        ILogger<ICspPlanner>? logger = null)
    {
        if (threshold <= 0.0 || double.IsNaN(threshold) || double.IsInfinity(threshold))
        {
            throw new ArgumentOutOfRangeException(nameof(threshold), "收敛阈值必须为有限正数。");
        }

        if (maxIterations < 0)
        {
            throw new ArgumentOutOfRangeException(nameof(maxIterations), "最大迭代轮数不能为负数。");
        }

        if (finalScaleTolerance < 0.0 || double.IsNaN(finalScaleTolerance) || double.IsInfinity(finalScaleTolerance))
        {
            throw new ArgumentOutOfRangeException(nameof(finalScaleTolerance), "最终 scale 容差必须为有限非负数。");
        }

        _threshold = threshold;
        _maxIterations = maxIterations;
        _enforceFinalScale = enforceFinalScale;
        _finalScaleTolerance = finalScaleTolerance;
        _logger = logger;
    }

    /// <summary>
    /// 执行 ICSP 规划，返回包含完整时间轴和收敛信息的轨迹。
    /// </summary>
    public PlannedTrajectory Plan(TrajectoryRequest request)
    {
        ArgumentNullException.ThrowIfNull(request);

        var waypoints = request.Program.Waypoints;
        if (waypoints.Count < 4)
        {
            throw new ArgumentException("ICSP 至少需要 4 个示教点。", nameof(request));
        }

        _logger?.LogInformation(
            "ICSP 规划开始: 名称={Name}, 路点数={WaypointCount}, 自由度={Dof}, threshold={Threshold}, maxIterations={MaxIterations}",
            request.Program.Name, waypoints.Count, request.Robot.DegreesOfFreedom, _threshold, _maxIterations);
        _logger?.LogDebug(
            "ICSP 输入路点: {Waypoints}",
            string.Join(" | ", waypoints.Select(wp => $"[{string.Join(", ", wp.Positions.Select(j => j.ToString("F4")))}]")));

        var qs = WaypointsToArray(waypoints);
        var (velLimits, accLimits, jerkLimits) = ExtractLimits(request.Robot);

        _logger?.LogDebug(
            "ICSP 约束限值: vel=[{Vel}], acc=[{Acc}], jerk=[{Jerk}]",
            string.Join(", ", velLimits.Select(v => v.ToString("F2"))),
            string.Join(", ", accLimits.Select(a => a.ToString("F2"))),
            string.Join(", ", jerkLimits.Select(j => j.ToString("F2"))));

        // 初始段时长直接取相邻示教点的关节空间欧氏距离。
        var segmentDurations = ComputeInitialDurations(qs);
        int nseg = segmentDurations.Length;
        int dof = qs[0].Length;

        double bestThreshold = double.PositiveInfinity;
        double[]? bestDurations = null;
        double[]? bestScales = null;
        CubicSplineInterpolator? bestSpline = null;
        int bestIterations = 0;
        double[]? bestWaypointTimes = null;

        for (int iteration = 0; iteration <= _maxIterations; iteration++)
        {
            var waypointTimes = CumulativeTimes(segmentDurations);
            var spline = new CubicSplineInterpolator(waypointTimes, qs);
            var (maxDq, maxDdq, maxDddq) = spline.SegmentMaxAbsDerivatives();

            var scales = new double[nseg];
            for (int seg = 0; seg < nseg; seg++)
            {
                double segScale = 0.0;
                for (int d = 0; d < dof; d++)
                {
                    double sv = Math.Abs(maxDq[seg, d] / velLimits[d]);
                    double sa = Math.Sqrt(Math.Abs(maxDdq[seg, d] / accLimits[d]));
                    double sj = Math.Cbrt(Math.Abs(maxDddq[seg, d] / jerkLimits[d]));
                    segScale = Math.Max(segScale, Math.Max(sv, Math.Max(sa, sj)));
                }

                scales[seg] = segScale;
            }

            double currentThreshold = 0.0;
            for (int seg = 0; seg < nseg; seg++)
            {
                currentThreshold += Math.Abs(scales[seg] - 1.0);
            }

            if (currentThreshold < bestThreshold)
            {
                bestThreshold = currentThreshold;
                bestDurations = (double[])segmentDurations.Clone();
                bestScales = (double[])scales.Clone();
                bestSpline = spline;
                bestIterations = iteration + 1;
                bestWaypointTimes = (double[])waypointTimes.Clone();
            }

            if (currentThreshold < _threshold)
            {
                _logger?.LogDebug(
                    "ICSP 第 {Iteration} 轮收敛: threshold={CurrentThreshold:E6}",
                    iteration + 1, currentThreshold);
                break;
            }

            for (int seg = 0; seg < nseg; seg++)
            {
                segmentDurations[seg] *= scales[seg];
            }
        }

        if (bestSpline is null || bestDurations is null || bestScales is null || bestWaypointTimes is null)
        {
            throw new InvalidOperationException("ICSP 规划未能产生有效结果。");
        }

        var globalScale = bestScales.Max();
        if (_enforceFinalScale && globalScale > 1.0 + _finalScaleTolerance)
        {
            _logger?.LogError(
                "ICSP 规划未收敛: global_scale={GlobalScale:F6} > {Tolerance:F6}, 段缩放=[{Scales}]",
                globalScale, 1.0 + _finalScaleTolerance,
                string.Join(", ", bestScales.Select(s => s.ToString("F4"))));
            throw new InvalidOperationException(
                $"ICSP 规划未收敛，global_scale={globalScale:F6} > {1.0 + _finalScaleTolerance:F6}，轨迹不可执行。");
        }

        _logger?.LogInformation(
            "ICSP 规划完成: 名称={Name}, 迭代轮数={Iterations}, 收敛阈值={Threshold:E6}, 总时长={Duration:F4}s, global_scale={GlobalScale:F6}",
            request.Program.Name, bestIterations, bestThreshold, bestWaypointTimes[^1], globalScale);
        _logger?.LogDebug(
            "ICSP 段时长: [{Durations}], 段缩放: [{Scales}]",
            string.Join(", ", bestDurations.Select(d => d.ToString("F4"))),
            string.Join(", ", bestScales.Select(s => s.ToString("F4"))));

        return new PlannedTrajectory(
            robot: request.Robot,
            originalProgram: request.Program,
            plannedWaypoints: waypoints,
            waypointTimes: bestWaypointTimes,
            segmentDurations: bestDurations,
            segmentScales: bestScales,
            method: PlanningMethod.Icsp,
            iterations: bestIterations,
            threshold: bestThreshold);
    }

    /// <summary>
    /// 把领域层路点列表转换成 double[][]，方便数学运算。
    /// </summary>
    private static double[][] WaypointsToArray(IReadOnlyList<JointWaypoint> waypoints)
    {
        var result = new double[waypoints.Count][];
        for (int i = 0; i < waypoints.Count; i++)
        {
            result[i] = waypoints[i].Positions.ToArray();
        }

        return result;
    }

    /// <summary>
    /// 从机器人配置中提取速度/加速度/jerk 限值数组。
    /// </summary>
    private static (double[] vel, double[] acc, double[] jerk) ExtractLimits(RobotProfile robot)
    {
        int dof = robot.DegreesOfFreedom;
        var vel = new double[dof];
        var acc = new double[dof];
        var jerk = new double[dof];
        for (int d = 0; d < dof; d++)
        {
            vel[d] = robot.JointLimits[d].VelocityLimit;
            acc[d] = robot.JointLimits[d].AccelerationLimit;
            jerk[d] = robot.JointLimits[d].JerkLimit;
        }

        return (vel, acc, jerk);
    }

    /// <summary>
    /// 计算初始段时长：取相邻路点在关节空间的欧氏距离。
    /// </summary>
    private static double[] ComputeInitialDurations(double[][] qs)
    {
        int n = qs.Length;
        var durations = new double[n - 1];
        for (int i = 0; i < n - 1; i++)
        {
            double sumSq = 0.0;
            for (int d = 0; d < qs[i].Length; d++)
            {
                double diff = qs[i + 1][d] - qs[i][d];
                sumSq += diff * diff;
            }

            durations[i] = Math.Sqrt(sumSq);
        }

        return durations;
    }

    /// <summary>
    /// 由段时长累加得到绝对时间节点（首项为 0）。
    /// </summary>
    private static double[] CumulativeTimes(double[] segmentDurations)
    {
        int nseg = segmentDurations.Length;
        var times = new double[nseg + 1];
        times[0] = 0.0;
        for (int i = 0; i < nseg; i++)
        {
            times[i + 1] = times[i] + segmentDurations[i];
        }

        return times;
    }
}