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


#define N 5
#define MAX_SV_RATIO 1e8


void svd(double **A, double *S2, int n);

void Solve_System(double **A_in, double *b, double *w, int rows);

double **Create_Matrix(int rows, int cols);

void My_Error(char error_text[]);

void svd(double **A, double *S2, int n);


int main(int argc, char **argv)
{
	double **Z, **sigma_Z;
	double *beta;
	double *Y;
	double *Sigma_YZ;
	double *mean_Z;
	double mean_Y;
	double SigmaYY;
	int i,j,k;


	// do memory allocation

	Z = Create_Matrix(N,N);

	sigma_Z = Create_Matrix(N,N);


	mean_Z = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(mean_Z))
    {
		printf("Cannot allocate mean_Z!!\n");
    }

	Sigma_YZ = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(Sigma_YZ))
    {
		printf("Cannot allocate Sigma_YZ!!\n");
    }

	Y = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(Y))
    {
		printf("Cannot allocate Y!!\n");
    }


	beta = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(beta))
    {
		My_Error("Cannot allocate beta!!\n");
    }



	// A fake Z matrix and fake Y
	srand( 10 );
	for (i = 0; i < N; i++)
	{

		for ( j = 0; j < N; j++ )
		{
			Z[i][j] = (double)rand()/(double)RAND_MAX;
		}
		Y[i] =  (double)rand()/(double)RAND_MAX;
	}


	// calcuate the mean values for Y and the columns of Z
	mean_Y = 0.0;
	for (i = 0; i < N; i++)
	{
		mean_Y = mean_Y + Y[i];

		mean_Z[i] = 0.0;
		for ( j = 0; j < N; j++ )
		{
			mean_Z[i] = mean_Z[i] + Z[j][i];
		}
		mean_Z[i] = mean_Z[i] / N;
	}
	mean_Y = mean_Y / N;


	// calculate the covariance matrix
	for (i = 0; i < N; i++)
	{
		Sigma_YZ[i] = 0;
		for ( j = 0; j < N; j++ )
		{
			Sigma_YZ[i] = Sigma_YZ[i] + ((Z[j][i]-mean_Z[i]) * (Y[j]-mean_Y));

			sigma_Z[i][j] = 0.0;
			for ( k = 0; k < N; k++ )
			{
				sigma_Z[i][j] = sigma_Z[i][j] + ((Z[k][i]-mean_Z[i]) * (Z[k][j]-mean_Z[j]));
			}
			sigma_Z[i][j] = sigma_Z[i][j] /(N-1);
		}
		Sigma_YZ[i] = Sigma_YZ[i] / (N-1);
	}

	// finally calculate the variance of Y (i.e. sigmaYY) for Omega calculations (not needed for gettinb betta)
	SigmaYY = 0.0;
	for (i = 0; i < N; i++)
	{
		SigmaYY = SigmaYY + (Y[i]-mean_Y) * (Y[i]-mean_Y);
	}
	SigmaYY = SigmaYY / (N-1);

	// Now solve for beta using SVD
	Solve_System(sigma_Z, Sigma_YZ, beta, N);
	// the beta's are in the vector beta

	return(1);


}


void Solve_System(double **A_in, double *b, double *w, int rows)
{

	int i, j, k;
	double tmp;

	double **A;
	double *S2;
	double *c;

	// need to create space for SVD

	c = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(c))
    {
		My_Error("Cannot allocate c!!\n");
    }
	
	S2 = (double *)malloc((unsigned)(N * (sizeof(double))));
	if (!(S2))
    {
		My_Error("Cannot allocate S2!!\n");
    }

	A = Create_Matrix(2*rows,rows); 
	
	// copy from A_in to A
	for (i = 0; i < N; i++)
	{
		for ( j = 0; j < N; j++ )
		{
			A[i][j]=A_in[i][j];
		}
	}

	svd(A, S2, rows);
	
	for (i = rows-1; i>=0; i--)                              /* "invert" S2 */
    {
		if (S2[i]*MAX_SV_RATIO > S2[0]) 
		{                                           /* SV large enough? */
			S2[i] = 1.0/S2[i];
		}
		else
			S2[i] = 0.0;                                /* delete direction */
    }
	
	for (i=0; i< rows; i++) 
    {                                 /* compute invA = V*invS2*(US)' */
		for (j=0; j< rows; j++) 
		{
			for (tmp=0.0, k=0; k < rows; k++)
				tmp += A[i+rows][k]*A[j][k]*S2[k];
			c[j] = tmp;
		}
		for (j=0; j< rows; j++) 
			A[i+rows][j] = c[j];  /* copy "c" into "A" */
    }
	
	for (i=0; i < rows; i++)             /* compute w = invA*b */
    {
		for (tmp=0.0, j=0; j< rows; j++)
			tmp += A[i+rows][j]*b[j];
		w[i] = tmp;
    }

	// free up the space for SVD
	free(A[0]);
	free(A);
	free(c);
	free(S2);

}


double **Create_Matrix(int rows, int cols)
{
	int  i;
	double **x;
	
	x = (double **) malloc((size_t) rows*sizeof(double *));
	if ( x == NULL )
		My_Error("Cannot allocated x in  Create_Matrix\n");
	
	x[0] = (double *) malloc((size_t) rows*cols*sizeof(double));
	if ( x[0] == NULL )
		My_Error("Cannot allocated x[0] in  Create_Matrix\n");
	
	for (i=1; i<rows; i++) 
		x[i] = x[i-1]+cols;
	
	return x;
}

void My_Error(char error_text[])
{
	printf("\n\nError ..\n\n");
	printf("%s\n",error_text);
	printf("...now exiting...\n\n");
	exit(-1);
}