/***********************************************************************
 *
 * MODULE:       lidarlib
 *
 * AUTHOR(S):    Roberto Antolin
 *
 * PURPOSE:      LIDAR Spline Interpolation
 *
 * COPYRIGHT:    (C) 2006 by Politecnico di Milano -
 *                           Polo Regionale di Como
 *
 *               This program is free software under the
 *               GNU General Public License (>=v2).
 *               Read the file COPYING that comes with GRASS
 *               for details.
 *
 **************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <string.h>
#include <grass/lidar.h>

/*----------------------------------------------------------------------------*/
/* Abscissa node index computation */

void node_x(double x, int *i_x, double *csi_x, double xMin, double deltaX)
{

    *i_x = (int)((x - xMin) / deltaX);
    *csi_x = (double)((x - xMin) - (*i_x * deltaX));

    return;
}

/*----------------------------------------------------------------------------*/
/* Ordinate node index computation */

void node_y(double y, int *i_y, double *csi_y, double yMin, double deltaY)
{

    *i_y = (int)((y - yMin) / deltaY);
    *csi_y = (double)((y - yMin) - (*i_y * deltaY));

    return;
}

/*----------------------------------------------------------------------------*/
/* Node order computation */

int order(int i_x, int i_y, int yNum)
{

    return (i_y + i_x * yNum);
}

/*----------------------------------------------------------------------------*/
/* Design matrix coefficients computation */

double phi_3(double csi)
{

    return ((pow(2 - csi, 3.) - pow(1 - csi, 3.) * 4) / 6.);
}

double phi_4(double csi)
{

    return (pow(2 - csi, 3.) / 6.);
}

double phi_33(double csi_x, double csi_y)
{

    return (phi_3(csi_x) * phi_3(csi_y));
}

double phi_34(double csi_x, double csi_y)
{

    return (phi_3(csi_x) * phi_4(csi_y));
}

double phi_43(double csi_x, double csi_y)
{

    return (phi_4(csi_x) * phi_3(csi_y));
}

double phi_44(double csi_x, double csi_y)
{

    return (phi_4(csi_x) * phi_4(csi_y));
}

double phi(double csi_x, double csi_y)
{

    return ((1 - csi_x) * (1 - csi_y));
}

/*----------------------------------------------------------------------------*/
/* Normal system computation for bicubic spline interpolation */

void normalDefBicubic(double **N, double *TN, double *Q, double **obsVect,
		      double deltaX, double deltaY, int xNum, int yNum,
		      double xMin, double yMin, int obsNum, int parNum,
		      int BW)
{

    int i, k, h, m, n, n0;	/* counters             */
    double alpha[4][4];		/* coefficients */

    int i_x;			/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x;

    int i_y;			/* y = (yMin + (i_y * deltaY) + csi_y) */
    double csi_y;

	/*--------------------------------------*/
    for (k = 0; k < parNum; k++) {
	for (h = 0; h < BW; h++)
	    N[k][h] = 0.;	/* Normal matrix inizialization */
	TN[k] = 0.;		/* Normal vector inizialization */
    }

	/*--------------------------------------*/

    for (i = 0; i < obsNum; i++) {

	node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
	node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);

	if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {

	    csi_x = csi_x / deltaX;
	    csi_y = csi_y / deltaY;

	    alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
	    alpha[0][1] = phi_43(1 + csi_x, csi_y);
	    alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
	    alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);

	    alpha[1][0] = phi_34(csi_x, 1 + csi_y);
	    alpha[1][1] = phi_33(csi_x, csi_y);
	    alpha[1][2] = phi_33(csi_x, 1 - csi_y);
	    alpha[1][3] = phi_34(csi_x, 2 - csi_y);

	    alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
	    alpha[2][1] = phi_33(1 - csi_x, csi_y);
	    alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
	    alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);

	    alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
	    alpha[3][1] = phi_43(2 - csi_x, csi_y);
	    alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
	    alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);

	    for (k = -1; k <= 2; k++) {
		for (h = -1; h <= 2; h++) {

		    if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
			((i_y + h) >= 0) && ((i_y + h) < yNum)) {
			for (m = k; m <= 2; m++) {

			    if (m == k)
				n0 = h;
			    else
				n0 = -1;

			    for (n = n0; n <= 2; n++) {
				if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
				    ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
				    N[order(i_x + k, i_y + h, yNum)][order
								     (i_x + m,
								      i_y + n,
								      yNum) -
								     order(i_x
									   +
									   k,
									   i_y
									   +
									   h,
									   yNum)]
					+=
					alpha[k + 1][h +
						     1] * (1 / Q[i]) *
					alpha[m + 1][n + 1];
				    /* 1/Q[i] only refers to the variances */
				}
			    }
			}
			TN[order(i_x + k, i_y + h, yNum)] +=
			    obsVect[i][2] * (1 / Q[i]) * alpha[k + 1][h + 1];
		    }
		}
	    }
	}
    }

    return;
}

