/* 
 *  Copyright 2004 by Soos, Antal 
 *  All rights reserved. Property of Soos, Antal. 
 *  Restricted rights to use, duplicate or disclose this code are 
 *  granted through contract.   
 */ 
/***************************************************************************/ 
/*                                                                         */ 
/*     RT_Control . cpp                                                    */ 
/*                                                                         */ 
/*     Real-time control: H8_MPC inplementation                            */ 
/*                                                                         */ 
/*                                                                         */ 
/***************************************************************************/ 
 //  DSP 2 <=> TMS320C6713
/************************************************************************/ 
#define _in_RT_CONTROL_CPP_ 
/************************************************************************/ 

#include "mmaccfg.h"
#include <csl_cache.h>

#include "global_var.h"
#include "dsk_lib.h"
#include <cstdlib>

extern "C"
{Void RT_Calculus() ;}
 


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

	#define MEDVECTOR_SZ 25 /* 1 (<-d/dt), 9 or 25 */
	
 #if MEDVECTOR_SZ == 9	
 float opt_med9(float * p);	
 #elif MEDVECTOR_SZ == 25
 float opt_med25(float * p); 
 #endif	
	
//----------------------------------------------------------------------------- 
//-----------------------------------------------------------------------------
Void RT_Calculus(Void)  // <-- Sample time interrupt 0.033s
{
	static int callH8c = NxT;

	
	static float yt,ut,ut_p,ut_p1;
	
  #if  H8_CONTROL 
#define C_RAND 4 /* 3 */
	int itemp1;
   static int i_rand = C_RAND;
   static float u_rand;
 #endif  
  // allocation for median calculation:	
	static float measurements[MEDVECTOR_SZ];

	
	
	float fRotationalPeriod;

 #if MEDVECTOR_SZ > 1 
 	float fvtemp[MEDVECTOR_SZ];
	float median;   
    int i;
 #endif   
    
  static unsigned int iperLED2 = LED2_PERIOD;


  DSK6713_LED_on(0);   // Measure the time for calculations
   
	//-----------------------------------------------------------------------------    
	// The delta omega:
	 //-----------------------------------------------------------------------------
  
    #if MEDVECTOR_SZ > 1 
        for(i=MEDVECTOR_SZ-1; i>0; i--)
        {
        measurements[i] = measurements[i-1];
        fvtemp[i] = measurements[i]; 
        }
    #endif    
     
    //---------------------------------------------------    
   // Access to input data:
              
        fRotationalPeriod = (float)uiRotationalPeriod;    
    //---------------------------------------------------        
   
        
                  
        //Calculates the median of the rotational period:
        // The omega deviation from median
    #if MEDVECTOR_SZ == 9
        measurements[0] = fRotationalPeriod;
        fvtemp[0] = fRotationalPeriod;
     	median = opt_med9(fvtemp);
     	yt = (fRotationalPeriod - median)/500.0; 
	
    #elif MEDVECTOR_SZ == 25
        measurements[0] = fRotationalPeriod;
        fvtemp[0] = fRotationalPeriod;
        median = opt_med25(fvtemp);
        yt = (fRotationalPeriod - median)/500.0; 
		// 
	
   #elif  MEDVECTOR_SZ == 1
     // Diff the input:
      
         yt = (fRotationalPeriod - measurements[0])/200.0;
         measurements[0] = fRotationalPeriod;
 
	#endif
	
	
	 // The system output if delta_Power_electric is used :
      // yt = rec->x[x];  // delta activ power !
	
//---------------------------------------------------------------------------------------------   
//---------------------------------------------------------------------------------------------   

     
//---------------------------------------------------------------------------------------------   
//---------------------------------------------------------------------------------------------         
         
         //LOG_printf(&trace, " w = %g",yt);    
       // yt = LIMIT(-1.0, yt, 1.0);

     
 	 //-----------------------------------------------------------------------------
     // Real time part of the control algorithm:
     //-----------------------------------------------------------------------------
     // The system input (acting on the system from the last sample):
    
 	  ut = ut_p1;      // the delay of 2 sample for the adaptation
 	  ut_p1 = ut_p;
 
 	 
 			
 							
 #if  H8_CONTROL   
      // The Real time part of the H8 Control algorithm   
      ut_p = pH8MPC->rtm_control(yt,ut);   // where: -1 < ut_p < 1
      
      
      
 #endif /* H8_CONTROL */
 
 #if PID_CONTROL
 		//The real time PID control
     ut_p = pH8MPC->rtm_PID_control(yt,ut);   // where: -1 < ut_p < 1
 #endif  /* PID_CONTROL */
 
             
      
       tra->x[1] = ut_p;  // The control sygnal for observations on scope
        
  #if  H8_CONTROL    
             
       i_rand--;                      
      if(i_rand <= 0)
      {
      	// include the random signal:
	  	itemp1 = rand() - RAND_MAX_HALF;  
	  	u_rand = (float) ((itemp1 * INV_RAND_MAX_HALF) *	RANDOM_SIGNAL_LEVEL);
	 	i_rand = C_RAND;
	  }
	  
	  ut_p += u_rand ;
	  
 #endif /* H8_CONTROL */	 
  
	  // Send the control and random signal to DSP1 for AVR inclusion:
	  if( DSK6713_DIP_get(0)==0) ut_p =0.0;   // If DIP SW 1 -> ON <-> NoC
	  //ut_p = 0.0;    // <- NoC
	  tra->x[4] = ut_p;  // <-- The control signal for AVR <----
       
//---------------------------------------------------------------------------------------------   
//---------------------------------------------------------------------------------------------         
     t_n += delta_t;  //increment the time 
//---------------------------------------------------------------------------------------------   
//---------------------------------------------------------------------------------------------         


	  // For observation:
	  //tra->x[1] = ut;  
      tra->x[2] = yt/2.0; //yt;    // Signal is sent to the DSP2DSP communication
 	  //tra->x[3] = ut_p;//ut_p;  //For test
  //---------------------------------------------------------------------------------------------    
   
   /************** >>>>> >>>>>> *********/ 
   
    pfxyz->x = rec->x[4];
    pfxyz->y = rec->x[5];
    pfxyz->z = median;  
    LOG_event(&trace3,log_ux[0],log_ux[1],log_ux[2]);         
                                         
    pfxyz->x = t_n;
    pfxyz->y = ut_p;
    pfxyz->z = yt;  
    LOG_event(&trace4,log_ux[0],log_ux[1],log_ux[2]);    
    
    pfxyz->x = rec->x[0];
    pfxyz->y = rec->x[1];
    pfxyz->z = rec->x[2];  
    LOG_event(&trace5,log_ux[0],log_ux[1],log_ux[2]); 
    
    
  
  /************** <<<<   <<<<< *********/             
              
 #if H8_CONTROL      
  //-----------------------------------------------------------------------------
     // Parameter estimation:
     //-----------------------------------------------------------------------------
     // 1st  algorithm:
     pFIX->update(yt, ut);
     //-----------------------------------------------------------------------------
     // 2nd algorithm:
      pRLS->update(yt, ut);  //OK
     //-----------------------------------------------------------------------------
     // 3th algorithm:  
     pLMS->update(yt, ut); //OK
     //-----------------------------------------------------------------------------
     // 4th algorithm:  
     pWCE->update(yt, ut); //OK
     //-----------------------------------------------------------------------------
     // 5th algorithm:  
     pKF->update(yt, ut); //OK
     //-----------------------------------------------------------------------------
     // 6th algorithm:  
     pFQR_RLS->update(yt, ut);  // OK ~
     //-----------------------------------------------------------------------------
     // 7th algorithm:  
     pIQR_RLS->update(yt, ut);   // OK
     //-----------------------------------------------------------------------------
     // 8th algorithm:
      pQR_LMS->update(yt, ut);  // OK
     //------------------------------------------------------------------------
    
       
	// tra->x[3] = pWCE->get_v_n()*100;   /// <<<- v = y_predicted - y (test)




/************** >>>>> >>>>>> *********/  

        
  	pfxyz->x = pFIX->get_v_n();
    pfxyz->y = pRLS->get_v_n();
    pfxyz->z = pLMS->get_v_n();  
    LOG_event(&trace6,log_ux[0],log_ux[1],log_ux[2]);   
 
    pfxyz->x = pWCE->get_v_n();
    pfxyz->y = pKF->get_v_n();
    pfxyz->z = pFQR_RLS->get_v_n();  
    LOG_event(&trace7,log_ux[0],log_ux[1],log_ux[2]);  
    
   	pfxyz->x = pIQR_RLS->get_v_n();
    pfxyz->y = pQR_LMS->get_v_n();
    pfxyz->z = fRotationalPeriod;  
    LOG_event(&trace8,log_ux[0],log_ux[1],log_ux[2]);   
     
           
    
 /************** <<<<   <<<<< *********/  


 #endif /* H8_CONTROL */
 

    if( DSK6713_DIP_get(1)==0) callH8c = NxT;    // If DIP SW 2 -> ON <-> Stop update
      
      
  //-----------------------------------------------------------------
  callH8c--;
  if(callH8c<=0){
   SWI_post(&H8_Calculations); //the baground H8 calculations
   callH8c = NxT;  
    
   } 
  //-----------------------------------------------------------------
 
   // Sygnalization:
    	iperLED2--;
  		if(iperLED2==0) // Indicate that the DSP is executing this co
  				{
  					DSK6713_LED_toggle(2);
  					iperLED2 = LED2_PERIOD;
  				}
  
      
     DSK6713_LED_off(0); // Measure the time for calculations			   			   
}
//-----------------------------------------------------------------------------


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

