private void GoToStation(int stationId, TrainStatus trainStatus)
{
// find destination station
var station = _network.GetStationById(stationId);
_log.Feed(_infoPin, $"Planning route to station {station.Name}");
// FIXME: wrong! correct to set this _only_ after arrival confirmation!
_lastStationGoneTo = stationId;
// FIXME: at the moment, this will only try the first stop of the station
foreach (var stop in station.Stops)
{
int destinationSegmentId = stop.SegmentId;
float destinationT = stop.T;
var result = _routePlanner.PlanRoute(trainStatus.SegmentId, trainStatus.T, trainStatus.Dir, destinationSegmentId, destinationT);
if (result != null)
{
var currentSegmentLength = _network.GetSegmentById(trainStatus.SegmentId).GetLength();
_currentPlanForStationId = stationId;
_currentPlanCommand = new TractionControllerCommand {
segmentsToFollow = new (int, SegmentEndpoint, float, float)[1 + result.route.Length]
private void GoToNearestStation(TrainStatus trainStatus, int?excludedStationId)
{
var(segmentId, t, dir) = (trainStatus.SegmentId, trainStatus.T, trainStatus.Dir);
var nearest = _network.FindNearestStationAlongTrack(segmentId, t, dir, excludedStationId, false);
if (nearest != null)
{
var(station, stop, distance, plan) = nearest.Value;
// Very rough ETA -- 30 seconds + distance over max speed
// var estimatedSecondsToGoal = 30 + distance / maxVelocity;
// _eta = DateTime.Now + TimeSpan.FromSeconds(estimatedSecondsToGoal); // FIXME: sim time instead of real time
// _planStation = station;
// _plan = plan;
_log.Feed(_infoPin, $"Set goal: station {station.Name}, {distance:F0} m away"); // , ETA {_eta:HH:mm:ss}
GoToStation(station.Id, trainStatus);
}
else
{
_log.Feed(_infoPin, $"GOTO_NEAREST_STATION: Cannot find any station; segmentId={segmentId} t={t} dir={dir}");
_state = State.NO_PATH;
}
}
// WaypointControllerCommand is NOT idempotent, because we want to for example recognize two separate GOTO_NEAREST_STATION commands
// Tried before: idempotent commands, nested switches... it was a mess
public TractionControllerCommand?Update(DateTime simTime, WaypointControllerCommand?maybeCommand, TrainStatus trainStatus, TractionController.State tcState)
{
// Process any commands
if (maybeCommand.HasValue)
{
var command = maybeCommand.Value;
switch (command.mode)
{
case Mode.STOP:
Stop();
break;
case Mode.GOTO_STATION:
GoToStation(command.gotoStationId.Value, trainStatus);
break;
case Mode.GOTO_NEAREST_STATION:
GoToNearestStation(trainStatus, _lastStationGoneTo);
break;
}
}
// State update
switch (_state)
{
case State.STOPPING:
// TODO: check if finished stopping -> go to state STOPPED
break;
case State.EN_ROUTE:
if (tcState == TractionController.State.IN_DESTINATION)
{
// (This assumes that the current plan goes all the way to destination)
_state = State.ARRIVED;
_lastStationArrivedAt = _currentPlanForStationId;
}
break;
}
_log.Feed(_statePin, _state.ToString());
// Output
switch (_state)
{
case State.NO_PATH:
case State.STOPPED:
case State.STOPPING:
case State.PLANNING:
case State.ARRIVED:
return(TractionControllerCommand.STOP);
case State.EN_ROUTE:
return(_currentPlanCommand.Value);
default:
throw new InvalidOperationException();
}
}
