#include <itpp/signal/sigfun.h>
#include <itpp/base/mat.h>
#include <itpp/base/algebra/inv.h>
#include <itpp/base/algebra/eigen.h>
#include <itpp/base/matfunc.h>
#include <itpp/base/random.h>
#include <itpp/base/specmat.h>
#include <itpp/stat/misc_stat.h>
#include <itpp/signal/window.h>
#include <itpp/signal/transforms.h>
#include <itpp/base/timing.h>
#include <itpp/base/itfile.h>
#include <itpp/srccode/audiofile.h>

//#include <itpp/base/itfile.h>
//#include <itpp/base/svec.h>
//#include <itpp/base/math/min_max.h>
//#include <itpp/base/algebra/svd.h>
//#include <itpp/base/math/trig_hyp.h>
//#include <itpp/base/sort.h>
//#include <itpp/signal/resampling.h>

using namespace itpp;

class cfastica
{
public:

	cfastica::cfastica(cmat nesmichane, double eps, bool smichat);		
	void symetricka_separace();
	void beleni();
	cmat fourier(cvec X, int velikostOkna);
	cmat komplexni_kovariance(cmat X);
	std::complex<double> ccov(cvec x1, cvec x2);

	double eps;				//parametr kontrastni funkce a2
	int n;					//pocet nezavislych komponent
	int m;					//pocet vzorku
	cmat A;					//mixujici matice komplexni
	cmat s;					//komplexni matice nezavislych komponent
	cmat separovane;
	cmat xold;				//pozorovane (smichane) smesi komplexnich signalu, nebelene
	cmat x;					//belene xold

	cvec d;					//eigenvalue
	cmat V;					//eigenvector
	cmat whiteningMatrix;	//belici matice	

	mat gWx;				//g(|w^Hx|^2), pomocny vypocet
	mat dgWx;				//g'(|w^Hx|^2), pomocny vypocet	

private:
};

cfastica::cfastica(cmat nesmichane, double eps, bool smichat = true)
{
	this->eps = eps;
	this->s = nesmichane;
	this->n = nesmichane.rows();
	this->m = nesmichane.cols();

	if(smichat)
	{
		this->A = randn_c(this->n, this->n);
		this->xold = this->A * this->s;
	}
	else
	{
		this->xold = this->s;
	}

	beleni();
}

void cfastica::beleni()
{
	cvec dx;				//eigenvalue kdyz je nehermitovska
	vec dx_hermit;			//eigenvalue kdyz je hermitovska
	cmat Vx;				//eigenvector
	cmat dxdiag;			//eigenvalues diagonalni matice
	cmat sqrtdxdiag;		//sqrt(inv(dx))
	cmat temp;
	cmat C_pomocna;

	C_pomocna = komplexni_kovariance(this->xold);

	if(C_pomocna.hermitian_transpose() == C_pomocna.transpose())	//neni hermitovska
	{
		eig(C_pomocna,dx,Vx);
	}
	else															//je hermitovska
	{
		eig_sym(C_pomocna,dx_hermit,Vx);
		dx = to_cvec(dx_hermit);
	}
	//podminka jestli je matice hermitovska

	dxdiag = diag(dx);

	temp = itpp::inv(dxdiag);

	sqrtdxdiag = zeros_c(dxdiag.rows(),dxdiag.cols());
	for(int i=0; i<sqrtdxdiag.rows(); i++)
		for(int j=0; j<sqrtdxdiag.cols(); j++)
			if(i==j)
				sqrtdxdiag(i,j) = std::pow(temp(i,j),0.5);

	this->whiteningMatrix = sqrtdxdiag * Vx.H();

	this->x = whiteningMatrix * this->xold;	
}

