#include "H8_MPC.hpp" 
#include "global_var.h"


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


/*  Description:  DSP C program, Using Bilinear Trans.      */
//--------------------------------------------------------------------------
float H8MPC_alg::rtm_PID_control(float fy__i, float fu__i) 
//float blnr_pss(float fy__i)
		{

#if PID_CONTROL

	#define Ks -0.6    		/*-0.2   -25 */
	#define T1  0.25     	/*0.25    0.25*/    
	#define T2  0.005    	/*0.005   0.5  */
	#define T3  0.5    	    /*0.5     0.15 */
	#define T4  0.06    	/*0.06    0.05 */
	#define T5  0.1    	    /*0.1     0.005 Integral*/
	#define T6  1.6   		/*1.6     0.6 Low pass*/





			static float	X1   = 0.0 ;
			static float	X2   = 0.0 ;
			static float	X3   = 0.0 ;
			static float	X4   = 0.0 ;
			static float	X5   = 0.0 ;

			static float	X10  = 0.0 ;
			static float	X20  = 0.0 ;
			static float	X30  = 0.0 ;
			static float	X40  = 0.0 ;
			static float	X50  = 0.0                                                     ;

			#define	h  0.03333333  /* T_sample in [s] */

			float upss;

	

	X1 = fy__i * Ks;
	X2 = (h * X10 + h * X1 - (h - 2*T6) * X20) / (h + 2*T6);
	X3 = (2 * T5* 1 * X2 - 2 * T5 * 1 * X20 - (h - 2*T5) * X30) / (h + 2*T5);
	X4 = ((h + 2*T1)  * X3 + (h - 2*T1) * X30 - (h - 2*T2) * X40) / (h + 2*T2);
	X5 = ((h + 2*T3)  * X4 + (h - 2*T3) * X40 - (h - 2*T4) * X50) / (h + 2*T4);

	X10 = X1;
	X20 = X2;
	X30 = X3;
	X40 = X4;
	X50 = X5;

	upss = X5;  /* output sould be in range +/- 1.0 */
	
	   	// Statistic calculation for the control:
	y_sys_n = fy__i;
	u_sys_n = upss;
    if(update_stat() == 1 ) // The results are redy: 
	{ 
	    count_n = NxT; // Start the next statistical evaluation 
		controlStatRedy = 1;
		
	  	   LOG_printf(&trace2,"t= %g",t_n);		
	       LOG_printf(&trace3,"PI_y = %g", get_output_error()); 
	       LOG_printf(&trace2,"PI_u = %g", get_control_effort()); 		
		
	}
    else controlStatRedy = 0;
	
	
	upss = LIMIT(-1.0, upss, 1.0);
	return  upss  ;
#else
	return 0.0;	
#endif
}

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