/*
 *  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 . hpp	                                                   */
/*                                                                         */
/*     System Identification - Common definitions.			   */
/*                                                                         */
/*                                                                         */
/***************************************************************************/

// Include files definition:
#ifndef _SYSIDENT_HPP_
#define _SYSIDENT_HPP_

#include "MatrixCal.hpp"
#include <cstdlib>


// Declaration of the system identification paramethers:

#define STATISTIC_IS_IMPLEMENTED 1  // for no statistic 0
#define NO_THETA_STATISTICS 1 // The statistic n theta is not implemented
#define NxT 183    // the lenght beatwen two decision t=N*T
                   // in ewery 10s <=> 400
// Filtering inplementation:
// Filter Selection (one of the following):
#define FLT_0 1  // No filter
#define FLT_1 0  // differentiation
#define FLT_2 0  // double differentiation
#define FLT_3 0  // Band pass: [ <---]->  baterwort  <-- OK
#define FLT_4 0  // Hghpass  filter -...             <-- OK OK !! OK
#define FLT_5 0  // FIR filter
#define FLT_6 0  // d/dt +  FIR filter

//-----------------------------------------------------------------------------
// Declaration of the sysID object
class sysid {  // system identification class

protected:
	Matrix psy;	// the observed system data matrix
			// dimension(psy) = [(m_s+m_a+m_b+m_c) x 1] 
			// psy = [1, -y_1...-y_a, u_1...u_b, v_1...v_c]^T
	Matrix theta;	// the estimated system paramether matrix 
		        // dimension(theta) = [(m_s+m_a+m_b+m_c) x 1] 
			// theta = [x_s, x_a...x_a, x_b...x_b, x_c...x_c]^T
	int m_s;     // The static component dimension m_s = { 1 or 0}
	int m_a;     // The dimension ofautoregresiv components of the model
        int m_b;     // The moving averadge dimension of the model
	int m_c;     // The error model dimension, if 0 => no error model

	int m_dim;	// model_dim = m_s + m_a + m_b + m_c;

	float v_n;    // model's error - residuals: v_n = y_n - psy' * theta

        float fio1 ;   // Integrator in remouve_DC function
        float fio2 ;
        float fio3 ;
        float fio4 ;
 


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

  //----------**** Start of  remouveDC *** ------------------------------- 
#if FLT_0   // No filter======================================================

#elif FLT_1    // single differentiation  ====================================
  float  stage_memory1;
#elif  FLT_2  // double differentiation ======================================
  float stage_memory1 ;
  float stage_memory2 ; 
#elif FLT_3   // Band pass HPF_2    =========================================== 
  float  u_nn1,u_nn2,u_nn3;
  float  DLY0[4];  /* delay storage elements (z-shifts) */
#define xNUM_STAGES 2         /* number of 2nd-order stages in filter */
  double DLY[xNUM_STAGES][2];  /* delay storage elements (z-shifts) */
#elif FLT_4   // Using a low pass filter ====================================
  double stage_memory1 ;
  double stage_memory2 ;  
  double stage_memory11 ;
  double stage_memory12 ;  
#elif FLT_5   // Using a 8-th order band pass filter ========================
#define xNUM_STAGES 8         
   float DLY[xNUM_STAGES];  /* delay storage elements (z-shifts) */
#elif FLT_6   // Using a 4-th order band pass filter ========================
  float  stage_memory1;
   float  stage_memory2;

#endif
  //----------****end of  remouveDC *** -------------------------------------



#if STATISTIC_IS_IMPLEMENTED //------------------------------------------------
	
        int count_n;	// counter for the sample lenght
	float inv_NxT;   // 1/NxT for faster calculation NxT (leng for statistic)
	float v_moav;	// moving averadge of the residuals
        float v_moav_sum;	// summ for moving averadge of the residuals
	float v_qume;	// quadratic nean (moving averadge) = mean squared value
        float v_qume_sum;	//sum for  quadratic nean (moving averadge) = squared value
	float v_vari;	// moving variance of the residuals
	float v_maxd;	// maximum deviation from the moving averadge
	float v_mind;	// minimum deviation from the moving averadge
	float v_mami;	// error range: v_mami = v_maxd - v_mind 

#if NO_THETA_STATISTICS 
	Matrix theta_moav;	// moving averadge of the estimated paramethers
	Matrix theta_qume;	// quadratic nean (moving averadge) 
	Matrix theta_vari;	// moving varianc of the estimated paramethers
	Matrix theta_maxd;	// Maximum deviation from the moving averadge
	Matrix theta_mind;	// minimum deviation from the moving averadge
	float  theta_frob;	// Frobenius norm of the theta covariance matrix
#endif //#IF NO_THETA_STATISTICS

	float performance_index ; // generalized performance index

        float *Va;  //temporary vector for stability analisis
        float *Vb;  //temporary vector for stability analisis
        float *Gamma;  //reflection coeficient 

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



public:


	sysid(int is, int ia, int ib, int ic);
//	void sysid(void);
	~sysid(void);

	void update_psy(float y_n, float u_n, float v_n);
	void update_psy(float y_n, float v_n);
	void update_psy(float u_n_1);
	void get_psy(Matrix *gpsy);

	//update_theta(float y_n, float u_n, float v_n); definied locally
	void get_theta(Matrix *gtheta);

        float DCfilter(float stage_input);


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

	int update_stat(void);  // Statistic calculation for the identification 
        float get_v_n(void); // residual
        float get_v_moav(void);	// moving averadge of the residuals
	float get_v_qume(void);	// quadratic nean (moving averadge) = mean squared value
	float get_v_vari(void);	// moving variance of the residuals
	float get_v_maxd(void);	// maximum deviation from the moving averadge
	float get_v_mind(void);	// minimum deviation from the moving averadge
	float get_v_mami(void);	// error range: v_mami = v_maxd - v_mind 

#if NO_THETA_STATISTICS 
	void get_theta_moav(Matrix *gtheta); // moving averadge of the estimated paramethers
	void get_theta_qume(Matrix *gtheta); // quadratic nean (moving averadge) 
	void get_theta_vari(Matrix *gtheta); // moving varianc of the estimated paramethers
	void get_theta_maxd(Matrix *gtheta); // Maximum deviation from the moving averadge
	void get_theta_mind(Matrix *gtheta); // minimum deviation from the moving averadge
	float  get_theta_frob(void);  // Frobenius norm of the theta covariance matrix
#endif //#IF NO_THETA_STATISTICS

	float get_performance_index(void); // generalized performance index
        int stable(void); // model stability chek

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

};
//-----------------------------------------------------------------------------

#endif // SYSIDENT_HPP