/*--------------------------------------------------------------------
 *	$Id: grdtrend.c,v 1.19 2006/04/13 01:44:31 pwessel Exp $
 *
 *	Copyright (c) 1991-2006 by P. Wessel and W. H. F. Smith
 *	See COPYING file for copying and redistribution conditions.
 *
 *	This program is free software; you can redistribute it and/or modify
 *	it under the terms of the GNU General Public License as published by
 *	the Free Software Foundation; version 2 of the License.
 *
 *	This program is distributed in the hope that it will be useful,
 *	but WITHOUT ANY WARRANTY; without even the implied warranty of
 *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *	GNU General Public License for more details.
 *
 *	Contact info: gmt.soest.hawaii.edu
 *--------------------------------------------------------------------*/
/* grdtrend <input.grd> -N[r]<n_model> [-T<trend.grd>] [-V]
	[-W<weight.grd] [-D<differences.grd]

Reads a grdfile and fits a trend surface.  Trend surface
is defined by:

m1 +m2*x + m3*y + m4*xy + m5*x*x + m6*y*y + m7*x*x*x
	+ m8*x*x*y + m9*x*y*y + m10*y*y*y.

n_model is set by the user to be an integer in [1,10]
which sets the number of model coefficients to fit.
Thus:
n_model = 1 gives the mean value of the surface,
n_model = 3 fits a plane,
n_model = 4 fits a bilinear surface,
n_model = 6 fits a biquadratic,
n_model = 10 fits a bicubic surface.

The user may write out grdfiles of the fitted surface
[-T<trend.grd>] and / or of the residuals (input data
minus fitted trend) [-D<differences.grd] and / or of
the weights used in iterative fitting [-W<weight.grd].
This last option applies only when the surface is fit
iteratively [-N<n>[r]].

A robust fit may be achieved by iterative fitting of
a weighted least squares problem, where the weights
are set according to a scale length based on the 
Median absolute deviation (MAD: Huber, 1982).  The
-N<n>r option achieves this.

Author:		W. H. F. Smith
Date:		21 May, 1991.
Version:	4
Calls:		uses the QR solution of the Normal
		equations furnished by Wm. Menke's
		C routine "gauss".  We gratefully
		acknowledge this contribution.
Revised:	12-JUN-1998 PW, for GMT 3.1

Remarks:

We adopt a translation and scaling of the x,y coordinates.
We choose x,y such that they are in [-1,1] over the range
of the grdfile.  If the problem is unweighted, all input
values are filled (no "holes" or NaNs in the input grdfile),
and n_model <= 4 (bilinear or simpler), then the normal
equations matrix (G'G in Menke notation) is diagonal under
this change of coordinates, and the solution is trivial.
In this case, it would be dangerous to try to accumulate
the sums which are the elements of the normal equations;
while they analytically cancel to zero, the addition errors
would likely prevent this.  Therefore we have written a
routine, grd_trivial_model(), to handle this case.

If the problem is more complex than the above trivial case,
(missing values, weighted problem, or n_model > 4), then
G'G is not trivial and we just naively accumulate sums in
the G'G matrix.  We hope that the changed coordinates in
[-1,1] will help the accuracy of the problem.  We also use
Legendre polynomials in this case so that the matrix elements
are conveniently sized near 0 or 1.

*/

#include "gmt.h"

struct GRDTREND_CTRL {	/* All control options for this program (except common args) */
	/* ctive is TRUE if the option has been activated */
	struct D {	/* -D<diffgrid> */
		BOOLEAN active;
		char *file;
	} D;
	struct N {	/* -N[r]<n_model> */
		BOOLEAN active;
		BOOLEAN robust;
		int value;
	} N;
	struct T {	/* -T<trend.grd> */
		BOOLEAN active;
		char *file;
	} T;
	struct W {	/* -W<weight.grd> */
		BOOLEAN active;
		char *file;
	} W;
};

#define GRDTREND_MAX_TABLE_COLS 10	/* Used by Menke routine gauss  */

