/*
 * This file is part of the ESO Common Pipeline Library
 * Copyright (C) 2001-2008,2014 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, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

/**
 * @defgroup cpl_matrix Matrices
 *
 * This module provides functions to create, destroy and use a @em cpl_matrix.
 * The elements of a @em cpl_matrix with M rows and N columns are counted 
 * from 0,0 to M-1,N-1. The matrix element 0,0 is the one at the upper left
 * corner of a matrix. The CPL matrix functions work properly only in the 
 * case the matrices elements do not contain garbage (such as @c NaN or 
 * infinity).
 *
 * @par Synopsis:
 * @code
 *   #include <cpl_matrix.h>
 * @endcode
 */

#include "cpl_matrix.h"
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <assert.h>

/**@{*/

#ifndef inline
#define inline /* inline */
#endif

#define dtiny(x, t) ((x) < 0.0 ? (x) >= -(t) : (x) <= (t))

#define CPL_ATTR_NONNULL

typedef enum {
    CPL_ERROR_NONE = 0,
    CPL_ERROR_NULL_INPUT,
    CPL_ERROR_ILLEGAL_INPUT,
    CPL_ERROR_INCOMPATIBLE_INPUT,
    CPL_ERROR_ILLEGAL_OUTPUT,
    CPL_ERROR_ACCESS_OUT_OF_RANGE,
    CPL_ERROR_SINGULAR_MATRIX,
    CPL_ERROR_DIVISION_BY_ZERO,
} cpl_error_code;


/*
 * The cpl_matrix type:
 */

struct _cpl_matrix {
    cpl_size nc;
    cpl_size nr;
    double  *m;
};

struct _cpl_vector {
    cpl_size n;
    double * d;
} ;

#define CX_MIN(a, b) (((a) < (b)) ? (a) : (b))
#define cpl_ensure_code(a, ret) \
    do { \
        if (!(a)) \
            return ret; \
    } while(0)

#define cpl_ensure(a, code, ret) \
    do { \
        if (!(a)) \
            return ret; \
    } while(0)

static void * cpl_malloc(size_t sz)
{
    return malloc(sz);
}

static void * cpl_calloc(size_t sz, size_t elsz)
{
    return calloc(sz, elsz);
}

static void cpl_free(void * p)
{
    free(p);
}

static cpl_error_code cpl_error_set_(cpl_error_code code)
{
    return code;
}

static cpl_error_code cpl_error_set(const char *a , cpl_error_code code)
{
    return code;
}

static void cpl_error_set_where(const char *a)
{
}

static cpl_error_code cpl_error_set_where_(void)
{
    return 0;
}

static cpl_error_code cpl_error_get_code(void)
{
    return 0;
}

/* minimal vector, no error checking */
cpl_vector * cpl_vector_new(cpl_size n)
{
    cpl_vector * v = cpl_malloc(sizeof(*v));
    v->d = cpl_malloc(n * sizeof(v->d[0]));
    v->n = n;
    return v;
}

cpl_vector * cpl_vector_duplicate(const cpl_vector * a)
{
    cpl_vector *v = cpl_vector_new(a->n);
    memcpy(v->d, a->d, a->n * sizeof(a->d[0]));
    return v;
}

cpl_vector * cpl_vector_wrap(long n, double *d)
{
    cpl_vector *v = cpl_malloc(sizeof(*v));
    v->n = n;
    v->d = d;
    return v;
}

void cpl_vector_delete(cpl_vector * v)
{
    if (v == NULL)
        return;
    cpl_free(v->d);
    cpl_free(v);
}

void cpl_vector_unwrap(cpl_vector * v)
{
    if (v == NULL)
        return;
    cpl_free(v);
}

long cpl_vector_get_size(const cpl_vector * v)
{
    return v->n;
}

void cpl_vector_set(cpl_vector * v, cpl_size i, double val)
{
    v->d[i] = val;
}

double cpl_vector_get(cpl_vector * v, cpl_size i)
{
    return v->d[i];
}

void cpl_vector_multiply(cpl_vector * v1, const cpl_vector * v2)
{
    long i;
    assert(v1->n == v2->n);
    for (i = 0; i < v1->n; i++) {
        v1->d[i] *= v2->d[i];
    }
}

void cpl_vector_power(cpl_vector * v, double power)
{
    long i;
    for (i = 0; i < v->n; i++) {
        v->d[i] = pow(v->d[i], power);
    }
}

/*-----------------------------------------------------------------------------
                        Private function prototypes
 -----------------------------------------------------------------------------*/

cpl_error_code cpl_matrix_product(cpl_matrix * self,
                                  const cpl_matrix * ma,
                                  const cpl_matrix * mb);
cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self);
cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self,
                                    cpl_matrix *rhs);

inline
static void swap_rows(cpl_matrix *, cpl_size, cpl_size) CPL_ATTR_NONNULL;
static void read_row(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL;
static void write_row(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL;
static void write_read_row(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL;
static
void read_column(const cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL;
static
void write_column(cpl_matrix *, const double *, cpl_size) CPL_ATTR_NONNULL;
static
void write_read_column(cpl_matrix *, double *, cpl_size) CPL_ATTR_NONNULL;
static cpl_error_code cpl_matrix_set_size_(cpl_matrix *, cpl_size, cpl_size);

/*-----------------------------------------------------------------------------
                              Function codes
 -----------------------------------------------------------------------------*/

/*
 * Private methods:
 */

/*----------------------------------------------------------------------------*/
/**
   @internal
   @brief   Set the size of a matrix while destroying its elements
   @param   self  The matrix to modify
   @param   nr    New number of matrix rows.
   @param   nc    New number of matrix columns.
   @note Any pointer returned by cpl_matrix_get_data_const() may be invalidated

*/
/*----------------------------------------------------------------------------*/
static
cpl_error_code cpl_matrix_set_size_(cpl_matrix * self, cpl_size nr, cpl_size nc)
{

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);

    if (self->nr != nr || self->nc != nc) {

        cpl_ensure_code(nr > 0, CPL_ERROR_ILLEGAL_INPUT);
        cpl_ensure_code(nc > 0, CPL_ERROR_ILLEGAL_INPUT);

        /* Need to resize the matrix */

        if (self->nr * self->nc != nr * nc) { 
            cpl_free(self->m);
            self->m = (double*)cpl_malloc((size_t)nr * (size_t)nc
                                          * sizeof(double));
        }
        self->nr = nr;
        self->nc = nc;
    }

    return CPL_ERROR_NONE;
}


/*----------------------------------------------------------------------------*/
/**
   @internal
   @brief   Swap two (different) rows 
   @param   self  The matrix
   @param   row1  One row
   @param   row2  Another row
   @see cpl_matrix_swap_rows()
   @note No error checking done here

   Optimized due to repeated usage in cpl_matrix_decomp_lu():
       1) Removed redundant error checking
          - this allows the compiler to make 'swap' a register variable.
       2) Use a single index for loop control and adressing the two rows
          - this redues the instruction count.

   This can have an effect when the swap is not memory bound.

*/
/*----------------------------------------------------------------------------*/
inline
static void swap_rows(cpl_matrix * self, cpl_size row1, cpl_size row2)
{                                                                
    double   swap;                                          
    cpl_size ncol = self->nc;                               
    double * pos1 = self->m + ncol * row1;                  
    double * pos2 = self->m + ncol * row2;                  
                                                                
    while (ncol--) {                                        
        swap = pos1[ncol];                                  
        pos1[ncol] = pos2[ncol];                            
        pos2[ncol] = swap;                                  
    }                                                       
}


/*
 * @internal
 * @brief
 *   Just read a matrix row into the passed buffer.
 *
 * @param matrix  Matrix where to read the row from.
 * @param pos     Row number.
 * @param row     Allocated buffer.
 *
 * @return Nothing.
 *
 * This private function reads the content of a matrix row into an allocated
 * buffer. No checks are performed.
 */
static void read_row(const cpl_matrix *matrix, double *row, cpl_size pos)
{

    cpl_size i;

    for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++)
        row[i] = matrix->m[pos];

}

/*
 * @internal
 * @brief
 *   Just write into a matrix row the values in buffer.
 *
 * @param matrix  Matrix where to write the buffer to.
 * @param pos     Row number.
 * @param row     Allocated buffer.
 * 
 * @return Nothing.
 * 
 * This private function writes the content of a buffer into a matrix row.
 * No checks are performed.
 */
static void write_row(cpl_matrix *matrix, const double *row, cpl_size pos)
{
    
    cpl_size i;

    for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++)
        matrix->m[pos] = *row++;

}

/*
 * @internal
 * @brief
 *   Just swap values in buffer with values in a matrix row.
 *
 * @param matrix  Matrix to access.
 * @param pos     Row to access.
 * @param row     Allocated buffer.
 *
 * @return Nothing.
 *
 * This private function exchanges the values in buffer with the values in
 * a chosen matrix row. No checks are performed.
 */
static void write_read_row(cpl_matrix *matrix, double *row, cpl_size pos)
{

    cpl_size    i;
    double swap;

    for (i = 0, pos *= matrix->nc; i < matrix->nc; i++, pos++) {
        swap = matrix->m[pos];
        matrix->m[pos] = row[i];
        row[i] = swap;
    }

}

/*
 * @internal
 * @brief
 *   Just read a matrix column into the passed buffer.
 *
 * @param matrix  Matrix where to read the column from.
 * @param pos     Column number.
 * @param column  Allocated buffer.
 *
 * @return Nothing.
 *
 * This private function reads the content of a matrix column into an 
 * allocated buffer. No checks are performed.
 */
static void read_column(const cpl_matrix *matrix, double *column, cpl_size pos)
{

    cpl_size i;

    for (i = 0; i < matrix->nr; i++, pos += matrix->nc)
        column[i] = matrix->m[pos];

}

/*
 * @internal
 * @brief
 *   Just write into a matrix column the values in buffer.
 *
 * @param matrix  Matrix where to write the buffer to.
 * @param pos     Column number.
 * @param column  Allocated buffer.
 * 
 * @return Nothing.
 * 
 * This private function writes the content of a buffer into a matrix column.
 * No checks are performed.
 */
static void write_column(cpl_matrix *matrix, const double *column, cpl_size pos)
{

    cpl_size i;
    cpl_size size = matrix->nr * matrix->nc;


    for (i = pos; i < size; i += matrix->nc)
        matrix->m[i] = *column++;

}

/*
 * @internal
 * @brief
 *   Just swap values in buffer with values in a matrix column.
 *
 * @param matrix  Matrix to access.
 * @param pos     Column to access.
 * @param column  Allocated buffer.
 *
 * @return Nothing.
 *
 * This private function exchanges the values in buffer with the values in
 * a chosen matrix column. No checks are performed.
 */
static void write_read_column(cpl_matrix *matrix, double *column, cpl_size pos)
{
 
    cpl_size    i;
    double swap;

    for (i = 0; i < matrix->nr; i++, pos += matrix->nc) {
        swap = matrix->m[pos];
        matrix->m[pos] = column[i];
        column[i] = swap;
    }

}

/**
 * @brief
 *   Create a zero matrix of given size.
 *
 * @param rows     Number of matrix rows.
 * @param columns  Number of matrix columns.
 *
 * @return Pointer to new matrix, or @c NULL in case of error.
 * 
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>rows</i> or <i>columns</i> are not positive numbers.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * This function allocates and initialises to zero a matrix of given size.
 * To destroy this matrix the function @c cpl_matrix_delete() should be used.
 */

cpl_matrix *cpl_matrix_new(cpl_size rows, cpl_size columns)
{


    cpl_matrix  *matrix;


    if (rows < 1) {
        (void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT);
        return NULL;
    }

    if (columns < 1) {
        (void)cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT);
        return NULL;
    }

    matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix));

    matrix->m = (double *)cpl_calloc((size_t)rows * (size_t)columns,
                                     sizeof(double));
    matrix->nr = rows;
    matrix->nc = columns;

    return matrix;

}


