A* implementation

“A*(A-star) is a best-first, graph search algorithm that finds the least cost path from a given initial node to one goal node It uses a distance-plus-cost heuristic function (usually denoted f(x)) to determine the order in which the search visits nodes in the tree.” (Wikipedia)

I implemented the A* algorithm few weeks ago in Java, I wonder what do you think about this impl, how can it be improved? mostly design and performance wise:

The A* main class

package org.simple.astar; 

import java.util.ArrayList;
import java.util.Collections;
import java.util.HashSet; 

import java.util.LinkedList;
import java.util.Set; 

public class AStar<S extends State> { 

 private S init;
 private S goal;
 private int expendedStates;
 private HuristicFunction<S> h;
 private ArrayList<S> openSet;
 private HashSet<S> closedSet; 

 public int getExpendedStates() {
 return expendedStates;
 } 

 public AStar(S init,S goal,HuristicFunction<S> h ){
   this.init = init;
   this.goal = goal;
   this.h = h;
   expendedStates = -1;
   init.setDistance(0);
 } 

 @SuppressWarnings("unchecked")
 public LinkedList<S> find(){
   HComparator hcomp = new HComparator(h,goal);
   openSet = new ArrayList<S>();
   closedSet = new HashSet<S>();
   S current = init;
   boolean foundOptimal = false;
   openSet.add(current);
   while(openSet.size()>0){
     Collections.sort(openSet, hcomp);//check
     current = openSet.get(0);
     ++expendedStates;
     foundOptimal = current.equals(goal);
     if(foundOptimal)
       break;
     removeState(current);
     Set<S> ne = current.getNeighbours();
     for (S state : ne){
       if(!closedSet.contains(state)){
         if(openSet.contains(state)){
           int stIndex = openSet.indexOf(state);
           if(state.getDistance()<openSet.get(stIndex).getDistance()){
            openSet.get(stIndex).setDistance(state.getDistance());
            openSet.get(stIndex).setPrevious(current);
           }
       }
       else
         openSet.add(state);
       }
     } 

   }
   LinkedList<S> result = new LinkedList<S>();
   if(!foundOptimal)
     return result;
   while(current.getPrevious()!= null){
     result.addFirst(current);
     current = (S)current.getPrevious();
   }
   result.addFirst(init);
   return result;
 } 

 private void removeState(S s){
   closedSet.add(s);
   openSet.remove(s);
 }
}

Heuristic interface:

package org.simple.astar; 

public interface HuristicFunction<N extends State> {
 public double getEvaluation(N current,N goal);
}

Heuristic example (BFS)

package org.simple.astar;

public class BFS<S extends State> implements HuristicFunction<S> {
	@Override
	public double getEvaluation(State current, State goal) {
		return 0;
	}
}

Comparator:

package org.simple.astar;
import java.util.Comparator; 

public class HComparator<S extends State> implements Comparator<S> {
 HuristicFunction<S> h;
 S goal; 

 public HComparator(HuristicFunction<S> h,S goal){
   this.h = h;
   this.goal = goal;
 }
 @Override
 public int compare(S o1, S o2) {
   Double e1 = o1.getDistance()+h.getEvaluation(o1,goal);
   Double e2 = o2.getDistance()+h.getEvaluation(o2,goal);
   if(e1.doubleValue() == e2.doubleValue())
     return o2.getDistance().compareTo(o1.getDistance());
   else
    return e1.compareTo(e2);
   }
}

State(node) abstract class

package org.simple.astar;
import java.util.Set; 

public abstract class State implements Cloneable {
 Integer distance;
 State previous; 

 public Integer getDistance() {
   return distance;
 } 

 public void setDistance(int distance) {
   this.distance = distance;
 } 

 public State getPrevious() {
   return previous;
 } 

 public void setPrevious(State previous) {
   this.previous = previous;
 } 

 @SuppressWarnings("unchecked")
 public abstract Set getNeighbours() ;
}

you can read more about path finding using A* here