/** @file  psMatrix.c
 *
 *  @brief Provides functions for linear algebra operations on psImages and psVectors.
 *
 *  Functions are provided to:
 *      Transpose a psImage
 *      Compute LUD
 *      Solve LUD
 *      Matrix inversion
 *      Calculate determinant
 *      Matrix addition
 *      Matrix subtraction
 *      Matrix multiplication
 *      Calculate Eigenvectors
 *      Convert matrix to vector
 *      Convert vector to matrix
 *
 *  These functions treat psImages as if they were matrices, therefore there is no psMatrix.
 *
 *  @author Ross Harman, MHPCC
 *  @author Robert DeSonia, MHPCC
 *  @author Andy Becker, University of Washington (SVD).
 *
 *  @version $Revision: 1.47.2.3 $ $Name: not supported by cvs2svn $
 *  @date $Date: 2007-03-30 20:25:50 $
 *
 *  Copyright 2004-2005 Maui High Performance Computing Center, University of Hawaii
 */

#ifdef HAVE_CONFIG_H
# include "config.h"
#endif

/******************************************************************************/
/*  INCLUDE FILES                                                             */
/******************************************************************************/
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_math.h>

#include "psMemory.h"
#include "psError.h"
#include "psImage.h"
#include "psVector.h"
#include "psMatrix.h"
#include "psAssert.h"

#include "psTrace.h"

/*****************************************************************************/
/* DEFINE STATEMENTS                                                         */
/*****************************************************************************/

