private static ISummary log(IPlan plan)
{
var log = new Log();
var previousState = plan.Trajectory.First().State;
var distance = 0.0;
foreach (var trajectory in plan.Trajectory)
{
var state = trajectory.State;
var action = trajectory.Action;
log.SimulationTimeChanged(trajectory.Time);
if (action != null)
{
log.ActionSelected(action);
}
log.StateUpdated(state);
distance += Distance.Between(previousState.Position, state.Position);
previousState = state;
}
return(new SimulationSummary(plan.TimeToGoal, Result.Suceeded, log.History, distance));
}