void cfastica::symetricka_separace()		//docasne Wrand a vraci cmat
{
	int maxcounter = 10;	//maximalni pocet iteraci
	int counter	= 0;		//poradi iterace
	cmat W;					//komplexni matice pro separaci
	cmat C;					//kovariancni matice x

	mat gWx;				//g(|w^Hx|^2), pomocny vypocet
	mat dgWx;				//g'(|w^Hx|^2), pomocny vypocet

	gWx.set_size(this->n, this->m);
	dgWx.set_size(this->n, this->m);

	itpp::randn_c(n,n,W);

	C = komplexni_kovariance(x);

	while ( counter < maxcounter)
	{		
		for (int i=0; i<n;i++)
		{
			vec temp, temp2, temp3, temp4, wtemp5, jednicky;
			cmat temp1;
			double meanVec2;
			mat wtemp3;

			mat jednicky2;

			cvec wtemp, wtemp6, meanVec1(this->n);
			cmat wtemp1, wtemp2, wtemp4;

			temp1 = W.get_col(i).hermitian_transpose() * x;
			temp2 = itpp::abs(temp1);
			temp3 = itpp::pow(temp2,2);

			temp4 = this->eps + temp3;

			jednicky = ones(temp4.size());
			temp = elem_div(jednicky,temp4);

			gWx.set_row(i,temp);

			temp = elem_div(-(jednicky),itpp::pow(temp4,2));

			dgWx.set_row(i,temp);

			/////////////////////////////
			/*
			W(:,j) = mean(x .* (ones(n,1)*conj(W(:,j)'*x)) .* (ones(n,1)*gWx(j,:)),2)
				- mean(gWx(j,:) + abs(W(:,j)'*x).^2 .* dgWx(j,:)) * W(:,j);
			*/
			/////////////////////////////

			wtemp2 = ones(this->n,1)*(conj(W.get_col(i).hermitian_transpose()*x));		
			
			mat pokusik(1,gWx.cols());
			pokusik.set_row(0,gWx.get_row(i));
			wtemp3 = ones(this->n,1)*pokusik;

			wtemp4 = elem_mult(x,wtemp2);

			wtemp4 = elem_mult(wtemp4,to_cmat(wtemp3));
			
			for(int j=0; j<this->n; j++)
			{
				meanVec1(j) = mean(wtemp4.get_row(j));
			}

			wtemp5 = elem_mult(temp3,(dgWx.get_row(i)));

			meanVec2 = mean(gWx.get_row(i) + wtemp5);

			wtemp6 = meanVec2*W.get_col(i);

			wtemp = meanVec1 - wtemp6;

			W.set_col(i,wtemp);
		}

		
		//this->pok = W.hermitian_transpose()*C*W;
		//eig(this->pok,d,V);	//docasne
		eig(W.hermitian_transpose()*C*W,d,V);
		

		cmat diagD = diag(d);

		cmat sqrtdiagD = zeros_c(diagD.rows(),diagD.cols());
		for(int i=0; i<sqrtdiagD.rows(); i++)
		{
			for(int j=0; j<sqrtdiagD.cols(); j++)
			{
				if(i==j)
				{
					sqrtdiagD(i,j) = std::pow(diagD(i,j),-0.5);
				}
			}
		}

		W = ((W * V) * sqrtdiagD) * V.hermitian_transpose();	//naka chyba,mozna sqrtdiad
		
		counter++;
	}

	separovane = W.H() * x;
	separovane /= 10;

}

cmat cfastica::komplexni_kovariance(cmat X)
{
	cmat C;
	int DimX = X.rows();

	C = zeros_c(DimX,DimX);
	for(int i=0; i<DimX; i++)
		for(int j=0; j<DimX; j++)
		{
			C(i,j) = ccov(X.get_row(i),X.get_row(j));			
		}

	return C;
}

std::complex<double> cfastica::ccov(cvec x1, cvec x2)
{
	int N = x1.size() - 1;
	//int N = x1.size();

	cvec x1c,x2c;

	double c_u12, c_v12, c_vu12, c_uv12;

	std::complex<double> c12;

	x1c = x1 - mean(x1);	//centrovane nahodne veliciny
	x2c = x2 - mean(x2);

	c_u12 = (elem_mult_sum(real(x1c),real(x2c)))/N;		//kovariance realnych casti
	c_v12 = (elem_mult_sum(imag(x1c),imag(x2c)))/N;		//kovariance imag. casti
	c_vu12 = (elem_mult_sum(imag(x1c),real(x2c)))/N;	//kriz. kovariance imag:real-1:2
	c_uv12 = (elem_mult_sum(real(x1c),imag(x2c)))/N;	//kriz. kovariance real:imag-1:2

	std::complex<double> temp(c_u12 + c_v12,c_vu12 - c_uv12);
	c12 = temp;

	return c12;
}

cmat cfastica::fourier(cvec X, int velikostOkna)
{
	int prekryvOkna = velikostOkna / 2;
	int delkaSignalu = X.length();
	int pocetSegmentu;

	cmat segmentovanySigFFT;
	mat okno;
	cmat M_x1, M_x2, M_x, temp;

	pocetSegmentu = 1 + (delkaSignalu-velikostOkna) / prekryvOkna;
	delkaSignalu = velikostOkna + (pocetSegmentu-1) * prekryvOkna;

	X.set_size(delkaSignalu,true);
	okno = diag(hann(velikostOkna));

	M_x1 = reshape(X, prekryvOkna, delkaSignalu / prekryvOkna);
	//vsechny radky,ale odstranen prvni sloupec
	M_x2 = M_x1;
	M_x2.del_col(0);
	M_x2.append_col(zeros_c(prekryvOkna));
	M_x = concat_vertical(M_x1, M_x2);

	temp = okno * M_x;

	segmentovanySigFFT.set_size(temp.rows(), temp.cols());

	for(int i=0; i<segmentovanySigFFT.cols(); i++)
		segmentovanySigFFT.set_col(i,itpp::fft(temp.get_col(i)));

	return segmentovanySigFFT;
}