[JAVA] Problema con algoritmo A*

[tete8646]

Salve a tutti sono un nuovo utente fresco fresco di registrazione
ho un problema con l'implementazione di quest'algoritmo su java. Per prima cosa ovviamente mi sono creato le classi grafo,nodi e archi per costruire il grafo orientato e pesato sugli archi, dopo di che ho iniziato a cimentarmi sulla classe A* ma non riesco a venirne fuori da ormai 2 giorni.
Il problema è che non riesco a gestire i due insiemi di collezioni aperto e chiuso poiche ogni volta che punto ad un nodo sul quale arrivo attraverso un altro arco mi aggiorna quel valore anche dentro il mio insieme dove ce lo stesso nodo salvato ma con un valore inferiore poiche vi ero arrivato tramite altra strada piu "corta" e non riesco a risolvere questo problema.
Vi posto il codice qui di seguito:

import java.util.*;
import java.util.logging.Logger;

import util.LoggerManager;

import model.IArc;
import model.IGraph;
import model.INode;
import model.Node;
import model.IPath;
import model.Path;

public class A_Star implements IA_Star {

	private IGraph g;		            
	private INode start;
	private INode end;	
	private Logger logger;	
	
	public A_Star() {
		logger = LoggerManager.getLogger();
	}	
	
	/* |||||  PRIMA VEDO CON A* IL VALORE DEL PERCORSO MIGLIORE |||||||| */
	
	public int Ast(INode iniz,INode fin,IGraph graph) {
		Set<INode> open=new TreeSet<INode>();
		Set<INode> closed=new TreeSet<INode>();
		Set<INode> openApp=new TreeSet<INode>();
		
		INode app=new Node(1000,1000);
		int valCammino=0;
		
		this.g=graph;
		this.start=iniz;
		this.end=fin;
		
		//create the open list of nodes, initially containing only our starting node
		//create the closed list of nodes, initially empty
		
		openApp.add(iniz);
				
		//while (we have not reached our goal)		
		
		while (!(openApp.isEmpty()))  {				
			
			open.addAll(aggiungiNodi(openApp,open));
			
			if(app.getId()!= fin.getId()){				
				//consider the best node in the open list (the node with the lowest f value)
				app=bestNode(openApp);
									
				
				if (app.getId()==this.end.getId()) {					
					return valCammino;
				}
		       
				else {
					//move the current node to the closed list and consider all of its neighbors
					Set<INode> neighbors=new TreeSet<INode>();
					neighbors.addAll(nextNodes(app));					
					closed.add(app);
					openApp.remove(app);				
					
					
					for (INode k : neighbors) {						
													
						
					   if ((Contain(k,closed)) && (k.getValCammino() > valCamminoLista(k,closed))) {  //this neighbor is in the 															closed list and our current g value is lower
							
						int newCamm = valCamminoLista(k,closed);								
				        	k.setValCammino(newCamm);  // update the neighbor with the new, lower, g value
				        	//change the neighbor's parent to our current node
				        	neighbors.clear();
				        	neighbors.addAll(nextNodes(k));
				        	
				        }					
						
					   if((Contain(k,openApp)) && (k.getValCammino() > valCamminoLista(k,openApp))) { //this neighbor is in the open 													    		       list and our current g value is lower
							 
							int newCamm = valCamminoLista(k,openApp);								
							k.setValCammino(newCamm);//update the neighbor with the new, lower, g value 
							//change the neighbor's parent to our current node
							neighbors.clear();
					        neighbors.addAll(nextNodes(k));
					        
					    }
						     
					    if(!(Contain(k,openApp)) && (!(Contain(k,closed)))) { // this neighbor is not in either the open or closed list
					    	IArc a=graph.getArc(app,k);					    	
					    	k.setValCammino(a.getWeight()+app.getValCammino());					    	
					    	openApp.add(k);  // add the neighbor to the open list and set its g value
					    }											
				         
					} // chiuso secondo for
		       
				}
				
			} // chiuso if subito dopo primo for			
			
			
		}       //chiuso while principale
		
		return valCammino;
	}	

  	
	private INode bestNode(Set<INode> open){
		
		INode ris=new Node(1000,1000);
		int i=open.size();
		
		for(INode n : open){
			
			if (i==1) {
				ris.setId(n.getId());
				ris.setValCammino(n.getValCammino());
			}	
					
			else if(n.getValCammino() < ris.getValCammino()) {
				ris.setId(n.getId());
				ris.setValCammino(n.getValCammino());
			}			
		}			
		return ris;
	}
	
	private Set<INode> nextNodes(INode k) {
		Set<INode> ris=new TreeSet<INode>();
		IArc[] archi=this.g.getExitingArcs(k);
		for(IArc a : archi){
			ris.add(a.getDestination());
			a.getDestination().setValCammino(a.getWeight() + k.getValCammino());
			
		}		
		return ris;
	}
	
	/*private INode nextNode(INode k) {
		INode ris=new Node(0,0);
		IArc[] arco=this.g.getExitingArcs(k);
		for(IArc a : arco){
			ris=a.getDestination();
			ris.setValCammino(a.getWeight() + k.getValCammino());
		}
		return ris;
	}*/
	
	private boolean Contain(INode n,Set<INode> lista){
		boolean ris=false;
		
		for(INode k : lista){
			if(k.getId()==n.getId()){
				ris=true;
			}
		}
		
		return ris;
	}
	
	private int valCamminoLista(INode k,Set<INode> close){
		int valCammino=0;
		for(INode s : close){						
			if(s.getId()==k.getId()){
				valCammino=s.getValCammino();				
			}			
		}		
		return valCammino;
	}
	
	private Set<INode> aggiungiNodi(Set<INode> app,Set<INode> open){
		Set<INode> ris=new TreeSet<INode>();
		ris.addAll(open);
		
		for(INode n : app){			
			if(Contain(n, ris)){
				for(INode k : ris){				
					if((k.getId()==n.getId()) &&                                              (k.getValCammino()>n.getValCammino())){
						k.setValCammino(n.getValCammino());
					}				
				}
			}
			
			else{					
				ris.add(n);
			}
		}
		
		return ris;
	}
}

So che è un po lungo ma se qualcuno puo darmi una mano ne sarei contentissimo...Le parti commentate sono cio che suggeriva un euristica che ho trovato su internet e ho cercato di seguirla il piu possibile.

Grazie in anticipo a tutti