/**************************************************************
*        Program to demonstrate the Butterworth filter        *
*           (removing frequencies greater than Fc)            *
*                                                             *
*       C++ version with graphic results by J-P Moreau, Paris *
*                       (www.jpmoreau.fr)                     *
*                                                             *
*                       (use with tgfft.mak and graph2d.cpp)  *
* ----------------------------------------------------------- *
* SAMPLE RUN:                                                 *
*                                                             *
* Input data file (tfft.dat):                                 *
*                                                             *
* (The test signal contains 3 frequencies: 50, 250, 500 hz)   *
*                                                             *
* 1024                                                        *
* 0.00000000000000E+0000   0.00000000000000E+0000             *
* 1.95503421309917E-0004   3.53914399999776E+0001             *
* 3.91006842619834E-0004   5.95684899999760E+0001             *
* 5.86510263929974E-0004   6.54621699999552E+0001             *
* 7.82013685239669E-0004   5.24038399999845E+0001             *
* ...                      ...                                *
* 1.98826979472187E-0001   2.77372500000183E-0001             *
* 1.99022482893497E-0001  -2.43361500000174E+0000             *
* 1.99217986314807E-0001  -4.84236799999780E+0000             *
* 1.99413489736116E-0001  -6.02247899999929E+0000             *
* 1.99608993157426E-0001  -5.45615399999951E+0000             *
* 1.99804496578736E-0001  -3.22824200000105E+0000             *
* 2.00000000000045E-0001  -2.96010699999982E-0003             *
*                                                             *
* Output file (tfilter.lst):                                  *
*                                                             *
*        Time        Input Signal      Filtered Signal        *
*  -------------------------------------------------------    *
*      0.0000         0.000000           0.000000             *
*      0.0002        35.391440           0.158005             *
*      0.0004        59.568490           1.278083             *
*      0.0006        65.462170           5.003994             *
*      0.0008        52.403840          12.809453             *
*      0.0010        26.229620          24.336649             *
*      ...           ...                ...                   *
*      0.1988         0.277373          -2.224546             *
*      0.1990        -2.433615          -0.802179             *
*      0.1992        -4.842368           0.588941             *
*      0.1994        -6.022479           1.320860             *
*      0.1996        -5.456154           1.036090             *
*      0.1998        -3.228242          -0.213007             *
*      0.2000        -0.002960          -1.978450             *
*                                                             *
* Note: The cut off frequency Fc is 500 hz, the filter order  *
*       is 4 (A Fourier analysis of the filtered signal shows *
*       that frequencies greater than Fc are satisfactorily   *
*       removed but with a slight time shift of the signal).  *
**************************************************************/
#include <stdio.h>
#include <math.h>
#include <windows.h>

HDC  hdc;
RECT rect;
HPEN hpen, hpenOld;

//Functions used here of Graph2D.cpp
void CourbeXY(int,int,float *,double,double);
void Legendes (int,char *,char *,char *,int,int,char *,int,int,char *);

//"home made" graphic commands for hdc environment used by above functions
void DrawPixel(int ix,int iy) {
  //sorry, no other available command found
  Rectangle(hdc,rect.left+ix,rect.top+iy,
	            rect.left+ix+2,rect.top+iy+1);
}

void Swap(int *i1,int *i2) {
  int it;
  it=*i1; *i1=*i2; *i2=it;
}

void DrawLine(int ix1,int iy1,int ix2,int iy2) {
  int i,il,ix,iy;
  double dx,dy;
  if (ix2<ix1) {
	Swap(&ix1,&ix2); Swap(&iy1,&iy2);
  }
  il=(int) sqrt((ix2-ix1)*(ix2-ix1)+(iy2-iy1)*(iy2-iy1));
  dx=(double) (ix2-ix1)/il; 
  dy=(double) (iy2-iy1)/il;
  ix=ix1; iy=iy1;
  for (i=1;i<il+1; i++) {
	DrawPixel(ix,iy);
	ix=(int) (ix1+i*dx); 
	iy=(int) (iy1+i*dy);
  }
}

