A* pathfinding now properly working... and in cpp too
pathfinding will now prefer roads. Code cleanup. A* will no longer always return shortest path