/****************************************************************/
/* 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 		*/	 

    float  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	*/

    float 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;
	}
}