/*
    Theseus - maximum likelihood superpositioning of macromolecular structures

    Copyright (C) 2004-2015 Douglas L. Theobald

    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.,
    59 Temple Place, Suite 330,
    Boston, MA  02111-1307  USA

    -/_|:|_|_\-
*/

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>
#include "ludcmp.h"
#define TINY 1e-15
#define INV_PRECISION 1e-8


void
matinv(double **a, double **outmat, int N, int *indx)
{
    double          d;
    int             i, j, k, l;
    int             tmpval;
    double         *col = malloc(N * sizeof(double));

    for (k = 0; k < N; k++)
    {
        for (l = 0; l < N; l++)
        {
            tmpval = (int) (a[k][l] / INV_PRECISION);
            a[k][l] = (double) tmpval * INV_PRECISION;
        }
    }

    ludcmp(a, N, indx, &d);        /* Decompose the matrix just once */

    for (j = 0; j < N; j++)        /* Find inverse by columns */
    {
        for (i = 0; i < N; i++)
            col[i] = 0.0;

        col[j] = 1.0;
        lubksb(a, N, indx, col);

        for (i = 0; i < N; i++)
            outmat[i][j] = col[i];
    }

    free(col);
}


void 
ludcmp(double **a, int n, int *indx, double *d)
{
    int             i, imax = 0, j, k;
    double          big, dum, sum, temp;
    double         *vv;

    vv = malloc(n * sizeof(double));

    if (!vv)
    {
        fprintf(stderr, "Error Allocating Vector Memory\n");
        exit(EXIT_FAILURE);
    }

    *d = 1.0;

    for (i = 0; i < n; i++)
    {
        big = 0.0;

        for (j = 0; j < n; j++)
        {
            if ((temp = fabs(a[i][j])) > big)
                big = temp;
        }

        if (big == 0.0)
        {
            fprintf(stderr, "\nERROR: Singular Matrix in Routine ludcmp()\n");

            for (j = 0; j < n; j++)
                printf(" %f ", a[i][j]);
            printf("/n");
            /* exit(EXIT_FAILURE); */
        }

        vv[i] = 1.0 / big;
    }

    for (j = 0; j < n; j++)
    {
        for (i = 0; i < j; i++)
        {
            sum = a[i][j];

            for (k = 0; k < i; k++)
                sum -= a[i][k] * a[k][j];

            a[i][j] = sum;
        }

        big = 0.0;

        for (i = j; i < n; i++)
        {
            sum = a[i][j];

            for (k = 0; k < j; k++)
                sum -= a[i][k] * a[k][j];

            a[i][j] = sum;

            if ((dum = vv[i] * fabs(sum)) >= big)
            {
                big = dum;
                imax = i;
            }
        }

        if (j != imax)
        {
            for (k = 0; k < n; k++)
            {
                dum = a[imax][k];
                a[imax][k] = a[j][k];
                a[j][k] = dum;
            }

            *d = -(*d);
            vv[imax] = vv[j];
        }

        indx[j] = imax;

        if (a[j][j] == 0.0)
            a[j][j] = TINY;

        if (j != n - 1)
        {
            dum = 1.0 / a[j][j];
            for (i = j + 1; i < n; i++)
                a[i][j] *= dum;
        }
    }

    free(vv);
}


void 
lubksb(double **a, int n, int *indx, double *b)
{
    int             i, ip, j, ii = -1;
    double          sum;

    for (i = 0; i < n; i++)
    {
        ip = indx[i];
        sum = b[ip];
        b[ip] = b[i];
 
        if (ii >= 0)
            for (j = ii; j < i; j++)
                sum -= a[i][j] * b[j];
        else if (sum)
            ii = i;

        b[i] = sum;
    }

    for (i = n - 1; i >= 0; i--)
    {
        sum = b[i];

        for (j = i + 1; j < n; j++)
            sum -= a[i][j] * b[j];

        b[i] = sum / a[i][i];
    }
}


/* Gaussian Elimination with Partial Pivoting (GEPP) */
#define N   5

#define SWAP(type,x,y)  {type temp; temp=x; x=y; y=temp;}

typedef double  real;