int main(int argc, char **argv)
{
	BOOLEAN	error = FALSE, trivial, weighted;

	char	*i_filename = NULL, format[BUFSIZ];

	int	i, j, k, ierror = 0, iterations, nxy;

	float	*data;		/* Pointer for array from input grdfile  */
	float	*trend;		/* Pointer for array containing fitted surface  */
	float	*resid;		/* Pointer for array containing residual surface  */
	float	*weight = VNULL;/* Pointer for array containing data weights  */

	double	chisq, old_chisq, zero_test = 1.0e-08, scale = 1.0;
	double	*xval;		/* Pointer for array of change of variable:  x[i]  */
	double	*yval;		/* Pointer for array of change of variable:  y[j]  */
	double	*gtg;		/* Pointer for array for matrix G'G normal equations  */
	double	*gtd;		/* Pointer for array for vector G'd normal equations  */
	double	*old;		/* Pointer for array for old model, used for robust sol'n  */
	double	*pstuff;	/* Pointer for array for Legendre polynomials of x[i],y[j]  */

	struct GRD_HEADER head_d, head_w;
	struct GRDTREND_CTRL *Ctrl;

	void	gauss(double *a, double *vec, int n, int nstore, double test, int *ierror, int itriag);		/* QR solution of the Normal equations  */
	void	set_up_vals(double *val, int nval, double vmin, double vmax, double dv, int pixel_reg);		/* Store x[i], y[j] once for all to save time  */
	void	load_pstuff(double *pstuff, int n_model, double x, double y, int newx, int newy);		/* Compute Legendre polynomials of x[i],y[j] as needed  */
	void	grd_trivial_model(float *data, int nx, int ny, double *xval, double *yval, double *gtd, int n_model);	/* Fit trivial models.  See Remarks above.  */
	void	compute_trend(float *trend, int nx, int ny, double *xval, double *yval, double *gtd, int n_model, double *pstuff);	/* Find trend from a model  */
	void	compute_resid(float *data, float *trend, float *resid, int nxy);	/* Find residuals from a trend  */
	void	compute_chisq(float *resid, float *weight, int nxy, double *chisq, double scale);	/* Find Chi-Squared from weighted residuals  */
	void	compute_robust_weight(float *resid, float *weight, int nxy, double *scale);	/* Find weights from residuals  */
	void	write_model_parameters(double *gtd, int n_model);	/* Do reports if gmtdefs.verbose == TRUE  */
	void	load_gtg_and_gtd(float *data, int nx, int ny, double *xval, double *yval, double *pstuff, double *gtg, double *gtd, int n_model, float *weight, int weighted);		/* Fill normal equations matrices  */
	void	*New_Grdtrend_Ctrl (), Free_Grdtrend_Ctrl (struct GRDTREND_CTRL *C);

/* Execution begins here with loop over arguments:  */

	argc = GMT_begin (argc, argv);

	Ctrl = (struct GRDTREND_CTRL *) New_Grdtrend_Ctrl ();		/* Allocate and initialize defaults in a new control structure */

	for (i = 1; i < argc; i++) {
		if (argv[i][0] == '-') {
			switch (argv[i][1]) {

				/* Common parameters */

				case 'V':
				case '\0':
					error += GMT_parse_common_options (argv[i], 0, 0, 0, 0);
					break;

				/* Supplemental parameters */

				case 'D':
					Ctrl->D.active = TRUE;
					if (argv[i][2])
						Ctrl->D.file = strdup (&argv[i][2]);
					else {
						fprintf (stderr, "%s: GMT SYNTAX ERROR -D option:  Must specify file name\n", GMT_program);
						error = TRUE;
					}
					break;
				case 'N':
					/* Must check for both -N[r]<n_model> and -N<n_model>[r] due to confusion */
					Ctrl->N.active = TRUE;
					if (strchr (argv[i], 'r')) Ctrl->N.robust = TRUE;
					j = (argv[i][2] == 'r') ? 3 : 2;
					if (argv[i][j]) Ctrl->N.value = atoi(&argv[i][j]);
					break;
				case 'T':
					Ctrl->T.active = TRUE;
					if (argv[i][2])
						Ctrl->T.file = strdup (&argv[i][2]);
					else {
						fprintf (stderr, "%s: GMT SYNTAX ERROR -T option:  Must specify file name\n", GMT_program);
						error = TRUE;
					}
					break;
				case 'W':
					Ctrl->W.active = TRUE;
					if (argv[i][2])
						Ctrl->W.file = strdup (&argv[i][2]);
					else {
						fprintf (stderr, "%s: GMT SYNTAX ERROR -W option:  Must specify file name\n", GMT_program);
						error = TRUE;
					}
					/* OK if this file doesn't exist:  */
					break;
				default:
					error = TRUE;
					GMT_default_error (argv[i][1]);
					break;
			}
		}
		else
			i_filename = argv[i];
	}

	if (argc == 1 || GMT_give_synopsis_and_exit) {
		fprintf (stderr, "grdtrend %s - Fit trend surface to gridded data\n\n", GMT_VERSION);
		fprintf (stderr,"usage:  grdtrend <input.grd> -N<n_model>[r] [-D<diff.grd>]\n");
		fprintf (stderr,"\t[-T<trend.grd>] [-V] [-W<weight.grd>]\n\n");

		if (GMT_give_synopsis_and_exit) exit (EXIT_FAILURE);

		fprintf (stderr,"\t<input.grd> is name of grdfile to fit trend to.\n");
		fprintf (stderr,"\t-N # model parameters to fit; integer in [1,10].  Append r for robust fit.\n");
		fprintf (stderr,"\n\tOPTIONS:\n");
		fprintf (stderr,"\t-D Supply filename to write grdfile of differences (input - trend).\n");
		fprintf (stderr,"\t-T Supply filename to write grdfile of trend.\n");
		GMT_explain_option ('V');
		fprintf (stderr,"\t-W Supply filename if you want to [read and] write grdfile of weights.\n");
		fprintf (stderr,"\t   If <weight.grd> can be read at run, and if robust = FALSE, weighted problem will be solved.\n");
		fprintf (stderr,"\t   If robust = TRUE, weights used for robust fit will be written to <weight.grd>.\n");
		GMT_explain_option ('.');
		exit (EXIT_FAILURE);
	}

	if (!i_filename) {
		fprintf (stderr, "%s: GMT SYNTAX ERROR:  Must specify input file\n", GMT_program);
		error++;
	}
	if (Ctrl->N.value <= 0 || Ctrl->N.value > 10) {
		fprintf (stderr, "%s: GMT SYNTAX ERROR -N option:  Specify 1-10 model parameters\n", GMT_program);
		error++;
	}

	if (error) exit (EXIT_FAILURE);

	GMT_put_history (argc, argv);	/* Update .gmtcommands4 */

/* End of argument parsing.  */

	trivial = ( (Ctrl->N.value < 5) && (!(Ctrl->N.robust)) && (!Ctrl->W.file) );

/* Read the input file:  */

	if (GMT_read_grd_info (i_filename, &head_d)) {
		fprintf (stderr, "%s: GMT SYNTAX ERROR:  File %s not found\n", GMT_program, i_filename);
		exit (EXIT_FAILURE);
	}

	GMT_grd_init (&head_d, argc, argv, TRUE);
	nxy = head_d.nx * head_d.ny;
	data = (float *) GMT_memory (VNULL, (size_t)nxy, sizeof (float), GMT_program);
	GMT_read_grd (i_filename, &head_d, data, 0.0, 0.0, 0.0, 0.0, GMT_pad, FALSE);

	/* Check for NaNs:  */
	i = 0;
	while (trivial && i < nxy) {
		if (GMT_is_fnan (data[i])) trivial = FALSE;
		i++;
	}

/* End input read section.  */

/* Allocate other required arrays:  */

	trend = (float *) GMT_memory (VNULL, (size_t)nxy, sizeof (float), GMT_program);
	resid = (float *) GMT_memory (VNULL, (size_t)nxy, sizeof (float), GMT_program);
	xval = (double *) GMT_memory (VNULL, (size_t)head_d.nx, sizeof (double), GMT_program);
	yval = (double *) GMT_memory (VNULL, (size_t)head_d.ny, sizeof (double), GMT_program);
	gtg = (double *) GMT_memory (VNULL, (size_t)(Ctrl->N.value*Ctrl->N.value), sizeof (double), GMT_program);
	gtd = (double *) GMT_memory (VNULL, (size_t)Ctrl->N.value, sizeof (double), GMT_program);
	old = (double *) GMT_memory (VNULL, (size_t)Ctrl->N.value, sizeof (double), GMT_program);
	pstuff = (double *) GMT_memory (VNULL, (size_t)Ctrl->N.value, sizeof (double), GMT_program);


/* If a weight array is needed, get one:  */

	weighted = (Ctrl->N.robust || Ctrl->W.file);
	if (weighted) {
		weight = (float *) GMT_memory (VNULL, (size_t)nxy, sizeof (float), GMT_program);
		if (!GMT_access (Ctrl->W.file, R_OK)) {	/* We have weights on input  */
			GMT_grd_init (&head_w, argc, argv, FALSE);
			if (GMT_read_grd_info (Ctrl->W.file, &head_w)) {
				fprintf (stderr, "%s: GMT SYNTAX ERROR:  File %s not found\n", GMT_program, Ctrl->W.file);
				exit (EXIT_FAILURE);
			}
			if (head_w.nx != head_d.nx || head_w.ny != head_d.ny) {
				fprintf (stderr,"%s:  Input weight file does not match input data file.  Ignoring.\n", GMT_program);
				for (i = 0; i < nxy; i++) weight[i] = 1.0;
			}
			else
				GMT_read_grd (Ctrl->W.file, &head_w, weight, 0.0, 0.0, 0.0, 0.0, GMT_pad, FALSE);
		}
		else {
			for (i = 0; i < nxy; i++) weight[i] = 1.0;
		}
	}

/* End of weight set up.  */

/* Set up xval and yval lookup tables:  */

	set_up_vals(xval, head_d.nx, head_d.x_min, head_d.x_max, head_d.x_inc,
		 head_d.node_offset);
	set_up_vals(yval, head_d.ny, head_d.y_min, head_d.y_max, head_d.y_inc,
		 head_d.node_offset);

/* End of set up of lookup values.  */

/* Do the problem:  */

	if (trivial) {
		grd_trivial_model(data, head_d.nx, head_d.ny, xval, yval, gtd, Ctrl->N.value);
		compute_trend(trend, head_d.nx, head_d.ny, xval, yval, gtd, Ctrl->N.value, pstuff);
		compute_resid(data, trend, resid, nxy);
	}
	else {	/* Problem is not trivial  !!  */

		load_gtg_and_gtd(data, head_d.nx, head_d.ny, xval, yval, pstuff, gtg, gtd, Ctrl->N.value, weight, weighted);
		gauss(gtg, gtd, Ctrl->N.value, Ctrl->N.value, zero_test, &ierror, 1);
		if (ierror) {
			fprintf (stderr,"%s:  Gauss returns error code %d\n", GMT_program, ierror);
			exit (EXIT_FAILURE);
		}
		compute_trend(trend, head_d.nx, head_d.ny, xval, yval, gtd, Ctrl->N.value, pstuff);
		compute_resid(data, trend, resid, nxy);

		if (Ctrl->N.robust) {
			compute_chisq(resid, weight, nxy, &chisq, scale);
			iterations = 1;
			sprintf(format, "%%s Robust iteration %%d:  Old Chi Squared:  %s  New Chi Squared %s\n", gmtdefs.d_format, gmtdefs.d_format);
			do {
				old_chisq = chisq;
				for(k = 0; k < Ctrl->N.value; k++) old[k] = gtd[k];
				compute_robust_weight(resid, weight, nxy, &scale);
				load_gtg_and_gtd(data, head_d.nx, head_d.ny, xval, yval, pstuff, gtg, gtd, Ctrl->N.value, weight, weighted);
				gauss(gtg, gtd, Ctrl->N.value, Ctrl->N.value, zero_test, &ierror, 1);
				if (ierror) {
					fprintf (stderr,"%s:  Gauss returns error code %d\n", GMT_program, ierror);
					exit (EXIT_FAILURE);
				}
				compute_trend(trend, head_d.nx, head_d.ny, xval, yval, gtd, Ctrl->N.value, pstuff);
				compute_resid(data, trend, resid, nxy);
				compute_chisq(resid, weight, nxy, &chisq, scale);
				if (gmtdefs.verbose) fprintf (stderr, format, GMT_program, iterations, old_chisq, chisq);
				iterations++;
			} while ( (old_chisq / chisq) > 1.0001);

			/* Get here when new model not significantly better; use old one:  */

			for(k = 0; k < Ctrl->N.value; k++) gtd[k] = old[k];
			compute_trend(trend, head_d.nx, head_d.ny, xval, yval, gtd, Ctrl->N.value, pstuff);
			compute_resid(data, trend, resid, nxy);
		}
	}

/* End of do the problem section.  */

/* Get here when ready to do output:  */

	if (gmtdefs.verbose) write_model_parameters(gtd, Ctrl->N.value);
	if (Ctrl->T.file) {
		strcpy (head_d.title, "trend surface");
		GMT_write_grd (Ctrl->T.file, &head_d, trend, 0.0, 0.0, 0.0, 0.0, GMT_pad, FALSE);
	}
	if (Ctrl->D.file) {
		strcpy (head_d.title, "trend residuals");
		GMT_write_grd (Ctrl->D.file, &head_d, resid, 0.0, 0.0, 0.0, 0.0, GMT_pad, FALSE);
	}
	if (Ctrl->W.file && Ctrl->N.robust) {
		strcpy (head_d.title, "trend weights");
		GMT_write_grd (Ctrl->W.file, &head_d, weight, 0.0, 0.0, 0.0, 0.0, GMT_pad, FALSE);
	}

/* That's all, folks!  */

	if (weighted) GMT_free ((void *)weight);
	GMT_free ((void *)pstuff);
	GMT_free ((void *)gtd);
	GMT_free ((void *)gtg);
	GMT_free ((void *)yval);
	GMT_free ((void *)xval);
	GMT_free ((void *)resid);
	GMT_free ((void *)trend);
	GMT_free ((void *)data);

	Free_Grdtrend_Ctrl (Ctrl);	/* Deallocate control structure */

	GMT_end (argc, argv);

	exit (EXIT_SUCCESS);
}