/**
 * @brief
 *   Create a new matrix from existing data.
 *
 * @param data     Existing data buffer.
 * @param rows     Number of matrix rows.
 * @param columns  Number of matrix columns.
 *
 * @return Pointer to new matrix, or @c NULL in case of error.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>data</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>rows</i> or <i>columns</i> are not positive numbers.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * This function creates a new matrix that will encapsulate the given 
 * data. At any error condition, a @c NULL pointer would be returned.
 * Note that the size of the input data array is not checked in any way, 
 * and it is expected to match the specified matrix sizes. The input 
 * array is supposed to contain in sequence all the new @em cpl_matrix 
 * rows. For instance, in the case of a 3x4 matrix, the input array 
 * should contain 12 elements 
 * @code
 *            0 1 2 3 4 5 6 7 8 9 10 11
 * @endcode
 * that would correspond to the matrix elements
 * @code
 *            0  1  2  3
 *            4  5  6  7
 *            8  9 10 11 
 * @endcode 
 * The data buffer is not copied, so it should not be deallocated while 
 * the matrix is still in use: the function @c cpl_matrix_delete() would 
 * take care of deallocating it. To avoid problems with the memory managment, 
 * the specified data buffer should be allocated using the functions of 
 * the @c cpl_memory module, and also statically allocated data should be 
 * strictly avoided. If this were not the case, this matrix should not be 
 * destroyed using @c cpl_matrix_delete(), but @c cpl_matrix_unwrap() 
 * should be used instead; moreover, functions implying memory handling 
 * (as @c cpl_matrix_set_size(), or @c cpl_matrix_delete_row() ) should not 
 * be used.
 */

cpl_matrix *cpl_matrix_wrap(cpl_size rows, cpl_size columns, double *data)
{


    cpl_matrix  *matrix;


    if (rows < 1 || columns < 1) {
        cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT);
        return NULL;
    }

    if (data == NULL) {
        cpl_error_set_(CPL_ERROR_NULL_INPUT);
        return NULL;
    }

    matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix));

    matrix->m = data;
    matrix->nr = rows;
    matrix->nc = columns;

    return matrix;

}


/**
 * @brief
 *   Delete a matrix.
 *
 * @param matrix  Pointer to a matrix to be deleted.
 *
 * @return Nothing.
 *
 * This function frees all the memory associated to a matrix. 
 * If @em matrix is @c NULL, nothing is done.
 */

void cpl_matrix_delete(cpl_matrix *matrix)
{

    if (matrix) {
        cpl_free(matrix->m);
        cpl_free(matrix);
    }

}


/**
 * @brief
 *   Delete a matrix, but not its data buffer.
 *
 * @param matrix  Pointer to a matrix to be deleted.
 *
 * @return Pointer to the internal data buffer.
 *
 * This function deallocates all the memory associated to a matrix, 
 * with the exception of its data buffer. This type of destructor
 * should be used on matrices created with the @c cpl_matrix_wrap() 
 * constructor, if the data buffer specified then was not allocated 
 * using the functions of the @c cpl_memory module. In such a case, the 
 * data buffer should be deallocated separately. See the documentation
 * of the function @c cpl_matrix_wrap(). If @em matrix is @c NULL, 
 * nothing is done, and a @c NULL pointer is returned.
 */

void *cpl_matrix_unwrap(cpl_matrix *matrix)
{

    void *data = NULL;

    if (matrix) {
        data = matrix->m;
        cpl_free(matrix);
    }

    return data;

}


/**
 * @brief
 *   Get the number of rows of a matrix.
 *
 * @param matrix  Pointer to the matrix to examine.
 *
 * @return Number of matrix rows, or zero in case of failure.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Determine the number of rows in a matrix.
 */

inline
cpl_size cpl_matrix_get_nrow(const cpl_matrix *matrix)
{

    if (matrix == NULL) {
        /* inline precludes using cpl_error_set_() due to its use of __func__ */
        (void) cpl_error_set("cpl_matrix_get_nrow", CPL_ERROR_NULL_INPUT);
        return 0;
    }

    return matrix->nr;

}


/**
 * @brief
 *   Get the number of columns of a matrix.
 *
 * @param matrix  Pointer to the matrix to examine.
 *
 * @return Number of matrix columns, or zero in case of failure.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Determine the number of columns in a matrix.
 */

inline
cpl_size cpl_matrix_get_ncol(const cpl_matrix *matrix)
{

    if (matrix == NULL) {
        /* inline precludes using cpl_error_set_() due to its use of __func__ */
        (void) cpl_error_set("cpl_matrix_get_ncol", CPL_ERROR_NULL_INPUT);
        return 0;
    }

    return matrix->nc;

}


/**
 * @brief
 *   Get the pointer to a matrix data buffer, or @c NULL in case of error.
 *
 * @param matrix  Input matrix.
 *
 * @return Pointer to the matrix data buffer.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * A @em cpl_matrix object includes an array of values of type @em double.
 * This function returns a pointer to this internal array, whose first 
 * element corresponds to the @em cpl_matrix element 0,0. The internal
 * array contains in sequence all the @em cpl_matrix rows. For instance,
 * in the case of a 3x4 matrix, the array elements
 * @code
 *            0 1 2 3 4 5 6 7 8 9 10 11
 * @endcode
 * would correspond to the following matrix elements:
 * @code
 *            0  1  2  3
 *            4  5  6  7
 *            8  9 10 11
 * @endcode
 *
 * @note
 *   Use at your own risk: direct manipulation of matrix data rules out 
 *   any check performed by the matrix object interface, and may introduce  
 *   inconsistencies between the information maintained internally, and 
 *   the actual matrix data and structure.
 */

inline
double *cpl_matrix_get_data(cpl_matrix *matrix)
{

    if (matrix == NULL) {
        /* inline precludes using cpl_error_set_() due to its use of __func__ */
        (void) cpl_error_set("cpl_matrix_get_data", CPL_ERROR_NULL_INPUT);
        return NULL;
    }

    return matrix->m;

}


/**
 * @brief
 *   Get the pointer to a matrix data buffer, or @c NULL in case of error.
 *
 * @param matrix  Input matrix.
 *
 * @return Pointer to the matrix data buffer.
 *
 * @see cpl_matrix_get_data
 */

