ParkingRobot/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Lateral/LateralPlanningInput.cs

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

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

/// <summary>Immutable inputs for one lateral solve over a single direction segment.</summary>
public sealed class LateralPlanningInput
{
    private const double Epsilon = 1e-12d;

    public LateralPlanningInput(DirectionSegmentView referenceSegment, StaticCorridor corridor,
        FrenetProjection startProjection, EmTerminalType terminalType, VehicleParameters vehicle,
        EmPlannerConfiguration configuration, IReadOnlyList<FrenetProjection> previousTrajectorySeed)
    {
        if (referenceSegment == null)
            throw new ArgumentNullException(nameof(referenceSegment));
        if (corridor == null)
            throw new ArgumentNullException(nameof(corridor));
        if (startProjection == null)
            throw new ArgumentNullException(nameof(startProjection));
        if (vehicle == null)
            throw new ArgumentNullException(nameof(vehicle));
        if (configuration == null)
            throw new ArgumentNullException(nameof(configuration));
        if (!Enum.IsDefined(typeof(EmTerminalType), terminalType))
            throw new ArgumentOutOfRangeException(nameof(terminalType));

        ReferenceSegment = referenceSegment;
        Corridor = CopyCorridor(corridor);
        ReferenceStations = CopyStationValues(Corridor.Stations);
        ValidateCorridorMatchesInput(referenceSegment, Corridor.Stations, startProjection);
        StartProjection = startProjection;
        TerminalType = terminalType;
        Vehicle = CopyVehicle(vehicle);
        Configuration = configuration.Copy();
        PreviousTrajectorySeed = CopySeed(previousTrajectorySeed, referenceSegment);
    }

    public DirectionSegmentView ReferenceSegment { get; }

    public StaticCorridor Corridor { get; }

    public IReadOnlyList<double> ReferenceStations { get; }

    public FrenetProjection StartProjection { get; }

    public EmTerminalType TerminalType { get; }

    public VehicleParameters Vehicle { get; }

    public EmPlannerConfiguration Configuration { get; }

    public IReadOnlyList<FrenetProjection> PreviousTrajectorySeed { get; }

    private static StaticCorridor CopyCorridor(StaticCorridor source)
    {
        var stations = new List<LateralInterval>(source.Stations.Count);
        for (int index = 0; index < source.Stations.Count; index++)
        {
            LateralInterval station = source.Stations[index];
            if (station == null)
                throw new ArgumentException("Corridor stations cannot be null.", nameof(source));
            stations.Add(new LateralInterval(station.ReferenceS, station.MinimumL, station.MaximumL, station.SeedL));
        }
        return new StaticCorridor(stations);
    }

    private static IReadOnlyList<double> CopyStationValues(IReadOnlyList<LateralInterval> stations)
    {
        if (stations.Count < 2)
            throw new ArgumentException("A lateral planning input requires at least two corridor stations.", nameof(stations));

        var copy = new List<double>(stations.Count);
        double previous = double.NegativeInfinity;
        for (int index = 0; index < stations.Count; index++)
        {
            double referenceS = stations[index].ReferenceS;
            if (referenceS <= previous)
                throw new ArgumentException("Corridor reference-S stations must be strictly increasing.", nameof(stations));
            copy.Add(referenceS);
            previous = referenceS;
        }
        return new ReadOnlyCollection<double>(copy);
    }

    private static void ValidateCorridorMatchesInput(DirectionSegmentView segment, IReadOnlyList<LateralInterval> stations,
        FrenetProjection start)
    {
        if (start.ReferencePoint == null || start.ReferencePoint.Direction != segment.Direction ||
            start.ReferenceS < -Epsilon || start.ReferenceS > segment.LengthMeters + Epsilon)
        {
            throw new ArgumentException("Start projection must belong to the selected direction segment.", nameof(start));
        }
        if (Math.Abs(stations[0].ReferenceS - start.ReferenceS) > Epsilon)
            throw new ArgumentException("The first corridor station must match the start projection reference S.", nameof(stations));
        if (start.LateralOffset < stations[0].MinimumL - Epsilon || start.LateralOffset > stations[0].MaximumL + Epsilon)
            throw new ArgumentException("Start projection is outside the first hard corridor interval.", nameof(start));

        for (int index = 0; index < stations.Count; index++)
        {
            if (stations[index].ReferenceS < -Epsilon || stations[index].ReferenceS > segment.LengthMeters + Epsilon)
                throw new ArgumentException("Corridor stations must lie inside the selected direction segment.", nameof(stations));
        }
    }

    private static IReadOnlyList<FrenetProjection> CopySeed(IReadOnlyList<FrenetProjection> seed,
        DirectionSegmentView segment)
    {
        var copy = new List<FrenetProjection>(seed == null ? 0 : seed.Count);
        if (seed != null)
        {
            for (int index = 0; index < seed.Count; index++)
            {
                FrenetProjection projection = seed[index];
                if (projection == null || projection.ReferencePoint.Direction != segment.Direction ||
                    projection.ReferenceS < -Epsilon || projection.ReferenceS > segment.LengthMeters + Epsilon)
                {
                    throw new ArgumentException("Previous lateral seed must belong to the selected direction segment.", nameof(seed));
                }
                copy.Add(projection);
            }
        }
        return new ReadOnlyCollection<FrenetProjection>(copy);
    }

    private static VehicleParameters CopyVehicle(VehicleParameters vehicle)
    {
        return new VehicleParameters
        {
            LengthMeters = vehicle.LengthMeters,
            WidthMeters = vehicle.WidthMeters,
            SafetyMarginMeters = vehicle.SafetyMarginMeters,
            MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
            MinimumTurningRadiusMeters = vehicle.MinimumTurningRadiusMeters,
        };
    }
}