#light

//------------ Generic A* starts here --------------

type   Graph<'a,'b > =
 { H_score: 'a   ->   'b;
   Separation: 'a   ->   'a   ->   'b;
   Neighbours: 'a   ->   'a list;
   Zero: 'b;}

let rec reconstruct_path came_from node =
  match   Map.find node came_from   with
  | None   ->

      node :: []
  | value   ->
      node :: reconstruct_path came_from value;;

let rec update graph x y old_f old_g old_from g_value =
  let   key_f = Map.containsKey y old_f
  let   key_g = Map.containsKey y old_g
  let   key_from = Map.containsKey (Some y) old_from
  match   (key_f, key_g, key_from)   with
  | (  true  ,_,_)   ->   update graph x y (Map.remove y old_f) old_g old_from g_value
  | (_,  true  ,_)   ->   update graph x y old_f (Map.remove y old_g) old_from g_value
  | (_,_,  true  )   ->   update graph x y old_f old_g (Map.remove (Some y) old_from) g_value
  | _   ->
    let   new_from = Map.add (Some y) (Some x) old_from
    let   new_g = Map.add y g_value old_g
    let   new_f = Map.add y (g_value + (graph.H_score y)) old_f   // Estimated total distance from start to goal through y.
    (new_f, new_g, new_from);;

  let rec scan graph x neighbours openset closedset f g from =
   match   neighbours   with
   | []   ->   (openset, f, g, from)
   | y::n   ->
    if   Set.contains y closedset   then
      scan graph x n openset closedset f g from
    else
      let   g0 = Map.find x g
      let   trial_g = g0 + graph.Separation x y
      if   Set.contains y openset   then
          let   old_g = Map.find y g
          if   trial_g < old_g   then
           let   (new_f, new_g, new_from) = update graph x y f g from trial_g
           scan graph x n openset closedset new_f new_g new_from
          else
           scan graph x n openset closedset f g from
      else
         let   new_open = Set.add y openset
         let   (new_f, new_g, new_from) = update graph x y f g from trial_g
         scan graph x n new_open closedset new_f new_g new_from;;

let  best_step graph openlist score =
  let   choice score h best best_value =
    let   v = Map.find h score
    match   best   with
    | None   ->   ((Some h), v)
    | _   when   Some v < Some best_value   ->   ((Some h), v)
    | _   ->   (best, best_value)

  let rec   best_step4 openlist score best best_value =
    match   openlist   with
    | []   ->   best
    | h::tail   ->
        let   pair = choice score h best best_value
        match   pair   with
        | (next, next_value)   ->   best_step4 tail score next next_value

  match   openlist   with
  | []   ->   None
  | list   ->

     best_step4 list score None graph.Zero;;

let  astar graph start goal =
  let   rec  astar_step graph goal closedset openset f_score g_score came_from =
     match  Set.count openset  with
     | 0  ->
       None;
     |_  ->
       let  l = Set.toList openset
       let  pt = best_step graph l f_score

       if  (Some goal) = pt  then
         let  path = reconstruct_path came_from (Some goal)
         Some path
       else
           match  pt  with
           | None  ->  None
           | Some x  ->
              let  next_open = Set.remove x openset
              let  next_closed = Set.add x closedset
              let  neighbours = graph.Neighbours x

              let  (new_open, new_f, new_g, new_from) = scan graph x neighbours next_open next_closed f_score g_score came_from
              astar_step graph goal next_closed new_open new_f new_g new_from

  let  closedset = Set.empty              // The set of nodes already evaluated.
  let  openset = Set.add start Set.empty  // The set of tentative nodes to be evaluated

  let  f_score = Map.add start (graph.H_score start) Map.empty
  let  g_score = Map.add start graph.Zero Map.empty

  let  came_from = Map.add (Some start) None Map.empty

  astar_step graph goal closedset openset f_score g_score came_from;;

 //------------ Generic A* ends here ----------------

 // specialize for the torch-carrier problem
 // bits 0-4 represent torch, 1m, 2m, 5m, 10m
 // set bit = on the near side

 // Every possible crossing of one or two
let   swaps =
   [3;5;9;17; 7; 11;13; 19;21;25];;

let   crossing_time swap =
   match   swap   with
   | x   when   x > 16   ->   10.0
   | x   when   x > 8   ->   5.0
   | x   when   x > 4   ->   2.0
   | _   ->   1.0

let   neighbour_nodes x =
   let   equivalent_point x =
     match   x &&& 1   with
     | 1   ->   x
     | _   ->   31^^^x

   let rec   compatible x y outlist inlist =
    match   inlist   with
    | []   ->   outlist
    | swap::tail   when   (y &&& swap) = swap   ->
       let   newlist = (x^^^swap)::outlist
       compatible x y newlist tail
    | swap2::tail2   ->   compatible x y outlist tail2

   compatible x (equivalent_point x) [] swaps;;

let   dist_between x y =
   crossing_time (x^^^y);;

  // presentation form
let rec display swap =
  let decorate value =
    match   value   with
    | ""  ->   value
    | _   ->   sprintf   "+%s"   value

  let rec compound swap bit =
    sprintf   "%d%s"   (int (crossing_time swap)) (decorate (display (swap^^^bit)))

  match   swap   with
  | x   when   x > 16   ->   compound swap 16
  | x   when   x > 8   ->   compound swap 8
  | x   when   x > 4   ->   compound swap 4
  | x   when   x > 2   ->   compound swap 2
  | _   ->      ""  ;;



let rec  print_result path prev time =
  let   print_swap swap from =
   let   transition = display swap
   match   from &&& 1   with
   | 1   ->   printfn   "    %s -->"   transition
   | _   ->   printfn   "<-- %s"   transition

  match   path   with
  | []   ->   time
  | (Some h)::trace   ->
     let   swap = h^^^prev
     print_swap swap h
     let   interval = time + (crossing_time swap)
     print_result trace h interval
  | _   ->   failwith   "Incomplete route"  ;;


let   main =
  let   g = {H_score=crossing_time;
    Separation=dist_between;
    Neighbours=neighbour_nodes;
    Zero=0.0}
  match   astar g 31 0   with
  | Some((Some h)::trace)   ->

     let   time = print_result trace h 0.0
     printfn   "Time taken = %d minutes"   (int time)
  | _   ->   printfn   "No solution"  ;;

printfn  "----"  ;;