Generic A* algorithm implemented. Not yet functioning