More improvements to starting pos/angle heuristic

Code is cleaner, better