using Flyshot.Core.Config;
using Flyshot.Core.Domain;
using Flyshot.Core.Planning;
using Flyshot.Core.Planning.Sampling;
using Flyshot.Core.Triggering;
using Flyshot.Native.Planning;
using Microsoft.Extensions.Logging;

namespace Flyshot.ControllerClientCompat;

/// <summary>
/// 负责把 ControllerClient 兼容层的轨迹输入转换为规划结果和触发时间轴。
/// </summary>
public sealed class ControllerClientTrajectoryOrchestrator
{
    /// <summary>
    /// 稠密轨迹离散限幅失败后允许统一拉长时间轴的最大次数。
    /// </summary>
    private const int MaxDenseLimitStretchIterations = 100;

    /// <summary>
    /// 每次离散限幅失败后统一放大的时间倍率。
    /// </summary>
    private const double DenseLimitStretchFactor = 1.01;

    /// <summary>
    /// 平滑起停重定时混合系数：0 表示不平滑，1 表示完全使用平滑时间律。
    /// 当前取值用于“弱平滑”，仅轻微拉伸首尾时间段，避免起停过慢。
    /// </summary>
    private const double SmoothStartStopBlend = 0.60;

    private readonly IReadOnlyDictionary<string, IPlanningBackend> _planningBackends;
    private readonly ShotTimelineBuilder _shotTimelineBuilder = new(new WaypointTimestampResolver());
    private readonly Dictionary<string, PlannedExecutionBundle> _flyshotCache = new(StringComparer.Ordinal);
    private readonly ILogger<ControllerClientTrajectoryOrchestrator>? _logger;

    /// <summary>
    /// 初始化轨迹编排器。
    /// </summary>
    /// <param name="planningBackends">显式注册的规划后端；为空时仅启用默认 Rust icsp 后端。</param>
    /// <param name="logger">日志记录器；允许 null。</param>
    /// <param name="loggerFactory">日志工厂；允许 null。</param>
    public ControllerClientTrajectoryOrchestrator(
        IEnumerable<IPlanningBackend>? planningBackends = null,
        ILogger<ControllerClientTrajectoryOrchestrator>? logger = null,
        ILoggerFactory? loggerFactory = null)
    {
        _logger = logger;
        _planningBackends = BuildPlanningBackendMap(planningBackends);
    }

