point temppt;
struct edge tempedge;

/*
#define PRINT_HEAP
*/

#define key_type FLOAT
#define KEY_FORMAT "\tK:%.3f\t"
#define key_extras struct {                                                   \
                       point pt;                                              \
                       point predecessor;                                     \
                       struct edge containing_edge;                           \
                   }

#define copy_key_extras( x, x_extras, y, y_extras )                           \
        temppt = (x_extras).pt;                                               \
        (x_extras).pt = (y_extras).pt;                                        \
        (y_extras).pt = temppt;                                               \
        if (y_extras.pt) {                                                    \
            setheapptr( (y_extras).pt, (y) );                                 \
        }                                                                     \
        if (x_extras.pt) {                                                    \
            setheapptr( (x_extras).pt, (x) );                                 \
        }                                                                     \
        temppt          = (x_extras).predecessor;                             \
        (x_extras).predecessor = (y_extras).predecessor;                      \
        (y_extras).predecessor = temppt;                                      \
        edgecopy( (x_extras).containing_edge, tempedge );                     \
        edgecopy( (y_extras).containing_edge, (x_extras).containing_edge );   \
        edgecopy( tempedge,            (y_extras).containing_edge)

#define printkeyextras(x)                                                    \
        printf("\tORG:(%.1f,%.1f)", (x).pt[0], (x).pt[1] );                  \
        printf("\t")


#include "fibheap.c"

void relax( u, v_pure, w, curredge )
/* Note: u is acutally in the examined_nodes nodes set */
struct fibonacci_heap *u;
point v_pure;
FLOAT w;
struct edge curredge;
{
    struct fibonacci_heap *v;
    v = heapptr( v_pure );
    if (v != NULL) {
        if ( FibHeapKey(v) > (FibHeapKey(u) + w) ) {
            FibHeapSetKey( v, (FibHeapKey(u) + w) );
            (FibHeapKeyExtras(v)).predecessor = (FibHeapKeyExtras(u)).pt;
            edgecopy( curredge, (FibHeapKeyExtras(v)).containing_edge );
        }
    }
}

printpath(goal)
point goal;
{
    struct fibonacci_heap *u;
    point pt = goal;

    printf("Path (reversed):\n");
    while (pt) {
        u = heapptr( pt );

        dest( (FibHeapKeyExtras(u)).containing_edge, goal );

        printf("\t(%.1f,%.1f) to ", goal[0], goal[1] );

        printf("(%.1f,%.1f)", (FibHeapKeyExtras(u)).pt[0],
                                                (FibHeapKeyExtras(u)).pt[1] );
	goal = joinpt((FibHeapKeyExtras(u)).containing_edge);
	if (  goal ) {
	  printf("via (%.3f,%.3f)", goal[0], goal[1]);
	}
	printf("\n");
                                   
        pt = (FibHeapKeyExtras(u)).predecessor;
    }
}
dijkstra(source,goal)
point source,goal;
{
    int i;
    struct edge curredge, nextedge;
    struct edge distinguished_edge;
    point pt[3];
    point endpt;
    struct fibonacci_heap *u;
    point v;

    FibHeapInit();

    printf("Building Heap\n" );

    if ((source==NULL) || (goal==NULL)) {
        printf("No distinguished nodes\n");
        exit(1);
    }
    if (heapptr(source)==NULL) {
        u = FibHeapAlloc();
        FibHeapKey(u) = 0;
        (FibHeapKeyExtras(u)).pt = source;
        FibHeapInsert(u);
        setheapptr( source, u );
    }

    /* go through all the edges one-by-one & put them into the heap */
    i=0;
    quadpathinit();
    curredge.orient = 0;
    curredge.quad = followquadpath();
    while (curredge.quad != (quadedge *) NULL) {
        org(curredge, pt[0]);
        if (pt[0] != (point) NULL) {
            lnext(curredge, nextedge);
            org(nextedge, pt[1]);
            i++;
            if (pt[0] == source) {
                /* anything with the source as org */
                edgecopy(curredge, distinguished_edge);
            }
            if (pt[1] == source) {
                /* anything with the dest as org */
                sym(curredge, distinguished_edge);
            }
            if (heapptr(pt[0])==NULL) {
                u = FibHeapAlloc();
                FibHeapKey(u) = MAX_FLOAT;
                (FibHeapKeyExtras(u)).pt = pt[0];
                edgecopy( curredge, (FibHeapKeyExtras(u)).containing_edge );
                FibHeapInsert(u);
                setheapptr( pt[0], u );
            }
            if (heapptr(pt[1])==NULL) {
                u = FibHeapAlloc();
                FibHeapKey(u) = MAX_FLOAT;
                (FibHeapKeyExtras(u)).pt = pt[1];
                edgecopy( nextedge, (FibHeapKeyExtras(u)).containing_edge );
                FibHeapInsert(u);
                setheapptr( pt[1], u );
            }
        }
        curredge.quad = followquadpath();
    }

    edgecopy( distinguished_edge,
                        (FibHeapKeyExtras(heapptr(source))).containing_edge );

    printf("Starting Dijkstra\n\n");

    while (minH != NULL) {
#ifdef PRINT_HEAP
        printf("%d NODES IN HEAP\n", NumNodesInRoot );
        printheap( minH, 0, NumNodesInRoot );
        printf("\n\n");
#endif
        u = FibHeapExtractMin();

        edgecopy( (FibHeapKeyExtras(u)).containing_edge, curredge );

        dest( curredge, endpt );

        do { /* go around edges org'd at u */
            onext(curredge,curredge);
            dest( curredge, v );
/*
            printf( "\t(%.1f,%.1f) to (%.1f,%.1f), cost=%.5f  totalC=%.5f\n",
                                (FibHeapKeyExtras(u)).pt[0],
                                (FibHeapKeyExtras(u)).pt[1],
                                v[0], v[1], cost(curredge),
                                FibHeapKey(u)+cost(curredge)   );
*/

            if ((FibHeapKeyExtras(u)).pt != v) {
                 /* happens because of quadedges... */
                relax( u, v, cost(curredge), curredge );
            }
        } while (v != endpt );
    }
    printpath( goal );
}