#include "StdAfx.h"
#include "discrete.h"

discrete::discrete()
{

}

discrete::discrete(array<double>^ _num, array<double>^ _den, float Tvz)
{
	create_transfer_function(_num, _den, Tvz);
}
void discrete::create_transfer_function(array<double>^ _num, array<double>^ _den, float Tvz)
{
	mat Fz;

	if (x == nullptr)
		x = gcnew array<double>(_den->Length-1);
	else
	{
		clear_states();
		System::Array::Resize(x, _den->Length-1);
	}
/*	for(int i=0; i<_den->Length-1;i++)
	{
		x[i] = 0;
	}*/

	Fz = c2d(array2mat(_num, 0),array2mat(_den, 0), Tvz);
	numZ = mat2array(Fz,0,0);
	denZ = mat2array(Fz,1,0);
}

void discrete::clear_states(void)
{
	System::Array::Clear(x,0,x->Length);
}

array<double>^ discrete::mat2array(mat _M, int rowcol, bool how) //prevedeni vektoru typu mat na array,rowcol radek/sloupec ze kteryho se prvky berou, how = 1 radkovej, how = 0 sloupcovej
{
	array<double>^ buf;
	if (how)//sloupcovej
	{
		buf = gcnew array<double>(_M.n_rows);
		for(int i=0;i<buf->Length;i++)
			buf[i] = _M(i,rowcol);
	}
	else //radkovej
	{	
		buf = gcnew array<double>(_M.n_cols);
		for(int i=0;i<buf->Length;i++)
			buf[i] = _M(rowcol,i);
	}
	return buf;
}

mat discrete::array2mat(array<double>^ pole, bool how)
{
	mat M;
	if (how)//sloupcovej
	{
		M.zeros(pole->Length, 1);
		for(int i=0;i<pole->Length;i++)
			M(i,0) = pole[i];
	}
	else //radkovej
	{	
		M.zeros(1, pole->Length);
		for(int i=0;i<pole->Length;i++)
			M(0,i) = pole[i];
	}
	return M;
}

mat discrete::matT(mat _R, int _rad)
{
	mat Rn, T(_rad, _R.n_cols);
	Rn.eye(_R.n_cols, _R.n_cols); //muzu to udelat takhlepac jsou ctvercovy
	for (int n=0; n<_rad; n++)
	{
		T.row(n) = Rn.row(0);
		if (n == 0)
			Rn = _R;
		else
			Rn *= _R;
	}
	return T;
}
mat discrete::matW(mat _T, mat _S, int _rad)
{
	mat W;
	W.zeros(_rad, _rad);
	for (int n=0; n<_rad; n++)
	{
		for (int k=0; k<_rad; k++)
		{
			if(k >= n)
				W(n,k) = 0;
			else
				W(n,k) = dot(_T.row(n-k-1),_S);
		}
	}
	return W;
}

unsigned int discrete::factorial(unsigned int cislo)
{
	unsigned int p = 1;
	if (cislo > 1)
	{
		for (unsigned int i=2; i<=cislo; i++)
			p *= i;
	}
	return p;
}

mat discrete::c2d (mat num, mat den, float Tvz)
{
	double del;
	int rad, a, b;
	mat A, B, R, I, AT, S, W, T, Tr, TW, Wr, vysl;
	vysl.zeros(1,1);
	a = num.n_elem;
	b = den.n_elem;
	if (den.n_elem > num.n_elem)       
	{
		del = den[0];	//musim najit cislo, kterym budu celej prenos delit, aby byla ve jm. na 1. miste 1
		for (unsigned int i=1; i<den.n_elem; i++)
		{
			den[i-1] = -den[i]/del; //prvni prvek v mat A neni, tak ho tu mazu
		}
		den.reshape(1, den.n_elem-1);

		for (unsigned int i=0; i<num.n_elem; i++)
		{
			num[i] = num[i]/del;
		}
		rad = den.n_elem;
		A.zeros(rad, rad);
		
		if (rad > 1)
			A.submat(0,1,A.n_cols-2,A.n_cols-1) = eye(rad-1,rad-1);

		A.col(0) = trans(den);
		B = trans(num);
		
		R = eye(rad, rad);
		I = eye(rad, rad);
		AT = A*Tvz;
		for (int i=1; i<20; i++)
		{
			R += AT/factorial(i);
			I += AT/factorial(i+1);
			AT *= A*Tvz;
		}
		I = Tvz * I;
		S = I * B;
		T = matT(R, rad);
		W = matW(T, S, rad);
		Tr = matT(R, rad+1);
		Wr = matW(Tr, S, rad+1);
		T = inv(T);
		vysl.set_size(2,rad+1);
		vysl.fill(0);
		vysl(1,0) = 1; 
		for (int l=0; l<rad; l++)
		{
			for(int j=0; j<rad; j++)
				vysl(1,rad-l) += - Tr(rad,j)*T(j,l);
		}
		TW = T*W;	//tady uz je T inverzni
		for (int l=0; l<rad; l++)
		{
			for(int j=0; j<rad; j++)
				vysl(0,rad-l) += - Tr(rad,j)*TW(j,l);
			vysl(0,rad-l) += Wr(rad,l);
		}
	}
	return vysl;
}

double discrete::output(mat _Fz, double _u)
{
	mat A, B, C, y, xx;
	int rad;

	rad = _Fz.n_cols-1;
	xx.zeros(rad,1);
	xx = array2mat(x,1);

	A.zeros(rad, rad);
	if (rad > 1)
		A.submat(0,1,A.n_cols-2,A.n_cols-1) = eye(rad-1,rad-1); //nad hl. diagonalou 1
	
	A.row(rad-1) = -fliplr(_Fz.submat(1,1,1,_Fz.n_cols-1));
	B.zeros(rad,1);
	B(rad-1,0) = 1;
	C = fliplr(_Fz.submat(0,1,0,_Fz.n_cols-1));

	y = C * xx;
	xx = A * xx + B * _u;

	x = mat2array(xx,0,1);
	return y(0);
}

double discrete::output(double _u) //vstup pouze rizeni, z poly opet vytvori Fz
{
	mat Fz;
	Fz.zeros(2,denZ->Length);
	Fz.row(0) = array2mat(numZ,0);
	Fz.row(1) = array2mat(denZ,0);
	return output(Fz, _u);
}