void set_up_vals (double *val, int nval, double vmin, double vmax, double dv, int pixel_reg)
{
	int	i;
	double  v, middle, drange, true_min, true_max;

	true_min = (pixel_reg) ? vmin + 0.5 * dv : vmin;
	true_max = (pixel_reg) ? vmax - 0.5 * dv : vmax;

	middle = 0.5 * (true_min + true_max);
	drange = 2.0 / (true_max - true_min);
	for (i = 0; i < nval; i++) {
		v = true_min + i * dv;
		val[i] = (v - middle) * drange;
	}
	/* Just to be sure no rounding outside:  */
	val[0] = -1.0;
	val[nval - 1] = 1.0;
	return;
}

void load_pstuff (double *pstuff, int n_model, double x, double y, int newx, int newy)
{
	/* If either x or y has changed, compute new Legendre polynomials as needed  */

	if (newx) {
		if (n_model >= 2) pstuff[1] = x;
		if (n_model >= 5) pstuff[4] = 0.5*(3.0*pstuff[1]*pstuff[1] - 1.0);
		if (n_model >= 7) pstuff[6] = (5.0*pstuff[1]*pstuff[4] - 2.0*pstuff[1])/3.0;
	}
	if (newy) {
		if (n_model >= 3) pstuff[2] = y;
		if (n_model >= 6) pstuff[5] = 0.5*(3.0*pstuff[2]*pstuff[2] - 1.0);
		if (n_model >= 10) pstuff[9] = (5.0*pstuff[2]*pstuff[5] - 2.0*pstuff[2])/3.0;
	}
	/* In either case, refresh cross terms:  */

	if (n_model >= 4) pstuff[3] = pstuff[1]*pstuff[2];
	if (n_model >= 8) pstuff[7] = pstuff[4]*pstuff[2];
	if (n_model >= 9) pstuff[8] = pstuff[1]*pstuff[5];

	return;
}

