Fill in Logic for CalcDist
Factor out old Waypoint Struct
Upgrade main pathing loop