/*
 * This file is part of the GIRAFFE Pipeline
 * Copyright (C) 2002-2019 European Southern Observatory
 *
 * 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; either version 2 of the License, or
 * (at your option) any later version.
 *
 * 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif

#include <math.h>

#include <cxmemory.h>
#include <cxmessages.h>

#include "gimath.h"


/**
 * defgroup gimath Mathematical Utility Functions
 *
 * The module implements mathematical utilities like Chebyshev polynomials,
 * non-linear fitting routines etc.
 */

/**@{*/

inline static cxdouble
_giraffe_chebyshev2d_eval(cxdouble ax, cxdouble bx, cxdouble ay, cxdouble by,
                          const cpl_matrix* coeffs, cxdouble x, cxdouble y)
{

    cxint i, k;
    cxint xorder;
    cxint yorder;

    const cxdouble* _coeffs = NULL;

    cxdouble x_n = (2.0 * x - ax - bx) / (bx - ax);
    cxdouble y_n = (2.0 * y - ay - by) / (by - ay);
    cxdouble cx0 = 1.0;
    cxdouble cx1 = x_n;
    cxdouble sum = 0.;


    cx_assert(coeffs != NULL);

    xorder = cpl_matrix_get_nrow(coeffs);
    yorder = cpl_matrix_get_ncol(coeffs);

    _coeffs = cpl_matrix_get_data_const(coeffs);
    cx_assert(_coeffs != NULL);

    for (i = 0, k = 0; i < xorder; i++) {

        register cxint j;

        register cxdouble cx2 = 0.;
        register cxdouble cy0 = 1.0;
        register cxdouble cy1 = y_n;


        if (i < 2) {
            cx2 = cx0;
        }
        else {
            cx2 = 2.0 * cx1 * x_n - cx0;
        }

        for (j = 0; j < yorder; j++) {

            cxdouble cy2 = 0.;

            if (j < 2) {
                cy2 = cy0;
            }
            else {
                cy2 = 2.0 * cy1 * y_n - cy0;
            }

            sum += cx2 * cy2 * _coeffs[k++];

            cy0 = cy1;
            cy1 = cy2;

        }

        cx0 = cx1;
        cx1 = cx2;

    }

    return sum;

}


cxdouble
giraffe_interpolate_linear(cxdouble x, cxdouble x_0, cxdouble y_0,
                           cxdouble x_1, cxdouble y_1)
{

    register cxdouble t = (x - x_0) / (x_1 - x_0);

    return (1. - t) * y_0 + t * y_1;

}


/**
 * @brief
 *   Computes 1D Chebyshev base
 *
 * @param  start   start value for X vector
 * @param  size    size/normalization factor for X vector
 * @param  order   Chebyshev polynom order
 * @param  m_x     X vector
 *
 * @return mBase - matrix[order,size] Chebyshev base.
 *
 * @see giraffe_chebyshev_base2d
 */

cpl_matrix *
giraffe_chebyshev_base1d(cxdouble start, cxdouble size, cxint order,
                         cpl_matrix *m_x)
{

    register cxint32 i, j, x_nrow, t_ncol;

    register cxdouble xn, dsize2, d2size;

    cxdouble *pmx, *pmt;

    cpl_matrix *m_t = NULL;


    /*
     * Preprocessing
     */

    dsize2 = size / 2.0;
    d2size = 2.0 / size;

    x_nrow = cpl_matrix_get_nrow(m_x);

    m_t = cpl_matrix_new(order, x_nrow);

    if (m_t == NULL) {
        return NULL;
    }

    t_ncol = x_nrow;

    pmx = cpl_matrix_get_data(m_x);
    pmt = cpl_matrix_get_data(m_t);


    /*
     * Processing
     */

    for (j = 0; j < t_ncol; j++) {

        /*
         *  Normalize array between [-1,1]
         *  x[n] = (array[n] - start - size / 2.0) * 2.0 / size;
         */

        xn = (pmx[j] - start - dsize2) * d2size;

        /*
         *  Ones for first column: T[0] = 1.0
         */

        pmt[j] = 1.0;

        if (order < 2) {
            continue;
        }

        /*
         *  X for second column: T[1] = x
         */

        pmt[j+t_ncol] = xn;

        for (i = 2; i < order; i++) {

            /*
             * compute Chebyshev coefficients for following columns
             * with: T[k] = 2 * x * T[k-1] - T[k-2]
             */

            pmt[j+i*t_ncol] = 2.0 * xn * pmt[j+(i-1)*t_ncol] -
                pmt[j+(i-2)*t_ncol];

        }
    }

    return m_t;

}


