/*
 *			
 */

/*)LIBRARY
*/

#ifdef DOCUMENTATION

title
index

synopsis

	int

description


bugs

author

	Design aids data services

#endif



/*
 *
 *
 *
 *	G E T L V . C
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 *	LINKAGE:	Build with the following libraries, and object modules:
 *
 *			LIBRARIES -		CMATH	(PML or equivalent)
 *						MVGGEO
 *						MVGFIO
 *						C
 *
 *			OBJECT MODULES -	CFLOAT
 *
 *	PARAMETERS:
 *			getlv(x_point, y_point, search_dist, levels_chan, ax_angle, err_str, mod)
 *                                \       /         |           |             |          \      \_ mode??
 *                            Co-ordinate for       |        levels file   angle in       |
 *                            required level    dist. from      channel   rads. of axis   err. mess
 *                                            Co-ord. to search
 *
 *	NOTE:	The levels file must have been open using 'fopenx(file_name, LEV_LEN)', and the file pointer passe
 *		to getlv();
 *
 */


#include	<stdio.h>

/* #define TESTING */		/* defined when testing is required with a main */
/* #define DEBUG */		/* defined only when debugging */

/*
 *
 *	Define all #define's
 *
 */

#define	HIT_DIST	0.3			/* Max. distance away from co-ord for an acceptable level */
#define	MAX_SECTORS	6			/* Max. No. of sectors being used */
#define	PI		3.1415927		/* PI */
#define	MAX_ANGLE	PI			/* Max. angle between two sectors when discarding a sector */
#define	TITLE_SIZE	22			/* size of a file title */
#define	LEV_LEN		24			/* record length of the levels file */
#define	FOUND		TRUE			/* some return value */


/*
 *
 *	Define all global variables to the file
 *
 */

static	struct	{			/* level record */
		double	x;					/* x co-ord */
		double	y;					/* y co-ord */
		double	z;					/*   level  */
	} lev_rec;

static	struct	{			/* first record in the levels file */
		int	numrecs;				/* No. records in the levels file */
		char	title[TITLE_SIZE];			/* file name */
	} lev_header;

/* sector variables */

static	double	level[MAX_SECTORS], _distance[MAX_SECTORS], _angle[MAX_SECTORS], pt_x[MAX_SECTORS], pt_y[MAX_SECTORS];

/* getlv() parameters */

static	double	xarg, yarg, lev_arg, sch_dst, axis_angle;
static	FILE	*lev_chan;
static	int	mode;
static	char	*err;


#ifdef TESTING

/*
 *
 *	M A I N (  ) - Main test routine. Reads a level file and passes co-ords to getlv()
 *
 *	MOD LOG:
 *			 9-FEB-85	TG	Initial edit.
 *			12-FEB-85	TG	Scanf() -> Atod() for better accuracy in double precision.
 *
 *
 */

main(argc, argv)
int	argc;
char	**argv;
{
	FILE	*lev_file;
	double	atod(), xarg1, yarg1, distnce, angl, lev, getlv();
	char	x_str[80], y_str[80], err_str[80], dis_str[80], mod_str[80], angl_str[80];
	int	mod;

	/* open the levels file (random access with a record length of 24 bytes) - filename in argv[1] */

	if ((lev_file = fopenx(argv[1], 24)) == NULL)
		error("MAIN GETLV: Can't open levels file <%s>\n", argv[1]);

	/* set up parm's */

	fprintf(stderr, "Quadrant size to search -->>");
	gets(dis_str);
	distnce = atod(dis_str);
	printf("distnce <%lf>\n", distnce);

	fprintf(stderr, "Sector angle -->>");
	gets(angl_str);
	angl = atod(angl_str);
	printf("angl <%lf>\n", angl);

	fprintf(stderr, "Mode -->>");
	gets(mod_str);
	mod = atod(mod_str);
	printf("mod <%d>\n", mod);

	/* loop around until EOF on 'stdin' */

	fprintf(stderr, "Co-ordinate [ x ] -->> ");
	gets(x_str);
	fprintf(stderr, "Co-ordinate [ y ] -->> ");
	gets(y_str);

	while ((*x_str) && (*y_str)) {
		xarg1 = atod(x_str);
		yarg1 = atod(y_str);
		printf("xarg1 <%lf> yarg1 <%lf>\n", xarg1, yarg1);
		lev = getlv(xarg1, yarg1, distnce, lev_file, angl, err_str, mod);
		printf("\n\tLevel is <%lf>\n\n", lev);
		fprintf(stderr, "Co-ordinate [ x ] -->> ");
		gets(x_str);
		if ((*x_str) != '\0') {
			fprintf(stderr, "Co-ordinate [ y ] -->> ");
			gets(y_str);
		} else *y_str = '\0';
	};
}

