/*
* $Id: gs_query.c,v 2.1 2006/02/09 03:08:57 glynn Exp $
*/

/*  gs_query.c
    Bill Brown, USACERL  
    January 1994
*/

#include <math.h>
#include <stdio.h>

#include <grass/gstypes.h>

/* Values needed for Ray-Convex Polyhedron Intersection Test below
 * originally by Eric Haines, erich@eye.com
 */
#ifndef	HUGE_VAL
#define	HUGE_VAL	1.7976931348623157e+308
#endif

/* return codes */
#define	MISSED		 0
#define	FRONTFACE	 1
#define	BACKFACE	-1
/* end Ray-Convex Polyhedron Intersection Test values */

/***********************************************************************/
/* Crude method of intersecting line of sight with closest part of surface. 
   Uses los vector to determine the point of first intersection
   which is returned in point. Returns 0 if los doesn't intersect.  */
/* los should ALREADY be in surf-world coordinates */
int gs_los_intersect1(int surfid, float (*los)[3], float *point)
{
    float dx, dy, dz, u_d[3];
    float a[3], incr, min_incr, tlen, len;
    int outside, above, below, edge, istep;
    float b[3];
    geosurf *gs;
    typbuff *buf;

    if (NULL == (gs = gs_get_surf(surfid))) {
	return (0);
    }

    if (0 == GS_v3dir(los[FROM], los[TO], u_d)) {
	return (0);
    }

    buf = gs_get_att_typbuff(gs, ATT_TOPO, 0);

    istep = edge = below = 0;

    len = 0.0;
    tlen = GS_distance(los[FROM], los[TO]);

    incr = tlen / 1000.0;
    min_incr = incr / 1000.0;

    dx = incr * u_d[X];
    dy = incr * u_d[Y];
    dz = incr * u_d[Z];

    a[X] = los[FROM][X];
    a[Y] = los[FROM][Y];
    a[Z] = los[FROM][Z];

    b[X] = a[X] - gs->x_trans;
    b[Y] = a[Y] - gs->y_trans;

    if (viewcell_tri_interp(gs, buf, b, 0)) {
	/* expects surface coords */
	b[Z] += gs->z_trans;

	if (a[Z] < b[Z]) {
	    /*  viewing from below surface  */
	    /*    don't use this method 
	       fprintf(stderr,"view from below\n");
	       below = 1;
	     */

	    return (0);
	}
    }

    while (incr > min_incr) {
	outside = 0;
	above = 0;
	b[X] = a[X] - gs->x_trans;
	b[Y] = a[Y] - gs->y_trans;

	if (viewcell_tri_interp(gs, buf, b, 0)) {
	    /* ignores masks */
	    b[Z] += gs->z_trans;
	    above = (a[Z] > b[Z]);
	}
	else {
	    outside = 1;

	    if (istep > 10) {
		edge = 1;
		below = 1;
	    }
	}

	while (outside || above) {
	    a[X] += dx;
	    a[Y] += dy;
	    a[Z] += dz;
	    len += incr;
	    outside = 0;
	    above = 0;
	    b[X] = a[X] - gs->x_trans;
	    b[Y] = a[Y] - gs->y_trans;

	    if (viewcell_tri_interp(gs, buf, b, 0)) {
		b[Z] += gs->z_trans;
		above = (a[Z] > b[Z]);
	    }
	    else {
		outside = 1;
	    }

	    if (len > tlen) {
		return 0;	/* over surface *//* under surface */
	    }
	}

	/* could look for spikes here - see if any data points along 
	   shadow of line on surf go above los */

	/* back up several spots? */
	a[X] -= (1.0 * dx);
	a[Y] -= (1.0 * dy);
	a[Z] -= (1.0 * dz);
	incr /= 2.0;
	++istep;
	dx = incr * u_d[X];
	dy = incr * u_d[Y];
	dz = incr * u_d[Z];
    }

    if ((edge) && (b[Z] - (a[Z] + dz * 2.0) > incr * u_d[Z])) {
	fprintf(stderr, "looking under surface\n");

	return 0;
    }

    point[X] = b[X];
    point[Y] = b[Y];
    point[Z] = b[Z] - gs->z_trans;

    return (1);
}

