using Flyshot.ControllerClientCompat;
using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Runtime.Common;
using Flyshot.Runtime.Fanuc;
using Flyshot.Runtime.Fanuc.Protocol;

namespace Flyshot.Core.Tests;

/// <summary>
/// 验证最小运行时编排链路会把规划结果交给控制器运行时，而不是停留在兼容层内存状态。
/// </summary>
public sealed class RuntimeOrchestrationTests
{
    /// <summary>
    /// 验证 FANUC 最小运行时执行轨迹后会更新状态快照与最终关节位置。
    /// </summary>
    [Fact]
    public void FanucControllerRuntime_ExecuteTrajectory_UpdatesSnapshotAndFinalJointPositions()
    {
        var runtime = new FanucControllerRuntime();
        var robot = TestRobotFactory.CreateRobotProfile();
        runtime.ResetRobot(robot, "FANUC_LR_Mate_200iD");
        runtime.SetActiveController(sim: true);
        runtime.Connect("192.168.10.101");
        runtime.EnableRobot(bufferSize: 2);

        var result = new TrajectoryResult(
            programName: "demo",
            method: PlanningMethod.Icsp,
            isValid: true,
            duration: TimeSpan.FromSeconds(1.2),
            shotEvents: Array.Empty<ShotEvent>(),
            triggerTimeline: Array.Empty<TrajectoryDoEvent>(),
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: false,
            originalWaypointCount: 4,
            plannedWaypointCount: 4);

        runtime.ExecuteTrajectory(result, [1.0, 2.0, 3.0, 4.0, 5.0, 6.0]);

        var snapshot = runtime.GetSnapshot();
        Assert.Equal("Connected", snapshot.ConnectionState);
        Assert.False(snapshot.IsInMotion);
        Assert.Equal([1.0, 2.0, 3.0, 4.0, 5.0, 6.0], snapshot.JointPositions);
    }

    /// <summary>
    /// 验证真机运行时会把 TCP 10010 状态通道健康度映射为可诊断连接状态。
    /// </summary>
    [Theory]
    [InlineData(FanucStateConnectionState.Connected, false, "Connected")]
    [InlineData(FanucStateConnectionState.Connected, true, "StateTimeout")]
    [InlineData(FanucStateConnectionState.TimedOut, true, "StateTimeout")]
    [InlineData(FanucStateConnectionState.Reconnecting, true, "Reconnecting")]
    [InlineData(FanucStateConnectionState.Disconnected, false, "Disconnected")]
    public void FanucControllerRuntime_ResolveRealConnectionState_ReflectsStateChannelHealth(
        FanucStateConnectionState state,
        bool isFrameStale,
        string expected)
    {
        var status = new FanucStateClientStatus(
            state,
            isFrameStale,
            lastFrameAt: null,
            reconnectAttemptCount: 0,
            lastErrorMessage: null);

        var actual = FanucControllerRuntime.ResolveRealConnectionState(status);

        Assert.Equal(expected, actual);
    }

    /// <summary>
    /// 验证只有已连接且未陈旧的 TCP 10010 帧会被 runtime 当作当前机器人状态使用。
    /// </summary>
    [Theory]
    [InlineData(FanucStateConnectionState.Connected, false, true)]
    [InlineData(FanucStateConnectionState.Connected, true, false)]
    [InlineData(FanucStateConnectionState.Reconnecting, false, false)]
    [InlineData(FanucStateConnectionState.TimedOut, false, false)]
    [InlineData(FanucStateConnectionState.Disconnected, false, false)]
    public void FanucControllerRuntime_ShouldUseStateFrame_RequiresConnectedFreshState(
        FanucStateConnectionState state,
        bool isFrameStale,
        bool expected)
    {
        var status = new FanucStateClientStatus(
            state,
            isFrameStale,
            lastFrameAt: null,
            reconnectAttemptCount: 0,
            lastErrorMessage: null);

        var actual = FanucControllerRuntime.ShouldUseStateFrame(status);

        Assert.Equal(expected, actual);
    }

