/**---------------------------------------------------------------------------
 ** 
 ** nav_hdg.c -- 
 ** 
 ** Author          : Pierre Jaccard
 ** Created On      : 1999/08/03 23:38:13
 ** Last Modified By: Pierre Jaccard
 ** Last Modified On: 1999/08/03 23:54:51
 ** Update Count    : 5
 ** Directory       : /home/pego/pcd1/codas3c/gfi/src/apps/nav/
 ** Version         : 0.0
 ** Status          : Unknown
 ** ---------------------------------------------------------------------- ** 
 ** DESCRIPTION: 
 ** 
 **    Undocumented.
 ** 
 ** ---------------------------------------------------------------------- ** 
 ** REVISIONS: 
 ** ---------------------------------------------------------------------- ** 
 ** CHANGES: 
 **------------------------------------------------------------------------**/

/* ##################################################################

	-Introduction:

	Programme [[nav_hdg]] parses through navigation files and generates an ascii
	file with ADCP times and gyro-heading corrections (these are defined as
	gyro_hdg minus gps_hdg). Additionnal columns are the heading value from the
	database and the gps heading used for the corrections.

	##################################################################
*/

#include "nav_hdg.h"

/* ------------------------------------------------------------------
	 -Init Options:
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int init_options(NAV_HDG_TYPE *rec)
#else

#endif
{

	rec->fps.lst = NULL;
	rec->fps.out = NULL;
	nav_lines_bits = 0x0;

	return(0);

}

/* ------------------------------------------------------------------
	 -Init nav_hdg:
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int init_nav_hdg(NAV_HDG_TYPE *rec, int year_base)
#else

#endif
{

	YMDHMS_TIME_TYPE time;

	int db_id = 1;
	int mem = 0;
	int acc = 0;
	int ierr = 0;
	
	/* Open the files */
	rec->fps.lst = check_fopen((char *) nav_list, "r");
	rec->fps.out = check_fopen((char *) out_file, "w");

	/* Opens the database */
	DBOPEN(&db_id, (char *) dbname, &acc, &mem, &ierr);
	if(DBERROR(&ierr, "DBOPEN"))
		return(1);

	/* Initialize options bits, by default ALL is selected */
	if(!nav_lines_bits)
		nav_lines_bits |= ALL_NAV_MESSAGES_BIT;

	/* Initialize file information structures */
	init_file_info(NULL, &(rec->fi), "r");
	init_file_info(rec->fps.lst, &(rec->fi), "r");

	/* The navigation time must be initialized earlier than the first navigation
		 time. This is because GGA messages do not contain day information. Hence,
		 day changes are detected if the new GGA time is smaller than the previous
		 navigation time (see function `read_gga_line' in file `misc_nav.c' for
		 more information). However, in order to keep day information from the
		 `first_time' parameter in the control file, this initial time must be on
		 the same day as the first navigation time. 

		 Hence, initialize `rec.nav.yd' to `first_time', and then initialize it to
		 the beginning of the specified day. */
	yd_to_ymdhmh_time(rec->nav.yd, year_base, &time);
	time.hour = time.minute = time.second = 0;
	rec->nav.yd = year_day(&time, year_base);

	return(0);
}

/* ------------------------------------------------------------------
	 -Get Profile Time:

	 Get the time of current profile in the database.

	 Return: 0 if successfull
	         1 on error
					-1 if end of database ahs been reached	 
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int get_profile_time(double *yd, int year_base)
#else

#endif
{

	YMDHMS_TIME_TYPE prf_time;
	int ierr;
  unsigned int n;

	/* Get curret progile time */
	n = sizeof(YMDHMS_TIME_TYPE);
	ierr = dbget_cnf(TIME, (char *) &prf_time, &n, "Getting profile time");
	if(ierr){
		if(ierr == SEARCH_BEYOND_END)
			return(-1);
		return(1);
	}

	/* Conversion into decimal day */
	*yd = year_day(&prf_time, year_base);
	
	return(0);
}

/* ------------------------------------------------------------------
	 -Next Profile Time:

	 Move forward one profile in the database and update the new profile time. 

	 Return: 0 if successfull
	         1 on error
					-1 if end of database ahs been reached	 
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int next_profile_time(double *yd, int year_base)
#else

#endif
{

	int steps=1, error;

	/* Move to next profile */
	DBMOVE(&steps, &error);
	if(error == SEARCH_BEYOND_END)
		return(-1);
	if(DBERROR(&error, NULL)){
		return(1);
	}
	/* Get profile time */
	error = get_profile_time(yd, year_base);
	if(test_error(error, "Getting profile time"))
		return(error);

	return(0);

}

