Shortest path finder in a two dimensional array (Simplified Dijkstra algorithm)