/* 
 * This version uses the shadow of the los projected down to
 * the surface to generate a line_on_surf, then follows each
 * point in that line until the los intersects it.
*/
int gs_los_intersect(int surfid, float **los, float *point)
{
    double incr;
    float p1, p2, u_d[3];
    int above, ret, num, i, usedx;
    float a[3], b[3];
    float bgn[3], end[3], a1[3];
    geosurf *gs;
    typbuff *buf;
    Point3 *points;

    if (NULL == (gs = gs_get_surf(surfid))) {
	return (0);
    }

    if (0 == GS_v3dir(los[FROM], los[TO], u_d)) {
	return (0);
    }

    buf = gs_get_att_typbuff(gs, ATT_TOPO, 0);

    GS_v3eq(bgn, los[FROM]);
    GS_v3eq(end, los[TO]);

    bgn[X] -= gs->x_trans;
    bgn[Y] -= gs->y_trans;

    end[X] -= gs->x_trans;
    end[Y] -= gs->y_trans;

    /* trans? */
    points = gsdrape_get_allsegments(gs, bgn, end, &num);

/* DEBUG
{
float t1[3], t2[3];

t1[X] = los[FROM][X] ;
t1[Y] = los[FROM][Y] ;

t2[X] = los[TO][X] ;
t2[Y] = los[TO][Y] ;

GS_set_draw(GSD_FRONT);
gsd_pushmatrix();
gsd_do_scale(1);
gsd_translate(gs->x_trans, gs->y_trans, gs->z_trans);
gsd_linewidth(1);
gsd_color_func(GS_default_draw_color());
gsd_line_onsurf(gs, t1, t2);
gsd_popmatrix();
GS_set_draw(GSD_BACK);
gsd_flush();
}
fprintf(stderr,"%d points to check\n", num);
fprintf(stderr,"point0 = %.6lf %.6lf %.6lf FT =%.6lf %.6lf %.6lf\n",
	points[0][X],points[0][Y],points[0][Z],
	los[FROM][X],los[FROM][Y],los[FROM][Z]);
fprintf(stderr,"incr1 = %.6lf: %.6lf %.6lf %.6lf\n",incr,u_d[X],u_d[Y],u_d[Z]);
fprintf(stderr,"first point below surf\n");
fprintf(stderr,"incr2 = %f\n", (float)incr);
fprintf(stderr,"(%d/%d) %f > %f\n", i,num, a[Z], points[i][Z]);
fprintf(stderr,"incr3 = %f\n", (float)incr);
fprintf(stderr,"all points above surf\n");
*/

    if (num < 2) {
	fprintf(stderr, "-----------------------------\n");
	fprintf(stderr, "%d points to check\n", num);

	return (0);
    }

    /* use larger of deltas for better precision */
    usedx = (fabs(u_d[X]) > fabs(u_d[Y]));
    if (usedx) {
	incr = ((points[0][X] - (los[FROM][X] - gs->x_trans)) / u_d[X]);
    }
    else if (u_d[Y]) {
	incr = ((points[0][Y] - (los[FROM][Y] - gs->y_trans)) / u_d[Y]);
    }
    else {
	point[X] = los[FROM][X] - gs->x_trans;
	point[Y] = los[FROM][Y] - gs->y_trans;

	return (viewcell_tri_interp(gs, buf, point, 1));
    }

/* DEBUG
fprintf(stderr,"-----------------------------\n");
fprintf(stderr,"%d points to check\n", num);
fprintf(stderr,"incr1 = %.6lf: %.9f %.9f %.9f\n",incr,u_d[X],u_d[Y],u_d[Z]);
fprintf(stderr,
"\tpoint0 = %.6f %.6f %.6f\n\tFT = %.6f %.6f %.6f\n\tpoint%d = %.6f %.6f\n",
	points[0][X],points[0][Y],points[0][Z],
	los[FROM][X],los[FROM][Y],los[FROM][Z],
	num-1, points[num-1][X],points[num-1][Y]);
*/

    /* This should bring us right above (or below) the first point */
    a[X] = los[FROM][X] + incr * u_d[X] - gs->x_trans;
    a[Y] = los[FROM][Y] + incr * u_d[Y] - gs->y_trans;
    a[Z] = los[FROM][Z] + incr * u_d[Z] - gs->z_trans;

    if (a[Z] < points[0][Z]) {
	/*  viewing from below surface  */
	/*  don't use this method */
/* DEBUG
fprintf(stderr,"first point below surf\n");
fprintf(stderr,"aZ= %.6f point0 = %.6f %.6f %.6f FT =%.6f %.6f %.6f\n",
	a[Z], points[0][X],points[0][Y],points[0][Z],
	los[FROM][X],los[FROM][Y],los[FROM][Z]);
*/
	return (0);
    }

    GS_v3eq(a1, a);
    GS_v3eq(b, a);

    for (i = 1; i < num; i++) {
	if (usedx) {
	    incr = ((points[i][X] - a1[X]) / u_d[X]);
	}
	else {
	    incr = ((points[i][Y] - a1[Y]) / u_d[Y]);
	}

	a[X] = a1[X] + (incr * u_d[X]);
	a[Y] = a1[Y] + (incr * u_d[Y]);
	a[Z] = a1[Z] + (incr * u_d[Z]);
	above = (a[Z] >= points[i][Z]);

	if (above) {
	    GS_v3eq(b, a);
	    continue;
	}

	/* 
	 * Now we know b[Z] is above points[i-1] 
	 * and a[Z] is below points[i]
	 * Since there should only be one polygon along this seg,
	 * just interpolate to intersect 
	 */

	if (usedx) {
	    incr = ((a[X] - b[X]) / u_d[X]);
	}
	else {
	    incr = ((a[Y] - b[Y]) / u_d[Y]);
	}

	if (1 == (ret = segs_intersect(1.0, points[i][Z],
				       0.0, points[i - 1][Z],
				       1.0, a[Z], 0.0, b[Z], &p1, &p2))) {
	    point[X] = points[i - 1][X] + (u_d[X] * incr * p1);
	    point[Y] = points[i - 1][Y] + (u_d[Y] * incr * p1);
	    point[Z] = p2;

	    return (1);
	}

	fprintf(stderr, "line of sight error %d\n", ret);

	return 0;
    }

    /* over surface */
    return 0;
}

