/*
 *  Copyright 2002 by Soos, Antal
 *  All rights reserved. Property of Soos, Antal.
 *  Restricted rights to use, duplicate or disclose this code are
 *  granted through contract.  
 */
/***************************************************************************/
/*                                                                         */
/*     sysIdent . cpp	                                                   */
/*                                                                         */
/*     System Identification - Common Procedures.			   */
/*                                                                         */
/*                                                                         */
/***************************************************************************/
// Include files definition:

#include "sysIdent.hpp"


//-----------------------------------------------------------------------------
// Declaration of the sysID object
//-----------------------------------------------------------------------------

sysid::sysid(void)
{
  
    int i;
 // Memory alocation for the matrices:
	//matrix_alloc(&theta, m_s+m_a+m_b+m_c, 1);	// theta:
	//float theta_v[ARMAX_dim+2];
	theta.mtx = theta_v;
	theta.row = ARMAX_dim;
	theta.col = 1;    
	theta.dim = ARMAX_dim;
    theta_v[0] = SATRT_OF_VECTOR;
    theta_v[ARMAX_dim+1] = END_OF_VECTOR;
   	for(i=1;i<=ARMAX_dim;i++)  theta_v[i]=(float)0.0;

	//matrix_alloc(&psy, m_s+m_a+m_b+m_c, 1);		// psy:
	//float psy_v[ARMAX_dim+2];
	psy.mtx = psy_v;
	psy.row = ARMAX_dim;
	psy.col = 1;    
	psy.dim = ARMAX_dim;
    psy_v[0] = SATRT_OF_VECTOR;
    psy_v[ARMAX_dim+1] = END_OF_VECTOR;
	for(i=1;i<=ARMAX_dim;i++)  psy_v[i]=(float)0.0;
	if(ARMAX_DC != 0) psy_v[1] = (float) 1.0;

//	print_matrix(&theta,"Memory for theta is alocated");
//	print_matrix(&psy,"Memory for psy is alocated");

// Initialise paramethers:
	count_n = NxT;
	v_qume =  (float)0.0;
	v_maxd =  (float)0.0;
	performance_index = (float) 0.0;

}

//-----------------------------------------------------------------------------
sysid::~sysid()
{

 }

//-----------------------------------------------------------------------------
void sysid::get_theta(Matrix *gtheta)
{ // Copy the theta matrix from private space to public space
  int i ;
 
   gtheta->row = theta.row;
   gtheta->col = 1; // It is a vector
	  
   if(theta.dim > gtheta->dim)  printlog("DIMENSION_EROOR in get_theta");

   for (i=1;i<=theta.row;i++)  gtheta->mtx[i] = theta.mtx[i];
}

//-----------------------------------------------------------------------------
void sysid::set_theta(Matrix *gtheta)
{ // Copy the theta matrix from private space to public space
  int i ;
 
   theta.row = gtheta->row ;
   theta.col = 1; // It is a vector
	  
   for (i=1;i<=theta.row;i++)  theta.mtx[i] = gtheta->mtx[i];
}

//-----------------------------------------------------------------------------

void sysid::update_psy(float y_n, float u_n, float v_n)
{ // Update the observation vector "psy"
 	
 for(int i=psy.row;i>1;i--) psy.mtx[i] = psy.mtx[i-1]; 

 if(ARMAX_a != 0) psy.mtx[ARMAX_DC+1] = -y_n;
 if(ARMAX_b != 0) psy.mtx[ARMAX_DC+ARMAX_a+1] = u_n;
 if(ARMAX_c != 0) psy.mtx[ARMAX_DC+ARMAX_a+ARMAX_b+1] = v_n;
}
//-----------------------------------------------------------------------------

void sysid::update_psy(float y_n, float v_n)
{ // Update the observation vector "psy" without the u_n update
 
 for(int i=psy.row;i>1;i--) psy.mtx[i] = psy.mtx[i-1]; 

  if(ARMAX_a != 0) psy.mtx[ARMAX_DC+1] = -y_n;
  //psy.mtx[ARMAX_DC+ARMAX_a+1] = u_n <- it will be updated in nex cycle
  if(ARMAX_c != 0) psy.mtx[ARMAX_DC+ARMAX_a+ARMAX_b+1] = v_n;
}
//-----------------------------------------------------------------------------

