From 14b080ff4d2759bc10f139b222dba87f6b04193a Mon Sep 17 00:00:00 2001
From: HU helene <helene_hu@live.com>
Date: Mon, 25 Mar 2024 17:13:15 +0100
Subject: [PATCH] #16 First astar implementation

---
 .../u_paris/gla/project/itinerary/Finder.java | 78 +++++++++++++++++--
 .../u_paris/gla/project/itinerary/Graph.java  |  4 +
 .../gla/project/itinerary/GraphNode.java      |  8 +-
 .../u_paris/gla/project/itinerary/Stop.java   | 21 +++++
 4 files changed, 102 insertions(+), 9 deletions(-)

diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java b/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java
index 23c3378..f0d67a0 100644
--- a/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java
+++ b/src/main/java/fr/u_paris/gla/project/itinerary/Finder.java
@@ -1,21 +1,83 @@
 package fr.u_paris.gla.project.itinerary;
 
-import java.util.List;
-
-public class Finder <T extends GraphNode> {
-    private final Graph<T> graph;
+import java.util.*;
 
+public class Finder <T extends GraphNode<T>> {
+    private Graph<T> graph;
     public Finder(Graph<T> graph) {
         this.graph = graph;
     }
 
-    public List<T> findPath(T from, T to) {
-        // TODO
+    public List<T> findPath(T startNode, T goalNode) {
+        PriorityQueue<T> openSet = new PriorityQueue<>(Comparator.comparingDouble(GraphNode::getF));
+        HashSet<T> closedSet = new HashSet<>();
+        HashMap<T, T> cameFrom = new HashMap<>();
+        HashMap<T, Double> gScore = new HashMap<>();
+        HashMap<T, Double> fScore = new HashMap<>();
+
+        // Initialize scores for all nodes to infinity
+        for (T node : graph.getNodes()) {
+            gScore.put(node, Double.POSITIVE_INFINITY);
+            fScore.put(node, Double.POSITIVE_INFINITY);
+        }
+
+        // The cost of going from start to start is zero
+        gScore.put(startNode, 0.0);
+        // For the first node, fScore = gScore + heuristic
+        fScore.put(startNode, startNode.getHeuristicCost(goalNode));
+        openSet.add(startNode);
+
+        while (!openSet.isEmpty()) {
+            T current = openSet.poll();
+
+            if (current.equals(goalNode)) {
+                return reconstructPath(cameFrom, current);
+            }
+
+            closedSet.add(current);
+
+            for (T neighbor : current.getNeighbors()) {
+                if (closedSet.contains(neighbor)) {
+                    continue; // Ignore the neighbor which is already evaluated.
+                }
+
+                double tentativeGScore = gScore.get(current) + current.getCost(neighbor);
+
+                if (!openSet.contains(neighbor)) {
+                    openSet.add(neighbor);
+                } else if (tentativeGScore >= gScore.get(neighbor)) {
+                    continue; // This is not a better path.
+                }
+
+                // This path is the best until now. Record it!
+                cameFrom.put(neighbor, current);
+                gScore.put(neighbor, tentativeGScore);
+                fScore.put(neighbor, tentativeGScore + neighbor.getHeuristicCost(goalNode));
+            }
+        }
+
+        // If we reach here, it means there's no path from start to goal
         return null;
     }
 
-    public List<T> findPath(double longitude, double latitude) {
-        // TODO
+    private List<T> reconstructPath(HashMap<T, T> cameFrom, T current) {
+        List<T> totalPath = new ArrayList<>();
+        totalPath.add(current);
+
+        while (cameFrom.containsKey(current)) {
+            current = cameFrom.get(current);
+            totalPath.add(0, current); // Add to the beginning of the list to maintain order
+        }
+
+        return totalPath;
+    }
+
+    //TODO:
+    public List<T> findPath(double longitude, double latitude){
         return null;
     }
+
+
 }
+
+
diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java b/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java
index 9f263e8..23f47e2 100644
--- a/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java
+++ b/src/main/java/fr/u_paris/gla/project/itinerary/Graph.java
@@ -12,4 +12,8 @@ public class Graph <T extends GraphNode> {
         this.nodes = nodes;
         this.connections = connections;
     }
+
+    public Set<T> getNodes() {
+        return nodes;
+    }
 }
diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java b/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java
index 2b9e2a6..197bcf4 100644
--- a/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java
+++ b/src/main/java/fr/u_paris/gla/project/itinerary/GraphNode.java
@@ -1,7 +1,13 @@
 package fr.u_paris.gla.project.itinerary;
 
 import java.util.List;
+import java.util.Set;
 
-public interface GraphNode{
+public interface GraphNode<T>{
     int getId();
+    double getHeuristicCost(T goalNode);
+
+    Set<T> getNeighbors();
+    double getCost(T neighbor);
+    double getF();
 }
diff --git a/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java b/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java
index 2cbd2db..2da2427 100644
--- a/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java
+++ b/src/main/java/fr/u_paris/gla/project/itinerary/Stop.java
@@ -27,6 +27,27 @@ public class Stop implements GraphNode {
         return id;
     }
 
+    @Override
+    public double getHeuristicCost(Object goalNode) {
+        return 0;
+    }
+
+    @Override
+    public Set getNeighbors() {
+        return lines;
+    }
+
+
+    @Override
+    public double getCost(Object neighbor) {
+        return 0;
+    }
+
+    @Override
+    public double getF() {
+        return 0;
+    }
+
     public String getName(){
         return name;
     }