/** Preprocessor macro to generate error for image dimensionality not set to PS_DIMEN_IMAGE */
#define PS_CHECK_DIMEN_AND_TYPE(NAME, PS_DIMEN, CLEANUP)                                             \
if (NAME->type.dimen != PS_DIMEN) {                                                                 \
    psError(PS_ERR_BAD_PARAMETER_TYPE, true,                                                        \
            "Invalid operation. %s has incorrect dimensionality %d.", #NAME, PS_DIMEN);             \
    CLEANUP;                                                                                  \
} else if(NAME->type.type!=PS_TYPE_F64 && NAME->type.type!=PS_TYPE_F32) {                           \
    psError(PS_ERR_BAD_PARAMETER_TYPE, true,                                                        \
            "Invalid operation. %s not PS_TYPE_F64.", #NAME);                                       \
    CLEANUP;                                                                                  \
}

/** Preprocessor macro to check that input is not equal to output */
#define PS_CHECK_POINTERS(NAME1, NAME2, CLEANUP)                                                     \
if (NAME1 == NAME2) {                                                                               \
    psError(PS_ERR_BAD_PARAMETER_VALUE, true,                                                       \
            "Invalid operation: Pointer to %s is same as %s.", #NAME1, #NAME2);                     \
    CLEANUP;                                                                                  \
}

/** Preprocessor macro to check that an image is square */
#define PS_CHECK_SQUARE(NAME, CLEANUP)                                                               \
if (NAME->numCols != NAME->numRows) {                                                               \
    psError(PS_ERR_BAD_PARAMETER_SIZE, true, "Invalid operation: %s not square array.", #NAME);     \
    CLEANUP;                                                                                  \
}

/** Preprocessor macro to initalize a GSL matrix. */
#define PS_GSL_MATRIX_INITIALIZE(LHS_NAME, RHS_NAME)                                                \
LHS_NAME.size1 = numRows;                                                                           \
LHS_NAME.size2 = numCols;                                                                           \
LHS_NAME.tda   = numCols;                                                                           \
LHS_NAME.data  = RHS_NAME;


/*****************************************************************************/
/* FILE STATIC FUNCTIONS                                                     */
/*****************************************************************************/

static void  psVectorToGslVector(gsl_vector *outGslVector, const psVector *inVector);
static void gslVectorToPsVector(psVector *outVector, gsl_vector *inGslVector);
static void  psImageToGslMatrix(gsl_matrix *outGslMatrix, const psImage *inImage);
static void gslMatrixToPsImage(psImage *outImage, gsl_matrix *inGslMatrix);

/** Static function to copy psF32 or psF64 vector data to a GSL vector */
static void  psVectorToGslVector(gsl_vector *outGslVector,
                                 const psVector *inVector)
{
    psU32 i = 0;
    psU32 n = 0;


    n = inVector->n;
    for(i=0; i<n; i++) {
        if(inVector->type.type == PS_TYPE_F32) {
            outGslVector->data[i] = (psF64)inVector->data.F32[i];
        } else {
            outGslVector->data[i] = inVector->data.F64[i];
        }
    }
}

/** Static function to copy GSL vector data to a psF32 or psF64 vector */
static void gslVectorToPsVector(psVector *outVector,
                                gsl_vector *inGslVector)
{
    psU32 i = 0;
    psU32 n = 0;


    n = outVector->n;
    for(i=0; i<n; i++) {
        if(outVector->type.type == PS_TYPE_F32) {
            outVector->data.F32[i] = (psF32)inGslVector->data[i];
        } else {
            outVector->data.F64[i] = inGslVector->data[i];
        }
    }
}

/** Static function to copy psF32 or psF64 image data to a GSL matrix */
static void  psImageToGslMatrix(gsl_matrix *outGslMatrix,
                                const psImage *inImage)
{
    psU32 i = 0;
    psU32 j = 0;
    psU32 numRows = 0;
    psU32 numCols = 0;


    numRows = inImage->numRows;
    numCols = inImage->numCols;
    if(inImage->type.type == PS_TYPE_F32) {
        for(i=0; i<numRows; i++) {
            for(j=0; j<numCols; j++) {
                outGslMatrix->data[i*numCols+j] = inImage->data.F32[i][j];
            }
        }
    } else {
        for(i=0; i<numRows; i++) {
            for(j=0; j<numCols; j++) {
                outGslMatrix->data[i*numCols+j] = inImage->data.F64[i][j];
            }
        }
    }
}

/** Static function to copy GSL matrix data to a psF32 or psF64 image */
static void gslMatrixToPsImage(psImage *outImage,
                               gsl_matrix *inGslMatrix)
{
    psU32 i = 0;
    psU32 j = 0;
    psU32 numRows = 0;
    psU32 numCols = 0;


    numRows = outImage->numRows;
    numCols = outImage->numCols;
    if(outImage->type.type == PS_TYPE_F32) {
        for(i=0; i<numRows; i++) {
            for(j=0; j<numCols; j++) {
                outImage->data.F32[i][j] = inGslMatrix->data[i*numCols+j];
            }
        }
    } else {
        for(i=0; i<numRows; i++) {
            for(j=0; j<numCols; j++) {
                outImage->data.F64[i][j] = inGslMatrix->data[i*numCols+j];
            }
        }
    }
}


/*****************************************************************************/
/* FUNCTION IMPLEMENTATION - PUBLIC                                          */
/*****************************************************************************/

psImage* psMatrixLUD(psImage* out,
                     psVector** perm,
                     const psImage* in)
{
    psS32 signum = 0;
    psS32 numRows = 0;
    psS32 numCols = 0;
    gsl_matrix *lu = NULL;
    gsl_permutation permGSL;


    #define psMatrixLUD_EXIT {psFree(out); return NULL;}

    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixLUD_EXIT);
    PS_CHECK_POINTERS(in, out, psMatrixLUD_EXIT);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixLUD_EXIT);
    PS_ASSERT_GENERAL_PTR_NON_NULL(perm, psMatrixLUD_EXIT);

    out = psImageRecycle(out, in->numCols, in->numRows, in->type.type);

    PS_CHECK_SQUARE(in, psMatrixLUD_EXIT); // gsl_linalg_LU_decomp would fail on non-square input.
    PS_CHECK_SQUARE(out, psMatrixLUD_EXIT);

    // Initialize data
    numRows = in->numRows;
    numCols = in->numCols;

    // Initialize GSL data
    permGSL.size = numCols;
    if (sizeof(size_t) == 4) {
        *perm = psVectorRecycle(*perm, numCols, PS_TYPE_S32);
    } else if (sizeof(size_t) == 8) {
        *perm = psVectorRecycle(*perm, numCols, PS_TYPE_S64);
    } else {
        psError(PS_ERR_UNKNOWN, true,
                "Failed to allocate the permutation vector; "
                "could not determine the cooresponding data type.");
        psMatrixLUD_EXIT;
    }

    (*perm)->n = numCols;
    permGSL.data = (psPtr)((*perm)->data.U8);
    lu = gsl_matrix_alloc(numRows, numCols);

    // Copy psImage data into GSL matrix data
    psImageToGslMatrix(lu, in);

    // Calculate LU decomposition
    gsl_linalg_LU_decomp(lu, &permGSL, &signum); // N.B., uses Gaussian Elimination with partial pivoting.

    // Copy GSL matrix data to psImage data
    gslMatrixToPsImage(out, lu);

    // Free GSL data
    gsl_matrix_free(lu);

    return out;
}

psVector* psMatrixLUSolve(psVector* out,
                          const psImage* LU,
                          const psVector* RHS,
                          const psVector* perm)
{
    psS32 numRows = 0;
    psS32 numCols = 0;
    gsl_matrix *lu;
    gsl_permutation permGSL;
    gsl_vector *b = NULL;
    gsl_vector *x = NULL;

    #define LUSOLVE_CLEANUP {psFree(out); return NULL;}

    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(LU, LUSOLVE_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(LU, PS_DIMEN_IMAGE, LUSOLVE_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(LU, LUSOLVE_CLEANUP);
    PS_ASSERT_GENERAL_VECTOR_NON_NULL(RHS, LUSOLVE_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(RHS, PS_DIMEN_VECTOR, LUSOLVE_CLEANUP);
    PS_ASSERT_GENERAL_VECTOR_NON_NULL(perm, LUSOLVE_CLEANUP);

    out = psVectorRecycle(out, LU->numRows, LU->type.type);

    PS_CHECK_POINTERS(out, RHS, LUSOLVE_CLEANUP);
    PS_CHECK_POINTERS(RHS, perm, LUSOLVE_CLEANUP);
    PS_CHECK_POINTERS(out, perm, LUSOLVE_CLEANUP);

    // Initialize data
    numRows = LU->numRows;
    numCols = LU->numCols;

    // Initialize GSL data
    lu = gsl_matrix_alloc(numRows, numCols);
    psImageToGslMatrix(lu, LU);
    b = gsl_vector_alloc(RHS->n);
    psVectorToGslVector(b, RHS);
    x = gsl_vector_alloc(RHS->n);

    out->n = numCols;
    permGSL.size = perm->n;
    permGSL.data = (psPtr)(perm->data.U8);

    // Solve for {x} in equation: {b} = [A]{x}
    gsl_linalg_LU_solve(lu, &permGSL, b, x);

    // Copy GSL vector data to psVector data
    gslVectorToPsVector(out, x);

    // Free GSL data
    gsl_vector_free(b);
    gsl_vector_free(x);
    gsl_matrix_free(lu);

    return out;
}

// This is a temporary gauss-jordan solver based on gene's
// version based on the Numerical Recipes version
bool psMatrixGJSolve(psImage *a,
                     psVector *b
                    )
{
    PS_ASSERT_IMAGE_NON_NULL(a, false);
    PS_ASSERT_VECTOR_NON_NULL(b, false);
    PS_ASSERT_IMAGE_TYPE(a, PS_TYPE_F64, false);
    PS_ASSERT_VECTOR_TYPE(b, PS_TYPE_F64, false);
    PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
#if 1					// Use a more robust LU decomposition from GSL
    // Initialize GSL data from a and b
    gsl_matrix *a_gsl = gsl_matrix_alloc(a->numRows, a->numCols);
    psImageToGslMatrix(a_gsl, a);
    gsl_vector *b_gsl = gsl_vector_alloc(b->n);
    psVectorToGslVector(b_gsl, b);
    
    gsl_permutation *perm_gsl = gsl_permutation_alloc(b->n);

    // Solve for {x} in equation: {b} = [A]{x}
    int signum = 0;
    if (gsl_linalg_LU_decomp(a_gsl, perm_gsl, &signum) != 0) {
	psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix.\n");
	psTrace("psLib.math", 4, "LU decomposition failed.\n");

	gsl_matrix_free(a_gsl);
	gsl_vector_free(b_gsl);
	gsl_permutation_free(perm_gsl);

	return false;
    }

    gsl_vector *x_gsl = gsl_vector_alloc(b->n);
    int solver_failed = gsl_linalg_LU_solve(a_gsl, perm_gsl, b_gsl, x_gsl);

    for (int i = 0; i < x_gsl->size && !solver_failed; i++) { // gsl_linalg_LU_solve doesn't always catch errors
	if (!gsl_finite(x_gsl->data[i])) {
	    solver_failed = 1;
	}
    }
    
    if (solver_failed != 0) {
	psError(PS_ERR_BAD_PARAMETER_VALUE, true, "LU solver failed.\n");
	psTrace("psLib.math", 4, "LU solver failed.\n");

	gsl_matrix_free(a_gsl);
	gsl_vector_free(x_gsl);
	gsl_vector_free(b_gsl);
	gsl_permutation_free (perm_gsl);

	return false;
    }
    
    gslVectorToPsVector(b, x_gsl);	// copy answer back to b
    gsl_vector_free(b_gsl);
    gsl_vector_free(x_gsl);

    // Invert A as we may want to use it for a covariance
    gsl_matrix *ia_gsl = gsl_matrix_alloc(a->numRows, a->numCols); // inverse matrix if it exists
    if (a->numRows == a->numCols) {
	if (gsl_linalg_LU_invert(a_gsl, perm_gsl, ia_gsl) != 0) {
	    psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix.\n");
	    psTrace("psLib.math", 4, "Singular Matrix (1).\n");
	    
	    gsl_matrix_free(a_gsl);
	    gsl_matrix_free(ia_gsl);
	    gsl_permutation_free (perm_gsl);
	    
	    return false;
	}
	
	// Copy a^{-1} back to a
	gslMatrixToPsImage(a, ia_gsl);
    }

    // Free GSL data
    gsl_matrix_free(a_gsl);
    gsl_matrix_free(ia_gsl);
    gsl_permutation_free (perm_gsl);

    return true;
#else
    int Nx = a->numCols;
    PS_ASSERT_VECTOR_SIZE(b, (long int)Nx, false);

    psF64 *vector = b->data.F64;
    psF64 **matrix = a->data.F64;

    int *indxc = psAlloc(Nx*sizeof(int));
    int *indxr = psAlloc(Nx*sizeof(int));
    int *ipiv  = psAlloc(Nx*sizeof(int));
    memset(ipiv, 0, Nx*sizeof(int));

    int irow = 0;
    int icol = 0;

    for (int i = 0; i < Nx; i++) {
        double big = 0.0;
        for (int j = 0; j < Nx; j++) {
            if (!isfinite(matrix[i][j])) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
                        "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
                        i, j, matrix[i][j]);
                goto fescape;
            }
            if (ipiv[j] != 1) {
                for (int k = 0; k < Nx; k++) {
                    if (ipiv[k] == 0) {
                        if (fabs(matrix[j][k]) >= big) {
                            big  = fabs(matrix[j][k]);
                            irow = j;
                            icol = k;
                        }
                    } else {
                        if (ipiv[k] > 1) {
			    psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (1).\n");
                            psTrace("psLib.math", 4, "Singular Matrix (1).\n");
                            goto fescape;
                        }
                    }
                }
            }
        }
        ipiv[icol]++;
        if (irow != icol) {
            for (int l = 0; l < Nx; l++) {
                PS_SWAP(matrix[irow][l], matrix[icol][l]);
            }
            PS_SWAP(vector[irow], vector[icol]);
        }
        indxr[i] = irow;
        indxc[i] = icol;
	if (matrix[icol][icol] > -FLT_EPSILON && matrix[icol][icol] < FLT_EPSILON) {
            psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (2).\n");
            psTrace (__func__, 4, "Singular Matrix (2).\n");
            goto fescape;
        }
        double pivinv = 1.0 / matrix[icol][icol];
        matrix[icol][icol] = 1.0;
        for (int l = 0; l < Nx; l++) {
            matrix[icol][l] *= pivinv;
        }
        vector[icol] *= pivinv;

        for (int ll = 0; ll < Nx; ll++) {
            if (ll != icol) {
                double dum = matrix[ll][icol];
                matrix[ll][icol] = 0.0;
                for (int l = 0; l < Nx; l++) {
                    matrix[ll][l] -= matrix[icol][l]*dum;
                }
                vector[ll] -= vector[icol]*dum;
            }
        }
    }

    for (int l = Nx - 1; l >= 0; l--) {
        if (indxr[l] != indxc[l]) {
            for (int k = 0; k < Nx; k++) {
                PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]);
            }
        }
    }
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return true;

fescape:
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return false;
#endif
}

// This is a temporary gauss-jordan solver based on gene's
// version based on the Numerical Recipes version
bool psMatrixGJSolveF32(psImage *a,
                        psVector *b
                       )
{
    PS_ASSERT_IMAGE_NON_NULL(a, false);
    PS_ASSERT_VECTOR_NON_NULL(b, false);
    PS_ASSERT_IMAGE_TYPE(a, PS_TYPE_F32, false);
    PS_ASSERT_VECTOR_TYPE(b, PS_TYPE_F32, false);
    PS_ASSERT_INT_EQUAL(a->numCols, a->numRows, false);
    int Nx = a->numCols;
    PS_ASSERT_VECTOR_SIZE(b, (long int)Nx, false);

    psF32 *vector = b->data.F32;
    psF32 **matrix = a->data.F32;
    int *indxc = psAlloc(Nx*sizeof(int));
    int *indxr = psAlloc(Nx*sizeof(int));
    int *ipiv  = psAlloc(Nx*sizeof(int));
    memset(ipiv, 0, Nx*sizeof(int));

    int irow = 0;
    int icol = 0;

    for (int i = 0; i < Nx; i++) {
        double big = 0.0;
        for (int j = 0; j < Nx; j++) {
            if (!isfinite(matrix[i][j])) {
                psError(PS_ERR_BAD_PARAMETER_VALUE, 3,
                        "Input matrix contains non-finite elements: matrix[%d][%d] is %.2f\n",
                        i, j, matrix[i][j]);
                goto fescape;
            }
            if (ipiv[j] != 1) {
                for (int k = 0; k < Nx; k++) {
                    if (ipiv[k] == 0) {
                        if (fabs(matrix[j][k]) >= big) {
                            big  = fabs(matrix[j][k]);
                            irow = j;
                            icol = k;
                        }
                    } else {
                        if (ipiv[k] > 1) {
                            // psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (1).\n");
                            psTrace("psLib.math", 4, "Singular Matrix (1).\n");
                            goto fescape;
                        }
                    }
                }
            }
        }
        ipiv[icol]++;
        if (irow != icol) {
            for (int l = 0; l < Nx; l++) {
                PS_SWAP(matrix[irow][l], matrix[icol][l]);
            }
            PS_SWAP(vector[irow], vector[icol]);
        }
        indxr[i] = irow;
        indxc[i] = icol;
        if (matrix[icol][icol] == 0.0) {
            // psError(PS_ERR_BAD_PARAMETER_VALUE, true, "Singular Matrix (2).\n");
            psTrace (__func__, 4, "Singular Matrix (2).\n");
            goto fescape;
        }
        double pivinv = 1.0 / matrix[icol][icol];
        matrix[icol][icol] = 1.0;
        for (int l = 0; l < Nx; l++) {
            matrix[icol][l] *= pivinv;
        }
        vector[icol] *= pivinv;

        for (int ll = 0; ll < Nx; ll++) {
            if (ll != icol) {
                double dum = matrix[ll][icol];
                matrix[ll][icol] = 0.0;
                for (int l = 0; l < Nx; l++) {
                    matrix[ll][l] -= matrix[icol][l]*dum;
                }
                vector[ll] -= vector[icol]*dum;
            }
        }
    }

    for (int l = Nx - 1; l >= 0; l--) {
        if (indxr[l] != indxc[l]) {
            for (int k = 0; k < Nx; k++) {
                PS_SWAP(matrix[k][indxr[l]], matrix[k][indxc[l]]);
            }
        }
    }
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return true;

fescape:
    psFree(ipiv);
    psFree(indxr);
    psFree(indxc);
    return false;
}

psImage* psMatrixInvert(psImage* out,
                        const psImage* in,
                        float *determinant)
{
    psS32 signum = 0;
    psS32 numRows = 0;
    psS32 numCols = 0;
    gsl_matrix *inv = NULL;
    gsl_matrix *lu = NULL;
    gsl_permutation *perm = NULL;

    #define INVERT_CLEANUP { psFree(out); return NULL; }
    // Error checks
    PS_ASSERT_GENERAL_PTR_NON_NULL(determinant, INVERT_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, INVERT_CLEANUP);
    PS_CHECK_POINTERS(in, out, INVERT_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, INVERT_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in, INVERT_CLEANUP);

    out = psImageRecycle(out, in->numCols, in->numRows, in->type.type);

    PS_CHECK_SQUARE(in, INVERT_CLEANUP);
    PS_CHECK_SQUARE(out, INVERT_CLEANUP);

    // Initialize data
    numRows = in->numRows;
    numCols = in->numCols;

    // Initialize GSL data
    perm = gsl_permutation_alloc(numRows);
    lu = gsl_matrix_alloc(numRows, numCols);
    inv = gsl_matrix_alloc(numRows, numCols);
    psImageToGslMatrix(lu, in);

    // Invert data and calculate determinant
    gsl_linalg_LU_decomp(lu, perm, &signum);
    gsl_linalg_LU_invert(lu, perm, inv);
    *determinant = (float)gsl_linalg_LU_det(lu, signum);

    // Copy GSL matrix data to psImage data
    gslMatrixToPsImage(out, inv);

    // Free GSL structs
    gsl_permutation_free(perm);
    gsl_matrix_free(lu);
    gsl_matrix_free(inv);

    return out;
}

float psMatrixDeterminant(const psImage* in)
{
    psS32 signum = 0;
    psS32 numRows = 0;
    psS32 numCols = 0;
    psF32 det = 0;
    gsl_matrix *lu = NULL;
    gsl_permutation *perm = NULL;

    #define DETERMINANT_EXIT { return 0; }
    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, DETERMINANT_EXIT);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, DETERMINANT_EXIT);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in, DETERMINANT_EXIT);
    PS_CHECK_SQUARE(in, DETERMINANT_EXIT);

    // Initialize data
    numRows = in->numRows;
    numCols = in->numCols;

    // Allocate GSL structs
    perm = gsl_permutation_alloc(numRows);
    lu = gsl_matrix_alloc(numRows, numCols);
    psImageToGslMatrix(lu, in);

    // Calculate determinant
    //    det = psAlloc(sizeof(psF32));
    gsl_linalg_LU_decomp(lu, perm, &signum);
    det = (psF32)gsl_linalg_LU_det(lu, signum);

    // Free GSL structs
    gsl_permutation_free(perm);
    gsl_matrix_free(lu);

    return det;
}

