ParkingRobot/Simulation/Core/SimulationWorld.cs

using MyParking.Shared;
using MyParking.Simulation.Models;

namespace MyParking.Simulation.Core;

/// <summary>
/// 管理离线仿真车辆、车队中心和成员布局。
/// </summary>
public sealed class SimulationWorld
{
    private readonly object _syncRoot = new();
    private Dictionary<int, SimulationVehicle> _vehicles = new();
    private SimulationConfigurationDto _configuration;

    public SimulationWorld()
    {
        _configuration = CreateDefaultConfiguration();
        ApplyConfigurationCore(_configuration);
    }

    public T WithVehicle<T>(
        int vehicleId,
        Func<SimulationVehicle, T> action)
    {
        lock (_syncRoot)
        {
            if (!_vehicles.TryGetValue(
                    vehicleId,
                    out var vehicle))
            {
                throw new KeyNotFoundException(
                    $"不存在车辆{vehicleId}。");
            }

            return action(vehicle);
        }
    }

    public void Step(double deltaTimeSeconds)
    {
        lock (_syncRoot)
        {
            foreach (var vehicle in _vehicles.Values)
                vehicle.Step(deltaTimeSeconds);
        }
    }

    public IReadOnlyList<VehicleStateDto> GetSnapshot()
    {
        lock (_syncRoot)
        {
            return _vehicles.Values
                .OrderBy(vehicle => vehicle.VehicleId)
                .Select(vehicle => vehicle.GetSnapshot())
                .ToArray();
        }
    }

    public SimulationConfigurationDto GetConfiguration()
    {
        lock (_syncRoot)
        {
            return CloneConfiguration(_configuration);
        }
    }

    public void ApplyConfiguration(
        SimulationConfigurationDto configuration)
    {
        ValidateConfiguration(configuration);

        lock (_syncRoot)
        {
            _configuration = CloneConfiguration(configuration);
            ApplyConfigurationCore(_configuration);
        }
    }

    public void Reset()
    {
        lock (_syncRoot)
        {
            ApplyConfigurationCore(_configuration);
        }
    }

    private void ApplyConfigurationCore(
        SimulationConfigurationDto configuration)
    {
        var fleetPoseInWorld = new Pose2D(
            configuration.FleetCenter.XMeters,
            configuration.FleetCenter.YMeters,
            configuration.FleetCenter.YawRadians);

        _vehicles = configuration.Vehicles
            .Take(configuration.VehicleCount)
            .Select(layout =>
            {
                var bodyPoseInFleet = new Pose2D(
                    layout.XMeters,
                    layout.YMeters,
                    layout.YawRadians);

                var bodyPoseInWorld =
                    FrameTransform2D.Compose(
                        fleetPoseInWorld,
                        bodyPoseInFleet);

                return new SimulationVehicle(
                    layout.VehicleId,
                    bodyPoseInWorld.XMeters,
                    bodyPoseInWorld.YMeters,
                    bodyPoseInWorld.YawRadians);
            })
            .ToDictionary(vehicle => vehicle.VehicleId);
    }

    private static void ValidateConfiguration(
        SimulationConfigurationDto configuration)
    {
        if (configuration.VehicleCount is < 1 or > 8)
        {
            throw new ArgumentOutOfRangeException(
                nameof(configuration.VehicleCount),
                "仿真车辆数量必须在1到8之间。");
        }

        if (configuration.FleetCenter == null)
        {
            throw new ArgumentException(
                "必须提供车队中心位姿。",
                nameof(configuration));
        }

        if (configuration.Vehicles == null ||
            configuration.Vehicles.Count <
            configuration.VehicleCount)
        {
            throw new ArgumentException(
                "成员布局数量不能少于车辆数量。",
                nameof(configuration));
        }

        var selectedLayouts = configuration.Vehicles
            .Take(configuration.VehicleCount)
            .ToArray();

        if (selectedLayouts.Any(layout =>
                layout.VehicleId <= 0) ||
            selectedLayouts
                .Select(layout => layout.VehicleId)
                .Distinct()
                .Count() != selectedLayouts.Length)
        {
            throw new ArgumentException(
                "车辆编号必须大于零且不能重复。",
                nameof(configuration));
        }

        var values = new[]
        {
            configuration.FleetCenter.XMeters,
            configuration.FleetCenter.YMeters,
            configuration.FleetCenter.YawRadians
        }.Concat(selectedLayouts.SelectMany(layout => new[]
        {
            layout.XMeters,
            layout.YMeters,
            layout.YawRadians
        }));

        if (values.Any(value =>
                double.IsNaN(value) ||
                double.IsInfinity(value)))
        {
            throw new ArgumentException(
                "车队和车辆布局不能包含NaN或无穷大。",
                nameof(configuration));
        }
    }

    private static SimulationConfigurationDto
        CreateDefaultConfiguration()
    {
        return new SimulationConfigurationDto(
            1,
            new FleetCenterDto(0.0, 0.0, 0.0),
            new[]
            {
                new VehicleLayoutDto(1, 0.0, 0.0, 0.0)
            });
    }

    private static SimulationConfigurationDto
        CloneConfiguration(
            SimulationConfigurationDto configuration)
    {
        return new SimulationConfigurationDto(
            configuration.VehicleCount,
            configuration.FleetCenter with { },
            configuration.Vehicles
                .Select(layout => layout with { })
                .ToArray());
    }
}