From 410f1c9a6ea44f5a2c95ca35941ef4e5503a71da Mon Sep 17 00:00:00 2001
From: pfaure <pfaure@etud.insa-toulouse.fr>
Date: Tue, 28 Apr 2020 09:53:55 +0200
Subject: [PATCH] =?UTF-8?q?Version=20Fonctionnelle=20:=20A*=20impl=C3=A9me?=
 =?UTF-8?q?nt=C3=A9=20(copie=20du=20code=20de=20Dijkstra)?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 .../shortestpath/AStarAlgorithm.java          | 152 ++++++++++++++++++
 .../graphs/algorithm/shortestpath/Label.java  |  19 ++-
 .../algorithm/shortestpath/LabelStar.java     |  22 +++
 3 files changed, 191 insertions(+), 2 deletions(-)
 create mode 100644 be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java

diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
index fd172f0..3bb2992 100644
--- a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/AStarAlgorithm.java
@@ -1,9 +1,161 @@
 package org.insa.graphs.algorithm.shortestpath;
 
+import org.insa.graphs.model.*;
+
+import java.util.ArrayList;
+import java.util.Collections;
+
+import org.insa.graphs.algorithm.AbstractInputData.Mode;
+import org.insa.graphs.algorithm.AbstractSolution.Status;
+import org.insa.graphs.algorithm.utils.*;
+
 public class AStarAlgorithm extends DijkstraAlgorithm {
 
     public AStarAlgorithm(ShortestPathData data) {
         super(data);
     }
 
+    @Override
+    protected ShortestPathSolution doRun() {
+
+        BinaryHeap<Label> tas = new BinaryHeap<Label>();
+        
+        final ShortestPathData data = getInputData();
+        
+        Graph graph = data.getGraph();
+        Node origin = data.getOrigin();
+        Node dest = data.getDestination();
+        int nbNodes = graph.size();
+        
+        LabelStar[] labels = new LabelStar[nbNodes];
+        //int nbNonMarque = nbNodes;
+        int MaxSpeed = data.getMaximumSpeed();
+        if (data.getMode() == Mode.LENGTH) {
+        	MaxSpeed = 1;
+        }
+        
+        for (Node node: graph.getNodes()) {
+        	double estimation = Point.distance(node.getPoint(), dest.getPoint())/MaxSpeed;
+        	labels[node.getId()] = new LabelStar(node, estimation);
+        }
+        
+        labels[origin.getId()].actualiser(0.0, null);
+        tas.insert(labels[origin.getId()]);
+        notifyOriginProcessed(origin);
+        
+        boolean encore = true;
+        
+        while ((!labels[dest.getId()].isMarque()) && encore) {
+        	
+        	try {
+            	Label current = tas.deleteMin();
+            	
+            	int currentID = current.getSommetCourant().getId();
+            	labels[currentID].marquer();
+            	notifyNodeMarked(current.getSommetCourant());
+            	
+            	// POUR LE TEST SUR LES COUTS CROISSANTS
+            	// System.out.println(labels[currentID].getCost());
+            	
+            	//nbNonMarque--;
+            	
+            	// POUR LE TEST SUR LES SUCCESSEURS
+            	//int compteur_successeurs = 0;
+            	for (Arc succ: labels[currentID].getSommetCourant().getSuccessors()) {
+            		// POUR LE TEST SUR LES SUCCESSEURS
+            		//compteur_successeurs++;
+            		if (!data.isAllowed(succ)) {
+            			continue;
+            		}
+	            	int succID = succ.getDestination().getId();
+	            	if (!labels[succID].isMarque()) {
+	            		if (labels[succID].getCost() > (labels[currentID].getCost() + data.getCost(succ))) {
+	            			if (labels[succID].getCost() < Double.POSITIVE_INFINITY) {
+	            				tas.remove(labels[succID]);
+	            			} else {
+	            				notifyNodeReached(succ.getDestination());
+	            			}
+	            			labels[succID].actualiser(labels[currentID].getCost() + data.getCost(succ), succ);
+	            			tas.insert(labels[succID]);
+	            		}        			
+	            	}
+            	}
+            	// POUR LE TEST SUR LES SUCCESSEURS
+            	/*if (compteur_successeurs != labels[currentID].getSommetCourant().getNumberOfSuccessors()) {
+            		System.out.println("ERREUR Nombre de successeurs visités : " + compteur_successeurs + "/" + labels[currentID].getSommetCourant().getNumberOfSuccessors());
+            	}
+            	System.out.println("Nombre de successeurs visités : " + compteur_successeurs + "/" + labels[currentID].getSommetCourant().getNumberOfSuccessors());
+        		*/
+        	}
+        	catch (EmptyPriorityQueueException e) {
+        		encore = false;
+        	}
+        	catch (ElementNotFoundException e) {
+        		encore = false;
+        	}
+        }
+        
+        ShortestPathSolution solution = null;
+        //Pour le test SOMME(couts) = Cout(Label(Destination))
+        double SommeCouts = 0.0;
+        
+     // Destination has no predecessor, the solution is infeasible...
+        if (labels[dest.getId()].getPere() == null) {
+            solution = new ShortestPathSolution(data, Status.INFEASIBLE);
+        }
+        else {
+
+            // The destination has been found, notify the observers.
+            notifyDestinationReached(dest);
+
+            // Create the path from the array of predecessors...
+            ArrayList<Arc> arcs = new ArrayList<>();
+            Arc arc = labels[dest.getId()].getPere();
+            
+            while (arc != null) {
+                arcs.add(arc);
+
+                //Pour le test SOMME(couts) = Cout(Label(Destination))
+                SommeCouts += data.getCost(arc);
+                arc = labels[arc.getOrigin().getId()].getPere();
+            }
+
+            // Reverse the path...
+            Collections.reverse(arcs);
+
+            // Create the final solution.
+            solution = new ShortestPathSolution(data, Status.OPTIMAL, new Path(graph, arcs));
+        }
+        
+        if (solution.getStatus() != Status.INFEASIBLE) {
+	        // Test si le path est valide
+	        if (!solution.getPath().isValid()) {
+	        	System.out.println("Erreur : PATH SOLUTION INVALID !");
+	    	}
+	        
+	        // Test s'il correspond a Shortest et Fastest de la classe Path
+	        // INUTILE, SUJET MAL COMPRIS
+	        /*if (data.getMode() == Mode.LENGTH) {
+	        	if (solution.getPath().getLength() != Path.createShortestPathFromNodes(graph, solution.getPath().getNodes()).getLength()) {
+	            	System.out.println("Erreur : PATH SOLUTION NOT SHORTEST !");
+	        	} else {
+	        		System.out.println("PATH SOLUTION SHORTEST !");
+	        	}
+	        } else {
+	        	if (solution.getPath().getMinimumTravelTime() != Path.createFastestPathFromNodes(graph, solution.getPath().getNodes()).getMinimumTravelTime()) {
+	            	System.out.println("Erreur : PATH SOLUTION NOT FASTEST !");
+	        	} else {
+	        		System.out.println("PATH SOLUTION FASTEST !");
+	        	}
+	        }*/
+	        
+	        if ((int)(SommeCouts * 1000) != (int)(labels[dest.getId()].getCost() * 1000)) {
+	        	System.out.println("Erreur : Cost label Dest (" + labels[dest.getId()].getCost() + ") != Somme(cost) (" + SommeCouts + ")");
+	    	}
+        }
+                
+        
+
+        return solution;
+    } 
 }
diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
index 6120205..78d70b4 100644
--- a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/Label.java
@@ -29,6 +29,14 @@ public class Label implements Comparable<Label>{
 		return this.cout;
 	}
 	
+	public double getEstimation() {
+		return 0;
+	}
+	
+	public double getTotalCost() {
+		return this.cout;
+	}
+	
 	public Arc getPere() {
 		return this.pere;
 	}
@@ -43,13 +51,20 @@ public class Label implements Comparable<Label>{
 	}	
 	
 	public int compareTo(Label o) {
-		double diff = this.cout - o.cout;
+		double diff = this.getTotalCost() - o.getTotalCost();
 		if (diff < 0.0) {
 			return -1;
 		} else if (diff > 0.0) {
 			return 1;
 		} else {
-			return 0;
+			diff = this.getEstimation() - o.getEstimation();
+			if (diff < 0.0) {
+				return -1;
+			} else if (diff > 0.0) {
+				return 1;
+			} else {
+				return 0;
+			}
 		}
 	}
 }
\ No newline at end of file
diff --git a/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java
new file mode 100644
index 0000000..541fb5c
--- /dev/null
+++ b/be-graphes-algos/src/main/java/org/insa/graphs/algorithm/shortestpath/LabelStar.java
@@ -0,0 +1,22 @@
+package org.insa.graphs.algorithm.shortestpath;
+
+import org.insa.graphs.model.Node;
+import java.lang.Comparable;
+
+public class LabelStar extends Label implements Comparable<Label>{
+	
+	private double estimation;
+	
+	public LabelStar(Node init_Node, double init_estimation) {
+		super(init_Node);
+		this.estimation = init_estimation;
+	}
+	
+	public double getEstimation() {
+		return this.estimation;
+	}
+	
+	public double getTotalCost() {
+		return this.estimation + this.getCost();
+	}
+}
\ No newline at end of file