inline
const double *cpl_matrix_get_data_const(const cpl_matrix *matrix)
{
    if (matrix == NULL) {
        /* inline precludes using cpl_error_set_() due to its use of __func__ */
        (void) cpl_error_set("cpl_matrix_get_data_const", CPL_ERROR_NULL_INPUT);
        return NULL;
    }

    return matrix->m;
}


/**
 * @brief
 *   Get the value of a matrix element.
 *
 * @param matrix  Pointer to a matrix.
 * @param row     Matrix element row.
 * @param column  Matrix element column.
 *
 * @return Value of the specified matrix element, or 0.0 in case of error.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The accessed element is beyond the <i>matrix</i> boundaries.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Get the value of a matrix element. The matrix rows and columns are 
 * counted from 0,0.
 */

double cpl_matrix_get(const cpl_matrix *matrix, cpl_size row, cpl_size column)
{




    if (matrix == NULL) {
        (void)cpl_error_set_(CPL_ERROR_NULL_INPUT);
        return 0.0;
    } 

    if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc) {
        (void)cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);
        return 0.0;
    }

    return matrix->m[column + row * matrix->nc];

}


/**
 * @brief
 *   Write a value to a matrix element.
 *
 * @param matrix  Input matrix.
 * @param row     Matrix element row.
 * @param column  Matrix element column.
 * @param value   Value to write.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The accessed element is beyond the <i>matrix</i> boundaries.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Write a value to a matrix element. The matrix rows and columns are 
 * counted from 0,0.
 */

inline
cpl_error_code cpl_matrix_set(cpl_matrix *matrix, 
                              cpl_size row, cpl_size column, double value)
{




    if (matrix == NULL)
        return cpl_error_set("cpl_matrix_set", CPL_ERROR_NULL_INPUT);

    if (row < 0 || row >= matrix->nr || column < 0 || column >= matrix->nc)
        return cpl_error_set("cpl_matrix_set", CPL_ERROR_ACCESS_OUT_OF_RANGE);

    matrix->m[column + row * matrix->nc] = value;

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Make a copy of a matrix.
 *
 * @param matrix  Matrix to be duplicated.
 *
 * @return Pointer to the new matrix, or @c NULL in case of error.
 * 
 * @error
 *   <table class="ec" align="center">
 *     <tr> 
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *   </table>
 * @enderror
 *
 * A copy of the input matrix is created. To destroy the duplicated matrix 
 * the function @c cpl_matrix_delete() should be used.
 */

cpl_matrix *cpl_matrix_duplicate(const cpl_matrix *matrix)
{


    cpl_matrix  *new_matrix = NULL;

    if (matrix) {
        const cpl_size size = matrix->nr * matrix->nc;

        new_matrix = (cpl_matrix *)cpl_malloc(sizeof(cpl_matrix));
        new_matrix->nr = matrix->nr;
        new_matrix->nc = matrix->nc;
        new_matrix->m = (double *)cpl_malloc((size_t)size * sizeof(double));
        memcpy(new_matrix->m, matrix->m, (size_t)size * sizeof(double));
    }
    else
        (void)cpl_error_set_(CPL_ERROR_NULL_INPUT);

    return new_matrix;

}

/**
 * @brief
 *   Write the same value to all matrix elements.
 *
 * @param matrix   Pointer to the matrix to access
 * @param value    Value to write
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *   </table>
 * @enderror
 *
 * Write the same value to all matrix elements.
 */

cpl_error_code cpl_matrix_fill(cpl_matrix *matrix, double value)
{


    cpl_size     size;
    double      *m;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    size = matrix->nr * matrix->nc;
    m = matrix->m;

    while (size--)
        *m++ = value;

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Write the same value to a matrix row.
 *
 * @param matrix Matrix to access
 * @param value  Value to write
 * @param row    Sequence number of row to overwrite.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The specified <i>row</i> is outside the <i>matrix</i> boundaries.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Write the same value to a matrix row. Rows are counted starting from 0. 
 */

cpl_error_code cpl_matrix_fill_row(cpl_matrix *matrix, double value,
                                   cpl_size row)
{


    double      *m;
    cpl_size     count;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    if (row < 0 || row >= matrix->nr)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);

    m = matrix->m + row * matrix->nc;
    count = matrix->nc;

    while (count--)
        *m++ = value;

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Write the same value to a matrix column.
 *
 * @param matrix Pointer to the matrix to access
 * @param value  Value to write
 * @param column Sequence number of column to overwrite
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The specified <i>column</i> is outside the <i>matrix</i> boundaries.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Write the same value to a matrix column. Columns are counted starting 
 * from 0.
 */

cpl_error_code cpl_matrix_fill_column(cpl_matrix *matrix, 
                                      double value, cpl_size column)
{


    cpl_size     count;
    double      *m;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    if (column < 0 || column >= matrix->nc)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);

    count = matrix->nr;
    m = matrix->m + column;

    while (count--) {
        *m = value;
        m += matrix->nc;
    }

    return CPL_ERROR_NONE;

}



/**
 * @brief
 *   Write the values of a matrix into another matrix.
 *
 * @param matrix    Pointer to matrix to be modified.
 * @param submatrix Pointer to matrix to get the values from.
 * @param row       Position of row 0 of @em submatrix in @em matrix.
 * @param col       Position of column 0 of @em submatrix in @em matrix.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> or <i>submatrix</i> are <tt>NULL</tt> 
 *         pointers.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         No overlap exists between the two matrices.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The values of @em submatrix are written to @em matrix starting at the 
 * indicated row and column. There are no restrictions on the sizes of 
 * @em submatrix: just the parts of @em submatrix overlapping @em matrix 
 * are copied. There are no restrictions on @em row and @em col either, 
 * that can also be negative. If the two matrices do not overlap, nothing 
 * is done, but an error condition is set.
 */

cpl_error_code cpl_matrix_copy(cpl_matrix *matrix, const cpl_matrix *submatrix,
                               cpl_size row, cpl_size col)
{


    cpl_size     endrow;
    cpl_size     endcol;
    cpl_size     subrow;
    cpl_size     subcol;
    cpl_size     r, c, sr, sc;
    double      *m;
    double      *sm;


    if (matrix == NULL || submatrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);


    endrow = row + submatrix->nr;
    endcol = col + submatrix->nc;


   /*
    * Check whether matrices overlap.
    */

    if (row >= matrix->nr || endrow < 1 || col >= matrix->nc || endcol < 1)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);


   /*
    * Define the overlap region on both matrices reference system:
    */

    if (row < 0) {
        subrow = -row;
        row = 0;
    }
    else
        subrow = 0;

    if (col < 0) {
        subcol = -col;
        col = 0;
    }
    else
        subcol = 0;

    if (endrow > matrix->nr)
        endrow = matrix->nr;

    if (endcol > matrix->nc)
        endcol = matrix->nc;

    for (r = row, sr = subrow; r < endrow; r++, sr++) {
        m = matrix->m + r * matrix->nc + col;
        sm = submatrix->m + sr * submatrix->nc + subcol;
        for (c = col, sc = subcol; c < endcol; c++, sc++)
            *m++ = *sm++;
    }

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Write the same value into a submatrix of a matrix.
 *
 * @param matrix    Pointer to matrix to be modified.
 * @param value     Value to write.
 * @param row       Start row of matrix submatrix.
 * @param col       Start column of matrix submatrix.
 * @param nrow      Number of rows of matrix submatrix.
 * @param ncol      Number of columns of matrix submatrix.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The specified start position is outside the <i>matrix</i> 
 *         boundaries.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>nrow</i> or <i>ncol</i> are not positive.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The specified value is written to @em matrix starting at the indicated 
 * row and column; @em nrow and @em ncol can exceed the input matrix 
 * boundaries, just the range overlapping the @em matrix is used in that
 * case.
 */

cpl_error_code cpl_matrix_fill_window(cpl_matrix *matrix, double value,
                                      cpl_size row, cpl_size col,
                                      cpl_size nrow, cpl_size ncol)
{


    cpl_size     endrow;
    cpl_size     endcol;
    cpl_size     r, c;
    double      *m;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    if (row < 0 || row >= matrix->nr || col < 0 || col >= matrix->nc)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);

    if (nrow < 1 || ncol < 1)
        return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT);

    endrow = row + nrow;

    if (endrow > matrix->nr)
        endrow = matrix->nr;

    endcol = col + ncol;

    if (endcol > matrix->nc)
        endcol = matrix->nc;

    for (r = row; r < endrow; r++) {
        m = matrix->m + r * matrix->nc + col;
        for (c = col; c < endcol; c++)
            *m++ = value;
    }

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Swap two matrix columns.
 *
 * @param matrix    Pointer to matrix to be modified.
 * @param column1   One matrix column.
 * @param column2   Another matrix column.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         Any of the specified columns is outside the <i>matrix</i> 
 *         boundaries.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The values of two given matrix columns are exchanged. Columns are 
 * counted starting from 0. If the same column number is given twice, 
 * nothing is done and no error is set.
 */

