/*
 * This file is part of the ESO SINFONI Pipeline
 * Copyright (C) 2004,2005 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, 51 Franklin St, Fifth Floor, Boston, MA  02111-1307  USA
 */
/*----------------------------------------------------------------------------
   
   File name     :    sinfo_matrix.c
   Author         :    Nicolas Devillard
   Created on    :    1994
   Description    :    basic 2d sinfo_eclipse_matrix handling routines

 ---------------------------------------------------------------------------*/
/*
 $Id: sinfo_matrix.c,v 1.4 2012/03/02 08:42:20 amodigli Exp $
 $Author: amodigli $
 $Date: 2012/03/02 08:42:20 $
 $Revision: 1.4 $
 */
#ifdef HAVE_CONFIG_H
#  include <config.h>
#endif
/*----------------------------------------------------------------------------
                                   Includes
 ---------------------------------------------------------------------------*/
#include "sinfo_matrix.h"
/**@{*/
/**
 * @addtogroup sinfo_utilities matrix functions
 *
 * TBD
 */


/*----------------------------------------------------------------------------
                                   Macros
 ---------------------------------------------------------------------------*/
#define dtiny(a) ((a)<0?(a)> -1.e-30:(a)<1.e-30)
/*----------------------------------------------------------------------------
                        Private function prototypes
 ---------------------------------------------------------------------------*/
static int gauss_pivot(double *ptra, double *ptrc, int n);
/*----------------------------------------------------------------------------
                              Function codes
 ---------------------------------------------------------------------------*/

/**
  @name     sinfo_create_mx
  @memo     Allocates a new sinfo_eclipse_matrix.
  @param    nr  Number of rows.
  @param    nc  Number of columns.
  @return   Pointer to newly allocated sinfo_eclipse_matrix.
  @doc

  Allocates a new sinfo_eclipse_matrix.
 */

Matrix
sinfo_create_mx(int nr, int nc)
{
    Matrix b;
    b = (Matrix)cpl_calloc(1,sizeof(sinfo_eclipse_matrix));
    b->m = (double*)cpl_calloc(nr*nc,sizeof(double));
    b->nr= nr;
    b->nc= nc;
    return b;
}

/**
  @name     sinfo_copy_mx
  @memo     Copy a sinfo_eclipse_matrix.
  @param    a   Matrix to copy.
  @return   Pointer to newly allocated sinfo_eclipse_matrix.
  @doc

  Copy a sinfo_eclipse_matrix.
 */
Matrix
sinfo_copy_mx(Matrix a)
{
    Matrix b = sinfo_create_mx(a->nr,a->nc);
    if (b!=NULL) {
        register int s = a->nr*a->nc;
        register double *mm = b->m+s;
        register double *am = a->m+s;
        while (s--) *--mm = *--am;
    }
    return b;
}

/**
  @name     sinfo_close_mx
  @memo     Frees memory associated to a sinfo_eclipse_matrix.
  @param    a   Matrix to free.
  @return   void
  @doc

  Free a sinfo_eclipse_matrix.
 */
void
sinfo_close_mx(Matrix a)
{
    if (a==NULL) return ;
    if (a->m != NULL)
        cpl_free(a->m);
    cpl_free(a);
    return;
}

/**
  @name     sinfo_mul_mx
  @memo     Multiplies 2 matrices.
  @param    a   Matrix on the left side of the multiplication.
  @param    b   Matrix on the right side of the multiplication.
  @return   Matrix a*b.
  @doc

  Multiply matrices.
 */
Matrix
sinfo_mul_mx(Matrix a, Matrix b)
{
    Matrix c, d;
    int n1=a->nr, n2=a->nc, n3=b->nc;
    register double *a0;
    register double *c0;
    register double *d0;
    register int i,j,k;

    if(n2!=b->nr) return NULL;
    c = sinfo_create_mx(n1,n3);
    d = sinfo_transp_mx(b);

    for (i=0,c0=c->m;i<n1;i++)
        for (j=0,d0=d->m;j<n3;j++,c0++)
            for (k=0,*c0=0,a0=a->m+i*n2;k<n2;k++)
                *c0 += *a0++ * *d0++;
    sinfo_close_mx(d);
    return c;
}

/**
  @name     sinfo_invert_mx
  @memo     Inverts a sinfo_eclipse_matrix.
  @param    aa  (Square) sinfo_eclipse_matrix to sinfo_invert
  @return   Newly allocated sinfo_eclipse_matrix.
  @doc
 
  The sinfo_eclipse_matrix inversion procedure is hardcoded for optimized speed
  in the case of 1x1, 2x2 and 3x3 matrices. This function is not suitable
  for large matrices.
 */
