/*
 *  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"
#include "MatrixCal.hpp"

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

sysid::sysid(int is, int ia, int ib, int ic)
{
  int i;
 // Initialisation of the model dimensions:
	m_s = is;
	m_a = ia;
	m_b = ib;
	m_c = ic;
	m_dim = is+ia+ib+ic;

 // Memory alocation for the matrices:

	matrix_alloc(&theta, m_s+m_a+m_b+m_c, 1);	// theta:
	matrix_alloc(&psy, m_s+m_a+m_b+m_c, 1);		// psy:

	if(m_s != 0) psy.mtx[1][1] = (float) 1.0;
//	print_matrix(&theta,"Memory for theta is alocated");
//	print_matrix(&psy,"Memory for psy is alocated");




#if FLT_0

#elif FLT_1    // differentiation
	stage_memory1  = 0;
#elif FLT_2    // differentiation
	stage_memory1=0.0;
	stage_memory2=0.0;
#elif FLT_3    // differentiation
	u_nn1 = u_nn2 = (float)0;
	  u_nn3 = (float)0;
#elif FLT_4    // differentiation
	  stage_memory1=0.0;
	  stage_memory2=0.0;
#elif FLT_5
	//The DC remouveDC(float stage_input), state initialization:
	fio1 = fio2 = fio3 = fio4 = 0.0;
	for(i=0;i<xNUM_STAGES;i++) DLY[i] = 0.0;
#elif FLT_6
	//The DC remouveDC(float stage_input), state initialization:

#endif








#if STATISTIC_IS_IMPLEMENTED //------------------------------------------------
#if NO_THETA_STATISTICS
	// Alocate memory for the neccessery matrices in statistic calculation:
	matrix_alloc(&theta_moav, m_s+m_a+m_b+m_c, 1);	// theta_mova = 0:
	matrix_alloc(&theta_qume, m_s+m_a+m_b+m_c, 1);	// theta_qume = 0:
	matrix_alloc(&theta_vari, m_s+m_a+m_b+m_c, 1);	// theta_vari = 0:
	matrix_alloc(&theta_maxd, m_s+m_a+m_b+m_c, 1);	// theta_maxd = 0:
	matrix_alloc(&theta_mind, m_s+m_a+m_b+m_c, 1);	// theta_maxd = 0:
#endif //#IF NO_THETA_STATISTICS
// Initialise paramethers:
	count_n = NxT;
	inv_NxT = (float)1/NxT;
	v_moav =  (float)0.0;
	v_qume =  (float)0.0;
	v_vari =  (float)0.0;
	v_maxd =  (float)0.0;

	performance_index = (float) 0.0;
	// Schor-Cohn stability test:
	Va = vectora( 1, m_a+1); /* Temporary vector for stability calculation*/
	Vb = vectora( 1, m_a+1); /* Temporary vector for stability calculation*/

#endif //STATISTIC_IS_IMPLEMENTED ---------------------------------------------

}

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

 // Remouve the alocation from the heep
	matrix_delete(&theta);
	matrix_delete(&psy);

#if STATISTIC_IS_IMPLEMENTED //------------------------------------------------
	free_vector(Va, 1, m_a+1);
	free_vector(Vb, 1, m_a+1);
#if NO_THETA_STATISTICS 
       matrix_delete(&theta_moav);// moving averadge of the estimated paramethers
       matrix_delete(&theta_qume);// quadratic nean (moving averadge) 
       matrix_delete(&theta_vari);// moving varianc of the estimated paramethers
       matrix_delete(&theta_maxd);// Maximum deviation from the moving averadge
#endif //#IF NO_THETA_STATISTICS
#endif //STATISTIC_IS_IMPLEMENTED ---------------------------------------------
	
//	print_matrix(&theta,"theta is deleted");
//	print_matrix(&psy,"psy is deleted");
    
}

//-----------------------------------------------------------------------------
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(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

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


}

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

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][1] = psy.mtx[i-1][1]; 

 if(m_a != 0) psy.mtx[m_s+1][1] = -y_n;
 if(m_b != 0) psy.mtx[m_s+m_a+1][1] = u_n;
 if(m_c != 0) psy.mtx[m_s+m_a+m_b+1][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][1] = psy.mtx[i-1][1]; 

  if(m_a != 0) psy.mtx[m_s+1][1] = -y_n;
  //psy.mtx[m_s+m_a+1][1] = u_n <- it will be updated in nex cycle
  if(m_c != 0) psy.mtx[m_s+m_a+m_b+1][1] = v_n;

}
//-----------------------------------------------------------------------------