/*----------------------------------------------------------------------------*/
/* Normal system correction - Introduzione della correzione dovuta alle 
   pseudosservazioni (Tykonov) - LAPALCIANO - */

void nCorrectLapl(double **N, double lambda, int xNum, int yNum,
		  double deltaX, double deltaY)
{

    int i_x, i_y;		/* counters     */
    int k, h, m, n, n0;		/* counters     */

    double alpha[5][5];		/* coefficients */

    double lambdaX, lambdaY;

	/*--------------------------------------*/
    lambdaX = lambda * (deltaY / deltaX);
    lambdaY = lambda * (deltaX / deltaY);

    alpha[0][0] = 0;
    alpha[0][1] = lambdaX * (1 / 36.);	/* There is lambda because Q^(-1) contains 1/(1/lambda) */
    alpha[0][2] = lambdaX * (1 / 9.);
    alpha[0][3] = lambdaX * (1 / 36.);
    alpha[0][4] = 0;

    alpha[1][0] = lambdaY * (1 / 36.);
    alpha[1][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
    alpha[1][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
    alpha[1][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
    alpha[1][4] = lambdaY * (1 / 36.);

    alpha[2][0] = lambdaY * (1 / 9.);
    alpha[2][1] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
    alpha[2][2] = -lambdaX * (2 / 3.) - lambdaY * (2 / 3.);
    alpha[2][3] = -lambdaX * (1 / 6.) + lambdaY * (2 / 9.);
    alpha[2][4] = lambdaY * (1 / 9.);

    alpha[3][0] = lambdaY * (1 / 36.);
    alpha[3][1] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
    alpha[3][2] = lambdaX * (2 / 9.) - lambdaY * (1 / 6.);
    alpha[3][3] = lambdaX * (1 / 18.) + lambdaY * (1 / 18.);
    alpha[3][4] = lambdaY * (1 / 36.);

    alpha[4][0] = 0;
    alpha[4][1] = lambdaX * (1 / 36.);
    alpha[4][2] = lambdaX * (1 / 9.);
    alpha[4][3] = lambdaX * (1 / 36.);
    alpha[4][4] = 0;

    for (i_x = 0; i_x < xNum; i_x++) {
	for (i_y = 0; i_y < yNum; i_y++) {

	    for (k = -2; k <= 2; k++) {
		for (h = -2; h <= 2; h++) {

		    if (((i_x + k) >= 0) && ((i_x + k) < xNum) &&
			((i_y + h) >= 0) && ((i_y + h) < yNum)) {

			for (m = k; m <= 2; m++) {

			    if (m == k)
				n0 = h;
			    else
				n0 = -2;

			    for (n = n0; n <= 2; n++) {
				if (((i_x + m) >= 0) &&
				    ((i_x + m) <= (xNum - 1)) &&
				    ((i_y + n) >= 0) &&
				    ((i_y + n) <= (yNum - 1))) {

				    if ((alpha[k + 2][h + 2] != 0) &&
					(alpha[m + 2][n + 2] != 0)) {
					N[order(i_x + k, i_y + h, yNum)][order
									 (i_x
									  + m,
									  i_y
									  + n,
									  yNum)
									 -
									 order
									 (i_x
									  + k,
									  i_y
									  + h,
									  yNum)]
					    +=
					    alpha[k + 2][h + 2] * alpha[m +
									2][n +
									   2];
				    }
				}
			    }
			}
		    }
		}
	    }
	}
    }

    return;
}

/*----------------------------------------------------------------------------*/
/* Normal system computation for bilinear spline interpolation */

void normalDefBilin(double **N, double *TN, double *Q, double **obsVect,
		    double deltaX, double deltaY, int xNum, int yNum,
		    double xMin, double yMin, int obsNum, int parNum, int BW)
{

    int i, k, h, m, n, n0;	/* counters     */
    double alpha[2][2];		/* coefficients */

    int i_x;			/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x;

    int i_y;			/* y = (yMin + (i_y * deltaY) + csi_y) */
    double csi_y;

	/*--------------------------------------*/
    for (k = 0; k < parNum; k++) {
	for (h = 0; h < BW; h++)
	    N[k][h] = 0.;	/* Normal matrix inizialization */
	TN[k] = 0.;		/* Normal vector inizialization */
    }

	/*--------------------------------------*/

    for (i = 0; i < obsNum; i++) {

	node_x(obsVect[i][0], &i_x, &csi_x, xMin, deltaX);
	node_y(obsVect[i][1], &i_y, &csi_y, yMin, deltaY);

	if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {

	    csi_x = csi_x / deltaX;
	    csi_y = csi_y / deltaY;

	    alpha[0][0] = phi(csi_x, csi_y);
	    alpha[0][1] = phi(csi_x, 1 - csi_y);
	    alpha[1][0] = phi(1 - csi_x, csi_y);
	    alpha[1][1] = phi(1 - csi_x, 1 - csi_y);

	    for (k = 0; k <= 1; k++) {
		for (h = 0; h <= 1; h++) {

		    if (((i_x + k) >= 0) && ((i_x + k) <= (xNum - 1)) &&
			((i_y + h) >= 0) && ((i_y + h) <= (yNum - 1))) {

			for (m = k; m <= 1; m++) {
			    if (m == k)
				n0 = h;
			    else
				n0 = 0;

			    for (n = n0; n <= 1; n++) {
				if (((i_x + m) >= 0) && ((i_x + m) < xNum) &&
				    ((i_y + n) >= 0) && ((i_y + n) < yNum)) {
				    N[order(i_x + k, i_y + h, yNum)][order
								     (i_x + m,
								      i_y + n,
								      yNum) -
								     order(i_x
									   +
									   k,
									   i_y
									   +
									   h,
									   yNum)]
					+=
					alpha[k][h] * (1 / Q[i]) *
					alpha[m][n];
				    /* 1/Q[i] only refers to the variances */
				}
			    }
			}
			TN[order(i_x + k, i_y + h, yNum)] +=
			    obsVect[i][2] * (1 / Q[i]) * alpha[k][h];
		    }
		}
	    }
	}
    }

    return;
}


/*----------------------------------------------------------------------------*/
/* Normal system correction - Introduzione della correzione dovuta alle 
   pseudosservazioni (Tykonov) - GRADIENTE - */
#ifdef notdef
void nCorrectGrad(double **N, double lambda, int xNum, int yNum,
		  double deltaX, double deltaY)
{

    int i;
    int parNum;

    double alpha[3];
    double lambdaX, lambdaY;

    lambdaX = lambda * (deltaY / deltaX);
    lambdaY = lambda * (deltaX / deltaY);

    parNum = xNum * yNum;

    alpha[0] = lambdaX / 2. + lambdaY / 2.;
    alpha[1] = -lambdaX / 4.;
    alpha[2] = -lambdaY / 4.;

    for (i = 0; i < parNum; i++) {
	N[i][0] += alpha[0];

	if ((i + 2) < parNum)
	    N[i][2] += alpha[2];

	if ((i + 2 * yNum) < parNum)
	    N[i][2 * yNum] += alpha[1];
    }
}
#endif

/*1-DELTA discretization */
void nCorrectGrad(double **N, double lambda, int xNum, int yNum,
		  double deltaX, double deltaY)
{

    int i;
    int parNum;

    double alpha[3];
    double lambdaX, lambdaY;

    lambdaX = lambda * (deltaY / deltaX);
    lambdaY = lambda * (deltaX / deltaY);

    parNum = xNum * yNum;

    alpha[0] = 2 * lambdaX + 2 * lambdaY;
    alpha[1] = -lambdaX;
    alpha[2] = -lambdaY;

    for (i = 0; i < parNum; i++) {
	N[i][0] += alpha[0];

	if ((i + 1) < parNum)
	    N[i][1] += alpha[2];

	if ((i + 1 * yNum) < parNum)
	    N[i][1 * yNum] += alpha[1];
    }

    return;
}

/*----------------------------------------------------------------------------*/
/* Observations estimation */

void obsEstimateBicubic(double **obsV, double *obsE, double *parV,
			double deltX, double deltY, int xNm, int yNm,
			double xMi, double yMi, int obsN)
{

    int i, k, h;		/* counters     */
    double alpha[4][4];		/* coefficients */

    int i_x;			/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x;

    int i_y;			/* y = (yMin + (i_y * deltaY) + csi_y) */
    double csi_y;

    for (i = 0; i < obsN; i++) {

	obsE[i] = 0;

	node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
	node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);

	if ((i_x >= -2) && (i_x <= xNm) && (i_y >= -2) && (i_y <= yNm)) {

	    csi_x = csi_x / deltX;
	    csi_y = csi_y / deltY;

	    alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
	    alpha[0][1] = phi_43(1 + csi_x, csi_y);
	    alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
	    alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);

	    alpha[1][0] = phi_34(csi_x, 1 + csi_y);
	    alpha[1][1] = phi_33(csi_x, csi_y);
	    alpha[1][2] = phi_33(csi_x, 1 - csi_y);
	    alpha[1][3] = phi_34(csi_x, 2 - csi_y);

	    alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
	    alpha[2][1] = phi_33(1 - csi_x, csi_y);
	    alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
	    alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);

	    alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
	    alpha[3][1] = phi_43(2 - csi_x, csi_y);
	    alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
	    alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);

	    for (k = -1; k <= 2; k++) {
		for (h = -1; h <= 2; h++) {
		    if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
			((i_y + h) >= 0) && ((i_y + h) < yNm))
			obsE[i] +=
			    parV[order(i_x + k, i_y + h, yNm)] * alpha[k +
								       1][h +
									  1];
		}
	    }
	}
    }

    return;
}


