Java Implementation of A Star Algorithm