psImage* psMatrixMultiply(psImage* out,
                          const psImage* in1,
                          const psImage* in2)
{
    gsl_matrix *m1 = NULL;
    gsl_matrix *m2 = NULL;
    gsl_matrix *m3 = NULL;

    #define MULTIPLY_CLEANUP { psFree(out); return NULL; }

    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in1, MULTIPLY_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in2, MULTIPLY_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in1, MULTIPLY_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in2, MULTIPLY_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(in1, PS_DIMEN_IMAGE, MULTIPLY_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(in2, PS_DIMEN_IMAGE, MULTIPLY_CLEANUP);
    PS_CHECK_POINTERS(in1, out, MULTIPLY_CLEANUP);
    PS_CHECK_POINTERS(in1, in2, MULTIPLY_CLEANUP);

    if (in1->numRows != in2->numCols) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true, "Invalid operation: number of rows of in1 != number of cols of in2.");
        MULTIPLY_CLEANUP;
    }
    if (in1->type.type != in2->type.type) {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true, "Invalid operation: data types of in1 and in2 must match.");
        MULTIPLY_CLEANUP;
    }

    out = psImageRecycle(out, in2->numCols, in1->numRows, in2->type.type);

    // Initialize GSL data
    m1 = gsl_matrix_alloc(in1->numRows, in1->numCols);
    psImageToGslMatrix(m1, in1);
    m2 = gsl_matrix_alloc(in2->numRows, in2->numCols);
    psImageToGslMatrix(m2, in2);
    m3 = gsl_matrix_alloc(out->numRows, out->numCols);

    // Perform multiplication
    // gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, m1, m2, 0.0, m3);
    gsl_linalg_matmult(m1, m2, m3);

    // Copy GSL matrix data to psImage data
    gslMatrixToPsImage(out, m3);

    // Free GSL structs
    gsl_matrix_free(m1);
    gsl_matrix_free(m2);
    gsl_matrix_free(m3);

    return out;
}

