/****************************************************************
 *
 *  MODULE:       v.net.steiner
 *
 *  AUTHOR(S):    Radim Blazek
 *
 *  PURPOSE:      Find Steiner tree for network
 *
 *  COPYRIGHT:    (C) 2001 by the GRASS Development Team
 *
 *                This program is free software under the
 *                GNU General Public License (>=v2).
 *                Read the file COPYING that comes with GRASS
 *                for details.
 *
 **************************************************************/
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <grass/gis.h>
#include <grass/vector.h>
#include <grass/dbmi.h>
#include <grass/glocale.h>

/* costs between 2 terminals */
typedef struct {
    int term1, term2;
    double cost;
} COST;

int nterms; /* number of terminals */
int nnodes; /* number of nodes */
int *terms; /* array of 1. terminals; 2. accepted steiner points; 3. tested
               steiner point */
COST *term_costs; /* costs between terminals */
COST *sp_costs;   /* costs between StP and terminals */
int *comps;       /* numbers of component, the terminal was assigned to */

/* pointer to array of pointers to costs to other nodes,
 * matrix: from-to, from < to:
 *   1-2 1-3 1-4 ... 1-nnodes
 *   2-3 2-4 ... 2-nnodes
 *   3-4 ... 3-nnodes
 *   ...
 *   (nnodes - 1)-nnodes
 *   !!! init costs for from or to node, before this cost is read by
 *   init_node_costs();
 */
double **nodes_costs;

int cmp(const void *, const void *);

/* Init all costs to/from given node */
int init_node_costs(struct Map_info *Map, int from)
{
    int to, ret, row, col;
    double cost;

    G_verbose_message(_("Init costs from node %d"), from);

    for (to = 1; to <= nnodes; to++) {
        if (from == to)
            continue;
        ret = Vect_net_shortest_path(Map, from, to, NULL, &cost);
        if (ret == -1) {
            G_debug(1, "Destination node %d is unreachable from node %d\n", to,
                    from);
            cost = -2;
        }

        if (from < to) {
            row = from - 1;
            col = to - from - 1;
        }
        else {
            row = to - 1;
            col = from - to - 1;
        }

        G_debug(3, "init costs %d - > %d = %f\n", from, to, cost);
        nodes_costs[row][col] = cost;
    }

    return 1;
}

/* Get cost from node to node.
 *  From or to costs must be init before!!!
 *  Returns: 1 - ok, 0 - not reachable
 */
int get_node_costs(int from, int to, double *cost)
{
    int row, col, tmp;

    if (from == to) {
        *cost = 0;
        return 1;
    }
    if (from > to) {
        tmp = from;
        from = to;
        to = tmp;
    }
    row = from - 1;
    col = to - from - 1;

    if (nodes_costs[row][col] == -2) {
        *cost = PORT_DOUBLE_MAX;
        return 0;
    }

    *cost = nodes_costs[row][col];
    return 1;
}

/* Calculate costs for MST on given set of terminals.
 *  If AList / NList is not NULL, list of arcs / nodes in MST is created
 *  Note: qsort() from more (say >30) terminals, takes long time (most of
 * mst()). To improve the speed, there are used two sorted queues of costs:
 *          1. for all combinations of in trms
 *          2. from 'sp' to all other terminals
 *        Because 1. is sorted only if new sp as added to list of terminals,
 *        and 2. is much shorter than 1., a lot of time is saved.
 */
