#include "header.h"

void VectorSub(gsl_vector_float *a,gsl_vector_float *b,gsl_vector_float *ans)  // 'substruct' function
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_sub(ans,b);
}

void VectorAdd(gsl_vector_float *a,gsl_vector_float *b,gsl_vector_float *ans)  // 'add' function
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_add(ans,b);
}


void VectorScale(gsl_vector_float *a,float x,gsl_vector_float *ans)  // 'scale multiply' function
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_scale(ans,x);
}

gsl_vector_float fCG(gsl_matrix_float A,gsl_vector_float x,gsl_vector_float b,float Tolerance,int MaxIte)  // CG function
{
	gsl_vector_float *tmp1,*tmp2,*subs;
	gsl_vector_float *y,*z;
	const float alpha0=0.0,alpha1=1.0;	// keep alpha0 always be 0.0 and alpha1 always be 1.0
	float beta=0.0,a1,a2,ftmp1,ftmp2;	
	int i;

	// Initialize each variable
	tmp1=gsl_vector_float_calloc((b).size);
	tmp2=gsl_vector_float_calloc((b).size);
	subs=gsl_vector_float_calloc((b).size);
	y=	gsl_vector_float_calloc((b).size);
	z=	gsl_vector_float_calloc((b).size);

	// Basic operations for CG
	gsl_blas_sgemv(CblasNoTrans,alpha1,&A,&x,beta,tmp1); // tmp1=Ax
	VectorSub(&b,tmp1,subs);     // subs=b-tmp1
	VectorScale(subs,-1.0,y);   // y=(-1.0)*subs= -subs
	gsl_blas_sgemv (CblasNoTrans,alpha1,&A,y,beta,z);   //z=Ay
	gsl_blas_sdsdot(alpha0,subs,y,&ftmp1);
	gsl_blas_sdsdot(alpha0,y,z,&ftmp2);
	a1=ftmp1/ftmp2;		  // a1=(subs'*y)/(y'*z)
	VectorScale(y,a1,tmp1);  	//tmp1= a1*y
	gsl_vector_float_add(&x,tmp1);   //x+= tmp1 --> x+= a1*y


	for(i=0;i<MaxIte;i++)
	{
		VectorScale(z,a1,tmp1); //tmp1=a1*z
	 	gsl_vector_float_sub(subs,tmp1); //subs-=tmp1
		if(gsl_blas_snrm2(subs)<Tolerance) 
			break;
		gsl_blas_sdsdot(alpha0,subs,z,&ftmp1);
		gsl_blas_sdsdot(alpha0,y,z,&ftmp2);
		a2=ftmp1/ftmp2;		  		// a2=(subs'*z)/(y'*z)
		VectorScale(y,a2,tmp1);		// tmp1=a2*y
		VectorScale(subs,-1.0,tmp2);  // tmp2=-subs
		VectorAdd(tmp1,tmp2,y);  	// y=(-subs)+(a2*y)
		gsl_blas_sgemv (CblasNoTrans,alpha1,&A,y,beta,z);//z=Ay
		gsl_blas_sdsdot(alpha0,subs,y,&ftmp1);
		gsl_blas_sdsdot(alpha0,y,z,&ftmp2);
		a1=ftmp1/ftmp2;		  // a1=(subs'*y)/(y'*z)
		VectorScale(y,a1,tmp1);  	//tmp1=a1*y
		gsl_vector_float_add(&x,tmp1);   //x+= tmp1 --> x+= a1*y
	}

	// release these variables
	gsl_vector_float_free(tmp1);
	gsl_vector_float_free(tmp2);
	gsl_vector_float_free(subs);
	gsl_vector_float_free(y);
	gsl_vector_float_free(z);

	return x;
}