    /// <summary>
    /// 验证普通轨迹会先进入 ICSP 规划，并沿用 ICSP 对示教点数量的约束。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_RejectsThreeTeachPoints()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();

        void Act() =>
            orchestrator.PlanOrdinaryTrajectory(
                robot,
                [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                ]);

        Assert.Throws<ArgumentException>(Act);
    }

    /// <summary>
    /// 验证已上传飞拍轨迹会经过 self-adapt-icsp 并生成拍照触发时间轴。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_BuildsShotTimeline()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);

        Assert.True(bundle.Result.IsValid);
        Assert.Single(bundle.Result.ShotEvents);
        Assert.Single(bundle.Result.TriggerTimeline);
    }

    /// <summary>
    /// 验证飞拍规划会把规划限速倍率纳入速度/加速度/Jerk 限制，而不是复用运行时下发倍率。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_AppliesPlanningSpeedScaleToLimits()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 1.0);
        var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, planningSpeedScale: 0.5);

        Assert.True(
            halfSpeed.Result.Duration.TotalSeconds > fullSpeed.Result.Duration.TotalSeconds * 1.9,
            $"半速规划时长应接近全速的 2 倍，实际 full={fullSpeed.Result.Duration.TotalSeconds}, half={halfSpeed.Result.Duration.TotalSeconds}");
    }

    /// <summary>
    /// 验证飞拍缓存键包含规划限速倍率，避免降速验证时误用 100% 速度下的规划结果。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_CacheKeyIncludesPlanningSpeedScale()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var options = new FlyshotExecutionOptions(useCache: true);

        var fullSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 1.0);
        var halfSpeed = orchestrator.PlanUploadedFlyshot(robot, uploaded, options, planningSpeedScale: 0.5);

        Assert.False(halfSpeed.Result.UsedCache);
        Assert.True(halfSpeed.Result.Duration > fullSpeed.Result.Duration);
    }

    /// <summary>
    /// 验证飞拍编排会使用 RobotConfig.json 中的 IO 保持周期。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_UsesRobotSettingsForHoldCycles()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var settings = new CompatibilityRobotSettings(
            useDo: true,
            ioAddresses: [7, 8],
            ioKeepCycles: 4,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);

        var doEvent = Assert.Single(bundle.Result.TriggerTimeline);
        Assert.Equal(4, doEvent.HoldCycles);
    }

    /// <summary>
    /// 验证 RobotConfig.json 关闭 use_do 时仍保留 ShotEvent 诊断信息，但不生成伺服 DO 事件。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_SuppressesDoTimeline_WhenUseDoIsFalse()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();
        var settings = new CompatibilityRobotSettings(
            useDo: false,
            ioAddresses: [7, 8],
            ioKeepCycles: 4,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded, settings: settings);

        Assert.Single(bundle.Result.ShotEvents);
        Assert.Empty(bundle.Result.TriggerTimeline);
    }

    /// <summary>
    /// 验证普通轨迹规划后会生成稠密关节采样序列。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanOrdinaryTrajectory_ReturnsDenseJointTrajectory()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();

        var bundle = orchestrator.PlanOrdinaryTrajectory(
            robot,
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
            ]);

        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.NotEmpty(bundle.Result.DenseJointTrajectory);

        // 验证时间单调递增。
        var times = bundle.Result.DenseJointTrajectory.Select(static row => row[0]).ToArray();
        for (var i = 1; i < times.Length; i++)
        {
            Assert.True(times[i] > times[i - 1], $"采样时间点应在索引 {i} 处单调递增。");
        }

        // 验证每行包含时间 + 6 个关节值。
        Assert.All(bundle.Result.DenseJointTrajectory, row => Assert.Equal(7, row.Count));
    }

    /// <summary>
    /// 验证飞拍轨迹规划后的稠密采样时间轴与伺服周期一致。
    /// </summary>
    [Fact]
    public void ControllerClientTrajectoryOrchestrator_PlanUploadedFlyshot_DenseTrajectoryUsesServoPeriod()
    {
        var orchestrator = new ControllerClientTrajectoryOrchestrator();
        var robot = TestRobotFactory.CreateRobotProfile();
        var uploaded = TestRobotFactory.CreateUploadedTrajectoryWithSingleShot();

        var bundle = orchestrator.PlanUploadedFlyshot(robot, uploaded);

        Assert.NotNull(bundle.Result.DenseJointTrajectory);
        Assert.True(bundle.Result.DenseJointTrajectory.Count > 1);

        // 采样周期应为 8ms（伺服周期）。
        var firstDt = bundle.Result.DenseJointTrajectory[1][0] - bundle.Result.DenseJointTrajectory[0][0];
        Assert.Equal(0.008, firstDt, precision: 3);
    }

    /// <summary>
    /// 验证兼容服务执行普通轨迹时会进入规划链路，而不是直接把最后一个路点写入状态。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_ExecuteTrajectory_RejectsThreeTeachPointsAfterPlanningIsIntroduced()
    {
        var service = TestRobotFactory.CreateCompatService();
        service.SetUpRobot("FANUC_LR_Mate_200iD");
        service.SetActiveController(sim: true);
        service.Connect("192.168.10.101");
        service.EnableRobot(2);

        void Act() =>
            service.ExecuteTrajectory(
                [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.5, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [1.0, 0.0, 0.0, 0.0, 0.0, 0.0]
                ]);

        Assert.Throws<ArgumentException>(Act);
    }

    /// <summary>
    /// 验证兼容服务初始化机器人时会把 RobotConfig.json 中的 acc_limit / jerk_limit 传给模型加载器。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_SetUpRobot_AppliesRobotConfigLimitScales()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            File.WriteAllText(
                Path.Combine(configRoot, "RobotConfig.json"),
                """
                {
                  "robot": {
                    "use_do": true,
                    "io_addr": [7, 8],
                    "io_keep_cycles": 4,
                    "acc_limit": 0.5,
                    "jerk_limit": 0.25,
                    "adapt_icsp_try_num": 3
                  },
                  "flying_shots": {}
                }
                """);

            var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
            var runtime = new RecordingControllerRuntime();
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                runtime,
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");

            var profile = Assert.IsType<RobotProfile>(runtime.LastRobotProfile);
            Assert.Equal(14.905, profile.JointLimits[2].AccelerationLimit, precision: 3);
            Assert.Equal(62.115, profile.JointLimits[2].JerkLimit, precision: 3);
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 验证 IsFlyshotTrajectoryValid(saveTrajectory=true) 会把规划后的结果点位导出到 Config/Data/name。
    /// </summary>
    [Fact]
    public void ControllerClientCompatService_IsFlyshotTrajectoryValid_SaveTrajectoryExportsPlannedData()
    {
        var configRoot = CreateTempConfigRoot();
        try
        {
            WriteRobotConfigWithDemoTrajectory(configRoot);
            var options = new ControllerClientCompatOptions { ConfigRoot = configRoot };
            var service = new ControllerClientCompatService(
                options,
                new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
                new RecordingControllerRuntime(),
                new ControllerClientTrajectoryOrchestrator(),
                new RobotConfigLoader());

            service.SetUpRobot("FANUC_LR_Mate_200iD");

            var valid = service.IsFlyshotTrajectoryValid(
                out var duration,
                "demo-flyshot",
                method: "icsp",
                saveTrajectory: true);

            var outputDir = Path.Combine(configRoot, "Data", "demo-flyshot");
            Assert.True(valid);
            Assert.True(duration > TimeSpan.Zero);
            Assert.True(File.Exists(Path.Combine(outputDir, "JointTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "JointDetialTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "CartTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "CartDetialTraj.txt")));
            Assert.True(File.Exists(Path.Combine(outputDir, "ShotEvents.json")));
            Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "JointDetialTraj.txt")));
            Assert.NotEmpty(File.ReadAllLines(Path.Combine(outputDir, "CartDetialTraj.txt")));
        }
        finally
        {
            Directory.Delete(configRoot, recursive: true);
        }
    }

    /// <summary>
    /// 创建只包含当前支持机器人模型和 RobotConfig.json 的临时运行配置根。
    /// </summary>
    private static string CreateTempConfigRoot()
    {
        var configRoot = Path.Combine(Path.GetTempPath(), "flyshot-runtime-tests", Guid.NewGuid().ToString("N"), "Config");
        var modelDir = Path.Combine(configRoot, "Models");
        Directory.CreateDirectory(modelDir);

        var sourceModel = Path.Combine(
            TestRobotFactory.GetReplacementRoot(),
            "Config",
            "Models",
            "LR_Mate_200iD_7L.robot");
        File.Copy(sourceModel, Path.Combine(modelDir, "LR_Mate_200iD_7L.robot"));

        return configRoot;
    }

    /// <summary>
    /// 写入包含一条飞拍轨迹的最小 RobotConfig.json，供兼容服务从统一配置恢复轨迹。
    /// </summary>
    /// <param name="configRoot">测试运行配置根。</param>
    private static void WriteRobotConfigWithDemoTrajectory(string configRoot)
    {
        File.WriteAllText(
            Path.Combine(configRoot, "RobotConfig.json"),
            """
            {
              "robot": {
                "use_do": true,
                "io_addr": [7, 8],
                "io_keep_cycles": 2,
                "acc_limit": 1.0,
                "jerk_limit": 1.0,
                "adapt_icsp_try_num": 5
              },
              "flying_shots": {
                "demo-flyshot": {
                  "traj_waypoints": [
                    [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                    [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
                  ],
                  "shot_flags": [false, true, false, false],
                  "offset_values": [0, 1, 0, 0],
                  "addr": [[], [7, 8], [], []]
                }
              }
            }
            """);
    }
}