/* Gaussian Elimination with Partial Pivoting (GEPP):
[ 1] For k in 1,...,N-1, do [2] to [10]:
[ 2]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[ 3]   If A(m,k)==0, then print "A is singular" and stop.
[ 4]   If m==k, then go to [7]; else, do [5] and [6]:
[ 5]     For j in k,...,N, interchange A(m,j) and A(k,j)
[ 6]     Interchange b(m) and b(k)
[ 7]   For i in k+1,...,N, do [8] to [10]:
[ 8]     Replace A(i,k) with A(i,k)/A(k,k)
[ 9]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
[10]     Replace  b(i)  with b(i) - A(i,k)*b(k)
[11] For k in N,...,1, do [12] and [13]:
[12]   For j in k+1,...,N, replace b(k) with b(k)-A(k,j)*b(j)
[13]   Replace b(k) with b(k)/A(k,k)
 */
void
gepp(real A[N][N], real b[N])
{
    int             i, j, k, m, mmax;
    real            amax;

    /* FORWARD ELIMINATION */
    for (k = 0; k < N - 1; k++)
    {
        /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
        amax = fabs(A[k][k]);
        mmax = k;
        for (m = k + 1; m < N; m++)
            if (amax < fabs(A[m][k]))
            {
                amax = fabs(A[m][k]);
                mmax = m;
            }
        if (amax == 0)
        {
            fprintf(stderr, "Matrix A is singular (at k=%d).\n", k);
            exit(EXIT_FAILURE);
        }
        /* Pivot, or interchange, if necessary */
        if (mmax != k)
        {
            for (j = k; j < N; j++)
                SWAP(real, A[mmax][j], A[k][j]);
            SWAP(real, b[mmax], b[k]);
        }
        for (i = k + 1; i < N; i++)
        {
            A[i][k] /= A[k][k];
            for (j = k + 1; j < N; j++)
                A[i][j] -= A[i][k] * A[k][j];
            b[i] -= A[i][k] * b[k];
        }
    }
    /* BACK SUBSTITUTION */
    for (k = N - 1; k >= 0; k--)
    {
        for (j = k + 1; j < N; j++)
            b[k] -= A[k][j] * b[j];
        b[k] /= A[k][k];
    }
    return;
}


/* Matrix Inversion with Partial Pivoting (MIPP):
[ 0'] Initialize B(i,j)=0 except that B(i,i)=1 for i in 1,...,N
[ 1 ] For k in 1,...,N-1, do [2] to [10']:
[ 2 ]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[ 3 ]   If A(m,k)==0, then print "A is singular" and stop.
[ 4 ]   If m==k, then go to [7]; else, do [5] and [6]:
[ 5 ]     For j in k,...,N, interchange A(m,j) and A(k,j)
[ 6']     For j in k,...,N, interchange B(m,j) and B(k,j)
[ 7 ]   For i in k+1,...,N, do [8] to [10']:
[ 8 ]     Replace A(i,k) with A(i,k)/A(k,k)
[ 9 ]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
[10']     For j in 1,...,N, replace B(i,j) with B(i,j)-A(i,k)*B(k,j)
[11 ] For k in N,...,1, do [12'] to [14']:
[12']   For n in 1,...,N, do [13'] and [14']
[13']     For j in k+1,...,N, replace B(k,n) with B(k,n)-A(k,j)*B(j,n)
[14']     Replace B(k,n) with B(k,n)/A(k,k)
 */
