Movement in FPS mode

And some cleaning up of the code