Changed the naming on some of the functions, specifically calc_heading_state --> adjust_heading_state. Added a lot of documentation via comments to the current code, and cleaned up some of the time intervals between states