/*===========================================================================
  Copyright (C) 2001 European Southern Observatory (ESO)
 
  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., 675 Massachusetss Ave, Cambridge, 
  MA 02139, USA.
 
  Corresponding concerning ESO-MIDAS should be addressed as follows:
    Internet e-mail: midas@eso.org
    Postal address: European Southern Observatory
            Data Management Division 
            Karl-Schwarzschild-Strasse 2
            D 85748 Garching bei Muenchen 
            GERMANY
===========================================================================*/
/* Program  : scatter.c                                                    */
/* Author   : G. Mulas  -  ITAL_FLAMES Consortium                          */
/* Date     :                                                              */
/*                                                                         */
/* Purpose  : Missing                                                      */
/*                                                                         */
/*                                                                         */
/* Input:  see interface                                                   */ 
/*                                                                      */
/* Output:                                                              */
/*                                                                         */
/* DRS Functions called:                                                   */
/* none                                                                    */ 
/*                                                                         */ 
/* Pseudocode:                                                             */
/* Missing                                                                 */ 
/*                                                                         */ 
/* Version  :                                                              */
/* Last modification date: 2002/08/05                                      */
/* Who     When        Why                Where                            */
/* AMo     02-08-05   Add header         header                            */
/*-------------------------------------------------------------------------*/

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


#include <uves_msg.h>
#include <string.h>             //memset
#include <flames_gauss_jordan.h> //DRS_VERBOSITY
#include <flames_newmatrix.h> //DRS_VERBOSITY
#include <flames_def_drs_par.h> //DRS_VERBOSITY
static int  nold = -1;
static int  *indxc, *indxr, *ipiv;
static double
d_abs_f(double x)
{
  if (x<0) return(-x);
  return(x);
}
#define SWAP(a,b) {float temp=(a);(a)=(b);(b)=(temp);}



/* Wikipedia pseudocode
 *
 * Gaussian elimination writes a given m × n matrix A uniquely as a product of
 * an invertible m × m matrix S and a row-echelon matrix T. Here, S is the
 * product of the matrices corresponding to the row operations performed.
 * The formal algorithm to compute T from A follows. We write A[i,j] for the
 * entry in row i, column j in matrix A with 1 being the first index. The
 * transformation is performed in place, meaning that the original matrix A is
 * lost and successively replaced by T.
 *
 *   for k = 1 ... min(m,n):
 *      Find the k-th pivot:
 *      i_max  := argmax (i = k ... m, abs(A[i, k]))
 *      if A[i_max, k] = 0
 *        error "Matrix is singular!"
 *      swap rows(k, i_max)
 *      Do for all rows below pivot:
 *      for i = k + 1 ... m:
 *        Do for all remaining elements in current row:
 *        for j = k + 1 ... n:
 *           A[i, j]  := A[i, j] - A[k, j] * (A[i, k] / A[k, k])
 *        Fill lower triangular matrix with zeros:
 *        A[i, k]  := 0
 */
