module Pathfinding = struct

- exception No_path_found

- let build_path maze current visited =

- let rec loop current nodes path = match nodes with

- if Maze.get maze node <> '@' && cost = (snd current) - 1

- && Maze.distance node (fst current) = 1

- then loop (node, cost) lst (node::path)

- else loop current lst path

- loop current (List.rev visited) []

+ exception No_path_found

- let find_path maze (goal:int*int) ants =

- let root = (goal, 0) in

- let rec loop queue visited =

- if queue = [] then raise No_path_found;

- let current = List.hd queue and queue = List.tl queue in

- let tile = Maze.get maze (fst current) in

+ type visited = {coords:int*int; cost:int}

+ module VisitedSet = Set.Make(struct

+ let cost_diff = b.cost - a.cost in

+ if cost_diff <> 0 then cost_diff

+ else Pervasives.compare a.coords b.coords

+ let build_path maze current visited =

+ let rec loop current nodes path =

+ if VisitedSet.is_empty nodes then path

+ let node = VisitedSet.min_elt nodes in

+ let nodes = VisitedSet.remove node nodes in

+ if Maze.get maze node.coords <> '@' && node.cost = current.cost - 1

+ && Maze.distance node.coords current.coords = 1

+ then loop node nodes (node.coords :: path)

+ else loop current nodes path

+ loop current visited []

+ let find_path maze goal ants =

+ let root = {coords=goal; cost=0} in

+ let queue = Queue.create () in

+ if Queue.is_empty queue then raise No_path_found;

+ let current = Queue.take queue in

+ let tile = Maze.get maze current.coords in

- | '@' -> build_path maze current visited

- | '#' -> loop queue visited

- let incr_cost node = node, (snd current) + 1 in

- let not_visited node = not (List.mem node visited) in

- let neighbors = List.map incr_cost (Maze.neighbor_nodes maze (fst current)) in

+ | '@' -> build_path maze current visited

+ let incr_cost coords = {coords=coords; cost=(succ current.cost)} in

+ let not_visited node = not (VisitedSet.mem node visited) in

+ let neighbors = List.map incr_cost (Maze.neighbor_nodes maze current.coords) in

let new_neighbors = List.filter not_visited neighbors in

- loop (queue @ new_neighbors) (visited @ new_neighbors)

+ let new_visited = List.fold_left (fun set elt -> VisitedSet.add elt set) visited new_neighbors in

+ List.iter (fun elt -> Queue.add elt queue) new_neighbors;

+ loop (VisitedSet.singleton root)

"@ # ######### #####\n" ^

- "## #########~~@~~## @ ##\n" ^

+ "## ######### ## @ ##\n" ^

"# ######### ## ######### ##\n" ^

"# ########### ## ### ##### ##\n" ^

"############## ##### ## ####\n" ^

+ "############## X #####"

let maze, goal, ants = Maze.Parser.from_string sample in

let before = Unix.gettimeofday () in

let path = Pathfinding.find_path maze goal ants in

ignore (Pathfinding.find_path maze goal ants)

let after = Unix.gettimeofday () in

- Maze.walk maze ~~(List.rev ~~path~~)~~;

Printf.printf "%d steps\n" (List.length path);

let time = (after -. before) *. 1000. in