int mst(
    struct Map_info *Map, int *trms,
    int ntrms,                   /* array of terminal, number of terminals */
    double *cst, double max_cst, /* cost, maximum cost */
    struct ilist *AList, struct ilist *NList, /* list of arcs/nodes in ST */
    int sp, /* Steiner point (node) to be tested with terminals, (0 = ignore) */
    int rebuild)
{ /* rebuild the sorted list of costs for terminals */
    int i, j, node1, node2, com1, com2, t1, t2, line;
    static int k;
    int tcpos, scpos; /* current position in the term_costs / sp_costs */
    double tcst;
    struct ilist *List;
    int nsteps, quse;
    int nall; /* number of terminals + sp ( if used ) */

    if (AList != NULL) {
        Vect_reset_list(AList);
    }
    List = Vect_new_list();

    /* Create sorted array for all combinations of terms */
    if (rebuild) {
        k = 0;
        for (i = 0; i < ntrms; i++) {
            for (j = i + 1; j < ntrms; j++) {
                term_costs[k].term1 = i;
                term_costs[k].term2 = j;
                get_node_costs(trms[i], trms[j], &tcst);
                term_costs[k].cost = tcst;
                k++;
            }
        }

        qsort((void *)term_costs, k, sizeof(COST),
              cmp); /* this takes most of a time in mst() */
        for (i = 0; i < k; i++) {
            G_debug(3, "  %d - %d cost = %f\n", term_costs[i].term1,
                    term_costs[i].term2, term_costs[i].cost);
        }
    }

    /* Create sorted array for all combinations of sp -> terms */
    if (sp > 0) {
        for (i = 0; i < ntrms; i++) {
            sp_costs[i].term1 = -1; /* not needed */
            sp_costs[i].term2 = i;
            get_node_costs(sp, trms[i], &tcst);
            sp_costs[i].cost = tcst;
        }
        qsort((void *)sp_costs, ntrms, sizeof(COST), cmp);
        for (i = 0; i < ntrms; i++) {
            G_debug(3, "  %d - %d cost = %f\n", sp_costs[i].term1,
                    sp_costs[i].term2, sp_costs[i].cost);
        }
    }

    tcst = 0;

    /* MST has number_of_terminals-1 arcs */
    if (sp > 0) {
        nall = ntrms + 1;
        nsteps = ntrms; /* i.e. + one StP */
    }
    else {
        nall = ntrms;
        nsteps = ntrms - 1;
    }
    G_debug(1, "nall = %d\n", nall);
    for (i = 0; i < nall; i++)
        comps[i] = 0;

    tcpos = 0;
    scpos = 0;
    G_debug(2, "nsteps = %d\n", nsteps);
    for (i = 0; i < nsteps; i++) {
        G_debug(2, "step = %d\n", i);
        /* Take the best (lowest costs, no cycle) from both queues */
        /* For both queues go to next lowest costs without cycle */
        /* treminal costs */
        for (j = tcpos; j < k; j++) {
            t1 = term_costs[j].term1;
            t2 = term_costs[j].term2;
            com1 = comps[t1];
            com2 = comps[t2];
            if (com1 != com2 ||
                com1 == 0) { /* will not create cycle -> candidate */
                tcpos = j;
                break;
            }
        }
        if (j == k) { /* arc without cycle not found */
            tcpos = -1;
        }

        /* StP costs */
        if (sp > 0) {
            for (j = scpos; j < ntrms; j++) {
                t1 = ntrms; /* StP is on first fre position */
                t2 = sp_costs[j].term2;
                com1 = comps[t1];
                com2 = comps[t2];
                G_debug(3, "scpos: j = %d comps(%d) = %d coms(%d) = %d\n", j,
                        t1, com1, t2, com2);
                if (com1 != com2 ||
                    com1 == 0) { /* will not create cycle -> candidate */
                    scpos = j;
                    G_debug(3, " ok -> scpos = %d\n", scpos);
                    break;
                }
            }
            if (j == ntrms) { /* arc without cycle not found */
                scpos = -1;
            }
        }
        else {
            scpos = -1;
        }
        /* Do not access invalid items even for debugging */
        if (tcpos != -1 && scpos != -1)
            G_debug(3, "tcost = %f, scost = %f\n", term_costs[tcpos].cost,
                    sp_costs[scpos].cost);

        /* Now we have positions set on lowest costs in each queue or -1 if no
         * more/not used */
        if (tcpos >= 0 && scpos >= 0) {
            if (term_costs[tcpos].cost < sp_costs[scpos].cost)
                quse = 1; /* use terms queue */
            else
                quse = 2; /* use sp queue */
        }
        else if (tcpos >= 0) {
            quse = 1; /* use terms queue */
        }
        else {
            quse = 2; /* use sp queue */
        }

        /* Now we know from which queue take next arc -> add arc to components
         */
        if (quse == 1) {
            t1 = term_costs[tcpos].term1;
            t2 = term_costs[tcpos].term2;
            tcst += term_costs[tcpos].cost;
            tcpos++;
        }
        else {
            t1 = ntrms;
            t2 = sp_costs[scpos].term2;
            tcst += sp_costs[scpos].cost;
            scpos++;
        }
        G_debug(3, "quse = %d t1 = %d t2 = %d\n", quse, t1, t2);
        G_debug(3, "tcst = %f (max = %f)\n", tcst, max_cst);

        com1 = comps[t1];
        com2 = comps[t2];
        comps[t1] = i + 1;
        comps[t2] = i + 1;
        G_debug(3, "comps(%d) = %d coms(%d) = %d\n", t1, i + 1, t2, i + 1);

        /* reset connected branches */
        for (j = 0; j < nall; j++) {
            if (comps[j] == com1 && com1 != 0)
                comps[j] = i + 1;

            if (comps[j] == com2 && com2 != 0)
                comps[j] = i + 1;
        }
        if (tcst > max_cst) {
            G_debug(3, "cost > max -> return\n");
            *cst = PORT_DOUBLE_MAX;
            return 1;
        }

        /* add to list of arcs */
        if (AList != NULL) {
            node1 = trms[t1];
            node2 = trms[t2];
            Vect_net_shortest_path(Map, node1, node2, List, NULL);
            for (j = 0; j < List->n_values; j++) {
                Vect_list_append(AList, abs(List->value[j]));
            }
        }
    }

    /* create list of nodes */
    if (NList != NULL) {
        Vect_reset_list(NList);
        for (i = 0; i < AList->n_values; i++) {
            line = AList->value[i];
            Vect_get_line_nodes(Map, line, &node1, &node2);
            Vect_list_append(NList, node1);
            Vect_list_append(NList, node2);
        }
    }

    *cst = tcst;

    Vect_destroy_list(List);

    return 1;
}

