/*
 * 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 <cxmessages.h>
#include <cxstring.h>

#include <cpl_msg.h>
#include <cpl_error.h>

#include "gimatrix.h"


/**
 * @defgroup gimatrix Matrix Utilities
 *
 * TBD
 */

/**@{*/

inline static void
_giraffe_swap(cxdouble *a, cxdouble *b)
{
    register cxdouble tmp = *a;

    *a = *b;
    *b = tmp;

    return;

}


inline static cxbool
_giraffe_tiny(cxdouble a)
{
    return a < 0. ? (a > -1.e-30) : (a < 1.e-30);
}

#ifdef GIRAFFE_USE_giraffe_matrix_gausspiv
/*
 * @brief  matrix_gausspiv
 *
 * @param ptra      A matrix line.
 * @param ptrc      A matrix line.
 * @param n         Number of rows in each line.
 *
 * @retval          int 1 if Ok, 0 else.
 *
 * Line simplification with Gauss method.
 *
 * The matrices @em ms[nx,ns], @em mse[nx,ns], @em msn[nx,ns] and
 * @em msy[nx,ns] are pre-allocated matrices.
 */

static cxint
_giraffe_matrix_gausspiv(cxdouble *ptra, cxdouble *ptrc, cxint n)
/* c(n,n) = a(n,n)^-1 */
{

    register cxint i;
    register cxint j;
    register cxint k;
    register cxint l;

    cxint maj;

    cxdouble max;
    cxdouble r;
    cxdouble t;
    cxdouble *ptrb;


    ptrb = (cxdouble *)cx_calloc(n * n, sizeof(cxdouble));

    for(i = 0; i < n; i++) {
        ptrb[i * n + i] = 1.0;
    }

    for (i = 1; i <= n; i++) {

        /* Search max in current column  */
        max = CX_ABS(*(ptra + n * i - n));
        maj = i;

        for (j = i; j <= n; j++) {
            if (CX_ABS(*(ptra + n * j + i - n - 1)) > max) {
                maj = j;
                max = CX_ABS(*(ptra + n * j + i - n - 1));
            }
        }

        /* swap lines i and maj */
        if (maj != i) {
            for (j = i;j <= n;j++) {
                r = *(ptra + n * maj + j - n - 1);
                *(ptra + n * maj + j - n - 1) = *(ptra + n * i + j - n - 1);
                *(ptra + n * i + j - n - 1) = r;
            }

            for(l = 0; l < n; l++) {
                r = *(ptrb + l * n + maj - 1);
                *(ptrb + l * n + maj - 1) = *(ptrb + l * n + i - 1);
                *(ptrb + l * n + i - 1) = r;
            }
        }

        /* Subtract line by line */
        for (j = i + 1; j <= n; j++) {
            t = (*(ptra + (n + 1) * i - n - 1));
            if (_giraffe_tiny(t) == TRUE) {
                return 0;
            }
            r = (*(ptra + n * j + i - n - 1)) / t;
            for(l = 0; l < n; l++) {
                *(ptrb + l * n + j - 1) -= r * (*(ptrb + l * n + i - 1));
            }
            for (k = i; k <= n; k++) {
                *(ptra + n * j + k - n - 1) -=
                    r * (*(ptra + n * i + k - n - 1));
            }
        }
    }

    /* Triangular system resolution */
    for(l = 0; l < n; l++) {
        for (i = n; i >= 1; i--) {
            t = (*(ptra + (n + 1) * i - n - 1));
            if (_giraffe_tiny(t) == TRUE) {
                return 0;
            }
            *(ptrc + l + (i - 1) * n) = (*(ptrb + l * n + i - 1)) / t;
            if (i > 1) {
                for (j = i - 1;j > 0;j--) {
                    *(ptrb + l * n + j - 1) -=
                        (*(ptra + n * j + i - n - 1)) *
                        (*(ptrc + l + (i - 1) * n));
                }
            }
        }
    }
    cx_free(ptrb);

    return 1;
}
#endif /* GIRAFFE_USE_giraffe_matrix_gausspiv */

