ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Builds immutable world-space trajectory points from validated LS/ST results.</summary>
public sealed class EmTrajectoryAssembler
{
    private readonly double outputTimeStepSeconds;
    private readonly double zeroSpeedHoldSeconds;
    private readonly int maximumPublishedSampleCount;

    public EmTrajectoryAssembler()
        : this(EmPlannerConfiguration.CreateDefault())
    {
    }

    public EmTrajectoryAssembler(EmPlannerConfiguration configuration)
    {
        if (configuration == null || configuration.Scheduling == null || configuration.Longitudinal == null)
            throw new ArgumentNullException(nameof(configuration));
        outputTimeStepSeconds = configuration.Scheduling.OutputTimeStepSeconds;
        zeroSpeedHoldSeconds = configuration.Longitudinal.ZeroSpeedHoldSeconds;
        maximumPublishedSampleCount = configuration.Scheduling.MaximumPublishedSampleCount;
        if (!IsFinite(outputTimeStepSeconds) || outputTimeStepSeconds <= 0d || !IsFinite(zeroSpeedHoldSeconds) ||
            zeroSpeedHoldSeconds < 0d || maximumPublishedSampleCount < 2)
        {
            throw new ArgumentOutOfRangeException(nameof(configuration));
        }
    }

    public EmTrajectory Assemble(LateralPath path, LongitudinalPlanningResult longitudinal, EmTrajectoryMetadata metadata)
    {
        EmPlanningStatus status = TryAssemble(path, longitudinal, metadata, out EmTrajectory trajectory,
            out string failureReason);
        if (status != EmPlanningStatus.Success)
            throw new ArgumentException(failureReason, nameof(longitudinal));
        return trajectory;
    }

    public EmPlanningStatus TryAssemble(LateralPath path, LongitudinalPlanningResult longitudinal,
        EmTrajectoryMetadata metadata, out EmTrajectory trajectory, out string failureReason)
    {
        trajectory = null;
        failureReason = string.Empty;
        if (longitudinal == null || longitudinal.Candidate == null ||
            (longitudinal.Status != EmPlanningStatus.Success && longitudinal.Status != EmPlanningStatus.SuccessWithFallback))
        {
            throw new ArgumentException("Trajectory assembly requires a successful longitudinal result.", nameof(longitudinal));
        }
        if (metadata == null)
            throw new ArgumentNullException(nameof(metadata));

        var interpolator = new LateralPathInterpolator(path);
        bool isFullDirectionSegment = metadata.PlanningScope == EmPlanningScope.FullDirectionSegment;
        double holdDurationSeconds = isFullDirectionSegment ? 0d : zeroSpeedHoldSeconds;
        if (TrajectorySampleSchedule.ExceedsMaximumSampleCount(longitudinal.Candidate, outputTimeStepSeconds,
                holdDurationSeconds, metadata.LongitudinalMode, isFullDirectionSegment, maximumPublishedSampleCount,
                out int requiredSampleCount))
        {
            failureReason = "FullSegmentResourceLimitExceeded: MaximumPublishedSampleCount=" +
                maximumPublishedSampleCount + ";required-at-least=" + requiredSampleCount;
            return EmPlanningStatus.FullSegmentResourceLimitExceeded;
        }
        var schedule = new TrajectorySampleSchedule(longitudinal.Candidate, outputTimeStepSeconds, holdDurationSeconds,
            metadata.LongitudinalMode, isFullDirectionSegment);
        double terminalPathS = path.Points[path.Points.Count - 1].PathS;
        var points = new List<EmTrajectoryPoint>(schedule.Samples.Count);
        double directionSign = metadata.Direction == TravelDirection.Forward ? 1d : -1d;

        for (int index = 0; index < schedule.Samples.Count; index++)
        {
            TrajectorySample sample = schedule.Samples[index];
            if (sample.PathS > terminalPathS + 1e-10d)
                throw new ArgumentException("Longitudinal PathS exceeds the assembled lateral path.", nameof(longitudinal));

            InterpolatedLateralPathPoint geometry = interpolator.Interpolate(sample.PathS);
            bool isTerminalAnchor = metadata.LongitudinalMode == EmLongitudinalMode.ExactStopAtBoundary &&
                index == schedule.TerminalAnchorSampleIndex;
            EmBoundaryType boundaryType = isTerminalAnchor ? ToBoundaryType(metadata.TerminalType) : EmBoundaryType.None;
            double signedSpeed = directionSign * sample.ProgressSpeed;
            points.Add(new EmTrajectoryPoint(geometry.X, geometry.Y, geometry.Yaw, signedSpeed, sample.TimeFromStart,
                geometry.VehicleCurvature, metadata.SegmentIndex, geometry.ReferenceS, sample.PathS, metadata.Direction,
                boundaryType, sample.Acceleration, sample.Jerk));
        }

        trajectory = new EmTrajectory(metadata, points);
        return EmPlanningStatus.Success;
    }

    private static EmBoundaryType ToBoundaryType(EmTerminalType terminalType)
    {
        switch (terminalType)
        {
            case EmTerminalType.RollingSafetyStop:
                return EmBoundaryType.RollingSafetyStop;
            case EmTerminalType.GearSwitch:
                return EmBoundaryType.GearSwitchApproach;
            case EmTerminalType.Goal:
                return EmBoundaryType.Goal;
            default:
                throw new ArgumentOutOfRangeException(nameof(terminalType));
        }
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}