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
  let () = Buffer.add_channel buffer channel size in
  let s = Buffer.contents buffer in
  let lines = split '\n' s in
  let height, width = List.length lines, String.length (List.nth lines 0) 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 coord =
  let x, y = coord in maze.(x).(y)

let maze_set maze coord value =
  let x, y = coord in 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 coord =
  let x, y = coord in
  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


module CoordTable = Hashtbl.Make(
  struct
    type t = int * int
    let equal a b = a = b
    let hash a = (fst a) * 64 + (snd a)
  end)

let default_hashtbl_size = 3000

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

let find_path maze start goal heuristic =
  let closedset = ref [] and openset = ref [start] and camefrom = CoordTable.create default_hashtbl_size in
  let gscore = CoordTable.create default_hashtbl_size and fscore = CoordTable.create default_hashtbl_size in
  let () = CoordTable.add gscore start 0; CoordTable.add fscore start (heuristic start goal) in
  let compare_fscore a b =
    let score_a = CoordTable.find fscore a in
    let score_b = CoordTable.find fscore b in
    score_a - score_b
  in
  let rec loop () =
    if !openset = [] then raise No_path_found
    else
      let current = List.hd !openset in
      if current = goal then List.rev (reconstruct_path camefrom goal [])
      else begin
        openset := List.tl !openset;
        closedset := current :: !closedset;

        let neighbors = neighbor_nodes maze current in
        for i = 0 to (List.length neighbors) - 1 do
          let neighbor = List.nth neighbors i in
          if is_passable maze neighbor && not (List.mem neighbor !closedset) then
            let tentative_gscore = (CoordTable.find gscore current) + 1 in
            let is_better =
              if not (List.mem neighbor !openset) then (openset := neighbor :: !openset; true)
              else tentative_gscore < CoordTable.find gscore neighbor
            in if is_better then begin
              CoordTable.add camefrom neighbor current;
              CoordTable.add gscore neighbor tentative_gscore;
              CoordTable.add fscore neighbor (tentative_gscore + (heuristic neighbor goal))
            end
        done;
        openset := List.sort (compare_fscore) !openset;
        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)