/// <summary>
/// 为运行时编排测试构造稳定的最小领域对象。
/// </summary>
internal static class TestRobotFactory
{
    /// <summary>
    /// 构造六轴测试机器人配置，避免运行时测试依赖真实 .robot 文件。
    /// </summary>
    /// <returns>可用于规划和运行时状态校验的机器人配置。</returns>
    public static RobotProfile CreateRobotProfile()
    {
        return new RobotProfile(
            name: "TestRobot",
            modelPath: "Models/Test.robot",
            degreesOfFreedom: 6,
            jointLimits: Enumerable.Range(1, 6)
                .Select(static index => new JointLimit($"J{index}", 10.0, 20.0, 100.0))
                .ToArray(),
            jointCouplings: Array.Empty<JointCoupling>(),
            servoPeriod: TimeSpan.FromMilliseconds(8),
            triggerPeriod: TimeSpan.FromMilliseconds(8));
    }

    /// <summary>
    /// 构造一条含单个拍照点的上传飞拍轨迹。
    /// </summary>
    /// <returns>可用于触发时间轴测试的上传轨迹。</returns>
    public static ControllerClientCompatUploadedTrajectory CreateUploadedTrajectoryWithSingleShot()
    {
        return new ControllerClientCompatUploadedTrajectory(
            name: "demo-flyshot",
            waypoints:
            [
                [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0],
                [0.3, 0.0, 0.0, 0.0, 0.0, 0.0]
            ],
            shotFlags: [false, true, false, false],
            offsetValues: [0, 1, 0, 0],
            addressGroups:
            [
                Array.Empty<int>(),
                [7, 8],
                Array.Empty<int>(),
                Array.Empty<int>()
            ]);
    }

