/*
    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 <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <float.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include "DLTmath.h"


/*
Calculate eigenvalues of a square, symmetric, real matrix, using GSL.
Eigenvalues are returned in ascending order, smallest first.
Pointer *eval must be allocated.
Input matrix **cov is NOT perturbed.
*/
void
EigenvalsGSL(const double **mat, const int dim, double *eval)
{
    double        *mat_cpy = NULL;

    mat_cpy = malloc(dim * dim * sizeof(double));
    memcpy(mat_cpy, &mat[0][0], dim * dim * sizeof(double));
    gsl_matrix_view m = gsl_matrix_view_array(mat_cpy, dim, dim);
    gsl_vector_view evalv = gsl_vector_view_array(eval, dim);
    gsl_eigen_symm_workspace *w = gsl_eigen_symm_alloc(dim);

    gsl_eigen_symm(&m.matrix, &evalv.vector, w);
    gsl_sort_vector(&evalv.vector);

    gsl_eigen_symm_free(w);
    free(mat_cpy);
}


/* This one destroys half of the input matrix **mat */
void
EigenvalsGSLDest(double **mat, const int dim, double *eval)
{
    gsl_matrix_view m = gsl_matrix_view_array(mat[0], dim, dim);
    gsl_vector_view evalv = gsl_vector_view_array(eval, dim);
    gsl_eigen_symm_workspace *w = gsl_eigen_symm_alloc(dim);
    gsl_eigen_symm(&m.matrix, &evalv.vector, w);
    gsl_eigen_symm_free(w);
}


/*
gsl_eigen_symmv()

This function computes the eigenvalues and eigenvectors of the real
symmetric matrix A. Additional workspace of the appropriate size must be
provided in w. The diagonal and lower triangular part of A are destroyed
during the computation, but the strict upper triangular part is not
referenced. The eigenvalues are stored in the vector eval and are
unordered. The corresponding eigenvectors are stored in the columns of
the matrix evec. For example, the eigenvector in the first column
corresponds to the first eigenvalue. The eigenvectors are guaranteed to
be mutually orthogonal and normalised to unit magnitude.
*/
void
EigenGSL(const double **mat, const int dim, double *eval, double **evec, int order)
{
    double        *mat_cpy = NULL;

    mat_cpy = malloc(dim * dim * sizeof(double));
    memcpy(mat_cpy, &mat[0][0], dim * dim * sizeof(double));
    gsl_matrix_view m = gsl_matrix_view_array(mat_cpy, dim, dim);
    gsl_matrix_view v = gsl_matrix_view_array(evec[0], dim, dim);
    gsl_vector_view evalv = gsl_vector_view_array(eval, dim);
    gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(dim);

    gsl_eigen_symmv(&m.matrix, &evalv.vector, &v.matrix, w);

    if (order == 0)
        gsl_eigen_symmv_sort(&evalv.vector, &v.matrix, GSL_EIGEN_SORT_VAL_ASC);
    else if (order == 1)
        gsl_eigen_symmv_sort(&evalv.vector, &v.matrix, GSL_EIGEN_SORT_VAL_DESC);

    gsl_eigen_symmv_free(w);
    free(mat_cpy);
}


/* This one destroys half of the input matrix **mat */
void
EigenGSLDest(double **mat, const int dim, double *eval, double **evec, int order)
{
    gsl_matrix_view m = gsl_matrix_view_array(mat[0], dim, dim);
    gsl_matrix_view v = gsl_matrix_view_array(evec[0], dim, dim);
    gsl_vector_view evalv = gsl_vector_view_array(eval, dim);
    gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(dim);
    gsl_eigen_symmv(&m.matrix, &evalv.vector, &v.matrix, w);

    if (order == 0)
        gsl_eigen_symmv_sort(&evalv.vector, &v.matrix, GSL_EIGEN_SORT_VAL_ASC);
    else if (order == 1)
        gsl_eigen_symmv_sort(&evalv.vector, &v.matrix, GSL_EIGEN_SORT_VAL_DESC);

    gsl_eigen_symmv_free(w);
}


void
CalcGSLSVD3(double **a, double **u, double *s, double **vt)
{
    memcpy(u[0], a[0], 9 * sizeof(double));
    // GSL says Jacobi SVD is more accurate the Golub
    svdGSLJacobiDest(u, 3, s, vt);
    Mat3TransposeIp(vt);
}


void
svdGSLDest(double **A, const int dim, double *singval, double **V)
{
    gsl_matrix_view a = gsl_matrix_view_array(A[0], dim, dim);
    gsl_matrix_view v = gsl_matrix_view_array(V[0], dim, dim);
    gsl_vector_view singv = gsl_vector_view_array(singval, dim);
    gsl_vector *work = gsl_vector_alloc(dim);

    gsl_linalg_SV_decomp(&a.matrix, &v.matrix, &singv.vector, work);

    gsl_vector_free(work);
}


void
svdGSLJacobiDest(double **A, const int dim, double *singval, double **V)
{
    gsl_matrix_view a = gsl_matrix_view_array(A[0], dim, dim);
    gsl_matrix_view v = gsl_matrix_view_array(V[0], dim, dim);
    gsl_vector_view singv = gsl_vector_view_array(singval, dim);

    gsl_linalg_SV_decomp_jacobi(&a.matrix, &v.matrix, &singv.vector);
}