    /// <summary>
    /// 对普通轨迹执行 ICSP 规划。
    /// </summary>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="waypoints">普通轨迹关节路点。</param>
    /// <param name="options">执行参数。</param>
    /// <param name="planningSpeedScale">规划速度倍率。</param>
    /// <param name="speedRatio">本次执行速度倍率；普通轨迹在规划阶段折算进有效限速倍率。</param>
    /// <returns>包含规划轨迹、空触发时间轴和执行结果的结果包。</returns>
    public PlannedExecutionBundle PlanOrdinaryTrajectory(
        RobotProfile robot,
        IReadOnlyList<IReadOnlyList<double>> waypoints,
        TrajectoryExecutionOptions? options = null,
        double planningSpeedScale = 1.0,
        double speedRatio = 1.0)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(waypoints);
        options ??= new TrajectoryExecutionOptions();
        var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(planningSpeedScale, speedRatio, "普通轨迹");
        var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);

        _logger?.LogInformation(
            "PlanOrdinaryTrajectory 开始: 路点数={WaypointCount}, method={Method}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}",
            waypoints.Count, options.Method, planningSpeedScale, speedRatio, effectivePlanningSpeedScale);

        var program = CreateProgram(
            name: "ordinary-trajectory",
            waypoints: waypoints,
            shotFlags: Enumerable.Repeat(false, waypoints.Count),
            offsetValues: Enumerable.Repeat(0, waypoints.Count),
            addressGroups: Enumerable.Range(0, waypoints.Count).Select(static _ => Array.Empty<int>()));

        var methodName = ParseOrdinaryMethod(options.Method);
        var backend = ResolvePlanningBackend(methodName);
        var request = new TrajectoryRequest(
            robot: planningRobot,
            program: program,
            method: backend.ResultMethod,
            saveTrajectoryArtifacts: options.SaveTrajectory);

        var plannedTrajectory = PlanByBackend(request, backend);
        var executionTrajectory = plannedTrajectory;
        var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref executionTrajectory, shapeTrajectoryEdges: false);
        var shotTimeline = new ShotTimeline(Array.Empty<ShotEvent>(), Array.Empty<TrajectoryDoEvent>());
        var result = CreateResult(
            executionTrajectory,
            shotTimeline,
            denseJointTrajectory,
            usedCache: false,
            triggerSampleIndexOffsetCycles: 0);

        _logger?.LogInformation(
            "PlanOrdinaryTrajectory 完成: 时长={Duration}s, 采样点数={SampleCount}",
            result.Duration.TotalSeconds,
            result.DenseJointTrajectory?.Count ?? 0);

        return new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: executionTrajectory);
    }

    /// <summary>
    /// 对已经上传的飞拍轨迹执行自适应 ICSP 规划并生成触发时间轴。
    /// </summary>
    /// <param name="robot">当前机器人配置。</param>
    /// <param name="uploaded">兼容层保存的上传轨迹。</param>
    /// <param name="options">执行参数。</param>
    /// <param name="settings">兼容层机器人设置。</param>
    /// <param name="planningSpeedScale">规划速度倍率。</param>
    /// <param name="speedRatio">本次飞拍使用的速度倍率；为空时使用配置默认值。</param>
    /// <returns>包含规划轨迹、触发时间轴和执行结果的结果包。</returns>
    public PlannedExecutionBundle PlanUploadedFlyshot(
        RobotProfile robot,
        ControllerClientCompatUploadedTrajectory uploaded,
        FlyshotExecutionOptions? options = null,
        CompatibilityRobotSettings? settings = null,
        double? planningSpeedScale = null,
        double? speedRatio = null)
    {
        ArgumentNullException.ThrowIfNull(robot);
        ArgumentNullException.ThrowIfNull(uploaded);
        options ??= new FlyshotExecutionOptions();
        settings ??= CreateDefaultRobotSettings();
        var requestedSpeedRatio = speedRatio ?? settings.SpeedRatio;
        var configuredPlanningSpeedScale = planningSpeedScale ?? settings.PlanningSpeedScale;
        var effectivePlanningSpeedScale = ResolveEffectivePlanningSpeedScale(configuredPlanningSpeedScale, requestedSpeedRatio, "飞拍");
        var planningRobot = ApplyPlanningSpeedScale(robot, effectivePlanningSpeedScale);

        _logger?.LogInformation(
            "PlanUploadedFlyshot 开始: name={Name}, waypoints={WaypointCount}, method={Method}, useCache={UseCache}, planningSpeedScale={PlanningSpeedScale}, speedRatio={SpeedRatio}, effectivePlanningSpeedScale={EffectivePlanningSpeedScale}, smoothStartStopTiming={SmoothStartStopTiming}",
            uploaded.Name,
            uploaded.Waypoints.Count,
            options.Method,
            options.UseCache,
            configuredPlanningSpeedScale,
            requestedSpeedRatio,
            effectivePlanningSpeedScale,
            settings.SmoothStartStopTiming);

        var program = CreateProgram(
            name: uploaded.Name,
            waypoints: uploaded.Waypoints,
            shotFlags: uploaded.ShotFlags,
            offsetValues: uploaded.OffsetValues,
            addressGroups: uploaded.AddressGroups);

        var methodName = ParseFlyshotMethod(options.Method);
        var backend = ResolvePlanningBackend(methodName);
        var cacheKey = CreateFlyshotCacheKey(planningRobot, uploaded, options, settings, effectivePlanningSpeedScale);
        if (options.UseCache && _flyshotCache.TryGetValue(cacheKey, out var cachedBundle))
        {
           _logger?.LogInformation("PlanUploadedFlyshot 命中缓存: name={Name}, cacheKey={CacheKey}", uploaded.Name, cacheKey);
         
           // 命中缓存时只替换 TrajectoryResult 的 usedCache 标志，规划轨迹和触发时间轴保持不可变复用。
           return cachedBundle;
        }

        var request = new TrajectoryRequest(
            robot: planningRobot,
            program: program,
            method: backend.ResultMethod,
            moveToStart: options.MoveToStart,
            saveTrajectoryArtifacts: options.SaveTrajectory,
            useCache: options.UseCache);

        var plannedTrajectory = PlanByBackend(request, backend, settings);
        var smoothedExecutionTrajectory = ApplyExecutionTiming(plannedTrajectory, settings);
        var denseJointTrajectory = CreateLimitCompliantDenseTrajectory(ref smoothedExecutionTrajectory, shapeTrajectoryEdges: false);
        var shotTimeline = _shotTimelineBuilder.Build(
            smoothedExecutionTrajectory,
            holdCycles: settings.IoKeepCycles,
            samplePeriod: planningRobot.ServoPeriod,
            useDo: settings.UseDo);
        var result = CreateResult(
            smoothedExecutionTrajectory,
            shotTimeline,
            denseJointTrajectory,
            usedCache: false,
            triggerSampleIndexOffsetCycles: settings.TriggerSampleIndexOffsetCycles);
        var bundle = new PlannedExecutionBundle(plannedTrajectory, shotTimeline, result, executionTrajectory: smoothedExecutionTrajectory);

        _logger?.LogInformation(
            "PlanUploadedFlyshot 完成: name={Name}, 时长={Duration}s, 触发事件数={TriggerCount}, 采样点数={SampleCount}",
            uploaded.Name, result.Duration.TotalSeconds, result.TriggerTimeline.Count, result.DenseJointTrajectory?.Count ?? 0);

        if (options.UseCache)
        {
            _flyshotCache[cacheKey] = bundle;
        }

        return bundle;
    }

    /// <summary>
    /// 按普通轨迹执行接口约束解析 method 参数。
    /// </summary>
    /// <param name="method">旧 SDK 传入的方法名。</param>
    /// <returns>领域层规划方法。</returns>
    private static string ParseOrdinaryMethod(string method)
    {
        var normalized = NormalizeMethod(method);
        return normalized switch
        {
            "icsp" => normalized,
            "icsp-csharp" => normalized,
            "icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
            "doubles" => normalized,
            _ => throw new ArgumentException($"Unsupported ExecuteTrajectory method: {method}", nameof(method))
        };
    }

    /// <summary>
    /// 按飞拍轨迹执行接口约束解析 method 参数。
    /// </summary>
    /// <param name="method">旧 SDK 传入的方法名。</param>
    /// <returns>领域层规划方法。</returns>
    private static string ParseFlyshotMethod(string method)
    {
        var normalized = NormalizeMethod(method);
        return normalized switch
        {
            "icsp" => normalized,
            "icsp-csharp" => normalized,
            "icsp_rust" => throw new ArgumentException("method=\"icsp_rust\" 已改名为 method=\"icsp\"。", nameof(method)),
            "self-adapt-icsp" => normalized,
            "doubles" => normalized,
            _ => throw new ArgumentException($"Unsupported ExecuteFlyShotTraj method: {method}", nameof(method))
        };
    }

    /// <summary>
    /// 使用显式注册的规划后端执行规划。
    /// </summary>
    /// <param name="request">规划请求。</param>
    /// <param name="backend">已解析的规划后端。</param>
    /// <param name="settings">当前机器人兼容配置；为空时使用默认补点次数。</param>
    /// <returns>规划轨迹。</returns>
    private static PlannedTrajectory PlanByBackend(
        TrajectoryRequest request,
        IPlanningBackend backend,
        CompatibilityRobotSettings? settings = null)
    {
        var context = new PlanningBackendContext(settings?.AdaptIcspTryNum ?? 5);
        return backend.Plan(request, context);
    }

    /// <summary>
    /// 从已注册后端中解析 method 对应的规划实现。
    /// </summary>
    /// <param name="methodName">已经归一化的外部 method 名称。</param>
    /// <returns>可执行该 method 的规划后端。</returns>
    private IPlanningBackend ResolvePlanningBackend(string methodName)
    {
        if (methodName == "doubles")
        {
            throw new NotSupportedException("doubles 轨迹规划尚未落地。");
        }

        if (_planningBackends.TryGetValue(methodName, out var backend))
        {
            return backend;
        }

        throw new NotSupportedException($"轨迹规划方法 {methodName} 未注册对应后端。");
    }

    /// <summary>
    /// 构造规划后端查找表，未显式注册时只启用默认 Rust icsp 后端。
    /// </summary>
    /// <param name="planningBackends">调用方显式注册的规划后端集合。</param>
    /// <returns>按 method 名称索引的后端表。</returns>
    private static IReadOnlyDictionary<string, IPlanningBackend> BuildPlanningBackendMap(IEnumerable<IPlanningBackend>? planningBackends)
    {
        var backends = planningBackends?.ToArray() ?? [];
        if (backends.Length == 0)
        {
            backends = [new RustIcspPlanningBackend()];
        }

        var map = new Dictionary<string, IPlanningBackend>(StringComparer.Ordinal);
        foreach (var backend in backends)
        {
            var methodName = NormalizeMethod(backend.MethodName);
            if (!map.TryAdd(methodName, backend))
            {
                throw new InvalidOperationException($"规划后端 method={methodName} 被重复注册。");
            }
        }

        return map;
    }

    /// <summary>
    /// 归一化旧 SDK 的 method 字符串。
    /// </summary>
    /// <param name="method">原始方法名。</param>
    /// <returns>小写短横线方法名。</returns>
    private static string NormalizeMethod(string method)
    {
        if (string.IsNullOrWhiteSpace(method))
        {
            return "icsp";
        }

        return method.Trim().ToLowerInvariant();
    }

    /// <summary>
    /// 为已上传飞拍轨迹构造包含参数和轨迹内容的缓存键，避免同名覆盖后误用旧规划结果。
    /// </summary>
    /// <param name="robot">机器人配置。</param>
    /// <param name="uploaded">上传轨迹。</param>
    /// <param name="options">执行参数。</param>
    /// <returns>缓存键。</returns>
    private static string CreateFlyshotCacheKey(
        RobotProfile robot,
        ControllerClientCompatUploadedTrajectory uploaded,
        FlyshotExecutionOptions options,
        CompatibilityRobotSettings settings,
        double planningSpeedScale)
    {
        var hash = new HashCode();
        hash.Add(robot.Name, StringComparer.Ordinal);
        hash.Add(planningSpeedScale);
        hash.Add(uploaded.Name, StringComparer.Ordinal);
        hash.Add(NormalizeMethod(options.Method), StringComparer.Ordinal);
        hash.Add(options.MoveToStart);
        hash.Add(options.SaveTrajectory);
        hash.Add(settings.UseDo);
        hash.Add(settings.IoKeepCycles);
        hash.Add(settings.TriggerSampleIndexOffsetCycles);
        hash.Add(settings.AdaptIcspTryNum);
        hash.Add(settings.SmoothStartStopTiming);

        foreach (var limit in robot.JointLimits)
        {
            hash.Add(limit.JointName, StringComparer.Ordinal);
            hash.Add(limit.VelocityLimit);
            hash.Add(limit.AccelerationLimit);
            hash.Add(limit.JerkLimit);
        }

        foreach (var waypoint in uploaded.Waypoints)
        {
            foreach (var value in waypoint)
            {
                hash.Add(value);
            }
        }

        foreach (var flag in uploaded.ShotFlags)
        {
            hash.Add(flag);
        }

        foreach (var offset in uploaded.OffsetValues)
        {
            hash.Add(offset);
        }

        foreach (var group in uploaded.AddressGroups)
        {
            foreach (var address in group)
            {
                hash.Add(address);
            }
        }

        return hash.ToHashCode().ToString("X8");
    }

    /// <summary>
    /// 构造编排器直接调用时的默认兼容配置，保持既有单元测试中的 DO 生成行为。
    /// </summary>
    /// <returns>默认机器人兼容配置。</returns>
    private static CompatibilityRobotSettings CreateDefaultRobotSettings()
    {
        return new CompatibilityRobotSettings(
            useDo: true,
            ioAddresses: Array.Empty<int>(),
            ioKeepCycles: 0,
            triggerSampleIndexOffsetCycles: 7,
            accLimitScale: 1.0,
            jerkLimitScale: 1.0,
            adaptIcspTryNum: 5);
    }

    /// <summary>
    /// 解析飞拍生成轨迹时真正使用的规划速度倍率。
    /// </summary>
    /// <param name="planningSpeedScale">配置或调用方传入的规划倍率。</param>
    /// <param name="speedRatio">本次执行速度倍率。</param>
    /// <returns>用于缩放规划关节限制的有效倍率。</returns>
    private static double ResolveEffectivePlanningSpeedScale(double planningSpeedScale, double speedRatio, string trajectoryKind)
    {
        if (speedRatio <= 0.0 || double.IsNaN(speedRatio) || double.IsInfinity(speedRatio))
        {
            throw new ArgumentOutOfRangeException(nameof(speedRatio), $"{trajectoryKind}速度倍率必须是有限正数。");
        }

        // 生成轨迹时把执行倍率折算进规划限制，保证 planning_speed_scale * speed_ratio 相同的配置产出同一条轨迹。
        return planningSpeedScale * speedRatio;
    }

    /// <summary>
    /// 按运行配置决定是否对规划结果做执行前时间轴重映射。
    /// </summary>
    /// <param name="plannedTrajectory">规划阶段得到的轨迹。</param>
    /// <param name="settings">当前 RobotConfig.json 解析出的兼容设置。</param>
    /// <returns>运行时真正用于采样和触发的轨迹。</returns>
    private static PlannedTrajectory ApplyExecutionTiming(PlannedTrajectory plannedTrajectory, CompatibilityRobotSettings settings)
    {
        // legacy-fit 模式需要严格保留 waypoint.txt 反推出的节点时间，不能再二次改写时间轴。
        return settings.SmoothStartStopTiming
            ? ApplySmoothStartStopTiming(plannedTrajectory)
            : plannedTrajectory;
    }

    /// <summary>
    /// 按规划全局速度倍率生成规划专用机器人约束。
    /// </summary>
    /// <param name="robot">原始机器人约束。</param>
    /// <param name="planningSpeedScale">规划阶段的全局速度倍率，1.0 表示不额外缩放。</param>
    /// <returns>已按速度倍率缩放后的规划机器人约束。</returns>
    private static RobotProfile ApplyPlanningSpeedScale(RobotProfile robot, double planningSpeedScale)
    {
        if (double.IsNaN(planningSpeedScale) || double.IsInfinity(planningSpeedScale) || planningSpeedScale <= 0.0)
        {
            throw new ArgumentOutOfRangeException(nameof(planningSpeedScale), "规划速度倍率必须是有限正数。");
        }

        if (Math.Abs(planningSpeedScale - 1.0) < 1e-12)
        {
            return robot;
        }

        // speed_ratio 已在调用方折算到规划倍率中；运行时只消费已经生成好的 8ms 稠密轨迹。
        var scaledLimits = robot.JointLimits
            .Select(limit => new JointLimit(
                limit.JointName,
                limit.VelocityLimit * planningSpeedScale,
                limit.AccelerationLimit * planningSpeedScale * planningSpeedScale,
                limit.JerkLimit * planningSpeedScale * planningSpeedScale * planningSpeedScale))
            .ToArray();

        return new RobotProfile(
            name: robot.Name,
            modelPath: robot.ModelPath,
            degreesOfFreedom: robot.DegreesOfFreedom,
            jointLimits: scaledLimits,
            jointCouplings: robot.JointCouplings,
            servoPeriod: robot.ServoPeriod,
            triggerPeriod: robot.TriggerPeriod);
    }

    /// <summary>
    /// 把兼容层输入数组转换成领域层 FlyshotProgram。
    /// </summary>
    /// <param name="name">轨迹名称。</param>
    /// <param name="waypoints">关节路点。</param>
    /// <param name="shotFlags">拍照标志。</param>
    /// <param name="offsetValues">偏移周期。</param>
    /// <param name="addressGroups">IO 地址组。</param>
    /// <returns>领域层飞拍程序。</returns>
    private static FlyshotProgram CreateProgram(
        string name,
        IEnumerable<IReadOnlyList<double>> waypoints,
        IEnumerable<bool> shotFlags,
        IEnumerable<int> offsetValues,
        IEnumerable<IReadOnlyList<int>> addressGroups)
    {
        return new FlyshotProgram(
            name: name,
            waypoints: waypoints.Select(static waypoint => new JointWaypoint(waypoint)).ToArray(),
            shotFlags: shotFlags.ToArray(),
            offsetValues: offsetValues.ToArray(),
            addressGroups: addressGroups.Select(static group => new IoAddressGroup(group)).ToArray());
    }

    /// <summary>
    /// 从规划轨迹和触发时间轴构造运行时可消费的稳定结果对象。
    /// </summary>
    /// <param name="plannedTrajectory">规划后的轨迹。</param>
    /// <param name="shotTimeline">触发时间轴。</param>
    /// <returns>运行时执行结果描述。</returns>
    private static TrajectoryResult CreateResult(
        PlannedTrajectory plannedTrajectory,
        ShotTimeline shotTimeline,
        IReadOnlyList<IReadOnlyList<double>> denseJointTrajectory,
        bool usedCache,
        int triggerSampleIndexOffsetCycles)
    {
        return new TrajectoryResult(
            programName: plannedTrajectory.OriginalProgram.Name,
            method: plannedTrajectory.Method,
            isValid: true,
            duration: TimeSpan.FromSeconds(plannedTrajectory.WaypointTimes[^1]),
            shotEvents: shotTimeline.ShotEvents,
            triggerTimeline: shotTimeline.TriggerTimeline,
            artifacts: Array.Empty<TrajectoryArtifact>(),
            failureReason: null,
            usedCache: usedCache,
            originalWaypointCount: plannedTrajectory.OriginalWaypointCount,
            plannedWaypointCount: plannedTrajectory.PlannedWaypointCount,
            triggerSampleIndexOffsetCycles: triggerSampleIndexOffsetCycles,
            denseJointTrajectory: denseJointTrajectory);
    }

    /// <summary>
    /// 生成满足离散速度、加速度和 Jerk 限制的稠密执行轨迹。
    /// </summary>
    private IReadOnlyList<IReadOnlyList<double>> CreateLimitCompliantDenseTrajectory(
        ref PlannedTrajectory executionTrajectory,
        bool shapeTrajectoryEdges)
    {
        for (var iteration = 0; iteration <= MaxDenseLimitStretchIterations; iteration++)
        {
            var denseJointTrajectory = TrajectorySampler.SampleJointTrajectory(
                executionTrajectory,
                samplePeriod: executionTrajectory.Robot.ServoPeriod.TotalSeconds,
                smoothStartStop: shapeTrajectoryEdges);

            try
            {
                TrajectoryLimitValidator.ValidateDenseJointTrajectory(
                    executionTrajectory.Robot,
                    denseJointTrajectory,
                    trajectoryName: executionTrajectory.OriginalProgram.Name,
                    validateJerk: false);
                return denseJointTrajectory;
            }
            catch (InvalidOperationException ex) when (iteration < MaxDenseLimitStretchIterations)
            {
                _logger?.LogWarning(ex, "稠密轨迹离散限幅校验失败，准备拉长时间轴重试");
                // 离散差分超限时统一拉长时间轴，保持路点几何不变并降低速度、加速度和 Jerk。
                executionTrajectory = StretchTrajectoryTiming(executionTrajectory, DenseLimitStretchFactor);
             _logger?.LogInformation(
                 "离散差分超限，拉长时间轴，iteration={Iteration}, factor={StretchFactor}",
                 iteration,
                 DenseLimitStretchFactor);
             _logger?.LogInformation("拉长之后的总时间={TotalTime}", executionTrajectory.WaypointTimes[^1]);
            }
        }

        throw new InvalidOperationException("稠密轨迹离散限幅校验未能产生有效结果。");
    }

    /// <summary>
    /// 按统一倍率拉长轨迹时间轴，保留原始路点和触发元数据。
    /// </summary>
    private  PlannedTrajectory StretchTrajectoryTiming(PlannedTrajectory trajectory, double stretchFactor)
    {
        var waypointTimes = trajectory.WaypointTimes.Select(time => time * stretchFactor).ToArray();
        var segmentDurations = trajectory.SegmentDurations.Select(duration => duration * stretchFactor).ToArray();
        var segmentScales = trajectory.SegmentScales.Select(scale => scale / stretchFactor).ToArray();

        return new PlannedTrajectory(
            robot: trajectory.Robot,
            originalProgram: trajectory.OriginalProgram,
            plannedWaypoints: trajectory.PlannedWaypoints,
            waypointTimes: waypointTimes,
            segmentDurations: segmentDurations,
            segmentScales: segmentScales,
            method: trajectory.Method,
            iterations: trajectory.Iterations,
            threshold: trajectory.Threshold);
    }

    /// <summary>
    /// 为飞拍执行生成平滑起停时间轴（仅重定时，不改几何路点）。
    /// 该方法保持 <see cref="PlannedTrajectory.PlannedWaypoints"/> 不变，只重映射 <see cref="PlannedTrajectory.WaypointTimes"/>，
    /// 让轨迹在起点和终点附近具有更柔和的速度变化，从而降低“突然起步/突然收尾”带来的离散差分尖峰。
    /// </summary>
    /// <param name="plannedTrajectory">规划器输出的原始轨迹（通常是线性时间轴）。</param>
    /// <returns>
    /// 若满足平滑条件，则返回新的重定时轨迹；若路点数量不足或总时长无效，则直接返回输入轨迹。
    /// </returns>
    private static PlannedTrajectory ApplySmoothStartStopTiming(PlannedTrajectory plannedTrajectory)
    {
        var originalTimes = plannedTrajectory.WaypointTimes;
        // 至少需要“起点-中间点-终点”三类点，才有可平滑的中间区间。
        if (originalTimes.Count < 3)
        {
            return plannedTrajectory;
        }

        var totalDuration = originalTimes[^1];
        // 轨迹总时长必须为正，避免后续归一化进度出现除零或无意义映射。
        if (totalDuration <= 0.0)
        {
            return plannedTrajectory;
        }

        var smoothedTimes = new double[originalTimes.Count];
        // 强制固定边界：起点仍在 0，终点仍在总时长，保证任务总耗时不变。
        smoothedTimes[0] = 0.0;
        smoothedTimes[^1] = totalDuration;

        for (var index = 1; index < originalTimes.Count - 1; index++)
        {
            // 把原始时刻归一化到 [0, 1]，用于统一时间律变换。
            var normalizedProgress = originalTimes[index] / totalDuration;
            // 先求完整平滑时间律对应的时刻，再与原线性时刻按比例混合。
            // 这里采用弱平滑（blend=0.35）：保留大部分原节奏，仅轻微降低首尾突变。
            var smoothedProgress = InvertSmoothStartStopProgress(normalizedProgress);
            var blendedProgress = ((1.0 - SmoothStartStopBlend) * normalizedProgress)
                + (SmoothStartStopBlend * smoothedProgress);
            smoothedTimes[index] = totalDuration * blendedProgress;
        }

        var segmentDurations = new double[smoothedTimes.Length - 1];
        for (var index = 0; index < segmentDurations.Length; index++)
        {
            // 重建每段持续时间，供后续稠密采样和限幅检查使用。
            segmentDurations[index] = smoothedTimes[index + 1] - smoothedTimes[index];
        }

        return new PlannedTrajectory(
            robot: plannedTrajectory.Robot,
            originalProgram: plannedTrajectory.OriginalProgram,
            plannedWaypoints: plannedTrajectory.PlannedWaypoints,
            waypointTimes: smoothedTimes,
            segmentDurations: segmentDurations,
            segmentScales: plannedTrajectory.SegmentScales,
            method: plannedTrajectory.Method,
            iterations: plannedTrajectory.Iterations,
            threshold: plannedTrajectory.Threshold);
    }

    /// <summary>
    /// 反解平滑时间律进度：给定目标进度 p，求满足 f(t)=p 的 t。
    /// 其中 f(t) 由 <see cref="EvaluateSmoothStartStopProgress"/> 给出，是单调递增的 7 次 smootherstep 曲线，
    /// 通过二分法可稳定得到对应的归一化时间，避免显式求高次方程根带来的复杂性和数值不稳定。
    /// </summary>
    /// <param name="normalizedProgress">目标归一化进度，理论范围 [0, 1]。</param>
    /// <returns>与目标进度对应的归一化时间，范围 [0, 1]。</returns>
    private static double InvertSmoothStartStopProgress(double normalizedProgress)
    {
        // 防御式裁剪，避免上游浮点误差将进度推到区间外。
        var target = Math.Clamp(normalizedProgress, 0.0, 1.0);
        var low = 0.0;
        var high = 1.0;
        // 固定迭代次数确保耗时可预测；40 次足以把区间收敛到工程可用精度。
        for (var iteration = 0; iteration < 40; iteration++)
        {
            var middle = (low + high) / 2.0;
            var progress = EvaluateSmoothStartStopProgress(middle);
            // f(middle) 小于目标，说明根在右半区间；否则在左半区间。
            if (progress < target)
            {
                low = middle;
            }
            else
            {
                high = middle;
            }
        }

        return (low + high) / 2.0;
    }

    /// <summary>
    /// 计算 7 次 smootherstep 的进度函数值 f(u)。
    /// 该函数在 u=0 和 u=1 处具有更高阶导数连续性，可让起停段速度变化更平滑，
    /// 适合用于飞拍轨迹的时间重参数化，减少离散导数在边界处的突变。
    /// </summary>
    /// <param name="normalizedTime">归一化时间 u，理论范围 [0, 1]。</param>
    /// <returns>归一化进度 f(u)，范围 [0, 1]。</returns>
    private static double EvaluateSmoothStartStopProgress(double normalizedTime)
    {
        // 先裁剪再计算多项式，确保数值稳定且不受外部越界输入影响。
        var u = Math.Clamp(normalizedTime, 0.0, 1.0);
        var u2 = u * u;
        var u3 = u2 * u;
        var u4 = u3 * u;
        var u5 = u4 * u;
        var u6 = u5 * u;
        var u7 = u6 * u;
        return (35.0 * u4) - (84.0 * u5) + (70.0 * u6) - (20.0 * u7);
    }
}