private Lst<FloorNum> StopAt(FloorNum floor, Lst<FloorNum> before, Lst<FloorNum> stops)
{
if (stops.Null()) { return before.Cons(floor).Reverse(); }
if (floor == stops.Hd()) { return before.ReverseAppend(stops); }
if (!stops.Tl().Null()) {
FloorNum X = stops.Hd(); FloorNum Y = stops.Tl().Hd(); Lst<FloorNum> afterXY = stops.Tl().Tl();
if (X < floor && floor < Y) { return before.ReverseAppend(afterXY.Cons(Y).Cons(floor).Cons(X)); }
if (Y < floor && floor < X) { return before.ReverseAppend(afterXY.Cons(Y).Cons(floor).Cons(X)); }
}
return StopAt(floor, before.Cons(stops.Hd()), stops.Tl());
}
private Time WaitTime(Dir dir, FloorNum floor, FloorNum now, Time t, Lst<FloorNum> stops)
{
if (stops.Null()) { return t + Math.Abs(floor - now); }
FloorNum next = stops.Hd();
Lst<FloorNum> after = stops.Tl();
if (floor == next) { return t + Math.Abs(floor - now); }
if (dir == Dir.Up && now <= floor && floor <= next) { return t + floor - now; }
if (dir == Dir.Down && now >= floor && floor >= next) { return t + now - floor; }
return WaitTime(dir, floor, next, t + Math.Abs(now - next) + StopTime, after);
}
private Lst<FloorNum> Insert(Dir dir, FloorNum floor, FloorNum now, Lst<FloorNum> before, Lst<FloorNum> stop)
{
if (stop.Null()) { return before.Cons(floor).Reverse(); }
FloorNum next = stop.Hd();
Lst<FloorNum> after = stop.Tl();
if (floor == next) { return before.ReverseAppend(stop); }
if (floor == now) { return before.ReverseAppend(stop); }
if (dir == Dir.Up && now < floor && floor < next) { return before.ReverseAppend(stop.Cons(floor)); }
if (dir == Dir.Down && next < floor && floor < now) { return before.ReverseAppend(stop.Cons(floor)); }
return Insert(dir, floor, next, before.Cons(next), after);
}