/*
 *  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.  
 */
/***************************************************************************/
/*                                                                         */
/*     sysControl . cpp	                                                   */
/*                                                                         */
/*     System Control - Common Procedures.       			   */
/*                                                                         */
/*                                                                         */
/***************************************************************************/
//  DSP 2 <=> TMS320C6713
// Include files definition:

#include "sysContr.hpp"

//-----------------------------------------------------------------------------
// Declaration of the sysCR object
//-----------------------------------------------------------------------------


syscr::syscr(void)
{
 

 // Memory alocation for the matrices:
	//matrix_alloc(&state, m_a, 1);	// state
	//float state_v[ARMAX_a+2];
	state.mtx = state_v;
	state.row = ARMAX_a;
	state.col = 1;    
	state.dim = ARMAX_a;
    state_v[0] = SATRT_OF_VECTOR;
    state_v[ARMAX_a+1] = END_OF_VECTOR;
 


	// Alocate memory for the neccessery matrices in statistic calculation:

	// Initialise paramethers:


}

//-----------------------------------------------------------------------------
syscr::~syscr(void)
{

    
}


//-----------------------------------------------------------------------------
void syscr::get_state(Matrix *gstate)
{ // Copy the theta matrix from private space to public space
  int i ;
 
   
   gstate->row = state.row;
   gstate->col = 1; // It is a vector
	  
   if(gstate->row > gstate->row)  printlog("DIMENSION_ERROR");
   for (i=1;i<=state.row;i++)  gstate->mtx[i] = state.mtx[i];
}


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

int syscr::update_stat(void)
{

	if (count_n == NxT) // if new sesion initialixe:
	{
		// Zero initialization of statistics

		y_sys_2 = 0.0;		// output's error sqare
		u_sys_2 = 0.0;		// control effort square 	

	}

	if( count_n > 1)
	{
		count_n--;
		
		// Statistic for 
		y_sys_2 += y_sys_n * y_sys_n;		
		u_sys_2 += u_sys_n * u_sys_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 
		y_sys_2 += y_sys_n * y_sys_n;		
		u_sys_2 += u_sys_n * u_sys_n;

		return 1;
	}
	else	return -1;
	
}


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

float syscr::get_output_error(void) // the system output error
{
	return y_sys_2;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

float syscr::get_control_effort(void) // the control effort
{
	return u_sys_2;
}
//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------