Java generic implementation of A* (A Star)