-- elhoim is full object organizer with editor and command interpreter.
-- Elhoim is Copyright (C) 2023 Manuel De Girardi ; 
--
--   This program is free software; you can redistribute it and/or modify
--   it under the terms of the GNU General Public License as published by
--   the Free Software Foundation; either version 2 of the License, or
--   (at your option) any later version.
--
--   This program is distributed in the hope that it will be useful,
--   but WITHOUT ANY WARRANTY; without even the implied warranty of
--   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
--   GNU General Public License for more details.
--
--   You should have received a copy of the GNU General Public License
--   along with this program; if not, write to the Free Software
--   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
--
-- Date := "2023-05-26 17:40:38"
-- Version := "0.6.6b"

with Text_Io;
package body El.Search is
   
   package body Path_Finding is
	 
      
      procedure Adjust(N : in out Node_Record; E : in Element) is
      begin
	 N.E := E;
      end Adjust;
      
      
      function Init return Node_Access is
	 New_Node : Node_Access;
      begin
	 New_Node := new Node_Record;
	 if New_Node /= null then
	    return New_Node;
	 else
	    return null;
	 end if;
      end Init;
      
      function Element_Of (Node : in Node_Record) return Element is
      begin
	 return Node.E;
      end Element_Of;
      
      
      
      function "=" (Left, Right : in Node_Record) return Boolean is
      begin
	 if (Left.Ucost = Right.Ucost) and( Left.Hcost = Right.Hcost) then
	    return True;
	 end if;
	 return False;
      end "=";
      
      
      function "<" (Left, Right : in Node_Record) return Boolean is
      begin
	 if (Left.Hcost - Left.Ucost) < ( Right.Hcost - Right.Ucost) then
	    return True;
	 end if;
	 return False;
      end "<";
      
      
      package Node_Lists is new Ada.Containers.Doubly_Linked_Lists(Node_Record, "=");
      
      package Nodes_Sorting is new Node_Lists.Generic_Sorting ("<");
      
      use Node_Lists;
      use Nodes_Sorting;
      subtype Node_List is Node_Lists.List;
      
      
      procedure Openned_Finalization (Open : out Element_List;
				   Openned : in Node_List) is
	 
	 N_Curs : Node_Lists.Cursor;
	 
      begin
	 
	 if Is_Empty(Openned) then	    
	    return;
	 end if;
	 N_Curs := Node_Lists.First(Openned);
	 for Iter in 1..Node_Lists.Length (Openned) loop
	    declare
	       Node : Node_Access := new Node_record ' (Node_Lists.Element(N_Curs));
	    begin	       
	       Element_Lists.Append (Open, Node.E);
	       exit when Iter = Node_Lists.Length (Openned);
	       N_Curs := Node_Lists.Next(N_Curs);
	    end;
	 end loop;
	 
      end Openned_Finalization;
      
      procedure Astar (Open : in Element_List;
		       Close  : out Element_List) is
	 
	 
	 
	 Openned : Node_List;	 
	 Closed : Node_List;
	 
	 Openned_Curs : Node_Lists.Cursor;
	 Close_Curs : Element_Lists.Cursor;
	 
	 Done : Boolean := False;
	 
	 Initial_Node : Node_Record;
	 Goal : Element;
	 Node : Node_Record;
      begin
	 Goal := Element_Lists.Last_Element(Open);
	 Close_Curs := Element_Lists.First(Open);
	 Adjust(Initial_Node, Element_Lists.First_Element(Open));	 
	 Node_Lists.Append(Openned, Initial_Node);
     Astar_Loop :
	 while not Is_Empty(Openned) loop
	    Nodes_Sorting.Sort(Openned);
	    declare
	       Openned_Find_Curs : Node_Lists.Cursor :=
		 Node_Lists.First(Openned);
	       Best_Node : Node_Record;
	       F : Float := 0.0;       --            successor.f = successor.g + successor.h
	    begin
	       Best_Node := Node_Lists.Element(Openned_Find_Curs);
	       Best_Node.Ucost := Uniform(Best_Node.E);
	       Best_Node.Hcost := Heuristic(Best_Node.E);
	       Node_Lists.Delete_First(Openned);
	       --Text_Io.Put_Line("New successors :");
	       declare
		  Best_Successors : El_Array := Successors(Best_Node.E);
		  
		  
		  
		  
	       begin
		  --Text_Io.Put_Line("Nb successors : " & Integer'Image(Best_Successors'Length));
		  for Succ in Best_Successors ' range loop
		     declare
			E : Element := Best_Successors(Succ); 
		     begin
			--Text_Io.Put_Line("Successor " & Integer'Image(Succ) & " : ");
			if Equal(E, Goal) then --          i) if successor is the goal, stop search
			   Adjust(Node, E);
			   Node_Lists.Append(Closed, Node);
			   exit Astar_Loop;			   
			else
			   Adjust(Node, E);
			   --          ii) else, compute both g and h for successor
			   --            successor.g = q.g + distance between 
			   --                                successor and q			   
			   Node.Ucost :=  Best_Node.ucost + uniform(Node.E);
			   
			   --            successor.h = distance from goal to 
			   --            successor (This can be done using many 
			   --Text_Io.Put_Line("             - ucost = " & float'Image(Node.Ucost));
			   Node.Hcost := Heuristic(Goal) - Heuristic(Node.E);
			   --Text_Io.Put_Line("             - hcost = " & float'Image(Node.Hcost));
			   
			   --            successor.f = successor.g + successor.h
			   F := Node.Ucost + Node.Hcost;
			   --Text_Io.Put_Line("             - F = " & float'Image(F));
			   declare
			      
			   begin
			      
			      if not Node_Lists.Is_Empty(Openned) then
				 declare
				    Open_curs : Node_Lists.Cursor := Node_Lists.First(Openned);
				    
				 begin
				    for Open in 1..Node_Lists.Length(Closed) loop
				       declare
					  Open_Node : Node_Record;
				       begin
					  Open_Node := Node_Lists.First_Element(Openned);
					  if Equal(Open_Node.E, Node.E) and then
					    (Open_Node.Ucost + Open_Node.Hcost) < F then
					     Done := True;
					  end if;
				       end;
				       exit when Open = Node_Lists.Length(Openned);
				       Open_Curs := Node_Lists.Next(Open_Curs);
				    end loop;
				 end;
				 if not Done then
				    
				    --Text_Io.Put_Line("Adding new node in closed"); --      e) push q on the closed list
				    Node_Lists.Append(Closed, Best_Node);
				    
				    if not Node_Lists.Is_Empty(Closed) then
				       declare
					  Close_curs : Node_Lists.Cursor := Node_Lists.First(Closed);
				       begin
					  for close in 1..Node_Lists.Length(Closed) loop
					     declare
						Close_Node : Node_Record := Node_Lists.Element(Close_Curs);
					     begin
						if Equal(Close_Node.E, Goal) and then
						  (Close_Node.Ucost + Close_Node.Hcost) < F then
						   Done := True;
						end if;
					     end;
					     exit when Close = Node_Lists.Length(Closed);
					     Close_Curs := Node_Lists.Next(Close_Curs);
					  end loop;
				       end;
				    end if;
				 end if;
				 if not Done then
				    
				    --Text_Io.Put_Line("Adding new node in openned");
				    
				    Node_Lists.Append(Openned, Node);
				 end if;
				 Done := False;
			      end if;
			   end;
			end if;
		     end;		     
		  end loop;
		  
		  
	       end;
	    end;
	    
	    
	 end loop Astar_Loop;
	 
	 Openned_Finalization(Close, Closed);
	 --Element_Lists.Reverse_Elements(Close);
	 
      end Astar;

      
      task body Astar_Task is
	 
	 End_Of_Program : Boolean := False;
	 End_Of_Task : Boolean := True;
	 
	 Openned : Node_List;    
	 Closed : Node_List;
	 
	 Openned_Curs : Node_Lists.Cursor;
	 
	 
	 Done : Boolean := False;
	 
	 
	 Initial_Node : Node_Record;
	 Goal : Element;
	 Node : Node_Record;
	 
      begin
	 while not End_Of_Program loop
	    loop
	       select
		  
		  accept Init (Open : in Element_List) do
		     Goal := Element_Lists.Last_Element(Open);
		     
		     Adjust(Initial_Node, Element_Lists.First_Element(Open));         
		     Node_Lists.Clear(Openned);
		     Node_Lists.Clear(Closed);
		     Node_Lists.Append(Openned, Initial_Node);
		     End_Of_Task := False;
		  end Init;
		  exit;
	       or
		  when End_Of_Task = True =>
		     accept Close_Set (Closed_Out : out Element_List; End_Of_Task : out Boolean) do
			--Text_Io.Put_Line("Astar task  : accept after search" );
			Openned_Finalization(Closed_Out, Closed);
			--Element_Lists.Reverse_Elements(Closed_Out);
			End_Of_Task := True;
		     end Close_Set;
	       or
		  accept Halt do
		     End_Of_Program := True;
		     End_Of_Task := True;
		     --Text_Io.Put_Line("Astart task  : exit process" );
		  end Halt;
		  exit;
	       end select;
	    end loop;
	    while not End_Of_Task loop
	       select
		  accept Halt do
		     End_Of_Program := True;
		  end Halt;
		  exit;
	       or
		  delay 0.05;
	      Astar_Loop :
		  while not Is_Empty(Openned) loop
		     select
			
			accept Init (Open : in Element_List) do
			   Goal := Element_Lists.Last_Element(Open);
			   
			   Adjust(Initial_Node, Element_Lists.First_Element(Open));         
			   Node_Lists.Clear(Openned);
			   Node_Lists.Clear(Closed);
			   Node_Lists.Append(Openned, Initial_Node);
			   End_Of_Task := False;
			end Init;
		     or
			accept Close_Set (Closed_Out : out Element_List; End_Of_Task : out Boolean) do
			   --Text_Io.Put_Line("Astar task  : accept durring search" );
			   Openned_Finalization(Closed_Out, Closed);
			   --Element_Lists.Reverse_Elements(Closed_Out);			   
			   End_Of_Task := False;
			end Close_Set;
		     or
			accept Halt do
			   End_Of_Program := True;
			   End_Of_Task := True;
			   Text_Io.Put_Line("Astar task  : exit process" );
			end Halt;
			exit Astar_Loop;
		     or
			delay 0.05;
		     end select;
		     Nodes_Sorting.Sort(Openned);
		     declare
			
			Best_Node : Node_Record;
			F : Float := 0.0;       --            successor.f = successor.g + successor.h
		     begin
			Best_Node := Node_Lists.First_Element(Openned);
			Best_Node.Ucost := Uniform(Best_Node.E);
			Best_Node.Hcost := Heuristic(Best_Node.E);
			Node_Lists.Delete_First(Openned);
			
			--Text_Io.New_Line;
			--Text_Io.Put_Line("New successors :" );
			declare
			   Best_Successors : El_Array := Successors(Best_Node.E);
			   
			   
			   
			   
			begin
			   --Text_Io.Put_Line("Nb successors : " & Integer'Image(Best_Successors'Length));
			   for Succ in Best_Successors ' range loop
			      
			      declare
				 E : Element := Best_Successors(Succ); 
			      begin
				 
				 if Equal(E, Goal) then --          i) if successor is the goal, stop search
				    End_Of_Task := True;
				    Adjust(Node, E);
				    Node_Lists.Append(Closed, Node);
				    exit Astar_Loop;                       
				 else
				    
				    Adjust(Node, E);
				    
				    --Text_Io.Put_Line("Adding best node in closed" );
				    
				    --          ii) else, compute both g and h for successor
				    --            successor.g = q.g + distance between 
				    --                                successor and q                      
				    Node.Ucost :=  Best_Node.ucost + uniform(Node.E);
				    
				    
				    
				    --Text_Io.Put_Line("             - ucost = " & float'Image(Node.Ucost));
				    
				    
				    --  --            successor.h = distance from goal to 
				    --  --            successor (This can be done using many 
				    Node.Hcost := Heuristic(Goal) - Heuristic(Node.E);
				    
				    --Text_Io.Put_Line("             - hcost = " & float'Image(Node.Hcost));
				    
				    
				    
				    --            successor.f = successor.g + successor.h
				    F := Node.Ucost + Node.Hcost;
				    --Text_Io.Put_Line("             - F     = " & float'Image(F));
				    --          iii) if a node with the same position as 
				    --              successor is in the OPEN list which has a 
				    --             lower f than successor, skip this successor
				    
				    
				    if not Node_Lists.Is_Empty(Openned) then
				       declare
					  Open_Curs : Node_Lists.Cursor := Node_Lists.First(Openned);
				       begin
					  for Open in 1..Node_Lists.Length(Openned) loop
					     
					     declare
						Open_Node : Node_Record := Node_Lists.Element(Open_Curs);
					     begin
						
						if Equal(Open_Node.E, Node.E) and then
						  
						  (Open_Node.Ucost + Open_Node.Hcost) < F then
						   Done := True;
						end if;
					     end;
					     
					     exit when Open = Node_Lists.Length(Openned);
					     Open_Curs := Node_Lists.Next(Open_Curs);
					  end loop;
				       end;
				    end if;
				    if not Done then
				       Node_Lists.Append(Closed, Best_Node);
						   
				       --          iV) if a node with the same position as 
				       --              successor  is in the CLOSED list which has
				       --              a lower f than successor, skip this successor
				       --              otherwise, add  the node to the open list
				       
				       if not Node_Lists.Is_Empty(Closed) then
					  declare
					     Close_Curs : Node_Lists.Cursor := Node_Lists.First(Closed);
					  begin
					     for Close in 1..Node_Lists.Length(Closed) loop
						
						declare
						   Close_Node : Node_Record := Node_Lists.Element(Close_Curs);
						begin
						   if Equal(Close_Node.E, Node.E) and then
						     (Close_Node.Ucost + Close_Node.Hcost) < F then
						      Done := True;						   
						   end if;
						end;
						
						exit when Close = Node_Lists.Length(Closed);
						Close_Curs := Node_Lists.Next(Close_Curs);
					     end loop;
					  end;
				       end if;
				    end if;
				    
				 end if;
			      end;
			      if not Done then
				       --Text_Io.Put_Line("Adding new node in openned" );
				       Node_Lists.Append(Openned, Node);
				    end if;
				    Done := False;
			   end loop;
			   
			end;	
			
			
		     end;
		  end loop Astar_Loop;
		  End_Of_Task := True;
		  --Text_Io.Put_Line("Astar : end of search." );
		  exit;
	       end select;
	    end loop;
	 end loop;
	 Text_Io.New_Line;
	 Text_Io.Put_Line("Astar task halted." );
      end Astar_Task;

      
   end Path_Finding;

end El.Search ;