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

    double 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  */
        double max = SINFO_ABS(*(ptra + n*i-n));
        int 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;
}

/**@}*/
