#include <v_lib.h>

vboolean matDiagIterate(matrix_p am,vector_p bv,vector_p trial_x,
	vec_t ftol,u32_t max_iter)
{
	u32_t i,k,N;
	vec_p b, x, xn, diag, *a;
	vector_p xnv, diagv;
	vec_t s, d, t;
	u32_t count;
	
	if(am->rows != am->cols || am->rows != bv->size ||
		am->rows != trial_x->size)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_ERROR,"matDiagIterate(%X,%X,%X): "
			"invalid parameter(s).\n",am,bv,trial_x);
#endif	/* CHECKED_BUILD */
		return false;
	}

	N = am->rows;
	xnv = vecAlloc(N);
	diagv = vecAlloc(N);
	x = trial_x->elements;
	xn = xnv->elements;
	b = bv->elements;
	diag = diagv->elements;
	a = am->cell;

	/* Compute 1/a[k][k] once */
	for(k=0;k<N;k++)
	{
		t = a[k][k];
		if(IsZero(t))
		{
#ifdef	CHECKED_BUILD
			conlPrintf(LOG_ERROR,
				"matDiagSolve(): diagonal element too small on row %u.\n",k);
#endif	/* CHECKED_BUILD */
			memFree(xnv);
			memFree(diagv);
			return false;
		}
		diag[k] = (vec_t)1 / t;
	}

	for(count=0;count<max_iter;count++)
	{
		/* Next iteration */
		for(k = 0;k < N;k++)
		{
			s = 0;
			for(i = 0;i < k;i++)
				s += a[k][i] * x[i];
			for(i = k+1;i < N;i++)
				s += a[k][i] * x[i];
			xn[k] = diag[k] * (b[k] - s);
		}
		/* Check if we done */
		d = 0;
		for(k=0;k<N;k++)
		{
			t = xn[k] - x[k];
			d += t * t;
		}
		memCopy(x,xn,N*sizeof(vec_t));
		if(d <= ftol)
			break;
	}

	if(count == max_iter)
	{
#ifdef	CHECKED_BUILD
		conlPrintf(LOG_TRACE,
			"matDiagIterate(): no convergence after %u iterations.\n",
			max_iter);
#endif	/* CHECKED_BUILD */
		return false;
	}
	memFree(xnv);
	memFree(diagv);
	return true;
}

#define N	3
#define	MAX_ITER	1024
#define	FTOL		1e-9

int main()
{
	u32_t k;
	vector_p xv, bv;
	matrix_p am;
	vec_p x, b, *a;

	logStartConsoleLog();
	logUpperLevel(LOG_TRACE);

	xv = vecAlloc(N); x=xv->elements;
	bv = vecAlloc(N); b=bv->elements;
	am = matAlloc(N,N); a=am->cell;
	
	b[0]=22;b[1]=32;b[2]=38;
	a[0][0]=10;	a[0][1]=3;	a[0][2]=2;
	a[1][0]=3;	a[1][1]=10;	a[1][2]=3;
	a[2][0]=2;	a[2][1]=3;	a[2][2]=10;
	
	vecZero(xv);
	matDiagIterate(am,bv,xv,FTOL,MAX_ITER);

	printf("x:\t");
	for(k=0;k<N;k++)
		printf("%g;\t",x[k]);
	puts("");

	
	memFree(xv);
	memFree(bv);
	memFree(am);
	return 0;
}
