// Urysohn.cpp : Defines the entry point for the console application.
//
#include <stdlib.h>
#include <memory.h>
#include <stdio.h>
#include <math.h>
#include <time.h>

//main method for Cholesky decomposition.
//input         n  size of matrix
//input/output  a  Symmetric positive def. matrix
//output        p  vector of resulting diag of a
//author:       <Vadum Kutsyy, kutsyy@hotmail.com>
void choldc1(int n, double** a, double* p) 
{
	int i,j,k;
    double sum;

	for( i = 0; i < n; i++ ) 
	{
		for( j = i; j < n; j++ ) 
		{
			sum = a[i][j];
            for( k = i - 1; k >= 0; k-- ) 
			{
				sum -= a[i][k] * a[j][k];
			}
            if(	i == j	) 
			{
				if(	sum <= 0 ) 
				{
					printf(" a is not positive definite!\n");
				}
                p[i] = sqrt(sum);
			}
            else 
			{
				a[j][i] = sum / p[i];
			}
		}
	 }
}

//Cholesky decomposition.
//input    n  size of matrix
//input    A  Symmetric positive def. matrix
//output   a  lower deomposed matrix
//uses     choldc1(int,MAT,VEC)
void choldc(int n, double** A, double** a) 
{
	int i,j;
	double* p = (double*)malloc( n * sizeof(double) );

    for( i = 0; i < n; i++ ) 
		for( j = 0; j < n; j++ ) 
			a[i][j] = A[i][j];

	choldc1(n, a, p);
    
	for( i = 0; i < n; i++ ) 
	{
		a[i][i] = p[i];
        for( j = i + 1; j < n; j++ ) 
		{
			a[i][j] = 0;
		}
	}

	if( p )
		free( p );
}

///////////////////////////////////////////////////////////////////
//solution for M * x = y
//M must be positive definite
void CholeskySolution(double** M, double* x, double* y, int size) 
{
	//declare matrix
	double** T = (double**)malloc( size * sizeof(double*) );
	for( int i = 0; i < size; ++i ) 
	{
		T[i] = (double*)malloc( size * sizeof( double ) );
	}

	//find lower triangular
	choldc(size, M, T);
	//make is symmetric
	for( int i = 0; i < size; ++i )
		for( int j = i+1; j < size; ++j )
			T[i][j] = T[j][i];

	double* v = (double*)malloc( size * sizeof( double ) );
	
	//solve lower triangular
	for( int i = 0; i < size; ++i) 
	{
		double sum = 0.0;
		for( int j = 0; j < i; ++j ) 
		{
			sum += T[i][j] * v[j];
		}
		v[i] = (y[i] - sum) / T[i][i];
	}

	//solve upper triangular
	for( int i = size - 1; i >= 0; --i) 
	{
		double sum = 0.0;
		for( int j = size - 1; j > i; --j ) 
		{
			sum += T[i][j] * x[j];
		}
		x[i] = (v[i] - sum) / T[i][i];
	}

	if( v )
		free( v);

	if( T )
	{
		for( int i = 0; i < size; ++i ) 
		{
			if(T[i])
				free(T[i]);
		}
	}
}

bool UrysohnIdentification(
	int** data, 
	double* result, 
	double** U, //this is for self test and reference only
	double** UI, 
	const int datarows,
	const int datacols,
	const int datamin,
	const int datamax) 
{
	if ((datamax - datamin) < 1)
		return false;

	const int range = datamax - datamin + 1;
	const int vectorsize = range * datacols;

	//declaration of data matrix
	double** M = (double**)malloc(vectorsize*sizeof(double*));
	if (!M)
		return false;
	for (int i=0; i<vectorsize; ++i) {
		M[i] = (double*)malloc(vectorsize*sizeof(double));
		if (!M[i])
			return false;
		memset(M[i], 0x00, vectorsize*sizeof(double));
	}

	double* V = (double*)malloc(vectorsize*sizeof(double));
	memset(V, 0x0, vectorsize*sizeof(double));

	int* position = (int*)malloc(datacols*sizeof(int));

	//population of data matrix
	for (int i=0; i<datarows; ++i) {
		for (int j=0; j<datacols; ++j) {
			position[j] = data[i][j] - datamin + range*j;
		}
		for (int k=0; k<datacols; ++k) {
			for (int m=0; m<datacols; ++m) {
				M[position[k]][position[m]] += 1.0;
			}
			V[position[k]] += result[i];
		}
	}

	//this is the self test
	double* testV = (double*)malloc(vectorsize*sizeof(double));
	memset(testV, 0x00, sizeof(double));
	
	double* s = (double*)malloc(vectorsize*sizeof(double));
	int counter = 0;
	for (int i=0; i<datacols; ++i) {
		for (int j=0; j<range; ++j) {
			s[counter++] = U[j][i];
		}
	}

	for (int i=0; i<vectorsize; ++i) {
		for (int j=0; j<vectorsize; ++j) {
			testV[i] += M[i][j]*s[j];
		}
	}

	if (s)
		free(s);

	double error = 0.0;
	for (int i=0; i<vectorsize; ++i) {
		error += fabs(testV[i] - V[i]);
	}
	printf("Self test completed, error is %f\n\n", error);

	if (testV)
		free(testV);
	//end of self test
	
	////////////////////////////////////////////////////////
	//Matrix and vecor is ready
	////////////////////////////////////////////////////////

	//make matrix non-degenerate
	double trace = 0.0;
	for (int k=0; k<vectorsize; ++k) {
		trace += M[k][k];
	}
	trace /= 10000.0;
	for (int k=0; k<vectorsize; ++k) {
		M[k][k] += trace;
	}

	//solve system
	double* solution = (double*)malloc(vectorsize*sizeof(double));
	memset(solution, 0x00, vectorsize*sizeof(double));
	CholeskySolution(M, solution, V, vectorsize);

	int cnt = 0;
	for (int i=0; i<datacols; ++i) {
		for (int j=0; j<range; ++j) {
			UI[j][i] = solution[cnt++];
		}
	}

	//release solution
	if (solution)
		free(solution);

	//release of position
	if (position)
		free(position);

	//vecor release
	if (V)
		free(V);

	//release of data matrix
	if (M) {
		for (int i=0; i<vectorsize; ++i) {
			if (M[i])
				free(M[i]);
		}
		free(M);
	}
	
	return true;
}