/**
 * @brief
 *   Computes 2D Chebyshev base
 *
 * @param  xstart  start value for X vector
 * @param  ystart  start value for Y vector
 * @param  xsize   size/normalization factor for X vector
 * @param  ysize   size/normalization factor for Y vector
 * @param  xorder  X Chebyshev polynom order
 * @param  yorder  Y Chebyshev polynom order
 * @param  m_x     X vector [nx]
 * @param  m_y     Y vector [nx]
 *
 * @return mBase - matrix[xorder*yorder,nx] Chebyshev base.
 *
 * @see giraffe_chebyshev_base1d
 *
 * @warning This function needs some thorough testing
 */

cpl_matrix *
giraffe_chebyshev_base2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
                         cxdouble ysize, cxint xorder, cxint yorder,
                         cpl_matrix *m_x, cpl_matrix *m_y)
{

    register cxint32 i, j, k, l;

    cxint32 x_nrow, y_nrow, /*t_nrow,*/ t_ncol;

    register cxdouble dxsize2, d2xsize;
    register cxdouble dysize2, d2ysize;
    register cxdouble x_n, y_n;
    register cxdouble bx0, bx1, bx2, by0, by1, by2;

    cxdouble *pmx, *pmy, *pmt;

    cpl_matrix *m_t = NULL;


    /*
     *  Preprocsesing
     */

    dxsize2 = xsize / 2.0;
    d2xsize = 2.0 / xsize;
    dysize2 = ysize / 2.0;
    d2ysize = 2.0 / ysize;

    x_nrow = cpl_matrix_get_nrow(m_x);
    y_nrow = cpl_matrix_get_nrow(m_y);

    if (x_nrow != y_nrow) {
        return NULL;
    }

    m_t = cpl_matrix_new(xorder * yorder, x_nrow);

    if (m_t == NULL) {
        return NULL;
    }

    /*t_nrow = cpl_matrix_get_nrow(m_t);*/
    t_ncol = cpl_matrix_get_ncol(m_t);

    pmx = cpl_matrix_get_data(m_x);
    pmy = cpl_matrix_get_data(m_y);
    pmt = cpl_matrix_get_data(m_t);


    /*
     * Processing
     */

    for (j = 0; j < t_ncol; j++) {

        /*
         *  Normalize array between [-1,1]
         *  x[n] = (array[n] - start - size / 2.0) * 2.0 / size;
         */

        x_n = (pmx[j] - xstart - dxsize2) * d2xsize;
        y_n = (pmy[j] - ystart - dysize2) * d2ysize;

        bx0 = 1.0;
        bx1 = x_n;

        for (l = 0,i = 0; i < xorder; i++) {

            if (i < 2) {
                bx2 = bx0;
            }
            else {
                bx2 = 2.0 * bx1 * x_n - bx0;
            }

            by0 = 1.0;
            by1 = y_n;

            for (k = 0; k < yorder; k++) {

                if (k < 2) {
                    by2 = by0;
                }
                else {
                    by2 = 2.0 * by1 * y_n - by0;
                }

                pmt[j + (l++) * t_ncol] = bx2 * by2;

                by0 = by1;
                by1 = by2;

            }

            bx0 = bx1;
            bx1 = bx2;

        }
    }

    return m_t;

}