/* ------------------------------------------------------------------
	 -Get Profile Heading:

	 Get the heading from current profile. Heading is read from the database
	 according to the setting of `db_hdg_from'. If no valid heading is found, or
	 the variable `db_hdg_from' is not correctly set, the heading value is set
	 to `BADDOUBLE'.

	 Return: 0 if successfull
	         1 on error
					-1 if end of database ahs been reached
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int get_profile_heading(double *hdg)
#else

#endif
{

	ANCILLARY_1_TYPE anc1;
	ANCILLARY_2_TYPE anc2;

	int ierr;
  unsigned int n;

	/* Initializations */
	*hdg = BADDOUBLE;

	/* Get heading value */
	if(db_hdg_from == DB_MEAN_HEADING_CODE){
		n = sizeof(ANCILLARY_1_TYPE);
		ierr = dbget_cnf(ANCILLARY_1, (char *) &anc1, &n, 
										 "Getting ANCILLARY_1 structure");
		if(!ierr)
			*hdg = anc1.mn_heading;
	}
	if(db_hdg_from == DB_LAST_HEADING_CODE){
		n = sizeof(ANCILLARY_2_TYPE);
		ierr = dbget_cnf(ANCILLARY_2, (char *) &anc2, &n, 
										 "Getting ANCILLARY_2 structure");
		if(!ierr)
			*hdg = anc2.last_heading;
	}
	
	/* Check for errors */
	if(ierr || !(*hdg < ADJ_BADFLOAT)){
		if(ierr == SEARCH_BEYOND_END)
			return(-1);
		return(1);
	}
		
	return(0);

}	

/* ------------------------------------------------------------------
	 -Next Nav Time:

	 Read the navigation lines until a line with valid time is encountered.

	 Return: The value of the navigation line flag, or EOF if end of navigation
					 informations. 

					 The navigation structure in `rec' is updated with the navigation
	         line data.
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int next_nav_time(NAV_HDG_TYPE *rec, int year_base)
#else

#endif
{

	int done=0;
	int flag;

	while(!done){
		flag = next_nav_ping(rec->fps.lst, &(rec->fi), &(rec->nav),
												 rec->nav.yd, year_base, -1.0, nav_lines_bits);
		done = ((flag == EOF) || is_nav_time(flag));
	}

	return(flag);

}

/* ------------------------------------------------------------------
	 -Next Valid Nav Line:

	 Reads navigation lines until a valid one is found. Valid lines are defined
	 by the value of `nav_lines_bits' and must contain consistent information. 

	 Return: The value of the navigatio line flag, or EOF if end of navigation
					 informations. 

					 The navigation structure in `rec' is updated with the navigation
	         line data.
	 ------------------------------------------------------------------ */

#if PROTOTYPE_ALLOWED
int next_valid_nav_line(NAV_HDG_TYPE *rec, int year_base)
#else

#endif
{

	int done=0;
	int flag;

	while(!done){
		flag = next_nav_ping(rec->fps.lst, &(rec->fi), &(rec->nav),
												 rec->nav.yd, year_base, -1.0, nav_lines_bits);
		done = ((flag == EOF) || (flag >= 0));
	}

	return(flag);

}
	
/* ##################################################################
	 -Main:
	 ################################################################## */