static int  *indxc, *indxr, *ipiv;
int
flames_gauss_jordan(double **mA, int n, double **mB, int m)
{

    int min_m_n= ( m < n ) ? m: n;
    int k=0;

    register int i=0;
    register int j=0;
    register int pivot;


    double eps=1.e-30, epsn=-1.e-30;

    int nr_mA=n;
    //int nc_mA=n;
    //int nr_mB=n;
    //int nc_mB=n;


    int ll=0;
    int irow=0;
    int icol=0;
    int l=0;
    double abs_ajk=0;


    /* initialisation part */

    /* we use an integer vector to know if pivot (divisor) was already found or
     * not we use another integer vector to store the location of found rows or
     * columns of the found pivots (divisors)
     */
    if (n > nold)
    {
        /* initialise arrays */
        if (nold > -1)        /* not the very first time */
        {

            free_ivector(ipiv,1,nold);
            free_ivector(indxr,1,nold);
            free_ivector(indxc,1,nold);
        }
        indxc=ivector(1,n);
        indxr=ivector(1,n);
        ipiv=ivector(1,n);
        nold = n;
    }

    for (j=1;j<=n;j++) ipiv[j]=0;


    /* solving linear equation system Ax=B */
    /* Note that our indexes all start from 1 and ends to the last element */
    for( i = 1; i <= n; i++ ){

        /* main loop over the columns to be reduced:
         *
         * we first need to find the so called input matrix pivot, which is the
         * input matrix element used as reference to start to normalise to all
         * the other elements. As wikipedia indicates the best choice is to get
         * as pivot the coefficient with maximum absolute value. Thus the
         * algorithm starts with a maximum search.
         * See also implementation by Stoer, Numerische Mathematik, 1. Teil.
         */
    	register double max=0.0;
        for ( j = 1; j <= n; j++ ) {
            /* outer loop of the search of the j-th pivot (divisor) element: */
            if ( ipiv[j] != 1 ) {
                /* search for maximum of matrix Ajk coefficients */
                for (k=1;k<=n;k++) {
                    pivot = ipiv[k];
                    if ( pivot == 0 ) {
                        //int jj = j * nr_mA;
                        abs_ajk = d_abs_f( mA[j][k] );
                        if ( abs_ajk >= max ) {
                            /* found a maximum candidate
                             * update its value and position in the matrix
                             */
                            max = abs_ajk;
                            irow=j;
                            icol=k;

                        }

                    } else if ( pivot > 1 ) {

                        uves_msg_error("GAUSSJ: Singular Matrix-1");
                        free_ivector(ipiv,1,nold);
                        free_ivector(indxr,1,nold);
                        free_ivector(indxc,1,nold);
                        nold = -1;
                        return(-1);

                    }

                }

            }

        }
        /* We now have the pivot, so we swap rows, for all rows below pivot,
         * if needed, to put the pivot element on the diagonal. The columns are
         * not physically interchanged, they are only re-labeled
         */

        ++(ipiv[icol]);

        if (irow != icol) {
            for (l = 1; l <= n; l++) {
                SWAP(mA[irow][l], mA[icol][l])
            }
            for (l = 1; l <= m; l++) {
                SWAP(mB[irow][l], mB[icol][l])
            }
        }

        indxr[i] = irow;
        indxc[i] = icol;
        register double factor = mA[icol][icol];
        /* As on real data factor may be small but not exactly zero we replace
         * the check versus 0.0 with a check versus a small number eps=1.e-30
         */
        if ( ( factor < eps ) && ( factor > epsn ) )  {
            //if ( mA[icol][icol] == 0.0 ) {
            /* We cannot divide by 0: matrix singular 2 */
            uves_msg_error("GAUSSJ: Singular Matrix-2");
            free_ivector(ipiv,1,nold);
            free_ivector(indxr,1,nold);
            free_ivector(indxc,1,nold);
            nold = -1;
            return(-2);

        }

        /* Now we can divide the pivot row by the pivot element */
        register double inv_pivot = 1.0 / factor;

        /* the diagonal element should be by definition 1: to prevent numerical
         * errors we set it as such
         */
        mA[icol][icol] = 1.0;
        for (l = 1; l <= n; l++) {
            mA[icol][l] *= inv_pivot;
        }

        for (l = 1; l <= m; l++) {
            mB[icol][l] *= inv_pivot;
        }

        for (ll = 1; ll <= n; ll++) {
            /* reduce the rows... except for the pivot one */
            if (ll != icol) {

                factor = mA[ll][icol];

                /* the reference column has to be 0.0 by definition */
                mA[ll][icol] = 0.0;

                for (l = 1; l <= n; l++) {
                    mA[ll][l] -= mA[icol][l] * factor;
                }

                for (l = 1; l <= m; l++) {
                    mB[ll][l] -= mB[icol][l] * factor;
                }
            }
        }
    }
    //free_ivector(ipiv,1,nold);

    /* re-permute columns */
    for (l = n; l >= 1; l--) {
        if (indxr[l] != indxc[l]) {
            for (k = 1; k <= n; k++) {
                SWAP( mA[k][indxr[l]], mA[k][indxc[l]]);
            }
        }
    }
    //free_ivector(indxr,1,nold);
    //free_ivector(indxc,1,nold);

    return 0;
}

#undef SWAP
