/*****************************************************************************
 *
 * MODULE:       Grass numerical math interface
 * AUTHOR(S):    Soeren Gebbert, Berlin (GER) Dec 2006
 *                 soerengebbert <at> googlemail <dot> com
 *
 * PURPOSE:      linear equation system solvers
 *                 part of the gmath library
 *
 * COPYRIGHT:    (C) 2010 by the GRASS Development Team
 *
 *               This program is free software under the GNU General Public
 *               License (>=v2). Read the file COPYING that comes with GRASS
 *               for details.
 *
 *****************************************************************************/

#include <assert.h>
#include <stdlib.h>
#include <math.h>
#include <grass/gmath.h>
#include <grass/gis.h>

/*!
 * \brief Adds a sparse vector to a sparse matrix at position row
 *
 * Return 1 for success and -1 for failure
 *
 * \param Asp G_math_spvector **
 * \param spvector G_math_spvector *
 * \param row int
 * \return int 1 success, -1 failure
 *
 * */
int G_math_add_spvector(G_math_spvector **Asp, G_math_spvector *spvector,
                        int row)
{
    if (Asp != NULL) {
        G_debug(5,
                "Add sparse vector %p to the sparse linear equation system at "
                "row %i\n",
                (void *)spvector, row);
        Asp[row] = spvector;
    }
    else {
        return -1;
    }

    return 1;
}

/*!
 * \brief Allocate memory for a sparse matrix
 *
 * \param rows int
 * \return G_math_spvector **
 *
 * */
G_math_spvector **G_math_alloc_spmatrix(int rows)
{
    G_math_spvector **spmatrix;

    G_debug(4, "Allocate memory for a sparse matrix with %i rows\n", rows);

    spmatrix = (G_math_spvector **)G_calloc(rows, sizeof(G_math_spvector *));

    return spmatrix;
}

/*!
 * \brief Allocate memory for a sparse vector
 *
 * \param cols int
 * \return G_math_spvector *
 *
 * */
G_math_spvector *G_math_alloc_spvector(int cols)
{
    G_math_spvector *spvector;

    G_debug(4, "Allocate memory for a sparse vector with %i cols\n", cols);

    spvector = (G_math_spvector *)G_calloc(1, sizeof(G_math_spvector));

    spvector->cols = cols;
    spvector->index = (unsigned int *)G_calloc(cols, sizeof(unsigned int));
    spvector->values = (double *)G_calloc(cols, sizeof(double));

    return spvector;
}

/*!
 * \brief Release the memory of the sparse vector
 *
 * \param spvector G_math_spvector *
 * \return void
 *
 * */
void G_math_free_spvector(G_math_spvector *spvector)
{
    if (spvector) {
        if (spvector->values)
            G_free(spvector->values);
        if (spvector->index)
            G_free(spvector->index);
        G_free(spvector);

        spvector = NULL;
    }

    return;
}

/*!
 * \brief Release the memory of the sparse matrix
 *
 * \param Asp G_math_spvector **
 * \param rows int
 * \return void
 *
 * */
void G_math_free_spmatrix(G_math_spvector **Asp, int rows)
{
    int i;

    if (Asp) {
        for (i = 0; i < rows; i++)
            G_math_free_spvector(Asp[i]);

        G_free(Asp);
        Asp = NULL;
    }

    return;
}

/*!
 *
 * \brief print the sparse matrix Asp to stdout
 *
 *
 * \param Asp (G_math_spvector **)
 * \param rows (int)
 * \return void
 *
 * */
void G_math_print_spmatrix(G_math_spvector **Asp, int rows)
{
    int i, j, out;
    unsigned int k;

    for (i = 0; i < rows; i++) {
        for (j = 0; j < rows; j++) {
            out = 0;
            for (k = 0; k < Asp[i]->cols; k++) {
                if (Asp[i]->index[k] == (unsigned int)j) {
                    fprintf(stdout, "%4.5f ", Asp[i]->values[k]);
                    out = 1;
                }
            }
            if (!out)
                fprintf(stdout, "%4.5f ", 0.0);
        }
        fprintf(stdout, "\n");
    }

    return;
}

/*!
 * \brief Convert a sparse matrix into a quadratic matrix
 *
 * This function is multi-threaded with OpenMP. It creates its own parallel
 * OpenMP region.
 *
 * \param Asp (G_math_spvector **)
 * \param rows (int)
 * \return (double **)
 *
 * */
double **G_math_Asp_to_A(G_math_spvector **Asp, int rows)
{
    int i;
    unsigned int j;

    double **A = NULL;

    A = G_alloc_matrix(rows, rows);

#pragma omp parallel for schedule(static) private(i, j)
    for (i = 0; i < rows; i++) {
        for (j = 0; j < Asp[i]->cols; j++) {
            A[i][Asp[i]->index[j]] = Asp[i]->values[j];
        }
    }
    return A;
}

