#include <math.h>
#include <stdio.h>
#include <assert.h>

/*
  Algorithms exposed in the paper
  'Simulation of High-Dimensional t-Student Copula with a Given Block Correlation Matrix'
  by Gerard torrent-Gironella and Josep Fortiana
*/

/* gcc -Wall -O3 -DNDEBUG -o cholesky cholesky.c -lm */

/* number of blocks (or sectors) */
#define K 3
/* original matrix dimension (or number of obligors) */
#define N 6

/*
 Cholesky algorithm for block matrix
 k = number of groups
 n = elements per block, size k
 d= diagonal per block, size k
 M = block values, size kxk
 H = cholesky values (diagonal not included), size kx(n[0]+...+n[k-1])
 v = cholesky values (only diagonal), size n[0]+...+n[k-1])
 if matrix is not definite positive, returns 1, else returns 0
*/
int chold(int k, int *n, double *d, double **M, double **H, double *v)
{
	int i, j, r, s, q;
	double val1=0.0, val2=0.0;
	
	assert(k>0 && n!=NULL && d!=NULL && M!=NULL && H!=NULL && v!=NULL);
	for(r=0; r<k; r++) assert(M[r]!=NULL && H[r]!=NULL);

	if (d[0] < 0.0) return 1;
	v[0] = sqrt(d[0]);
	
	for(r=0; r<k; r++)
	{
		H[r][0]=M[r][0]/v[0];
	}
	
	for(i=-1, r=0; r<k; r++)
	{
		for(q=0; q<n[r]; q++)
		{
			i++;
			if (i==0) continue;
			if (q == 0)
			{
				for(val1=0.0, j=0; j<i; j++)
				{
					val1 += H[r][j]*H[r][j];
				}
			}
			else
			{
				val1 += H[r][i-1]*H[r][i-1];
			}
			if (d[r]-val1 < 0.0) return 1;
			v[i] = sqrt(d[r]-val1);
			for(s=r; s<k; s++)
			{
				if (q==n[r]-1 && s==r) continue;
				for(val2=0.0, j=0; j<i; j++)
				{
					val2 += H[r][j]*H[s][j];
				}
				H[s][i] = (M[s][r]-val2)/v[i];
			}
		}
	}

	return 0;
}

/*
 Multiplies a Cholesky matrix by a vector
 k = number of groups
 n = elements per block, size k
 H = cholesky values (diagonal not included), size kx(n[0]+...+n[k-1])
 v = cholesky values (only diagonal), size n[0]+...+n[k-1])
 x = input vector, size=n[0]+...+n[k]
 y = output vector, size=n[0]+...+n[k]
*/
void mult(int k, int *n, double **H, double *v, double *x, double *y)
{
	int i, j, r;
	
	for(i=-1, r=0; r<k; r++)
	{
		i++;
		y[i] = 0.0;
		for(j=0; j<i; j++) 
		{
			y[i] += H[r][j]*x[j];
		}
		
		for(j=1; j<n[r]; j++)
		{
			i++;
			y[i] = y[i-1] + H[r][i-1]*x[i-1];
		}
	}

	for(j=0; j<=i; j++)
	{
		y[j] += v[j]*x[j];
	}
}

/*
 find the inverse of the Cholesky matrix
 k = number of groups
 n = elements per block, size k
 H = cholesky values (diagonal not included), size kx(n[0]+...+n[k-1])
 v = cholesky values (only diagonal), size n[0]+...+n[k-1])
 J = inverse values (diagonal not included), size=nxk
 y = inverse values (only diagonal), size=n
*/
void inv(int k, int *n, double **H, double *v, double **J, double *w)
{
	int i, j, r, q, s, t;
	double sum;

	for(i=-1, r=0; r<k; r++) for(q=0; q<n[r]; q++) 
	{ 
		i++; 
		w[i] = 1.0/v[i]; 
	}
	
	for(i=0, r=0; r<k; r++)
	{
		for(j=i-1, s=r; s<k; s++)
		{
			sum = -H[s][i]*w[i];
			for(t=i+1; t<j; t++)
			{
				sum -= H[s][t]*J[t][r];
			}

			for(q=0; q<n[s]; q++)
			{
				j++;
				if (j==i && s==r) continue;
				sum -= H[s][j-1]*J[j-1][r];
				J[j][r] = sum/v[j];
			}
		}	
		i += n[r];
	}
}