#endif


#ifdef DEBUG

/*
 *
 *	D U M P _ S E C T O R S (  ) - Debug routine for printing out the contents of each sector
 *
 *	MOD LOG:
 *			9-FEB-85	TG	Initial edit.
 *
 */

dump_sectors(str)
char	*str;
{
	int	sec;

	printf("%s\n", str);
	for (sec=0 ; sec < MAX_SECTORS ; sec++)
		printf("%4d  \t%8.2lf\t%8.2lf\t%8.2lf\t%8.2lf\t%8.2lf\t\n",sec,_distance[sec],_angle[sec],level[sec],
				pt_x[sec],pt_y[sec]);
}

#endif


/*
 *	G E T L V (  ) - Get a level from the level file open on channel 'lev_chan'
 *		   	 for the point (xarg , yarg).
 *
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */
	 
double	getlv(x_point, y_point, search_dist, levels_chan, ax_angle, err_str, mod)
double	x_point, y_point, search_dist, ax_angle;
char	*err_str;
int	mod;
FILE	*levels_chan;
{
	int	i;
	double	select_level();

	xarg = x_point;						/* copy arguments to global vars. */
	yarg = y_point;
	sch_dst = search_dist;
	lev_chan = levels_chan;
	axis_angle = ax_angle;
	err = err_str;
	mode = mod;

	init_sectors();						/* init all sector vars. to zero */

	frget(&lev_header, LEV_LEN, lev_chan, 1L);		/* find out how many records there are in the levels file */

#ifdef DEBUG
	printf("GETLV: numrecs <%d>\n", lev_header.numrecs);
#endif

	for (i=2 ; i < lev_header.numrecs ; i++) {		/* read all the levels in the file */
		frget(&lev_rec, LEV_LEN, lev_chan, (long)(i));

	/* store the record if within the search dist.*/

		if ((lev_rec.x >= (xarg - sch_dst)) && (lev_rec.x <= (xarg + sch_dst)) &&
		    (lev_rec.y >= (yarg - sch_dst)) && (lev_rec.y <= (yarg + sch_dst))) {
#ifdef DEBUG
			printf("GETLV: x <%lf> y <%lf> z <%lf>\n", lev_rec.x, lev_rec.y, lev_rec.z);
#endif
			if ((sector(axis_angle)) == FOUND) {	/* found an acceptable level ? */
				return(lev_rec.z);		/* Yes */
			};
		};
	};

#ifdef DEBUG
	dump_sectors("GETLV: ");
	printf("GETLV: Selecting level for point\n");
#endif
	return( select_level() );				/* calculate the required level and return it */
}


/*
 *	I N I T _ S E C T O R S (  ) - Initializes all sectors to zero.
 *
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Initial edit.
 *
 */

init_sectors()
{
	int	i;

	for (i=0 ; i < MAX_SECTORS ; i++)
		_angle[i] = _distance[i] = level[i] = 0.0;
}


/*
 *	S E C T O R (  ) - Place the current point into a sector, if a point all ready exits in that sector
 *			   then choose the closest point to (xarg , yarg), and place it in the sector.
 *
 *			   If the current point is within the 'HIT_DIST' from (xarg , yarg) the place the level
 *			   into 'lev_arg', and return 'FOUND'.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	sector(ax_ang)
double	ax_ang;							/* 'axis_angle' */
{
	/*
		NOTE: ALL ANGLES ARE IN RADIANS.
	*/

	double	ang_gbl, ang_loc, dist, distance(), getangle(), dmod(), dint();
	int	sec_tor;

	ang_gbl = getangle(xarg, yarg, lev_rec.x, lev_rec.y);	/* find out the angle the point is on */
	ang_loc = ang_gbl - ax_ang;				/* find out the angle of the point relative to the 'axis_angle' */

	sec_tor = (int) (dint(ang_loc + 0.5));			/* calc a sector No. */

	sec_tor = (int) (dmod((double)(sec_tor), (double)(MAX_SECTORS)));	/* rationalize 'sec_tor' to be [0 - 5] */
	if (sec_tor < 0)
		sec_tor = sec_tor + 6;
#ifdef DEBUG

	printf("SECTOR: ang_loc <%lf> sector <%d>\n", ang_loc, sec_tor);
#endif
	dist = distance(lev_rec.x, lev_rec.y, xarg, yarg);	/* calc. the distance between the current point, and (xarg , yarg) */

	if (dist < HIT_DIST) {					/* reasonable level found ? */
		lev_arg = lev_rec.z;				/* level found */
		*err = '\0';					/* no error message */
#ifdef DEBUG
		printf("SECTOR: Point is within HIT_DIST\n");
#endif
		return( FOUND );
	};

	/* check to see if we have already stored a point in 'sec_tor', if so then see which one to disregard */

	if ((_distance[sec_tor] == 0) || (_distance[sec_tor] > dist)) {	/* record new sector values ? */
		_distance[sec_tor] = dist;			/* Yes */
		pt_x[sec_tor] = lev_rec.x;
		pt_y[sec_tor] = lev_rec.y;
		level[sec_tor] = lev_rec.z;
		_angle[sec_tor] = ang_loc;
	};

	return( !FOUND );
}