/*--------------------------------------------------------------------------------------*/
/* Data interpolation in a generic point */

double dataInterpolateBicubic(double x, double y, double deltaX,
			      double deltaY, int xNum, int yNum, double xMin,
			      double yMin, double *parVect)
{

    double z;			/* abscissa, ordinate and associated value */

    int k, h;			/* counters             */
    double alpha[4][4];		/* coefficients */

    int i_x, i_y;		/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x, csi_y;	/* y = (yMin + (i_y * deltaY) + csi_y) */


    z = 0;

    node_x(x, &i_x, &csi_x, xMin, deltaX);
    node_y(y, &i_y, &csi_y, yMin, deltaY);

    if ((i_x >= -2) && (i_x <= xNum) && (i_y >= -2) && (i_y <= yNum)) {

	csi_x = csi_x / deltaX;
	csi_y = csi_y / deltaY;

	alpha[0][0] = phi_44(1 + csi_x, 1 + csi_y);
	alpha[0][1] = phi_43(1 + csi_x, csi_y);
	alpha[0][2] = phi_43(1 + csi_x, 1 - csi_y);
	alpha[0][3] = phi_44(1 + csi_x, 2 - csi_y);

	alpha[1][0] = phi_34(csi_x, 1 + csi_y);
	alpha[1][1] = phi_33(csi_x, csi_y);
	alpha[1][2] = phi_33(csi_x, 1 - csi_y);
	alpha[1][3] = phi_34(csi_x, 2 - csi_y);

	alpha[2][0] = phi_34(1 - csi_x, 1 + csi_y);
	alpha[2][1] = phi_33(1 - csi_x, csi_y);
	alpha[2][2] = phi_33(1 - csi_x, 1 - csi_y);
	alpha[2][3] = phi_34(1 - csi_x, 2 - csi_y);

	alpha[3][0] = phi_44(2 - csi_x, 1 + csi_y);
	alpha[3][1] = phi_43(2 - csi_x, csi_y);
	alpha[3][2] = phi_43(2 - csi_x, 1 - csi_y);
	alpha[3][3] = phi_44(2 - csi_x, 2 - csi_y);

	for (k = -1; k <= 2; k++) {
	    for (h = -1; h <= 2; h++) {
		if (((i_x + k) >= 0) && ((i_x + k) < xNum) && ((i_y + h) >= 0)
		    && ((i_y + h) < yNum))
		    z += parVect[order(i_x + k, i_y + h, yNum)] * alpha[k +
									1][h +
									   1];
	    }
	}
    }

    return z;
}


