Implemented wavefront algorithm for basic grid, no obstacle