cpl_error_code cpl_matrix_swap_columns(cpl_matrix *matrix, 
                                       cpl_size column1, cpl_size column2)
{


    double       swap;
    cpl_size     nrow;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    if (column1 < 0 || column1 >= matrix->nc || 
        column2 < 0 || column2 >= matrix->nc)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);


    if (column1 == column2)
        return CPL_ERROR_NONE;
   
    nrow = matrix->nr;

    while (nrow--) {
        swap = matrix->m[column1];
        matrix->m[column1] = matrix->m[column2];
        matrix->m[column2] = swap;
        column1 += matrix->nc;
        column2 += matrix->nc;
    }

    return CPL_ERROR_NONE;
 
}


/**
 * @brief
 *   Swap a matrix column with a matrix row.
 *
 * @param matrix    Pointer to matrix to be modified.
 * @param row       Matrix row.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ACCESS_OUT_OF_RANGE</td>
 *       <td class="ecr">
 *         The specified <i>row</i> is outside the <i>matrix</i> boundaries.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is not square.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The values of the indicated row are exchanged with the column having 
 * the same sequence number. Rows and columns are counted starting from 0. 
 */

cpl_error_code cpl_matrix_swap_rowcolumn(cpl_matrix *matrix, cpl_size row)
{


    double       swap;
    cpl_size     i;
    cpl_size     posr, posc;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);

    if (matrix->nr != matrix->nc)
        return cpl_error_set_(CPL_ERROR_ILLEGAL_INPUT);

    if (row < 0 || row >= matrix->nr)
        return cpl_error_set_(CPL_ERROR_ACCESS_OUT_OF_RANGE);


    posr = row;
    posc = row * matrix->nc;

    for (i = 0; i < matrix->nr; i++) {
        swap = matrix->m[posr];
        matrix->m[posr] = matrix->m[posc];
        matrix->m[posc] = swap;
        posr += matrix->nc;
        posc++;
    }

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Reverse order of rows in matrix.
 *
 * @param matrix    Pointer to matrix to be reversed.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The order of the rows in the matrix is reversed in place.
 */

cpl_error_code cpl_matrix_flip_rows(cpl_matrix *matrix)
{


    cpl_size     i, j;

 
    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);


    for (i = 0, j = matrix->nr - 1; i < j; i++, j--)
        swap_rows(matrix, i, j);

    return CPL_ERROR_NONE;
    
}


/**
 * @brief
 *   Reverse order of columns in matrix.
 *
 * @param matrix    Pointer to matrix to be reversed.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The order of the columns in the matrix is reversed in place.
 */

cpl_error_code cpl_matrix_flip_columns(cpl_matrix *matrix)
{


    cpl_size     i, j;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);


    for (i = 0, j = matrix->nc - 1; i < j; i++, j--) 
        cpl_matrix_swap_columns(matrix, i, j);

    return CPL_ERROR_NONE;
 
}


/**
 * @brief
 *   Create transposed matrix.
 *
 * @param matrix    Pointer to matrix to be transposed.
 *
 * @return Pointer to transposed matrix. If a @c NULL pointer is passed,
 *   a @c NULL pointer is returned.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The transposed of the input matrix is created. To destroy the new matrix 
 * the function @c cpl_matrix_delete() should be used.
 */

cpl_matrix *cpl_matrix_transpose_create(const cpl_matrix *matrix)
{


    cpl_matrix  *transposed = NULL;
    cpl_size     i, j;
    double      *m;
    double      *tm;


    if (matrix == NULL) {
        cpl_error_set_(CPL_ERROR_NULL_INPUT);
        return NULL;
    }


    tm = (double *)cpl_malloc((size_t)matrix->nc * (size_t)matrix->nr
                              * sizeof(*tm));
    transposed = cpl_matrix_wrap(matrix->nc, matrix->nr, tm);
    m = matrix->m;

    for (i = 0; i < matrix->nr; i++)
        for (j = 0, tm = transposed->m + i; 
             j < matrix->nc; j++, tm += matrix->nr)
            *tm = *m++;

    return transposed;

}


/**
 * @brief
 *   Reframe a matrix.
 *
 * @param matrix    Pointer to matrix to be modified.
 * @param top       Extra rows on top.
 * @param bottom    Extra rows on bottom.
 * @param left      Extra columns on left.
 * @param right     Extra columns on right.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_OUTPUT</td>
 *       <td class="ecr">
 *         Attempt to shrink <i>matrix</i> to zero size (or less).
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The input matrix is reframed according to specifications. Extra rows 
 * and column on the sides might also be negative, as long as they are 
 * compatible with the matrix sizes: the input matrix would be reduced 
 * in size accordingly, but an attempt to remove all matrix columns 
 * and/or rows is flagged as an error because zero length matrices are 
 * illegal. The old matrix elements contained in the new shape are left 
 * unchanged, and new matrix elements added by the reshaping are initialised 
 * to zero. No reshaping (i.e., all the extra rows set to zero) would
 * not be flagged as an error.
 *
 * @note
 *   The pointer to the matrix data buffer may change, therefore 
 *   pointers previously retrieved by calling @c cpl_matrix_get_data() 
 *   should be discarded.
 */

cpl_error_code cpl_matrix_resize(cpl_matrix *matrix, 
                                 cpl_size top, cpl_size bottom,
                                 cpl_size left, cpl_size right)
{


    cpl_matrix  *resized;
    cpl_size     nr, nc;


    if (matrix == NULL)
        return cpl_error_set_(CPL_ERROR_NULL_INPUT);


    if (top == 0 && bottom == 0 && left == 0 && right == 0)
        return CPL_ERROR_NONE;

    nr = matrix->nr + top + bottom;
    nc = matrix->nc + left + right;

    if (nr < 1 || nc < 1)
        return cpl_error_set_(CPL_ERROR_ILLEGAL_OUTPUT);

    resized = cpl_matrix_new(nr, nc);

    cpl_matrix_copy(resized, matrix, top, left);

    cpl_free(matrix->m);
    matrix->m = cpl_matrix_unwrap(resized);
    matrix->nr = nr;
    matrix->nc = nc;

    return CPL_ERROR_NONE;

}


/**
 * @brief
 *   Resize a matrix.
 *
 * @param matrix    Pointer to matrix to be resized.
 * @param rows      New number of rows.
 * @param columns   New number of columns.
 *
 * @return @c CPL_ERROR_NONE on success.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         The input <i>matrix</i> is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_OUTPUT</td>
 *       <td class="ecr">
 *         Attempt to shrink <i>matrix</i> to zero size (or less).
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The input matrix is resized according to specifications. The old
 * matrix elements contained in the resized matrix are left unchanged,
 * and new matrix elements added by an increase of the matrix number of
 * rows and/or columns are initialised to zero.
 *
 * @note
 *   The pointer to the matrix data buffer may change, therefore 
 *   pointers previously retrieved by calling @c cpl_matrix_get_data() 
 *   should be discarded.
 */

cpl_error_code cpl_matrix_set_size(cpl_matrix *matrix, cpl_size rows,
                                   cpl_size columns)
{

    return cpl_matrix_resize(matrix, 0, rows - matrix->nr, 
                             0, columns - matrix->nc)
        ? cpl_error_set_where_() : CPL_ERROR_NONE;
   
}


/**
 * @brief
 *   Rows-by-columns product of two matrices.
 *
 * @param matrix1   Pointer to left side matrix.
 * @param matrix2   Pointer to right side matrix.
 *
 * @return Pointer to product matrix, or @c NULL in case of error.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr> 
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         An input matrix is a <tt>NULL</tt> pointer.
 *       </td> 
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         The number of columns of the first matrix is not equal to 
 *         the number of rows of the second matrix.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Rows-by-columns product of two matrices. The number of columns of the 
 * first matrix must be equal to the number of rows of the second matrix.
 * To destroy the new matrix the function @c cpl_matrix_delete() should 
 * be used.
 */