/*
 *	S E L E C T _ L E V E L (  ) - select three levels from diff. sectors.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

double	select_level()
{
	int	sec_tor, closest, second, third, n;
	double	solve_for_level();

	for (sec_tor = 0 ; sec_tor < MAX_SECTORS ; sec_tor++) {	/* lock out all sectors with nothing interesting in them */
		if (_distance[sec_tor] == 0.0)
			_distance[sec_tor] = -1.0;
	};

	while ((n = numleft()) > 3) {				/* throw away all but the best 3 points */
#ifdef DEBUG
		printf("SELECT_LEVEL: num_left <%d>\n", n);
#endif
		discard();
	};
#ifdef DEBUG
	dump_sectors("SELECT_LEVEL: ");
#endif

	closest = next();					/* find the closest point */
	second = next();					/* find the second closest point */
	third = next();						/* find the third closest point */
#ifdef DEBUG
	printf("SELECT_LEVEL: closest <%d> second <%d> third <%d>\n", closest, second, third);
#endif
	return( solve_for_level(closest, second, third) );
}


/*
 *	 N U M L E F T (  ) - Return the number of points not already locked out.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	numleft()
{
	int	i, count;

	count = 0;

	for (i=0 ; i < MAX_SECTORS ; i++) {
		if (_distance[i] > -1.0)				/* locked out ? */
			count++;				/* No - then count it laddy */
	};
	return(count);
}



/*
 *	D I S C A R D (  ) - Mark the point furthest away from (xarg , yarg) as locked out if the angle between the prev.
 *			     unlocked sector, and the next unlocked sector is less than 'MAX_ANGLE'.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

discard()
{
	int	k, furthest_left;
	double	dst,d;

	dst = 0.0;

	for (k=0 ; k < MAX_SECTORS ; k++) {
		d = _distance[k];				/* used to cut down on line length only */
		if ((d > dst) && (d > 0.0)) {			/* further than prev. point ? */
			furthest_left = k;			/* Yes - then mark it as such */
			dst = d;
		};
	};

	if (angle_ok(furthest_left) == TRUE)			/* can we discard this sector ? */
		_distance[furthest_left] = -2.0;			/* Yes */
	else
		_distance[furthest_left] = 0.0;			/* No */
}


/*
 *	A N G L E _ O K (  ) - Return TRUE if the angle between the prev. unlocked sector, and the next 
 *			       unlocked sector is less than 'MAX_ANGLE'.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	angle_ok(sec_tor)
int	sec_tor;
{
	int	ok, prev, nxt;
	double	arcangle();

	ok = TRUE;						/* init. the return value to be TRUE */

	prev = prev_1(sec_tor);					/* find the prev. unlocked sector */
	nxt = next_1(sec_tor);					/* find the  next unlocked sector */
#ifdef DEBUG
	printf("ANGLE_OK: prev <%d> nxt <%d>\n", prev, nxt);
#endif
	if ((arcangle(_angle[prev], _angle[nxt], 1)) > MAX_ANGLE) {	/* angle too great ? */
#ifdef DEBUG
		printf("ANGLE_OK: angle too great\n");
#endif
		ok = FALSE;					/* Yes */
	};

	return(ok);
}


/*
 *	P R E V _ 1 (  ) - Return the sector No. of the prev. unlocked sector.
 *
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	prev_1(sec_tor)
int	sec_tor;
{
	int	k, prev_one;

	prev_one = -1;						/* init. 'prev_one' so that we know if we can't find a prev one */

	k = sec_tor;						/* init k */
	if (--k < 0)						/* rationalize k to [0 -> 5] */
		k = 5;

	while (k != sec_tor) {					/* loop around until we find our selves again */
		if (_distance[k] >= 0.0) {			/* have we found a prev. one ? */
			prev_one = k;				/* yes - then bomb out of the loop */
			break;
		};
		if (--k < 0)					/* rationalize k again */
			k = 5;
	};

	if (prev_one == -1)
		error("PREV_1: Less than three points left. Sec_tor <%d> - Call a programmer\7\n", sec_tor);

	return(prev_one);
}


