#include "header.h"

void VectorAdd(gsl_vector_float *a,gsl_vector_float *b,gsl_vector_float *ans)
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_add(ans,b);
}

void VectorSub(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 VectorScale(gsl_vector_float *a,double x,gsl_vector_float *ans)
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_scale(ans,x);
}

int fGC(gsl_matrix_float *A,gsl_vector_float *x,gsl_vector_float *b,float Tol,int MaxIte)
{
	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;
////////////////////////////////Initial///////////////////////////////////////////////////////////////////
//if you use the library error handler to abort your program then it isn't necessary to check every alloc. 
//////////////////////////////////////////////////////////////////////////////////////////////////////////
	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
	VectorSub(b,tmp,r);         // r=b-tmp;
	VectorScale(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);
	VectorScale(w,a,tmp);  	//tmp=a*w
	gsl_vector_float_add(x,tmp);   //x+=a*w;
	B=0;
	for(i=0;i<MaxIte;i++)
	{
		VectorScale(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);
		VectorScale(w,B,tmp);	// tmp=B*w
		VectorScale(r,-1,tmp2); // tmp2=-r;
		VectorAdd(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);
		VectorScale(w,a,tmp);  	//tmp=a*w
		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;
}