psImage* psMatrixTranspose(psImage* out,
                           const psImage* in)
{
    psU32 i = 0;
    psU32 j = 0;
    psS32 numRowsIn = 0;
    psS32 numColsIn = 0;
    psS32 numRowsOut = 0;
    psS32 numColsOut = 0;

    #define TRANSPOSE_CLEANUP { psFree(out); return NULL; }
    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, TRANSPOSE_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, TRANSPOSE_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in, TRANSPOSE_CLEANUP);
    PS_CHECK_POINTERS(in, out, TRANSPOSE_CLEANUP);

    out = psImageRecycle(out, in->numCols, in->numRows, in->type.type);

    // Initialize data
    numRowsIn = in->numRows;
    numColsIn = in->numCols;
    numRowsOut = out->numRows;
    numColsOut = out->numCols;

    if(numRowsIn!=numColsOut && numRowsOut!=numColsIn) {
        psError(PS_ERR_BAD_PARAMETER_VALUE, true, _("Number of rows do not match number of columns."));
        TRANSPOSE_CLEANUP;
    }

    if(out->type.type == PS_TYPE_F32) {
        for(i=0; i<numRowsOut; i++) {
            for(j=0; j<numColsOut; j++) {
                out->data.F32[i][j] = in->data.F32[j][i];
            }
        }
    } else {
        for(i=0; i<numRowsOut; i++) {
            for(j=0; j<numColsOut; j++) {
                out->data.F64[i][j] = in->data.F64[j][i];
            }
        }
    }

    return out;
}

