Break out the main logic loop into a separate function.
Replace the old spaghetti code with a state machine.

Still a bit of spaghettiness, but this is a lot better, and fixed a few bugs.
Added a latex drawing of the state machine.