/*  ALPHABET.C

  Demonstrate the alpha-beta filter with a particular
  data profile, with and without gaussian noise, and
  with different alpha-beta values.

  The graphic features here assume the presence of a
  VGA monitor.

  This code was written for Borland C++, Version 3.1,
  coded for MS-DOS.
*/

#include <graphics.h>
#include <stdlib.h>
#include <stdio.h>
#include <conio.h>
#include <alloc.h>
#include <time.h>
#include <math.h>

#define PLOT1           120    /* range of x for first
				   part of plot */
#define PLOT2           310    /* range of x for second
				   part of plot */
#define PLOT3           430    /* range of x for third
				   part of plot */
#define PLOTEND         640    /* end of x range */


#define STEADY_STATE1   120.0  /* first steady state y
				   value */
#define STEADY_STATE2   400.0  /* second steady state y
				   value */
#define STEADY_STATE3    70.0  /* third steady state y
				   value */
#define STEADY_STATEBOT 480.0  /* bottom limit for
				   steady state
				   values */

#define ALPHA           0.25   /* arbitrary alpha
				   value */

void alpha_beta(double *y,double *y_smoothed,
		double alpha,double beta);
void profile(double *y);
void gauss_noise(double *noise);
void plot_it(double *y);
void message(char *string1,char *string2,
	     char *string3);

