using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;

namespace Flyshot.Core.Tests;

/// <summary>
/// 锁定 Task 4 的最小兼容面，覆盖 ICSP、自适应补点以及飞拍触发时间轴。
/// </summary>
public sealed class PlanningCompatibilityTests
{
    /// <summary>
    /// 验证 ICSP 规划至少会生成严格递增的 waypoint 时间轴。
    /// </summary>
    [Fact]
    public void ICspPlanner_ReturnsMonotonicWaypointTimes()
    {
        var request = new TrajectoryRequest(
            robot: CreateRobotProfile([1, 1, 1, 1, 1, 1], [2, 2, 2, 2, 2, 2], [10, 10, 10, 10, 10, 10]),
            program: CreateProgram(
                new[]
                {
                    new[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 },
                    new[] { 0.4, 0.1, 0.0, 0.0, 0.0, 0.0 },
                    new[] { 0.8, 0.3, 0.0, 0.0, 0.0, 0.0 },
                    new[] { 1.0, 0.2, 0.0, 0.0, 0.0, 0.0 }
                }),
            method: PlanningMethod.Icsp);

        var trajectory = new ICspPlanner().Plan(request);

        Assert.Equal(4, trajectory.WaypointTimes.Count);
        Assert.All(trajectory.WaypointTimes.Zip(trajectory.WaypointTimes.Skip(1)), pair => Assert.True(pair.Second > pair.First));
    }

    /// <summary>
    /// 验证普通 ICSP 在最终最优解仍超限时会显式失败，而不是返回不可执行轨迹。
    /// </summary>
    [Fact]
    public void ICspPlanner_Throws_WhenFinalGlobalScaleExceedsOne()
    {
        var request = new TrajectoryRequest(
            robot: CreateRobotProfile([0.1], [0.1], [0.1]),
            program: CreateProgram(
                new[]
                {
                    new[] { 0.0 },
                    new[] { 10.0 },
                    new[] { 20.0 },
                    new[] { 30.0 }
                }),
            method: PlanningMethod.Icsp);

        var planner = new ICspPlanner(maxIterations: 0);

        var exception = Assert.Throws<InvalidOperationException>(() => planner.Plan(request));
        Assert.Contains("global_scale", exception.Message);
    }

    /// <summary>
    /// 验证 speed09 风格的大跳变样本在 self-adapt-icsp 下会通过补中点收敛。
    /// </summary>
    [Fact]
    public void SelfAdaptIcspPlanner_InsertsMidpoints_ForEol9Case()
    {
        var workspaceRoot = GetWorkspaceRoot();
        var configPath = Path.Combine(workspaceRoot, "Rvbust", "EOL9 EAU 90", "eol9_eau_90.json");
        var modelPath = Path.Combine(workspaceRoot, "flyshot-replacement", "Config", "Models", "LR_Mate_200iD_7L.json");

        var config = new RobotConfigLoader().Load(configPath);
        var baseProfile = new RobotModelLoader().LoadProfile(modelPath, config.Robot.AccLimitScale, config.Robot.JerkLimitScale);
        var constrainedProfile = ScaleRobotProfile(baseProfile, velocityScale: 0.9, accelerationScale: 0.9 * 0.9, jerkScale: 0.9 * 0.9 * 0.9);

        var request = new TrajectoryRequest(
            robot: constrainedProfile,
            program: config.Programs["EOL9_EAU_90"],
            method: PlanningMethod.SelfAdaptIcsp);

        var trajectory = new SelfAdaptIcspPlanner().Plan(request, adaptIcspTryNum: config.Robot.AdaptIcspTryNum);

        Assert.True(trajectory.InsertedWaypointCount > 0);
        Assert.True(trajectory.PlannedWaypointCount > trajectory.OriginalWaypointCount);
        Assert.True(trajectory.SegmentScales.Max() <= 1.0005);
    }

    /// <summary>
    /// 验证补点后仍然能按原始示教点顺序找回时间戳，而不是错误地绑定到新增中点。
    /// </summary>
    [Fact]
    public void WaypointTimestampResolver_UsesOriginalTeachPointsAfterInsertion()
    {
        var originalProgram = CreateProgram(
            new[]
            {
                new[] { 0.0, 0.0 },
                new[] { 1.0, 0.0 },
                new[] { 2.0, 0.0 },
                new[] { 3.0, 0.0 }
            });

        var trajectory = new PlannedTrajectory(
            robot: CreateRobotProfile([1, 1], [1, 1], [1, 1]),
            originalProgram: originalProgram,
            plannedWaypoints:
            [
                new JointWaypoint([0.0, 0.0]),
                new JointWaypoint([0.5, 0.0]),
                new JointWaypoint([1.0, 0.0]),
                new JointWaypoint([2.0, 0.0]),
                new JointWaypoint([2.5, 0.0]),
                new JointWaypoint([3.0, 0.0])
            ],
            waypointTimes: [0.0, 0.25, 0.5, 1.0, 1.5, 2.0],
            segmentDurations: [0.25, 0.25, 0.5, 0.5, 0.5],
            segmentScales: [1.0, 1.0, 1.0, 1.0, 1.0],
            method: PlanningMethod.SelfAdaptIcsp,
            iterations: 3,
            threshold: 0.0);

        var timestamps = new WaypointTimestampResolver().Resolve(trajectory);

        Assert.Equal([0.0, 0.5, 1.0, 2.0], timestamps);
    }

