#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <grass/gis.h>
#include <grass/vector.h>
#include <grass/dbmi.h>
#include <grass/dgl/graph.h>
#include <grass/glocale.h>
#include "alloc.h"

int alloc_from_centers_loop_tt(struct Map_info *Map, NODE *Nodes,
                               CENTER *Centers, int ncenters,
                               int tucfield)
{
    int center, line, nlines, i;
    int node1;
    int cat;
    struct line_cats *Cats;
    struct line_pnts *Points;
    double cost, n1cost, n2cost;
    int ret;

    Cats = Vect_new_cats_struct();
    Points = Vect_new_line_struct();
    
    nlines = Vect_get_num_lines(Map);

    for (i = 2; i <= (nlines * 2 + 2); i++) {
	Nodes[i].center = -1;/* NOTE: first two items of Nodes are not used */
	Nodes[i].cost = -1;
	Nodes[i].edge = 0;
    }

    for (center = 0; center < ncenters; center++) {
	G_percent(center, ncenters, 1);
	node1 = Centers[center].node;
	Vect_net_get_node_cost(Map, node1, &n1cost);
	G_debug(2, "center = %d node = %d cat = %d", center, node1,
		Centers[center].cat);

	for (line = 1; line <= nlines; line++) {
	    G_debug(5, "  node1 = %d line = %d", node1, line);
	    Vect_net_get_node_cost(Map, line, &n2cost);
	    /* closed, left it as not attached */

	    if (Vect_read_line(Map, Points, Cats, line) < 0)
		continue;
	    if (Vect_get_line_type(Map, line) != GV_LINE)
		continue;
	    if (!Vect_cat_get(Cats, tucfield, &cat))
		continue;

	    for (i = 0; i < 2; i++) {
		if (i == 1)
		    cat *= -1;

		ret =
		    Vect_net_ttb_shortest_path(Map, node1, 0, cat, 1,
					       tucfield, NULL,
					       &cost);
		if (ret == -1) {
		    continue;
		}		/* node unreachable */

		/* We must add center node costs (not calculated by Vect_net_shortest_path() ), but
		 *  only if center and node are not identical, because at the end node cost is add later */
		if (ret != 1)
		    cost += n1cost;

		G_debug(5,
			"Arc nodes: %d %d cost: %f (x old cent: %d old cost %f",
			node1, line, cost, Nodes[line * 2 + i].center,
			Nodes[line * 2 + i].cost);
		if (Nodes[line * 2 + i].center == -1 ||
		    Nodes[line * 2 + i].cost > cost) {
		    Nodes[line * 2 + i].cost = cost;
		    Nodes[line * 2 + i].center = center;
		}
	    }
	}
    }
    G_percent(1, 1, 1);

    Vect_destroy_cats_struct(Cats);
    Vect_destroy_line_struct(Points);

    return 0;
}

int alloc_to_centers_loop_tt(struct Map_info *Map, NODE *Nodes,
                               CENTER *Centers, int ncenters,
                               int tucfield)
{
    int center, line, nlines, i;
    int node1;
    int cat;
    struct line_cats *Cats;
    struct line_pnts *Points;
    double cost, n1cost, n2cost;
    int ret;

    Cats = Vect_new_cats_struct();
    Points = Vect_new_line_struct();
    
    nlines = Vect_get_num_lines(Map);

    for (i = 2; i <= (nlines * 2 + 2); i++) {
	Nodes[i].center = -1;/* NOTE: first two items of Nodes are not used */
	Nodes[i].cost = -1;
	Nodes[i].edge = 0;
    }

    for (line = 1; line <= nlines; line++) {
	G_debug(5, "  line = %d", line);
	Vect_net_get_node_cost(Map, line, &n2cost);
	/* closed, left it as not attached */

	if (Vect_read_line(Map, Points, Cats, line) < 0)
	    continue;
	if (Vect_get_line_type(Map, line) != GV_LINE)
	    continue;
	if (!Vect_cat_get(Cats, tucfield, &cat))
	    continue;

	for (center = 0; center < ncenters; center++) {
	    G_percent(center, ncenters, 1);
	    node1 = Centers[center].node;
	    Vect_net_get_node_cost(Map, node1, &n1cost);
	    G_debug(2, "center = %d node = %d cat = %d", center, node1,
		    Centers[center].cat);

	    for (i = 0; i < 2; i++) {
		if (i == 1)
		    cat *= -1;

		ret =
		    Vect_net_ttb_shortest_path(Map, cat, 1, node1, 0,
					       tucfield, NULL,
					       &cost);
		if (ret == -1) {
		    continue;
		}		/* node unreachable */

		/* We must add center node costs (not calculated by Vect_net_shortest_path() ), but
		 *  only if center and node are not identical, because at the end node cost is add later */
		if (ret != 1)
		    cost += n1cost;

		G_debug(5,
			"Arc nodes: %d %d cost: %f (x old cent: %d old cost %f",
			node1, line, cost, Nodes[line * 2 + i].center,
			Nodes[line * 2 + i].cost);
		if (Nodes[line * 2 + i].center == -1 ||
		    Nodes[line * 2 + i].cost > cost) {
		    Nodes[line * 2 + i].cost = cost;
		    Nodes[line * 2 + i].center = center;
		}
	    }
	}
    }
    G_percent(1, 1, 1);

    Vect_destroy_cats_struct(Cats);
    Vect_destroy_line_struct(Points);

    return 0;
}

