Fix some bugs with movement and clean the code