Added functions: costToCome, findNeighbors