Matrix
sinfo_invert_mx(Matrix aa)
{
    Matrix bb;
    int test=1;

    if(aa->nr!=aa->nc) return NULL;
    bb = sinfo_create_mx(aa->nr,aa->nc);

    if(aa->nr==1)
    {
        double det;
        register double ted;
        det= *(aa->m);
        if(dtiny(det)) test=0;
        ted=1./det;
        *(bb->m)=ted;
    }
    else if(aa->nr==2)
    {
        double det;
        register double ted;
        register double *mm=aa->m;
        double a= *(mm++),b= *(mm++);
        double c= *(mm++),d= *(mm);
        det=a*d-b*c;
        if(dtiny(det)) test=0;
        ted=1./det;
        mm=bb->m;
        *(mm++)= d*ted,*(mm++)= -b*ted;
        *(mm++)= -c*ted,*(mm)= a*ted;
    }
    else if(aa->nr==3)
    {
        double det;
        register double ted;
        register double *mm=aa->m;
        double a= *(mm++),b= *(mm++),c= *(mm++);
        double d= *(mm++),e= *(mm++),f= *(mm++);
        double g= *(mm++),h= *(mm++),i= *(mm);
        det=a*e*i-a*h*f-b*d*i+b*g*f+c*d*h-c*g*e;
        if(dtiny(det)) test=0;
        ted=1./det;
        mm=bb->m;
        *(mm++)=(e*i-f*h)*ted,
                *(mm++)=(c*h-b*i)*ted,
                *(mm++)=(b*f-e*c)*ted;

        *(mm++)=(f*g-d*i)*ted,
                *(mm++)=(a*i-g*c)*ted,
                *(mm++)=(d*c-a*f)*ted;

        *(mm++)=(d*h-g*e)*ted,
                *(mm++)=(g*b-a*h)*ted,
                *(mm)=(a*e-d*b)*ted;
    }
    else
    {
        Matrix temp=sinfo_copy_mx(aa);
        if(gauss_pivot(temp->m,bb->m,aa->nr)==0) test=0;
        sinfo_close_mx(temp);
    }
    if(test==0)
    {
        sinfo_msg_error("not invertible, aborting inversion");
        return NULL;
    }
    return bb;
}

/**
  @name     sinfo_transp_mx
  @memo     Transposes a sinfo_eclipse_matrix.
  @param    a   Matrix to transpose.
  @return   Newly allocated sinfo_eclipse_matrix.
  @doc

  Transpose a sinfo_eclipse_matrix.
 */
Matrix
sinfo_transp_mx(Matrix a)
{
    register int nc=a->nc, nr=a->nr;
    register double *a0;
    register double *b0;
    register int i,j;
    Matrix b = sinfo_create_mx(nc,nr);

    if (b == (Matrix)NULL) return b ;
    for (i=0,b0=b->m;i<nc;i++)
        for (j=0,a0=a->m+i;j<nr;j++,a0+=nc,b0++)
            *b0 = *a0;
    return b;
}

/**
  @name     gauss_pivot
  @memo     Line simplification with Gauss method.
  @param    ptra    A sinfo_eclipse_matrix line.
  @param    ptrc    A sinfo_eclipse_matrix line.
  @param    n       Number of rows in each line.
  @return   int 1 if Ok, 0 else.
  @doc
 
  This function is used only for the general case in sinfo_eclipse_matrix 
  inversion.
 */
static int
gauss_pivot(double *ptra, double *ptrc, int n)
/* c(n,n) = a(n,n)^-1 */
{
#define SINFO_ABS(a) (((a) > 0) ? (a) : -(a))

    register int i,j,k,l;
    int maj;
    double max,r,t;
    double *ptrb;

    ptrb=(double *)cpl_calloc(n*n,sizeof(double));
    for(i=0;i<n;i++)
        ptrb[i*n+i]= 1.0;

    for (i=1;i <= n;i++)
    {
        /* Search max in current column  */
        max = SINFO_ABS(*(ptra + n*i-n));
        maj = i;
        for (j = i;j <= n;j++)
            if (SINFO_ABS(*(ptra+n*j+i-n-1)) > max)
            {
                maj = j;
                max = SINFO_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(dtiny(t)) 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(dtiny(t)) 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));
        }
    cpl_free(ptrb);
    return(1);
}

/**
  @name     sinfo_least_sq_mx
  @memo     Compute the solution of an equation using a pseudo-inverse.
  @param    A   Matrix.
  @param    B   Matrix.
  @return   Pointer to newly allocated sinfo_eclipse_matrix.
  @doc
 
  The equation is XA=B.
 
  The pseudo-inverse solution to this equation is defined as:
  \begin{verbatim}
  P = B.tA.inv(A.tA)
  \end{verbatim}
 
  P is solving the equation using a least-squares criterion.
  Demonstration left to the reader.
 */

Matrix 
sinfo_least_sq_mx(
    Matrix    A,
    Matrix    B
)
{
    Matrix    m1,
            m2,
            m3,
            m4,
            m5 ;



    m1 = sinfo_transp_mx(A) ;
    m2 = sinfo_mul_mx(A, m1) ;
    m3 = sinfo_invert_mx(m2) ;
    m4 = sinfo_mul_mx(B, m1) ;
    m5 = sinfo_mul_mx(m4, m3) ;

    sinfo_close_mx(m1) ;
    sinfo_close_mx(m2) ;
    sinfo_close_mx(m3) ;
    sinfo_close_mx(m4) ;

    return m5 ;
}

/**
  @name     sinfo_print_mx
  @memo     Prints out a sinfo_eclipse_matrix on stdout.
  @param    M       Matrix to print out
  @param    name    Name of the sinfo_eclipse_matrix to print out.
  @return   void
  @doc
 
  The sinfo_eclipse_matrix name is printed out, then all values row by row.
  Used for debugging purposes mostly.
 */

void 
sinfo_print_mx(
    Matrix    M,
    const char *    name
)
{
    int    i, j ;

    fprintf(stdout, "# sinfo_eclipse_matrix %s is [%d x %d]\n", 
        name, M->nr, M->nc) ;
    for (j=0 ; j<M->nr ; j++) {
        for (i=0 ; i<M->nc ; i++) {
            fprintf(stdout, "%g\t", M->m[i+j*M->nc]) ;
        }
        fprintf(stdout, "\n") ;
    }
    fprintf(stdout, "\n") ;
}

/**@}*/