void compute_trend (float *trend, int nx, int ny, double *xval, double *yval, double *gtd, int n_model, double *pstuff)
{
	int	i, j, k, ij;

	for (ij = 0, j = 0; j < ny; j++) {
		for (i = 0; i < nx; i++, ij++) {
			load_pstuff(pstuff, n_model, xval[i], yval[j], 1, (!(i)));
			trend[ij] = (float)gtd[0];
			for (k = 1; k < n_model; k++) {
				trend[ij] += (float)(pstuff[k]*gtd[k]);
			}
		}
	}
}

void compute_resid (float *data, float *trend, float *resid, int nxy)
{
	int	i;

	for (i = 0; i < nxy; i++) {
		if (GMT_is_fnan (data[i])) {
			resid[i] = data[i];
		}
		else {
			resid[i] = data[i] - trend[i];
		}
	}
	return;
}

void grd_trivial_model (float *data, int nx, int ny, double *xval, double *yval, double *gtd, int n_model)
{
	/* Routine to fit up elementary polynomial model of grd data, 
	model = gtd[0] + gtd[1]*x + gtd[2]*y + gtd[3] * x * y,
	where x,y are normalized to range [-1,1] and there are no
	NaNs in grdfile, and problem is unweighted least squares.  */

	int	i, j, ij;
	double	x2, y2, sumx2 = 0.0, sumy2 = 0.0, sumx2y2 = 0.0;

	/* First zero the model parameters to use for sums:  */

	for (i = 0; i < n_model; i++) gtd[i] = 0.0;

	/* Now accumulate sums:  */

	for (ij = 0, j = 0; j < ny; j++) {
		y2 = yval[j] * yval[j];
		for (i = 0; i < nx; i++, ij++) {
			x2 = xval[i] * xval[i];
			sumx2 += x2;
			sumy2 += y2;
			sumx2y2 += (x2 * y2);
			gtd[0] += data[ij];
			if (n_model >= 2) gtd[1] += data[ij] * xval[i];
			if (n_model >= 3) gtd[2] += data[ij] * yval[j];
			if (n_model == 4) gtd[3] += data[ij] * xval[i] * yval[j];
		}
	}

	/* See how trivial it is?  */

	gtd[0] /= (nx * ny);
	if (n_model >= 2) gtd[1] /= sumx2;
	if (n_model >= 3) gtd[2] /= sumy2;
	if (n_model == 4) gtd[3] /= sumx2y2;

	return;
}