void
CholeskyGSLDest(double **A, const int dim)
{
    gsl_matrix_view a = gsl_matrix_view_array(A[0], dim, dim);
    gsl_linalg_cholesky_decomp(&a.matrix);
}


/* static void */
/* write_C_mat(const double **mat, const int dim, int precision, int wrap) */
/* { */
/*     int i, j; */
/*  */
/*     if (wrap == 0) */
/*         wrap = 5; */
/*  */
/*     if (precision == 0) */
/*         precision = 6; */
/*  */
/*     printf("\n\nstatic double mat[%d][%d] = \n{\n", dim, dim); */
/*  */
/*     for (i = 0; i < dim; ++i) */
/*     { */
/*         printf("    {"); */
/*         for (j = 0; j < dim; ++j) */
/*         { */
/*             if (j < dim - 1) */
/*                 printf("% *.*f, ", precision + 1, precision, mat[i][j]); */
/*             else */
/*                 printf("% *.*f", precision + 1, precision, mat[i][j]); */
/*  */
/*             if ((j+1) % wrap == 0 && j != dim - 1) */
/*                 printf("     \n"); */
/*         } */
/*  */
/*         if (i != dim - 1) */
/*             printf("},\n"); */
/*         else */
/*             printf("}\n"); */
/*     } */
/*     printf("};\n\n"); */
/*     fflush(NULL); */
/* } */


/* Calculates the Moore-Penrose pseudoinverse of a symmetric, square matrix.
   Uses GSL to do the singular value decomposition inmat = U S V^T .
   Then constructs the pseudoinverse by V S^-1 U^T .
   Note that here S^-1 is the inverse of only the nonzero elements of S.
   Also note that GSL returns V and not V^T (unlike LAPACK), so we have
   to account for that in the matrix multiplication, since U & V are
   asymmetric in general. */
void
PseudoinvSymGSL(const double **inmat, double **outmat, int n, double tol)
{
    double        **u = MatAlloc(n, n);
    double        **v = MatAlloc(n, n);
    double         *s = malloc(n * sizeof(double));
    int             i, j, k;

    memcpy(u[0], inmat[0], n * n * sizeof(double));

    svdGSLDest(u, n, s, v);

/*     write_C_mat((const double **) u, n, 5, 10); */
/*     write_C_mat((const double **) v, n, 5, 10); */

    for (i = 0; i < n; ++i)
    {
        if (s[i] > tol)
            s[i] = 1.0 / s[i];
        else
            s[i] = 0.0;
    }

    memset(outmat[0], 0, n*n*sizeof(double));

//     for (i = 0; i < n; ++i)
//         for (j = 0; j < n; ++j)
//             outmat[i][j] = 0.0;

    /* (i x k)(k x j) = (i x j) */
    for (i = 0; i < n; ++i)
        for (j = 0; j < n; ++j)
            for (k = 0; k < n; ++k)
                outmat[i][j] += (v[i][k] * s[k] * u[j][k]);

/*     write_C_mat((const double **) outmat, n, 5, 10); */

    free(s);
    MatDestroy(&u);
    MatDestroy(&v);
}




/* Calculates the Moore-Penrose pseudoinverse of a symmetric matrix
   (if necessary). Returns the condition number of the matrix.
   */
double
InvSymEigenOp(double **invmat, const double **mat, int n,
              double *evals, double **evecs, const double tol)
{
    double                   cond;
    int                      i, j, k;
    double                 **tmpmat = MatAlloc(n, n);

    memcpy(tmpmat[0], mat[0], n * n * sizeof(double));

    EigenGSLDest(tmpmat, n, evals, evecs, 0);

    cond = evals[n-1] / evals[0];

    for (i = 0; i < n; ++i)
    {
        if (evals[i] > tol)
            evals[i] = 1.0 / evals[i];
        else
            evals[i] = 0.0;
    }

    memset(invmat[0], 0, n * n * sizeof(double));

    for (i = 0; i < n; ++i)
        for (j = 0; j <= i; ++j)
            for (k = 0; k < n; ++k)
                invmat[i][j] += (evecs[i][k] * evals[k] * evecs[j][k]);

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

    MatDestroy(&tmpmat);

    return(cond);
}


/* Calculates A = LDL^t, where L is a matrix of right eigenvectors and D is
   a diagonal matrix of eigenvalues, in one fell swoop. Except here the
   eigenvalues are delivered as a 1 x n vector. */
/* This function is consistent with eigensym() above - 2006-05-10 */
void
EigenReconSym(double **mat, const double **evecs, const double *evals, const int n)
{
    int             i, j, k;

    /* (i x k)(k x j) = (i x j) */
    for (i = 0; i < n; ++i)
    {
        for (j = 0; j < n; ++j)
        {
            mat[i][j] = 0.0;
            for (k = 0; k < n; ++k)
                mat[i][j] += (evecs[i][k] * evals[k] * evecs[j][k]);
        }
    }
}