#include "header.h"

/* I used the Octave code on the Conjugate Gradients
 * Wikipedia page and translated it into C BLAS.  The
 * comments above every section of code gives the equivalent
 * octave code.
 */
void
cg(gsl_matrix_float *A, gsl_vector_float *b, gsl_vector_float *x)
{
    int i, vsize = (*A).size2;
    float a, num, den, beta;

    /* Initialize Vectors:
     *
     * Initial guess set to vector of zeros by calloc */
                      x     = gsl_vector_float_calloc(vsize);
    gsl_vector_float *x0    = gsl_vector_float_calloc(vsize);
    gsl_vector_float *r     = gsl_vector_float_calloc(vsize);
    gsl_vector_float *nr    = gsl_vector_float_calloc(vsize);
    gsl_vector_float *w     = gsl_vector_float_calloc(vsize);
    gsl_vector_float *wtemp = gsl_vector_float_calloc(vsize);
    gsl_vector_float *Ax0   = gsl_vector_float_calloc(vsize);
    gsl_vector_float *z     = gsl_vector_float_calloc(vsize);

    /* r = b - Ax0 */
    gsl_vector_float_memcpy(r, b);
    gsl_blas_sgemv(CblasNoTrans, 1., A, x0, 0.0, Ax0);
    gsl_blas_saxpy(-1., Ax0, r);  

    /* FYI: The above code is redundant unless you want to
     * make x0 different than a zero vector.  Right now,
     * all can be accomplished with r = b;
     */

    /* w = -r */
    gsl_vector_float_memcpy(w, r);
    gsl_blas_sscal(-ALPHA, w);

    /* z = A*w */
    gsl_blas_sgemv(CblasNoTrans, ALPHA, A, w, BETA, z);

    /* a = (r'*w)/(w'*z) */
    gsl_blas_sdot(r, w, &num); 
    gsl_blas_sdot(w, z, &den);
    a = num / den;

    /* x = x0 + a*w */
    gsl_vector_float_memcpy(x, x0);
    gsl_blas_saxpy(a, w, x);

    /* beta = 0 */
    beta = 0;

    /* loop */
    for (i = 0; i < vsize; i++)
    {
        /* r = r - a*z */
        gsl_blas_saxpy(-a, z, r);  

        /* if( norm(r) < 1e-10 ): break; */
        if (gsl_blas_snrm2(r) < TOLERANCE)
        {
            break;
        }
        /* beta = (r'*z)/(w'*z) */
        gsl_blas_sdot(r, z, &num);
        gsl_blas_sdot(w, z, &den);
        beta = num / den;

        /* w = -r + beta*w */
        gsl_vector_float_memcpy(nr, r);
        gsl_blas_sscal(-ALPHA, nr);
        gsl_blas_saxpy(beta, w, nr);
        gsl_vector_float_memcpy(w, nr);

        /* z = A*w */
        gsl_blas_sgemv(CblasNoTrans, ALPHA, A, w, BETA, z);

        /* a = (r'*w)/(w'*z) */
        gsl_blas_sdot(r, w, &num);
        gsl_blas_sdot(w, z, &den);
        a = num / den;

        /* x = x + a*w */
        gsl_blas_saxpy(a, w, x);
    }

    printf("%d\n", vsize);
    gsl_vector_float_fprintf(stdout, x, "%f");
}