/*static cpl_matrix *
_giraffe_matrix_inverse(cpl_matrix *aa)
{
    cxint test = 1;
    cxint aa_ncol = 0;
    cxint aa_nrow = 0;

    cxdouble *pd_temp = NULL;
    cxdouble *pd_bb = NULL;

    cpl_matrix *bb = NULL;
    cpl_matrix *temp = NULL;

    aa_ncol = cpl_matrix_get_ncol(aa);
    aa_nrow = cpl_matrix_get_nrow(aa);

    if(aa_nrow != aa_ncol) {
        return NULL;
    }

    bb = cpl_matrix_new(aa_nrow, aa_ncol);

    temp = cpl_matrix_duplicate(aa);

    pd_temp = cpl_matrix_get_data(temp);
    pd_bb = cpl_matrix_get_data(bb);

    if (_giraffe_matrix_gausspiv(pd_temp, pd_bb, aa_nrow) == 0) {
        test = 0;
    }

    cpl_matrix_delete(temp);

    if (test == 0) {
        cpl_matrix_delete(bb);
        return NULL;
    }

    return bb;
}*/


/**
 * @brief
 *   Compute sigma of matrix elements, with a given mean value
 *
 * @param matrix  Input matrix
 * @param mean    Value to be used as mean value during calculation
 *
 * @return
 *   Sigma of matrix
 *
 * Function computes sigma of matrix elements, with a given mean value
 *
 * @note
 *  This function might be moved to CPL.
 */

cxdouble
giraffe_matrix_sigma_mean(const cpl_matrix *matrix, cxdouble mean)
{

    cxulong size = 0;
    cxulong size2 = 0;

    const cxdouble *pt = NULL;

    cxdouble diff = 0.;
    cxdouble sigma = 0.;


    cx_assert(matrix != NULL);

    size = cpl_matrix_get_ncol(matrix) * cpl_matrix_get_nrow(matrix);
    size2 = size - 1;

    pt = cpl_matrix_get_data_const(matrix);

    while (size--) {
        diff = *pt++ - mean;
        sigma += diff * diff;
    }

    return sqrt(sigma / (cxdouble)size2);

}


/**
 * @brief
 *   Compute sigma of matrix fit
 *
 * @param  matrix      Original matrix
 * @param  matrix_fit  Matrix to compare
 *
 * @return
 *   Sigma of fit
 *
 * Function computes sigma of fit between two matrixes
 *
 * @note
 *   This function may be moved to CPL
 */

cxdouble
giraffe_matrix_sigma_fit(const cpl_matrix *matrix,
                         const cpl_matrix *matrix_fit)
{

    cxint ancol;
    cxint anrow;
    cxint fncol;
    cxint fnrow;

    cxulong size;
    cxulong size2;

    const cxdouble *pta = NULL;
    const cxdouble *ptf = NULL;

    cxdouble diff  = 0.;
    cxdouble sigma = 0.;


    cx_assert(matrix != NULL);
    cx_assert(matrix_fit != NULL);

    ancol = cpl_matrix_get_ncol(matrix);
    anrow = cpl_matrix_get_nrow(matrix);
    fncol = cpl_matrix_get_ncol(matrix_fit);
    fnrow = cpl_matrix_get_nrow(matrix_fit);

    if ((ancol * anrow) != (fncol * fnrow)) {
        return 0.0;
    }

    size  = ancol * anrow;
    size2 = size - 1;

    pta = cpl_matrix_get_data_const(matrix);
    ptf = cpl_matrix_get_data_const(matrix_fit);

    while (size--) {
        diff = *pta++ - *ptf++;
        sigma += diff * diff;
    }

    return sqrt(sigma / (cxdouble) size2);

}


/**
 * @brief
 *   Converts a matrix into an image.
 *
 * @param matrix  The matrix to convert.
 *
 * @return
 *   The newly image created from the matrix, or @c NULL if the
 *   image cannot be created.
 *
 * The function converts a CPL matrix into a CPL image. The matrix elements
 * are copied into the image pixel buffer.
 */

cpl_image *
giraffe_matrix_create_image(const cpl_matrix *matrix)
{

    cpl_image *image = NULL;


    if (matrix) {
        cxint nx = cpl_matrix_get_ncol(matrix);
        cxint ny = cpl_matrix_get_nrow(matrix);


        image = cpl_image_new(nx, ny, CPL_TYPE_DOUBLE);

        if (image) {
            cxsize sz = nx * ny;
            cxdouble *pixels = cpl_image_get_data_double(image);

            memcpy(pixels, cpl_matrix_get_data_const(matrix),
                   sz * sizeof(cxdouble));
        }
    }

    return image;

}

#define PIX_STACK_SIZE 50

/**
 * @brief
 *   Sort in place the matrix elements in ascending order.
 *
 * @param mA  The matrix to sort.
 *
 * @return
 *   0 on success, -1 on failure
 *
 * The input matrix @em mA is sorted in place. On the successfull return
 * the matrix @em mA contains the original values sorted in ascending order.
 */


