Removed complex heading fix code. Added a few recovery states, and emergency defense