void OutText(int ix,int iy,char *s) {
  RECT rct;
  rct.left=ix; rct.right=ix+10*strlen(s);
  rct.top=iy-5; rct.bottom=iy+15;
  DrawText(hdc, s, -1, &rct,
                DT_SINGLELINE | DT_CENTER | DT_VCENTER);
}


// **** Begin Filter section ****

#define  SIZE      1025
#define  REAL      double

typedef  REAL Filter_Coef[6][10];
typedef  REAL Memory_Coef[3][10];

FILE   *f_in, *f_out;
int    i,order,ndata;
REAL   dt,Fc,t,temp,tbegin,tend,yy;
float  signal[SIZE],filtered[SIZE]; 

Filter_Coef C;
Memory_Coef D;
int    NSections;
REAL   Tg,Xdc;


/**********************************************************************
*          Filtering a signal F(t) by Butterworth method              *
*             (removing frequencies greater then Fc)                  *
* ------------------------------------------------------------------- *  
* Calling mode:   Filter(Xs,Xd,NSections,C,D);                        *
* ------------------------------------------------------------------- *
* INPUTS:                                                             *
* -------                                                             *
*        Xd.......:  current value of input signal (REAL)             *
*        NSections: Number of required 2nd order sections (integer)   *
*                   = n/2     for n even                              *
*                   = (n+1)/2 for n odd                               *
*        n........: order of filter (1 to 4)                          *
*        C........: Table[1..5,1..NSections] of filter coefficients   *
*                   calculated previously by BUTTERWORTH procedure    *
*        D........: Table[1..2,1..NSections] of coefficients defining *
*                   the filter memory, initialized by INIT procedure. *
* ------------------------------------------------------------------- *
* OUTPUTS:                                                            *
* -------                                                             *
*        D........: Table updated after the call to Filter procedure  *
*        Xs.......: current value of filtered signal (REAL)           *
* ------------------------------------------------------------------- *
* Référence                                                           *
* ---------                                                           *
*  "Lawrence R.Rabiner et Bernard Gold                                *
*   Theory and application of digital processing.                     *
*   Prentice Hall Inc., EnglewoodclIFfs,NEW JERSEY,1975."             *
*                                                                     *
*                                   C++ version by J-P Moreau, Paris  *
*               from Fortran version by J-P Dumont / Tuan Dang Trong  *
**********************************************************************/
void Filter( REAL *Xs,REAL Xd, int NSections,
 	         Filter_Coef C, Memory_Coef D )  {
  REAL x,y,err;
  int i;
  x=Xd;
  for (i=1; i<NSections+1; i++) {
    err=x+C[1][i]*D[1][i]+C[2][i]*D[2][i];
    y=C[5][i]*(err +C[3][i]*D[1][i]+C[4][i]*D[2][i]);
    D[2][i]=D[1][i];
    D[1][i]=err;
    x=y;
  }
  *Xs=x;
}

/*************************************************************************
*                       INIT FILTER PROCEDURE                            *
* ---------------------------------------------------------------------- *
* The filter response is initialized to stationnary value for a constant *
* input signal value.                                                    *
*                                                                        *
* Calling mode:   INIT(Xdc,C,NSections,D);                               *
* ---------------------------------------------------------------------- *
* INPUTS:                                                                *
* ------                                                                 *
*        Xdc......: constant input value (REAL)                          *
*        C........: Table[1..5,1..NSections] of filter coefficients      *
*                   calculated previously by BUTTERWORTH procedure       *
*        NSections: Number of required 2nd order sections (integer)      *
*                   = n/2     for n even                                 *
*                   = (n+1)/2 for n odd                                  *
*        n........: order of filter (1 to 4)                             *
* ---------------------------------------------------------------------- *
* OUTPUTS:                                                               *
* -------                                                                *
*        D........: Table[1..2,1..NSections] of coefficients defining    *
*                   the filter memory, initialized by INIT procedure.    *
*************************************************************************/
void Init(REAL Xdc, Filter_Coef C, int NSections, Memory_Coef D) {
  REAL dc,Csum;
  int i,j;
  dc=Xdc;
  for (j=1; j<NSections+1; j++) {
    D[2][j]=dc/(1-C[1][j]-C[2][j]);
    D[1][j]=D[2][j];
    Csum=0;
    for (i=1; i<5; i++) Csum=Csum + C[i][j];
    dc=C[5][j]*(dc+D[2][j]*Csum);
  } //j loop
} // Init()
  

