using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
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, "FlyingShot", "FlyingShot", "Models", "LR_Mate_200iD_7L.robot");

        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);
    }

    /// <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.");
    }
}