/**
 * @brief
 *   Computes 2D Chebyshev base (transposed matrix)
 *
 * @param  xstart  start value for X vector
 * @param  ystart  start value for Y vector
 * @param  xsize   size/normalization factor for X vector
 * @param  ysize   size/normalization factor for Y vector
 * @param  xorder  X Chebyshev polynom order
 * @param  yorder  Y Chebyshev polynom order
 * @param  m_x     X vector [nx]
 * @param  m_y     X vector [nx]
 *
 * @return mBase - matrix[xorder*yorder,nx] Chebyshev base.
 *
 * @see giraffe_chebyshev_base1d
 */

cpl_matrix *
giraffe_chebyshev_base2dt(cxdouble xstart, cxdouble ystart, cxdouble xsize,
                          cxdouble ysize, cxint xorder, cxint yorder,
                          cpl_matrix *m_x, cpl_matrix *m_y)
{

    register cxint32 i, j, k, l;

    cxint32 x_nrow, y_nrow, t_nrow, t_ncol;

    register cxdouble dxsize2, d2xsize;
    register cxdouble dysize2, d2ysize;
    register cxdouble x_n, y_n;
    register cxdouble bx0, bx1, bx2, by0, by1, by2;

    cxdouble *pmx, *pmy, *pmt;

    cpl_matrix *m_t = NULL;


    /*
     * Preprocess
     */

    dxsize2 = xsize / 2.0;
    d2xsize = 2.0 / xsize;
    dysize2 = ysize / 2.0;
    d2ysize = 2.0 / ysize;

    x_nrow = cpl_matrix_get_nrow(m_x);
    y_nrow = cpl_matrix_get_nrow(m_y);

    if (x_nrow != y_nrow) {
        return NULL;
    }

    m_t = cpl_matrix_new(x_nrow, xorder * yorder);

    if (m_t == NULL) {
        return NULL;
    }

    t_nrow = cpl_matrix_get_nrow(m_t);
    t_ncol = cpl_matrix_get_ncol(m_t);

    pmx = cpl_matrix_get_data(m_x);
    pmy = cpl_matrix_get_data(m_y);
    pmt = cpl_matrix_get_data(m_t);


    /*
     * Process
     */

    for (j = 0; j < t_nrow; j++) {

        /*
         *  Normalize array between [-1,1]
         *  x[n] = (array[n] - start - size / 2.0) * 2.0 / size;
         */

        x_n = (pmx[j] - xstart - dxsize2) * d2xsize;
        y_n = (pmy[j] - ystart - dysize2) * d2ysize;

        bx0 = 1.0;
        bx1 = x_n;

        for (l = 0, i = 0; i < xorder; i++) {

            if (i < 2) {
                bx2 = bx0;
            }
            else {
                bx2 = 2.0 * bx1 * x_n - bx0;
            }

            by0 = 1.0;
            by1 = y_n;

            for (k = 0; k < yorder; k++) {

                if (k < 2) {
                    by2 = by0;
                }
                else {
                    by2 = 2.0 * by1 * y_n - by0;
                }

                pmt[j * t_ncol + (l++)] = bx2 * by2;

                by0 = by1;
                by1 = by2;

            }

            bx0 = bx1;
            bx1 = bx2;

        }
    }

    return m_t;
}


/**
 * @brief
 *   Computes 1D Chebyshev fit
 *
 * @param  start  start value for X vector
 * @param  size   size/normalization factor for X vector
 * @param  m_c    Chebyshev coefficients [xorder]
 * @param  m_x    X vector[nx]
 *
 * @return mFit - matrix[nx] Chebyshev fit.
 *
 * @see giraffe_chebyshev_base1d, giraffe_chebyshev_fit2d
 */