/***********************************************************************/
/* Ray-Convex Polyhedron Intersection Test 
 * originally by Eric Haines, erich@eye.com
 *
 * This test checks the ray against each face of a polyhedron, checking whether
 * the set of intersection points found for each ray-plane intersection
 * overlaps the previous intersection results.  If there is no overlap (i.e.
 * no line segment along the ray that is inside the polyhedron), then the
 * ray misses and returns 0; else 1 is returned if the ray is entering the
 * polyhedron, -1 if the ray originates inside the polyhedron.  If there is
 * an intersection, the distance and the nunber of the face hit is returned.
 *
 *  org, dir	origin and direction of ray 
 *  tmax    	maximum useful distance along ray
 *  phdrn[] 	list of planes in convex polyhedron
 *  ph_num  	number of planes in convex polyhedron
 * *tresult 	returned: distance of intersection along ray
 * *pn	    	returned: number of face hit (0 to ph_num-1)
 *
 */
int RayCvxPolyhedronInt(Point3 org, Point3 dir, double tmax, Point4 * phdrn,
			int ph_num, double *tresult, int *pn)
{
    double tnear, tfar, t, vn, vd;
    int fnorm_num, bnorm_num;	/* front/back face # hit */

    tnear = -HUGE_VAL;
    tfar = tmax;

    /* Test each plane in polyhedron */
    for (; ph_num--;) {
	/* Compute intersection point T and sidedness */
	vd = DOT3(dir, phdrn[ph_num]);
	vn = DOT3(org, phdrn[ph_num]) + phdrn[ph_num][W];

	if (vd == 0.0) {
	    /* ray is parallel to plane - check if ray origin is inside plane's
	       half-space */
	    if (vn > 0.0) {
		/* ray origin is outside half-space */
		return (MISSED);
	    }
	}
	else {
	    /* ray not parallel - get distance to plane */
	    t = -vn / vd;

	    if (vd < 0.0) {
		/* front face - T is a near point */
		if (t > tfar) {
		    return (MISSED);
		}

		if (t > tnear) {
		    /* hit near face, update normal */
		    fnorm_num = ph_num;
		    tnear = t;
		}
	    }
	    else {
		/* back face - T is a far point */
		if (t < tnear) {
		    return (MISSED);
		}

		if (t < tfar) {
		    /* hit far face, update normal */
		    bnorm_num = ph_num;
		    tfar = t;
		}
	    }
	}
    }

    /* survived all tests */
    /* Note: if ray originates on polyhedron, may want to change 0.0 to some
     * epsilon to avoid intersecting the originating face.
     */
    if (tnear >= 0.0) {
	/* outside, hitting front face */
	*tresult = tnear;
	*pn = fnorm_num;

	return (FRONTFACE);
    }
    else {
	if (tfar < tmax) {
	    /* inside, hitting back face */
	    *tresult = tfar;
	    *pn = bnorm_num;

	    return (BACKFACE);
	}
	else {
	    /* inside, but back face beyond tmax */
	    return (MISSED);
	}
    }
}