psImage* psMatrixEigenvectors(psImage* out,
                              const psImage* in)
{
    psS32 numRows = 0;
    psS32 numCols = 0;
    gsl_vector *eVals = NULL;
    gsl_eigen_symmv_workspace *w = NULL;
    gsl_matrix *outGSL = NULL;
    gsl_matrix *inGSL = NULL;

    #define EIGENVECTORS_CLEANUP { psFree(out); return NULL; }
    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, EIGENVECTORS_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, EIGENVECTORS_CLEANUP);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(in, EIGENVECTORS_CLEANUP);
    PS_CHECK_POINTERS(in, out, EIGENVECTORS_CLEANUP);

    out = psImageRecycle(out, in->numCols, in->numRows, in->type.type);

    // Initialize data
    numRows = in->numRows;
    numCols = in->numCols;

    inGSL = gsl_matrix_alloc(numRows, numCols);
    psImageToGslMatrix(inGSL, in);
    outGSL = gsl_matrix_alloc(numRows, numCols);

    // Allocate GSL structs
    eVals = gsl_vector_alloc(numRows);
    w = gsl_eigen_symmv_alloc(numRows);

    // Non-square matrices not allowed
    PS_CHECK_SQUARE(in, EIGENVECTORS_CLEANUP);
    PS_CHECK_SQUARE(out, EIGENVECTORS_CLEANUP);

    // Calculate Eigenvalues and Eigenvectors...Eigenvalues not currently used
    gsl_eigen_symmv(inGSL, eVals, outGSL, w);

    // Copy GSL matrix data to psImage data
    gslMatrixToPsImage(out, outGSL);

    // Free GSL structs
    gsl_matrix_free(inGSL);
    gsl_matrix_free(outGSL);
    gsl_eigen_symmv_free(w);
    gsl_vector_free(eVals);

    return out;
}

