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

typedef struct
{
	float x, y, z;
}
Constraint;

double alpha = 2.0;
Constraint *constraint = NULL;
double computeRBF( unsigned , unsigned );

int main(int argc, char* argv[])
{
	unsigned constraintCount = 0, i = 0;
	unsigned constraintSize = sizeof(Constraint);

	//////////////////////// read constraint data ////////////////////////

	FILE* fp = fopen("C:/tmp2/PolySphereConstraints.txt", "r");
	if( !fp )
	{
		perror("Failed to open file");
		return -1;
	}

	while( !feof(fp) )
	{
		constraint = (Constraint*) realloc( constraint , constraintSize*(1 + i ) );
		fscanf( fp , "%f %f %f", &constraint[i].x, &constraint[i].y , &constraint[i].z );
		//printf( "%f %f %f\n", constraint[i].x, constraint[i].y , constraint[i].z );
		i++;
	}

	constraintCount = i;
	fclose(fp);

	////////////////////////  construct RBF matrix ////////////////////////

	unsigned matrixDim = constraintCount + 4;
	double *matrixData = new double[ matrixDim * matrixDim ]();

	for( unsigned i = 0; i < constraintCount; ++i )
	{
		for( unsigned j = 0; j < i; ++j )
		{
			matrixData[i*matrixDim + j] = matrixData[j*matrixDim + i] = computeRBF( j , i );
		}

		matrixData[i*matrixDim + i] = 1.0;	// i = j
		//matrixData[matrixDim*constraintCount + i] = matrixData[matrixDim*i + constraintCount] = 1.0;	

		// initialize C's 
		matrixData[matrixDim*(constraintCount + 0) + i ] = matrixData[matrixDim*i + (constraintCount + 0)] = constraint[i].x;
		matrixData[matrixDim*(constraintCount + 1) + i ] = matrixData[matrixDim*i + (constraintCount + 1)] = constraint[i].y;
		matrixData[matrixDim*(constraintCount + 2) + i ] = matrixData[matrixDim*i + (constraintCount + 2)] = constraint[i].z;
		matrixData[matrixDim*(constraintCount + 3) + i ] = matrixData[matrixDim*i + (constraintCount + 3)] = 1.0;
	}

	// initialize 4x4 zeros
	for( unsigned i = 0; i < 4; ++i )
	{
		for( unsigned j = 0; j < 4; ++j )
		{
			matrixData[(constraintCount + i)*matrixDim + (constraintCount + j)] = 0.0;		
		}
	}

	free(constraint);

	//////////////////////// write matrix to file ////////////////////////

	FILE* nfp = fopen("C:/tmp2/WendlandRBFMatrix.txt", "w" );
	for( unsigned i = 0; i < matrixDim; ++i )
	{
		for( unsigned j = 0; j < matrixDim; ++j )
		{
			fprintf( nfp , "%0.48f ", matrixData[i*matrixDim + j] );
		}
		fprintf( nfp , "\n" );
	}
	fclose(nfp);

	return 0;
}

///////

double computeRBF( unsigned i, unsigned j )
{
	Constraint &point1 = constraint[i];
	Constraint &point2 = constraint[j];

	double radius2 = ( point1.x - point2.x )*( point1.x - point2.x )  + ( point1.y - point2.y )*( point1.y - point2.y ) + ( point1.z - point2.z )*( point1.z - point2.z );
	double radius = sqrt(radius2);

	double r = radius/alpha;
	return ( r < 1.0 ) ? pow( (1 - r) , 4 ) * (4*r + 1) : 0;
}