    /// <summary>
    /// 验证触发时间轴会使用原始 waypoint 时间、offset 周期和地址组生成 ShotEvent/TrajectoryDoEvent。
    /// </summary>
    [Fact]
    public void ShotTimelineBuilder_MapsOffsetsToShotEventsAndTriggerTimeline()
    {
        var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
        var program = new FlyshotProgram(
            name: "demo",
            waypoints:
            [
                new JointWaypoint([0.0, 0.0]),
                new JointWaypoint([1.0, 0.0]),
                new JointWaypoint([2.0, 0.0])
            ],
            shotFlags: [false, true, false],
            offsetValues: [0, 1, 0],
            addressGroups:
            [
                new IoAddressGroup(Array.Empty<int>()),
                new IoAddressGroup([2, 4]),
                new IoAddressGroup(Array.Empty<int>())
            ]);

        var trajectory = new PlannedTrajectory(
            robot: robot,
            originalProgram: program,
            plannedWaypoints: program.Waypoints,
            waypointTimes: [0.0, 0.5, 1.0],
            segmentDurations: [0.5, 0.5],
            segmentScales: [1.0, 1.0],
            method: PlanningMethod.Icsp,
            iterations: 1,
            threshold: 0.0);

        var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
            .Build(trajectory, holdCycles: 2, samplePeriod: TimeSpan.FromMilliseconds(16));

        var shotEvent = Assert.Single(timeline.ShotEvents);
        Assert.Equal(0.508, shotEvent.TriggerTime, precision: 3);
        Assert.Equal([2, 4], shotEvent.AddressGroup.Addresses);

        var doEvent = Assert.Single(timeline.TriggerTimeline);
        Assert.Equal(1, doEvent.WaypointIndex);
        Assert.Equal(1, doEvent.OffsetCycles);
        Assert.Equal(2, doEvent.HoldCycles);
        Assert.NotNull(doEvent.ReferenceJointsDegrees);
        Assert.Equal(RadiansToDegrees(1.0), doEvent.ReferenceJointsDegrees![0], precision: 6);
        Assert.Equal(0.0, doEvent.ReferenceJointsDegrees![1], precision: 6);
    }

    /// <summary>
    /// 验证时间轴会把示教点关节角保存到 DO 事件里，供运行时在近时窗内做最小关节差绑定。
    /// </summary>
    [Fact]
    public void ShotTimelineBuilder_PopulatesReferenceJointsDegreesForRuntimeBinding()
    {
        var robot = CreateRobotProfile([1, 1], [1, 1], [1, 1]);
        var program = new FlyshotProgram(
            name: "demo",
            waypoints:
            [
                new JointWaypoint([0.0, 0.0]),
                new JointWaypoint([Math.PI / 6.0, -Math.PI / 4.0])
            ],
            shotFlags: [false, true],
            offsetValues: [0, 0],
            addressGroups:
            [
                new IoAddressGroup(Array.Empty<int>()),
                new IoAddressGroup([1])
            ]);

        var trajectory = new PlannedTrajectory(
            robot: robot,
            originalProgram: program,
            plannedWaypoints: program.Waypoints,
            waypointTimes: [0.0, 0.5],
            segmentDurations: [0.5],
            segmentScales: [1.0],
            method: PlanningMethod.Icsp,
            iterations: 1,
            threshold: 0.0);

        var timeline = new ShotTimelineBuilder(new WaypointTimestampResolver())
            .Build(trajectory, holdCycles: 1, samplePeriod: TimeSpan.FromMilliseconds(8));

        var trigger = Assert.Single(timeline.TriggerTimeline);
        Assert.NotNull(trigger.ReferenceJointsDegrees);
        Assert.Equal(30.0, trigger.ReferenceJointsDegrees![0], precision: 6);
        Assert.Equal(-45.0, trigger.ReferenceJointsDegrees![1], precision: 6);
    }