psVector* psMatrixToVector(psVector* outVector,
                           const psImage* inImage)
{
    psS32 size = 0;

    #define psMatrixToVector_EXIT {psFree(outVector); return NULL;}

    // Error checks
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(inImage, psMatrixToVector_EXIT);
    PS_CHECK_DIMEN_AND_TYPE(inImage, PS_DIMEN_IMAGE, psMatrixToVector_EXIT);
    PS_ASSERT_GENERAL_IMAGE_NON_EMPTY(inImage, psMatrixToVector_EXIT);

    if (inImage->numRows == 1) {
        // Create transposed row vector
        outVector = psVectorRecycle(outVector, inImage->numCols, inImage->type.type);
        outVector->type.dimen = PS_DIMEN_TRANSV;
    } else if (inImage->numCols == 1) {
        // Create non-transposed column vector
        outVector = psVectorRecycle(outVector, inImage->numRows, inImage->type.type);
    } else {
        psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                "Image does not have dim with 1 col or 1 row: (%d x %d).",
                inImage->numRows, inImage->numCols);
        psMatrixToVector_EXIT;
    }

    // More checks
    if (outVector->type.dimen == PS_DIMEN_VECTOR) {
        PS_CHECK_DIMEN_AND_TYPE(outVector, PS_DIMEN_VECTOR, psMatrixToVector_EXIT);

        if (outVector->n == 0) {
            outVector->n = inImage->numRows;
        }

        if (outVector->n != inImage->numRows) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image and vector sizes differ: (%d vs %ld).",
                    inImage->numRows, outVector->n);
            psMatrixToVector_EXIT;
        }

        size = PSELEMTYPE_SIZEOF(inImage->type.type) * inImage->numRows;

    } else if (outVector->type.dimen == PS_DIMEN_TRANSV) {
        PS_CHECK_DIMEN_AND_TYPE(outVector, PS_DIMEN_TRANSV, psMatrixToVector_EXIT);

        if (outVector->n == 0) {
            outVector->n = inImage->numCols;
        }

        if (outVector->n != inImage->numCols) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image and vector sizes differ: (%d vs %ld).",
                    inImage->numCols, outVector->n);
            psMatrixToVector_EXIT;
        }

        size = PSELEMTYPE_SIZEOF(inImage->type.type) * inImage->numCols;
    }

    memcpy(outVector->data.U8, inImage->data.U8[0], size);

    return outVector;
}

