/* need to declare this before visit() */
static void order_vertices(struct cfgstruct *cfg, uint startvextex, uint numvertices, uint *vertices, struct stackstruct* order);

/**  Find and order SCCs with Tarjan's algorithm - recursive.
  *  See http://www.ics.uci.edu/~eppstein/161/960220.html#sca for more info on Tarjan's
  */
static uint visit(uint p /** Current vertex */, struct stackstruct *L, uint *dfsnum, uint *low, struct cfgstruct *cfg, struct stackstruct* order, uint curnum)
{
    L->stack[L->index] = p;
    L->index++;

    low[p] = curnum;

    dfsnum[p] = curnum;

    for(uint i = cfg->vertices[p].numexits; i > 0; i--)  /* By doing it in reverse order, the reverse postorder will look nice ordered */
    {
        uint q = cfg->vertices[p].exits[i - 1];

        switch(dfsnum[q])
        {
            case UINT_MAX: /* Not yet visited, visit it */
                curnum = visit(q, L, dfsnum, low, cfg, order, curnum + 1);
                if(low[q] < low[p])
                    low[p] = low[q];
                break;

            case UINT_MAX-1:    /* Visited but no longer on the stack */
                break;

            default:    /* Visited and on the stack */
                if(dfsnum[q] < low[p])
                    low[p] = dfsnum[q];
                break;
        }
    }

    if(low[p] == dfsnum[p]) /* Found a (possibly trivial) SCC */
    {
        uint numvertices = 0;

        uint v;
        do
        {
            L->index--;
            v = L->stack[L->index];
            dfsnum[v] = UINT_MAX-1; /* No longer to be used */
            numvertices++;          /* Increase number of vertices in component */
        } while(v != p);

        dbgprint(CFG_DEBUG, "Found SCC:");
        for(uint j = 0;j < numvertices; j++)
        {
            dbgprint(CFG_DEBUG, " %d",  L->stack[L->index + j]);
        }
        dbgprint(CFG_DEBUG, "\n");

        if(numvertices == 1)        /* Found a trivial (single vertex) SCC */
        {
            assert(order->index > 0);
            order->index--;
            order->stack[order->index] = p; /* Put that vertex in the order */
        }
        else
        {
            /* Calculate indegrees from outside the SCC */
            uint *indegrees, *insccmap;
            indegrees = xcalloc(cfg->numvertices * sizeof(*indegrees));
            insccmap  = xcalloc(cfg->numvertices * sizeof(*insccmap ));

            for(uint i = 0; i < numvertices; i++)
                insccmap[L->stack[L->index + i]] = 1;

            for(uint i = 0; i < cfg->numvertices; i++)
                if(insccmap == 0)
                    for(uint j = 0; j < cfg->vertices[i].numexits; j++)
                        indegrees[cfg->vertices[i].exits[j]]++;

            xfree(insccmap);

            uint max = 0, maxidx = 0;   /* Default is the first */

            for(uint i = 0; i < numvertices; i++)
            {
                if(indegrees[L->stack[L->index + i]] > max)
                {
                    max = indegrees[L->stack[L->index + i]];
                    maxidx = i;
                }
            }

            dbgprint(CFG_DEBUG, "Highest indegree: %d with %d\n", L->stack[L->index + maxidx], max);

            xfree(indegrees);

            if(numvertices == 2)    /* We know what this will be so no need to recurse again */
            {
                assert(order->index > 0);
                order->index--;
                order->stack[order->index] = L->stack[L->index + (maxidx == 0 ? 1 : 0)]; /* Lowest indegree */

                assert(order->index > 0);
                order->index--;
                order->stack[order->index] = L->stack[L->index + maxidx]; /* Greatest indegree */
            }
            else                    /* Sort that */
            {
                order_vertices(cfg, L->stack[L->index + maxidx], numvertices, &L->stack[L->index], order);
            }
        }
    }

    return curnum;
}

/** Approximately topologically sort all vertices in a graph.
 *  Strongly connected components are found with Tarjan's algorithm
 *  and if necessary sorted with this function,
 *  after choosing the first vertex by indegree.
 *  We return the SCC's in a reverse postorder.
 */
static void order_vertices(struct cfgstruct *cfg, uint startvertex, uint numvertices, uint *vertices, struct stackstruct* order)
{
    uint *dfsnum, *low;
    struct stackstruct L;

    L.stack = xmalloc(numvertices * sizeof(*L.stack));    /* This may overallocate a bit, but whatever */
    L.index = 0;

    low     = xmalloc(cfg->numvertices * sizeof(*low));
    dfsnum  = xmalloc(cfg->numvertices * sizeof(*dfsnum));

    for(uint i = 0; i < cfg->numvertices; i++)
        dfsnum[i] = UINT_MAX-1;             /* All are not to be used unless */
    for(uint i = 0; i < numvertices; i++)
        dfsnum[vertices[i]] = UINT_MAX;     /* they are specifed in the list. */

    dfsnum[startvertex] = UINT_MAX-1;       /* The starting vertex is never to be used */

    /* Emulate the startvertex so it is not in an SCC */
    uint curnum = 0;
    for(uint i = cfg->vertices[startvertex].numexits; i > 0; i--)  /* By doing it in reverse order, the reverse postorder will look nice ordered */
    {
        uint q = cfg->vertices[startvertex].exits[i - 1];
        if(dfsnum[q] == UINT_MAX)
        {
            curnum = visit(q, &L, dfsnum, low, cfg, order, curnum); /* Do it! */
            curnum++;
        }
    }

    dbgprint(CFG_DEBUG, "Found SCC: %d\n", startvertex);

    assert(order->index > 0);
    order->index--;
    order->stack[order->index] = startvertex; /* Put the startvertex in the order */

    /* Free stuff */
    xfree(L.stack);
    xfree(low);
    xfree(dfsnum);
}