Реализация фибоначчиевой кучи для оптимизации алгоритма Дейкстры

#include <stdio.h>
#include <malloc.h>
#include <limits.h>
#define MAX_PQSIZE  16
typedef unsigned int uint_t;
 
typedef struct {
    uint_t v, d;
} vert_t;
 
typedef struct {
    uint_t p, d, v;
} res_t;
 
typedef struct {
    vert_t* arr;
    uint_t  cnt;
    uint_t  len;
    int (*cmp)(const vert_t*, const vert_t*);
} pqueue_t;
 
static void _pqueue_heapify(pqueue_t* pq, size_t i);
static int _pqueue_alloc(pqueue_t* pq, size_t n);
inline void   pqueue_init(pqueue_t* pq, int (*cmp)(const vert_t*, const vert_t*));
inline vert_t pqueue_top(pqueue_t* pq);
inline int    pqueue_empty(const pqueue_t* pq);
inline size_t pqueue_size(const pqueue_t* pq);
int    pqueue_push(pqueue_t* pq, uint_t v, uint_t d);
void   pqueue_pop(pqueue_t* pq);
void   pqueue_clear(pqueue_t* pq);
 
//-----------------
 
typedef struct node {
    uint_t index;
    uint_t cost;
    struct node* next;
} node_t;
 
typedef struct graph {
    node_t** arr;
    size_t   cnt;
} graph_t;
 
static int node_new(graph_t* q, uint_t x, uint_t y, uint_t cost);
inline void graph_init(graph_t* g);
int    graph_input(FILE* _in, graph_t* g);
void   graph_free(graph_t* g);
res_t* graph_shortest(graph_t* g, uint_t s);
 
 
int main(void){
    res_t*  res;
    uint_t  i, j, s, t;
    graph_t g;
/*  ввод из файла
    FILE* fp = fopen("graph.txt", "rt");
    if(fp == NULL)
        return 1;
 
    graph_init(&g);
    if(! graph_input(fp, &g)){
        fclose(fp);
        return 2;
    }
    fclose(fp);
*/
    graph_init(&g);
    if(! graph_input(stdin, &g))
        return 2;
 
    //найти путь от нулевой вершины во все другие
    s   = 0;
    res = graph_shortest(&g, s);
    if(res == NULL){
        graph_free(&g);
        free(res);
        return 3;
    }
 
    //вывод всех путей от заданной вершины во все другие
    for(i = 0; i < g.cnt; ++i){
        if(i == s)
            continue;
 
        j = 0;
        res[j++].v = i;
        for(t = i; t != s; t = res[t].p)
            res[j++].v = res[t].p;
 
        printf("length: %u  path: ", res[i].d);
        for(t = 0; t < j; ++t)
            printf("%u ", res[j - 1 - t].v);
        putchar('\n');
    }
    free(res);
    graph_free(&g);
    return 0;
}
 
static int compare(const vert_t* a, const vert_t* b){ 
    return (a->d < b->d); 
}
 
//поиск кратчайшего пути "алгоритмом Дейкстра"
res_t* graph_shortest(graph_t* g, uint_t s){
    const node_t* k;
    res_t*   res;
    vert_t   ver;
    uint_t   i;
    pqueue_t pq;
 
    res = (res_t*)malloc(g->cnt * sizeof(res_t));
    if(res == NULL)
        return NULL;
 
    for(i = 0; i < g->cnt; ++i){
        res[i].d = UINT_MAX;
        res[i].v = res[i].p = 0;
    }
 
    res[s].d = 0;
    pqueue_init(&pq, compare);
    if(! pqueue_push(&pq, s, 0))
        goto err;
 
    while(! pqueue_empty(&pq)){
        ver = pqueue_top(&pq);
        pqueue_pop(&pq);
        if(ver.d > res[ver.v].d)
            continue;
 
        for(k = g->arr[ver.v]; k != NULL; k = k->next){
 
            if((res[ver.v].d + k->cost) < res[k->index].d){
                res[k->index].d = res[ver.v].d + k->cost;
                res[k->index].p = ver.v;
                if(! pqueue_push(&pq, k->index, res[k->index].d))
                    goto err;
            }
        }
    }
    pqueue_clear(&pq);
    return res;
err:
    free(res);
    pqueue_clear(&pq);
    return NULL;
}
 
/* ввод из входного потока неориентированного взвешенного графа
   формат представлен в виде списка рёбер графа:
   кол-во вершин  кол-во рёбер
   ребро(2-вершины)  вес */
