Метод кратчайшего пути взвешенного ориентированного графика - что можно улучшить?

Question

Итак, я разработал следующий код для вычисления кратчайшего пути во взвешенном ориентированном графе. Связь между двумя вершинами представлена ​​в матрице двойным, а несвязность бесконечностью. Как я могу улучшить свой код? Также я использовал minHeap, потому что минимальным элементом всегда является root, но я думаю об использовании OrderedDoubleLinkedList, так как он имеет метод removeFirst. Какой подход я должен использовать?

public Iterator iteratorShortestPathConnectionCost(T startVertex, T targetVertex,int difficulty) {
        int startIndex = this.getIndex(startVertex);
        int targetIndex = this.getIndex(targetVertex);


        int previousPosition = -1;         //the variable will be used to store the previous vertice
        double weight;
        double[] pathWeight = new double[this.numVertices];//an array that will be used to store the weight between the starter vertix and the other vertexs
        int[] previous = new int[this.numVertices]; //an array that stores the vertexs that belong to the shortest path
        boolean[] visited = new boolean[this.numVertices]; //an array that tells if a vertex has already been visited or not

        LinkedHeap<Double> traversalMinHeap = new LinkedHeap<>();
        LinkedStack<Integer> invertedResult = new LinkedStack<>();
        ArrayUnorderedList<T> resultList = new ArrayUnorderedList<>(10);

        if (!indexIsValid(startIndex) || !indexIsValid(targetIndex)) {
            return resultList.iterator();
        }

        //Set distance from starting vertix to all vertexs to infinity
        for (int i = 0; i < this.numVertices; i++) {
            pathWeight[i] = Double.POSITIVE_INFINITY;
        }

        //Set distance from starting vertex to himself to 0
        pathWeight[startIndex] = 0;

        //Set all vertexs as unvisited
        for (int i = 0; i < this.numVertices; i++) {
            visited[i] = false;
        }

        previous[startIndex] = -1;

        //Visit the index with the smallest distance from starting vertex, that means visit the start index because the distance is 0
        visited[startIndex] = true;
        weight = 0;

        for (int i = 0; i < this.numVertices; i++) {
            if (!visited[i] && this.getAdjMatrixWeight()[startIndex][i] !=Infinity) { //checks if the vertex has not been visited yet and if it is connected with start path
                pathWeight[i] = pathWeight[startIndex] + this.getAdjMatrixWeight()[startIndex][i]*difficulty;
                previous[i] = startIndex;
                traversalMinHeap.addElement(pathWeight[i]);
            }
        }

        do {
            try {
                weight = (traversalMinHeap.removeMin());
            } catch (EmptyCollectionException ex) {
            }

            traversalMinHeap = new LinkedHeap<>();

           // if (weight == Double.POSITIVE_INFINITY) {
              //  return resultList.iterator();
            //} else {

            //store previous
                for (int i = 0; i < this.numVertices; i++) {
                    if ((pathWeight[i] == weight) && !visited[i]) {
                        for (int j = 0; j < this.numVertices; j++) {
                            if ((this.getAdjMatrixWeight()[i][j] !=Infinity)) {
                                previousPosition = i;
                            }
                        }
                    }
                }
                if (previousPosition != -1) { 
                    visited[previousPosition] = true;
                } else { // if the previous position is -1 it´s because there were no vertexs connect to the starting vertex
                    return resultList.iterator(); 
                }
            //}

            for (int i = 0; i < this.numVertices; i++) {
                if (!visited[i]) {
                    if ((this.getAdjMatrixWeight()[previousPosition][i] != Infinity) && (pathWeight[previousPosition] + this.getAdjMatrixWeight()[previousPosition][i]) < pathWeight[i]) {
                        pathWeight[i] = pathWeight[previousPosition] + this.getAdjMatrixWeight()[previousPosition][i]*difficulty;
                        previous[i] = previousPosition;
                    }
                    traversalMinHeap.addElement(pathWeight[i]);
                }
            }
        } while (!traversalMinHeap.isEmpty() && !visited[targetIndex]);

        previousPosition = targetIndex;
        invertedResult.push(previousPosition);

        do {
            previousPosition = previous[previousPosition];
            invertedResult.push(previousPosition);
        } while (previousPosition != startIndex && previousPosition != -1);

        while (!invertedResult.isEmpty() && previousPosition != -1) {
            try {
                resultList.addToRear(this.vertices[(invertedResult.pop())]);
            } catch (EmptyCollectionException ex) {
            }
        }

        return resultList.iterator();