/**
 * \file plus_node.c
 *
 * \brief Vector library - update topo for nodes (lower level functions)
 *
 * Lower level functions for reading/writing/manipulating vectors.
 *
 * This program is free software under the GNU General Public License
 * (>=v2). Read the file COPYING that comes with GRASS for details.
 *
 * \author CERL (probably Dave Gerdes), Radim Blazek
 *
 * \date 2001-2006
 */

#include <stdlib.h>
#include <math.h>
#include <grass/Vect.h>
#include <grass/glocale.h>

static double dist_squared(double, double, double, double);

/*!
 * \brief Add line info to node
 *
 * Line will be negative if END node
 *
 * 'node' must of course already exist space will be alloced to add 'line' to array
 *
 * \param[in] plus pointer to Plus_head structure
 * \param[in] nodeid node id
 * \param[in] lineid line id
 * \param[in] points line geometry
 * \param[in] type line type 
 *
 * \return -1 on error      
 * \return 0 line not added (degenerate)
 * \return new number of lines in node 
 */
int
dig_node_add_line(struct Plus_head *plus, int nodeid, int lineid,
		  struct line_pnts *points, int type)
{
    register int i, j, nlines;
    float angle;
    int ret;
    P_NODE *node;

    G_debug(3, "dig_node_add_line(): node = %d line = %d", nodeid, lineid);

    node = plus->Node[nodeid];
    nlines = node->n_lines;

    /* reallocate memory */
    ret = dig_node_alloc_line(node, 1);
    if (ret == -1)
	return -1;

    if (type & GV_LINES) {
	if (lineid < 0)
	    angle = dig_calc_end_angle(points, 0);
	else
	    angle = dig_calc_begin_angle(points, 0);
    }
    else {
	angle = -9.;
    }
    G_debug(3, "    angle = %f", angle);

    /* make sure the new angle is less than the empty space at end */
    node->angles[nlines] = 999.;

    for (i = 0; i <= nlines; i++) {	/* alloced for 1 more */
	if (angle < node->angles[i]) {
	    /* make room for insertion */
	    for (j = nlines - 1; j >= i; j--) {
		node->angles[j + 1] = node->angles[j];
		node->lines[j + 1] = node->lines[j];
	    }
	    node->angles[i] = angle;
	    node->lines[i] = lineid;
	    break;
	}
    }

    node->n_lines++;

    G_debug(3,
	    "dig_node_add_line(): line %d added position %d n_lines: %d angle %f",
	    lineid, i, node->n_lines, angle);

    return ((int)node->n_lines);
}


/*!
 * \brief Add new node to plus structure 
 *
 * \param[in] plus pointer to Plus_head structure
 * \param[in] x,y,z coordinates
 *
 * \return -1 on error      
 * \return number of node
 */
int dig_add_node(struct Plus_head *plus, double x, double y, double z)
{
    int nnum;
    P_NODE *node;

    /* First look if we have space in array of pointers to nodes
     *  and reallocate if necessary */
    G_debug(3, "dig_add_node(): n_nodes = %d, alloc_nodes = %d",
	    plus->n_nodes, plus->alloc_nodes);
    if (plus->n_nodes >= plus->alloc_nodes) {	/* array is full */
	if (dig_alloc_nodes(plus, 1000) == -1)
	    return -1;
    }

    /* allocate node structure */
    nnum = plus->n_nodes + 1;

    plus->Node[nnum] = dig_alloc_node();

    node = plus->Node[nnum];
    node->x = x;
    node->y = y;
    node->z = z;

    dig_spidx_add_node(plus, nnum, x, y, z);

    plus->n_nodes++;

    G_debug(3, "new node = %d, n_nodes = %d, alloc_nodes = %d", nnum,
	    plus->n_nodes, plus->alloc_nodes);

    return (nnum);
}

/*!
 * \brief Return actual index into node arrays of the first set of matching coordinates
 *
 * \param[in] plus pointer to Plus_head structure
 * \param[in] x,y coordinates
 * \param[in] thresh threshold value
 *
 * \return node index
 * \return -1 if no node found
 */
int dig_which_node(struct Plus_head *plus, double x, double y, double thresh)
{
    register int i;
    register int first_time;
    register int have_match;
    int winner;
    double least_dist, dist;
    P_NODE *node;

    first_time = 1;
    have_match = 0;
    winner = 0;
    least_dist = 0.0;
    for (i = 1; i <= plus->n_nodes; i++) {
	if (plus->Node[i] == NULL)
	    continue;

	node = plus->Node[i];
	if ((fabs(node->x - x) <= thresh) && (fabs(node->y - y) <= thresh)) {
	    dist = dist_squared(x, y, node->x, node->y);
	    if (first_time) {
		least_dist = dist;
		first_time = 0;
		winner = i;
		have_match = 1;
	    }
	    if (dist < least_dist) {
		least_dist = dist;
		winner = i;
	    }
	}
    }

    if (!have_match)
	return (-1);

    return (winner);
}				/*  which_node ()  */

/*!
 * \brief Return line angle
 *
 * Lines is specified by line id in topology, NOT by order number.
 * Negative id if looking for line end point.
 *
 * \param[in] plus pointer to Plus_head structure
 * \param[in] nodeid node id
 * \param[in] lineid line id
 *
 * \return line angle <-PI,PI>
 * \return 0 not reached
 */
float dig_node_line_angle(struct Plus_head *plus, int nodeid, int lineid)
{
    int i, nlines;
    P_NODE *node;

    G_debug(3, "dig_node_line_angle: node = %d line = %d", nodeid, lineid);

    node = plus->Node[nodeid];
    nlines = node->n_lines;

    for (i = 0; i < nlines; i++) {
	if (node->lines[i] == lineid)
	    return (node->angles[i]);
    }

    G_fatal_error(_("Attempt to read line angle for the line which is not connected to the node: "
		   "node %d, line %d"), nodeid, lineid);

    return 0.0;			/* not reached */
}

static double dist_squared(double x1, double y1, double x2, double y2)
{
    double dx, dy;

    dx = x1 - x2;
    dy = y1 - y2;
    return (dx * dx + dy * dy);
}