cpl_matrix *
giraffe_chebyshev_fit1d(cxdouble start, cxdouble size, cpl_matrix *m_c,
                        cpl_matrix *m_x)
{

    register cxint32 i, j, jj, order;

    cxint32 x_nrow, c_nrow, c_ncol, t_nrow, t_ncol;

    register cxdouble xn;
    register cxdouble dsize2, d2size;
    register cxdouble *f0 = NULL;
    register cxdouble *t0 = NULL;
    register cxdouble *c0 = NULL;

    cxdouble *pmc, *pmx, *pmt, *pmf;

    cpl_matrix *m_t;   /* 1D Chabyshev base transposed */
    cpl_matrix *m_f;   /* 1D Chabyshev fit */


    /*
     * Preprocess
     */

    dsize2 = size / 2.0;
    d2size = 2.0 / size;

    c_nrow = cpl_matrix_get_nrow(m_c);
    c_ncol = cpl_matrix_get_ncol(m_c);
    x_nrow = cpl_matrix_get_nrow(m_x);
    order  = c_nrow;

    m_t = cpl_matrix_new(x_nrow, order);

    if (m_t == NULL) {
        return NULL;
    }

    m_f = cpl_matrix_new(c_nrow, x_nrow);

    if (m_f == NULL) {
        cpl_matrix_delete(m_t);
        return NULL;
    }

    t_nrow = cpl_matrix_get_nrow(m_t);
    t_ncol = cpl_matrix_get_ncol(m_t);

    pmc = cpl_matrix_get_data(m_c);
    pmx = cpl_matrix_get_data(m_x);
    pmt = cpl_matrix_get_data(m_t);
    pmf = cpl_matrix_get_data(m_f);


    /*
     * Process
     */

    for (j = 0; j < t_nrow; j++) {

        /*
         *  Normalize array between [-1,1]
         *  x[n] = (array[n] - start - size / 2.0) * 2.0 / size;
         */

        xn = (pmx[j] - start - dsize2) * d2size;

        /*
         *  Ones for first column: T[0] = 1.0
         */

        jj = j * t_ncol;

        pmt[jj] = 1.0;

        if (order < 2) {
            continue;
        }

        /*
         *  X for second column: T[1] = x
         */

        pmt[jj+1] = xn;

        for (i = 2; i < order; i++) {

            /*
             *  Compute Chebyshev coefficients for following columns
             *  with: T[k] = 2 * x * T[k-1] - T[k-2]
             */

            pmt[jj + i] = 2.0 * xn * pmt[jj + (i - 1)] - pmt[jj + (i - 2)];

        }
    }

    for (i = 0, f0 = pmf ; i < c_nrow; i++) {
        for (j = 0, t0 = pmt; j < t_nrow; j++, f0++) {
            for (jj = 0, *f0 = 0, c0 = pmc + i * c_ncol; jj < c_ncol; jj++) {
                *f0 += *c0++ * *t0++;
            }
        }
    }

    cpl_matrix_delete(m_t);

    return m_f;

}


/**
 * @brief
 *   Computes 2D Chebyshev fit
 *
 * @param  xstart start value for X vector
 * @param  ystart start value for Y vector
 * @param  xsize  size/normalization factor for X vector
 * @param  ysize  size/normalization factor for Y vector
 * @param  m_c    Chebyshev coefficients [xorder,yorder]
 * @param  m_x    X vector [nx]
 * @param  m_y    Y vector [nx]
 *
 * @return mFit - matrix[nx] Chebyshev fit.
 *
 * @see giraffe_chebyshev_fit1d, giraffe_chebyshev_base2d
 */

cpl_matrix *
giraffe_chebyshev_fit2d(cxdouble xstart, cxdouble ystart, cxdouble xsize,
                        cxdouble ysize, const cpl_matrix* m_c,
                        const cpl_matrix* m_x, const cpl_matrix* m_y)
{

    cxint i;
    cxint nx;

    const cxdouble* _x = NULL;
    const cxdouble* _y = NULL;

    cxdouble bx = xstart + xsize;
    cxdouble by = ystart + ysize;

    cpl_matrix *f = NULL;


    if (m_c == NULL || m_x == NULL || m_y == NULL) {
        return NULL;
    }

    nx = cpl_matrix_get_nrow(m_x);

    if (nx != cpl_matrix_get_nrow(m_y)) {
        return NULL;
    }

    f = cpl_matrix_new(nx, 1);

    if (f == NULL) {
        return NULL;
    }

    _x = cpl_matrix_get_data_const(m_x);
    _y = cpl_matrix_get_data_const(m_y);


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

        cxdouble sum = _giraffe_chebyshev2d_eval(xstart, bx, ystart, by,
                                                 m_c, _x[i], _y[i]);
        cpl_matrix_set(f, i, 0, sum);

    }

    return f;

}


