AutoForge/src/AutoForge.Simulation/DynamicsDiagnosticProvider.cs

303 lines
14 KiB
C#

using System.Collections.Concurrent;
using System.Text.Json;
using AutoForge.Core.ExecutionTargets;
using AutoForge.Core.Simulation;
using AutoForge.Core.Vehicles;
using AutoForge.Diagnostics;
namespace AutoForge.Simulation;
public sealed class DynamicsDiagnosticTransportFactory : IDynamicsDiagnosticTransportFactory
{
public const string ProviderIdentifier = "autoforge.simulation.dynamics";
private readonly Dictionary<(string ProfileIdentifier, int Revision), SimulationDefinition> _definitions;
private readonly IDynamicsPacer _pacer;
private readonly ConcurrentDictionary<string, DynamicsDiagnosticTransport> _active =
new(StringComparer.Ordinal);
public DynamicsDiagnosticTransportFactory(IEnumerable<SimulationDefinition> definitions, IDynamicsPacer? pacer = null)
{
ArgumentNullException.ThrowIfNull(definitions);
_pacer = pacer ?? new TaskDelayDynamicsPacer();
_definitions = definitions.ToDictionary(
definition => (definition.ProfileIdentifier.ToUpperInvariant(), definition.ProfileRevision));
Descriptor = new DiagnosticProviderDescriptor(
new ExecutionTargetDescriptor(ProviderIdentifier, ExecutionTargetKind.Simulation),
"Synthetic fixed-step dynamics provider");
}
public DiagnosticProviderDescriptor Descriptor { get; }
public IDiagnosticTransport OpenTransport(VehicleProfile profile, DiagnosticSessionIdentity identity) =>
throw new InvalidOperationException("Dynamics provider sessions must be explicitly opened with a dynamics configuration.");
public DynamicsSessionBinding OpenDynamicsTransport(
VehicleProfile profile,
DiagnosticSessionIdentity identity,
DynamicsConfiguration configuration)
{
ArgumentNullException.ThrowIfNull(profile);
ArgumentNullException.ThrowIfNull(identity);
ArgumentNullException.ThrowIfNull(configuration);
if (identity.Target.Kind != ExecutionTargetKind.Simulation ||
!StringComparer.OrdinalIgnoreCase.Equals(identity.Target.ProviderIdentifier, ProviderIdentifier) ||
!StringComparer.OrdinalIgnoreCase.Equals(identity.ProfileIdentifier, profile.ProfileIdentifier) ||
identity.ProfileRevision != profile.Revision ||
!StringComparer.OrdinalIgnoreCase.Equals(identity.VehicleIdentifier, profile.Vehicle.VehicleIdentifier) ||
!StringComparer.Ordinal.Equals(identity.DynamicsConfigurationIdentifier, configuration.ConfigurationIdentifier) ||
identity.DynamicsRunIdentifier is null)
{
throw new ArgumentException("Dynamics session identity does not bind to the exact selected profile, provider and configuration.", nameof(identity));
}
configuration.ValidateAgainst(profile);
if (!_definitions.TryGetValue((profile.ProfileIdentifier.ToUpperInvariant(), profile.Revision), out var definition))
{
throw new DiagnosticProviderException(
DiagnosticProviderFailure.ConfigurationNotFound,
$"No dynamics endpoint mapping exists for profile '{profile.ProfileIdentifier}' revision {profile.Revision}.");
}
definition.ValidateAgainst(profile);
var engineModule = definition.Modules.FirstOrDefault(module =>
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, configuration.EngineModuleIdentifier));
var transmissionModule = definition.Modules.FirstOrDefault(module =>
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, configuration.TransmissionModuleIdentifier));
if (engineModule is null || transmissionModule is null)
{
throw new ArgumentException("The selected configuration modules must have explicit diagnostic endpoint mappings.", nameof(configuration));
}
var engine = new DynamicsEngine(configuration, identity.DynamicsRunIdentifier);
var control = new DynamicsRunControl(engine, _pacer);
var transport = new DynamicsDiagnosticTransport(
profile,
identity,
configuration,
engine,
definition.Modules
.Where(module => StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, engineModule.ModuleIdentifier) ||
StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, transmissionModule.ModuleIdentifier))
.ToArray(),
closed => _active.TryRemove(closed, out _));
if (!_active.TryAdd(identity.SessionIdentifier, transport))
{
_ = control.DisposeAsync();
throw new DiagnosticProviderException(
DiagnosticProviderFailure.SessionConflict,
$"Dynamics session '{identity.SessionIdentifier}' is already active.");
}
return new DynamicsSessionBinding(transport, control);
}
}
internal sealed class DynamicsDiagnosticTransport : IDiagnosticTransport
{
private readonly VehicleProfile _profile;
private readonly DiagnosticSessionIdentity _identity;
private readonly DynamicsConfiguration _configuration;
private readonly DynamicsEngine _engine;
private readonly IReadOnlyDictionary<string, VirtualModuleDefinition> _modules;
private readonly Action<string> _onClosed;
private int _closed;
public DynamicsDiagnosticTransport(
VehicleProfile profile,
DiagnosticSessionIdentity identity,
DynamicsConfiguration configuration,
DynamicsEngine engine,
IReadOnlyList<VirtualModuleDefinition> modules,
Action<string> onClosed)
{
_profile = profile;
_identity = identity;
_configuration = configuration;
_engine = engine;
_modules = modules.ToDictionary(module => module.Address, StringComparer.OrdinalIgnoreCase);
_onClosed = onClosed;
ExpectedEndpoints = Array.AsReadOnly(modules
.Select(module => new DiagnosticEndpoint(module.ModuleIdentifier, module.Address))
.ToArray());
}
public IReadOnlyList<DiagnosticEndpoint> ExpectedEndpoints { get; }
public ValueTask<TransportReply> SendAsync(ReadOnlyMemory<byte> payload, CancellationToken cancellationToken)
{
cancellationToken.ThrowIfCancellationRequested();
if (Volatile.Read(ref _closed) != 0)
{
return ValueTask.FromResult(new TransportReply(
TransportOutcome.Cancelled,
ReadOnlyMemory<byte>.Empty,
0,
"Dynamics diagnostic transport is closed."));
}
if (!SyntheticProtocolV1.TryDecodeRequest(payload, out var request) || request is null)
{
return ValueTask.FromResult(new TransportReply(
TransportOutcome.NoResponse,
ReadOnlyMemory<byte>.Empty,
0,
"Request frame was malformed."));
}
if (request.Version != 2)
{
return Reply(request, false, "UNSUPPORTED_VERSION", "Dynamics protocol version 2 is required.");
}
if (!StringComparer.Ordinal.Equals(request.SessionIdentifier, _identity.SessionIdentifier) ||
!StringComparer.Ordinal.Equals(request.ConfigurationIdentifier, _identity.ConfigurationIdentifier) ||
request.ProfileRevision != _identity.ProfileRevision ||
!StringComparer.Ordinal.Equals(request.ProfileIdentifier, _identity.ProfileIdentifier))
{
return Reply(request, false, "SESSION_MISMATCH", "Request session/profile identity is not bound to this dynamics transport.");
}
if (!_modules.TryGetValue(request.TargetAddress, out var module))
{
return Reply(request, false, "UNKNOWN_ADDRESS", "No dynamics endpoint is mapped to this address.");
}
if (!module.ImplementedServices.Contains(request.Service, StringComparer.Ordinal))
{
return Reply(request, false, "UNSUPPORTED_SERVICE", $"Service '{request.Service}' is not implemented by this virtual endpoint.");
}
cancellationToken.ThrowIfCancellationRequested();
object data;
try
{
data = request.Service switch
{
DiagnosticServices.Probe => new
{
moduleIdentifier = module.ModuleIdentifier,
implementedServices = module.ImplementedServices
},
DiagnosticServices.ReadIdentity => IdentityData(module),
DiagnosticServices.ReadTroubleCodes => new { moduleIdentifier = module.ModuleIdentifier, codes = Array.Empty<object>() },
DiagnosticServices.ClearTroubleCodes => new { moduleIdentifier = module.ModuleIdentifier, clearedCount = 0 },
DiagnosticServices.ReadSignals => ReadDynamicSignals(module, request),
_ => throw new DiagnosticProtocolException("UNSUPPORTED_SERVICE", $"Service '{request.Service}' is not defined by dynamics protocol v2.")
};
}
catch (DiagnosticProtocolException exception)
{
return Reply(request, false, exception.FailureCode, exception.Message);
}
return ValueTask.FromResult(new TransportReply(
TransportOutcome.Response,
SyntheticProtocolV2.EncodePositiveResponse(request, data),
1));
}
public ValueTask DisposeAsync()
{
if (Interlocked.Exchange(ref _closed, 1) == 0)
{
_onClosed(_identity.SessionIdentifier);
}
return ValueTask.CompletedTask;
}
private object IdentityData(VirtualModuleDefinition module)
{
var identity = module.Identity;
var profileModule = _profile.GetModule(module.ModuleIdentifier);
if (!StringComparer.OrdinalIgnoreCase.Equals(identity.Role, profileModule.Role) ||
!StringComparer.OrdinalIgnoreCase.Equals(identity.Name, profileModule.Name) ||
!StringComparer.OrdinalIgnoreCase.Equals(identity.CalibrationIdentifier, profileModule.CalibrationIdentifier))
{
throw new DiagnosticProtocolException("INVALID_MAPPING", "Dynamics endpoint identity does not match the selected profile.");
}
return new
{
moduleIdentifier = module.ModuleIdentifier,
role = identity.Role,
name = identity.Name,
hardwareIdentifier = identity.HardwareIdentifier,
softwareIdentifier = identity.SoftwareIdentifier,
firmwareIdentifier = identity.FirmwareIdentifier,
calibrationIdentifier = identity.CalibrationIdentifier
};
}
private object ReadDynamicSignals(VirtualModuleDefinition module, SyntheticDiagnosticRequest request)
{
lock (_engine.SyncRoot)
{
var status = _engine.Status;
if (status.State == DynamicsLifecycleState.Faulted)
{
throw new DiagnosticProtocolException(
"DYNAMICS_RUN_FAULTED",
status.Failure ?? "Dynamics run failed; telemetry is unavailable.");
}
var state = _engine.ReadTelemetry(module.ModuleIdentifier);
var signals = StringComparer.OrdinalIgnoreCase.Equals(module.ModuleIdentifier, _configuration.EngineModuleIdentifier)
? EngineSignals(state)
: TransmissionSignals(state);
return new
{
moduleIdentifier = module.ModuleIdentifier,
configurationIdentifier = _configuration.ConfigurationIdentifier,
runIdentifier = _identity.DynamicsRunIdentifier,
modelStepIndex = state.StepIndex,
modelTimeSeconds = state.ModelTimeSeconds,
origin = "SYNTHETIC_DYNAMIC",
captureLogicalTimeTicks = request.LogicalTimeTicks,
signals
};
}
}
private static object[] EngineSignals(DynamicsTelemetryState state) =>
[
Signal("engine.running", state.EngineRunning, "boolean"),
Signal("engine.speed", state.EngineSpeedRpm, "rpm"),
Signal("vehicle.speed", state.VehicleSpeedMetersPerSecond, "m/s"),
Signal("engine.throttle", state.Throttle, "ratio"),
Signal("engine.load", state.EngineLoad, "ratio"),
Signal("engine.torque", state.EngineTorqueNewtonMeters, "N*m"),
Signal("engine.fuel-rate", state.FuelRateKilogramsPerSecond, "kg/s"),
Signal("engine.fuel-consumed", state.FuelConsumedKilograms, "kg"),
Signal("engine.temperature", state.EngineTemperatureCelsius, "degC"),
Signal("vehicle.distance", state.DistanceMeters, "m"),
Signal("vehicle.acceleration", state.AccelerationMetersPerSecondSquared, "m/s^2"),
Signal("driver.brake", state.Brake, "ratio")
];
private static object[] TransmissionSignals(DynamicsTelemetryState state) =>
[
Signal("transmission.gear", state.Gear, "gear"),
Signal("transmission.shift-count", state.ShiftCount, "count"),
Signal("transmission.coupling-slip", state.CouplingSlipFraction, "ratio"),
Signal("vehicle.speed", state.VehicleSpeedMetersPerSecond, "m/s"),
Signal("engine.speed", state.EngineSpeedRpm, "rpm")
];
private static object Signal(string identifier, object value, string unit) =>
new { signalIdentifier = identifier, value, unit, quality = "Good" };
private static ValueTask<TransportReply> Reply(
SyntheticDiagnosticRequest request,
bool success,
string code,
string message) =>
ValueTask.FromResult(new TransportReply(
TransportOutcome.Response,
success
? SyntheticProtocolV2.EncodePositiveResponse(request, new { })
: SyntheticProtocolV2.EncodeNegativeResponse(request, code, message),
1));
}