/*
    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 <stdio.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "eigen_gsl.h"
#include "MatUtils.h"


void
MatPrint(double **matrix, const int size)
{
    int             i, j;

    printf("\n");
    for (i = 0; i < size; ++i)
    {
        printf("[");
        for (j = 0; j < size; ++j)
            printf(" % 8.2e", matrix[i][j]);
        printf(" ]\n");
    }
    printf("\n");

    fflush(NULL);
}


void
MatPrintRec(double **matrix, const int n, const int m)
{
    int             i, j;

    printf("\n");
    for (i = 0; i < n; ++i)
    {
        printf(" [");
        for (j = 0; j < m; ++j)
            printf(" % 14.8f", matrix[i][j]);
        printf(" ]\n");
    }
    printf("\n");

    fflush(NULL);
}


void
MatDestroy(double ***matrix_ptr)
{
    if (matrix_ptr)
    {
        double **matrix = *matrix_ptr;

        if (matrix)
        {
            if (matrix[0])
            {
                free(matrix[0]);
                matrix[0] = NULL;
            }

            free(matrix);
            matrix = NULL;
        }
    }
}


void
MatCharDestroy(char ***matrix_ptr)
{
    if (matrix_ptr)
    {
        char **matrix = *matrix_ptr;

        if (matrix)
        {
            if (matrix[0])
            {
                free(matrix[0]);
                matrix[0] = NULL;
            }

            free(matrix);
            matrix = NULL;
        }
    }
}


double
**MatAlloc(const int rows, const int cols)
{
    int             i;
    double        **matrix = NULL;
    double         *matspace = NULL;

    matspace = (double *) calloc((rows * cols), sizeof(double));
    if (matspace == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate matrix in MatAlloc(): (%d x %d)\n", rows, cols);
        exit(EXIT_FAILURE);
    }

    /* allocate room for the pointers to the rows */
    matrix = (double **) malloc(rows * sizeof(double *));
    if (matrix == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate row pointers in MatAlloc(): (%d)\n", rows);
        exit(EXIT_FAILURE);
    }

    /*  now 'point' the pointers */
    for (i = 0; i < rows; i++)
        matrix[i] = matspace + (i * cols);

    return(matrix);
}


char
**MatCharAlloc(const int rows, const int cols)
{
    int             i;
    char          **matrix = NULL;
    char           *matspace = NULL;

    matspace = (char *) calloc((rows * cols), sizeof(char));
    if (matspace == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate matrix in MatCharAlloc(): (%d x %d)\n", rows, cols);
        exit(EXIT_FAILURE);
    }

    /* allocate room for the pointers to the rows */
    matrix = (char **) malloc(rows * sizeof(char *));
    if (matrix == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate row pointers in MatCharAlloc(): (%d)\n", rows);
        exit(EXIT_FAILURE);
    }

    /*  now 'point' the pointers */
    for (i = 0; i < rows; i++)
        matrix[i] = matspace + (i * cols);

    return(matrix);
}


void
MatIntDestroy(int ***matrix_ptr)
{
    if (matrix_ptr)
    {
        int **matrix = *matrix_ptr;

        if (matrix)
        {
            if (matrix[0])
            {
                free(matrix[0]);
                matrix[0] = NULL;
            }

            free(matrix);
            matrix = NULL;
        }
    }
}


int
**MatIntInit(const int rows, const int cols)
{
    int             i;
    int           **matrix = NULL;
    int            *matspace = NULL;

    matspace = (int *) calloc((rows * cols), sizeof(int));
    if (matspace == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate matrix (%d x %d)\n", rows, cols);
        exit(EXIT_FAILURE);
    }

    /* allocate room for the pointers to the rows */
    matrix = (int **) malloc(rows * sizeof(int *));
    if (matrix == NULL)
    {
        perror("\n ERROR");
        printf("\n ERROR: Failure to allocate pointers (%d)\n", rows);
        exit(EXIT_FAILURE);
    }

    /*  now 'point' the pointers */
    for (i = 0; i < rows; i++)
        matrix[i] = matspace + (i * cols);

    return(matrix);
}


Matrix3D
*Mat3DInit(const int rows, const int cols, const int depth)
{
    Matrix3D       *matrix3d = NULL;
    int             i;

    matrix3d = (Matrix3D *) malloc(sizeof(Matrix3D));

    matrix3d->rows = rows;
    matrix3d->cols = cols;
    matrix3d->depth = depth;

    /* allocate room for the pointers to the rows */
    matrix3d->matrix  = (double ***) malloc(rows * sizeof(double **));
    matrix3d->matrixc = (double **)  malloc(rows * cols * sizeof(double *));
    matrix3d->matrixd = (double *)   calloc((rows * cols * depth), sizeof(double));

    if (matrix3d->matrix  == NULL ||
        matrix3d->matrixc == NULL ||
        matrix3d->matrixd == NULL)
    {
        perror("\n ERROR");
        puts("\n ERROR: Failure to allocate pointers \n\n");
        exit(EXIT_FAILURE);
    }

    /*  now 'point' the pointers, IN THIS ORDER */
    for (i = 0; i < (rows * cols); i++)
        matrix3d->matrixc[i] = matrix3d->matrixd + (i * depth);

    for (i = 0; i < rows; i++)
        matrix3d->matrix[i] = matrix3d->matrixc + (i * cols);

    return(matrix3d);
}


