#include <stdio.h>
#include <math.h>
#include <stdlib.h>
int main()
{
  int frames = 256;
  short * input;
  input = (short *)malloc(frames * 4);
  short * output;
  output = (short *)malloc(frames * 4);
  
  int i;  
  int rc;
  
  float f, p, q;             //filter coefficients
  float b0, b1, b2, b3, b4;  //filter buffers (beware denormals!)
  float t1, t2;              //temporary buffers
  
  float freqc = 1.0;
  float res = .8;
  float in;
  float out;
  b0 = 0;b1 = 0;b2 = 0;b3 = 0;b4 = 0;
  int k;
  k = 441000;
  while (1){  
    rc = read(0, input, frames*4);  
    for (i = 0; i < (frames*2); i+=2)
      {
	
	freqc = freqc / 1.0001;
	if (freqc < .0001) freqc = 1.0;
	
	q = 1.0f - freqc;
	p = freqc + 0.8f * freqc * q;
	f = p + p - 1.0f;
	q = res * (1.0f + 0.5f * q * (1.0f - q + 5.6f * q * q));
	
	// Filter (in [-1.0...+1.0])

	in = ((float)input[i])/32767.0;
	in -= q * b4;                          //feedback
	t1 = b1;  b1 = (in + b0) * p - b1 * f;
	t2 = b2;  b2 = (b1 + t1) * p - b2 * f;
	t1 = b3;  b3 = (b2 + t2) * p - b3 * f;
	b4 = (b3 + t1) * p - b4 * f;
	b4 = b4 - b4 * b4 * b4 * 0.166667f;    //clipping
	b0 = in;
	out = b4;
	if (out > 1) out = 1; if (out < -1) out = -1;
	
	
	
	output[i] =  (out * 32767);
	output[i+1] =  (out * 32767);
      }
    
    rc = fwrite(output, sizeof(short),(frames*2),stdout);
    
  } //while
} //main