/*
 * The following routines have been built from knowledge gathered
 * around the Web. I am not aware of any copyright problem with
 * them, so use it as you want.
 * N. Devillard - 1998
 */


#define PIX_SORT(a,b) { if ((a)>(b)) PIX_SWAP((a),(b)); }
#define PIX_SWAP(a,b) { float temp=(a);(a)=(b);(b)=temp; }


/*----------------------------------------------------------------------------
   Function :   opt_med9()
   In       :   pointer to an array of 9 pixelvalues
   Out      :   a pixelvalue
   Job      :   optimized search of the median of 9 pixelvalues
   Notice   :   in theory, cannot go faster without assumptions on the
                signal.
                Formula from:
                XILINX XCELL magazine, vol. 23 by John L. Smith
  
                The input array is modified in the process
                The result array is guaranteed to contain the median
                value
                in middle position, but other elements are NOT sorted.
 ---------------------------------------------------------------------------*/

 #if MEDVECTOR_SZ == 9
 
float opt_med9(float * p)
{
    PIX_SORT(p[1], p[2]) ; 
	PIX_SORT(p[4], p[5]) ; 
	PIX_SORT(p[7], p[8]) ;
    PIX_SORT(p[0], p[1]) ; 
	PIX_SORT(p[3], p[4]) ; 
	PIX_SORT(p[6], p[7]) ;
    PIX_SORT(p[1], p[2]) ; 
	PIX_SORT(p[4], p[5]) ; 
	PIX_SORT(p[7], p[8]) ;
    PIX_SORT(p[0], p[3]) ; 
	PIX_SORT(p[5], p[8]) ; 
	PIX_SORT(p[4], p[7]) ;
    PIX_SORT(p[3], p[6]) ; 
	PIX_SORT(p[1], p[4]) ; 
	PIX_SORT(p[2], p[5]) ;
    PIX_SORT(p[4], p[7]) ; 
	PIX_SORT(p[4], p[2]) ; 
	PIX_SORT(p[6], p[4]) ;
    PIX_SORT(p[4], p[2]) ; 
	return(p[4]) ;
}