/*
 Multiplies the inverse of the Cholesky matrix by a vector
 k = number of groups
 n = elements per block, size k
 J = inverse cholesky values (diagonal not included), size (n[0]+...+n[k-1]xk)
 w = inverse cholesky values (only diagonal), size n[0]+...+n[k-1])
 x = input vector, size=n[0]+...+n[k]
 y = output vector, size=n[0]+...+n[k]
*/
void multinv(int k, int *n, double **J, double *w, double *x, double *y)
{
	int i,j,r,q,num;
	double sum=0.0;
	
	for(num=0,r=0; r<k; r++) for(q=0; q<n[r]; q++) 
	{
		y[num] = w[num]*x[num]; 
		num++; 
	}
	
	for(i=-1,r=0; r<k; r++)
	{
		for(sum=0.0, q=0; q<n[r]; q++)
		{
			i++;
			if (q > 0) 
			{
				y[i] += sum*J[i][r];
			}
			sum += x[i];
		}
		for(j=i+1; j<num; j++)
		{
			y[j] += sum*J[j][r];
		}
	}
}

/* auxiliar function */
int fsum(int pos, int *n)
{
	int i, ret = 0;
	for(i=0; i<pos; i++)
	{
		ret += n[i];
	}
	return ret;
}

/* auxiliar function */
void printMatrix(double **M, int rows, int cols)
{
	int i, j;

	for(i=0; i<rows; i++)
	{
		for(j=0; j<cols; j++)
		{
			printf("%lf\t", M[i][j]);
		}
		printf("\n");
	}
}

/* auxiliar function */
void printVector(double *v, int size)
{
	int i;

	for(i=0; i<size; i++)
	{
		printf("%lf\t", v[i]);
	}
	printf("\n");
}

/*
  main procedure
*/
int main(int argc, char *argv[])
{
	/*
	A = [
	1.0 0.5 0.5 0.2 0.2 0.1; 
	0.5 1.0 0.5 0.2 0.2 0.1; 
	0.5 0.5 1.0 0.2 0.2 0.1; 
	0.2 0.2 0.2 1.0 0.4 0.15; 
	0.2 0.2 0.2 0.4 1.0 0.15; 
	0.1 0.1 0.1 0.15 0.15 1.0; ]
	*/
	double MV[K*K] = {0.5, 0.2, 0.1, 0.2, 0.4, 0.15, 0.1, 0.15, 0.3};
	double *M[K] = {&MV[0*K], &MV[1*K], &MV[2*K]};
	int n[K] = {3, 2, 1};
	double d[N] = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
	double HV[K*N], v[N];
	double *H[K];
	double JV[K*N], w[N];
	double *J[N];
	double x[N], y[N];
	int i, j;

	for(i=0,j=0; i<K; i++) { j += n[i]; }
	assert(j == N);

	for(i=0;i<K; i++) { H[i] = &HV[i*N]; }
	for(i=0;i<N; i++) { J[i] = &JV[K*i]; }
	for(i=0; i<K; i++) for(j=0; j<N; j++) { H[i][j]=0.0; J[j][i]=0.0; } 
	
	int rc = chold(K, n, d, M, H, v);
	if (rc > 0) {
		printf("matrix not definite positive\n");
		return 1;
	}

	printf("H = \n");
	printMatrix(H, 3, 6);
	printf("v = \n");
	printVector(v, 6);

	inv(3, n, H, v, J, w);

	printf("J = \n");
	printMatrix(J, 6, 3);
	printf("w = \n");
	printVector(w, 6);
	
	for(i=0;i<N; i++) { x[i] = (double)(i+1.0); }
	mult(K, n, H, v, x, y);
	//multinv(3, n, J, w, x, y);

	printf("MULT = \n");
	printVector(y, 6);

	return(0);
}