Converted program to the new state machine system. There is a bit of room for better implementation, but this is fine for now.