    /// <summary>
    /// 构造一份真实依赖注入等价的兼容服务，覆盖运行时和编排器协作。
    /// </summary>
    /// <returns>可执行 ControllerClient 兼容语义的服务实例。</returns>
    public static ControllerClientCompatService CreateCompatService()
    {
        var options = new ControllerClientCompatOptions
        {
            ConfigRoot = GetConfigRoot()
        };

        return new ControllerClientCompatService(
            options,
            new ControllerClientCompatRobotCatalog(options, new RobotModelLoader()),
            new FanucControllerRuntime(),
            new ControllerClientTrajectoryOrchestrator(),
            new RobotConfigLoader());
    }

    /// <summary>
    /// 定位 replacement 仓库内的运行配置根目录。
    /// </summary>
    /// <returns>当前仓库 Config 目录。</returns>
    public static string GetConfigRoot()
    {
        return Path.Combine(GetReplacementRoot(), "Config");
    }

    /// <summary>
    /// 定位 replacement 仓库根目录，供测试读取仓库内固化配置。
    /// </summary>
    /// <returns>replacement 仓库根目录。</returns>
    public static string GetReplacementRoot()
    {
        var current = new DirectoryInfo(AppContext.BaseDirectory);
        while (current is not null)
        {
            if (File.Exists(Path.Combine(current.FullName, "FlyshotReplacement.sln")))
            {
                return current.FullName;
            }

            current = current.Parent;
        }

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

    /// <summary>
    /// 定位父工作区根目录，供兼容服务加载真实机器人模型。
    /// </summary>
    /// <returns>父工作区根目录。</returns>
    public 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.");
    }
}

