Unit test helper function works! :D
Added WrapAngle() global helper
Style fixes in Trajectory