cpl_matrix *cpl_matrix_product_create(const cpl_matrix *matrix1, 
                                      const cpl_matrix *matrix2)
{

    cpl_matrix  *self;

    cpl_ensure(matrix1 != NULL,            CPL_ERROR_NULL_INPUT,         NULL);
    cpl_ensure(matrix2 != NULL,            CPL_ERROR_NULL_INPUT,         NULL);
    cpl_ensure(matrix1->nc == matrix2->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

    /* Create data-buffer without overhead of initializing */
    self = cpl_matrix_wrap(matrix1->nr, matrix2->nc,
                           cpl_calloc((size_t)matrix1->nr * (size_t)matrix2->nc
                                      , sizeof(*self)));

    (void) cpl_matrix_product(self, matrix1, matrix2);

    return self;

}


/*
 * @brief Replace a matrix by its LU-decomposition
 * @param self  n X n non-singular matrix to decompose
 * @param perm  n-integer array to be filled with the row permutations
 * @param psig  On success set to 1/-1 for an even/odd number of permutations
 * @return CPL_ERROR_NONE on success, or the relevant CPL error code
 * @note If the matrix is singular the elements of self become undefined
 * @see Golub & Van Loan, Matrix Computations, Algorithms 3.2.1 (Outer Product
 *  Gaussian Elimination) and 3.4.1 (Gauss Elimination with Partial Pivoting).
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         An input pointer is <tt>NULL</tt>.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>self</i> is not an n by n matrix.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         <i>self</i> and <i>perm</i> have incompatible sizes.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_TYPE_MISMATCH</td>
 *       <td class="ecr">
 *         <i>perm</i> is not of type CPL_TYPE_INT.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td>
 *       <td class="ecr">
 *         <i>self</i> is singular.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 */
cpl_error_code cpl_matrix_decomp_lu(cpl_matrix * self, int * iperm,
                                    int * psig)
{

    const cpl_size n     = cpl_matrix_get_ncol(self);
    int            nn;
    cpl_size       i, j;
    const double * aread;
    double       * awrite;

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(iperm != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(psig != NULL, CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT);

    nn = (int)n;

    cpl_ensure_code((cpl_size)nn == n, CPL_ERROR_ILLEGAL_INPUT);

    aread = awrite = self->m;

    /* No row permutations yet */
    *psig = 1;
    for (i = 0; i < n; i++) {
        iperm[i] = i;
    }

    for (j = 0; j < n - 1; j++) {
        /* Find maximum in the j-th column */

        double pivot = fabs(aread[j + j * n]);
        cpl_size ipivot = j;

        for (i = j + 1; i < n; i++) {
           const double aij = fabs(aread[j + i * n]);

           if (aij > pivot) {
               pivot = aij;
               ipivot = i;
           }
        }

        if (pivot <= 0.0) {
            return CPL_ERROR_SINGULAR_MATRIX;
        }

        if (ipivot > j) {
            /* The maximum is below the diagonal - swap rows */
            const int iswap = iperm[j];

            iperm[j] = iperm[ipivot];
            iperm[ipivot] = iswap;

            *psig = - (*psig);

            swap_rows(self, j, ipivot);
        }

        pivot = aread[j + j * n];

        for (i = j + 1; i < n; i++) {
            const double aij = aread[j + i * n] / pivot;
            cpl_size k;

            awrite[j + i * n] = aij;

            for (k = j + 1; k < n; k++) {
                const double ajk = aread[k + j * n];
                const double aik = aread[k + i * n];
                awrite[k + i * n] = aik - aij * ajk;
            }
        }
    }

    /* Check if A(n,n) is non-zero */
    return fabs(aread[n * (n-1) + (n-1)]) > 0.0 ? CPL_ERROR_NONE
        : CPL_ERROR_SINGULAR_MATRIX;

}

/*
 * @brief Solve a LU-system
 * @param self  n X n LU-matrix from cpl_matrix_decomp_lu()
 * @param rhs   m right-hand-sides to be replaced by their solution
 * @param perm  n-integer array filled with the row permutations, or NULL
 * @return CPL_ERROR_NONE on success, or the relevant CPL error code
 * @see cpl_matrix_decomp_lu()
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         An input pointer is <tt>NULL</tt>.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>self</i> is not an n by n matrix.
 *       </td>
 *     </tr>
 *     <tr> 
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         The array or matrices not have the same number of rows.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_TYPE_MISMATCH</td>
 *       <td class="ecr">
 *         <i>perm</i> is non-NULL and not of type CPL_TYPE_INT.
 *       </td>
 *     </tr>
 *     <tr> 
 *       <td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td>
 *       <td class="ecr">
 *         The main diagonal of U contains a zero. This error can only occur
 *         if the LU-matrix does not come from a successful call to
 *         cpl_matrix_decomp_lu().
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 */
cpl_error_code cpl_matrix_solve_lu(const cpl_matrix * self,
                                   cpl_matrix * rhs, const int * iperm)
{

    cpl_size n, i, j, k;
    double * x = NULL; /* Avoid false uninit warning */
    const double * aread;
    const double * bread;
    double * bwrite;

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(rhs  != NULL, CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(self->nc == self->nr, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(rhs->nr == self->nr,  CPL_ERROR_INCOMPATIBLE_INPUT);

    n = self->nc;

    aread = self->m;
    bread = bwrite = rhs->m;

    if (iperm == NULL) {
        return CPL_ERROR_NULL_INPUT;
    }
    else {
        x = (double*) cpl_malloc((size_t)n * sizeof(*x));
    }

    for (k=0; k < rhs->nc; k++) {
        if (iperm != NULL) {
            /* Un-permute the rows in column k */
            for (i = 0; i < n; i++) {
                x[i] = bread[rhs->nc * i + k];
            }
            for (i = 0; i < n; i++) {
                bwrite[rhs->nc * i + k] = x[iperm[i]];
            }
        }

        /* Forward substitution in column k */
        for (i = 1; i < n; i++) {
            double tmp = bread[rhs->nc * i + k];
            for (j = 0; j < i; j++) {
                const double aij = aread[n * i + j];

                tmp -= aij * bread[rhs->nc * j + k];
            }
            bwrite[rhs->nc * i + k] = tmp;
        }

        /* Back substitution in column k */
        for (i = n - 1; i >= 0; i--) {
            double tmp = bread[rhs->nc * i + k];
            for (j = i + 1; j < n; j++) {
                const double aij = aread[n * i + j];

                tmp -= aij * bread[rhs->nc * j + k];
            }

            /* Check for a bug in the calling function */
            if (aread[n * i + i] == 0.0) break;

            bwrite[rhs->nc * i + k] = tmp/aread[n * i + i];

        }

        if (i >= 0) break;
    }


    return k == rhs->nc ? CPL_ERROR_NONE
        : cpl_error_set_(CPL_ERROR_DIVISION_BY_ZERO);

}


/*----------------------------------------------------------------------------*/
/**
   @brief Replace a matrix by its Cholesky-decomposition, L * transpose(L) = A
   @param self  N by N symmetric positive-definite matrix to decompose
   @return CPL_ERROR_NONE on success, or the relevant CPL error code
   @note Only the upper triangle of self is read, L is written in the lower
         triangle
   @note If the matrix is singular the elements of self become undefined
   @see Golub & Van Loan, Matrix Computations, Algorithm 4.2.1
        (Cholesky: Gaxpy Version).

   @error
     <table class="ec" align="center">
       <tr>
         <td class="ecl">CPL_ERROR_NULL_INPUT</td>
         <td class="ecr">
           An input pointer is <tt>NULL</tt>.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
         <td class="ecr">
           <i>self</i> is not an n by n matrix.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td>
         <td class="ecr">
           <i>self</i> is not symmetric, positive definite.
         </td>
       </tr>
     </table>
   @enderror
 */
/*----------------------------------------------------------------------------*/
cpl_error_code cpl_matrix_decomp_chol(cpl_matrix * self)
{

    const cpl_size n = cpl_matrix_get_ncol(self);
    double * awrite = cpl_matrix_get_data(self);
    const double * aread = awrite;
    double sub = 0.0; /* Fix (false) uninit warning */
    cpl_size i, j = 0; /* Fix (false) uninit warning */

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT);

    for (i = 0; i < n; i++) {
        for (j = i; j < n; j++) {
            cpl_size k;

            sub = aread[n * i + j];
            for (k = i-1; k >= 0; k--) {
                sub -= aread[n * i + k] * aread[n * j + k];
            }

            if (j > i) {
                awrite[n * j + i] = sub/aread[n * i + i];
            } else if (sub > 0.0) {
                awrite[n * i + i] = sqrt(sub);
            } else {
                break;
            }
        }
        if (j < n) break;
    }


    return i == n ? CPL_ERROR_NONE
        : CPL_ERROR_SINGULAR_MATRIX;
}



/*----------------------------------------------------------------------------*/
/**
   @brief Solve a L*transpose(L)-system
   @param self  N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol()
   @param rhs   M right-hand-sides to be replaced by their solution
   @return CPL_ERROR_NONE on success, or the relevant CPL error code
   @see cpl_matrix_decomp_chol()
   @note Only the lower triangle of self is accessed

   @error
     <table class="ec" align="center">
       <tr>
         <td class="ecl">CPL_ERROR_NULL_INPUT</td>
         <td class="ecr">
           An input pointer is <tt>NULL</tt>.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
         <td class="ecr">
           <i>self</i> is not an n by n matrix.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
         <td class="ecr">
           The specified matrices do not have the same number of rows.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td>
         <td class="ecr">
           The main diagonal of L contains a zero. This error can only occur
           if the L*transpose(L)-matrix does not come from a successful call to
           cpl_matrix_decomp_chol().
         </td>
       </tr>
     </table>
   @enderror
 */
/*----------------------------------------------------------------------------*/
cpl_error_code cpl_matrix_solve_chol(const cpl_matrix * self,
                                     cpl_matrix * rhs)
{

    const cpl_size n    = cpl_matrix_get_ncol(self);
    const cpl_size nrhs = cpl_matrix_get_ncol(rhs);
    cpl_size i, j, k;
    const double * aread;
    const double * bread;
    double * bwrite;

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(rhs  != NULL, CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(cpl_matrix_get_nrow(self) == n, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(cpl_matrix_get_nrow(rhs)  == n, CPL_ERROR_INCOMPATIBLE_INPUT);

    aread = cpl_matrix_get_data_const(self);
    bread = bwrite = cpl_matrix_get_data(rhs);

    for (k=0; k < nrhs; k++) {
        /* Forward substitution in column k */
        for (i = 0; i < n; i++) {
            double sub = bread[nrhs * i + k];
            for (j = i-1; j >= 0; j--) {
                sub -= aread[n * i + j] * bread[nrhs * j + k];
            }
            cpl_ensure_code(aread[n * i + i] != 0.0,
                            CPL_ERROR_DIVISION_BY_ZERO);
            bwrite[nrhs * i + k] = sub/aread[n * i + i];
        }

        /* Back substitution in column k */

        for (i = n-1; i >= 0; i--) {
            double sub = bread[nrhs * i + k];
            for (j = i+1; j < n; j++) {
                sub -= aread[n * j + i] * bread[nrhs * j + k];
            }
            bwrite[nrhs * i + k] = sub/aread[n * i + i];
        }
    }

    return CPL_ERROR_NONE;

}




/**
 * @brief
 *   Solution of a linear system.
 *
 * @param coeff   A non-singular N by N matrix.
 * @param rhs     A matrix containing one or more right-hand-sides.
 * @note rhs must have N rows and may contain more than one column,
 *       which each represent an independent right-hand-side.
 *
 * @return A newly allocated solution matrix with the size as rhs,
 *         or @c NULL on error.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         Any input is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>coeff</i> is not a square matrix.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         <i>coeff</i> and <i>rhs</i> do not have the same number of rows.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td>
 *       <td class="ecr">
 *         <i>coeff</i> is singular (to working precision).
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * Compute the solution of a system of N equations with N unknowns:
 *
 *   coeff * X = rhs
 *
 * @em coeff must be an NxN matrix, and @em rhs a NxM matrix.
 * M greater than 1 means that multiple independent right-hand-sides
 * are solved for.
 * To destroy the solution matrix the function @c cpl_matrix_delete()
 * should be used.
 * 
 */

cpl_matrix *cpl_matrix_solve(const cpl_matrix *coeff, const cpl_matrix *rhs)
{

    cpl_matrix * lu;
    cpl_matrix * x;
    int        * iperm;
    cpl_size     n;
    int          sig;
    cpl_error_code error;


    cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL);
    cpl_ensure(rhs   != NULL, CPL_ERROR_NULL_INPUT, NULL);

    n = coeff->nc;

    cpl_ensure(coeff->nr == n, CPL_ERROR_ILLEGAL_INPUT, NULL);
    cpl_ensure(rhs->nr   == n, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

    lu = cpl_matrix_duplicate(coeff);

    iperm = (int*) cpl_malloc((size_t)n * sizeof(*iperm));

    if (cpl_matrix_decomp_lu(lu, iperm, &sig)) {
        cpl_matrix_delete(lu);
        cpl_free(iperm);
        (void)cpl_error_set_where_();
        return NULL;
    }

    x = cpl_matrix_duplicate(rhs);

    /* should not be able to fail at this point */
    error = cpl_matrix_solve_lu(lu, x, iperm);

    cpl_matrix_delete(lu);
    cpl_free(iperm);

    if (error) {
        cpl_matrix_delete(x);
        x = NULL;
        (void)cpl_error_set_where_();
    }

    return x;
}


/**
 * @brief
 *   Solution of overdetermined linear equations in a least squares sense.
 *
 * @param coeff   The N by M matrix of coefficients, where N >= M.
 * @param rhs     An N by K matrix containing K right-hand-sides.
 * @note rhs may contain more than one column,
 *       which each represent an independent right-hand-side.
 *
 * @return A newly allocated M by K solution matrix,
 *         or @c NULL on error.
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         Any input is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         <i>coeff</i> and <i>rhs</i> do not have the same number of rows.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td>
 *       <td class="ecr">
 *         The matrix is (near) singular and a solution cannot be computed.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * The following linear system of N equations and M unknowns
 * is given:
 *
 * @code
 *   coeff * X = rhs
 * @endcode
 *
 * where @em coeff is the NxM matrix of the coefficients, @em X is the
 * MxK matrix of the unknowns, and @em rhs the NxK matrix containing 
 * the K right hand side(s).
 *
 * The solution to the normal equations is known to be a least-squares
 * solution, i.e. the 2-norm of coeff * X - rhs is minimized by the
 * solution to
 *    transpose(coeff) * coeff * X = transpose(coeff) * rhs.
 *
 * In the case that coeff is square (N is equal to M) it gives a faster
 * and more accurate result to use cpl_matrix_solve().
 *
 * The solution matrix should be deallocated with the function
 * @c cpl_matrix_delete(). 
 */

cpl_matrix *cpl_matrix_solve_normal(const cpl_matrix *coeff, 
                                    const cpl_matrix *rhs)
{

    cpl_matrix * solution;
    cpl_matrix * At;
    cpl_matrix * AtA;

    cpl_ensure(coeff != NULL, CPL_ERROR_NULL_INPUT, NULL);
    cpl_ensure(rhs   != NULL, CPL_ERROR_NULL_INPUT, NULL);
    cpl_ensure(rhs->nr == coeff->nr, CPL_ERROR_INCOMPATIBLE_INPUT, NULL);

    At  = cpl_matrix_transpose_create(coeff);
    solution = cpl_matrix_product_create(At, rhs);

    AtA = cpl_matrix_product_normal_create(At);

    cpl_matrix_delete(At);

    if (cpl_matrix_solve_spd(AtA, solution)) {
        cpl_matrix_delete(solution);
        solution = NULL;
        (void)cpl_error_set_where_();
    }

    cpl_matrix_delete(AtA);

    return solution;

}


/*----------------------------------------------------------------------------*/
/**
   @internal
   @brief
     Solution of Symmetric, Positive Definite system of linear equations.
 *
   @param self   The N by N Symmetric, Positive Definite matrix.
   @param rhs    An N by K matrix containing K right-hand-sides.
   @note rhs may contain more than one column,
         which each represent an independent right-hand-side. The solution
         is written in rhs. Only the upper triangular part of self is read,
         while the lower triangular part is modified.
   @return CPL_ERROR_NONE on success, or the relevant CPL error code
 *
   @error
     <table class="ec" align="center">
       <tr>
         <td class="ecl">CPL_ERROR_NULL_INPUT</td>
         <td class="ecr">
           <i>self</i> or <i>rhs</i> is a <tt>NULL</tt> pointer.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
         <td class="ecr">
           <i>self</i> and <i>rhs</i> do not have the same number of rows.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
         <td class="ecr">
           <i>self</i> is not an N by N matrix.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_SINGULAR_MATRIX</td>
         <td class="ecr">
           <i>self</i> is (near) singular and a solution cannot be computed.
         </td>
       </tr>
     </table>
   @enderror
 */
/*----------------------------------------------------------------------------*/

cpl_error_code cpl_matrix_solve_spd(cpl_matrix *self,
                                    cpl_matrix *rhs)
{

    cpl_ensure_code(!cpl_matrix_decomp_chol(self), cpl_error_get_code());

    cpl_ensure_code(!cpl_matrix_solve_chol(self, rhs), cpl_error_get_code());

    return CPL_ERROR_NONE;

}


/**
 * @internal
 * @brief Compute A = B * transpose(B)
 *
 * @param self     Pre-allocated M x M matrix to hold the result
 * @param other    M x N Matrix to multiply with its transpose
  @return   CPL_ERROR_NONE or the relevant CPL error code on error
 * @note Only the upper triangle of A is computed, while the elements
 *       below the main diagonal have undefined values.
 * @see cpl_matrix_product_create()
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         An input matrix is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *     <tr>
 *       <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
 *       <td class="ecr">
 *         <i>self</i> is not an M by M matrix.
 *       </td>
 *     </tr>
 *     <tr> 
 *       <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
 *       <td class="ecr">
 *         The two matrices have a different number of rows.
 *       </td>
 *     </tr>
 *     <tr> 
 *
 *   </table>
 * @enderror
 *
 */

cpl_error_code cpl_matrix_product_normal(cpl_matrix * self,
                                         const cpl_matrix * other)
{

    double         sum;
    const double * ai = cpl_matrix_get_data_const(other);
    const double * aj;
    double       * bwrite = cpl_matrix_get_data(self);
    const cpl_size m = cpl_matrix_get_nrow(self);
    const cpl_size n = cpl_matrix_get_ncol(other);
    cpl_size       i, j, k;


    cpl_ensure_code(self  != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(other != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(cpl_matrix_get_ncol(self)  == m, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(cpl_matrix_get_nrow(other) == m,
                    CPL_ERROR_INCOMPATIBLE_INPUT);

#ifdef CPL_MATRIX_PRODUCT_NORMAL_RESET
    /* Initialize all values to zero.
       This is done to avoid access of uninitilized memory in case
       someone passes the matrix to for example cpl_matrix_dump(). */
    (void)memset(bwrite, 0, m * n * sizeof(*bwrite));
#endif

    /* The result at (i,j) is the dot-product of i'th and j'th row */
    for (i = 0; i < m; i++, bwrite += m, ai += n) {
        aj = ai; /* aj points to first entry in j'th row */
        for (j = i; j < m; j++, aj += n) {
            sum = 0.0;
            for (k = 0; k < n; k++) {
                sum += ai[k] * aj[k];
            }
            bwrite[j] = sum;
        }
    }

    return CPL_ERROR_NONE;
}


/**
 * @internal
 * @brief Create and compute A = B * transpose(B)
 *
 * @param self     M x N Matrix
 * @return Pointer to created M x M product matrix, or @c NULL on error.
 * @note Only the upper triangle of A is computed, while the elements
 *       below the main diagonal have undefined values.
 * @see cpl_matrix_product_create()
 *
 * @error
 *   <table class="ec" align="center">
 *     <tr>
 *       <td class="ecl">CPL_ERROR_NULL_INPUT</td>
 *       <td class="ecr">
 *         An input matrix is a <tt>NULL</tt> pointer.
 *       </td>
 *     </tr>
 *   </table>
 * @enderror
 *
 * To destroy the new matrix the function @c cpl_matrix_delete() should
 * be used.
 */

cpl_matrix * cpl_matrix_product_normal_create(const cpl_matrix * self)
{

    const size_t m       = cpl_matrix_get_nrow(self);
    cpl_matrix * product = cpl_matrix_wrap((cpl_size)m, (cpl_size)m,
                                           cpl_malloc(m * m * sizeof(double)));

    if (cpl_matrix_product_normal(product, self)) {
        cpl_matrix_delete(product);
        product = NULL;
        (void)cpl_error_set_where_();
    }

    return product;
}

/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief    Fill a matrix with the product of A * B
  @param    self  The matrix to fill, is or else will be set to size M x N
  @param    ma    The matrix A, of size M x K
  @param    mb    The matrix B, of size K * N
  @return   CPL_ERROR_NONE or the relevant CPL error code on error
  @note If upon entry, self has a size m *n, which differs from its size on
        return, then the result of preceeding calls to cpl_matrix_get_data()
        are invalid.
  @see  cpl_matrix_product_create()

*/
/*----------------------------------------------------------------------------*/

cpl_error_code cpl_matrix_product(cpl_matrix * self,
                                  const cpl_matrix * ma,
                                  const cpl_matrix * mb)
{
    const size_t   bs = 48;
    double       * ds;
    const double * d1 = cpl_matrix_get_data_const(ma);
    const double * d2 = cpl_matrix_get_data_const(mb);

    const size_t   nr = cpl_matrix_get_nrow(ma);
    const size_t   nc = cpl_matrix_get_ncol(mb);
    const size_t   nk = cpl_matrix_get_nrow(mb);
    size_t         i, j, k, ib, jb, kb;

    cpl_ensure_code(ma   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(mb   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code((size_t)ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT);

    if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_();

    ds = cpl_matrix_get_data(self);

    /* simple blocking to reduce cache misses */
    for (i = 0; i < nr; i += bs) {
        for (j = 0; j < nc; j += bs) {
            for (k = 0; k < nk; k += bs) {
                for (ib = i; ib < CX_MIN(i + bs, nr); ib++) {
                    for (jb = j; jb < CX_MIN(j + bs, nc); jb++) {
                        double sum = 0.;
                        for (kb = k; kb < CX_MIN(k + bs, nk); kb++) {
                            sum += d1[ib * nk + kb] * d2[kb * nc + jb];
            }
                        ds[ib * nc + jb] += sum;
        }
    }
            }
        }
    }



    return CPL_ERROR_NONE;

}

/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief    Fill a matrix with the product of A * B'
  @param    self  The matrix to fill, is or else will be set to size M x N
  @param    ma    The matrix A, of size M x K
  @param    mb    The matrix B, of size N x K
  @return   CPL_ERROR_NONE or the relevant CPL error code on error
  @note     The use of the transpose of B causes a more efficient memory access
  @note     Changing the order of A and B is allowed, it transposes the result
  @see      cpl_matrix_product_create()

*/
/*----------------------------------------------------------------------------*/
cpl_error_code cpl_matrix_product_transpose(cpl_matrix * self,
                                            const cpl_matrix * ma,
                                            const cpl_matrix * mb)
{

    double         sum;

    double       * ds;
    const double * d1 = cpl_matrix_get_data_const(ma);
    const double * d2 = cpl_matrix_get_data_const(mb);
    const double * di;

    const cpl_size      nr = cpl_matrix_get_nrow(ma);
    const cpl_size      nc = cpl_matrix_get_nrow(mb);
    const cpl_size      nk = cpl_matrix_get_ncol(mb);
    cpl_size       i, j, k;


    cpl_ensure_code(ma   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(mb   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(ma->nc == nk, CPL_ERROR_INCOMPATIBLE_INPUT);

    if (cpl_matrix_set_size_(self, nr, nc)) return cpl_error_set_where_();

    ds = cpl_matrix_get_data(self);

    for (i = 0; i < nr; i++, d1 += nk) {
        /* Since ma and mb are addressed in the same manner,
           they can use the same index, k */

        di = d2; /* di points to first entry in i'th row */
        for (j = 0; j < nc; j++, di += nk) {
            sum = 0.0;
            for (k = 0; k < nk; k++) {
                sum += d1[k] * di[k];
            }
            ds[nc * i + j] = sum;
        }
    }

    return CPL_ERROR_NONE;

}

/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief   Fill a matrix with the product of B * A * B'
  @param   self  The matrix to fill, is or else will be set to size N x N
  @param   ma    The matrix A, of size M x M
  @param   mb    The matrix B, of size N x M
  @return  CPL_ERROR_NONE or the relevant CPL error code on error
  @note    Requires 2 * N * M * (N + M) FLOPs and M doubles of temporary storage
  @see     cpl_matrix_product_create()

  The intermediate result, C = A * B', is computed one column at a time,
  each column of C is then used once to compute one row of the final result.

  The performed sequence of floating point operations is exactly the same as
  in the combination of the two calls
    cpl_matrix_product_transpose(C, A, B);
    cpl_matrix_product_create(B, C);
  and the result is therefore also exactly identical.

  The difference lies in the reduced use of temporary storage and in the
  unit stride access, both of which lead to better performance (for large
  matrices).
  
*/
/*----------------------------------------------------------------------------*/
cpl_error_code cpl_matrix_product_bilinear(cpl_matrix * self,
                                           const cpl_matrix * ma,
                                           const cpl_matrix * mb)
{

    double       * ds;
    double       * cj; /* Holds one column of A * B' */
    const double * d1 = cpl_matrix_get_data_const(ma);
    const double * d2 = cpl_matrix_get_data_const(mb);

    const cpl_size      nr = cpl_matrix_get_nrow(mb);
    const cpl_size      nc = cpl_matrix_get_ncol(mb);
    cpl_size       i, j, k;


    cpl_ensure_code(ma   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(mb   != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(ma->nr == nc, CPL_ERROR_INCOMPATIBLE_INPUT);
    cpl_ensure_code(ma->nc == nc, CPL_ERROR_INCOMPATIBLE_INPUT);

    if (cpl_matrix_set_size_(self, nr, nr)) return cpl_error_set_where_();

    ds = cpl_matrix_get_data(self);
    cj = cpl_malloc((size_t)nc * sizeof(*cj));

    for (j = 0; j < nr; j++) {

        /* First compute the jth column of C = A * B', whose kth element
           is the dot-product of kth row of A and the jth row of B,
           both of which are read with unit stride. */
        for (k = 0; k < nc; k++) {
            double sum = 0.0;
            for (i = 0; i < nc; i++) {
                sum += d1[nc * k + i] * d2[nc * j + i];
            }
            cj[k] = sum;
        }

        /* Then compute (i, j)th element of the result, which
           is the dot-product of the ith row of B and the jth column of C,
           both of which are read with unit stride. */
        for (i = 0; i < nr; i++) {
            double sum = 0.0;

            for (k = 0; k < nc; k++) {
                sum += d2[nc * i + k] * cj[k];
            }
            ds[nr * i + j] = sum;
        }
    }

    cpl_free(cj);

    return CPL_ERROR_NONE;

}

/*----------------------------------------------------------------------------*/
/**
   @internal
   @brief Solve a L*transpose(L)-system with a transposed Right Hand Side
   @param self  N by N L*transpose(L)-matrix from cpl_matrix_decomp_chol()
   @param rhs   M right-hand-sides to be replaced by their solution
   @return CPL_ERROR_NONE on success, or the relevant CPL error code
   @see cpl_matrix_solve_chol()
   @note Only the lower triangle of self is accessed
   @note The use of the transpose of rhs causes a more efficient memory access

   @error
     <table class="ec" align="center">
       <tr>
         <td class="ecl">CPL_ERROR_NULL_INPUT</td>
         <td class="ecr">
           An input pointer is <tt>NULL</tt>.
         </td>
       </tr>
       <tr>
         <td class="ecl">CPL_ERROR_ILLEGAL_INPUT</td>
         <td class="ecr">
           <i>self</i> is not an n by n matrix.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_INCOMPATIBLE_INPUT</td>
         <td class="ecr">
           Selfs number of rows differs from rhs' number of columns.
         </td>
       </tr>
       <tr> 
         <td class="ecl">CPL_ERROR_DIVISION_BY_ZERO</td>
         <td class="ecr">
           The main diagonal of L contains a zero. This error can only occur
           if the L*transpose(L)-matrix does not come from a successful call to
           cpl_matrix_decomp_chol().
         </td>
       </tr>
     </table>
   @enderror

 */
/*----------------------------------------------------------------------------*/
cpl_error_code cpl_matrix_solve_chol_transpose(const cpl_matrix * self,
                                               cpl_matrix * rhs)
{

    const cpl_size n = cpl_matrix_get_ncol(self);
    const cpl_size nrhs = cpl_matrix_get_nrow(rhs);
    cpl_size i, j, k;
    const double * aread;
    const double * ai;
    double * bk;
    double sub;

    cpl_ensure_code(self != NULL, CPL_ERROR_NULL_INPUT);
    cpl_ensure_code(rhs  != NULL, CPL_ERROR_NULL_INPUT);

    cpl_ensure_code(self->nr == n, CPL_ERROR_ILLEGAL_INPUT);
    cpl_ensure_code(rhs->nc  == n, CPL_ERROR_INCOMPATIBLE_INPUT);

    aread = self->m;

    /* bk points to first entry in k'th right hand side */
    bk = rhs->m;

    for (k=0; k < nrhs; k++, bk += n) {

        /* Forward substitution in column k */

        /* Since self and rhs are addressed in the same manner,
           they can use the same index, j */
        ai = aread; /* ai points to first entry in i'th row */
        for (i = 0; i < n; i++, ai += n) {
            sub = 0.0;
            for (j = 0; j < i; j++) {
                sub += ai[j] * bk[j];
            }
            cpl_ensure_code(k > 0 || ai[j] != 0.0, CPL_ERROR_DIVISION_BY_ZERO);
            bk[j] = (bk[j] - sub) / ai[j];
        }

        /* Back substitution in column k */

        for (i = n-1; i >= 0; i--) {
            sub = bk[i];
            for (j = i+1; j < n; j++) {
                sub -= aread[n * j + i] * bk[j];
            }
            bk[i] = sub/aread[n * i + i];
        }
    }

    return CPL_ERROR_NONE;

}


/* ---------------------------------------------------------------------------*/
/**
* @brief generic 1d vandermonde matrix
*
* @param sample  sampling positions
* @param degree  degree of polynomial
* @param func    function evaluating polynomials from [0, degree] at
*                sampling point
* @param offset  offset of func (1 for indexing starting with 1)
* @return matrix containing the vandermonde matrix
*/
/* ---------------------------------------------------------------------------*/
cpl_matrix * vander1d(
          const cpl_vector * sample,
          cpl_size           degree,
          void (*func)(double, double *, int),
          size_t offset)
{
    size_t i;
    const size_t nr = cpl_vector_get_size(sample);
    const size_t nc = degree + 1;
    cpl_matrix * V = cpl_matrix_new(nr, nc);
    double * v = cpl_matrix_get_data(V);
    const double * d = sample->d;
    for (i = 0; i < nr; i++) {
        if (offset != 0) {
            double tmp[nc + offset];
            func(d[i], tmp, nc);
            memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v));
        }
        else {
            func(d[i], &v[i * nc], nc);
        }
    }

    return V;
}

cpl_matrix * vander2d(
          const cpl_vector * sample_x,
          const cpl_vector * sample_y,
          cpl_size           degree,
          void (*func)(double, double, double *, int),
          size_t offset)
{
    size_t i;
    const size_t nr = cpl_vector_get_size(sample_x);
    const size_t nc = degree + 1;
    cpl_matrix * V = cpl_matrix_new(nr, nc);
    double * v = cpl_matrix_get_data(V);
    const double * dx = sample_x->d;
    const double * dy = sample_y->d;
    assert(cpl_vector_get_size(sample_y) == nr);
    for (i = 0; i < nr; i++) {
        if (offset != 0) {
            double tmp[nc + offset];
            func(dx[i], dy[i], tmp, nc);
            memcpy(&v[i * nc], &tmp[offset], nc * sizeof(*v));
        }
        else {
            func(dx[i], dy[i], &v[i * nc], nc);
        }
    }

    return V;
}

static void polynomial(double x, double * p, int ncoefs)
{
    int i;
    p[0] = 1.;
    for (i = 1; i < ncoefs; i++) {
        p[i] = pow(x, i);
    }
}

cpl_matrix * polyvander1d(
        const cpl_vector * sample,
        cpl_size           degree)
{
    return vander1d(sample, degree, &polynomial, 0);
}

void lsqfit(
         const cpl_matrix * design,
         const cpl_vector * values,
         const cpl_vector * errors,
         cpl_matrix **coef)
{
    /* weight response and design */
    cpl_vector * vrhs = cpl_vector_duplicate(errors);
    cpl_vector_power(vrhs, -1);
    cpl_matrix * wdesign = cpl_matrix_duplicate(design);
    long i, j;
    for (i = 0; i < cpl_vector_get_size(errors); i++) {
        double w = vrhs->d[i];
        for (j = 0; j < cpl_matrix_get_ncol(wdesign); j++) {
            cpl_matrix_set(wdesign, i, j,
                           cpl_matrix_get(wdesign, i, j) * w);
        }
    }

    cpl_vector_multiply(vrhs, values);
    cpl_matrix * rhs = cpl_matrix_wrap(cpl_vector_get_size(vrhs), 1,
                                       vrhs->d);

    /* solve Ax = b */
    /* cpl_matrix_solve_normal(design, rhs) + covariance */
    {
        cpl_matrix * At  = cpl_matrix_transpose_create(wdesign);
        cpl_matrix * AtA = cpl_matrix_product_normal_create(At);

        /* RRt = AtA */
        cpl_matrix_decomp_chol(AtA);
        /* solve for pseudo inverse: (RRt)P=At*/
        cpl_matrix_solve_chol(AtA, At);
        /* compute solution to system Ax=b -> x=Pb */
        *coef = cpl_matrix_product_create(At, rhs);
        /* compute covariance matrix cov(b) = PPt */
        //cpl_matrix * cov = cpl_matrix_new(cpl_matrix_get_ncol(At),
        //                        cpl_matrix_get_ncol(At));
        //cpl_matrix_product_transpose(cov, At, At);

        cpl_matrix_delete(At);
        cpl_matrix_delete(AtA);
    }

    cpl_matrix_unwrap(rhs);
    cpl_vector_delete(vrhs);
    cpl_matrix_delete(wdesign);

    return;
}

void lsqfit_nr(
    double x[], double y[], double sig[], int ndat, double a[],
    int ma,
    void (*funcs)(double, double [], int))
{
    int i;
    cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]);
    cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]);
    cpl_vector * vS;
    cpl_matrix * design = vander1d(vX, ma - 1, funcs, 1);
    cpl_matrix * mcoef;

    if (sig) {
        vS = cpl_vector_wrap(ndat, &sig[1]);
    }
    else {
        vS = cpl_vector_new(ndat);
        for (i= 0; i < ndat; i++)
            cpl_vector_set(vS, i, 1.);
    }
    lsqfit(design, vY, vS, &mcoef);

    for (i = 1; i <= ma; i++) {
        a[i] = cpl_matrix_get(mcoef, i - 1, 0);
    }
    cpl_vector_unwrap(vX);
    cpl_vector_unwrap(vY);
    if (sig) {
        cpl_vector_unwrap(vS);
    }
    else {
        cpl_vector_delete(vS);
    }
    cpl_matrix_delete(design);
    cpl_matrix_delete(mcoef);
}


void lsqfit2d_nr(
    double x[], double y[], double z[], double sig[], int ndat, double a[],
    int ma,
    void (*funcs)(double, double, double [], int))
{
    int i;
    cpl_vector * vX = cpl_vector_wrap(ndat, &x[1]);
    cpl_vector * vY = cpl_vector_wrap(ndat, &y[1]);
    cpl_vector * vZ = cpl_vector_wrap(ndat, &z[1]);
    cpl_vector * vS;
    cpl_matrix * design = vander2d(vX, vY, ma - 1, funcs, 1);
    cpl_matrix * mcoef;

    if (sig) {
        vS = cpl_vector_wrap(ndat, &sig[1]);
    }
    else {
        vS = cpl_vector_new(ndat);
        for (i= 0; i < ndat; i++)
            cpl_vector_set(vS, i, 1.);
    }
    lsqfit(design, vZ, vS, &mcoef);

    for (i = 1; i <= ma; i++) {
        a[i] = cpl_matrix_get(mcoef, i - 1, 0);
    }
    cpl_vector_unwrap(vX);
    cpl_vector_unwrap(vY);
    cpl_vector_unwrap(vZ);
    if (sig) {
        cpl_vector_unwrap(vS);
    }
    else {
        cpl_vector_delete(vS);
    }
    cpl_matrix_delete(design);
    cpl_matrix_delete(mcoef);
}


cpl_vector * eval_poly(
      cpl_matrix * design,
      cpl_matrix * coef)
{
    cpl_matrix * fvalues = cpl_matrix_product_create(design, coef);
    cpl_vector * res = cpl_vector_wrap(cpl_matrix_get_nrow(fvalues),
                                       cpl_matrix_get_data(fvalues));
    cpl_matrix_unwrap(fvalues);
    return res;
}


