private void addState(SearchState state, int dist, int edgeDist) {
    int estimatedDist = dist + h.estimateCostToGoal(state);
    if (estimatedDist >= OO)
      return;

    SearchState oldState = stateSet.get(state);
    if (oldState == null) {
      stateSet.put(state, state);
      oldState = state;
    }
    if (state.prevState != null)
      oldState.backLinks.add(new BackLink(state.prevState, edgeDist, state.prevPieceIndex));
    Integer curDist = stateDistMap.get(state);
    if (curDist == null || dist < curDist) {
      oldState.prevState = state.prevState;
      oldState.prevPieceIndex = state.prevPieceIndex;
      stateDistMap.put(oldState, dist);
      stateDistQueue.add(new StateDist(oldState, estimatedDist));
    }
  }