/*********************************************************************
*          Calculates the Butterworth filter coefficients            *
* ------------------------------------------------------------------ *  
*  Calling mode:   Butterworth(Fc,Ts,n,C,NSections,Tg);              *
* ------------------------------------------------------------------ *
*  INPUTS:                                                           *
*  ------                                                            *
*         Fc.......: Cut off frequency                               *
*         Ts.......: Sampling time of input signal                   *
*         n........: Order of filter                                 *
* ------------------------------------------------------------------ *
*  OUTPUTS:                                                          *
*  -------                                                           *
*         C........: Table[1..5,1..NSections] of filter coefficients *
*                    calculated previously by BUTTERWORTH procedure  *
*         NSections: Number of required 2nd order sections (integer) *
*                    = n/2     for n even                            *
*                    = (n+1)/2 for n odd                             *
*         Tg.......: Group delay in seconds                          *
*********************************************************************/
void Butterworth(REAL Fc,REAL Ts,int n,Filter_Coef C,
                 int *NSections, REAL *Tg )  {
  int Ns2,i,Modn;
  REAL Arg,Rep,Omega,OmegaSq,temp,W0,W1,m;
  REAL HALF,ONE,TWO,ZERO,Pi;

  HALF=0.5;
  ONE =1.0;
  TWO =2.0;
  ZERO=0.0;
  Pi   = 3.1415926535;
  
  Arg=Pi*Ts*Fc;
  if (fabs(Arg) > 2.0*Pi) {
    m=int(Arg/2.0/Pi);
    Arg=Arg-(m*2.0*Pi);
  }
  Omega= tan(Arg);   
  OmegaSq=Omega*Omega;
  Modn=(n % 2);
  if (Modn==0) 
	temp=HALF;
  else 
	temp=ZERO;
  Ns2=n/2;
  *NSections=Ns2+Modn;
  *Tg=ZERO;
  if (n>1) 
    for (i=1; i<Ns2+1; i++) {
      Rep=Omega*cos(Pi*(i-temp)/n);
      *Tg=*Tg+Ts*Rep/OmegaSq;
      W0=TWO*Rep;
      W1=ONE +W0+OmegaSq;
      C[1][i]=-TWO*(OmegaSq-ONE)/W1;
      C[2][i]=-(ONE-W0+OmegaSq)/W1;
      C[3][i]=TWO;
      C[4][i]=ONE;
      C[5][i]=OmegaSq/W1;
    }
    if (temp == ZERO) {
      C[1][*NSections]=(ONE-Omega)/(ONE+Omega);
      C[2][*NSections]= ZERO;
      C[3][*NSections]= ONE;
      C[4][*NSections]= ZERO;
      C[5][*NSections]= Omega/(ONE+Omega);
      *Tg= *Tg+Ts/(TWO*Omega);
    }
} // Butterworth()


