1 files changed, 124 insertions, 4 deletions

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;
+    }*/
 }