void sysid::update_psy(float u_n_1)
{ // Update the observation vector "psy"
 
 psy.mtx[m_s+m_a+1][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(gpsy->row > gpsy->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

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

}



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


float sysid::DCfilter(float stage_input)
{   /* THE APROPRIARE FILTER RUTINE : */
 
    double temp, stage_output;
    extern double   T_sample ;
    int i;
    float ftemp;


#if FLT_0   //No filter
    //--------------------------------------------------------------------------- 
   stage_output  =   stage_input;
#elif FLT_1    // differentiation
   //---------------------------------------------------------------------------- 
   stage_output  =  (stage_input - stage_memory1)/ T_sample;
   stage_memory1 = stage_input;
#elif  FLT_2  //  double differentiator
   //----------------------------------------------------------------------------- 
  
    stage_output  = (stage_input - (2 * stage_memory1) + stage_memory2 ) / T_sample ; 
    stage_memory2 = stage_memory1;
    stage_memory1 = stage_input; 
      stage_input  = stage_output ; 


#elif FLT_3   // 
    //---------------------------------------------------------------------------- 

      /*   ftemp =  (stage_input - u_nn1)/ T_sample ;
   u_nn1 = stage_input;
   stage_input =  ftemp; */

   //       Filter Generated from File: LPF_2.IIR
              
#define   A0   7.74711328234E-02
#define   A1   1.54942265647E-01                
#define   A2   7.74711328234E-02

  /* denominator coefficients */
#define   B00   -9.96006240057E-01
#define   B01    2.70813958843E-01
#define   B10   -1.26649652048E+00
#define   B11    6.15935113979E-01

  /* actual filter routine */    

        temp = stage_input - B00 * DLY0[0] - B01 * DLY0[1];
	stage_input = A0 * temp + A1 * DLY0[0] + A2 * DLY0[1];

        DLY0[1] = DLY0[0];
        DLY0[0] = temp;

	temp = stage_input - B10 * DLY0[2] - B11 * DLY0[3];
        stage_output = A0 * temp + A1 * DLY0[2] + A2 * DLY0[3];
	//stage_input  =  A0 * temp + A1 * DLY0[2] + A2 * DLY0[3];

        DLY0[3] = DLY0[2];
        DLY0[2] = temp;
       
#elif FLT_4   // Using a low pass filter
   //----------------------------------------------------------------------------- 
#define A1   1.37009246195E-0001
#define A2   1.37009246195E-001  
#define B1  -7.25981507610E-0001 


	/*stage_output  =  (stage_input - stage_memory1)  / T_sample;
	stage_memory1 = stage_input;
	stage_input =  stage_output ;*/
	
	  /*	stage_output  = (stage_input - (2 * stage_memory1) + stage_memory2 ) / T_sample ; 
	stage_memory2 = stage_memory1;
	stage_memory1 = stage_input;
   	stage_input  = stage_output ;  */


	 temp = stage_input  - B1 * stage_memory11 ;
	stage_output  = A1* temp + A2 * stage_memory11;
        stage_memory11  = temp;
        stage_input  = stage_output ;  


   temp = stage_input  - B1 * stage_memory12 ;
   stage_output  = A1* temp + A2 * stage_memory12;
     
   stage_memory12  = temp;
   /*
   fio1 +=  stage_output;  // integrating the system output (without DC)
   stage_output = fio1;   */

#elif FLT_5   
   //---------------------------------------------------------------------------- 
   //float  A[xNUM_STAGES] = {1.0, 0.4, 0.3, 0.2, 0.1, 0.1, 0.08, 0.05};
 //float DLY[NUM_STAGES];  /* delay storage elements (z-shifts) */
   float  A[xNUM_STAGES] = {1.0, 0.2, 0.01, 0.01, .05, 0.02, 0.0, 0.0};

    for (i=1; i<xNUM_STAGES; i++) DLY[i] = DLY[i-1];
    DLY[0] = stage_input;

    stage_output =0;
    for (i=0; i<xNUM_STAGES; i++) stage_output += DLY[i]*A[i] ;
       
    //--------------------------------------------------------------------------
#elif FLT_6
#define RO (1.0 - 0.2) //0.2

    stage_output  =  (stage_input - stage_memory1) + RO * stage_memory2;
    stage_memory1 = stage_input;
    stage_memory2 =  stage_output ;
    

        fio1 = fio2 ;   
        fio2 = fio3 ;
        fio3 = fio4 ;
        fio4 = stage_output ;
	//stage_output = (fio1 + fio2  )/2.0;
	stage_output = (fio1 + fio2 + fio3 )/3.0;
	//stage_output = (fio1 + fio2 + fio3 + fio4 )/4.0;
	
   //-----------------------------------------------------------------------------
#endif

     return stage_output;
}
//-----------------------------------------------------------------------------



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



//-----------------------------------------------------------------------------
#if STATISTIC_IS_IMPLEMENTED //------------------------------------------------

int sysid::update_stat(void)
{

	if (count_n == NxT) // if new sesion initialixe:
	{
	        // Zero initialization: v statistics
		v_moav_sum =  (float)0.0;
		//v_qume =  (float)0.0;
		//v_vari =  (float)0.0;
		v_maxd =  v_n;	// Max calculation initialization
		v_mind =  v_n;  // Min calculation initialization
		v_qume_sum = (float)0.0 ;
		
#if NO_THETA_STATISTICS 	// Zero initialization: theta statistics
		zeromat(&theta_moav);
		zeromat(&theta_qume);
		zeromat(&theta_vari);
		
		copymat(&theta_maxd, &theta);  // Max calculation initialization
		copymat(&theta_mind, &theta);  // Min calculation initialization
		// printf("\n\n\n ======================================="); 
#endif //#IF NO_THETA_STATISTICS

	}

	if( count_n > 1)
	{
		count_n--;
		

		// Statistic for the residuals: v
		v_moav_sum += v_n;
		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;


#if NO_THETA_STATISTICS	// Statistic for the estimated paramethers:
		addmat(&theta_moav, &theta_moav, &theta);
		
		squaremat(&theta_vari, &theta);  //theta_vari used as temporary matrix
		addmat(&theta_qume, &theta_qume, &theta_vari);
		
		maxmatel(&theta_maxd, &theta);  // <- Maximum element selection 
		minmatel(&theta_mind, &theta);	// <- Minimum element selection
#endif //#IF NO_THETA_STATISTICS

		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_moav_sum += v_n;
		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;

#if NO_THETA_STATISTICS
		// Statistic for the estimated partemethers: theta
		addmat(&theta_moav, &theta_moav, &theta);
		
		squaremat(&theta_vari, &theta);  //theta_vari used as temporary matrix
		addmat(&theta_qume, &theta_qume, &theta_vari);
		
		maxmatel(&theta_maxd, &theta);  // <- Maximum element selection 
		minmatel(&theta_mind, &theta);	// <- Minimum element selection
#endif //#IF NO_THETA_STATISTICS


	 	//Calculate the final statistics:
		v_vari = (float) inv_NxT * (v_qume_sum - (v_moav_sum * v_moav_sum));
		v_vari = v_vari  * v_vari;
		v_qume = (float) (v_qume_sum * inv_NxT); // <- quadratic mean
		v_moav = (float) (v_moav_sum * inv_NxT); // <- moving averadge
		v_moav = fabs(v_moav);                 // <- absolut value of moving averadge
		v_mami = v_maxd - v_mind ; // <- error range


#if NO_THETA_STATISTICS
		// Statistic for the estimated partemethers:
		squaremat(&theta_vari, &theta_moav);  //theta_vari used as temporary 
		submat(&theta_vari, &theta_qume, &theta_vari);
		cmulmat(&theta_vari, inv_NxT, &theta_vari); // <- Variance
		theta_frob = frobenius_norm(&theta_vari); //<- Frobenius norm(Variance)
#endif //#IF NO_THETA_STATISTICS

		//performance_index= theta_frob * v_mami * v_qume +  v_vari;
		//performance_index=   v_qume ; //1st
		//performance_index=  v_mami ;
		//performance_index=  v_mami * v_qume; // 2nd


#define MEMORYS 0.3333
#define NEWSEL (1.0-MEMORYS)

		performance_index= (MEMORYS*performance_index) + (NEWSEL * v_mami * v_qume ); // <-- Memory function!!!
		//performance_index=  (v_mami * v_qume) / theta_frob; // 3th 
		
		//cmulmat(&theta_qume, inv_NxT, &theta_qume); // <- quadratic mean
		//sqrtmat(&theta_qume, &theta_qume);		// root-mean-square

		//cmulmat(&theta_moav, inv_NxT, &theta_moav); // <- moving averadge

		
		return 1;
	}

	else	return -1;
	
}

//-----------------------------------------------------------------------------
float sysid::get_v_n(void)   //  residual
{
	return v_n*v_n;
}
//-----------------------------------------------------------------------------
float sysid::get_v_moav(void)	// moving averadge of the residuals
{
	return v_moav;
}
//-----------------------------------------------------------------------------
float sysid::get_v_qume(void)	// quadratic nean (moving averadge) = mean squared value
{
	return v_qume;
}
//-----------------------------------------------------------------------------
float sysid::get_v_vari(void)	// moving variance of the residuals
{
	return v_vari;
}
//-----------------------------------------------------------------------------
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;
}
//-----------------------------------------------------------------------------

#if NO_THETA_STATISTICS 
//-----------------------------------------------------------------------------
void sysid::get_theta_moav(Matrix *gtheta)	// moving averadge of the estimated paramethers
{// Copy the psy matrix from private space to public space
	int i ;
	gtheta->row = theta_moav.row;
	gtheta->col = 1; // It is a vector
	  
	if(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

	for (i=1;i<=theta_moav.row;i++)  gtheta->mtx[i][1] = theta_moav.mtx[i][1];
}
//-----------------------------------------------------------------------------
void sysid::get_theta_qume(Matrix *gtheta)	// quadratic nean (moving averadge) 
{// Copy the psy matrix from private space to public space
	int i ;
	gtheta->row = theta_qume.row;
	gtheta->col = 1; // It is a vector
	  
	if(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

	for (i=1;i<=theta_qume.row;i++)  gtheta->mtx[i][1] = theta_qume.mtx[i][1];
}
//-----------------------------------------------------------------------------
void sysid::get_theta_vari(Matrix *gtheta)	// moving varianc of the estimated paramethers
{// Copy the psy matrix from private space to public space
	int i ;
	gtheta->row = theta_vari.row;
	gtheta->col = 1; // It is a vector
	  
	if(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

	for (i=1;i<=theta_vari.row;i++)  gtheta->mtx[i][1] = theta_vari.mtx[i][1];
}
//-----------------------------------------------------------------------------
void sysid::get_theta_maxd(Matrix *gtheta)	// Maximum deviation from the moving averadge
{// Copy the psy matrix from private space to public space
	int i ;
	gtheta->row = theta_maxd.row;
	gtheta->col = 1; // It is a vector
	  
	if(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

	for (i=1;i<=theta_maxd.row;i++)  gtheta->mtx[i][1] = theta_maxd.mtx[i][1];
}
//-----------------------------------------------------------------------------
void sysid::get_theta_mind(Matrix *gtheta)	// minimum deviation from the moving averadge
{// Copy the psy matrix from private space to public space
	int i ;
	gtheta->row = theta_mind.row;
	gtheta->col = 1; // It is a vector
	  
	if(gtheta->row > gtheta->al_row)  sg_error(SMALL_MATRIX_ALOCATION);

	for (i=1;i<=theta_mind.row;i++)  gtheta->mtx[i][1] = theta_mind.mtx[i][1];
}
//-----------------------------------------------------------------------------
float  sysid::get_theta_frob(void)	// Frobenius norm of the theta covariance matrix
{
	return theta_frob;
}
//-----------------------------------------------------------------------------
#endif //#IF NO_THETA_STATISTICS

//-----------------------------------------------------------------------------
float sysid::get_performance_index(void) // generalized performance index
{
	return performance_index;
}
//-----------------------------------------------------------------------------

//----------------------------------------------------------------------------- 
//----------------------------------------------------------------------------- 
// 
//		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)  
{ 
    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] 
*/
/*
    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
*/
    return 1;  // No Stability investigation!

    ftemp = theta.mtx[m_s+1][1]; /* Normalization parameter */ 
          /* Normalization of the parameters: */
    for(i=1;i < m_a;i++) Vb[i] = Va[i] = (theta.mtx[m_s+i+1][1]/ ftemp) ;
	
    ftemp = Va[m_a-1];   // <- gamma_1
    
    if(fabs(ftemp) > 1) return 0;     // stability tests

    for(j=m_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
}

//-----------------------------------------------------------------------------
#endif //STATISTIC_IS_IMPLEMENTED ---------------------------------------------
//-----------------------------------------------------------------------------