3 files changed, 140 insertions, 22 deletions

diff --git a/Pathfinding Visualizer/Assets/Scripts/Main.cs b/Pathfinding Visualizer/Assets/Scripts/Main.cs
index 42866f0..bcf897a 100644
--- a/Pathfinding Visualizer/Assets/Scripts/Main.cs
+++ b/Pathfinding Visualizer/Assets/Scripts/Main.cs
@@ -146,8 +146,7 @@ public class Main : MonoBehaviour
 
         if ( true )
         {
-            Pathfinder astar = new Pathfinder(world, agent, obstacles, samples, 0, 0);
-            astar.go();
+            Pathfinder path = new Pathfinder(world, agent, obstacles, samples, 0, 0);
         }
     }
 
diff --git a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs
index 4ad2070..b1b1aa1 100644
--- a/Pathfinding Visualizer/Assets/Scripts/PathNode.cs
+++ b/Pathfinding Visualizer/Assets/Scripts/PathNode.cs
@@ -4,19 +4,19 @@ using System.Collections.Generic;
 using System.Linq;
 using UnityEngine;
 
-	class Node : IComparable<Node>
+	public class Node //: IComparable<Node>
 	{
 		// helper variables just to make indexing arrays easier to read
 		public static int row = 0;
 		public static int col = 1;
 
-		Node parent;
-		Vector2Int agent;
-		List<Vector2Int> samples;
-		char lastMove; // for output/animation
-		int distanceTraveled; // for calculating f(n) value
-		double heuristic;
-		public double fn; // distance traveled + heuristic value
+		public Node parent;
+		public Vector2Int agent;
+		public List<Vector2Int> samples;
+		public char lastMove; // for output/animation
+		public int distanceTraveled; // for calculating f(n) value
+		public double heuristic;
+		public static double fn; // distance traveled + heuristic value
 		bool canSample; // for expansion 
 
 		// construct node :)
@@ -28,7 +28,7 @@ using UnityEngine;
 			this.lastMove = lastMove;
 			this.distanceTraveled = distanceTraveled;
 			this.heuristic = heuristic;
-			this.fn = distanceTraveled + heuristic;
+			fn = distanceTraveled + heuristic;
 		}
 
 		// determine valid moves and collect children to be sent back to search
@@ -101,7 +101,6 @@ using UnityEngine;
 			{
 				Node child = new Node(this, this.agent, this.samples, 'S', this.distanceTraveled + 1, this.heuristic);
 			
-				// PROBLEM?
 				child.samples.RemoveAt(onSample.y);
 
 				//SampleWorld.nodesGenerated++;
@@ -191,18 +190,18 @@ using UnityEngine;
 		}
 
 		// we use to override comparisons for priorityQueues so that our heuristic calculations are actually used
-		public int CompareTo(Node other)
+/*		public int CompareTo(Node other)
 		{
 			if (this.fn > other.fn)
-			return 1;
+				return 1;
 			else
-			return -1;
-		}
+				return -1;
+		}*/
 	
 	}
 
 	// helper class for Node
-	class NodeComparator : Comparer<Node>
+/*	class NodeComparator : Comparer<Node>
 	{
 		// used to sort nodes based on sum of distance traveled + heuristic estimation of work left
 		public override int Compare(Node a, Node b)
@@ -212,7 +211,7 @@ using UnityEngine;
 			else
 			return -1;
 		}
-	}
+	}*/
 	// helper class for Node
 	public class State
 	{
diff --git a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs
index eae7233..884a05a 100644
--- a/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs
+++ b/Pathfinding Visualizer/Assets/Scripts/Pathfinder.cs
@@ -1,6 +1,8 @@
 using System.Collections;
 using System.Collections.Generic;
 using UnityEngine;
+using System.Linq;
+
 
 public class Pathfinder : MonoBehaviour
 {
@@ -32,14 +34,17 @@ public class Pathfinder : MonoBehaviour
 
         algorithm = algo;
         heuristic = heu;
-    }
 
-    public void go()
-    {
+        printInfo();
+
+        StartWork();
 
+    }
+    public void printInfo()
+    {
         Debug.Log("Agent: " + agent[0] + "," + agent[1]);
 
-        for(int i = 0; i < obstacles.Count; i++)
+        for (int i = 0; i < obstacles.Count; i++)
         {
             Debug.Log("Obstacle " + i + ": " + obstacles[i].x + ", " + obstacles[i].y);
         }
@@ -48,6 +53,121 @@ public class Pathfinder : MonoBehaviour
         {
             Debug.Log("Sample " + i + ": " + samples[i].x + ", " + samples[i].y);
         }
+    }
+
+    public void StartWork()
+    {
+        Node initialState = new Node(null, agent, samples, '0', 0, 0);
+        Stack<Node> result = new Stack<Node>();
+
+        if (algorithm == 0)
+        {
+            result = StartAStar(initialState);
+
+        }
+    }
+
+    public static Stack<Node> StartAStar(Node initialState)
+    {
+        Stack<Node> solution = new Stack<Node>();
+
+        Queue<Node> open = new Queue<Node>();
+
+        
+
+        //PriorityQueue<Node> open = new PriorityQueue<Node>();
+
+        open.Enqueue(initialState);
+        int cap = 10000;
+        while (true && cap > 0)
+        {
+            if (!open.Any())
+                return solution;
+
+            Node currentNode = open.Dequeue();
+
+            // 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);
+                    currentNode = currentNode.parent;
+                }
+                return solution;
+            }
+
+            // if not, expand children and continue
+            List<Node> children = currentNode.expand(-1);
+
+            if (heuristic.Equals("h0"))
+                h0(children);
+           /* else if (heuristic.Equals("h1"))
+                h1(children);
+            else if (heuristic.Equals("h2"))
+                h2(children);*/
+
+            foreach (Node child in children)
+            {
+                open.Enqueue(child);
+            }
+            cap--;
+        }
+
+
+    }
+    // HEURISTICS FOR ASTAR
+    public static List<Node> h0(List<Node> children)
+    {
+        List<Node> sorted = new List<Node>();
+
+        for (int i = 0; i < children.Count(); i++)
+        {
+            // estimated moves left: zero
+            children[i].heuristic = 0;
+            sorted.Add(children[i]);
+        }
+
+        sorted = sorted.OrderBy(Node => Node.fn).ToList();
+
+        return sorted;
+    }
 
+/*    public static List<Node> h1(List<Node> children)
+    {
+        Queue<Node> sorted = new PriorityQueue<>(new NodeComparator());
+
+        for (int i = 0; i < children.size(); i++)
+        {
+            // estimated moves left: samples left
+            children.get(i).heuristic = children.get(i).samples.size();
+            sorted.add(children.get(i));
+        }
+        return children;
     }
+
+    public static Queue<Node> h2(List<Node> children)
+    {
+        Queue<Node> sorted = new PriorityQueue<>(new NodeComparator());
+
+        if (children.size() == 0)
+            return sorted;
+
+        Node child;
+
+        for (int i = 0; i < children.size(); i++)
+        {
+            // estimated moves left: 
+            // distance to nearest sample + 1 (+1 to account for sample action)
+            child = children.get(i);
+            child.heuristic = Node.distance(child.agent, child.nearestSample()) + 1;
+
+            sorted.add(child);
+        }
+
+        return sorted;
+    }*/
 }