void sysid::update_psy(float u_n_1)
{ // Update the observation vector "psy"
 
 psy.mtx[ARMAX_DC+ARMAX_a+1] = u_n_1;
 
}
//-----------------------------------------------------------------------------
void sysid::get_psy(Matrix *gpsy)
{// Copy the psy matrix from private space to public space
	int i ;
	gpsy->row = psy.row;
	gpsy->col = 1; // It is a vector
	  
	if(psy.dim > gpsy->dim)  printlog("DIMENSION_EROOR in get_psy");

	for (i=1;i<=psy.row;i++)  gpsy->mtx[i] = psy.mtx[i];
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------


int sysid::update_stat(void)
{

	if (count_n == NxT) // if new sesion initialixe:
	{
	        // Zero initialization: v statistics
		v_maxd =  v_n;	// Max calculation initialization
		v_mind =  v_n;  // Min calculation initialization
		v_qume_sum = (float)0.0 ;
		
	}

	if( count_n > 1)
	{
		count_n--;
		
		// Statistic for the residuals: v
		v_qume_sum += (v_n * v_n);
		if (v_maxd < v_n) v_maxd = v_n;
		if (v_mind > v_n) v_mind = v_n;

		return 0;
	}

	else if( count_n == 1)	// the NxT lengt is achived meke the data redy:
	{
		count_n = 0; //calculate for the last sample in this period:
		
		// Statistic for the residuals: v
		v_qume_sum += (v_n * v_n);
		if (v_maxd < v_n) v_maxd = v_n;
		if (v_mind > v_n) v_mind = v_n;

	 	//Calculate the final statistics:
		v_qume = (float) (v_qume_sum * inv_NxT); // <- quadratic mean
		v_mami = v_maxd - v_mind ; // <- error range

// <-- Memory function
#define MEMORYS 0.3333
#define NEWSEL (1.0-MEMORYS)

		performance_index= (MEMORYS*performance_index) + (NEWSEL * v_mami * v_qume ); // <-- Memory function!!!
		
		stable_coeficients = stable(); // Flag for stability if 1, unstable if 0
				
		return 1;
	}

	else	return -1;	
}

//-----------------------------------------------------------------------------
float sysid::get_v_n(void)   //  residual
{
	return v_n*v_n;
}
//-----------------------------------------------------------------------------
float sysid::get_v_qume(void)	// quadratic nean (moving averadge) = mean squared value
{
	return v_qume;
}
//-----------------------------------------------------------------------------
float sysid::get_v_maxd(void)	// maximum deviation from the moving averadge
{
	return v_maxd;
}
//-----------------------------------------------------------------------------
float sysid::get_v_mind(void)	// minimum deviation from the moving averadge
{
	return v_mind;
}
//-----------------------------------------------------------------------------
float sysid::get_v_mami(void)	// error range: v_mami = v_maxd - v_mind 
{
	return v_mami;
}
//-----------------------------------------------------------------------------
float sysid::get_performance_index(void) // generalized performance index
{
	return performance_index;
}
//-----------------------------------------------------------------------------
int sysid::get_stable_coeficients(void) // stable_coeficients flag 1/0
{
	return stable_coeficients;
}
//-----------------------------------------------------------------------------
//----------------------------------------------------------------------------- 
//----------------------------------------------------------------------------- 
// 
//		Schor-Cohn stability test 
//                if the estimated model is stable the code function will
//		  return 1 if unstable the return value is 0	
//               stable -> 1, unstable -> 0
// Usage:	 
//			test = schur_cohn(&theta, &temp1, &temp2,MMAC_1,MMAC_a);   
// Dependences: - structure Matrix	-> MatrixCal.hpp 
//-----------------------------------------------------------------------------      
int sysid::stable(void)  
{ 

     return 1;
    int i,j;
    float ftemp;
/* theta <=> [ DC, a1, a2, a3,...a_m_a, b1,...] if m_s = 1
   theta <=> [ a1, a2, a3,...a_m_a, b1,...]     if m_s = 0 
   where:
   psy = [1,-y_n-1,...-y_n-m_a,u_n-1,...u_n-m_b,v_n-1,...v_n-m_c] 
*/
		// Schor-Cohn stability test:
	//Va = vectora( 1, m_a+1); /* Temporary vector for stability calculation*/
	float Va[ARMAX_a+2];

	//Vb = vectora( 1, m_a+1); /* Temporary vector for stability calculation*/
	float Vb[ARMAX_a+2];


/*
    Backward Levinson (step-down) recursion Matlab code: 
    function gamma=atog(a) 
    % function gamma=atog(a) 
    % 
    % Step-down recursion, page 236 of Hayes text 
    % 
    a=a(:); 
    p=length(a); 
    a=a(2:p)/a(1); 
    gamma(p-1)=a(p-1); 
    for j=p-1:-1:2 a=(a(1:j-1)-gamma(j)*flipud(conj(a(1:j-1))))./(1-abs(gamma(j))^2); 
    gamma(j-1)=a(j-1); 
    end 

    An interesting aside is that the backward Levinson recursion, when used to 
    determine filter stability, is called the ihSchur-CohnlD stability test. 
    All of the reflection coefficients must be less than one in magnitude 
    for the filter to be stable, which is easily checked in the algorithm. 
    Inverse Levinson-Durbin recursions
*/
    
    ftemp = theta.mtx[ARMAX_DC+1]; /* Normalization parameter */ 
          /* Normalization of the parameters: */
    for(i=1;i < ARMAX_a;i++) Vb[i] = Va[i] = (theta.mtx[ARMAX_DC+i+1]/ ftemp) ;
	
    ftemp = Va[ARMAX_a-1];   // <- gamma_1
    
    if(fabs(ftemp) > 1) return 0;     // stability tests

    for(j=ARMAX_a-1;j>=2;j--)
    {
	for(i=1;i<=j;i++)     Va[i] = Va[i] - (ftemp * Vb[j-i]);

	ftemp = 1 - (ftemp * ftemp );

	for(i=1;i<=j-1;i++)   Va[i] = Vb[i] = Va[i]/ftemp;    
	
	ftemp = Va[j-1];  // <- gamma_i
	
	if(fabs(ftemp) > 1) return 0;  // stability tests
	    
    }
					       
    return 1; // system model is stable all |gamma_i| < 1
}

//-----------------------------------------------------------------------------

//-----------------------------------------------------------------------------