let h = 5
let w = 5

let grid = [|
  [| 0; 0; 1; 0; 0 |];
  [| 1; 0; 1; 0; 1 |];
  [| 0; 0; 0; 0; 0 |];
  [| 0; 1; 1; 1; 0 |];
  [| 0; 0; 0; 0; 0 |]
|]

let step dist q r c nr nc =
  if nr >= 0 && nr < h && nc >= 0 && nc < w
     && grid.(nr).(nc) = 0 && dist.(nr).(nc) = -1 then begin
    dist.(nr).(nc) <- dist.(r).(c) + 1;
    Queue.push (nr * 10 + nc) q
  end

let bfs sr sc tr tc =
  let dist = Array.init h (fun _ -> Array.make w (-1)) in
  let q = Queue.create () in
  dist.(sr).(sc) <- 0;
  Queue.push (sr * 10 + sc) q;
  let go = ref true in
  while !go do
    if Queue.is_empty q then go := false
    else if dist.(tr).(tc) <> -1 then go := false
    else begin
      let rc = Queue.pop q in
      let r = rc / 10 in
      let c = rc mod 10 in
      step dist q r c (r - 1) c;
      step dist q r c (r + 1) c;
      step dist q r c r (c - 1);
      step dist q r c r (c + 1)
    end
  done;
  dist.(tr).(tc)

;;

bfs 0 0 4 4