    /// <summary>
    /// 验证稠密轨迹生成后会复核离散加速度，避免采样结果绕过 ICSP 段 scale 校验。
    /// </summary>
    [Fact]
    public void TrajectoryLimitValidator_Throws_WhenDenseTrajectoryAccelerationExceedsLimit()
    {
        var robot = CreateRobotProfile([100.0], [10.0], [1000.0]);

        var exception = Assert.Throws<InvalidOperationException>(() =>
            TrajectoryLimitValidator.ValidateDenseJointTrajectory(
                robot,
                [
                    [0.0, 0.0],
                    [0.008, 0.0],
                    [0.016, 0.01]
                ],
                trajectoryName: "dense-acceleration-check"));

        Assert.Contains("加速度", exception.Message);
        Assert.Contains("dense-acceleration-check", exception.Message);
    }

    /// <summary>
    /// 验证飞拍链路临时关闭 jerk 复核时，仍保留速度/加速度校验，但不会再被 jerk 单独阻断。
    /// </summary>
    [Fact]
    public void TrajectoryLimitValidator_CanSkipJerkValidation_ForFlyshotPath()
    {
        var robot = CreateRobotProfile([100.0], [1000.0], [10.0]);
        IReadOnlyList<IReadOnlyList<double>> rows =
        [
            [0.0, 0.0],
            [0.008, 0.0],
            [0.016, 0.01],
            [0.024, 0.02]
        ];

        var exception = Assert.Throws<InvalidOperationException>(() =>
            TrajectoryLimitValidator.ValidateDenseJointTrajectory(
                robot,
                rows,
                trajectoryName: "dense-jerk-check"));

        Assert.Contains("Jerk", exception.Message);

        TrajectoryLimitValidator.ValidateDenseJointTrajectory(
            robot,
            rows,
            trajectoryName: "dense-jerk-check-skip",
            validateJerk: false);
    }

    /// <summary>
    /// 验证飞拍最终 J519 队列的 jerk 硬约束使用严格限值，不再沿用历史上放大 4 倍的宽容口径。
    /// </summary>
    [Fact]
    public void TrajectoryLimitValidator_ThrowsForJ519Jerk_WhenStrictFinalQueueValidationIsEnabled()
    {
        var robot = CreateRobotProfile([100.0], [1000.0], [100.0]);
        var samples = new[]
        {
            new J519SendSample(0, 0.000, 0.000, 1.0, [0.0]),
            new J519SendSample(1, 0.008, 0.008, 1.0, [0.0]),
            new J519SendSample(2, 0.016, 0.016, 1.0, [0.003754]),
            new J519SendSample(3, 0.024, 0.024, 1.0, [0.007508])
        };

        var exception = Assert.Throws<InvalidOperationException>(() =>
            TrajectoryLimitValidator.ValidateJ519SendSamples(
                robot,
                samples,
                trajectoryName: "strict-final-j519",
                validateJerk: true));

        Assert.Contains("Jerk", exception.Message);
        Assert.Contains("strict-final-j519", exception.Message);
    }

    /// <summary>
    /// 验证飞拍最终发送队列在请求倍率下严格 jerk 超限时直接拒绝执行，而不是自动改写 speed_ratio。
    /// </summary>
    [Fact]
    public void FlyshotExecutionSendSequenceBuilder_ThrowsInsteadOfRewritingRequestedSpeedRatio()
    {
        var robot = CreateRobotProfile([100.0], [10000.0], [800.0]);
        var program = CreateProgram(
            [
                [0.0],
                [0.001]
            ]);
        var trajectory = new PlannedTrajectory(
            robot: robot,
            originalProgram: program,
            plannedWaypoints: program.Waypoints,
            waypointTimes: [0.0, 0.024],
            segmentDurations: [0.024],
            segmentScales: [1.0],
            method: PlanningMethod.Icsp,
            iterations: 1,
            threshold: 0.0);
        var result = new TrajectoryResult(
            programName: "strict-builder",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.024),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 2,
            plannedWaypointCount: 2);
        var initialSamples = J519SendTrajectorySampler.SamplePlannedTrajectory(
            trajectory,
            servoPeriodSeconds: 0.008,
            speedRatio: 1.0);

        Assert.Throws<InvalidOperationException>(() =>
            TrajectoryLimitValidator.ValidateJ519SendSamples(
                robot,
                initialSamples,
                trajectoryName: "strict-builder-initial",
                validateJerk: true));

        var exception = Assert.Throws<InvalidOperationException>(() =>
            FlyshotExecutionSendSequenceBuilder.Build(
                robot,
                trajectory,
                result,
                servoPeriodSeconds: 0.008,
                requestedSpeedRatio: 1.0));