int graph_input(FILE* _in, graph_t* g){
    uint_t i, vn, em, x, y, cost;
    if((fscanf(_in, "%u %u", &vn, &em) != 2) || (vn <= 1) || (em <= 1))
        return 0;
 
    g->arr = (node_t**)malloc(vn * sizeof(node_t*));
    if(g->arr == NULL)
        return 0;
 
    g->cnt = vn;
    for(i = 0; i < vn; ++i)
        g->arr[i] = NULL;
 
    for(i = 0; i < em; ++i){
        if(fscanf(_in, "%u %u %u", &x, &y, &cost) == 3){
            if((x < vn) && (y < vn)){
                if(! node_new(g, x, y, cost))
                    goto err;
                if(! node_new(g, y, x, cost))
                    goto err;
            }
        } else
            break;
    }
    return 1;
err:
    graph_free(g);
    return 0;
}
 
//удаление графа из памяти
void graph_free(graph_t* g){
    node_t* t;
    uint_t  i;
    for(i = 0; i < g->cnt; ++i){
        while(g->arr[i] != NULL){
            t = g->arr[i];
            g->arr[i] = g->arr[i]->next;
            free(t);
        }
    }
    if(g->arr != NULL)
        free(g->arr);
    g->arr = NULL;
    g->cnt = 0;
}
 
inline void graph_init(graph_t* g){ 
    g->arr = NULL;
    g->cnt = 0;
}
 
static int node_new(graph_t* q, uint_t x, uint_t y, uint_t cost){
    node_t* p = (node_t*)malloc(sizeof(node_t));
    if(p != NULL){
        p->next   = q->arr[x];
        p->index  = y;
        p->cost   = cost;
        q->arr[x] = p;
    }
    return (p != NULL);
}
 
//---------------------
 
//вставка в приоритетную очередь
int pqueue_push(pqueue_t* pq, uint_t v, uint_t d){
    vert_t t;
    size_t i, p;
    if(! _pqueue_alloc(pq, 1))
        return 0;
    
    pq->arr[pq->cnt].v = v;
    pq->arr[pq->cnt].d = d;
    if(++pq->cnt == 1)
        return 1;
 
    i = pq->cnt - 1;
    p = (i - 1) >> 1;
    while((i > 0) && !(*pq->cmp)(&pq->arr[p], &pq->arr[i])){
        t = pq->arr[p];
        pq->arr[p] = pq->arr[i];
        pq->arr[i] = t;
 
        i = p;
        p = (i - 1) >> 1;
    }
    return 1;
}
 
//удаление элемента
void pqueue_pop(pqueue_t* pq){
    if(pq->cnt > 0){
        pq->arr[0] = pq->arr[--pq->cnt];
        _pqueue_heapify(pq, 0);
    }
}
 
//удаление всех
void pqueue_clear(pqueue_t* pq){
    if(pq->arr != NULL)
        free(pq->arr);
    pq->arr = NULL;
    pq->cnt = 0;
    pq->len = MAX_PQSIZE;
}
 
//инициализация
inline void pqueue_init(pqueue_t* pq, int (*cmp)(const vert_t*, const vert_t*)){
    pq->arr = NULL;
    pq->cnt = 0;
    pq->len = MAX_PQSIZE;
    pq->cmp = cmp;
}
 
inline vert_t pqueue_top(pqueue_t* pq){ return pq->arr[0]; }
inline int    pqueue_empty(const pqueue_t* pq){ return (pq->cnt == 0); }
inline size_t pqueue_size(const pqueue_t* pq) { return pq->cnt; }
 
static void _pqueue_heapify(pqueue_t* pq, size_t i){
    vert_t t;
    size_t r, l, h;
    while(1){
        l = (i << 1) + 1;
        r = l + 1; //(i << 1) + 2
        
        if((l < pq->cnt) && (*pq->cmp)(&pq->arr[l], &pq->arr[i]))
            h = l;
        else
            h = i;
 
        if((r < pq->cnt) && (*pq->cmp)(&pq->arr[r], &pq->arr[h]))
            h = r;
 
        if(h != i){
            t = pq->arr[i];
            pq->arr[i] = pq->arr[h];
            pq->arr[h] = t;
            i = h;
        } else
            break;
    }
}
 
static int _pqueue_alloc(pqueue_t* pq, size_t n){
    size_t  m;
    vert_t* t;
    if(pq->arr == NULL){
        m       = (n > pq->len) ? n : pq->len;
        pq->arr = (vert_t*)malloc(m * sizeof(vert_t));
        if(pq->arr == NULL)
            return 0;
        pq->len = m;
    } else if((pq->cnt + n) >= pq->len){
        m = pq->cnt + n + (pq->len >> 1);
        t = (vert_t*)realloc(pq->arr, m * sizeof(vert_t));
        if(t == NULL)
            return 0;
        pq->arr = t;
        pq->len = m;
    }
    return 1;
}