#include "header.h"

void vecread(gsl_vector_float *a,gsl_vector_float *b,gsl_vector_float *ans)// add vector
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_add(ans,b);
}

void vecsub(gsl_vector_float *a,gsl_vector_float *b,gsl_vector_float *ans)
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_sub(ans,b);
}

void vecsca(gsl_vector_float *a,double x,gsl_vector_float *ans)//vector scale
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_scale(ans,x);
}

int cg(gsl_matrix_float *A,gsl_vector_float *x,gsl_vector_float *b,float Tol,int MaxIte)// cg stands for "conjugate gradient algorithm function"
{
	gsl_vector_float *tmp,*tmp2,*r;
	gsl_vector_float *w,*z;
	const float alpha0=0.0,alpha1=1.0;
	float beta=0;	
	float ftmp1,ftmp2,a,B;
	int i;

	tmp=gsl_vector_float_calloc((*b).size);
	tmp2=gsl_vector_float_calloc((*b).size);
	r=gsl_vector_float_calloc((*b).size);
	w=gsl_vector_float_calloc((*b).size);
	z=gsl_vector_float_calloc((*b).size);

	gsl_blas_sgemv(CblasNoTrans,alpha1,A,x,beta,tmp); //tmp=Ax
	vecsub(b,tmp,r);         // r=b-tmp;
	vecsca(r,-1.0,w);      // w=-r;
	gsl_blas_sgemv (CblasNoTrans,alpha1,A,w,beta,z);//z=Aw
	gsl_blas_sdsdot(alpha0,r,w,&ftmp1);
	gsl_blas_sdsdot(alpha0,w,z,&ftmp2);
	a=ftmp1/ftmp2;		  // a=(r'*w)/(w'*z);
	vecsca(w,a,tmp);  	//tmp=a*w
	gsl_vector_float_add(x,tmp);   //x+=a*w;
	B=0;
	for(i=0;i<MaxIte;i++)
	{
		vecsca(z,a,tmp); //tmp=a*z
	 	gsl_vector_float_sub(r,tmp); //r-=tmp
		if(gsl_blas_snrm2(r)<Tol) 
			break;
		gsl_blas_sdsdot(alpha0,r,z,&ftmp1);
		gsl_blas_sdsdot(alpha0,w,z,&ftmp2);
		B=ftmp1/ftmp2;		  // B=(r'*z)/(w'*z);
		vecsca(w,B,tmp);	
		vecsca(r,-1,tmp2); 
		vecread(tmp,tmp2,w);  // w=-r+B*w;
		gsl_blas_sgemv (CblasNoTrans,alpha1,A,w,beta,z);//z=Aw
		gsl_blas_sdsdot(alpha0,r,w,&ftmp1);
		gsl_blas_sdsdot(alpha0,w,z,&ftmp2);
		a=ftmp1/ftmp2;		  // a=(r'*w)/(w'*z);
		vecsca(w,a,tmp);  	
		gsl_vector_float_add(x,tmp);   //x+=a*w;
	}
	gsl_vector_float_free(tmp);
	gsl_vector_float_free(tmp2);
	gsl_vector_float_free(r);
	gsl_vector_float_free(w);
	gsl_vector_float_free(z);
	return 1;
}