#define SWAP(a,b) {swap=(a);(a)=(b);(b)=swap;}

/**
 * @brief Linear equation solution by Gauss-Jordan elimination.
 *
 * @par Synopsys:
 * @code
 *   #include <MathUtils/MatrixOperations.h>
 *
 *   Matrix mA,mB;
 *
 *   res = gauss_jordan(mA,n,mB,m);
 * @endcode
 *
 * @param mA       - input matrix [n,n]
 * @param n        - size of input matrix
 * @param mB       - right-hand side matrix [n,m]
 * @param m        - size of right-hand side matrix
 *
 * @par Description:
 *  @a mA[1..n][1..n] is the input matrix. @a mB[1..n][1..m] is input
 *  containing the @a m right-hand side vectors. On output, @a mA is replaced
 *  by its matrix inverse,
 *  and @a mB is replaced by the corresponding set of solution vectors.
 *
 *  @retval 0 on success, -1 or -2 if Matrix singular
 */


cxint
giraffe_gauss_jordan(
    cpl_matrix *mA,
    cxint       n,
    cpl_matrix *mB,
    cxint       m
) {

    cxint          *indxc, *indxr, *ipiv;
    register cxint i, icol = 0, irow = 0, j, jj, k, l, ll;
    register cxdouble  big, dum, pivinv, swap;

    cxdouble *pd_mA = NULL, *pd_mB = NULL;
    cxint     nr_mA, nr_mB;

    pd_mA = cpl_matrix_get_data(mA);
    pd_mB = cpl_matrix_get_data(mB);
    nr_mA = cpl_matrix_get_nrow(mA);
    nr_mB = cpl_matrix_get_nrow(mB);

    indxc = (cxint *) cx_calloc(n, sizeof(cxint));
    indxr = (cxint *) cx_calloc(n, sizeof(cxint));
    ipiv  = (cxint *) cx_calloc(n, sizeof(cxint));

    for (i = 0; i < n; i++) {
        /* main loop over the columns to be reduced */
        big = 0.0;
        for (j = 0; j < n; j++) {
            /* outer loop of the search of a pivot element */
            jj = j * nr_mA;
            if (ipiv[j] != 1) {
                for (k = 0; k < n; k++) {
                    if (ipiv[k] == 0) {
                        if (fabs(pd_mA[jj + k]) >= big) {
                            big = fabs(pd_mA[jj + k]);
                            irow = j;
                            icol = k;
                        }
                    } else if (ipiv[k] > 1) {
                        cx_free((cxptr) ipiv);
                        cx_free((cxptr) indxr);
                        cx_free((cxptr) indxc);
                        /* matrix singular 1 */
                        return -1;
                    }
                }
            }
        }

        /* We now have the pivot, so we interchange rows, if needed, to put
         * the pivot element on the diagonal. The columns are not physically
         * interchanged, only relabeled
         */

        ++(ipiv[icol]);

        if (irow != icol) {
            for (l = 0; l < n; l++) {
                SWAP(pd_mA[irow * nr_mA + l], pd_mA[icol * nr_mA + l])
            }
            for (l = 0; l < m; l++) {
                SWAP(pd_mB[irow * nr_mB + l], pd_mB[icol * nr_mB + l])
            }
        }

        indxr[i] = irow;
        indxc[i] = icol;

        if (pd_mA[icol * nr_mA + icol] == 0.0) {
            cx_free((cxptr) ipiv);
            cx_free((cxptr) indxr);
            cx_free((cxptr) indxc);
            /* matrix singular 2 */
            return -2;
        }

        /* divide the pivot row by the pivot element */
        pivinv = 1.0 / pd_mA[icol * nr_mA + icol];
        pd_mA[icol * nr_mA + icol] = 1.0;

        for (l = 0; l < n; l++) {
            pd_mA[icol * nr_mA + l] *= pivinv;
        }

        for (l = 0; l < m; l++) {
            pd_mB[icol * nr_mB + l] *= pivinv;
        }

        for (ll = 0; ll < n; ll++) {
            /* reduce the rows... except for the pivot one */
            if (ll != icol) {
                dum = pd_mA[ll * nr_mA + icol];
                pd_mA[ll * nr_mA + icol] = 0.0;

                for (l = 0; l < n; l++) {
                    pd_mA[ll * nr_mA + l] -= pd_mA[icol * nr_mA + l] * dum;
                }

                for (l = 0; l < m; l++) {
                    pd_mB[ll * nr_mB + l] -= pd_mB[icol * nr_mB + l] * dum;
                }
            }
        }
    }

    cx_free((cxptr) ipiv);

    /* unscramble the solution in view of the column interchanges */
    for (l = (n-1); l >= 0; l--) {
        if (indxr[l] != indxc[l]) {
            for (k = 0; k < n; k++) {
                SWAP(pd_mA[k * nr_mA + indxr[l]], pd_mA[k * nr_mA + indxc[l]]);
            }
        }
    }
    cx_free((cxptr)indxr);
    cx_free((cxptr)indxc);

    return 0;

} /* end of gauss_jordan() */