void
Mat3DDestroy(Matrix3D **matrix3d_ptr)
{
    Matrix3D *matrix3d = *matrix3d_ptr;

    free(matrix3d->matrix);
    free(matrix3d->matrixc);
    free(matrix3d->matrixd);

    free(matrix3d);
    *matrix3d_ptr = NULL;
}


double
MatFrobNorm(const double **mat1, const double **mat2, const int row, const int col)
{
    int             i, j;
    double          frobnorm, tmp;

    frobnorm = 0.0;
    for (i = 0; i < row; ++i)
    {
        for (j = 0; j < col; ++j)
        {
            tmp = mat2[i][j] - mat1[i][j];
            frobnorm += tmp * tmp;
        }
    }
/* printf("\nfrobnorm % e", sqrt(frobnorm / (row * col))); */
    return(sqrt(frobnorm / (row * col)));
}


double
MatDiff(const double **mat1, const double **mat2, const int row, const int col)
{
    int             i, j;
    double          frobnorm, tmp;

    frobnorm = 0.0;
    for (i = 0; i < row; ++i)
    {
        for (j = 0; j < col; ++j)
        {
            tmp = mat2[i][j] - mat1[i][j];
            frobnorm += fabs(tmp);
        }
    }
/* printf("\nfrobnorm % e", sqrt(frobnorm / (row * col))); */
    return(frobnorm / (row * col));
}


void
MatCpySqr(double **matrix2, const double **matrix1, const int dim)
{
    memcpy(&matrix2[0][0], &matrix1[0][0], dim * dim * sizeof(double));
}


void
MatCpyGen(double **matrix2, const double **matrix1, const int rows, const int cols)
{
    memcpy(&matrix2[0][0], &matrix1[0][0], rows * cols * sizeof(double));
}


void
MatMultGenUSVOp(double **c, const double **u, double *s, const double **v,
                const int udim, const int sdim, const int vdim)
{
    int             i, j, k;
    double          si, vij;

/*     for (i = 0; i < udim; ++i) */
/*     { */
/*         for (j = 0; j < vdim; ++j) */
/*         { */
/*             c[i][j] = 0.0; */
/*             for (k = 0; k < sdim; ++k) */
/*                 c[i][j] += (u[i][k] * s[k] * v[k][j]); */
/*         } */
/*     } */

    memset(&c[0][0], 0, udim * vdim * sizeof(double));

    for (i = 0; i < sdim; ++i)
    {
        si = s[i];
        for (j = 0; j < vdim; ++j)
        {
            vij = v[i][j];
            for (k = 0; k < udim; ++k)
                c[k][j] += (u[k][i] * si * vij);
        }
    }
}


void
MatMultGen(double **C, const double **A, const int ni, const int nk, const double **B, const int nj)
{
    int             i, j, k;

    /* (i x k)(k x j) = (i x j) */
    for (i = 0; i < ni; ++i)
    {
        for (j = 0; j < nj; ++j)
        {
            C[i][j] = 0.0;
            for (k = 0; k < nk; ++k)
                C[i][j] += (A[i][k] * B[k][j]);
        }
    }
}


void
MatMultGenIp(double **A, const int nk, const int ni, const double **B, const int nj)
{
    int             i, j, k;
    double        **tmpmat = MatAlloc(ni, nj);

    /* (i x k)(k x j) = (k x j) */
    for (i = 0; i < ni; ++i)
    {
        for (j = 0; j < nj; ++j)
        {
            tmpmat[i][j] = 0.0;
            for (k = 0; k < nk; ++k)
                tmpmat[i][j] += (A[i][k] * B[k][j]);
        }
    }

    MatCpyGen(A, (const double **) tmpmat, ni, nj);
    MatDestroy(&tmpmat);
}


/* C = A' B */
void
MatTransMultGen(double **C, const double **A, const int nk, const int ni, const double **B, const int nj)
{
    int             i, j, k;

    /* (k x i)(i x j) = (k x j) */
    for (i = 0; i < ni; ++i)
    {
        for (j = 0; j < nj; ++j)
        {
            C[i][j] = 0.0;
            for (k = 0; k < nk; ++k)
                C[i][j] += (A[k][i] * B[k][j]);
        }
    }
}