int main()
{
	const int datarows = 10000;
	const int datacols = 4;
	const int datamin  = 10;
	const int datamax  = 71;
	const int range = datamax-datamin+1;

	srand((unsigned)time(0));

	//data storage declaration
	int** data = (int**)malloc(datarows*sizeof(int*));
	for (int i=0; i<datarows; ++i) {
		data[i] = (int*)malloc(datacols*sizeof(int));
	}

	//operator declaration
	double** U  = (double**)malloc(range*sizeof(double*));
	for (int i=0; i<range; ++i) {
		U[i] = (double*)malloc(datacols*sizeof(double));
	}

	double* result = (double*)malloc(datarows*sizeof(double));
	memset(result, 0x00, datarows*sizeof(double));

	//data population
	for (int i=0; i<datarows; ++i) {
		for (int j=0; j<datacols; ++j) {
			data[i][j] = rand()%range + datamin;
		}
	}

	//operator population
	for (int i=0; i<range; ++i) {
		for (int j=0; j<datacols; ++j) {
			U[i][j] = double(rand()%20-10);
		}
	}

	//result calculation
	for (int i=0; i<datarows; ++i) {
		for (int j=0; j<datacols; ++j) {
			result[i] += U[data[i][j]-datamin][j];
		}
	}

	///////////////////////////////////////////////////////////////////
	//Data is ready
	///////////////////////////////////////////////////////////////////

	//identified operator declaration
	double** UI  = (double**)malloc(range*sizeof(double*));
	for (int i=0; i<range; ++i) {
		UI[i] = (double*)malloc(datacols*sizeof(double));
		memset(UI[i], 0x00, datacols*sizeof(double));
	}

	if (UrysohnIdentification(data, result, U, UI, datarows, datacols, datamin, datamax)) {

		//result output
		printf("Original and identified operators\n\n");
		for (int i=0; i<range; ++i) {
			for (int j=0; j<datacols; ++j) {
				printf("%8.2f", U[i][j]);
			}
			printf("    ||  ");
			for (int j=0; j<datacols; ++j) {
				printf("%8.2f", UI[i][j]);
			}
			printf("\n");
		}
		printf("\n\n");
	}
	else {
		printf("Failed to identify operator\n");
	}

	//calculate average error in data modeling
	double currenterror = 0.0;
	double totalerror   = 0.0;
	double totalvalue   = 0.0;
	for (int i=0; i<datarows; ++i) {
		currenterror = 0.0;
		for (int j=0; j<datacols; ++j) {
			currenterror += UI[data[i][j]-datamin][j];
		}
		totalerror += fabs(result[i] - currenterror);
		totalvalue += fabs(result[i]);
	}
	printf("Average error in data modeling %f on the average value %f\n\n", totalerror/double(datarows),
		totalvalue/double(datarows));
	//end of calculation of average error in data modeling 

	//identified operator release
	if (UI) {
		for (int i=0; i<range; ++i)
			if (UI[i])
				free(UI[i]);
		free(UI);
	}

	//result release
	if (result)
		free(result);

	//operator release
	if (U) {
		for (int i=0; i<range; ++i)
			if (U[i])
				free(U[i]);
		free(U);
	}

	//data release
	if (data) {
		for (int i=0; i<datarows; ++i)
			if (data[i])
				free(data[i]);
		free(data);
	}

	return 0;
}