/***********************************************************************/
void gs_get_databounds_planes(Point4 * planes)
{
    float n, s, w, e, b, t;
    Point3 tlfront, brback;

    GS_get_zrange(&b, &t, 0);
    gs_get_xrange(&w, &e);
    gs_get_yrange(&s, &n);

    tlfront[X] = tlfront[Y] = 0.0;
    tlfront[Z] = t;

    brback[X] = e - w;
    brback[Y] = n - s;
    brback[Z] = b;

    /* top */
    planes[0][X] = planes[0][Y] = 0.0;
    planes[0][Z] = 1.0;
    planes[0][W] = -(DOT3(planes[0], tlfront));

    /* bottom */
    planes[1][X] = planes[1][Y] = 0.0;
    planes[1][Z] = -1.0;
    planes[1][W] = -(DOT3(planes[1], brback));

    /* left */
    planes[2][Y] = planes[2][Z] = 0.0;
    planes[2][X] = -1.0;
    planes[2][W] = -(DOT3(planes[2], tlfront));

    /* right */
    planes[3][Y] = planes[3][Z] = 0.0;
    planes[3][X] = 1.0;
    planes[3][W] = -(DOT3(planes[3], brback));

    /* front */
    planes[4][X] = planes[4][Z] = 0.0;
    planes[4][Y] = -1.0;
    planes[4][W] = -(DOT3(planes[4], tlfront));

    /* back */
    planes[5][X] = planes[5][Z] = 0.0;
    planes[5][Y] = 1.0;
    planes[5][W] = -(DOT3(planes[5], brback));

    return;
}

/***********************************************************************/
/* Gets all current cutting planes & data bounding planes, intersects 
   los with resulting convex polyhedron, then replaces los[FROM] with
   first point on ray inside data.
*/
int gs_setlos_enterdata(Point3 * los)
{
    Point4 planes[12];		/* MAX_CPLANES + 6  - should define this */
    Point3 dir;
    double dist, maxdist;
    int num, ret, retp;		/* might want to tell if retp is a clipping plane */

    gs_get_databounds_planes(planes);
    num = gsd_get_cplanes(planes + 6);
    GS_v3dir(los[FROM], los[TO], dir);
    maxdist = GS_distance(los[FROM], los[TO]);

    ret = RayCvxPolyhedronInt(los[0], dir, maxdist,
			      planes, num + 6, &dist, &retp);

    if (ret == MISSED) {
	return (0);
    }

    if (ret == FRONTFACE) {
	GS_v3mult(dir, (float) dist);
	GS_v3add(los[FROM], dir);
    }

    return (1);
}

/***********************************************************************/
/* DEBUG ****
void pr_plane(int pnum)
{
    switch(pnum)
    {
	case 0:
	    fprintf(stderr,"top plane");
	
	    break;
	case 1:
	    fprintf(stderr,"bottom plane");
	
	    break;
	case 2:
	    fprintf(stderr,"left plane");
	
	    break;
	case 3:
	    fprintf(stderr,"right plane");
	
	    break;
	case 4:
	    fprintf(stderr,"front plane");
	
	    break;
	case 5:
	    fprintf(stderr,"back plane");
	
	    break;
	default:
	    fprintf(stderr,"clipping plane %d", 6 - pnum);
	
	    break;
    }
    
    return;
}
********/