let split separator s =
  let list = ref [] in
  let start = ref 0 in
  let () = try
      while true do
        let index = String.index_from s !start separator in
        list := (String.sub s !start (index - !start)) :: !list;
        start := index + 1
      done
    with Not_found -> list := (String.sub s !start ((String.length s) - !start)) :: !list
  in List.rev !list

let read_maze filename =
  let channel = open_in_bin filename in
  let size = in_channel_length channel in
  let buffer = Buffer.create size in
  Buffer.add_channel buffer channel size;
  let s = Buffer.contents buffer in
  let lines = split '\n' s in
  let height, width = List.length lines, String.length (List.hd lines) in
  let maze = Array.make_matrix width height ' ' in
  for x = 0 to width -1 do
    for y = 0 to height -1 do
      maze.(x).(y) <- (List.nth lines y).[x]
    done
  done;
  maze

let width maze = Array.length maze
let height maze = Array.length maze.(0)

let maze_get maze (x, y) = maze.(x).(y)

let maze_set maze (x, y) value = maze.(x).(y) <- value

let draw maze =
  for y = 0 to (height maze) -1 do
    for x = 0 to (width maze) -1 do
      print_char (maze_get maze (x, y))
    done;
    print_newline ()
  done

let distance (x1, y1) (x2, y2) =
  let square x = x * x in
  let sqrt_int x = int_of_float (sqrt (float_of_int x)) in
  sqrt_int (square (x1 - x2) + square (y1 - y2))

let neighbor_nodes maze (x, y) =
  let all = [x -1, y; x +1, y; x, y -1; x, y +1;] in
  let filter (x, y) = x >= 0 && y >= 0 && x < width maze && y < height maze in
  List.filter filter all

let is_passable maze coord = maze_get maze coord <> '#'

exception No_path_found

let default_hashtbl_size = 3000

let rec reconstruct_path hashtbl node path =
  if Hashtbl.mem hashtbl node then
    let path = node :: path
    in reconstruct_path hashtbl (Hashtbl.find hashtbl node) path
  else
    node :: path

let find_path maze start goal heuristic =
  let gscore = Hashtbl.create default_hashtbl_size
  and fscore = Hashtbl.create default_hashtbl_size in

  let module CoordSet = Set.Make(
    struct
      type t = int * int
      let compare a b =
        let score_a = try Hashtbl.find fscore a with Not_found -> max_int in
        let score_b = try Hashtbl.find fscore b with Not_found -> max_int in
        let diff = score_a - score_b in
        if diff = 0 then Pervasives.compare a b else diff
    end) in

  Hashtbl.add gscore start 0;
  Hashtbl.add fscore start (heuristic start goal);

  let closedset = ref []
  and openset = ref (CoordSet.singleton start)
  and camefrom = Hashtbl.create default_hashtbl_size in

  let rec loop () =
    if CoordSet.is_empty !openset then raise No_path_found
    else
      let current = CoordSet.min_elt !openset in
      if current = goal then List.rev (reconstruct_path camefrom goal [])
      else begin
        openset := CoordSet.remove current !openset;
        closedset := current :: !closedset;

        let set_scores node real_gscore =
          Hashtbl.add camefrom node current;
          Hashtbl.add gscore node real_gscore;
          Hashtbl.add fscore node (real_gscore + (heuristic node goal))
        in let neighbors = neighbor_nodes maze current in

        let explore node =
          if is_passable maze node && not (List.mem node !closedset) then
            let tentative_gscore = (Hashtbl.find gscore current) + 1 in
            if not (CoordSet.mem node !openset) then begin
              set_scores node tentative_gscore;
              openset := CoordSet.add node !openset;
            end
            else if tentative_gscore < Hashtbl.find gscore node then set_scores node tentative_gscore
        in List.iter explore neighbors;
        loop ()
      end
  in loop ()

let () =
  let maze = read_maze "maze.txt" in
  let path =
    for i = 0 to 1000 do
      ignore (find_path maze (0, 0) (78, 29) distance)
    done;
    find_path maze (0, 0) (78, 29) distance
  in
  let walk coords = maze_set maze coords '.' in
  List.iter walk path;
  draw maze;
  Printf.printf "%d steps\n" (List.length path)