Algorithms: new queue based A*

 * Faster algorithm

 * Amends tests