Can enter in and out of modes

Now onto end conditions and minor stuff and tests