#include "header.h"

void VectorSubtraction(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 VectorAddition(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 VectorScalar(gsl_vector_float *a,float x,gsl_vector_float *ans) 
{
	gsl_vector_float_memcpy(ans,a);
	gsl_vector_float_scale(ans,x);
}

gsl_vector_float ConjugateGradient(gsl_matrix_float A,gsl_vector_float x,gsl_vector_float b,float Tolerance,int MaxIterations)  // CG function
{
	gsl_vector_float *tmp1,*tmp2,*subs;
	gsl_vector_float *y,*z;
	const float alpha0=0.0,alpha1=1.0;	// alpha0 and alpha 1 are always constant at 0 and 1
	float beta=0.0,a1,a2,ftmp1,ftmp2;	
	int i;

	//using these numerous variable are inefficient, but Robin's way that made the most sense in my head.
	tmp1=gsl_vector_float_calloc((b).size);
	tmp2=gsl_vector_float_calloc((b).size);
	subs=gsl_vector_float_calloc((b).size);			//initializing the temporary variables
	y=	gsl_vector_float_calloc((b).size);
	z=	gsl_vector_float_calloc((b).size);


	gsl_blas_sgemv(CblasNoTrans,alpha1,&A,&x,beta,tmp1); // tmp1=Ax
	VectorSubtraction(&b,tmp1,subs);     // subs=b-tmp1
	VectorScalar(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)
	VectorScalar(y,a1,tmp1);  	//tmp1= a1*y
	gsl_vector_float_add(&x,tmp1);   //x+= tmp1 --> x+= a1*y

	//cg loop
	for(i=0;i<MaxIterations;i++)
	{
		VectorScalar(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)
		VectorScalar(y,a2,tmp1);		// tmp1=a2*y
		VectorScalar(subs,-1.0,tmp2);  // tmp2=-subs
		VectorAddition(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)
		VectorScalar(y,a1,tmp1);  	//tmp1=a1*y
		gsl_vector_float_add(&x,tmp1);   //x+= tmp1 --> x+= a1*y
	}

	// free the memory for the 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;
}