/* 
 *  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" 
 #include "mmaccfg.h"
//----------------------------------------------------------------------------- 
//Initialization: 
//----------------------------------------------------------------------------- 

 
FIX_alg::FIX_alg(float lambda) : sysid()						  
{ 
 
	//matrix_alloc(&Mtemp, m_s+m_a+m_b+m_c, m_s+m_a+m_b+m_c); //temporary matrix
	//float Mtemp_v[1+2];
	Mtemp.mtx = Mtemp_v;
	Mtemp.row = 1;
	Mtemp.col = 1;    
	Mtemp.dim = 1;
	Mtemp_v[1] = 0.0;
    Mtemp_v[0] = SATRT_OF_VECTOR;
    Mtemp_v[1+1] = END_OF_VECTOR;

  // print_matrix(&Mtemp,"Mtemp");    
} 
 
//----------------------------------------------------------------------------- 
// 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];		// 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;
	    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 ;
  
  theta.row = settheta->row;
  theta.col = settheta->col; 
   for (i=1;i<=theta.row;i++)  theta.mtx[i] =  settheta->mtx[i] ;
}

//-----------------------------------------------------------------------------
 
FIX_alg::~FIX_alg()  // deInitialization 
{ 

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