void compute_chisq (float *resid, float *weight, int nxy, double *chisq, double scale)
{
	int	i;
	double	tmp;

	*chisq = 0.0;
	for (i = 0; i < nxy; i++) {
		if (GMT_is_fnan (resid[i]))continue;
		tmp = resid[i];
		if (scale != 1.0) tmp /= scale;
		tmp *= tmp;

		if (weight[i] == 1.0) {
			*chisq += tmp;
		}
		else {
			/* Weight has already been squared  */
			*chisq += (tmp * weight[i]);
		}
	}
	return;
}

void compute_robust_weight (float *resid, float *weight, int nxy, double *scale)
{
	int	i, j, j2;
	double	r, mad;

	for (i = j = 0; i < nxy; i++) {
		if (GMT_is_fnan (resid[i]))continue;
		weight[j] = (float)fabs((double)resid[i]);
		j++;
	}

	qsort ((void *)weight, (size_t)j, sizeof(float), GMT_comp_float_asc);

	j2 = j / 2;
	if (j%2) {
		mad = weight[j2];
	}
	else {
		mad = 0.5 *(weight[j2] + weight[j2 - 1]);
	}

	/* Adjust mad to equal Gaussian sigma:  */

	*scale = 1.4826 * mad;

	/* Use weight according to Huber (1981), but squared:  */

	for (i = 0; i < nxy; i++) {
		if (GMT_is_fnan (resid[i])) {
			weight[i] = resid[i];
			continue;
		}
		r = fabs(resid[i]) / (*scale);

		weight[i] = (float)((r <= 1.5) ? 1.0 : (3.0 - 2.25/r) / r);
	}
	return;
}

