#include "Header.h"

//conjugate gradient algorithm
//
// solves A x = b   for x
// where
//		.   A is a symmetric, positive definite, and real Matrix
//		.	b is a vector that conforms with A (A columns == b rows)
//		.	x is a vector that conforms with A (A columns == x rows)
//returns x
Vec * CG(const gsl_matrix_float * A, const gsl_vector_float * b, float tol, int maxItn)
{
	int i;						//used in for loops
	int itn;					//counter for # of iterations in loop
	float c1, c2, c3, af, bf;	//temp floats
	float res0, res;			//residuals
	Vec * solutionX;			//the return value
	//vectors used in the CG method
	gsl_vector_float * x;		//solution x to be converted to Vec * before return
	gsl_vector_float * r;
	gsl_vector_float * p;
	gsl_vector_float * pTemp;
	gsl_vector_float * v;
	gsl_vector_float * vTemp;
	
	//vectors allocation
	solutionX = createVec(b->size);
	x = 		gsl_vector_float_alloc(b->size);
	r = 		gsl_vector_float_alloc(b->size);
	p = 		gsl_vector_float_alloc(b->size);
	pTemp = 	gsl_vector_float_alloc(b->size);
	v = 		gsl_vector_float_alloc(b->size);
	vTemp = 	gsl_vector_float_alloc(b->size);
	
	//initilizing x elements to zero
	gsl_vector_float_set_zero(x);
	
	//copying b to r and p
	gsl_vector_float_memcpy(r, b);
	gsl_vector_float_memcpy(p, b);

	//c1=r'*rS
	gsl_blas_sdot(r, r, &c1);
	
	//initial residual
	res0 = sqrt(c1);
	res = res0;
	
	//while not reached tol needed AND MAX_ITRN
	for(itn =0; itn < MAX_ITRN && res > (tol*res0); itn++)
	{
		//v=A*p;
		gsl_blas_sgemv(CblasNoTrans, 1.0, A, p, 0.0, v);
		
		//c2=p'*v;
		gsl_blas_sdot(p, v, &c2);
		
		//Update solution x
		af = c1 / c2;
		gsl_vector_float_memcpy(pTemp, p);		//to preserve p elements
		gsl_vector_float_scale(pTemp, af);		//	pTemp = af*pTemp    (equals to pTemp = af*p)
		gsl_vector_float_add(x, pTemp);			//	x = x + pTemp

		//Update residual
		gsl_vector_float_memcpy(vTemp, v);		//to preserve v elements
		gsl_vector_float_scale(vTemp, af);		//	vTemp = af*vTemp    (equals to vTemp = af*v)
		gsl_vector_float_sub(r, vTemp);			//	r = r - vTemp

		//Compute a new search direction
		gsl_blas_sdot(r, r, &c3);	//c3=r'*r;
		bf = c3 / c1;
		gsl_vector_float_scale(p, bf);		//	p = bf*p
		gsl_vector_float_add(p, r);			//	p = (bf*p) + r

		//for next iteration
		res=sqrt(c3);
		c1 = c3;
	}
	
	//copy x values to solutionX for return
	for(i =0; i < solutionX->n; i++)
	{
		solutionX->p[i] = x->data[i];
	}

	//free memory resources
	gsl_vector_float_free(x);
	gsl_vector_float_free(r);
	gsl_vector_float_free(p);
	gsl_vector_float_free(pTemp);
	gsl_vector_float_free(v);
	gsl_vector_float_free(vTemp);
	
	return solutionX;
}