/*
 *	N E X T _ 1 (  )- Return the sector No. of the prev. unlocked sector.
 *
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	next_1(sec_tor)
int	sec_tor;
{
	int	k, next_one;

	next_one = -1;						/* init. 'next_one' so that we know if we can't find a next one */

	k = sec_tor;						/* init k */
	if (++k > 5)						/* rationalize k to [0 -> 5] */
		k = 0;

	while (k != sec_tor) {					/* loop around until we find our selves again */
		if (_distance[k] >= 0.0) {			/* have we found a next one ? */
			next_one = k;				/* yes - then bomb out of the loop */
			break;
		};
		if (++k > 5)					/* rationalize k again */
			k = 0;
	};

	if (next_one == -1)
		error("NEXT_1: Less than three points left. Sec_tor <%d> - Call a programmer\7\n", sec_tor);

	return(next_one);
}


/*
 *	N E X T (  ) - Return the closest point to the origin, and mark it as locked out so the next closest can be found
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

int	next()
{
	double	dst,d;
	int	k, closest_1;

	dst = sch_dst * 2.0;					/* record the max distance away the point may be from (xarg , yarg) */

	closest_1 = -1;						/* so we know if we found one or not */

	for (k=0 ; k < MAX_SECTORS ; k++) {
		d = _distance[k];				/* used to shorten the code only */
		if ((d < dst) && (d > 0)) {			/* closer than the prev. pt ? */
			closest_1 = k;				/* Yes - Then record it as such */
			dst = d;
		};
	};

	if (closest_1 == -1) {					/* did we find a closest point ? */
#ifdef DEBUG
		printf("NEXT: Finding a zero\n");
#endif
		for (k=0 ; k < MAX_SECTORS ; k++) {		/* No - then find the first point marked as a distance of zero */
			if (_distance[k] == 0.0) {
				closest_1 = k;
				break;
			};
		};
	};

	if (closest_1 == -1)
		error("NEXT: Can't find a next sector - Call a programmer\7\n");

	_distance[closest_1] = -1.0;				/* mark the closest distance as locked out so we can find the next closest pt next time */

	return(closest_1);
}


/*
 *	S O L V E _ F O R _ L E V E L (  ) - Calculate the level for (xarg , yarg) using 'sec1', 'sec2', 'sec3' sectors.
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

double	solve_for_level(sec1, sec2, sec3)
int	sec1, sec2, sec3;
{
	double	x4, y4, z4, interpolate_level();

/* find the intersection of (pt_x1 , pt_y1) -> (xarg , yarg) , (pt_x2 , pt_y2) -> (pt_x3 , pt_y3) */

	intersect(&x4, &y4, pt_x[sec1], pt_y[sec1], xarg, yarg, pt_x[sec2], pt_y[sec2], pt_x[sec3], pt_y[sec3]);

#ifdef DEBUG
	printf("SOLVE_FOR_LEVEL: intersection - x4 <%lf> y4 <%lf>\n", x4, y4);
#endif

/* find a level for (x4 , y4) */

	z4 = interpolate_level(x4, y4, pt_x[sec2], pt_y[sec2], level[sec2], pt_x[sec3], pt_y[sec3], level[sec3]);

#ifdef DEBUG
	printf("SOLVE_FOR_LEVEL: level z4 <%lf>\n", z4);
#endif

/* find a level for (xarg , yarg) */

	lev_arg = interpolate_level(xarg, yarg, pt_x[sec1], pt_y[sec1], level[sec1], x4, y4, z4);

	return(lev_arg);
}


/*
 *	I N T E R P O L A T E _ L E V E L (  ) - Return a level for (x3 , y3).
 *
 *	MOD LOG:
 *			9-FEB-85 TG	Conversion from B4S.
 *
 */

double	interpolate_level(x3, y3, x1, y1, z1, x2, y2, z2)
double	x3, y3, x1, y1, z1, x2, y2, z2;
{
	double	dist12, dist13, distance();

	dist13 = distance(x3, y3, x1, y1);			/* distance between (x1 , y1), and (x3 , y3) */
	dist12 = distance(x2, y2, x1, y1);			/* distance between (x1 , y1), and (x2 , y2) */

	return(z1 + (z2 - z1) * dist13 / dist12);
}