Untested improvements. Should be close to being able to run a simple elevator algorithm.