/*!
 * \brief Convert a symmetric sparse matrix into a symmetric band matrix
 *
 \verbatim
 Symmetric matrix with bandwidth of 3

 5 2 1 0
 2 5 2 1
 1 2 5 2
 0 1 2 5

 will be converted into the band matrix

 5 2 1
 5 2 1
 5 2 0
 5 0 0

 \endverbatim
 * \param Asp (G_math_spvector **)
 * \param rows (int)
 * \param bandwidth (int)
 * \return (double **) the resulting ymmetric band matrix [rows][bandwidth]
 *
 * */
double **G_math_Asp_to_sband_matrix(G_math_spvector **Asp, int rows,
                                    int bandwidth)
{
    unsigned int i, j;

    double **A = NULL;

    assert(rows >= 0 && bandwidth >= 0);

    A = G_alloc_matrix(rows, bandwidth);

    for (i = 0; i < (unsigned int)rows; i++) {
        for (j = 0; j < Asp[i]->cols; j++) {
            if (Asp[i]->index[j] == i) {
                A[i][0] = Asp[i]->values[j];
            }
            else if (Asp[i]->index[j] > i) {
                A[i][Asp[i]->index[j] - i] = Asp[i]->values[j];
            }
        }
    }
    return A;
}

/*!
 * \brief Convert a quadratic matrix into a sparse matrix
 *
 * This function is multi-threaded with OpenMP. It creates its own parallel
 * OpenMP region.
 *
 * \param A (double **)
 * \param rows (int)
 * \param epsilon (double) -- non-zero values are greater then epsilon
 * \return (G_math_spvector **)
 *
 * */
G_math_spvector **G_math_A_to_Asp(double **A, int rows, double epsilon)
{
    int i, j;

    int nonull, count = 0;

    G_math_spvector **Asp = NULL;

    Asp = G_math_alloc_spmatrix(rows);

#pragma omp parallel for schedule(static) private(i, j, nonull, count)
    for (i = 0; i < rows; i++) {
        nonull = 0;
        /*Count the number of non zero entries */
        for (j = 0; j < rows; j++) {
            if (A[i][j] > epsilon)
                nonull++;
        }
        /*Allocate the sparse vector and insert values */
        G_math_spvector *v = G_math_alloc_spvector(nonull);

        count = 0;
        for (j = 0; j < rows; j++) {
            if (A[i][j] > epsilon) {
                v->index[count] = j;
                v->values[count] = A[i][j];
                count++;
            }
        }
        /*Add vector to sparse matrix */
        G_math_add_spvector(Asp, v, i);
    }
    return Asp;
}

/*!
 * \brief Convert a symmetric band matrix into a sparse matrix
 *
 * WARNING:
 * This function is experimental, do not use.
 * Only the upper triangle matrix of the band structure is copied.
 *
 * \param A (double **) the symmetric band matrix
 * \param rows (int)
 * \param bandwidth (int)
 * \param epsilon (double) -- non-zero values are greater then epsilon
 * \return (G_math_spvector **)
 *
 * */
G_math_spvector **G_math_sband_matrix_to_Asp(double **A, int rows,
                                             int bandwidth, double epsilon)
{
    int i, j;

    int nonull, count = 0;

    G_math_spvector **Asp = NULL;

    Asp = G_math_alloc_spmatrix(rows);

    for (i = 0; i < rows; i++) {
        nonull = 0;
        /*Count the number of non zero entries */
        for (j = 0; j < bandwidth; j++) {
            if (A[i][j] > epsilon)
                nonull++;
        }

        /*Allocate the sparse vector and insert values */

        G_math_spvector *v = G_math_alloc_spvector(nonull);

        count = 0;
        if (A[i][0] > epsilon) {
            v->index[count] = i;
            v->values[count] = A[i][0];
            count++;
        }

        for (j = 1; j < bandwidth; j++) {
            if (A[i][j] > epsilon && i + j < rows) {
                v->index[count] = i + j;
                v->values[count] = A[i][j];
                count++;
            }
        }
        /*Add vector to sparse matrix */
        G_math_add_spvector(Asp, v, i);
    }
    return Asp;
}

/*!
 * \brief Compute the matrix - vector product
 * of sparse matrix **Asp and vector x.
 *
 * This function is multi-threaded with OpenMP and can be called within a
 * parallel OpenMP region.
 *
 * y = A * x
 *
 *
 * \param Asp (G_math_spvector **)
 * \param x (double) *)
 * \param y (double * )
 * \param rows (int)
 * \return (void)
 *
 * */
void G_math_Ax_sparse(G_math_spvector **Asp, double *x, double *y, int rows)
{
    int i;
    unsigned int j;

    double tmp;

#pragma omp for schedule(static) private(i, j, tmp)
    for (i = 0; i < rows; i++) {
        tmp = 0;
        for (j = 0; j < Asp[i]->cols; j++) {
            tmp += Asp[i]->values[j] * x[Asp[i]->index[j]];
        }
        y[i] = tmp;
    }
    return;
}