void
MatTransMultGenIp(double **A, const int nk, const int ni, const double **B, const int nj)
{
    int             i, j, k;
    double        **tmpmat = MatAlloc(ni, nj);

    /* (i x k)(k x j) = (k x j) */
    for (i = 0; i < ni; ++i)
    {
        for (j = 0; j < nj; ++j)
        {
            tmpmat[i][j] = 0.0;
            for (k = 0; k < nk; ++k)
                tmpmat[i][j] += (A[k][i] * B[k][j]);
        }
    }

    MatCpyGen(A, (const double **) tmpmat, ni, nj);
    MatDestroy(&tmpmat);
}


void
MatMultSym(double **C, const double **A, const double **B, const int len)
{
    int             i, j, k;

    for (i = 0; i < len; ++i)
    {
        for (j = 0; j < len; ++j)
        {
            C[i][j] = 0.0;
            for (k = 0; k < len; ++k)
                C[i][j] += (A[i][k] * B[k][j]);
        }
    }

/*     for (i = 0; i < len; ++i) */
/*         for (j = 0; j < i; ++j) */
/*             C[j][i] = C[i][j]; */
}


/* yes, correct */
void
MatMultSymDiag(double **C, const double **A, const double **B, const int len)
{
    int             i, j;

    for (i = 0; i < len; ++i)
        for (j = 0; j <= i; ++j)
            C[i][j] = C[j][i] = A[i][j] * B[j][j];
}


void
MatTransIp(double **mat, const int dim)
{
    int             i, j;
    double          tmp;

    for (i = 0; i < dim; ++i)
    {
        for (j = 0; j < i; ++j)
        {
            tmp = mat[i][j];
            mat[i][j] = mat[j][i];
            mat[j][i] = tmp;
        }
    }
}


void
MatTransOp(double **outmat, const double **inmat, const int dim)
{
    int             i, j;

    for (i = 0; i < dim; ++i)
        for (j = 0; j < dim; ++j)
            outmat[i][j] = inmat[j][i];
}


void
cholesky(double **mat, const int dim, double *p)
{
    int             i, j, k;
    double          sum;

    for (i = 0; i < dim; ++i)
    {
        for (j = 0; j < dim; ++j)
        {
            sum = mat[i][j];

            for (k = i - 1; k >= 0; --k)
                sum -= (mat[i][k] * mat[j][k]);

            if (i == j)
            {
                if (sum > 0.0)
                {
                    p[i] = sqrt(sum);
                }
                else
                {
                    p[i] = 0.0;
                    fprintf(stderr, "\nERROR: matrix in cholesky() singular ...");
                    exit(EXIT_FAILURE);
                }
            }
            else
            {
                mat[j][i] = sum / p[i];
            }
        }
    }

    for (i = 0; i < dim; ++i)
        for (j = i+1; j < dim; ++j)
            mat[i][j] = 0.0;
}


double
MatSymLnDet(const double **mat, const int dim)
{
    double         *evals = malloc(dim * sizeof(double));
    double        **evecs = MatAlloc(dim, dim);
    int             i;
    double          lndet;

    //eigensym(mat, evals, evecs, dim);
    EigenGSL((const double **) mat, dim, evals, evecs, 0);

    lndet = 0.0;
    for (i = 0; i < dim; ++i)
        lndet += log(fabs(evals[i]));

/*     for (i = 0; i < dim; ++i) */
/*         printf("\nevals: % 12.6e", evals[i]); */

    MatDestroy(&evecs);
    free(evals);

    return(lndet);
}


double
MatTrace(const double **mat, const int dim)
{
    int             i;
    double          trace;

    trace = 0.0;
    for (i = 0; i < dim; ++i)
        trace += mat[i][i];

    return(trace);
}


int
TestZeroOffDiag(const double **mat, const int dim, const double precision)
{
    int             i, j;

    for (i = 0; i < dim; ++i)
        for (j = 0; j < dim; ++j)
            if (i !=j && mat[i][j] > precision)
                return(0);

    return(1);
}


int
TestIdentMat(const double **mat, const int dim, const double precision)
{
    int             i, j;
    double          frobnorm, tmp;

    frobnorm = 0.0;
    for (i = 0; i < dim; ++i)
    {
        for (j = 0; j < dim; ++j)
        {
            if (i == j)
            {
                tmp = 1.0 - mat[i][i];
                frobnorm += tmp * tmp;
            }
            else
            {
                tmp = mat[i][j];
                frobnorm += tmp * tmp;
            }
        }
    }

    if (sqrt(frobnorm / dim) < precision)
    {
        return(1);
    }
    else
        return(0);
}


double
FrobDiffNormIdentMat(const double **mat, const int dim)
{
    int             i, j;
    double          frobnorm, tmp;

    frobnorm = 0.0;
    for (i = 0; i < dim; ++i)
    {
        for (j = 0; j < dim; ++j)
        {
            if (i == j)
            {
                tmp = 1.0 - mat[i][i];
                frobnorm += tmp * tmp;
            }
            else
            {
                tmp = mat[i][j];
                frobnorm += tmp * tmp;
            }
        }
    }

    return(sqrt(frobnorm / dim));
}