/*----------------------------------------------------------------------------*/
/* Observations estimation */

void obsEstimateBilin(double **obsV, double *obsE, double *parV, double deltX,
		      double deltY, int xNm, int yNm, double xMi, double yMi,
		      int obsN)
{

    int i, k, h;		/* counters     */
    double alpha[2][2];		/* coefficients */

    int i_x;			/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x;

    int i_y;			/* y = (yMin + (i_y * deltaY) + csi_y) */
    double csi_y;

    for (i = 0; i < obsN; i++) {

	obsE[i] = 0;

	node_x(obsV[i][0], &i_x, &csi_x, xMi, deltX);
	node_y(obsV[i][1], &i_y, &csi_y, yMi, deltY);

	if ((i_x >= -1) && (i_x < xNm) && (i_y >= -1) && (i_y < yNm)) {

	    csi_x = csi_x / deltX;
	    csi_y = csi_y / deltY;

	    alpha[0][0] = phi(csi_x, csi_y);
	    alpha[0][1] = phi(csi_x, 1 - csi_y);
	    alpha[1][0] = phi(1 - csi_x, csi_y);
	    alpha[1][1] = phi(1 - csi_x, 1 - csi_y);

	    for (k = 0; k <= 1; k++) {
		for (h = 0; h <= 1; h++) {
		    if (((i_x + k) >= 0) && ((i_x + k) < xNm) &&
			((i_y + h) >= 0) && ((i_y + h) < yNm))
			obsE[i] +=
			    parV[order(i_x + k, i_y + h, yNm)] * alpha[k][h];
		}
	    }
	}
    }

    return;
}