int alloc_from_centers(dglGraph_s *graph, NODE *Nodes, CENTER *Centers, int ncenters)
{
    int i, nnodes;
    dglHeap_s heap;
    dglEdgesetTraverser_s et;
    int have_node_costs;
    dglInt32_t ncost;

    nnodes = dglGet_NodeCount(graph);

    /* initialize nodes */
    for (i = 1; i <= nnodes; i++) {
	Nodes[i].cost = -1;
	Nodes[i].center = -1;
	Nodes[i].edge = 0;
    }

    ncost = 0;
    have_node_costs = dglGet_NodeAttrSize(graph);

    dglHeapInit(&heap);

    for (i = 0; i < ncenters; i++) {
	int v = Centers[i].node;

	if (Nodes[v].cost == 0)
	    continue;		/* ignore duplicates */
	Nodes[v].cost = 0;		/* make sure all centers are processed first */
	Nodes[v].center = i;
	dglHeapData_u heap_data;

	heap_data.ul = v;
	dglHeapInsertMin(&heap, 0, ' ', heap_data);
    }
    while (1) {
	dglInt32_t v, dist;
	dglHeapNode_s heap_node;
	dglHeapData_u heap_data;
	dglInt32_t *edge;
	dglInt32_t *node;

	if (!dglHeapExtractMin(&heap, &heap_node))
	    break;
	v = heap_node.value.ul;
	dist = heap_node.key;
	if (Nodes[v].cost < dist)
	    continue;

	node = dglGetNode(graph, v);

	if (have_node_costs && Nodes[v].edge) {
	    memcpy(&ncost, dglNodeGet_Attr(graph, node),
		   sizeof(ncost));
	    if (ncost > 0)
		dist += ncost;
	    /* do not go through closed nodes */
	    if (ncost < 0)
		continue;
	}

	dglEdgeset_T_Initialize(&et, graph,
				dglNodeGet_OutEdgeset(graph, node));

	for (edge = dglEdgeset_T_First(&et); edge;
	     edge = dglEdgeset_T_Next(&et)) {
	    dglInt32_t *to = dglEdgeGet_Tail(graph, edge);
	    dglInt32_t to_id = dglNodeGet_Id(graph, to);
	    dglInt32_t d = dglEdgeGet_Cost(graph, edge);

	    if (Nodes[to_id].cost < 0 || Nodes[to_id].cost > dist + d) {
		Nodes[to_id].cost = dist + d;
		Nodes[to_id].edge = dglEdgeGet_Id(graph, edge);
		Nodes[to_id].center = Nodes[v].center;
		heap_data.ul = to_id;
		dglHeapInsertMin(&heap, dist + d, ' ', heap_data);
	    }
	}

	dglEdgeset_T_Release(&et);
    }

    dglHeapFree(&heap, NULL);

    return 0;
}

int alloc_to_centers(dglGraph_s *graph, NODE *Nodes, CENTER *Centers, int ncenters)
{
    int i, nnodes;
    dglHeap_s heap;
    dglEdgesetTraverser_s et;
    int have_node_costs;
    dglInt32_t ncost;

    if (graph->Version < 2) {
	G_warning("Directed graph must be version 2 or 3 for distances to centers");
	return -1;
    }

    nnodes = dglGet_NodeCount(graph);

    /* initialize nodes */
    for (i = 1; i <= nnodes; i++) {
	Nodes[i].cost = -1;
	Nodes[i].center = -1;
	Nodes[i].edge = 0;
    }

    ncost = 0;
    have_node_costs = dglGet_NodeAttrSize(graph);

    dglHeapInit(&heap);

    for (i = 0; i < ncenters; i++) {
	int v = Centers[i].node;

	if (Nodes[v].cost == 0)
	    continue;		/* ignore duplicates */
	Nodes[v].cost = 0;		/* make sure all centers are processed first */
	Nodes[v].center = i;
	dglHeapData_u heap_data;

	heap_data.ul = v;
	dglHeapInsertMin(&heap, 0, ' ', heap_data);
    }
    while (1) {
	dglInt32_t v, dist;
	dglHeapNode_s heap_node;
	dglHeapData_u heap_data;
	dglInt32_t *edge;
	dglInt32_t *node;

	if (!dglHeapExtractMin(&heap, &heap_node))
	    break;
	v = heap_node.value.ul;
	dist = heap_node.key;
	if (Nodes[v].cost < dist)
	    continue;

	node = dglGetNode(graph, v);

	if (have_node_costs && Nodes[v].edge) {
	    memcpy(&ncost, dglNodeGet_Attr(graph, node),
		   sizeof(ncost));
	    if (ncost > 0)
		dist += ncost;
	    /* do not go through closed nodes */
	    if (ncost < 0)
		continue;
	}

	dglEdgeset_T_Initialize(&et, graph,
				dglNodeGet_InEdgeset(graph, node));

	for (edge = dglEdgeset_T_First(&et); edge;
	     edge = dglEdgeset_T_Next(&et)) {
	    dglInt32_t *from = dglEdgeGet_Head(graph, edge);
	    dglInt32_t from_id = dglNodeGet_Id(graph, from);
	    dglInt32_t d = dglEdgeGet_Cost(graph, edge);

	    if (Nodes[from_id].cost < 0 || Nodes[from_id].cost > dist + d) {
		Nodes[from_id].cost = dist + d;
		Nodes[from_id].edge = dglEdgeGet_Id(graph, edge);
		Nodes[from_id].center = Nodes[v].center;
		heap_data.ul = from_id;
		dglHeapInsertMin(&heap, dist + d, ' ', heap_data);
	    }
	}

	dglEdgeset_T_Release(&et);
    }

    dglHeapFree(&heap, NULL);

    return 0;
}