void
mipp(real A[N][N], real B[N][N])
{
    int             i, j, k, m, n, mmax;
    real            amax;

    /* INITIALIZE B */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
            B[i][j] = (i == j ? 1 : 0);

    /* FORWARD ELIMINATION */
    for (k = 0; k < N - 1; k++)
    {
        /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
        amax = fabs(A[k][k]);
        mmax = k;
        for (m = k + 1; m < N; m++)
            if (amax < fabs(A[m][k]))
            {
                amax = fabs(A[m][k]);
                mmax = m;
            }
        if (amax == 0)
        {
            fprintf(stderr, "Matrix A is singular (at k=%d).\n", k);
            exit(EXIT_FAILURE);
        }
        /* Pivot, or interchange, if necessary */
        if (mmax != k)
        {
            for (j = k; j < N; j++)
                SWAP(real, A[mmax][j], A[k][j]);
            for (j = 0; j < N; j++)
                SWAP(real, B[mmax][j], B[k][j]);
        }
        for (i = k + 1; i < N; i++)
        {
            A[i][k] /= A[k][k];
            for (j = k + 1; j < N; j++)
                A[i][j] -= A[i][k] * A[k][j];
            for (j = 0; j < N; j++)
                B[i][j] -= A[i][k] * B[k][j];
        }
    }
    /* BACK SUBSTITUTION */
    for (k = N - 1; k >= 0; k--)
        for (n = 0; n < N; n++)
        {
            for (j = k + 1; j < N; j++)
                B[k][n] -= A[k][j] * B[j][n];
            B[k][n] /= A[k][k];
        }
    return;
}

/* LU Decomposition with Partial Pivoting (LUPP)
[0"] For k in 1,...,N, let pi(k)=k
[1 ] For k in 1,...,N-1, do [2] to [9]:
[2 ]   Find the largest value |A(m,k)| in column k, for m in k,...,N
[3 ]   If A(m,k)==0, then print "A is singular" and stop.
[4 ]   If m==k, then go to [7]; else, do [5"] and [6"]:
[5"]     For j in 1,...,N, interchange A(m,j) and A(k,j)
[6"]     Interchange pi(m) and pi(k)
[7 ]   For i in k+1,...,N, do [8] and [9]:
[8 ]     Replace A(i,k) with A(i,k)/A(k,k)
[9 ]     For j in k+1,...,N, replace A(i,j) with A(i,j)-A(i,k)*A(k,j)
*/
void
lupp(real A[N][N], int pi[N])
{
    int             i, j, k, m, mmax;
    real            amax;

    /* PERMUTATION INITIALIZATION: */
    for (k = 0; k < N; k++)
        pi[k] = k;

    /* FORWARD ELIMINATION */
    for (k = 0; k < N - 1; k++)
    {
        /* Find the largest value |A(m,k)| in column k, for m in k,...,N */
        amax = fabs(A[k][k]);
        mmax = k;
        for (m = k + 1; m < N; m++)
            if (amax < fabs(A[m][k]))
            {
                amax = fabs(A[m][k]);
                mmax = m;
            }
        if (amax == 0)
        {
            fprintf(stderr, "Matrix A is singular (at k=%d).\n", k);
            exit(EXIT_FAILURE);
        }
        /* Pivot, or interchange, if necessary */
        if (mmax != k)
        {
            for (j = 0; j < N; j++)
                SWAP(real, A[mmax][j], A[k][j]);
            SWAP(int, pi[mmax], pi[k]);
        }
        for (i = k + 1; i < N; i++)
        {
            A[i][k] /= A[k][k];
            for (j = k + 1; j < N; j++)
                A[i][j] -= A[i][k] * A[k][j];
        }
    }
    return;
}

/* Linear System Solution from LU and Pivoting Data (SSLU)
[10] Permute b(1),...,b(N) to the order b(pi(1)),...,b(pi(N))
[11] For k in 1,...,N-1, do [12]:
[12]   For i in k+1,...,N, replace b(i) with b(i)-A(i,k)*b(k)
[13] For k in N,...,1, do [14] and [15]:
[14]   For j in k+1,...,N, replace b(k) with b(k)-A(k,j)*b(j)
[15]   Replace b(k) with b(k)/A(k,k)
*/
void
sslu(const real A[N][N], const int pi[N], real b[N])
{
    int             i, j, k;
    real            bp[N];

    /* PRE-PIVOTING */
    for (k = 0; k < N; k++)
        bp[k] = b[pi[k]];
    for (k = 0; k < N; k++)
        b[k] = bp[k];

    /* FORWARD REDUCTION */
    for (k = 0; k < N - 1; k++)
        for (i = k + 1; i < N; i++)
            b[i] -= A[i][k] * b[k];

    /* BACK SUBSTITUTION */
    for (k = N - 1; k >= 0; k--)
    {
        for (j = k + 1; j < N; j++)
            b[k] -= A[k][j] * b[j];
        b[k] /= A[k][k];
    }
    return;
}