psImage* psVectorToMatrix(psImage* outImage,
                          const psVector* inVector)
{
    psS32 size = 0;

    #define VECTORTOMATRIX_CLEANUP {psFree(outImage); return NULL; }
    // Error checks
    PS_ASSERT_GENERAL_VECTOR_NON_NULL(inVector, VECTORTOMATRIX_CLEANUP);

    if (inVector->type.dimen == PS_DIMEN_VECTOR) {
        PS_CHECK_DIMEN_AND_TYPE(inVector, PS_DIMEN_VECTOR, VECTORTOMATRIX_CLEANUP);
        PS_ASSERT_GENERAL_VECTOR_NON_EMPTY(inVector, VECTORTOMATRIX_CLEANUP);

        outImage = psImageRecycle(outImage, 1, inVector->n, inVector->type.type);

        // More checks for PS_DIMEN_VECTOR
        if (outImage->numCols > 1) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image has more than 1 column: numCols = %d.",
                    outImage->numCols);
            VECTORTOMATRIX_CLEANUP;
        } else if (outImage->numRows != inVector->n) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image and vector sizes differ: (%d vs %ld).",
                    outImage->numRows, inVector->n);
            VECTORTOMATRIX_CLEANUP;
        }

        size = PSELEMTYPE_SIZEOF(outImage->type.type) * outImage->numRows;

    } else if (inVector->type.dimen == PS_DIMEN_TRANSV) {
        PS_CHECK_DIMEN_AND_TYPE(inVector, PS_DIMEN_TRANSV, VECTORTOMATRIX_CLEANUP);
        PS_ASSERT_GENERAL_VECTOR_NON_EMPTY(inVector, VECTORTOMATRIX_CLEANUP);
        outImage = psImageRecycle(outImage, inVector->n, 1, inVector->type.type);
        // More checks for PS_DIMEN_TRANSV
        if (outImage->numRows > 1) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image has more than 1 row: numRows = %d.",
                    outImage->numRows);
            VECTORTOMATRIX_CLEANUP;
        } else if (outImage->numCols != inVector->n) {
            psError(PS_ERR_BAD_PARAMETER_SIZE, true,
                    "Image and vector sizes differ: (%d vs %ld).",
                    outImage->numCols, inVector->n);
            VECTORTOMATRIX_CLEANUP;
        }

        size = PSELEMTYPE_SIZEOF(outImage->type.type) * outImage->numCols;
    }

    PS_ASSERT_GENERAL_IMAGE_NON_NULL(outImage, VECTORTOMATRIX_CLEANUP);
    PS_CHECK_DIMEN_AND_TYPE(outImage, PS_DIMEN_IMAGE, VECTORTOMATRIX_CLEANUP);

    memcpy(outImage->data.U8[0], inVector->data.U8, size);

    return outImage;
}