#undef SWAP

/**
 *  @brief  compute abcissa ordinate mesh.
 *
 * @par Synopsys:
 * @code
 *   #include <giraffe.h>
 *   #include <Preprocessing/slight.h>
 *
 *   Matrix  mXi, mYi;
 *
 *   computeImageCoordinates(nx, ny, mXi, mYi);
 * @endcode
 *
 *  @param nx  - size of mesh in X
 *  @param ny  - size of mesh in Y
 *
 *  @retval mXi - abcissa indices [nx,ny].
 *  @retval mYi - ordinates indices [nx,ny].
 *
 * @par Description:
 * returns two real array @a mXi[nx,ny] and @a mYi[nx,ny] representing
 * a grid of indices with row-only, and column-only variation.
 * @a mXi[nx,ny] and @a mYi[nx,ny] should already exist.
 *
 */

void
giraffe_compute_image_coordinates(
    cxlong nx,
    cxlong ny,
    cpl_matrix *mXi,
    cpl_matrix *mYi
) {

    register cxlong i, j, k;
    register cxdouble *pd_mXi = NULL, *pd_mYi = NULL;

    if ((mXi != NULL) && (mYi != NULL)) {

        pd_mXi = cpl_matrix_get_data(mXi);
        pd_mYi = cpl_matrix_get_data(mYi);

        for (j = 0; j < nx; j++) {
            for (k = 0; k < ny; k++) {
                i = k + j * ny;
                pd_mXi[i] = (cxdouble) j;
                pd_mYi[i] = (cxdouble) k;
            }
        }
    } else if (mXi != NULL) {

        pd_mXi = cpl_matrix_get_data(mXi);

        for (j = 0; j < nx; j++) {
            for (k = 0; k < ny; k++) {
                i = k + j * ny;
                pd_mXi[i] = (cxdouble) j;
            }
        }
    } else if (mYi != NULL) {

        pd_mYi = cpl_matrix_get_data(mYi);

        for (j = 0; j < nx; j++) {
            for (k = 0; k < ny; k++) {
                i = k + j * ny;
                pd_mYi[i] = (cxdouble) k;
            }
        }
    }

    return;

} /* end of giraffe_compute_image_coordinates() */

/**@}*/