void Test_filter()  {
  extern MaxX, MaxY;
  //client drawing zone in pixels
  MaxX=(rect.right-rect.left);
  MaxY=(rect.bottom-rect.top);
  //open input and output file
  f_in = fopen("tfft.dat","r");
  f_out = fopen("tfilter.lst","w");    
  //read number of input signal points in input file
  fscanf(f_in,"%d",&ndata);
  //read ndata couples T(i), Y(i) in input data file
  for (i=1; i<ndata+1; i++) {
    fscanf(f_in,"%lf %lf",&temp,&yy);
    signal[i]=(float) yy;
    if (i==1)  tbegin=temp;
    if (i==ndata)  tend=temp;
  }
  fclose(f_in);
  //calculate sampling increment dt of input signal
  dt = (tend-tbegin)/(ndata-1);
  //input cut off frequency and order of filter (1 to 4)
  Fc=500.0; order=4; Xdc=0; //no constant value

  //call filtering procedures
  // 1. Calculate the filter coefficients
  Butterworth(Fc,dt,order,C,&NSections,&Tg);

  // 2. Initialize filter memory
  Init(Xdc,C,NSections,D);

  // 3. Recursively call Butterworth filter
  for (i=1; i<ndata+1; i++) {
    Filter(&temp,signal[i],NSections,C,D);
    filtered[i]=(float) temp;
  }

  //print input and filtered signals to output file
  t=tbegin;
  fprintf(f_out,"\n");
  fprintf(f_out,"        Time        Input Signal      Filtered Signal   \n");
  fprintf(f_out,"  ------------------------------------------------------\n");
  for (i=1; i<ndata+1; i++) {
    fprintf(f_out,"       %6.4f     %12.6f       %12.6f\n",t, signal[i], filtered[i]);
    t+=dt;
  }
  fclose(f_out);

  //shift index
  for (i=0; i<ndata; i++) {
	signal[i]=signal[i+1];
	filtered[i]=filtered[i+1];
  }

  //Draw input signal F(t)
  CourbeXY(ndata,5,signal,tbegin,tend);
  Legendes (5,"  INPUT SIGNAL (TFFT.DAT)  ","Time s","Acc. m/s2",
	        MaxX-200,50,"1024 points",
			0,0,"");
  //Draw filtered signal
  CourbeXY(ndata,3,filtered,tbegin,tend);
  Legendes (3,"  FILTERED SIGNAL (Fc=500 hz)  ","Time s","Acc. m/s2",
	        MaxX/2-200,(MaxY/2)+50,"Numerical results in file tfilter.lst.",
			0,0,"");
}
// **** End Filter section ****


//Handle window Paint and Destroy messages
long FAR PASCAL /*_export*/ WndProc(HWND hwnd,
    UINT message, UINT wParam, LONG lParam) 
{
   
    PAINTSTRUCT ps;

    switch (message) {
        case WM_PAINT:
	    hdc = BeginPaint(hwnd, &ps);
	    GetClientRect(hwnd, &rect);
            hpen = CreatePen(PS_SOLID, 1, RGB(0, 0, 255));
            hpenOld = SelectObject(hdc,hpen);

            Test_filter();
               
            SelectObject(hdc,hpenOld);
            DeleteObject(hpen);
            EndPaint(hwnd, &ps);
            return 0;
        case WM_DESTROY:
            PostQuitMessage(0);
            return 0;
    }
    return DefWindowProc(hwnd, message, wParam, lParam);
}

// main program
int PASCAL WinMain(HANDLE hInstance, HANDLE hPrevInstance,
    LPSTR lpszCmdParam, int nCmdShow)
{
    static char szAppName[] = "Tgfilter";
    HWND        hwnd;
    MSG         msg;
    WNDCLASS    wndclass;

    if (!hPrevInstance) {
        wndclass.style         = CS_HREDRAW | CS_VREDRAW;
        wndclass.lpfnWndProc   = WndProc;
        wndclass.cbClsExtra    = 0;
        wndclass.cbWndExtra    = 0;
        wndclass.hInstance     = hInstance;
        wndclass.hIcon         = LoadIcon(NULL, IDI_APPLICATION);
        wndclass.hCursor       = LoadCursor(NULL, IDC_ARROW);
        wndclass.hbrBackground = GetStockObject(WHITE_BRUSH);
        wndclass.lpszMenuName  = NULL;
        wndclass.lpszClassName = szAppName;

        RegisterClass(&wndclass);
    }

    hwnd = CreateWindow(szAppName,  // window class name
        " BUTTERWORTH FILTER ",     // window caption
        WS_OVERLAPPEDWINDOW,        // window style                       
        CW_USEDEFAULT,              // initial x position
        CW_USEDEFAULT,              // initial y position
        CW_USEDEFAULT,              // initial x size
        CW_USEDEFAULT,              // initial y size
        NULL,                       // parent window handle
        NULL,                       // window menu handle
        hInstance,                  // program instance handle
        NULL);                      // creation parameters

   ShowWindow(hwnd, nCmdShow);
   UpdateWindow(hwnd);

   while (GetMessage(&msg, NULL, 0, 0)) {
       TranslateMessage(&msg);
       DispatchMessage(&msg);
   }
   return msg.wParam;
}

//end of file tgfilter.cpp