Optimise pathfinding, add heap and comparable point