FlyShotHost/tests/Flyshot.Core.Tests/RustIcspPlannerParityTests.cs

using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Native.Planning;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证 Rust ICSP 与现有 C# 基准实现保持逐项数值一致。
/// </summary>
public sealed class RustIcspPlannerParityTests
{
    /// <summary>
    /// 验证基础六自由度兼容样本的时间轴、段时长、段缩放和迭代信息完全对齐。
    /// </summary>
    [Fact]
    public void RustIcspPlanner_MatchesManagedIcspPlanner_ForCompatibilityFixture()
    {
        TrajectoryRequest request = CreateCompatibilityRequest();
        PlannedTrajectory managed = new ICspPlanner().Plan(request);
        PlannedTrajectory native = new RustIcspPlanner().Plan(request);

        Assert.Equal(PlanningMethod.Icsp, managed.Method);
        Assert.Equal(PlanningMethod.IcspRust, native.Method);
        AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
    }

    /// <summary>
    /// 验证关闭最终 scale 拒绝后，多自由度非均匀路点仍与托管算法逐项对齐。
    /// </summary>
    [Fact]
    public void RustIcspPlanner_MatchesManagedIcspPlanner_ForNonUniformFixture()
    {
        TrajectoryRequest request = CreateNonUniformRequest();
        var options = NativeIcspPlannerOptions.Default with { EnforceFinalScale = false };
        PlannedTrajectory managed = new ICspPlanner(enforceFinalScale: false).Plan(request);
        PlannedTrajectory native = new RustIcspPlanner(options).Plan(request);

        Assert.Equal(PlanningMethod.Icsp, managed.Method);
        Assert.Equal(PlanningMethod.IcspRust, native.Method);
        AssertTrajectoryMatches(managed, native, tolerance: 1e-9);
    }

    /// <summary>
    /// 创建与现有 PlanningCompatibilityTests 基础用例一致的六自由度请求。
    /// </summary>
    /// <returns>可由默认 ICSP 配置收敛的稳定规划请求。</returns>
    private static TrajectoryRequest CreateCompatibilityRequest()
    {
        return CreateRequest(
            velocityLimits: [1, 1, 1, 1, 1, 1],
            accelerationLimits: [2, 2, 2, 2, 2, 2],
            jerkLimits: [10, 10, 10, 10, 10, 10],
            waypointRows:
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.4, 0.1, 0.0, 0.0, 0.0, 0.0],
                [0.8, 0.3, 0.0, 0.0, 0.0, 0.0],
                [1.0, 0.2, 0.0, 0.0, 0.0, 0.0]
            ]);
    }

    /// <summary>
    /// 创建用于覆盖多自由度、非均匀段距离和不同约束的规划请求。
    /// </summary>
    /// <returns>关闭最终 scale 拒绝后可用于数值对拍的稳定请求。</returns>
    private static TrajectoryRequest CreateNonUniformRequest()
    {
        return CreateRequest(
            velocityLimits: [1.2, 0.9, 1.5],
            accelerationLimits: [2.5, 1.8, 3.0],
            jerkLimits: [12.0, 9.0, 15.0],
            waypointRows:
            [
                [0.0, 0.0, 0.0],
                [0.2, -0.1, 0.4],
                [0.9, 0.3, 0.7],
                [1.1, 0.8, 0.2],
                [1.6, 0.4, -0.1]
            ]);
    }

    /// <summary>
    /// 根据逐关节限制和路点矩阵创建完整领域请求。
    /// </summary>
    /// <param name="velocityLimits">逐关节速度上限。</param>
    /// <param name="accelerationLimits">逐关节加速度上限。</param>
    /// <param name="jerkLimits">逐关节 jerk 上限。</param>
    /// <param name="waypointRows">按路点排列的关节位置矩阵。</param>
    /// <returns>可同时传给 C# 与 Rust 规划器的请求。</returns>
    private static TrajectoryRequest CreateRequest(
        IReadOnlyList<double> velocityLimits,
        IReadOnlyList<double> accelerationLimits,
        IReadOnlyList<double> jerkLimits,
        IReadOnlyList<double[]> waypointRows)
    {
        var robot = new RobotProfile(
            name: "RustParityRobot",
            modelPath: "Models/RustParity.robot",
            degreesOfFreedom: velocityLimits.Count,
            jointLimits: velocityLimits
                .Select((velocity, index) => new JointLimit(
                    $"J{index + 1}",
                    velocity,
                    accelerationLimits[index],
                    jerkLimits[index]))
                .ToArray(),
            jointCouplings: [],
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
        var program = new FlyshotProgram(
            name: "rust-parity",
            waypoints: waypointRows.Select(row => new JointWaypoint(row)).ToArray(),
            shotFlags: Enumerable.Repeat(false, waypointRows.Count).ToArray(),
            offsetValues: Enumerable.Repeat(0, waypointRows.Count).ToArray(),
            addressGroups: Enumerable.Range(0, waypointRows.Count)
                .Select(_ => new IoAddressGroup([]))
                .ToArray());

        return new TrajectoryRequest(robot, program, PlanningMethod.Icsp);
    }

    /// <summary>
    /// 逐字段比较两套规划实现的数值结果。
    /// </summary>
    /// <param name="managed">现有 C# ICSP 基准结果。</param>
    /// <param name="native">Rust ICSP 原生结果。</param>
    /// <param name="tolerance">所有浮点数组和 threshold 使用的绝对误差。</param>
    private static void AssertTrajectoryMatches(
        PlannedTrajectory managed,
        PlannedTrajectory native,
        double tolerance)
    {
        Assert.Equal(managed.Iterations, native.Iterations);
        Assert.Equal(managed.WaypointTimes.Count, native.WaypointTimes.Count);
        Assert.Equal(managed.SegmentDurations.Count, native.SegmentDurations.Count);
        Assert.Equal(managed.SegmentScales.Count, native.SegmentScales.Count);
        Assert.InRange(Math.Abs(managed.Threshold - native.Threshold), 0.0, tolerance);

        AssertSequenceMatches(managed.WaypointTimes, native.WaypointTimes, tolerance);
        AssertSequenceMatches(managed.SegmentDurations, native.SegmentDurations, tolerance);
        AssertSequenceMatches(managed.SegmentScales, native.SegmentScales, tolerance);
    }

    /// <summary>
    /// 按相同索引比较两个浮点序列的绝对误差。
    /// </summary>
    /// <param name="expected">托管基准序列。</param>
    /// <param name="actual">Rust 原生序列。</param>
    /// <param name="tolerance">允许的最大绝对误差。</param>
    private static void AssertSequenceMatches(
        IReadOnlyList<double> expected,
        IReadOnlyList<double> actual,
        double tolerance)
    {
        for (int index = 0; index < expected.Count; index++)
        {
            Assert.InRange(Math.Abs(expected[index] - actual[index]), 0.0, tolerance);
        }
    }
}