cxint
giraffe_matrix_sort(cpl_matrix *mA)
{
    register cxint i;
    register cxint ir;
    register cxint j;
    register cxint j_stack;
    register cxint k;
    register cxint l;

    register cxdouble a;
    register cxdouble *pix_arr = NULL;

    cxint i_stack[PIX_STACK_SIZE] ;


    pix_arr = cpl_matrix_get_data(mA);
    ir = cpl_matrix_get_nrow(mA) * cpl_matrix_get_ncol(mA);

    l = 1 ;
    j_stack = 0 ;
    for (;;) {
        if (ir - l < 7) {
            for (j = l + 1 ; j <= ir ; j++) {
                a = pix_arr[j - 1];
                for (i = j - 1 ; i >= 1 ; i--) {
                    if (pix_arr[i - 1] <= a) {
                        break;
                    }
                    pix_arr[i] = pix_arr[i - 1];
                }
                pix_arr[i] = a;
            }
            if (j_stack == 0) {
                break;
            }
            ir = i_stack[j_stack-- - 1];
            l  = i_stack[j_stack-- - 1];
        }
        else {
            k = (l + ir) >> 1;
            _giraffe_swap(&pix_arr[k - 1], &pix_arr[l]);
            if (pix_arr[l] > pix_arr[ir - 1]) {
                _giraffe_swap(&pix_arr[l], &pix_arr[ir - 1]);
            }
            if (pix_arr[l - 1] > pix_arr[ir - 1]) {
                _giraffe_swap(&pix_arr[l - 1], &pix_arr[ir - 1]);
            }
            if (pix_arr[l] > pix_arr[l - 1]) {
                _giraffe_swap(&pix_arr[l], &pix_arr[l - 1]);
            }
            i = l + 1;
            j = ir;
            a = pix_arr[l - 1];
            for (;;) {
                do {
                    i++;
                } while (pix_arr[i - 1] < a);

                do {
                    j--;
                } while (pix_arr[j - 1] > a);

                if (j < i) {
                    break;
                }
                _giraffe_swap(&pix_arr[i - 1], &pix_arr[j - 1]);
            }
            pix_arr[l - 1] = pix_arr[j - 1];
            pix_arr[j - 1] = a;
            j_stack += 2;
            if (j_stack > PIX_STACK_SIZE) {
                /* stack too small in pixel_qsort: aborting */
                return -1 ;
            }
            if (ir - i + 1 >= j - l) {
                i_stack[j_stack - 1] = ir;
                i_stack[j_stack - 2] = i;
                ir = j - 1;
            }
            else {
                i_stack[j_stack - 1] = j - 1;
                i_stack[j_stack - 2] = l;
                l = i;
            }
        }
    }

    return 0;

}

#undef PIX_STACK_SIZE


/**
 * @brief
 *   Computes the solution of an equation using a pseudo-inverse.
 *
 * @param mA  left hand side [nr, nc] coefficients matrix.
 * @param mB  right hand side [nr, nc] matrix of the equation.
 *
 * @return
 *   The function returns the solution vector as a [nr, 1] column
 *   matrix.
 *
 * The function solves the linear system
 * @code
 *   mX * mA = mB
 * @endcode
 * for the unknown column matrix mX. The solution is given by the expression
 * @code
 *   mX = mB * transpose(mA) * inverse(mA * transpose(mA))
 * @endcode
 * The solution matrix is solving the equation according to a least-squares
 * criterion. To destroy this matrix the function @c cpl_matrix_delete()
 * should be used.
 *
 * @note
 *   This function works properly only in the case all the elements of
 *   the input matrix do not contain garbage (such as @c NaN or infinity).
 */

cpl_matrix *
giraffe_matrix_leastsq(const cpl_matrix* mA, const cpl_matrix* mB)
{

    cpl_matrix* m1 = NULL;
    cpl_matrix* m2 = NULL;
    cpl_matrix* m3 = NULL;
    cpl_matrix* mX = NULL;


    cx_assert(mA != NULL);
    cx_assert(mB != NULL);
    cx_assert(cpl_matrix_get_ncol(mA) == cpl_matrix_get_ncol(mB));

    m1 = cpl_matrix_transpose_create(mA);
    m2 = cpl_matrix_product_create(mA, m1);
    m3 = cpl_matrix_invert_create(m2);

    if (m3 == NULL) {
        cpl_matrix_delete(m2);
        m2 = NULL;

        cpl_matrix_delete(m1);
        m1 = NULL;

        return NULL;
    }

    cpl_matrix_delete(m2);

    m2 = cpl_matrix_product_create(mB, m1);

    cpl_matrix_delete(m1);
    m1 = NULL;

    mX = cpl_matrix_product_create(m2, m3);

    cpl_matrix_delete(m2);
    m2 = NULL;

    cpl_matrix_delete(m3);
    m3 = NULL;

    return mX;

}


