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