/// <summary>
/// 记录 ResetRobot 入参的测试运行时，用于验证兼容服务传递的机器人配置。
/// </summary>
internal sealed class RecordingControllerRuntime : IControllerRuntime
{
    /// <summary>
    /// 获取最近一次 ResetRobot 收到的机器人配置。
    /// </summary>
    public RobotProfile? LastRobotProfile { get; private set; }

    /// <inheritdoc />
    public void ResetRobot(RobotProfile robot, string robotName)
    {
        LastRobotProfile = robot;
    }

    /// <inheritdoc />
    public void SetActiveController(bool sim)
    {
    }

    /// <inheritdoc />
    public void Connect(string robotIp)
    {
    }

    /// <inheritdoc />
    public void Disconnect()
    {
    }

    /// <inheritdoc />
    public void EnableRobot(int bufferSize)
    {
    }

    /// <inheritdoc />
    public void DisableRobot()
    {
    }

    /// <inheritdoc />
    public void StopMove()
    {
    }

    /// <inheritdoc />
    public double GetSpeedRatio() => 1.0;

    /// <inheritdoc />
    public void SetSpeedRatio(double ratio)
    {
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetTcp() => [0.0, 0.0, 0.0];

    /// <inheritdoc />
    public void SetTcp(double x, double y, double z)
    {
    }

    /// <inheritdoc />
    public bool GetIo(int port, string ioType) => false;

    /// <inheritdoc />
    public void SetIo(int port, bool value, string ioType)
    {
    }

    /// <inheritdoc />
    public IReadOnlyList<double> GetJointPositions() => Array.Empty<double>();

    /// <inheritdoc />
    public IReadOnlyList<double> GetPose() => Array.Empty<double>();

    /// <inheritdoc />
    public ControllerStateSnapshot GetSnapshot()
    {
        return new ControllerStateSnapshot(
            capturedAt: DateTimeOffset.UtcNow,
            connectionState: "Connected",
            isEnabled: true,
            isInMotion: false,
            speedRatio: 1.0,
            jointPositions: Array.Empty<double>(),
            cartesianPose: Array.Empty<double>(),
            activeAlarms: Array.Empty<RuntimeAlarm>());
    }

    /// <inheritdoc />
    public void ExecuteTrajectory(TrajectoryResult result, IReadOnlyList<double> finalJointPositions)
    {
    }
}