/* 
 *  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.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     FIX . cpp	                                                   */ 
/*                                                                         */ 
/*     Fix (estimated) system parameter inplementation.                    */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 
#include "FIX.hpp" 
 
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 

 
FIX_alg::FIX_alg(int is,int ia,int ib,int ic) : sysid(is,ia,ib,ic)						  
{ 
	matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix

} 
 
//----------------------------------------------------------------------------- 
// Implementation: 
//----------------------------------------------------------------------------- 
 
void FIX_alg::update(float y_n, float u_n_1) 
{ 
	// y_n the systems output at time n*T (present time)  
 extern int selectionflag;

	//Pre update: 
	update_psy(u_n_1); // control signal from the end of privious calculation  
 
	// Residual in system identification (predicion error): 
	mulmat_t1(&Mtemp, &psy, &theta);  // v = y_n - psy' * theta ;  
	v_n = y_n - Mtemp.mtx[1][1];		// Residual in system identification  
		
	 
	// Post Update: 
	update_psy(y_n, v_n); // Updates the psy vector with time shift 
	 //(u_n will be calculated based on the estimated theta vector in future) 
 
 
 
	// Statistic calculation for the system identification: 
    if(update_stat() == 1 ) // The results are redy: 
	{ 
	    selectionflag = 1;
	    //	printf("\n KF ---->"); 
	    //	printf("\n v: N(%g, %g), v_H2 = %g",v_moav,v_vari,v_qume); 
	    //	printf("\n%g < v < %g <> v_range = %g",v_mind,v_maxd,v_mami); 
	    //	printf("\ttheta_frob = %g",theta_frob); 
	    //	printf("\t space = %g \n",performance_index); 
	    
	    count_n = NxT; // Start the next statistical evaluation 
	 
	}
    else selectionflag = 0;
  
} 
 
//----------------------------------------------------------------------------- 

//-----------------------------------------------------------------------------
void FIX_alg::set_theta(Matrix *settheta)
{ // Copy the theta matrix from private space to public space
  int i ;
 
   
  //if(settheta->row == theta.row) sg_error(INCORRECT_MATRIX_SIZE); // Error
  //if(settheta->col = theta.col)  sg_error(INCORRECT_MATRIX_SIZE); // Error
  theta.row = settheta->row;
  theta.col = settheta->col; 
   for (i=1;i<=theta.row;i++)  theta.mtx[i][1] =  settheta->mtx[i][1] ;
 
}

//-----------------------------------------------------------------------------
 
FIX_alg::~FIX_alg()  // deInitialization 
{ 
// Remouve the alocation from the heep 
	matrix_delete(&Mtemp);
} 
 
//-----------------------------------------------------------------------------