        Assert.Contains("speed_ratio=1.000000", exception.Message);
        Assert.Contains("planning_speed_scale", exception.Message);
    }

    /// <summary>
    /// 验证飞拍最终发送队列通过严格校验时，请求 speed_ratio 会原样成为最终倍率。
    /// </summary>
    [Fact]
    public void FlyshotExecutionSendSequenceBuilder_KeepsRequestedSpeedRatioWhenQueuePasses()
    {
        var robot = CreateRobotProfile([100.0], [10000.0], [10000.0]);
        var program = CreateProgram(
            [
                [0.0],
                [0.001]
            ]);
        var trajectory = new PlannedTrajectory(
            robot: robot,
            originalProgram: program,
            plannedWaypoints: program.Waypoints,
            waypointTimes: [0.0, 0.024],
            segmentDurations: [0.024],
            segmentScales: [1.0],
            method: PlanningMethod.Icsp,
            iterations: 1,
            threshold: 0.0);
        var result = new TrajectoryResult(
            programName: "rewrite-speedratio-builder",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(0.024),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 2,
            plannedWaypointCount: 2);

        var prepared = FlyshotExecutionSendSequenceBuilder.Build(
            robot,
            trajectory,
            result,
            servoPeriodSeconds: 0.008,
            requestedSpeedRatio: 0.8);

        Assert.Equal(0.8, prepared.RequestSpeedRatio, precision: 6);
        Assert.Equal(0.8, prepared.FinalSpeedRatio, precision: 6);
        Assert.Equal(0, prepared.StretchIterationCount);
        Assert.All(prepared.Samples, sample => Assert.Equal(0.8, sample.SpeedRatio, precision: 6));
        Assert.All(prepared.TimingRows, row =>
        {
            Assert.Equal(0.8, row[3], precision: 6);
            Assert.Equal(0.8, row[4], precision: 6);
            Assert.Equal(0.0, row[5], precision: 6);
        });
    }

    /// <summary>
    /// 构造一个最小 RobotProfile，便于规划层单元测试聚焦在时间轴逻辑上。
    /// </summary>
    private static RobotProfile CreateRobotProfile(
        IReadOnlyList<double> velocityLimits,
        IReadOnlyList<double> accelerationLimits,
        IReadOnlyList<double> jerkLimits)
    {
        return new RobotProfile(
            name: "TestRobot",
            modelPath: "Models/Test.robot",
            degreesOfFreedom: velocityLimits.Count,
            jointLimits: velocityLimits
                .Select((velocity, index) => new JointLimit(
                    $"J{index + 1}",
                    velocity,
                    accelerationLimits[index],
                    jerkLimits[index]))
                .ToArray(),
            jointCouplings: Array.Empty<JointCoupling>(),
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 构造一个默认不带拍照点的最小 FlyshotProgram。
    /// </summary>
    private static FlyshotProgram CreateProgram(IEnumerable<double[]> waypointRows)
    {
        var rows = waypointRows.ToArray();
        return new FlyshotProgram(
            name: "demo",
            waypoints: rows.Select(row => new JointWaypoint(row)).ToArray(),
            shotFlags: Enumerable.Repeat(false, rows.Length).ToArray(),
            offsetValues: Enumerable.Repeat(0, rows.Length).ToArray(),
            addressGroups: Enumerable.Range(0, rows.Length).Select(_ => new IoAddressGroup(Array.Empty<int>())).ToArray());
    }

    /// <summary>
    /// 在不丢失 couple 元数据的前提下，按比例缩放机器人关节限制。
    /// </summary>
    private static RobotProfile ScaleRobotProfile(RobotProfile source, double velocityScale, double accelerationScale, double jerkScale)
    {
        return new RobotProfile(
            name: source.Name,
            modelPath: source.ModelPath,
            degreesOfFreedom: source.DegreesOfFreedom,
            jointLimits: source.JointLimits
                .Select(limit => new JointLimit(
                    limit.JointName,
                    limit.VelocityLimit * velocityScale,
                    limit.AccelerationLimit * accelerationScale,
                    limit.JerkLimit * jerkScale))
                .ToArray(),
            jointCouplings: source.JointCouplings,
            servoPeriod: source.ServoPeriod,
            triggerPeriod: source.TriggerPeriod);
    }

    /// <summary>
    /// 定位父工作区根目录，读取真实的 EOL9 样本和机器人模型。
    /// </summary>
    private static string GetWorkspaceRoot()
    {
        var current = new DirectoryInfo(AppContext.BaseDirectory);
        while (current is not null)
        {
            if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
            {
                return Path.GetFullPath(Path.Combine(current.FullName, ".."));
            }

            current = current.Parent;
        }

        throw new DirectoryNotFoundException("Unable to locate the flyshot workspace root.");
    }

    private static double RadiansToDegrees(double radians)
    {
        return radians * 180.0 / Math.PI;
    }
}