// This code supplied by Andy Becker (becker@astro.washington.edu)
psImage *psMatrixSVD(psImage* evec, psVector* eval, const psImage* in)
{
    #define psMatrixSVD_EXIT {psFree(evec); psFree(eval); return NULL;}

    // Error checks  Missing one for eval
    PS_ASSERT_GENERAL_IMAGE_NON_NULL(in, psMatrixSVD_EXIT);
    PS_CHECK_POINTERS(in, evec, psMatrixSVD_EXIT);
    PS_CHECK_DIMEN_AND_TYPE(in, PS_DIMEN_IMAGE, psMatrixSVD_EXIT);

    // evec ends up : numCols x numCols
    evec = psImageRecycle(evec,  in->numCols, in->numCols, in->type.type);
    eval = psVectorRecycle(eval, in->numCols, in->type.type);

    // Initialize data
    int numRows = in->numRows;
    int numCols = in->numCols;

    gsl_matrix *A = gsl_matrix_alloc(numRows, numCols);
    gsl_matrix *V = gsl_matrix_alloc(numCols, numCols);
    gsl_vector *S = gsl_vector_alloc(numCols);
    gsl_vector *work = gsl_vector_alloc(numCols);

    // Copy psImage data into GSL matrix data
    psImageToGslMatrix(A, in);

    // Calculate SVD decomposition
    gsl_linalg_SV_decomp(A, V, S, work);

    // Copy GSL matrix data to psImage data
    gslMatrixToPsImage(evec, V);
    gslVectorToPsVector(eval, S);

    // Take the square root of eval
    for (int i = 0; i < eval->n; i++) {
        eval->data.F64[i] = sqrt(eval->data.F64[i]);
        /* make sure that these things are sorted! */
        if (i > 0) {
            assert(eval->data.F64[i] <= eval->data.F64[i-1]);
        }
    }

    // Free GSL data
    gsl_matrix_free(A);
    gsl_matrix_free(V);
    gsl_vector_free(S);
    gsl_vector_free(work);

    return evec;
}