/*----------------------------------------------------------------------------
   Function :   opt_med25()
   In       :   pointer to an array of 25 pixelvalues
   Out      :   a pixelvalue
   Job      :   optimized search of the median of 25 pixelvalues
   Notice   :   in theory, cannot go faster without assumptions on the
                signal.
  				Code taken from Graphic Gems.
 ---------------------------------------------------------------------------*/
#elif  MEDVECTOR_SZ == 25

float opt_med25(float * p)
{

    PIX_SORT(p[0], p[1]) ;   
	PIX_SORT(p[3], p[4]) ;   
	PIX_SORT(p[2], p[4]) ;
    PIX_SORT(p[2], p[3]) ;   
	PIX_SORT(p[6], p[7]) ;   
	PIX_SORT(p[5], p[7]) ;
    PIX_SORT(p[5], p[6]) ;   
	PIX_SORT(p[9], p[10]) ;  
	PIX_SORT(p[8], p[10]) ;
    PIX_SORT(p[8], p[9]) ;   
	PIX_SORT(p[12], p[13]) ; 
	PIX_SORT(p[11], p[13]) ;
    PIX_SORT(p[11], p[12]) ; 
	PIX_SORT(p[15], p[16]) ; 
	PIX_SORT(p[14], p[16]) ;
    PIX_SORT(p[14], p[15]) ; 
	PIX_SORT(p[18], p[19]) ; 
	PIX_SORT(p[17], p[19]) ;
    PIX_SORT(p[17], p[18]) ; 
	PIX_SORT(p[21], p[22]) ; 
	PIX_SORT(p[20], p[22]) ;
    PIX_SORT(p[20], p[21]) ; 
	PIX_SORT(p[23], p[24]) ; 
	PIX_SORT(p[2], p[5]) ;
    PIX_SORT(p[3], p[6]) ;   
	PIX_SORT(p[0], p[6]) ;   
	PIX_SORT(p[0], p[3]) ;
    PIX_SORT(p[4], p[7]) ;   
	PIX_SORT(p[1], p[7]) ;   
	PIX_SORT(p[1], p[4]) ;
    PIX_SORT(p[11], p[14]) ; 
	PIX_SORT(p[8], p[14]) ;  
	PIX_SORT(p[8], p[11]) ;
    PIX_SORT(p[12], p[15]) ; 
	PIX_SORT(p[9], p[15]) ;  
	PIX_SORT(p[9], p[12]) ;
    PIX_SORT(p[13], p[16]) ; 
	PIX_SORT(p[10], p[16]) ; 
	PIX_SORT(p[10], p[13]) ;
    PIX_SORT(p[20], p[23]) ; 
	PIX_SORT(p[17], p[23]) ; 
	PIX_SORT(p[17], p[20]) ;
    PIX_SORT(p[21], p[24]) ; 
	PIX_SORT(p[18], p[24]) ; 
	PIX_SORT(p[18], p[21]) ;
    PIX_SORT(p[19], p[22]) ; 
	PIX_SORT(p[8], p[17]) ;  
	PIX_SORT(p[9], p[18]) ;
    PIX_SORT(p[0], p[18]) ;  
	PIX_SORT(p[0], p[9]) ;   
	PIX_SORT(p[10], p[19]) ;
    PIX_SORT(p[1], p[19]) ;  
	PIX_SORT(p[1], p[10]) ;  
	PIX_SORT(p[11], p[20]) ;
    PIX_SORT(p[2], p[20]) ;  
	PIX_SORT(p[2], p[11]) ;  
	PIX_SORT(p[12], p[21]) ;
    PIX_SORT(p[3], p[21]) ;  
	PIX_SORT(p[3], p[12]) ;  
	PIX_SORT(p[13], p[22]) ;
    PIX_SORT(p[4], p[22]) ;  
	PIX_SORT(p[4], p[13]) ;  
	PIX_SORT(p[14], p[23]) ;
    PIX_SORT(p[5], p[23]) ;  
	PIX_SORT(p[5], p[14]) ;  
	PIX_SORT(p[15], p[24]) ;
    PIX_SORT(p[6], p[24]) ;  
	PIX_SORT(p[6], p[15]) ;  
	PIX_SORT(p[7], p[16]) ;
    PIX_SORT(p[7], p[19]) ;  
	PIX_SORT(p[13], p[21]) ; 
	PIX_SORT(p[15], p[23]) ;
    PIX_SORT(p[7], p[13]) ;  
	PIX_SORT(p[7], p[15]) ;  
	PIX_SORT(p[1], p[9]) ;
    PIX_SORT(p[3], p[11]) ;  
	PIX_SORT(p[5], p[17]) ;  
	PIX_SORT(p[11], p[17]) ;
    PIX_SORT(p[9], p[17]) ;  
	PIX_SORT(p[4], p[10]) ;  
	PIX_SORT(p[6], p[12]) ;
    PIX_SORT(p[7], p[14]) ;  
	PIX_SORT(p[4], p[6]) ;   
	PIX_SORT(p[4], p[7]) ;
    PIX_SORT(p[12], p[14]) ; 
	PIX_SORT(p[10], p[14]) ; 
	PIX_SORT(p[6], p[7]) ;
    PIX_SORT(p[10], p[12]) ; 
	PIX_SORT(p[6], p[10]) ;  
	PIX_SORT(p[6], p[17]) ;
    PIX_SORT(p[12], p[17]) ; 
	PIX_SORT(p[7], p[17]) ;  
	PIX_SORT(p[7], p[10]) ;
    PIX_SORT(p[12], p[18]) ; 
	PIX_SORT(p[7], p[12]) ;  
	PIX_SORT(p[10], p[18]) ;
    PIX_SORT(p[12], p[20]) ; 
	PIX_SORT(p[10], p[20]) ; 
	PIX_SORT(p[10], p[12]) ;

    return (p[12]);
}

#endif

#undef PIX_SORT
#undef PIX_SWAP



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