Revamped angleBetween(), funnel is working now

still need to redo heuristic in A*