void write_model_parameters (double *gtd, int n_model)
{
	int	i;
	char	pbasis[10][16];
	char format[BUFSIZ];

	sprintf(pbasis[0], "Mean");
	sprintf(pbasis[1], "X");
	sprintf(pbasis[2], "Y");
	sprintf(pbasis[3], "X*Y");
	sprintf(pbasis[4], "P2(x)");
	sprintf(pbasis[5], "P2(y)");
	sprintf(pbasis[6], "P3(x)");
	sprintf(pbasis[7], "P2(x)*P1(y)");
	sprintf(pbasis[8], "P1(x)*P2(y)");
	sprintf(pbasis[9], "P3(y)");

	sprintf(format, "Coefficient fit to %%s:  %s\n", gmtdefs.d_format);
	for(i = 0; i < n_model; i++) fprintf (stderr, format, pbasis[i], gtd[i]);

	return;
}

void load_gtg_and_gtd (float *data, int nx, int ny, double *xval, double *yval, double *pstuff, double *gtg, double *gtd, int n_model, float *weight, int weighted)
{
	/* Routine to load the matrix G'G (gtg) and vector G'd (gtd)
	for the normal equations.  Routine uses indices i,j to refer
	to the grdfile of data, and k,l to refer to the k_row, l_col
	of the normal equations matrix.  We need sums of [weighted]
	data and model functions in gtg and gtd.  We save time by
	loading only lower triangular part of gtg and then filling
	by symmetry after i,j loop.  */

	int	i, j, ij, k, l, n_used;

/*	First zero things out to start:  */

	n_used = 0;
	for (k = 0; k < n_model; k++) {
		gtd[k] = 0.0;
		for (l = 0; l < n_model; l++) {
			gtg[k*n_model+l] = 0.0;
		}
	}

/*  Now get going.  Have to load_pstuff separately in i and j,
	because it is possible that we skip data when i = 0.
	Loop over all data:  */

	for (ij = 0, j = 0; j < ny; j++ ) {
		load_pstuff(pstuff, n_model, xval[0], yval[j], 0, 1);
		for (i = 0; i < nx; i++, ij++) {

			if (GMT_is_fnan (data[ij]))continue;

			n_used++;
			load_pstuff(pstuff, n_model, xval[i], yval[j], 1, 0);

/* If weighted  */	if (weighted) {
				/* Loop over all gtg and gtd elements:  */
				gtd[0] += (data[ij] * weight[ij]);
				gtg[0] += (weight[ij]);
				for (k = 1; k < n_model; k++) {
					gtd[k] += (data[ij] * weight[ij] * pstuff[k]);
					gtg[k] += (weight[ij] * pstuff[k]);
					for (l = k; l < n_model; l++) {
						gtg[k + l*n_model] += (pstuff[k]*pstuff[l]*weight[ij]);
					}
				}
			}
/* If !weighted  */	else {
				/* Loop over all gtg and gtd elements:  */
				gtd[0] += data[ij];
				for (k = 1; k < n_model; k++) {
					gtd[k] += (data[ij] * pstuff[k]);
					gtg[k] += pstuff[k];
					for (l = k; l < n_model; l++) {
						gtg[k + l*n_model] += (pstuff[k]*pstuff[l]);
					}
				}
/* End if  */		}
		}
	}
/* End of loop over data i,j  */

/* Now if !weighted, use more accurate sum for gtg[0], and set symmetry:  */

	if (!(weighted)) gtg[0] = n_used;

	for (k = 0; k < n_model; k++) {
		for (l = 0; l < k; l++) {
			gtg[l + k*n_model] = gtg[k + l*n_model];
		}
	}
/* That is all there is to it!  */

	return;
}

