#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <math.h>
#include <grass/gis.h>
#include <grass/vector.h>
#include <grass/dbmi.h>
#include <grass/glocale.h>

/* Result code */
#define SP_OK          0	/* Path found */
#define SP_UNREACHABLE 1	/* Node is not reachable */
#define SP_NOPOINT     2	/* Missing point of given category */

#define INPUT_MODE_NODE  1
#define INPUT_MODE_COOR  2

typedef struct
{				/* category index */
    int cat;			/* line category */
    int line;			/* line number */
} CIDX;

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

int path(struct Map_info *In, struct Map_info *Out, char *filename,
	 int nfield, double maxdist, int segments)
{
    FILE *in_file = NULL;
    int i, nlines, line, npoints, type, cat, id, fcat, tcat, fline, tline,
	fnode, tnode, count;
    int ret, sp, input_mode, unreachable, nopoint, formaterr;
    struct ilist *AList;
    double cost;
    struct line_pnts *Points, *OPoints, *FPoints, *TPoints;
    struct line_cats *Cats;
    CIDX *Cidx, *Citem;
    char buf[2000], dummy[2000];
    double fx, fy, tx, ty;

    /* Attribute table */
    dbString sql;
    dbDriver *driver;
    struct field_info *Fi;

    if (filename) {
	/* open input file */
	if ((in_file = fopen(filename, "r")) == NULL)
	    G_fatal_error(_("Unable to open input file <%s>"), filename);
    }

    AList = Vect_new_list();
    Points = Vect_new_line_struct();
    OPoints = Vect_new_line_struct();
    FPoints = Vect_new_line_struct();
    TPoints = Vect_new_line_struct();
    Cats = Vect_new_cats_struct();
    db_init_string(&sql);

    /* Create category index for input points */
    npoints = Vect_get_num_primitives(In, GV_POINT);
    Cidx = (CIDX *) G_malloc(npoints * sizeof(CIDX));

    nlines = Vect_get_num_lines(In);
    count = 0;
    for (line = 1; line <= nlines; line++) {
	type = Vect_read_line(In, NULL, Cats, line);
	if (type != GV_POINT)
	    continue;

	Vect_cat_get(Cats, nfield, &cat);

	if (cat < 0)
	    continue;

	Cidx[count].cat = cat;
	Cidx[count].line = line;
	count++;
    }

    if (count < npoints)
	G_warning(_("[%d] points without category (nfield: [%d])"),
		  npoints - count, nfield);

    npoints = count;

    qsort((void *)Cidx, npoints, sizeof(CIDX), cmp);

    /* Create table */
    Fi = Vect_default_field_info(Out, 1, NULL, GV_1TABLE);
    Vect_map_add_dblink(Out, 1, NULL, Fi->table, GV_KEY_COLUMN, Fi->database,
			Fi->driver);

    driver = db_start_driver_open_database(Fi->driver, Fi->database);
    if (driver == NULL)
	G_fatal_error(_("Unable to open database <%s> by driver <%s>"),
		      Fi->database, Fi->driver);

    sprintf(buf,
	    "create table %s ( cat integer, id integer, fcat integer, tcat integer, "
	    "sp integer, cost double precision, fdist double precision, tdist double precision )",
	    Fi->table);

    db_set_string(&sql, buf);
    G_debug(2, db_get_string(&sql));

    if (db_execute_immediate(driver, &sql) != DB_OK) {
	db_close_database_shutdown_driver(driver);
	G_fatal_error(_("Unable to create table: '%s'"), db_get_string(&sql));
    }

    if (db_create_index2(driver, Fi->table, GV_KEY_COLUMN) != DB_OK)
	G_warning(_("Cannot create index"));

    if (db_grant_on_table
	(driver, Fi->table, DB_PRIV_SELECT, DB_GROUP | DB_PUBLIC) != DB_OK)
	G_fatal_error(_("Cannot grant privileges on table <%s>"), Fi->table);

    db_begin_transaction(driver);

    /* Read stdin, find shortest path, and write connecting line and new database record */
    cat = 0;
    formaterr = nopoint = unreachable = 0;
    while (1) {

	if (!filename) {
	    if (fgets(buf, sizeof(buf), stdin) == NULL)
		break;
	}
	else {
	    if (G_getl2(buf, sizeof(buf) - 1, in_file) == 0)
		break;
	}


	double fdist, tdist;

	G_chop(buf);

	ret =
	    sscanf(buf, "%d %lf %lf %lf %lf %s", &id, &fx, &fy, &tx, &ty,
		   dummy);
	if (ret == 5) {
	    input_mode = INPUT_MODE_COOR;
	}
	else {
	    ret = sscanf(buf, "%d %d %d %s", &id, &fcat, &tcat, dummy);
	    if (ret != 3) {
		G_warning(_("Wrong input format: %s"), buf);
		formaterr++;
		continue;
	    }
	    input_mode = INPUT_MODE_NODE;
	}

	G_debug(3, "mode = %d", input_mode);

	cat++;			/* Output category */
	sp = SP_OK;
	cost = fdist = tdist = 0;

	if (input_mode == INPUT_MODE_NODE) {
	    /* From */
	    Citem =
		(CIDX *) bsearch((void *)&fcat, Cidx, npoints, sizeof(CIDX),
				 cmp);
	    if (Citem == NULL) {
		G_warning(_("No point with category [%d]"), fcat);
		sp = SP_NOPOINT;
		fline = 0;
		fnode = 0;
		nopoint++;
	    }
	    else {
		fline = Citem->line;
		type = Vect_read_line(In, Points, NULL, fline);
		fnode = Vect_find_node(In, Points->x[0], Points->y[0], Points->z[0], 0, 0);
		/* Vect_get_line_nodes(In, fline, &fnode, NULL); */
	    }
	    G_debug(3, "from: cat = %5d point(line) = %5d node = %5d", fcat,
		    fline, fnode);

	    /* To */
	    Citem =
		(CIDX *) bsearch((void *)&tcat, Cidx, npoints, sizeof(CIDX),
				 cmp);
	    if (Citem == NULL) {
		G_warning(_("No point with category [%d]"), tcat);
		sp = SP_NOPOINT;
		tline = 0;
		tnode = 0;
		nopoint++;
	    }
	    else {
		tline = Citem->line;
		type = Vect_read_line(In, Points, NULL, tline);
		tnode = Vect_find_node(In, Points->x[0], Points->y[0], Points->z[0], 0, 0);
		/* Vect_get_line_nodes(In, tline, &tnode, NULL); */
	    }
	    G_debug(3, "to  : cat = %5d point(line) = %5d node = %5d", tcat,
		    tline, tnode);

	    if (sp != SP_NOPOINT) {
		ret = Vect_net_shortest_path(In, fnode, tnode, AList, &cost);

		if (ret == -1) {
		    sp = SP_UNREACHABLE;
		    unreachable++;
		    G_warning(_("Point with category [%d] is not reachable "
				"from point with category [%d]"), tcat, fcat);
		}
		else {
		    /* Write new line connecting 'from' and 'to' */
		    G_debug(3, "Number of arcs = %d, total costs = %f",
			    AList->n_values, cost);

		    Vect_reset_cats(Cats);
		    Vect_cat_set(Cats, 1, cat);

		    if (segments) {
			for (i = 0; i < AList->n_values; i++) {
			    line = AList->value[i];
			    Vect_read_line(In, Points, NULL, abs(line));

			    if (line > 0) {
				Vect_write_line(Out, GV_LINE, Points, Cats);
			    }
			    else {
				Vect_reset_line(OPoints);
				Vect_append_points(OPoints, Points,
						   GV_BACKWARD);
				Vect_write_line(Out, GV_LINE, OPoints, Cats);
			    }
			}
		    }
		    else {
			Vect_reset_line(OPoints);

			for (i = 0; i < AList->n_values; i++) {
			    line = AList->value[i];
			    Vect_read_line(In, Points, NULL, abs(line));
			    if (line > 0)
				Vect_append_points(OPoints, Points,
						   GV_FORWARD);
			    else
				Vect_append_points(OPoints, Points,
						   GV_BACKWARD);
			}

			Vect_write_line(Out, GV_LINE, OPoints, Cats);
		    }
		}
	    }
	}
	else {			/* INPUT_MODE_COOR */
	    fcat = tcat = 0;
	    ret =
		Vect_net_shortest_path_coor(In, fx, fy, 0.0, tx, ty, 0.0,
					    maxdist, maxdist, &cost, OPoints,
					    AList, FPoints, TPoints, &fdist,
					    &tdist);

	    if (ret == 0) {
		sp = SP_UNREACHABLE;
		unreachable++;
		G_warning(_("Point %f,%f is not reachable from "
			    "point %f,%f"), tx, ty, fx, fy);
	    }
	    else {
		Vect_reset_cats(Cats);
		Vect_cat_set(Cats, 1, cat);

		if (segments) {
		    /* From point to the first node */
		    if (FPoints->n_points > 0)
			Vect_write_line(Out, GV_LINE, FPoints, Cats);

		    /* On the network */
		    for (i = 0; i < AList->n_values; i++) {
			line = AList->value[i];

			Vect_read_line(In, Points, NULL, abs(line));

			if (line > 0) {
			    Vect_write_line(Out, GV_LINE, Points, Cats);
			}
			else {
			    Vect_reset_line(OPoints);
			    Vect_append_points(OPoints, Points, GV_BACKWARD);
			    Vect_write_line(Out, GV_LINE, OPoints, Cats);
			}
		    }

		    /* From last node to point */
		    if (TPoints->n_points > 0)
			Vect_write_line(Out, GV_LINE, TPoints, Cats);

		}
		else {
		    Vect_write_line(Out, GV_LINE, OPoints, Cats);
		}
	    }
	}

	sprintf(buf,
		"insert into %s values ( %d, %d, %d, %d, %d, %f, %f, %f)",
		Fi->table, cat, id, fcat, tcat, sp, cost, fdist, tdist);
	db_set_string(&sql, buf);
	G_debug(3, db_get_string(&sql));

	if (db_execute_immediate(driver, &sql) != DB_OK) {
	    db_close_database_shutdown_driver(driver);
	    G_fatal_error(_("Cannot insert new record: %s"),
			  db_get_string(&sql));
	}
    }

    db_commit_transaction(driver);

    db_close_database_shutdown_driver(driver);

    Vect_destroy_list(AList);
    Vect_destroy_line_struct(Points);
    Vect_destroy_line_struct(OPoints);
    Vect_destroy_cats_struct(Cats);

    if (filename)
	fclose(in_file);

    if (formaterr)
	G_warning(_("[%d] input format errors"), formaterr);
    if (nopoint)
	G_warning(_("[%d] points of given category missing"), nopoint);
    if (unreachable)
	G_warning(_("%d destination(s) unreachable (including points out "
		    "of threshold)"), unreachable);

    return 1;
}

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

    if (p1->cat < p2->cat)
	return -1;
    if (p1->cat > p2->cat)
	return 1;
    return 0;
}