/*!
 * @brief
 *   Solve a linear system using the Cholesky decomposition.
 *
 * @param A   The design matrix of the system
 * @param b   The right hand side of the system
 * @param Cb  The covariance matrix of the right hand side.
 * @param Cx  The covariance matrix of the solution vector.
 *
 * @return
 *   The function returns the solution vector of the linear system, or
 *   @c NULL in case of an error.
 *
 * The function solves the over-determined linear system Ax = b, where
 * @em A is an @c m times @c n matrix, @em b is an @c m times @c 1 matrix
 * (a column vector), and @em x is the @c n times @c 1 solution matrix.
 *
 * A covariance matrix @em Cb of the right hand side vector @em b may be given.
 * If it is not @em NULL, it is when the system is solved. It has to be a @c m
 * times @c m matrix.
 *
 * Similarly if the covariance matrix of the solution vector @em x, @em Cx,
 * is not @c NULL, the computed covariance matrix of the solution is stored
 * there. It must be a @c n times @c n matrix. Any contents of this matrix is
 * overwritten, and its contents is undetermined if the function returns with
 * an error.
 */

cpl_matrix*
giraffe_matrix_solve_cholesky(const cpl_matrix* A, const cpl_matrix* b,
                              const cpl_matrix* Cb, cpl_matrix* Cx)
{

    const char* const _id = "giraffe_matrix_solve_cholesky";

    cxint m = 0;
    cxint n = 0;

    cpl_matrix* AT   = NULL;
    cpl_matrix* ATC  = NULL;
    cpl_matrix* ATCA = NULL;
    cpl_matrix* ATCb = NULL;
    cpl_matrix* C    = NULL;
    cpl_matrix* X    = NULL;
    cpl_matrix* x    = NULL;

    cpl_error_code status = CPL_ERROR_NONE;


    if ((A == NULL) || (b == NULL)) {

        cpl_error_set(_id, CPL_ERROR_NULL_INPUT);
        return NULL;

    }

    m = cpl_matrix_get_nrow(A);
    n = cpl_matrix_get_ncol(A);

    if ((cpl_matrix_get_nrow(b) != m) || (cpl_matrix_get_ncol(b) != 1)) {

        cpl_error_set(_id, CPL_ERROR_INCOMPATIBLE_INPUT);
        return NULL;

    }

    if (Cb != NULL) {

        if ((cpl_matrix_get_nrow(Cb) != m) || (cpl_matrix_get_ncol(Cb) != m)) {
            cpl_error_set(_id, CPL_ERROR_INCOMPATIBLE_INPUT);
            return NULL;
        }

    }

    if (Cx != NULL) {

        if ((cpl_matrix_get_nrow(Cx) != n) || (cpl_matrix_get_ncol(Cx) != n)) {
            cpl_error_set(_id, CPL_ERROR_ILLEGAL_INPUT);
            return NULL;
        }

    }


    if (Cb != NULL) {

        /*
         * Speed up matrix inversion in case it is a non-singular, diagonal
         * matrix.
         */

        if (cpl_matrix_is_diagonal(Cb, CX_MINDOUBLE) == TRUE) {

            register cxint i = 0;

            C = cpl_matrix_new(m, m);

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

                register cxdouble value = cpl_matrix_get(Cb, i, i);

                if (value <= CX_MINDOUBLE) {

                    cpl_matrix_delete(C);
                    C = NULL;

                    break;
                }

                cpl_matrix_set(C, i, i, 1. / value);

            }

        }
        else {
            C = cpl_matrix_invert_create(Cb);
        }

        if (C == NULL) {
            cpl_error_set(_id, CPL_ERROR_SINGULAR_MATRIX);
            return NULL;
        }

    }
    else {

        /*
         * If no covariance matrix is given, it is assumed that the components
         * of b are statistically independent, and they all are used with
         * the same (arbitrary) weight, i.e. the covariance matrix has
         * non-zero entries in the diagonal, and these entries are all the
         * same constant.
         *
         * Using 1 as the constant value, the covariance matrix is the identity
         * matrix and its inverse is the identity matrix itself.
         */

        C = cpl_matrix_new(m, m);
        cpl_matrix_fill_diagonal(C, 1., 0);

    }


    AT  = cpl_matrix_transpose_create(A);
    ATC = cpl_matrix_product_create(AT, C);

    cpl_matrix_delete(AT);
    AT = NULL;

    cpl_matrix_delete(C);
    C = NULL;


    ATCA = cpl_matrix_product_create(ATC, A);
    ATCb = cpl_matrix_product_create(ATC, b);

    cpl_matrix_delete(ATC);
    ATC = NULL;


    /*
     * Cholesky decomposition of the matrix ATCA
     */

    status = cpl_matrix_decomp_chol(ATCA);

    if (status != CPL_ERROR_NONE) {

        cpl_matrix_delete(ATCA);
        ATCA = NULL;

        cpl_matrix_delete(ATCb);
        ATCb = NULL;

        return NULL;

    }


    /*
     * Create a temporary storage for the solution x and its covariance
     * matrix. This is done by passing the following right hand side matrix
     * to the solver. It contains the (n x n) identity matrix in the
     * columns 0 to n - 1, and the vector ATCb in its last column.
     * The solver will replace the first column with the sought solution,
     * and the identity matrix with the covariance matrix of the computed
     * solution.
     */

    X = cpl_matrix_new(n, n + 1);

    cpl_matrix_fill_diagonal(X, 1., 0);
    cpl_matrix_copy(X, ATCb, 0, n);

    cpl_matrix_delete(ATCb);
    ATCb = NULL;


    status = cpl_matrix_solve_chol(ATCA, X);

    cpl_matrix_delete(ATCA);
    ATCA = NULL;

    if (status != CPL_ERROR_NONE) {
        cpl_matrix_delete(X);
        X = NULL;
    }


    /*
     * Decompose the result of the solver into the solution and its
     * covariance matrix (if requested).
     */

    x = cpl_matrix_extract_column(X, n);

    if (Cx != NULL) {
        cpl_matrix_copy(Cx, X, 0, 0);
    }

    cpl_matrix_delete(X);
    X = NULL;

    return x;

}