#if PROTOTYPE_ALLOWED
void do_it(FILE *fp_cnt)
#else
void do_it(fp_cnt)
#endif
{

	NAV_HDG_TYPE rec;

	YMDHMS_TIME_TYPE first_time, start_time, stop_time;
	
	char first_time_str[30];
	int year_base;
	int error, flag, n;
	double yd, prev_yd, start_day, stop_day;
	double sum, sumsq, mn, std, prev_hdg, h, db_hdg;
	char str[120], msg[120];

	/* Initialize options */
	init_options(&rec);

	/* Get the parameters */
	if(get_parameters(fp_cnt, nav_hdg_params, nav_hdg_name_lists)) exit(-1);
	print_parameters(stdout, nav_hdg_params, nav_hdg_name_lists, "\n");
	sprintf(first_time_str, "%s  %s", day0_str, time0_str);

	/* Read and process all options */
	if(error_found(execute_options(fp_cnt, nav_hdg_options, ECHO)<=0,
		 "while reading av_hdg options."))
		goto on_error;

	/* Convert start time and set year_base */
	if(error_found(str_to_ymdhms(&first_time, first_time_str), 
								 "Invalid time for start_time"))
		exit(-1);
	year_base = (int) first_time.year;
	prev_yd = year_day(&first_time, year_base);
	rec.nav.yd = prev_yd;
	/* Message */
	yd_to_ymdhms_str(str, prev_yd, year_base);
	sprintf(msg, "\n%%%%%% FIRST TIME: %s\n", str); 
	report_msg(msg);

	/* Initializations */
	if(error_found(init_nav_hdg(&rec, year_base), "While initializing nav_hdg"))
		goto on_error;
	
	/* Get the first profile time */
	error = get_profile_time(&yd, year_base);
	if(error_found(error, "Getting first profile time"))
		goto on_error;
	yd_to_ymdhms_str(str, yd, year_base);
	sprintf(msg, "\n%%%%%% FIRST PROFILE TIME: %s\n", str); 
	report_msg(msg);

	/* Get the first navigaton time */
	error = next_nav_time(&rec, year_base);
	if(test_error(error, "Getting first nav time"))
		goto on_error;
	yd_to_ymdhms_str(str, rec.nav.yd, year_base);
	sprintf(msg, "\n%%%%%% FIRST NAV TIME: %s\n", str); 
	report_msg(msg);
	
	/* ------------------------------------------------------------------ 
		 Loop through time ranges 
		 ------------------------------------------------------------------ */
	while(get_time_range(fp_cnt, &start_time, &stop_time)==1){
	
		/* Get decimal days of time range */
		start_day = year_day(&start_time, year_base);
		stop_day  = year_day(&stop_time,  year_base);

		/* Message */
		time_range_to_str(str, &start_time, &stop_time);
		sprintf(msg, "\n%%%%%% TIME RANGE: %s\n", str);
		report_msg(msg);

		/* ---------------------------------
			 Loop outside time range
			 --------------------------------- */
		while(yd < start_day){
			/* Update time of previous profile */
			prev_yd = yd;
			/* Get next profile time */
			error = next_profile_time(&yd, year_base);
			if(test_error(error, "Getting next profile time"))
				goto on_error;
		}
		
		/* ---------------------------------
			 Loop until correct navigation time 
			 inside interval defined by prev_yd 
			 and yd 
			 --------------------------------- */
		while(rec.nav.yd < prev_yd){
			error = next_nav_time(&rec, year_base); 
			if(test_error(error, "Getting next nav time"))
				goto on_error;
		}
		
		/* ---------------------------------
			 Loop inside time range
			 --------------------------------- */
		while(yd <= stop_day){
			
			/* Message */			
			yd_to_ymdhms_str(str, yd, year_base);
			sprintf(msg, "\n%%%%%% PROFILE TIME: %s\n", str); 
			report_msg(msg);

			/* Initialize heading statistics */
			sum = sumsq = prev_hdg = 0.0;
			n   = 0;

			/* ---------------------------------
				 Loop inside profile
				 --------------------------------- */
			while(rec.nav.yd < yd){
				/* Get next valid line */
				flag = next_valid_nav_line(&rec, year_base);
				if(test_error(flag, "Getting next valid nav line")){
					error = flag;
					goto on_error;
				}
				/* Update heading statistics */
				if(is_nav_heading(flag)){
					if(rec.nav.heading < ADJ_BADFLOAT){
						h = nearest_angle(prev_hdg, rec.nav.heading);
						sum += h;
						sumsq += h*h;
						n++;
						prev_hdg = h;
					}
				}
			}
			/* Calculate mean GPS heading */
			mn = (n > 0) ? (sum/n) : BADFLOAT;
			std = ((n > 0) && (mn < ADJ_BADFLOAT)) ? sqrt(sumsq/n - mn*mn) : BADFLOAT;
			mn = to_360(mn);

			/* Get corresponding heading from database */
			error = get_profile_heading(&db_hdg);
			if(test_error(error, "Getting heading from database"))
				goto on_error;

			/* Update output ascii file */
			if((mn < ADJ_BADFLOAT) && (db_hdg < ADJ_BADFLOAT))
				fprintf(rec.fps.out, "%12.6f %12.6f %12.6f %12.6f %12.6f %3d\n",
								yd, (db_hdg - mn), db_hdg, mn, std, n);
			
			/* Next profile time */
			error = next_profile_time(&yd, year_base);
			if(test_error(error, "Getting next profile time"))
				goto on_error;
			
		}

	}

	error = 0;

on_error:

	/* Close the files and database */
	if(rec.fps.lst) fclose(rec.fps.lst);
	if(rec.fps.out) fclose(rec.fps.out);
	if(rec.fi.fp)   fclose(rec.fi.fp);
	if(db && db->db_is_open){
		DBCLOSE(&error);
		DBERROR(&error, "DBCLOSE");
	}

	if(error){
		if(error == -1)
			printf("\n\n\t nav_hdg terminated at EOF \n\n");
		else
			printf("\n\n\t nav_hdg terminated with ERROR \n\n");
	}
	else
		printf("\n\n\t nav_hdg terminated normally \n\n");

	return;

}



	

	
	
