(* ::Package:: *)

Print["Loading TreeSearchN.m"];

(* --------------------------------------------- *)
(* Parametros para busqueda general *)

Clear[DefineProblem, InitialState, GoalPredicate, InitFringe,
      FringeInsert, FringeRemove, FringeEmpty, Successors];
DefineProblem[opciones___]:=
   Block[{is, gp, if, fi, fr, fe, succ},
         (* se asignan los valores definidos por el usuario ,
          si alguno son omitido se asignan valores default definidos
          por la funcion Options *)
         {is, gp, if, fi, fr, fe, succ} = 
              {InitialState, GoalPredicate, InitFringe,
               FringeInsert, FringeRemove, FringeEmpty, Successors}
                  /. {opciones} /. Options[DefineProblem]; 
         P[IS[is], GP[gp], IF[if], FI[fi], FR[fr], FE[fe], S[succ]]
         ];

Options[DefineProblem] = 
    {InitialState -> {}, GoalPredicate -> True,
     InitFringe->InitStack, FringeInsert -> PushList, 
     FringeRemove -> Pop, FringeEmpty->EmptyStack,
     Successors->{}
     };
(*Funciones para obtener cada uno de los parametros de la busqueda*)
InitialState[P[IS[is_],gp_, if_, fi_, fr_, fe_, s_]]:= is;
GoalPredicate[P[is_,GP[gp_], if_, fi_, fr_, fe_, s_]]:= gp;
InitFringe[P[is_,gp_, IF[if_], fi_, fr_, fe_, s_]]:= if;
FringeInsert[P[is_,gp_, if_, FI[fi_], fr_, fe_, s_]]:= fi;
FringeRemove[P[is_,gp_, if_, fi_, FR[fr_], fe_, s_]]:= fr;
FringeEmpty[P[is_,gp_, if_, fi_, fr_, FE[fe_], s_]]:= fe;
Successors[P[is_,gp_, if_, fi_, fr_, fe_, S[s_]]]:= s;

(* --------------------------------------------- *)
(* Funciones para el manejo de nodos en el arbol de busqueda *)

Clear[MakeNode, State, Parent, Action, NDepth, G, H];

(* para crear un nodo *)
(* State -> Estado
   Parent-> Padre del estado
   Action-> Como se obtuvo el hijo (e.g. direccion del
   desplazamiento, incremento, etc.)
   NDepth-> Produndidad del nodo
   G-> Por averiguar
   H-> Por averiguar
   PSF->Accion del sucesor
*)
MakeNode[opciones___]:=
   Block[{s, p, a, d, g, h, psf},
         {s, p, a, d, g, h, psf} = 
              {State, Parent, Action, NDepth, G, H, PSF}
                  /. {opciones} /. Options[MakeNode];
         (*  Si se porporciona el padre, la profundidad se computa 
            utilizando la del padre y NDepth es ignorada
         *)
         If[p=!=Null, d=NDepth[p]+1];
         Node[S[s],P[p], A[a], ND[d], G[g], H[h], PSF[psf]] 
         (* Definicion de nodo
                                                     *)
         ];

Options[MakeNode] = 
    {State -> {}, Parent -> Null, Action -> Null,
     NDepth -> 0, G -> 0, H -> 0, PSF -> {}
     };

State[Node[S[s_], p_, a_, d_, g_, h_, psf_]]=s;
Parent[Node[s_, P[p_], a_, d_, g_, h_, psf_]]=p;
Action[Node[s_, p_, A[a_], d_, g_, h_, psf_]]=a;
NDepth[Node[s_, p_, a_, ND[d_], g_, h_, psf_]]=d;
G[Node[s_, p_, a_, d_, G[g_], h_, psf_]]=g;
H[Node[s_, p_, a_, d_, g_, H[h_], psf_]]=h;
PSF[Node[s_, p_, a_, d_, g_, h_, PSF[psf_]]]=psf;

(* --------------------------------------------- *)
(* Funciones que definen las estrategias de busqueda *)

(* DFS *)

Clear[InitStack, Push, PushList, Pop, EmptyStack];
SetAttributes[InitStack, HoldFirst];
InitStack[stack_] := stack = {};

SetAttributes[Push, HoldFirst];
Push[stack_,e_] := stack = {e,stack};

SetAttributes[PushList, HoldFirst];
PushList[stack_, {}] := stack;
PushList[stack_,l_] := 
  Block[{Traverse},
        PushList[stack, Rest[l]];
        Push[stack, l[[1]]]];

SetAttributes[Pop, HoldFirst];
Pop[stack_] := With[{tmp=stack[[1]]}, stack = stack[[2]]; tmp];

EmptyStack[stack_] := stack === {};


