From c49365523bcd8680a915a4ff7cc861bae93bdb81 Mon Sep 17 00:00:00 2001
From: Simon O'Shea <osheas1@wit.edu>
Date: Tue, 8 Aug 2023 18:35:53 -0400
Subject: [PATCH] broken AStar has been implemented

need to fix sorting problem because the returned order of movements does not make any sense
---
 Pathfinding Visualizer/Assets/Scripts/Main.cs | 19 +++++++++-
 .../Assets/Scripts/Pathfinder.cs              | 38 +++++++++++--------
 2 files changed, 40 insertions(+), 17 deletions(-)

diff --git a/Pathfinding Visualizer/Assets/Scripts/Main.cs b/Pathfinding Visualizer/Assets/Scripts/Main.cs
index bcf897a..0733f2b 100644
--- a/Pathfinding Visualizer/Assets/Scripts/Main.cs	
+++ b/Pathfinding Visualizer/Assets/Scripts/Main.cs	
@@ -14,7 +14,7 @@ public class Main : MonoBehaviour
 
 
     // Coordinates for pathfinding
-    public Vector2Int agent = new Vector2Int();
+    public Vector2Int agent = new Vector2Int(-1, -1);
     public List<Vector2Int> obstacles = new List<Vector2Int>();
     public List<Vector2Int> samples = new List<Vector2Int>();
 
@@ -102,6 +102,9 @@ public class Main : MonoBehaviour
     public void ResetGrid()
     {
         world.reset();
+        agent = new Vector2Int(-1, -1);
+        obstacles = new List<Vector2Int>();
+        samples = new List<Vector2Int>();
     }
    
     public void StartAlgorithm()
@@ -115,7 +118,7 @@ public class Main : MonoBehaviour
                 int index = x * world.GetHeight() + y;
                 int value = world.GetValue(x, y);
 
-                Debug.Log("x: " + x + " y: " + y + " | value: " + value);
+                //Debug.Log("x: " + x + " y: " + y + " | value: " + value);
 
                 // Add coordinate to obstacle array
                 if (value == 1)
@@ -143,6 +146,18 @@ public class Main : MonoBehaviour
                 }
             }
 
+        if (agent.x == -1 && agent.y == -1)
+        {
+            Debug.Log("NO AGENT!");
+            return;
+        }
+
+        if (samples.Count == 0)
+        {
+            Debug.Log("NO SAMPLES!");
+            return;
+        }
+        
 
         if ( true )
         {
diff --git a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs
index 884a05a..a3fe8e0 100644
--- a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs	
+++ b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs	
@@ -4,7 +4,7 @@ using UnityEngine;
 using System.Linq;
 
 
-public class Pathfinder : MonoBehaviour
+public class Pathfinder 
 {
 
     // used to store all unique states that agent has visited (holds agent and sample coords)
@@ -58,45 +58,53 @@ public class Pathfinder : MonoBehaviour
     public void StartWork()
     {
         Node initialState = new Node(null, agent, samples, '0', 0, 0);
-        Stack<Node> result = new Stack<Node>();
+        List<Node> result = new List<Node>();
 
         if (algorithm == 0)
         {
             result = StartAStar(initialState);
-
+            Debug.Log(" "); 
+            for(int i = 0; i < result.Count; i++)
+            {
+                Node n = result[i];
+                Debug.Log(n.lastMove);
+            }
         }
     }
 
-    public static Stack<Node> StartAStar(Node initialState)
+    public static List<Node> StartAStar(Node initialState)
     {
-        Stack<Node> solution = new Stack<Node>();
+        List<Node> solution = new List<Node>();
 
-        Queue<Node> open = new Queue<Node>();
+        List<Node> open = new List<Node>();
 
         
 
         //PriorityQueue<Node> open = new PriorityQueue<Node>();
 
-        open.Enqueue(initialState);
-        int cap = 10000;
+        open.Add(initialState);
+        int cap = 1000;
         while (true && cap > 0)
         {
             if (!open.Any())
                 return solution;
 
-            Node currentNode = open.Dequeue();
+            open = open.OrderBy(Node => Node.fn).ToList();
+
+            Node currentNode = open[0];
+            open.RemoveAt(0);
+
 
             // check whether we are in goal state
             if (currentNode.samples.Count() == 0)
             {
-
-                var orderedList = open.OrderBy(Node => Node.fn);
-
                 while (currentNode.parent != null)
                 {
-                    solution.Push(currentNode);
+                    Debug.Log("Agent at: " + currentNode.agent.x + ", " + currentNode.agent.y);
+                    solution.Add(currentNode);
                     currentNode = currentNode.parent;
                 }
+
                 return solution;
             }
 
@@ -112,12 +120,12 @@ public class Pathfinder : MonoBehaviour
 
             foreach (Node child in children)
             {
-                open.Enqueue(child);
+                open.Add(child);
             }
             cap--;
         }
 
-
+        return solution;
     }
     // HEURISTICS FOR ASTAR
     public static List<Node> h0(List<Node> children)