#include <inttypes.h>
#include <limits>
#include "../debug.h"
#include "spq/spq.h"

void dijkstra(double* d, double* p, size_t* jc, size_t* ir, double* cost, int64_t src, int64_t n) {
	ASSERT(n > 0, "Graph should be non-empty.");
    
    static const double inf = 1. / 0.;
    
	double maxcost = cost[0];
	for(int64_t i = 1; i < n; ++i)
		if(cost[i] > maxcost)
			maxcost = cost[i];
	ASSERT(maxcost < inf, "Costs must be finite.");

	for(int64_t i = 0; i < n; ++i) {
		d[i] = inf;
		p[i] = -1;
	}

    bool *pprocessed = new bool[n]();

	spq::spq_t pq(n, (uint64_t)maxcost);
    d[src] = 0;
	pq.insert(src, 0);
    while(!pq.is_empty()) {
        uint64_t dmin = -1;
		spq::node_t *pmin_nodes = pq.delete_mins(dmin);
		for(spq::node_t *pmin_node = pmin_nodes; pmin_node; pmin_node = pmin_node->_pnext)
            pprocessed[pmin_node->_name] = true;
		for(spq::node_t *pmin_node = pmin_nodes; pmin_node; pmin_node = pmin_node->_pnext) {
			uint64_t u = pmin_node->_name;
			for(int64_t i = jc[u]; i < jc[u + 1]; ++i) {
				int64_t v = ir[i];
				if(pprocessed[v])
                    continue;
            	double alt_route_cost = dmin + cost[i];
            	if(d[v] > alt_route_cost) {
                	d[v] = alt_route_cost;
					p[v] = u + 1;
					if(pq.contains(v))
						pq.decrease_key(v, (int64_t)d[v]);
					else
						pq.insert(v, (int64_t)d[v]);
            	}
        	}
		}
    }

    delete [] pprocessed;
}
