// KMathematica.cpp: implementation of the KMathematica class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Calibrtwocama.h"
#include "KMathematica.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

KMathematica::KMathematica()
{

}

KMathematica::~KMathematica()
{

}

// returns a list of all possible permutations of a given array
int ** KMathematica::PermutationMatrix(int *arr, int n)
{
	int ** out  = NULL;
	int ** temp = NULL;

	int i,j,k,tS,missed,prime;
	
	int size = Permutation(n,n);

	// allocate memory for output
	out = new int*[size]; 
	for( i=0; i<size; i++) 
		out[i] = new int[n];

	int * tarr = new int[n-1];

	tS = Permutation(n-1,n-1);

	if( n == 1 ) 
	{
		out[0][0] = arr[0];
		delete []tarr; tarr = NULL;
		return out;
	}

	for( i=0; i<n; i++)
	{
		// take n-1 elements of the array "arr"
		k = 0;
		for( j=0; j<n; j++)
		{
			if( j == i )
			{
				missed = arr[j];
				continue;
			}

			tarr[k] = arr[j];
			k++;
		}

		temp = PermutationMatrix(tarr, n-1); // compute n-1 permutation
		
		for( j=0; j<tS; j++ )
		{
			prime = j+i*tS;
			out[prime][0] = missed;

			for( k=1; k<n; k++)
			{
				out[prime][k] = temp[j][k-1];
			}
		}
		// delete temp;
		for(j=0;j<tS;j++)
			delete temp[j];
		delete []temp;
	}

	delete []tarr; tarr = NULL;
	return out;
}

long KMathematica::Factorial(int n)
{
	long ret=1;
	
	for( int i=n; i>0; i-- )
	{
		ret *= i;
	}

	return ret;
}

int * KMathematica::Sort(int *arr, int n)
{
	return 0;
}

int KMathematica::Place(int number, int *arr, int n)
{
	return -1;
}

long KMathematica::Permutation(int n, int m)
{
	return Combination(n,m)*Factorial(m);
}

long KMathematica::Combination(int n, int m)
{
	long ret=1;

	if( m > n/2 )
		m = n-m;
	
	for(int i=m; i>0; i-- )
	{
		ret *= n;
		n--;
	}

	for( i=m; i>0; i-- )
	{
		ret /= i;
	}

	return ret;
}

double KMathematica::FindOrientation(CvPoint2D32f *points, int n)
{
	float a = 0;
	float b = 0;
	float c = 0;

	int pointNumber = n;

	CvPoint2D32f center;
	center = FindCenterofMass( points, n );

	for( int i=0; i<n; i++ ) {
		a +=   (points[i].x-center.x+1)*(points[i].x-center.x+1);
		b += 2*(points[i].x-center.x+1)*(points[i].y-center.y+1);
		c +=   (points[i].y-center.y+1)*(points[i].y-center.y+1);
	}

	double dOrientationAngle = (double)atan2(b,(a-c))/2; 

	return dOrientationAngle/PI*180;
}

CvPoint2D32f KMathematica::FindCenterofMass(CvPoint2D32f *points, int n)
{
	CvPoint2D32f center;

	center.x = 0;
	center.y = 0;

	for(int i=0; i<n; i++) {
		
		center.x += points[i].x;
		center.y += points[i].y;
	}

	if( n != 0 ) {
		center.x /=  n;
		center.y /=  n;
	}

	return center;
}



double KMathematica::MapAnglePi2mPi(double theta)
{
   if( theta<-PI ) theta += 2*PI;
   if ( theta>PI ) theta -= 2*PI;
   return theta;
}

// returns a list of all possible (n,m) combinations of a given array
int ** KMathematica::CombinationMatrix(int *arr, int n, int m)
{
	int i,j,k,tS,missed,prime;
	int ** out = NULL;
	int ** temp = NULL;

	if( n < m ) return NULL;

	int size = Combination(n,m);

	// allocate memory for output
	out = new int*[size]; 
	for( i=0; i<size; i++) 
		out[i] = new int[m];

	int * tarr = new int[n];

	if( n == 1 ) 
	{
		out[0][0] = arr[0];
		delete []tarr; tarr = NULL;
		return out;
	}

	prime = 0;

	for( i=0; i<n; i++)
	{
		// take n-1 elements of the array "arr"
		k = 0;
		for( j=0; j<n; j++)
		{
			if( j <= i )
			{
				missed = arr[j];
				continue;
			}

			tarr[k] = arr[j];
			k++;
		}

		if( k == 0 )
		{
			delete []tarr; tarr = NULL;
			return out;
		}

		tS = Combination(k,m-1);

		if( m-1 > 0 )
		{
			temp = CombinationMatrix(tarr, k ,m-1); // compute n-1 by m-1 combination
			
			if( temp == NULL )
			{
				continue;
			}

			for( j=0; j<tS; j++ )
			{
				out[prime][0] = missed;

				for( k=1; k<m; k++)
				{
					out[prime][k] = temp[j][k-1];
				}
				prime++;
			}	
			for(j=0;j<tS;j++)
				delete temp[j];
			delete []temp;
		}
		else
		{
			temp = NULL;
			for( j=0; j<n; j++ )
			{
				out[prime][0] = arr[j];
				prime++;
			}
			delete []tarr; tarr = NULL;
			return out;
		}
	}

	delete []tarr; tarr = NULL;
	return out;
}

int KMathematica::Modulus(int n, int m)
{
	int ret = n%m;

	return ret<0 ? ret+m : ret;
}