void main(void)
{
  int x,graphdriver=DETECT,graphmode,errorcode;

  double *y_pure,*y_noisy,*y_smoothed,*noise;
  double alpha,beta;

  /**********************************/
  /* allocate memory for the arrays */
  /**********************************/

  if((y_pure =
     (double *)malloc(PLOTEND*sizeof(double)))==
     NULL) {
    printf("\a");
    cprintf("Unable to allocate space for array ");
    cprintf("y_pure[]\n\r");
    exit(1);
  }
  if((y_noisy =
     (double *)malloc(PLOTEND*sizeof(double)))==
     NULL) {
    printf("\a");
    cprintf("Unable to allocate space for array ");
    cprintf("y_noisy[]\n\r");
    exit(1);
  }
  if((y_smoothed =
     (double *)malloc(PLOTEND*sizeof(double)))==
     NULL) {
    printf("\a");
    cprintf("Unable to allocate space for array ");
    cprintf("y_smoothed[]\n\r");
    exit(1);
  }
  if((noise =
    (double *)malloc(PLOTEND*sizeof(double)))==NULL) {
    printf("\a");
    cprintf("Unable to allocate space for array ");
    cprintf("noise[]\n\r");
    exit(1);
  }

  /****************************/
  /* initialize graphics mode */
  /****************************/

  registerbgidriver(EGAVGA_driver);
  initgraph(&graphdriver,&graphmode,"");
  errorcode=graphresult();
  if(errorcode != grOk) {
    printf("\a");
    cprintf("Graphics error: %s\n\r",
	    grapherrormsg(errorcode));
    exit(1);
  }

  /*************************************************/
  /* generate the pure and corrupted data profiles */
  /*************************************************/

  /* generate the clean profile */
  profile(y_pure);

  /* generate the noise */
  gauss_noise(noise);

  /* corrupt the clean data with noise */
  for(x=0;x<PLOTEND;++x)
    y_noisy[x] = y_pure[x] + noise[x];

  /****************************************/
  /* plot a pure profile with underdamped */
  /* alpha-beta smoothing                 */
  /****************************************/

  /* plot the clean profile */
  message("Clean data profile","","");
  plot_it(y_pure);

  /* compute and plot the smoothed result
      over the clean profile */
  alpha = ALPHA; /* arbitrary alpha value */
  beta = alpha*alpha / (2.0 - alpha); /* standard,
					  underdamped
					  beta value */
  alpha_beta(y_pure,y_smoothed,alpha,beta);
  message("","with",
	  "Underdamped alpha-beta smoothing");
  plot_it(y_smoothed);

  /* clear the screen and replot
      the smoothed result */
  cleardevice();
  message("Underdamped alpha-beta smoothing of clean \
profile","","");
  plot_it(y_smoothed);

  /*****************************************/
  /* plot a noisy profile with underdamped */
  /* alpha-beta smoothing                  */
  /*****************************************/

  /* clear the screen and plot the profile */
  cleardevice();
  message("Noisy profile","","");
  plot_it(y_noisy);

  /* compute and plot the smoothed result over the
      noisy profile */
  alpha_beta(y_noisy,y_smoothed,alpha,beta);
  message("","with",
	  "Underdamped alpha-beta smoothing");
  plot_it(y_smoothed);

  /* clear the screen and replot the
      smoothed result */
  cleardevice();
  message("Underdamped alpha-beta smoothing of noisy \
profile","","");
  plot_it(y_smoothed);

  /* plot the pure profile over the estimate based on
      the noisy version */
  message(""," with","Clean data profile");
  plot_it(y_pure);

  /********************************************/
  /* plot a pure profile with near-critically */
  /* damped alpha-beta smoothing              */
  /********************************************/

  /* clear the screen and plot the clean profile */
  cleardevice();
  message("Clean data profile","","");
  plot_it(y_pure);

  /* compute and plot the smoothed result over the
      clean profile */
  alpha = ALPHA; /* arbitrary alpha value */
  beta = 0.8*(2.0 - alpha*alpha -
	 2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
  alpha_beta(y_pure,y_smoothed,alpha,beta);
  message("","with","Near-critically damped \
alpha-beta smoothing");
  plot_it(y_smoothed);

  /*********************************************/
  /* plot a noisy profile with near-critically */
  /* damped alpha-beta smoothing               */
  /*********************************************/

  /* clear the screen and plot the noisy profile */
  cleardevice();
  message("Noisy profile","","");
  plot_it(y_noisy);

  /* compute and plot the smoothed response over the
      noisy profile */
  beta = 0.8*(2.0 - alpha*alpha -
	 2.0*sqrt(1 - alpha*alpha))/(alpha*alpha);
  alpha_beta(y_noisy,y_smoothed,alpha,beta);
  message("Noisy profile","with",
	  "Near-critically damped alpha-beta \
smoothing");
  plot_it(y_smoothed);

  /* clear the screen and replot the smoothed result */
  cleardevice();
  message("Near-critically damped alpha-beta \
smoothing of noisy profile","","");
  plot_it(y_smoothed);

  /* plot the pure profile over the estimate based on
      the noisy version */
  message(""," with","Clean data profile");
  plot_it(y_pure);

  /* prepare to exit */
  closegraph();
}

void alpha_beta(double *y,double *y_smoothed,
		double alpha,double beta)

/* demonstrate the alpha-beta algorithm */

{
  int i;

  double s_est,v_est,s_pred,error;

  /* initialize the filter */
  s_est = y[0];
  v_est = 0.0;

  /* loop through all of the position measurements */
  for(i=0;i<PLOTEND;++i) {

    /***********************************/
    /* the alpha-beta filter algorithm */
    /* (with T absorbed into v)        */
    /***********************************/

    /* predict the new position */
    s_pred = s_est + v_est;

    /* compute the measurement error */
    error = y[i] - s_pred;

    /* estimate the true position and velocity */
    s_est = s_pred + alpha*error;
    v_est = v_est  +  beta*error;

    /*****************************/
    /* update the smoothed array */
    /*****************************/

    y_smoothed[i] = s_est;
  }
}

void profile(double *y)

/* generate the profile to be studied */

{
  int i;

  /* first steady-state portion */
  for(i=0;i<PLOT1;++i) y[i] = STEADY_STATE1;

  /* ramp from first steady state level to second
     steady state level */
  for(i=PLOT1;i<PLOT2;++i)
    y[i] = (i-PLOT1) * (STEADY_STATE2-STEADY_STATE1) /
		       (PLOT2-PLOT1) + STEADY_STATE1;

  /* second steady-state portion */
  for(i=PLOT2;i<PLOT3;++i) y[i] = STEADY_STATE2;

  /* step to third steady-state portion */
  for(i=PLOT3;i<PLOTEND;++i) y[i] = STEADY_STATE3;
}

void gauss_noise(double *noise)

/* generate mean-zero gaussian noise using Box-Muller
   method */

{
  int i;

  double drand1,drand2,pi,sigma=10.0,mean=0.0;

  /* define pi */
  pi = 4.0 * atan(1.0);

  /* seed the random number generator */
  randomize();

  /* generate gaussian noise using the Box-Muller
     method */
  for(i=0;i<PLOTEND;++i) {

    /* compute two double random numbers */
    drand1 = (double)rand()/RAND_MAX;
    drand2 = (double)rand()/RAND_MAX;

    /* compute the noise term */
    if(drand1 < 1e-10) drand1 = 1e-10; /* avoid
					   division
					   by zero */
    noise[i] = sqrt(2.0*log(1.0/drand1)) *
	       cos(2.0 * pi * drand2) * sigma + mean;
  }
}

void plot_it(double *y)

/* plot one member of array y for each horizontal
   pixel position */

{
  int x,y_old;

  /* plot the profile */
  y_old = (int)(y[0]+0.5);
  for(x=0;x<PLOTEND;++x) {
    line(x,y_old,x,(int)(y[x]+0.5));
    y_old = (int)(y[x]+0.5);
  }

  /* wait for key press */
  getch();
}

void message(char *string1,char *string2,
	     char *string3)

/* print a message of three lines centered at bottom of
   screen */

{
  int midx,maxy;

  /* get screen parameters */
  midx=getmaxx()/2;
  maxy=getmaxy();

  /* print pieces of message, centered */
  settextjustify(CENTER_TEXT,TOP_TEXT);
  outtextxy(midx,maxy-30,string1);
  outtextxy(midx,maxy-20,string2);
  outtextxy(midx,maxy-10,string3);
}