/*--------------------------------------------------------------------------------------*/
/* Data interpolation in a generic point */

double dataInterpolateBilin(double x, double y, double deltaX, double deltaY,
			    int xNum, int yNum, double xMin, double yMin,
			    double *parVect)
{

    double z;			/* abscissa, ordinate and associated value */

    int k, h;			/* counters             */
    double alpha[2][2];		/* coefficients */

    int i_x, i_y;		/* x = (xMin + (i_x * deltaX) + csi_x) */
    double csi_x, csi_y;	/* y = (yMin + (i_y * deltaY) + csi_y) */

    z = 0;

    node_x(x, &i_x, &csi_x, xMin, deltaX);
    node_y(y, &i_y, &csi_y, yMin, deltaY);

    if ((i_x >= -1) && (i_x < xNum) && (i_y >= -1) && (i_y < yNum)) {

	csi_x = csi_x / deltaX;
	csi_y = csi_y / deltaY;

	alpha[0][0] = phi(csi_x, csi_y);
	alpha[0][1] = phi(csi_x, 1 - csi_y);

	alpha[1][0] = phi(1 - csi_x, csi_y);
	alpha[1][1] = phi(1 - csi_x, 1 - csi_y);

	for (k = 0; k <= 1; k++) {
	    for (h = 0; h <= 1; h++) {
		if (((i_x + k) >= 0) && ((i_x + k) < xNum) && ((i_y + h) >= 0)
		    && ((i_y + h) < yNum))
		    z += parVect[order(i_x + k, i_y + h, yNum)] * alpha[k][h];
	    }
	}
    }

    return z;
}