Оптимизация программы

[Unknown_friend]

Здравствуйте,

есть программа которая ищет самые короткие пути из 0 узла до всех остальных узлов в графике, по алгоритму Дейкстры с использованием приоритетных очередей.
Цель, оптимизировать функцию pq_update():

typedef struct {
   int size;    // the maximal number of entries in the array - максимальное колличество записей в массиве
   int len;     // the current number of entries in the array - текущее количество записей в массиве
   int *cost;   // array with entries (costs)
   int *label;  // array with vertex labels 
   int *heapIDX;   // this is need for implementing pq_update()
                   // array with indexes of cost vertices, i.e.,where the particular 
                   // heapIDX[id] is the index where the cost of the vertex with the 
                   // label id is tored in cost. 
                   // E.g., cost of the vertex with the label id is at the heapIDX[id] 
                   // position in the cost array, thus, cost is cost[ heapIDX[ id ] ]
} pq_heap_s;

_Bool pq_update(void *_pq, int label, int cost){
    _Bool ret = false;
   pq_heap_s *pq = (pq_heap_s*)_pq;
   if (
         pq 
         && pq->len < pq->size
         && label >= 0
         && label < pq->size
         && pq->heapIDX[label] != -1 //vertex with the label is in the pq 
      ) {

      pq->cost[pq->heapIDX[label]] = cost; // update the cost, but heap property is not satified 
      // assert(pq_is_heap(pq, 0));

      pq_heap_s *pqBackup = (pq_heap_s*)pq_alloc(pq->size); //create backup of the heap
      pqBackup->len = pq->len;
      for (int i = 0; i < pq->len; ++i) { // backup the help
         pqBackup->cost[i]= pq->cost[i]; //just cost and labels
         pqBackup->label[i] = pq->label[i];
      }

      pq->len = 0; //clear all vertices in the current heap
      for (int i = 0; i < pqBackup->len; ++i) { //create new heap from the backup with updated cost for index
         pq_push(pq, pqBackup->label[i], pqBackup->cost[i]);
      }
      pq_free(pqBackup); // release the queue 
      // assert(pq_is_heap(pq, 0));
      ret = true;
 }
   return ret;
}

Данная функция обновляет значение пути до узла, до которого был найден более короткий путь, чем был перед этим. При этом естественно должны быть соблюдены все свойства приоритетной очереди. 
Вот как я написал решение: 

_Bool pq_update(void *_pq, int label, int cost){
    _Bool ret = false;
   pq_heap_s *pq = (pq_heap_s*)_pq;
   if (
         pq 
         && pq->len < pq->size
         && label >= 0
         && label < pq->size
         && pq->heapIDX[label] != -1 //vertex with the label is in the pq 
      ) {
int i = pq->heapIDX[label] - 1;
      while(i>0){
           if(cost <= pq->cost[i]){
             pq->cost[i+1] = pq->cost[i];
             pq->label[i+1] = pq->label[i];
             pq->heapIDX[pq->label[i]] = i+1;
             }else{
              ret = true;
              break;}
              i--;                 
           }
           pq->cost[i+1] = cost;
           pq->label[i+1] = label;
           pq->heapIDX[label] = i+1;
}
   return ret;
}

Работает неправильно, несколько узлов считает неправильно.
Подскажите, в чем ошибка?
Спасибо.