int main(int argc, char **argv)
{
    int i, j, ret;
    int nlines, type, ltype, afield, tfield, geo, cat;
    int sp, nsp, nspused, node, line;
    struct Option *map, *output, *afield_opt, *tfield_opt, *afcol, *type_opt,
        *term_opt, *nsp_opt;
    struct Flag *geo_f;
    struct GModule *module;
    struct Map_info Map, Out;
    int *testnode;         /* array all nodes: 1 - should be tested as Steiner,
                            * 0 - no need to test (unreachable or terminal) */
    struct ilist *TList;   /* list of terminal nodes */
    struct ilist *StArcs;  /* list of arcs on Steiner tree */
    struct ilist *StNodes; /* list of nodes on Steiner tree */
    struct boxlist *pointlist;
    double cost, tmpcost;
    struct cat_list *Clist;
    struct line_cats *Cats;
    struct line_pnts *Points;

    /* Initialize the GIS calls */
    G_gisinit(argv[0]);

    module = G_define_module();
    G_add_keyword(_("vector"));
    G_add_keyword(_("network"));
    G_add_keyword(_("steiner tree"));
    module->label =
        _("Creates Steiner tree for the network and given terminals.");
    module->description =
        _("Note that 'Minimum Steiner Tree' problem is NP-hard "
          "and heuristic algorithm is used in this module so "
          "the result may be sub optimal.");

    map = G_define_standard_option(G_OPT_V_INPUT);
    output = G_define_standard_option(G_OPT_V_OUTPUT);

    type_opt = G_define_standard_option(G_OPT_V_TYPE);
    type_opt->key = "arc_type";
    type_opt->options = "line,boundary";
    type_opt->answer = "line,boundary";
    type_opt->label = _("Arc type");

    afield_opt = G_define_standard_option(G_OPT_V_FIELD);
    afield_opt->key = "arc_layer";
    afield_opt->answer = "1";
    afield_opt->label = _("Arc layer");

    tfield_opt = G_define_standard_option(G_OPT_V_FIELD);
    tfield_opt->key = "node_layer";
    tfield_opt->answer = "2";
    tfield_opt->label = _("Node layer (used for terminals)");

    afcol = G_define_option();
    afcol->key = "acolumn";
    afcol->type = TYPE_STRING;
    afcol->required = NO;
    afcol->description = _("Arcs' cost column (for both directions)");

    term_opt = G_define_standard_option(G_OPT_V_CATS);
    term_opt->key = "terminal_cats";
    term_opt->required = YES;
    term_opt->description =
        _("Categories of points on terminals (layer is specified by nlayer)");

    nsp_opt = G_define_option();
    nsp_opt->key = "npoints";
    nsp_opt->type = TYPE_INTEGER;
    nsp_opt->required = NO;
    nsp_opt->multiple = NO;
    nsp_opt->answer = "-1";
    nsp_opt->description = _("Number of Steiner points (-1 for all possible)");

    geo_f = G_define_flag();
    geo_f->key = 'g';
    geo_f->description =
        _("Use geodesic calculation for longitude-latitude projects");

    if (G_parser(argc, argv))
        exit(EXIT_FAILURE);

    Cats = Vect_new_cats_struct();
    Points = Vect_new_line_struct();

    type = Vect_option_to_types(type_opt);
    afield = atoi(afield_opt->answer);

    TList = Vect_new_list();
    StArcs = Vect_new_list();
    StNodes = Vect_new_list();

    Clist = Vect_new_cat_list();
    tfield = atoi(tfield_opt->answer);
    Vect_str_to_cat_list(term_opt->answer, Clist);

    G_debug(1, "Input categories:\n");
    for (i = 0; i < Clist->n_ranges; i++) {
        G_debug(1, "%d - %d\n", Clist->min[i], Clist->max[i]);
    }

    if (geo_f->answer)
        geo = 1;
    else
        geo = 0;

    Vect_check_input_output_name(map->answer, output->answer, G_FATAL_EXIT);

    Vect_set_open_level(2);

    if (Vect_open_old(&Map, map->answer, "") < 0)
        G_fatal_error(_("Unable to open vector map <%s>"), map->answer);

    nnodes = Vect_get_num_nodes(&Map);
    nlines = Vect_get_num_lines(&Map);

    /* Create list of terminals based on list of categories */
    for (i = 1; i <= nlines; i++) {
        ltype = Vect_get_line_type(&Map, i);
        if (!(ltype & GV_POINT))
            continue;

        Vect_read_line(&Map, Points, Cats, i);
        if (!(Vect_cat_get(Cats, tfield, &cat)))
            continue;
        node = Vect_find_node(&Map, Points->x[0], Points->y[0], Points->z[0], 0,
                              0);
        if (!node) {
            G_warning(_("Point is not connected to the network (cat=%d)"), cat);
            continue;
        }
        if (Vect_cat_in_cat_list(cat, Clist)) {
            Vect_list_append(TList, node);
        }
    }

    nterms = TList->n_values;
    /* GTC Terminal refers to an Steiner tree endpoint */
    G_message(_("Number of terminals: %d\n"), nterms);

    if (nterms < 2) {
        /* GTC Terminal refers to an Steiner tree endpoint */
        G_fatal_error(_("Not enough terminals (< 2)"));
    }

    /* Number of steiner points */
    nsp = atoi(nsp_opt->answer);
    if (nsp > nterms - 2) {
        nsp = nterms - 2;
        G_warning(_("Requested number of Steiner points > than possible"));
    }
    else if (nsp == -1) {
        nsp = nterms - 2;
    }

    G_message(_("Number of Steiner points set to %d\n"), nsp);

    testnode = (int *)G_malloc((nnodes + 1) * sizeof(int));
    for (i = 1; i <= nnodes; i++)
        testnode[i] = 1;

    /* Alloc arrays of costs for nodes, first node at 1 (0 not used) */
    nodes_costs = (double **)G_malloc((nnodes) * sizeof(double *));
    for (i = 0; i < nnodes; i++) {
        nodes_costs[i] = (double *)G_malloc((nnodes - i) * sizeof(double));
        for (j = 0; j < nnodes - i; j++)
            nodes_costs[i][j] = -1; /* init, i.e. cost was not calculated yet */
    }

    /* alloc memory from each to each other (not directed) terminal */
    i = nterms + nterms - 2; /*  max number of terms + Steiner points */
    comps = (int *)G_malloc(i * sizeof(int));
    i = i * (i - 1) / 2; /* number of combinations */
    term_costs = (COST *)G_malloc(i * sizeof(COST));

    /* alloc memory for costs from Stp to each other terminal */
    i = nterms + nterms - 2 - 1; /*  max number of terms + Steiner points - 1 */
    sp_costs = (COST *)G_malloc(i * sizeof(COST));

    terms = (int *)G_malloc((nterms + nterms - 2) *
                            sizeof(int)); /* i.e. +(nterms - 2)  St Points */
    /* Create initial parts from list of terminals */
    G_debug(1, "List of terminal nodes (%d):\n", nterms);
    for (i = 0; i < nterms; i++) {
        G_debug(1, "%d\n", TList->value[i]);
        terms[i] = TList->value[i];
        testnode[terms[i]] = 0; /* do not test as Steiner */
    }

    /* Build graph */
    Vect_net_build_graph(&Map, type, afield, 0, afcol->answer, NULL, NULL, geo,
                         0);

    /* Init costs for all terminals */
    for (i = 0; i < nterms; i++)
        init_node_costs(&Map, terms[i]);

    /* Test if all terminal may be connected */
    for (i = 1; i < nterms; i++) {
        ret = get_node_costs(terms[0], terms[i], &cost);
        if (ret == 0) {
            /* GTC Terminal refers to an Steiner tree endpoint */
            G_fatal_error(_("Terminal at node [%d] cannot be connected "
                            "to terminal at node [%d]"),
                          terms[0], terms[i]);
        }
    }

    /* Remove not reachable from list of SP candidates */
    j = 0;
    for (i = 1; i <= nnodes; i++) {
        ret = get_node_costs(terms[0], i, &cost);
        if (ret == 0) {
            testnode[i] = 0;
            G_debug(2,
                    "node %d removed from list of Steiner point candidates\n",
                    i);
            j++;
        }
    }

    G_message(_("[%d] (not reachable) nodes removed from list "
                "of Steiner point candidates"),
              j);

    /* calc costs for terminals MST */
    ret = mst(&Map, terms, nterms, &cost, PORT_DOUBLE_MAX, NULL, NULL, 0,
              1); /* no StP, rebuild */
    G_message(_("MST costs = %f"), cost);

    /* Go through all nodes and try to use as steiner points -> find that which
     * saves most costs */
    nspused = 0;
    for (j = 0; j < nsp; j++) {
        sp = 0;
        G_verbose_message(_("Search for [%d]. Steiner point"), j + 1);

        for (i = 1; i <= nnodes; i++) {
            G_percent(i, nnodes, 1);
            if (testnode[i] == 0) {
                G_debug(3, "skip test for %d\n", i);
                continue;
            }
            ret =
                mst(&Map, terms, nterms + j, &tmpcost, cost, NULL, NULL, i, 0);
            G_debug(2, "cost = %f x %f\n", tmpcost, cost);
            if (tmpcost < cost) { /* sp candidate */
                G_debug(
                    3, "  steiner candidate node = %d mst = %f (x last = %f)\n",
                    i, tmpcost, cost);
                sp = i;
                cost = tmpcost;
            }
        }
        if (sp > 0) {
            G_message(_("Steiner point at node [%d] was added "
                        "to terminals (MST costs = %f)"),
                      sp, cost);
            terms[nterms + j] = sp;
            init_node_costs(&Map, sp);
            testnode[sp] = 0;
            nspused++;
            /* rebuild for nex cycle */
            ret = mst(&Map, terms, nterms + nspused, &tmpcost, PORT_DOUBLE_MAX,
                      NULL, NULL, 0, 1);
        }
        else { /* no steiner found */
            G_message(_("No Steiner point found -> leaving cycle"));
            break;
        }
    }

    G_message(_("Number of added Steiner points: %d "
                "(theoretic max is %d).\n"),
              nspused, nterms - 2);

    /* Build lists of arcs and nodes for final version */
    ret = mst(&Map, terms, nterms + nspused, &cost, PORT_DOUBLE_MAX, StArcs,
              StNodes, 0, 0);

    /* Calculate true costs, which may be lower than MST if steiner points were
     * not used */

    if (nsp < nterms - 2) {
        G_message(_("Spanning tree costs on complete graph = %f\n"
                    "(may be higher than resulting Steiner tree costs!!!)"),
                  cost);
    }
    else
        G_message(_("Steiner tree costs = %f"), cost);

    /* Write arcs to new map */
    if (Vect_open_new(&Out, output->answer, Vect_is_3d(&Map)) < 0)
        G_fatal_error(_("Unable to create vector map <%s>"), output->answer);

    Vect_hist_command(&Out);

    G_debug(1, "Steiner tree:");
    G_debug(1, "Arcs' categories (layer %d, %d arcs):", afield,
            StArcs->n_values);

    for (i = 0; i < StArcs->n_values; i++) {
        line = StArcs->value[i];
        ltype = Vect_read_line(&Map, Points, Cats, line);
        Vect_write_line(&Out, ltype, Points, Cats);
        Vect_cat_get(Cats, afield, &cat);
        G_debug(1, "arc cat = %d", cat);
    }

    G_debug(1, "Nodes' categories (layer %d, %d nodes):", tfield,
            StNodes->n_values);

    pointlist = Vect_new_boxlist(0);
    for (i = 0; i < StNodes->n_values; i++) {
        double x, y, z;
        struct bound_box box;

        node = StNodes->value[i];

        Vect_get_node_coor(&Map, node, &x, &y, &z);
        box.E = box.W = x;
        box.N = box.S = y;
        box.T = box.B = z;
        Vect_select_lines_by_box(&Map, &box, GV_POINT, pointlist);

        nlines = Vect_get_node_n_lines(&Map, node);
        for (j = 0; j < pointlist->n_values; j++) {
            line = pointlist->id[j];
            ltype = Vect_read_line(&Map, Points, Cats, line);
            if (!(ltype & GV_POINT))
                continue;
            if (!(Vect_cat_get(Cats, tfield, &cat)))
                continue;
            Vect_write_line(&Out, ltype, Points, Cats);
            G_debug(1, "node cat = %d", cat);
        }
    }

    Vect_build(&Out);

    G_message(n_("A Steiner tree with %d arc has been built",
                 "A Steiner tree with %d arcs has been built",
                 StArcs->n_values),
              StArcs->n_values);

    /* Free, ... */
    Vect_destroy_list(StArcs);
    Vect_destroy_list(StNodes);
    Vect_close(&Map);
    Vect_close(&Out);

    exit(EXIT_SUCCESS);
}

int cmp(const void *pa, const void *pb)
{
    COST *p1 = (COST *)pa;
    COST *p2 = (COST *)pb;

    if (p1->cost < p2->cost)
        return -1;

    if (p1->cost > p2->cost)
        return 1;

    return 0;
}