(* BFS *)

Clear[Newqueue, Emptyqueue, Enqueue, EnqueueList, Dequeue];

qptr = 1; qlen = 2; qfirst = 3; qlast = 4; qelem = 5;
SetAttributes[Newqueue, HoldFirst];
Newqueue[q_Symbol] := q={Unique[queue], 0, 0, 0, Null};

Emptyqueue[Q_] := Q[[qlen]] == 0;

SetAttributes[Enqueue, HoldFirst];
Enqueue[Q_, elem_] := (
	Q[[qlen]]++;
	Q[[qfirst]]++; 
	Q[[qptr]][Q[[qfirst]]] = elem;
	);

SetAttributes[EnqueueList, HoldFirst];
EnqueueList[q_, list_] := Map[Enqueue[q,#]&, list];

SetAttributes[Dequeue, HoldFirst];
Dequeue[Q_] := (
	Q[[qlen]]--;
	Q[[qlast]]++;
	Q[[qelem]] = Q[[qptr]][Q[[qlast]]]; 
	Q[[qptr]][Q[[qlast]]] =.;
	Q[[qelem]]
	);

(* A* *)
Clear[InitStartList, InsertStarElem, InsertStarList, RemoveStarList, EmptyStarList];
SetAttributes[InitStartList, HoldFirst];
InitStarList[slist_] := slist = {};

SetAttributes[InsertStarElem, HoldFirst];

InsertStarElem[slist_, node_] :=Module[{i = 1, s = Length[slist], m = Floor[Length[slist]/2],p,x=H[node]},
        While[s - i > 1 && H[slist[[m]]] =!= x ,
              If[H[slist[[m]]] > x , s = m, i = m]; 
              m = Floor[(s + i)/2];	   
        ];
        If[Length[slist]>0,
 	   p=If[m>0 && H[slist[[m]]] === x, m, If[x <= H[slist[[i]]], i, If[x >= H[slist[[s]]], s + 1, s]]],p=1];
        slist=Insert[slist, node, p]            
];

SetAttributes[InsertStarList, HoldFirst]; 

InsertStarList[slist_, nlist_]:= slist =  Module[{i},
                    For[i=1,i<=Length[nlist], i++, slist=InsertStarElem[slist,nlist[[i]]]];
                    slist
                   ];

SetAttributes[RemoveStarList, HoldFirst];

RemoveStarList[slist_]:= With[{tmp=First[slist]}, slist=Rest[slist]; tmp]

SetAttributes[EmptyStartList, HoldFirst];

EmptyStarList[slist_]:= Length[slist]== 0;


(* --------------------------------------------- *)
(* Hashing functions *)

(* Hash functions are implemented using closures *)
(* MakeHash returns two functions.  Usage: {HashIt, CheckIt} = MakeHash[]*)
Clear[MakeHash]
MakeHash[]:= 
    Module[{MyHash},
           {(MyHash[#]=1)&,
            (MyHash[#]===1)&}]

(* --------------------------------------------- *)
(* General Search *)

Clear[GeneralSearch];
GeneralSearch[Problem_, opciones___]:=
    Block[{fringe, node, s, sn, i=0, HashIt, CheckIt, deb, checked={}},
        {deb} = {Debugging}
                  /. {opciones} /. Options[GeneralSearch];
          {HashIt, CheckIt} = MakeHash[];
          If[deb, Print["*",InitFringe[Problem]]];
          InitFringe[Problem][fringe];
          If[deb, Print["fringe=", fringe]]; 
          FringeInsert[Problem]
                      [fringe, {MakeNode[State->InitialState[Problem]]}];
          If[deb, Print["fringe=",fringe]];
          While[fringe!= {}, i++;
                If[deb, Print[i,"-------"]];
                node=FringeRemove[Problem][fringe];
                If[Mod[i, 10000]==0, Print["i=",i," node =", node]];
                If[deb, Print["fringe=",fringe]];
                If[deb, Print["node=",node," Goal? -> ", 
                              GoalPredicate[Problem][node],
                              ", CheckIt ->", CheckIt[State[node]]]];
                If[!CheckIt[State[node]], (* unregistered node *)
                    Block[{},
                          HashIt[State[node]]; (* node checks in *)
                          If[GoalPredicate[Problem][node],
                              Return[node]];
                          s=Successors[Problem][node];
                          If[deb, Print["s=",s]];
                          FringeInsert[Problem][fringe, s];
                          If[deb, Print["fringe=",fringe]];
                         ], 
                    If[deb, Print["reg. ", node]]
                  ]];
                  $Failed];

Options[GeneralSearch] = {Debugging -> False};
(* --------------------------------------------- *)

