1094 lines
36 KiB
C#
1094 lines
36 KiB
C#
using AutoForge.Core.Simulation;
|
|
using AutoForge.Diagnostics;
|
|
|
|
namespace AutoForge.Simulation;
|
|
|
|
internal sealed record DynamicsTelemetryState(
|
|
long StepIndex,
|
|
double ModelTimeSeconds,
|
|
bool EngineRunning,
|
|
double EngineSpeedRpm,
|
|
double VehicleSpeedMetersPerSecond,
|
|
int Gear,
|
|
double Throttle,
|
|
double Brake,
|
|
double EngineLoad,
|
|
double EngineTorqueNewtonMeters,
|
|
double FuelRateKilogramsPerSecond,
|
|
double FuelConsumedKilograms,
|
|
double EngineTemperatureCelsius,
|
|
double DistanceMeters,
|
|
double AccelerationMetersPerSecondSquared,
|
|
double CouplingSlipFraction,
|
|
int ShiftCount,
|
|
DynamicsScenarioPhaseKind Phase);
|
|
|
|
internal sealed class DynamicsEngine
|
|
{
|
|
internal object SyncRoot { get; } = new();
|
|
private readonly DynamicsConfiguration _configuration;
|
|
private readonly List<DynamicsTelemetryState> _samples = [];
|
|
private long _stepIndex;
|
|
private int _phaseIndex;
|
|
private long _phaseStepCount;
|
|
private double _phaseElapsedSeconds;
|
|
private double _completedScenarioSeconds;
|
|
private double _speed;
|
|
private int _gear;
|
|
private bool _engineRunning;
|
|
private double _engineSpeedRpm;
|
|
private double _engineTemperature;
|
|
private double _fuelConsumed;
|
|
private double _distance;
|
|
private double _throttle;
|
|
private double _brake;
|
|
private double _engineLoad;
|
|
private double _engineTorque;
|
|
private double _fuelRate;
|
|
private double _acceleration;
|
|
private double _couplingSlip;
|
|
private double _cruiseIntegral;
|
|
private int _shiftCount;
|
|
private DynamicsLifecycleState _lifecycle = DynamicsLifecycleState.Stopped;
|
|
private string? _failure;
|
|
|
|
private sealed record MutableState(
|
|
long StepIndex,
|
|
int PhaseIndex,
|
|
long PhaseStepCount,
|
|
double PhaseElapsedSeconds,
|
|
double CompletedScenarioSeconds,
|
|
double Speed,
|
|
int Gear,
|
|
bool EngineRunning,
|
|
double EngineSpeedRpm,
|
|
double EngineTemperature,
|
|
double FuelConsumed,
|
|
double Distance,
|
|
double Throttle,
|
|
double Brake,
|
|
double EngineLoad,
|
|
double EngineTorque,
|
|
double FuelRate,
|
|
double Acceleration,
|
|
double CouplingSlip,
|
|
double CruiseIntegral,
|
|
int ShiftCount,
|
|
DynamicsLifecycleState Lifecycle,
|
|
string? Failure,
|
|
int SampleCount);
|
|
|
|
public DynamicsEngine(DynamicsConfiguration configuration, string runIdentifier)
|
|
{
|
|
_configuration = configuration ?? throw new ArgumentNullException(nameof(configuration));
|
|
ArgumentException.ThrowIfNullOrWhiteSpace(runIdentifier);
|
|
RunIdentifier = runIdentifier;
|
|
ResetState();
|
|
}
|
|
|
|
public string RunIdentifier { get; }
|
|
public DynamicsConfiguration Configuration => _configuration;
|
|
public DynamicsLifecycleState Lifecycle => _lifecycle;
|
|
public int SampleCount => _samples.Count;
|
|
public int CruiseSampleCount => _samples.Count(sample => sample.Phase == DynamicsScenarioPhaseKind.Cruise);
|
|
|
|
public DynamicsRunStatus Status
|
|
{
|
|
get
|
|
{
|
|
lock (SyncRoot)
|
|
{
|
|
var phase = _phaseIndex < _configuration.Scenario.Count
|
|
? _configuration.Scenario[_phaseIndex]
|
|
: null;
|
|
var totalDuration = _configuration.Scenario.Sum(item => item.DurationSeconds);
|
|
var completedDuration = _completedScenarioSeconds + _phaseElapsedSeconds;
|
|
var phaseProgress = phase is null
|
|
? 1
|
|
: Math.Clamp(_phaseElapsedSeconds / phase.DurationSeconds, 0, 1);
|
|
return new DynamicsRunStatus(
|
|
RunIdentifier,
|
|
_configuration.ConfigurationIdentifier,
|
|
_stepIndex,
|
|
_stepIndex * _configuration.Model.StepSeconds,
|
|
_lifecycle,
|
|
_phaseIndex,
|
|
phase?.Kind.ToString(),
|
|
_phaseElapsedSeconds,
|
|
_lifecycle == DynamicsLifecycleState.Completed || totalDuration == 0
|
|
? 1
|
|
: Math.Clamp(completedDuration / totalDuration, 0, 1),
|
|
phaseProgress,
|
|
_failure);
|
|
}
|
|
}
|
|
}
|
|
|
|
public DynamicsTelemetryState ReadTelemetry(string moduleIdentifier)
|
|
{
|
|
lock (SyncRoot)
|
|
{
|
|
if (!StringComparer.OrdinalIgnoreCase.Equals(moduleIdentifier, _configuration.EngineModuleIdentifier) &&
|
|
!StringComparer.OrdinalIgnoreCase.Equals(moduleIdentifier, _configuration.TransmissionModuleIdentifier))
|
|
{
|
|
throw new KeyNotFoundException($"Dynamics module '{moduleIdentifier}' is not part of this run.");
|
|
}
|
|
|
|
return CurrentState();
|
|
}
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
RequireLifecycle(DynamicsLifecycleState.Stopped);
|
|
_lifecycle = DynamicsLifecycleState.Running;
|
|
}
|
|
|
|
public void Pause()
|
|
{
|
|
RequireLifecycle(DynamicsLifecycleState.Running);
|
|
_lifecycle = DynamicsLifecycleState.Paused;
|
|
}
|
|
|
|
public void Resume()
|
|
{
|
|
RequireLifecycle(DynamicsLifecycleState.Paused);
|
|
_lifecycle = DynamicsLifecycleState.Running;
|
|
}
|
|
|
|
public void Stop()
|
|
{
|
|
if (_lifecycle is DynamicsLifecycleState.Closed or DynamicsLifecycleState.Faulted or DynamicsLifecycleState.Completed)
|
|
{
|
|
throw new InvalidOperationException($"A {_lifecycle} dynamics run cannot be stopped.");
|
|
}
|
|
|
|
_lifecycle = DynamicsLifecycleState.Stopped;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
if (_lifecycle == DynamicsLifecycleState.Closed)
|
|
{
|
|
throw new InvalidOperationException("A closed dynamics run cannot be reset.");
|
|
}
|
|
|
|
ResetState();
|
|
}
|
|
|
|
public void Close()
|
|
{
|
|
if (_lifecycle != DynamicsLifecycleState.Closed)
|
|
{
|
|
_lifecycle = DynamicsLifecycleState.Closed;
|
|
}
|
|
}
|
|
|
|
public DynamicsRunStatus AdvanceOne()
|
|
{
|
|
if (_lifecycle is DynamicsLifecycleState.Closed or DynamicsLifecycleState.Faulted or DynamicsLifecycleState.Completed)
|
|
{
|
|
throw new InvalidOperationException($"A {_lifecycle} dynamics run cannot advance.");
|
|
}
|
|
|
|
if (_phaseIndex >= _configuration.Scenario.Count)
|
|
{
|
|
_lifecycle = DynamicsLifecycleState.Completed;
|
|
return Status;
|
|
}
|
|
|
|
var previous = CaptureMutableState();
|
|
try
|
|
{
|
|
var model = _configuration.Model;
|
|
var step = model.StepSeconds;
|
|
var phase = _configuration.Scenario[_phaseIndex];
|
|
UpdateGearAndEngineState(phase);
|
|
UpdateDriverInputs(phase);
|
|
UpdateEngineAndForces(step, phase);
|
|
if (!AllFiniteAndInDomain())
|
|
{
|
|
throw new ArithmeticException("Dynamics left its finite supported domain.");
|
|
}
|
|
|
|
_stepIndex++;
|
|
_phaseStepCount++;
|
|
_phaseElapsedSeconds = _phaseStepCount * step;
|
|
_samples.Add(CurrentState());
|
|
AdvanceScenarioIfReady(phase);
|
|
}
|
|
catch (ArithmeticException exception)
|
|
{
|
|
RestoreMutableState(previous);
|
|
Fault($"Dynamics calculation failed explicitly: {exception.Message}");
|
|
}
|
|
|
|
return Status;
|
|
}
|
|
|
|
internal DynamicsTelemetryState CurrentState() => new(
|
|
_stepIndex,
|
|
_stepIndex * _configuration.Model.StepSeconds,
|
|
_engineRunning,
|
|
_engineSpeedRpm,
|
|
_speed,
|
|
_gear,
|
|
_throttle,
|
|
_brake,
|
|
_engineLoad,
|
|
_engineTorque,
|
|
_fuelRate,
|
|
_fuelConsumed,
|
|
_engineTemperature,
|
|
_distance,
|
|
_acceleration,
|
|
_couplingSlip,
|
|
_shiftCount,
|
|
_configuration.Scenario[Math.Min(_phaseIndex, _configuration.Scenario.Count - 1)].Kind);
|
|
|
|
internal IReadOnlyList<DynamicsTelemetryState> Samples => Array.AsReadOnly(_samples.ToArray());
|
|
|
|
private void UpdateGearAndEngineState(DynamicsScenarioPhase phase)
|
|
{
|
|
var model = _configuration.Model;
|
|
if (phase.Kind == DynamicsScenarioPhaseKind.WarmIdle)
|
|
{
|
|
_engineRunning = true;
|
|
_gear = 0;
|
|
}
|
|
else if (phase.Kind is DynamicsScenarioPhaseKind.Accelerate or DynamicsScenarioPhaseKind.Cruise)
|
|
{
|
|
_engineRunning = true;
|
|
if (_gear == 0)
|
|
{
|
|
_gear = 1;
|
|
}
|
|
|
|
var lockedRpm = CoupledEngineSpeed(_gear);
|
|
var inSlip = lockedRpm < model.IdleSpeedRpm ||
|
|
_speed < model.LaunchSlipEndSpeedMetersPerSecond && _gear == 1;
|
|
if (!inSlip)
|
|
{
|
|
while (_gear < model.ForwardGearRatios.Count)
|
|
{
|
|
var gearIndex = _gear - 1;
|
|
if (lockedRpm < model.UpshiftSpeedsRpm[gearIndex] &&
|
|
lockedRpm <= model.MaximumEngineSpeedRpm)
|
|
{
|
|
break;
|
|
}
|
|
|
|
_gear++;
|
|
_shiftCount++;
|
|
_cruiseIntegral = 0;
|
|
lockedRpm = CoupledEngineSpeed(_gear);
|
|
}
|
|
|
|
if (lockedRpm > model.MaximumEngineSpeedRpm)
|
|
{
|
|
throw new ArithmeticException(
|
|
$"Locked drivetrain speed {lockedRpm:F1} rpm exceeds the configured {model.MaximumEngineSpeedRpm:F1} rpm limit in top gear.");
|
|
}
|
|
|
|
var activeGearIndex = _gear - 1;
|
|
if (lockedRpm <= model.DownshiftSpeedsRpm[activeGearIndex] && _gear > 1)
|
|
{
|
|
_gear--;
|
|
_shiftCount++;
|
|
_cruiseIntegral = 0;
|
|
}
|
|
}
|
|
}
|
|
else if (phase.Kind is DynamicsScenarioPhaseKind.Brake or DynamicsScenarioPhaseKind.ReturnIdle)
|
|
{
|
|
_engineRunning = true;
|
|
if (_speed <= 0.1)
|
|
{
|
|
_gear = 0;
|
|
}
|
|
else if (_gear > 1)
|
|
{
|
|
var gearIndex = _gear - 1;
|
|
if (CoupledEngineSpeed(_gear) <= model.DownshiftSpeedsRpm[gearIndex])
|
|
{
|
|
_gear--;
|
|
_shiftCount++;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void UpdateDriverInputs(DynamicsScenarioPhase phase)
|
|
{
|
|
var model = _configuration.Model;
|
|
var roadLoad = RoadLoadForce(_speed);
|
|
_brake = 0;
|
|
switch (phase.Kind)
|
|
{
|
|
case DynamicsScenarioPhaseKind.WarmIdle:
|
|
_throttle = 0;
|
|
break;
|
|
case DynamicsScenarioPhaseKind.Accelerate:
|
|
{
|
|
if (_speed >= phase.TargetSpeedMetersPerSecond)
|
|
{
|
|
_throttle = 0;
|
|
break;
|
|
}
|
|
|
|
var requestedForce = model.VehicleMassKilograms * 2.0 + roadLoad;
|
|
_throttle = ThrottleForForce(requestedForce);
|
|
break;
|
|
}
|
|
case DynamicsScenarioPhaseKind.Cruise:
|
|
{
|
|
var error = phase.TargetSpeedMetersPerSecond - _speed;
|
|
_cruiseIntegral = Math.Clamp(_cruiseIntegral + error * model.StepSeconds, -5, 5);
|
|
var requestedForce = roadLoad + 1_600 * error + 35 * _cruiseIntegral;
|
|
_throttle = ThrottleForForce(Math.Max(0, requestedForce));
|
|
break;
|
|
}
|
|
case DynamicsScenarioPhaseKind.Brake:
|
|
_throttle = 0;
|
|
_brake = Math.Clamp(
|
|
(model.VehicleMassKilograms * 3.2 + roadLoad) / model.MaximumBrakeForceNewtons,
|
|
0,
|
|
1);
|
|
break;
|
|
case DynamicsScenarioPhaseKind.ReturnIdle:
|
|
_throttle = 0;
|
|
if (_speed > 0.005)
|
|
{
|
|
_brake = Math.Clamp(
|
|
(model.VehicleMassKilograms * 2 + roadLoad) / model.MaximumBrakeForceNewtons,
|
|
0,
|
|
1);
|
|
}
|
|
break;
|
|
default:
|
|
throw new ArgumentOutOfRangeException(nameof(phase));
|
|
}
|
|
|
|
if (phase.Kind is DynamicsScenarioPhaseKind.Accelerate or DynamicsScenarioPhaseKind.Cruise)
|
|
{
|
|
var maximumSpeed = Math.Min(
|
|
phase.TargetSpeedMetersPerSecond,
|
|
MaximumCoupledSpeedMetersPerSecond());
|
|
var allowableAcceleration = (maximumSpeed - _speed) / model.StepSeconds;
|
|
var allowableForce = roadLoad + model.VehicleMassKilograms * Math.Max(0, allowableAcceleration);
|
|
_throttle = Math.Min(_throttle, ThrottleForForce(allowableForce));
|
|
}
|
|
}
|
|
|
|
private void UpdateEngineAndForces(double step, DynamicsScenarioPhase phase)
|
|
{
|
|
var model = _configuration.Model;
|
|
var previousSpeed = _speed;
|
|
var lockedRpm = _gear > 0 ? CoupledEngineSpeed(_gear) : 0;
|
|
var slipping = _engineRunning &&
|
|
_gear == 1 &&
|
|
(_speed < model.LaunchSlipEndSpeedMetersPerSecond || lockedRpm < model.IdleSpeedRpm);
|
|
if (!_engineRunning)
|
|
{
|
|
_engineSpeedRpm = 0;
|
|
}
|
|
else if (_gear == 0)
|
|
{
|
|
_engineSpeedRpm += (model.WarmIdleTargetRpm - _engineSpeedRpm) * Math.Min(1, step * 4);
|
|
}
|
|
else if (slipping)
|
|
{
|
|
_engineSpeedRpm = Math.Min(
|
|
model.MaximumEngineSpeedRpm,
|
|
model.IdleSpeedRpm + Math.Min(2_000, _throttle * 2_000));
|
|
}
|
|
else
|
|
{
|
|
_engineSpeedRpm = lockedRpm;
|
|
}
|
|
|
|
_engineTorque = _engineRunning
|
|
? TorqueAtSpeed(_engineSpeedRpm) * _throttle
|
|
: 0;
|
|
var maximumTorque = TorqueAtSpeed(Math.Max(model.IdleSpeedRpm, _engineSpeedRpm));
|
|
_engineLoad = maximumTorque <= 0 ? 0 : Math.Clamp(_engineTorque / maximumTorque, 0, 1);
|
|
var engineAngularVelocity = _engineSpeedRpm * (Math.PI * 2) / 60;
|
|
var enginePower = Math.Max(0, _engineTorque * engineAngularVelocity);
|
|
_fuelRate = _engineRunning
|
|
? model.IdleFuelRateKilogramsPerSecond + enginePower * model.FuelKilogramsPerJoule
|
|
: 0;
|
|
|
|
var couplingFactor = slipping
|
|
? Math.Clamp(_speed / model.LaunchSlipEndSpeedMetersPerSecond, 0.2, 1)
|
|
: 1;
|
|
_couplingSlip = slipping ? 1 - couplingFactor : 0;
|
|
var gearRatio = _gear > 0 ? model.ForwardGearRatios[_gear - 1] : 0;
|
|
var tractionForce = _engineRunning && _gear > 0
|
|
? _engineTorque * gearRatio * model.FinalDriveRatio * model.DrivetrainEfficiency /
|
|
model.WheelRadiusMeters * couplingFactor
|
|
: 0;
|
|
var roadLoad = RoadLoadForce(_speed);
|
|
var brakeForce = _brake * model.MaximumBrakeForceNewtons;
|
|
_acceleration = (tractionForce - roadLoad - brakeForce) / model.VehicleMassKilograms;
|
|
_speed = Math.Max(0, _speed + _acceleration * step);
|
|
if (_speed == 0 && _acceleration < 0)
|
|
{
|
|
_acceleration = (0 - previousSpeed) / step;
|
|
}
|
|
|
|
if (_engineRunning && _gear > 0 && !slipping)
|
|
{
|
|
_engineSpeedRpm = CoupledEngineSpeed(_gear);
|
|
_engineTorque = TorqueAtSpeed(_engineSpeedRpm) * _throttle;
|
|
maximumTorque = TorqueAtSpeed(Math.Max(model.IdleSpeedRpm, _engineSpeedRpm));
|
|
_engineLoad = maximumTorque <= 0 ? 0 : Math.Clamp(_engineTorque / maximumTorque, 0, 1);
|
|
engineAngularVelocity = _engineSpeedRpm * (Math.PI * 2) / 60;
|
|
enginePower = Math.Max(0, _engineTorque * engineAngularVelocity);
|
|
_fuelRate = model.IdleFuelRateKilogramsPerSecond + enginePower * model.FuelKilogramsPerJoule;
|
|
}
|
|
|
|
if (_engineRunning && _gear > 0)
|
|
{
|
|
_couplingSlip = CouplingSlipFraction(_engineSpeedRpm, CoupledEngineSpeed(_gear));
|
|
}
|
|
else
|
|
{
|
|
_couplingSlip = 0;
|
|
}
|
|
|
|
_distance += (previousSpeed + _speed) * 0.5 * step;
|
|
_fuelConsumed += _fuelRate * step;
|
|
var thermalTarget = Math.Max(model.AmbientTemperatureCelsius, model.EquilibriumEngineTemperatureCelsius) +
|
|
Math.Min(15, enginePower / 100_000 * 4);
|
|
_engineTemperature += (thermalTarget - _engineTemperature) * model.ThermalResponsePerSecond * step;
|
|
|
|
if (phase.Kind == DynamicsScenarioPhaseKind.ReturnIdle && _speed <= 0.005)
|
|
{
|
|
_speed = 0;
|
|
_acceleration = 0;
|
|
_gear = 0;
|
|
_couplingSlip = 0;
|
|
}
|
|
}
|
|
|
|
private double ThrottleForForce(double forceNewtons)
|
|
{
|
|
if (!_engineRunning || _gear <= 0 || forceNewtons <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
var model = _configuration.Model;
|
|
var torque = TorqueAtSpeed(Math.Max(_engineSpeedRpm, model.IdleSpeedRpm));
|
|
var coupling = _speed < model.LaunchSlipEndSpeedMetersPerSecond && _gear == 1
|
|
? Math.Clamp(_speed / model.LaunchSlipEndSpeedMetersPerSecond, 0.2, 1)
|
|
: 1;
|
|
var forceAtFullThrottle = torque * model.ForwardGearRatios[_gear - 1] *
|
|
model.FinalDriveRatio * model.DrivetrainEfficiency /
|
|
model.WheelRadiusMeters * coupling;
|
|
return forceAtFullThrottle <= 0
|
|
? 0
|
|
: Math.Clamp(forceNewtons / forceAtFullThrottle, 0, 1);
|
|
}
|
|
|
|
private double TorqueAtSpeed(double rpm)
|
|
{
|
|
var curve = _configuration.Model.EngineTorqueCurve;
|
|
if (rpm <= curve[0].EngineSpeedRpm)
|
|
{
|
|
return curve[0].TorqueNewtonMeters;
|
|
}
|
|
|
|
for (var index = 1; index < curve.Count; index++)
|
|
{
|
|
if (rpm <= curve[index].EngineSpeedRpm)
|
|
{
|
|
var lower = curve[index - 1];
|
|
var upper = curve[index];
|
|
var fraction = (rpm - lower.EngineSpeedRpm) /
|
|
(upper.EngineSpeedRpm - lower.EngineSpeedRpm);
|
|
return lower.TorqueNewtonMeters +
|
|
(upper.TorqueNewtonMeters - lower.TorqueNewtonMeters) * fraction;
|
|
}
|
|
}
|
|
|
|
return curve[^1].TorqueNewtonMeters;
|
|
}
|
|
|
|
private double CoupledEngineSpeed(int gear)
|
|
{
|
|
var model = _configuration.Model;
|
|
return _speed / (Math.PI * 2 * model.WheelRadiusMeters) * 60 *
|
|
model.ForwardGearRatios[gear - 1] * model.FinalDriveRatio;
|
|
}
|
|
|
|
private static double CouplingSlipFraction(double engineSpeedRpm, double wheelSpeedRpm)
|
|
{
|
|
var maximumSpeed = Math.Max(engineSpeedRpm, wheelSpeedRpm);
|
|
return maximumSpeed == 0 ? 0 : Math.Abs(engineSpeedRpm - wheelSpeedRpm) / maximumSpeed;
|
|
}
|
|
|
|
private double MaximumCoupledSpeedMetersPerSecond()
|
|
{
|
|
var model = _configuration.Model;
|
|
return model.MaximumEngineSpeedRpm * (2 * Math.PI * model.WheelRadiusMeters) / 60 /
|
|
(model.ForwardGearRatios[^1] * model.FinalDriveRatio);
|
|
}
|
|
|
|
private double RoadLoadForce(double speed) =>
|
|
_configuration.Model.VehicleMassKilograms *
|
|
_configuration.Model.GravityMetersPerSecondSquared *
|
|
_configuration.Model.RollingResistanceCoefficient +
|
|
0.5 * _configuration.Model.AirDensityKilogramsPerCubicMeter *
|
|
_configuration.Model.DragAreaSquareMeters * speed * speed;
|
|
|
|
private void AdvanceScenarioIfReady(DynamicsScenarioPhase phase)
|
|
{
|
|
var model = _configuration.Model;
|
|
var succeeded = phase.Kind switch
|
|
{
|
|
DynamicsScenarioPhaseKind.WarmIdle => _phaseElapsedSeconds >= phase.DurationSeconds,
|
|
DynamicsScenarioPhaseKind.Accelerate => _speed >= phase.TargetSpeedMetersPerSecond,
|
|
DynamicsScenarioPhaseKind.Cruise => _phaseElapsedSeconds >= phase.DurationSeconds,
|
|
DynamicsScenarioPhaseKind.Brake => _speed <= 0.1,
|
|
DynamicsScenarioPhaseKind.ReturnIdle =>
|
|
_phaseElapsedSeconds >= phase.DurationSeconds && _speed <= 0.1,
|
|
_ => false
|
|
};
|
|
|
|
if (succeeded)
|
|
{
|
|
_completedScenarioSeconds += _phaseElapsedSeconds;
|
|
_phaseIndex++;
|
|
_phaseStepCount = 0;
|
|
_phaseElapsedSeconds = 0;
|
|
_cruiseIntegral = 0;
|
|
if (_phaseIndex >= _configuration.Scenario.Count)
|
|
{
|
|
_lifecycle = DynamicsLifecycleState.Completed;
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
if (_phaseElapsedSeconds >= phase.DurationSeconds &&
|
|
phase.Kind is DynamicsScenarioPhaseKind.Accelerate or DynamicsScenarioPhaseKind.Brake or DynamicsScenarioPhaseKind.ReturnIdle)
|
|
{
|
|
Fault($"Scenario phase '{phase.Kind}' did not meet its target within {phase.DurationSeconds} virtual seconds.");
|
|
}
|
|
}
|
|
|
|
private bool AllFiniteAndInDomain()
|
|
{
|
|
var values = new[]
|
|
{
|
|
_speed, _engineSpeedRpm, _throttle, _brake, _engineLoad, _engineTorque,
|
|
_fuelRate, _fuelConsumed, _engineTemperature, _distance, _acceleration
|
|
};
|
|
return values.All(double.IsFinite) &&
|
|
_speed >= 0 &&
|
|
_engineSpeedRpm >= 0 &&
|
|
_throttle is >= 0 and <= 1 &&
|
|
_brake is >= 0 and <= 1 &&
|
|
_engineLoad is >= 0 and <= 1 &&
|
|
_engineTorque >= 0 &&
|
|
_fuelRate >= 0 &&
|
|
_fuelConsumed >= 0 &&
|
|
_distance >= 0 &&
|
|
_couplingSlip is >= 0 and <= 1 &&
|
|
_engineSpeedRpm <= _configuration.Model.MaximumEngineSpeedRpm &&
|
|
_gear >= 0 &&
|
|
_gear <= _configuration.Model.ForwardGearRatios.Count;
|
|
}
|
|
|
|
private void Fault(string message)
|
|
{
|
|
_failure = message;
|
|
_lifecycle = DynamicsLifecycleState.Faulted;
|
|
}
|
|
|
|
private void RequireLifecycle(DynamicsLifecycleState expected)
|
|
{
|
|
if (_lifecycle != expected)
|
|
{
|
|
throw new InvalidOperationException($"Dynamics lifecycle transition requires {expected}; current state is {_lifecycle}.");
|
|
}
|
|
}
|
|
|
|
private void ResetState()
|
|
{
|
|
var initial = _configuration.InitialConditions;
|
|
_stepIndex = 0;
|
|
_phaseIndex = 0;
|
|
_phaseStepCount = 0;
|
|
_phaseElapsedSeconds = 0;
|
|
_completedScenarioSeconds = 0;
|
|
_speed = initial.VehicleSpeedMetersPerSecond;
|
|
_gear = initial.Gear;
|
|
_engineRunning = initial.EngineRunning;
|
|
_engineSpeedRpm = initial.EngineSpeedRpm;
|
|
_engineTemperature = initial.EngineTemperatureCelsius;
|
|
_fuelConsumed = initial.FuelConsumedKilograms;
|
|
_distance = initial.DistanceMeters;
|
|
_throttle = 0;
|
|
_brake = 0;
|
|
_engineLoad = 0;
|
|
_engineTorque = 0;
|
|
_fuelRate = 0;
|
|
_acceleration = 0;
|
|
_couplingSlip = 0;
|
|
_cruiseIntegral = 0;
|
|
_shiftCount = 0;
|
|
_failure = null;
|
|
_samples.Clear();
|
|
_lifecycle = DynamicsLifecycleState.Stopped;
|
|
}
|
|
|
|
private MutableState CaptureMutableState() =>
|
|
new(
|
|
_stepIndex,
|
|
_phaseIndex,
|
|
_phaseStepCount,
|
|
_phaseElapsedSeconds,
|
|
_completedScenarioSeconds,
|
|
_speed,
|
|
_gear,
|
|
_engineRunning,
|
|
_engineSpeedRpm,
|
|
_engineTemperature,
|
|
_fuelConsumed,
|
|
_distance,
|
|
_throttle,
|
|
_brake,
|
|
_engineLoad,
|
|
_engineTorque,
|
|
_fuelRate,
|
|
_acceleration,
|
|
_couplingSlip,
|
|
_cruiseIntegral,
|
|
_shiftCount,
|
|
_lifecycle,
|
|
_failure,
|
|
_samples.Count);
|
|
|
|
private void RestoreMutableState(MutableState state)
|
|
{
|
|
_stepIndex = state.StepIndex;
|
|
_phaseIndex = state.PhaseIndex;
|
|
_phaseStepCount = state.PhaseStepCount;
|
|
_phaseElapsedSeconds = state.PhaseElapsedSeconds;
|
|
_completedScenarioSeconds = state.CompletedScenarioSeconds;
|
|
_speed = state.Speed;
|
|
_gear = state.Gear;
|
|
_engineRunning = state.EngineRunning;
|
|
_engineSpeedRpm = state.EngineSpeedRpm;
|
|
_engineTemperature = state.EngineTemperature;
|
|
_fuelConsumed = state.FuelConsumed;
|
|
_distance = state.Distance;
|
|
_throttle = state.Throttle;
|
|
_brake = state.Brake;
|
|
_engineLoad = state.EngineLoad;
|
|
_engineTorque = state.EngineTorque;
|
|
_fuelRate = state.FuelRate;
|
|
_acceleration = state.Acceleration;
|
|
_couplingSlip = state.CouplingSlip;
|
|
_cruiseIntegral = state.CruiseIntegral;
|
|
_shiftCount = state.ShiftCount;
|
|
_lifecycle = state.Lifecycle;
|
|
_failure = state.Failure;
|
|
if (_samples.Count > state.SampleCount)
|
|
{
|
|
_samples.RemoveRange(state.SampleCount, _samples.Count - state.SampleCount);
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
public interface IDynamicsPacer
|
|
{
|
|
Task DelayAsync(TimeSpan delay, CancellationToken cancellationToken);
|
|
}
|
|
|
|
internal sealed class TaskDelayDynamicsPacer : IDynamicsPacer
|
|
{
|
|
public async Task DelayAsync(TimeSpan delay, CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
if (delay < TimeSpan.FromMilliseconds(1))
|
|
{
|
|
await Task.Yield();
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
return;
|
|
}
|
|
|
|
await Task.Delay(delay, cancellationToken).ConfigureAwait(false);
|
|
}
|
|
}
|
|
|
|
internal sealed class DynamicsRunControl : IDynamicsControl
|
|
{
|
|
private readonly DynamicsEngine _engine;
|
|
private readonly IDynamicsPacer _pacer;
|
|
private readonly SemaphoreSlim _gate = new(1, 1);
|
|
private readonly CancellationTokenSource _lifetime = new();
|
|
private double _pacingFactor = 1;
|
|
private CancellationTokenSource? _runnerCancellation;
|
|
private Task<DynamicsRunStatus>? _runnerTask;
|
|
private bool _disposed;
|
|
|
|
public DynamicsRunControl(DynamicsEngine engine, IDynamicsPacer? pacer = null)
|
|
{
|
|
_engine = engine ?? throw new ArgumentNullException(nameof(engine));
|
|
_pacer = pacer ?? new TaskDelayDynamicsPacer();
|
|
}
|
|
|
|
public DynamicsRunStatus Status
|
|
{
|
|
get
|
|
{
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
return _engine.Status;
|
|
}
|
|
}
|
|
}
|
|
public double PacingFactor => Volatile.Read(ref _pacingFactor);
|
|
|
|
public Task<DynamicsRunStatus> StartAsync(CancellationToken cancellationToken = default) =>
|
|
StartOrResumeAsync(resume: false, cancellationToken);
|
|
|
|
public Task<DynamicsRunStatus> PauseAsync(CancellationToken cancellationToken = default) =>
|
|
StopRunnerAsync(pause: true, reset: false, cancellationToken);
|
|
|
|
public Task<DynamicsRunStatus> ResumeAsync(CancellationToken cancellationToken = default) =>
|
|
StartOrResumeAsync(resume: true, cancellationToken);
|
|
|
|
public Task<DynamicsRunStatus> StopAsync(CancellationToken cancellationToken = default) =>
|
|
StopRunnerAsync(pause: false, reset: false, cancellationToken);
|
|
|
|
public Task<DynamicsRunStatus> ResetAsync(CancellationToken cancellationToken = default) =>
|
|
StopRunnerAsync(pause: false, reset: true, cancellationToken);
|
|
|
|
private async Task<DynamicsRunStatus> StartOrResumeAsync(bool resume, CancellationToken cancellationToken)
|
|
{
|
|
await _gate.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
EnsureOpen();
|
|
EnsureNoActiveAdvancementOwner();
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (resume)
|
|
{
|
|
_engine.Resume();
|
|
}
|
|
else
|
|
{
|
|
_engine.Start();
|
|
}
|
|
}
|
|
|
|
_ = StartRunnerLocked(CancellationToken.None);
|
|
return Status;
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
}
|
|
|
|
private async Task<DynamicsRunStatus> StopRunnerAsync(
|
|
bool pause,
|
|
bool reset,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
Task<DynamicsRunStatus>? runner;
|
|
await _gate.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
EnsureOpen();
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (reset)
|
|
{
|
|
_engine.Reset();
|
|
}
|
|
else if (pause)
|
|
{
|
|
_engine.Pause();
|
|
}
|
|
else
|
|
{
|
|
_engine.Stop();
|
|
}
|
|
}
|
|
|
|
runner = CancelRunnerLocked();
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
|
|
if (runner is not null)
|
|
{
|
|
await runner.ConfigureAwait(false);
|
|
}
|
|
|
|
return Status;
|
|
}
|
|
|
|
public async Task<DynamicsRunStatus> StepAsync(int stepCount = 1, CancellationToken cancellationToken = default)
|
|
{
|
|
if (stepCount is < 1 or > 100_000)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(stepCount), "Explicit step count must be between 1 and 100000.");
|
|
}
|
|
|
|
await _gate.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
EnsureOpen();
|
|
if (_engine.Lifecycle is not (DynamicsLifecycleState.Stopped or DynamicsLifecycleState.Paused))
|
|
{
|
|
throw new InvalidOperationException("Explicit stepping is allowed only while stopped or paused.");
|
|
}
|
|
|
|
for (var step = 0; step < stepCount; step++)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (_engine.Lifecycle is DynamicsLifecycleState.Completed or DynamicsLifecycleState.Faulted)
|
|
{
|
|
break;
|
|
}
|
|
|
|
_engine.AdvanceOne();
|
|
}
|
|
}
|
|
|
|
return Status;
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<DynamicsRunStatus> SetPacingFactorAsync(double factor, CancellationToken cancellationToken = default)
|
|
{
|
|
if (!double.IsFinite(factor) || factor is < 0.25 or > 1_000)
|
|
{
|
|
throw new ArgumentOutOfRangeException(nameof(factor), "Pacing factor must be finite and between 0.25 and 1000.");
|
|
}
|
|
|
|
await _gate.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
EnsureOpen();
|
|
Volatile.Write(ref _pacingFactor, factor);
|
|
return Status;
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
}
|
|
|
|
public async Task<DynamicsRunStatus> RunScenarioAsync(CancellationToken cancellationToken = default)
|
|
{
|
|
Task<DynamicsRunStatus> runner;
|
|
await _gate.WaitAsync(cancellationToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
EnsureOpen();
|
|
EnsureNoActiveAdvancementOwner();
|
|
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (_engine.Lifecycle == DynamicsLifecycleState.Stopped)
|
|
{
|
|
_engine.Start();
|
|
}
|
|
else if (_engine.Lifecycle == DynamicsLifecycleState.Paused)
|
|
{
|
|
_engine.Resume();
|
|
}
|
|
else if (_engine.Lifecycle != DynamicsLifecycleState.Running)
|
|
{
|
|
throw new InvalidOperationException($"A scenario cannot run from lifecycle state {_engine.Lifecycle}.");
|
|
}
|
|
}
|
|
|
|
runner = StartRunnerLocked(cancellationToken);
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
|
|
return await runner.ConfigureAwait(false);
|
|
}
|
|
|
|
private Task<DynamicsRunStatus> StartRunnerLocked(CancellationToken callerToken)
|
|
{
|
|
EnsureNoActiveAdvancementOwner();
|
|
|
|
var source = CancellationTokenSource.CreateLinkedTokenSource(_lifetime.Token, callerToken);
|
|
_runnerCancellation = source;
|
|
_runnerTask = RunLoopAsync(source, callerToken);
|
|
return _runnerTask;
|
|
}
|
|
|
|
private void EnsureNoActiveAdvancementOwner()
|
|
{
|
|
if (_runnerTask is { IsCompleted: false })
|
|
{
|
|
throw new InvalidOperationException(
|
|
"A dynamics advancement owner is still active or settling; retry after its cleanup completes.");
|
|
}
|
|
}
|
|
|
|
private async Task<DynamicsRunStatus> RunLoopAsync(
|
|
CancellationTokenSource source,
|
|
CancellationToken callerToken)
|
|
{
|
|
var linkedToken = source.Token;
|
|
try
|
|
{
|
|
while (true)
|
|
{
|
|
linkedToken.ThrowIfCancellationRequested();
|
|
TimeSpan pacingDelay;
|
|
await _gate.WaitAsync(linkedToken).ConfigureAwait(false);
|
|
try
|
|
{
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (_engine.Lifecycle == DynamicsLifecycleState.Running)
|
|
{
|
|
_engine.AdvanceOne();
|
|
if (_engine.Lifecycle != DynamicsLifecycleState.Running)
|
|
{
|
|
return Status;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
return Status;
|
|
}
|
|
}
|
|
|
|
pacingDelay = TimeSpan.FromSeconds(
|
|
_engine.Configuration.Model.StepSeconds / PacingFactor);
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
|
|
await _pacer.DelayAsync(pacingDelay, linkedToken).ConfigureAwait(false);
|
|
await Task.Yield();
|
|
}
|
|
}
|
|
catch (OperationCanceledException) when (_lifetime.IsCancellationRequested)
|
|
{
|
|
return Status;
|
|
}
|
|
catch (OperationCanceledException) when (callerToken.IsCancellationRequested)
|
|
{
|
|
await PauseIfRunningAsync().ConfigureAwait(false);
|
|
throw;
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
return Status;
|
|
}
|
|
finally
|
|
{
|
|
await _gate.WaitAsync().ConfigureAwait(false);
|
|
try
|
|
{
|
|
if (ReferenceEquals(_runnerCancellation, source))
|
|
{
|
|
_runnerCancellation = null;
|
|
_runnerTask = null;
|
|
source.Dispose();
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
}
|
|
}
|
|
|
|
public async ValueTask DisposeAsync()
|
|
{
|
|
Task<DynamicsRunStatus>? runner;
|
|
await _gate.WaitAsync().ConfigureAwait(false);
|
|
try
|
|
{
|
|
if (_disposed)
|
|
{
|
|
return;
|
|
}
|
|
|
|
_disposed = true;
|
|
_lifetime.Cancel();
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
_engine.Close();
|
|
}
|
|
runner = CancelRunnerLocked();
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
|
|
if (runner is not null)
|
|
{
|
|
await runner.ConfigureAwait(false);
|
|
}
|
|
|
|
_lifetime.Dispose();
|
|
}
|
|
|
|
private async Task PauseIfRunningAsync()
|
|
{
|
|
await _gate.WaitAsync().ConfigureAwait(false);
|
|
try
|
|
{
|
|
lock (_engine.SyncRoot)
|
|
{
|
|
if (_engine.Lifecycle == DynamicsLifecycleState.Running)
|
|
{
|
|
_engine.Pause();
|
|
}
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_gate.Release();
|
|
}
|
|
}
|
|
|
|
private Task<DynamicsRunStatus>? CancelRunnerLocked()
|
|
{
|
|
var runner = _runnerTask;
|
|
_runnerCancellation?.Cancel();
|
|
return runner;
|
|
}
|
|
|
|
private void EnsureOpen()
|
|
{
|
|
if (_disposed)
|
|
{
|
|
throw new ObjectDisposedException(nameof(DynamicsRunControl));
|
|
}
|
|
}
|
|
|
|
}
|