/****************************************************************/
/* find_normal() - Function to find the set of normal equations */
/*                 that allow a quadratic trend surface to be   */
/*                 fitted through N  points using least squares */
/*                 V.1.0, Jo Wood, 27th November, 1994.         */

/****************************************************************/

#include "param.h"

void find_normal(double **normal,	/* Matrix of cross-products.    */
		 double *w)
{				/* Weights matrix.                */
    int edge = EDGE;		/* Store (wsize-1)/2 to save    */

    /* on computation               */

    double x, y,			/* Local coordinates of window. */
      x1 = 0, y1 = 0,		/* coefficients of X-products.  */
	x2 = 0, y2 = 0,
	x3 = 0, y3 = 0,
	x4 = 0, y4 = 0,
	xy2 = 0, x2y = 0, xy3 = 0, x3y = 0, x2y2 = 0, xy = 0, N = 0;

    int row, col;		/* Pass through local window.   */


    /* Initialise sums-of-squares and cross products matrix */

    for (row = 0; row < 6; row++)
	for (col = 0; col < 6; col++)
	    normal[row][col] = 0.0;


    /* Calculate matrix of sums of squares and cross products */

    for (row = 0; row < wsize; row++)
	for (col = 0; col < wsize; col++) {
	    x = resoln * (col - edge);
	    y = resoln * (row - edge);

	    x4 += x * x * x * x * *(w + row * wsize + col);
	    x2y2 += x * x * y * y * *(w + row * wsize + col);
	    x3y += x * x * x * y * *(w + row * wsize + col);
	    x3 += x * x * x * *(w + row * wsize + col);
	    x2y += x * x * y * *(w + row * wsize + col);
	    x2 += x * x * *(w + row * wsize + col);

	    y4 += y * y * y * y * *(w + row * wsize + col);
	    xy3 += x * y * y * y * *(w + row * wsize + col);
	    xy2 += x * y * y * *(w + row * wsize + col);
	    y3 += y * y * y * *(w + row * wsize + col);
	    y2 += y * y * *(w + row * wsize + col);

	    xy += x * y * *(w + row * wsize + col);

	    x1 += x * *(w + row * wsize + col);

	    y1 += y * *(w + row * wsize + col);

	    N += *(w + row * wsize + col);

	}

    /* --- Store cross-product matrix elements. --- */

    normal[0][0] = x4;
    normal[0][1] = normal[1][0] = x2y2;
    normal[0][2] = normal[2][0] = x3y;
    normal[0][3] = normal[3][0] = x3;
    normal[0][4] = normal[4][0] = x2y;
    normal[0][5] = normal[5][0] = x2;

    normal[1][1] = y4;
    normal[1][2] = normal[2][1] = xy3;
    normal[1][3] = normal[3][1] = xy2;
    normal[1][4] = normal[4][1] = y3;
    normal[1][5] = normal[5][1] = y2;

    normal[2][2] = x2y2;
    normal[2][3] = normal[3][2] = x2y;
    normal[2][4] = normal[4][2] = xy2;
    normal[2][5] = normal[5][2] = xy;

    normal[3][3] = x2;
    normal[3][4] = normal[4][3] = xy;
    normal[3][5] = normal[5][3] = x1;

    normal[4][4] = y2;
    normal[4][5] = normal[5][4] = y1;

    normal[5][5] = N;
}


/****************************************************************/
/* find_obs() - Function to find the observed vector as part of */
/*              the set of normal equations for least squares.  */
/*              V.1.0, Jo Wood, 11th December, 1994.            */

/****************************************************************/

void find_obs(DCELL * z,	/* Local window of elevs.       */
	      double *obs,	/* Observed column vector.      */
	      double *w)
{				/* Weighting matrix.              */

    int row, col,		/* Counts through local window. */
      edge = EDGE,		/* EDGE = (wsize-1)/2.          */
	offset;			/* Array offset for weights & z */

    double x, y;			/* Local window coordinates.    */

    for (row = 0; row < 6; row++)	/* Initialise column vector.        */
	obs[row] = 0.0;


    for (row = 0; row < wsize; row++)
	for (col = 0; col < wsize; col++) {
	    x = resoln * (col - edge);
	    y = resoln * (row - edge);
	    offset = row * wsize + col;

	    obs[0] += *(w + offset) * *(z + offset) * x * x;
	    obs[1] += *(w + offset) * *(z + offset) * y * y;
	    obs[2] += *(w + offset) * *(z + offset) * x * y;
	    obs[3] += *(w + offset) * *(z + offset) * x;
	    obs[4] += *(w + offset) * *(z + offset) * y;

	    if (!constrained)	/* If constrained, should remain 0.0 */
		obs[5] += *(w + offset) * *(z + offset);
	}
}


/****************************************************************/
/* find_weight() Function to find the weightings matrix for the */
/*               observed cell values.                          */
/*               Uses an inverse distance function that can be  */
/*               calibrated with an exponent (0= no decay,      */
/*               1=linear decay, 2=squared distance decay etc.) */
/*               V.1.1, Jo Wood, 11th May, 1995.                */

/****************************************************************/

void find_weight(double *weight_ptr)
{
    int edge = EDGE;		/* EDGE = (wsize-1)/2.          */
    int row, col;		/* Counts through the rows and  */

    /* columns of weights matrix.   */

    double dist;		/* Distance to centre of kernel. */


    /* --- Find inverse distance of all cells to centre. --- */

    for (row = 0; row < wsize; row++)
	for (col = 0; col < wsize; col++) {
	    dist = 1.0 /
		pow(sqrt
		    ((edge - col) * (edge - col) +
		     (edge - row) * (edge - row)) + 1.0, exponent);
	    *(weight_ptr + row * wsize + col) = dist;
	}
}