[#4] pure_pursuit code cleanup

• For the theory of pure pursuit controller and the derivation of the formula, please refer to "Integrated Mobile Robot Control" by Omead Amidi (CMU-RI-TR-90-17, Equation 3.10 on Page 21). -Rewrite a few functions so they are easier to understand. -Verified in LGsim and L5 test track