/* ********************************************************************** */
/*                                                                        */
/*                                CONTROL()                               */
/*                                =========                               */
/*                                                                        */
/*         compute the control signal and output to the system input      */
/*                                                                        */
/* ********************************************************************** */
#include <stdio.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/msg.h>
#include "apss.h"

#include "global.h"
#include "MatrixCal.hpp"   // For the random number generation 

//#define  NOISE_LIMIT  0.0025
#define  NOISE_LIMIT    0.005 //0.00001
#define  CONTROL_LIMIT  0.1 

#define IMPULSE_LENGTH 2718   /* empirically derived length of impulse response */
double filt(double);    /* actual filter routine */

void control(float u_control)
{
   
    double  Upss ;
   static float frand;
   static int   irand  = 0;

   
        
	//	rand += NOISE_LIMIT*random_1(&idnum);

	//printf("\n idnum_0 =% ld ",idnum);

	Upss = u_control ;

           
       // if(irand >= 0) 	{
	  frand = NOISE_LIMIT*random_1(&idnum); 
	  //   irand = -3;  // 3, 6
	  //	}
	  //	irand++;
        frand=filt(frand);      /* initial zero input for display purposes only */

        Upss += frand;

	//if (NOISE_LIMIT > fabs(u_control))
	//  {
	//Upss += NOISE_LIMIT*random_1(&idnum); 
	            //printf(" rand =%g idnum=%ld ",rand, idnum);   
	    
	   // }
	    //if( sample_no >= beginControl )

     if (Upss > CONTROL_LIMIT) Upss= CONTROL_LIMIT;
      else if (Upss < -CONTROL_LIMIT) Upss=-CONTROL_LIMIT;
     
      
	  msgout.dataout.Upss = Upss ;

	if (msgsnd(queueid2, (struct msgbuf *) &msgout,
			sizeof(msgout.dataout), 0) == -1 ) {
		printf("\nError: send error\n") ;
                printf("\terrno = %d\n", errno );
                exit( 1 );
	        }
}




/*
**************************************************************************
C Lang. Filter Realization Copyright (C) by HYPERCEPTION, INC.
**************************************************************************
*
*       Filter Generated from File: BP_B_01.IIR
*
*       Filter Order: M=40;  Number of Sections: N=(M+1)/2=20
*
*       Modified Direct Form II cascade of 2nd order sections:
*
*                N
*             _______              -1        -2
*               | |  { A0i +  A1i z  +  A2i z  }
*       H(z)= K | |   -------------------------- 
*               i=1                -1        -2
*                    {  1  +  B1i z  +  B2i z  }
*
*                                                                 1/N
*        In diagram below, all Aji have first been multiplied by K,
*        in order to distribute K through all sections evenly.
*
*                    A01                 A02                    A0i
*  x(n) -->- + ----.---- + -->-- + ----.---- + - .... - + ----.---- + -> y(n)
*           /|  z-1|     |\     /|  z-1|     |\        /|  z-1|     |\
*          | |_____v_____| |   | |_____v_____| |      | |_____v_____| |
*          |  -B11 | A11   |   |  -B12 | A12   |      |  -B1i | A1i   |
*          |       |       |   |       |       |      |       |       |
*          |    z-1|       |   |    z-1|       |      |    z-1|       |
*          |_______v_______|   |_______v_______|      |_______v_______|
*             -B21   A21          -B22   A22             -B2i   A2i
*
*
**************************************************************************
*
*/
#define NUM_STAGES 20         /* number of 2nd-order stages in filter */

double A[NUM_STAGES][3] = { /* numerator coefficients */
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 },
                           {  9.01352817405E-0002,  0.00000000000E+0000, -9.01352817405E-0002 }
                          };

double B[NUM_STAGES][2] = { /* denominator coefficients */
                           { -1.99019555630E+0000,  9.94076400689E-0001 },
                           { -1.97827875222E+0000,  9.82210645269E-0001 },
                           { -1.96604538819E+0000,  9.70107662582E-0001 },
                           { -1.95320080417E+0000,  9.57486255816E-0001 },
                           { -1.93937748831E+0000,  9.44004309772E-0001 },
                           { -1.92407732724E+0000,  9.29208935249E-0001 },
                           { -1.90657487446E+0000,  9.12456845975E-0001 },
                           { -1.88576071418E+0000,  8.92796537128E-0001 },
                           { -1.86000897493E+0000,  8.68923391653E-0001 },
                           { -1.82807611748E+0000,  8.40200570265E-0001 },
                           { -1.79414896782E+0000,  8.11342245010E-0001 },
                           { -1.76866072663E+0000,  7.92137217901E-0001 },
                           { -1.75578972492E+0000,  7.85767517796E-0001 },
                           { -1.75426946871E+0000,  7.90545367990E-0001 },
                           { -1.76242450387E+0000,  8.04641096740E-0001 },
                           { -1.77905041726E+0000,  8.26750972642E-0001 },
                           { -1.80325549241E+0000,  8.55885064883E-0001 },
                           { -1.83427089300E+0000,  8.91165906731E-0001 },
                           { -1.87131375083E+0000,  9.31690292075E-0001 },
                           { -1.91348365359E+0000,  9.76428005803E-0001 }
                          };

double DLY[NUM_STAGES][2];  /* delay storage elements (z-shifts) */

double filt(double stage_input)    /* actual filter routine */
{
    double temp, stage_output;
    int   i;

    for (i=0; i<NUM_STAGES; i++)
    {
        temp = stage_input - B[i][0] * DLY[i][0] - B[i][1] * DLY[i][1];
        stage_output = A[i][0] * temp + A[i][1] * DLY[i][0] + A[i][2] * DLY[i][1];

        DLY[i][1] = DLY[i][0];
        DLY[i][0] = temp;

        stage_input = stage_output;  /* output becomes input for next stage */
    }

    return stage_output;
}