/**
 * @brief
 *   Set all elements of a matrix to zero.
 *
 * @param matrix  The matrix to update.
 *
 * @return
 *   The function returns 0 on success, or a non-zero value otherwise.
 *
 * The function replaces the value of each matrix element of @em matrix
 * with @c 0.
 */

cxint
giraffe_matrix_clear(cpl_matrix *matrix)
{
    cxint nr_matrix;
    cxint nc_matrix;

    cxdouble *pd_matrix = NULL;

    cx_assert(matrix != NULL);

    pd_matrix = cpl_matrix_get_data(matrix);
    nc_matrix = cpl_matrix_get_ncol(matrix);
    nr_matrix = cpl_matrix_get_nrow(matrix);

    memset(pd_matrix, 0, nr_matrix * nc_matrix * sizeof(cxdouble));

    return 0;

}


/**
 * @brief
 *   Output a maximum number of rows of the input matrix
 *
 * @param matrix    Matrix to print.
 * @param max_rows  Maximum number of rows to print.
 *
 * @return
 *   Nothing.
 *
 * Function prints part of a matrix. Printed are all columns,
 * but only up to @em max_rows, number of rows.
 * Each column has the column number printed as a header.
 *
 * @note
 *   This function should be moved to CPL proper...
 *
 */

void
giraffe_matrix_dump(const cpl_matrix *matrix, cxint max_rows)
{

    cxint i;
    cxint j;
    cxint k;
    cxint nc;
    cxint nr;
    /*cxint ncw;*/

    const cxdouble *pd_m = NULL;

    cx_string *buffer = NULL;
    cx_string *tmp = NULL;

    if (matrix == NULL) {
        return;
    }

    pd_m = cpl_matrix_get_data_const(matrix);

    nr = cpl_matrix_get_nrow(matrix);
    nc = cpl_matrix_get_ncol(matrix);

    if (nr > max_rows) {
        nr = max_rows;
    }

    buffer = cx_string_new();
    tmp = cx_string_new();

    /* print header */
    for (i = 0; i < nc; i++) {
        /*ncw =*/ cx_string_sprintf(tmp, "      %d", i);
        cx_string_append(buffer, cx_string_get(tmp));
    }

    cpl_msg_debug("", "%s", cx_string_get(buffer));

    /* print values */
    for (k = 0, i = 0; i < nr; i++) {
        /*ncw =*/ cx_string_sprintf(buffer,"  %d", i);
        for (j = 0; j < nc; j++, k++) {
            /*ncw =*/ cx_string_sprintf(tmp, " %+18.12f", pd_m[k]);
            cx_string_append(buffer, cx_string_get(tmp));
        }

        cpl_msg_debug("", "%s", cx_string_get(buffer));
    }

    cx_string_delete(tmp);
    cx_string_delete(buffer);

    return;

}
/**@}*/