void gauss (double *a, double *vec, int n, int nstore, double test, int *ierror, int itriag)
{
 
/* subroutine gauss, by william menke */
/* july 1978 (modified feb 1983, nov 85) */
 
/* a subroutine to solve a system of n linear equations in n unknowns*/
/* where n doesn't exceed GRDTREND_MAX_TABLE_COLS */
/* gaussian reduction with partial pivoting is used */
/*      a               (sent, destroyed)       n by n matrix           */
/*      vec             (sent, overwritten)     n vector, replaced w/ solution*/
/*      nstore          (sent)                  dimension of a  */
/*      test            (sent)                  div by zero check number*/
/*      ierror          (returned)              zero on no error*/
/*      itriag          (sent)                  matrix triangularized only*/
/*                                               on TRUE useful when solving*/
/*                                               multiple systems with same a */
        static int isub[GRDTREND_MAX_TABLE_COLS], l1;
        int line[GRDTREND_MAX_TABLE_COLS], iet, ieb, i = 0, j, k, l, j2;
        double big, testa, b, sum;

        iet=0;  /* initial error flags, one for triagularization*/
        ieb=0;  /* one for backsolving */

/* triangularize the matrix a*/
/* replacing the zero elements of the triangularized matrix */
/* with the coefficients needed to transform the vector vec */

        if (itriag) {   /* triangularize matrix */
 
                for( j=0; j<n; j++ ) {      /*line is an array of flags*/
                        line[j]=0; 
                        /* elements of a are not moved during pivoting*/
                        /* line=0 flags unused lines */
                        }    /*end for j*/
                        
                for( j=0; j<n-1; j++ ) {
                        /*  triangularize matrix by partial pivoting */
                       big = 0.0; /* find biggest element in j-th column*/
                                  /* of unused portion of matrix*/
                       for( l1=0; l1<n; l1++ ) {
                               if( line[l1]==0 ) {
                                       testa=(double) fabs((double) (*(a+l1*nstore+j)));
                                       if (testa>big) {
                                                i=l1;
                                                big=testa;
                                                } /*end if*/
                                        } /*end if*/
                                } /*end for l1*/
                       if( big<=test) {   /* test for div by 0 */
                               iet=1;
                               } /*end if*/
 
                       line[i]=1;  /* selected unused line becomes used line */
                       isub[j]=i;  /* isub points to j-th row of tri. matrix */
 
                       sum=1.0/(*(a+i*nstore+j)); 
                                /*reduce matrix towards triangle */
                       for( k=0; k<n; k++ ) {
                                if( line[k]==0 ) {
                                        b=(*(a+k*nstore+j))*sum;
                                        for( l=j+1; l<n; l++ ) {
                                               *(a+k*nstore+l)= (*(a+k*nstore+l)) -b*(*(a+i*nstore+l));
                                               } /*end for l*/
                                       *(a+k*nstore+j)=b;
                                        } /*end if*/
                                } /*end for k*/
                        } /*end for j*/
 
               for( j=0; j<n; j++ ) {
                        /*find last unused row and set its pointer*/
                        /*  this row contains the apex of the triangle*/
                        if( line[j]==0) {
                                l1=j;   /*apex of triangle*/
                                isub[n-1]=j;
                                break;
                                } /*end if*/
                        } /*end for j*/
 
                } /*end if itriag true*/
                
        /*start backsolving*/
        
        for( i=0; i<n; i++ ) {  /* invert pointers. line(i) now gives*/
                                /* row no in triang matrix of i-th row*/
                                /* of actual matrix */
                line[isub[i]] = i;
                } /*end for i*/
 
        for( j=0; j<n-1; j++) { /*transform the vector to match triang. matrix*/
               b=vec[isub[j]];
               for( k=0; k<n; k++ ) {
                      if (line[k]>j) {  /* skip elements outside of triangle*/
                                vec[k]=vec[k]-(*(a+k*nstore+j))*b;
                                } /*end if*/
                        } /*end for k*/
                } /*end for j*/
 
      b = *(a+l1*nstore+(n-1));   /*apex of triangle*/
      if( ((double)fabs( (double) b))<=test) {
                /*check for div by zero in backsolving*/
                ieb=2;
                } /*end if*/
      vec[isub[n-1]]=vec[isub[n-1]]/b;
 
      for( j=n-2; j>=0; j-- ) { /* backsolve rest of triangle*/
                sum=vec[isub[j]];
                for( j2=j+1; j2<n; j2++ ) {
                        sum = sum - vec[isub[j2]] * (*(a+isub[j]*nstore+j2));
                        } /*end for j2*/
                        b = *(a+isub[j]*nstore+j);
               if( ((double)fabs((double)b))<=test) {
                        /* test for div by 0 in backsolving */
                        ieb=2;
                        } /*end if*/
                vec[isub[j]]=sum/b;   /*solution returned in vec*/
                } /*end for j*/

/*put the solution vector into the proper order*/

      for( i=0; i<n; i++ ) {    /* reorder solution */
                for( k=i; k<n; k++ ) {  /* search for i-th solution element */
                        if( line[k]==i ) {
                                j=k;
                                break;
                                } /*end if*/
                        } /*end for k*/
               b = vec[j];       /* swap solution and pointer elements*/
               vec[j] = vec[i];
               vec[i] = b;
               line[j] = line[i];
                } /*end for i*/
 
      *ierror = iet + ieb;   /* set final error flag*/
}

void *New_Grdtrend_Ctrl () {	/* Allocate and initialize a new control structure */
	struct GRDTREND_CTRL *C;
	
	C = (struct GRDTREND_CTRL *) GMT_memory (VNULL, 1, sizeof (struct GRDTREND_CTRL), "New_Grdtrend_Ctrl");
	
	/* Initialize values whose defaults are not 0/FALSE/NULL */
		
	return ((void *)C);
}

void Free_Grdtrend_Ctrl (struct GRDTREND_CTRL *C) {	/* Deallocate control structure */
	if (C->D.file) GMT_free ((void *)C->D.file);	
	if (C->T.file) GMT_free ((void *)C->T.file);	
	if (C->W.file) GMT_free ((void *)C->W.file);	
	GMT_free ((void *)C);	
}
