// Sparse Dijkstra
// (c) David Bindel
// http://www.cims.nyu.edu/~dbindel/code/mex/dijkstra.c

/*
 * [d, p] = dijkstra(A, s)
 * 
 * Implementation of Dijkstra's method using a Matlab sparse matrix
 * as an adjacency matrix.  Zero entries represent non-existent edges.
 * Uses linear search for simplicity -- change to a heap if this ever
 * becomes the code bottleneck.
 *
 * Inputs:
 *   A - sparse adjacency matrix (transpose of the usual)
 *   s - label of source node (one-based)
 *   
 * Outputs:
 *   d - distance vector from Dijkstra
 *   p - predecessor vector from Dijkstra
 */

#include <stdint.h>
#include <string.h>
#include "mex.h"

/* Sift ith element up the heap
 */
static void upheap(double* d, int64_t* h2d, int64_t* d2h, int64_t i)
{
    int64_t    it = h2d[i];
    double dt = d[it];

    while (i > 1 && d[h2d[i/2]] > dt) {
        h2d[i]      = h2d[i/2];
        d2h[h2d[i]] = i;
        i           = i/2;
    }

    h2d[i]      = it;
    d2h[h2d[i]] = i;
}


/* Sift ith element down the heap
 */
static void downheap(double* d, int64_t* h2d, int64_t* d2h, int64_t i, int64_t n)
{
    int64_t    it = h2d[i];
    double dt = d[it];

    while (2*i < n) {
        int64_t lc = 2*i; 
        int64_t rc = 2*i+1;
        int64_t mc = (rc == n || d[h2d[lc]] <= d[h2d[rc]]) ? lc : rc;

        if (dt > d[h2d[mc]]) {
            h2d[i] = h2d[mc];
            d2h[h2d[i]] = i;
            i = mc;
        } else {
            break;
        }
    } 

    h2d[i]      = it;
    d2h[h2d[i]] = i;
}


/* Add item to queue or update
 */
static void enqueue(int64_t i, double* d, int64_t* h2d, int64_t* d2h, int64_t *hn)
{
    /* Use one-based indexing within heap */
    --d;
    ++i;

    if (d2h[i] == 0) {

        /* Sift new item up */
        *hn     = *hn + 1;
        d2h[i]  = *hn;
        h2d[*hn] = i;
        upheap(d, h2d, d2h, *hn);


    } else {

        /* Move old item */
        upheap  (d, h2d, d2h, d2h[i]);
        downheap(d, h2d, d2h, d2h[i], *hn);

    }
}

/* Dequeue existing item
 */
/* Add item to queue or update
 */
static int64_t dequeue(double* d, int64_t* h2d, int64_t* d2h, int64_t *hn)
{
    int64_t top;

    /* Use one-based indexing within heap */
    --d;

    /* Dequeue top item */
    top      = h2d[1];
    d2h[top] = 0;

    /* Move tail item into hole */
    h2d[1]      = h2d[*hn];
    d2h[h2d[1]] = 1;
    --*hn;

    /* Sift down to correct place */
    downheap(d, h2d, d2h, 1, *hn);

    return top-1;
}


/* Run Dijkstra's algorithm
 */
void dijkstra(double* d, double* p, 
              size_t* jc, size_t* ir, double* cost, 
              int64_t src, int64_t n)
{
    int64_t* h2d = (int64_t*) mxMalloc((n+1) * sizeof(int64_t));
    int64_t* d2h = (int64_t*) mxMalloc((n+1) * sizeof(int64_t));
    int64_t i, j, hn;
    
    /* Initialize */
    memset(h2d, 0, (n+1)*sizeof(int64_t));
    memset(d2h, 0, (n+1)*sizeof(int64_t));
    for (i = 0; i < n; ++i) {
        d[i]   = 1./0.;
        p[i]   = -1;
    }

    hn = 0;
    d[src] = 0;
    enqueue(src, d, h2d, d2h, &hn);

    while (hn > 0) {

        /* Find and dequeue closest node */
        int64_t u = dequeue(d, h2d, d2h, &hn);
        double du = d[u];

        /* Update neighbor costs */
        for (i = jc[u]; i < jc[u+1]; ++i) {
            int64_t v = ir[i];
            if (cost[i] != 0 && d[v] > du + cost[i]) {
                d[v] = d[u] + cost[i];
                p[v] = u+1;
                enqueue(v, d, h2d, d2h, &hn);
            }
        }
    }

    mxFree(h2d);
    mxFree(d2h);
}