/* ===========================================================================
*
*                            PUBLIC DOMAIN NOTICE
*               National Center for Biotechnology Information
*
*  This software/database is a "United States Government Work" under the
*  terms of the United States Copyright Act.  It was written as part of
*  the author's official duties as a United States Government employee and
*  thus cannot be copyrighted.  This software/database is freely available
*  to the public for use. The National Library of Medicine and the U.S.
*  Government have not placed any restriction on its use or reproduction.
*
*  Although all reasonable efforts have been taken to ensure the accuracy
*  and reliability of the software and data, the NLM and the U.S.
*  Government do not and cannot warrant the performance or results that
*  may be obtained by using this software or data. The NLM and the U.S.
*  Government disclaim all warranties, express or implied, including
*  warranties of performance, merchantability or fitness for any particular
*  purpose.
*
*  Please cite the author in any work or product based on this material.
*
* ===========================================================================*/

/** @file nlm_linear_algebra.c
 * Basic matrix and vector operations
 *
 * @author E. Michael Gertz
 */

#include <math.h>
#include <stdlib.h>
#include "matrix_adjust.h"

/* Documented in nlm_linear_algebra.h. */
inline MatrixFloat**
Nlm_DenseMatrixNew(size_t nrows,
    size_t ncols)
{
    MatrixFloat** mat;     /* the new matrix */

    mat = (MatrixFloat**)calloc(nrows, sizeof(MatrixFloat*));
    if (mat != NULL) {
        mat[0] = (MatrixFloat*)malloc((size_t)nrows *
            (size_t)ncols * sizeof(MatrixFloat));
        if (mat[0] != NULL) {
            for (size_t i = 1; i < nrows; i++) {
                mat[i] = &mat[0][i * ncols];
            }
        }
        else {
            free(mat);
            mat = NULL;
        }
    }
    return mat;
}


/* Documented in nlm_linear_algebra.h. */
inline MatrixFloat**
Nlm_LtriangMatrixNew(int n)
{
    int i;                      /* iteration index */
    MatrixFloat** L;                /* the new, lower triangular matrix */
    size_t nelts;               /* the number of elements in
                                   the matrix */
    nelts = ((size_t)n * (n + 1)) / 2;

    L = (MatrixFloat**)calloc(n, sizeof(MatrixFloat*));
    if (L != NULL) {
        L[0] = (MatrixFloat*)calloc(nelts, sizeof(MatrixFloat)); // was malloc
        if (L[0] != NULL) {
            for (i = 1; i < n; i++) {
                L[i] = L[i - 1] + i;
            }
        }
        else {
            free(L);
            L = NULL;
        }
    }
    return L;
}


/* Documented in nlm_linear_algebra.h. */
inline void
Nlm_DenseMatrixFree(MatrixFloat*** mat)
{
    if (*mat != NULL) {
        free((*mat)[0]);
        free(*mat);
    }
    *mat = NULL;
}


/* Documented in nlm_linear_algebra.h. */
inline int** Nlm_Int4MatrixNew(int nrows, int ncols)
{
    int i;             /* iteration index */
    int** mat;     /* the new matrix */

    mat = (int**)calloc(nrows, sizeof(int*));
    if (mat != NULL) {
        mat[0] = (int*)malloc((size_t)nrows *
            (size_t)ncols * sizeof(int));
        if (mat[0] != NULL) {
            for (i = 1; i < nrows; i++) {
                mat[i] = &mat[0][i * ncols];
            }
        }
        else {
            free(mat);
            mat = NULL;
        }
    }
    return mat;
}


/* Documented in nlm_linear_algebra.h. */
inline void
Nlm_Int4MatrixFree(int*** mat)
{
    if (*mat != NULL) {
        free((*mat)[0]);
        free(*mat);
    }
    *mat = NULL;
}


/* Documented in nlm_linear_algebra.h. */
inline void
Nlm_FactorLtriangPosDef(MatrixFloat** A, int n)
{
    int i, j, k;                /* iteration indices */
    MatrixFloat temp;                /* temporary variable for intermediate
                                   values in a computation */

    for (i = 0; i < n; i++) {
        for (j = 0; j < i; j++) {
            temp = A[i][j];
            for (k = 0; k < j; k++) {
                temp -= A[i][k] * A[j][k];
            }
            A[i][j] = temp / A[j][j];
        }
        temp = A[i][i];
        for (k = 0; k < i; k++) {
            temp -= A[i][k] * A[i][k];
        }
        A[i][i] = sqrt(temp);
    }
}


/* Documented in nlm_linear_algebra.h. */
inline void Nlm_SolveLtriangPosDef(MatrixFloat x[], int n,
    MatrixFloat** L)
{
    int i, j;                   /* iteration indices */
    MatrixFloat temp;                /* temporary variable for intermediate
                                   values in a computation */

                                   /* At point x = b in the equation L L\T y = b */

                                   /* Forward solve; L z = b */
    for (i = 0; i < n; i++) {
        temp = x[i];
        for (j = 0; j < i; j++) {
            temp -= L[i][j] * x[j];
        }
        x[i] = temp / L[i][i];
    }
    /* Now x = z.  Back solve the system L\T y = z */
    for (j = n - 1; j >= 0; j--) {
        x[j] /= L[j][j];
        for (i = 0; i < j; i++) {
            x[i] -= L[j][i] * x[j];
        }
    }
    /* Now x = y, the solution to  L L\T y = b */
}


/* Documented in nlm_linear_algebra.h. */
inline MatrixFloat
Nlm_EuclideanNorm(const MatrixFloat v[], int n)
{
    MatrixFloat sum = 1.0;   /* sum of squares of elements in v */
    MatrixFloat scale = 0.0;   /* a scale factor for the elements in v */
    int i;                /* iteration index */

    for (i = 0; i < n; i++) {
        if (v[i] != 0.0) {
            MatrixFloat absvi = fabs(v[i]);
            if (scale < absvi) {
                sum = 1.0 + sum * (scale / absvi) * (scale / absvi);
                scale = absvi;
            }
            else {
                sum += (absvi / scale) * (absvi / scale);
            }
        }
    }
    return scale * sqrt(sum);
}


/* Documented in nlm_linear_algebra.h. */
inline void Nlm_AddVectors(MatrixFloat y[], int n, MatrixFloat alpha, const MatrixFloat x[])
{
    int i;                     /* iteration index */

    for (i = 0; i < n; i++) {
        y[i] += alpha * x[i];
    }
}


/* Documented in nlm_linear_algebra.h. */
inline MatrixFloat
Nlm_StepBound(const MatrixFloat x[], int n, const MatrixFloat step_x[], MatrixFloat max)
{
    int i;                 /* iteration index */
    MatrixFloat alpha = max;    /* current largest permitted step */

    for (i = 0; i < n; i++) {
        MatrixFloat alpha_i;    /* a step to the boundary for the current i */

        alpha_i = -x[i] / step_x[i];
        if (alpha_i >= 0 && alpha_i < alpha) {
            alpha = alpha_i;
        }
    }
    return alpha;
}