#include <stdio.h>
#include <string.h>
#include <math.h>

#define min(x,y) ((x)<(y)?(x):(y))
#define max(x,y) ((x)>(y)?(x):(y))
#define abs(x)   ((x) >= 0 ? (x) : (-(x)))

#define PI2 (2 * 3.141592653589793238462643383279)

/**
 * Geschreven in aug/sept 2003 door KB om van C-media AD-storing (44100/4 Hz) af te komen
 */

struct RiffHeader{
  char title[4]; // RIFF
  long size; // bestandsgrootte - 8 Bytes, soms niet goed ingevuld
  char type[4]; // WAVE, hopelijk
};

struct Chunk{
  char title[4];
  long size; // van wat volgt
};

struct WaveFormat{
  short format; // 1=PCM
  short channels; // 2=stereo
  long rate; // in Hz
  long bytesps; // gemiddelde, voor leesroutine
  short align; //
  short bitsps; // 8 of 16
  char acReserved[32];
};

/**
 * Eenvoudig eerste-orde filter dat hoge frequenties verzwakt (ons stoorsignaal met 3dB);
 * daarnaast een subsonisch filter en een bandsperfilter (gelijkend op Fourier-analyse)
 */
class BandpassFilter{
  long iPrev;
  float fSubsonic; // zeer laagfrequente deel van het signaal
  float* afSin;
  float* afCos;
  float a1, a2; // coeff. van cos resp. sin
  long k; // monsternummer
  float fVarLimit;
  static const int T_SUBSONIC= 4410; // tijdconstante 0.1 sec

  public:
  BandpassFilter(){
    iPrev= 0;
    fSubsonic= 0;
    a1= a2= 0;
    k= 0;
    afCos= new float[4];
    afCos[0]= 1;
    afCos[1]= 0;
    afCos[2]= -1;
    afCos[3]= 0;
    afSin= new float[4];
    afSin[0]= 0;
    afSin[1]= 1;
    afSin[2]= 0;
    afSin[3]= -1;
    //fVarLimit= 6400.0 * 6400.0; // 20 % van maximum
    fVarLimit= 32000.0 * 32000.0;
  }
  short get(short iValue){
    int nInFilter= min(k, T_SUBSONIC); // max. T_SUBSONIC elementen in het filter
    fSubsonic = ((nInFilter * fSubsonic) + iValue) / (nInFilter + 1);
    int iSonic= iValue - (int) fSubsonic;
    int k4= (int) (k % 4L);
    float fVar= 1.0 * iValue * iValue;
    if (fVar < fVarLimit){
      // bepaal coefficienten van cos en sin
      a1= .97 * a1 + .03 * (2.0 * afCos[k4] * iValue); // tijdconstante 33/4 periodes
      a2= .97 * a2 + .03 * (2.0 * afSin[k4] * iValue); // tijdconstante 33/4 periodes
    } else{
      a1*=.97;
      a2*=.97;
    }
    k++;
    int iFilt1= (iValue - (int)fSubsonic - (int)((a1 * afCos[k4]) + (a2 * afSin[k4])));
    /* nu nog even door een laagdoorlaatfilter halen: */
    iPrev+= (iFilt1 - iPrev)/2; // halfwaardetijd gelijk aan 1/bemonsterfrequentie
    int iFilt2= iPrev;
    iFilt2= max(-32767, iFilt2);
    iFilt2= min( 32767, iFilt2);
    return (short) iFilt2;
  }
};

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

/**
 * Converteert/bewerkt WAV-files.
 */
int main(int nArgs, char* asArg[]){
  if (nArgs < 2){
    fprintf(stderr, "Parameters missing. Try cmfilter --help.\n");
    return 1;
  }
  if (!strcmp(asArg[1], "--help")){
    printf("cmfilter filters WAVE files.\n\
Usage: cmfilter SOURCE.WAV [DEST.WAV]\n\
Omitting DEST.WAV produces a time sequence of (un)filtered channels to stdout.\n");
    return 0;
  }
  int nOptions= 0;
  char* sInFile= asArg[nOptions + 1];
  char* sOutFile= asArg[nOptions + 2];

  FILE* pfIn= fopen(sInFile, "rb");
  if (!pfIn){
    perror(sInFile);
    return 1;
  }

  FILE* pfOut= sOutFile ? fopen(sOutFile, "w+b") : NULL;
  if (sOutFile && !pfOut){
    perror(sOutFile);
    return 1;
  }

  RiffHeader rh;
  fread(&rh, sizeof(RiffHeader), 1, pfIn);
  fprintf(stderr, "%.4s: %.4s\n", rh.title, rh.type);

  Chunk chunkFmt;
  /* Dat is de eerste die we nu verwachten */
  fread(&chunkFmt, sizeof(Chunk), 1, pfIn);
  //fprintf(stderr, "%.4s: %d Bytes\n", chunkFmt.title, chunkFmt.size);

  WaveFormat wfIn;
  fread(&wfIn, chunkFmt.size, 1, pfIn);
  fprintf(stderr, "%d channels, %d Hz, %d bits\n",
    wfIn.channels,
    wfIn.rate,
    wfIn.bitsps);

  bool bEOF= false;
  while(!bEOF){
    Chunk chunk;
    if (fread(&chunk, sizeof(Chunk), 1, pfIn) != 1){
      fprintf(stderr, "Unexpected EOF on %d bytes; fail.\n", ftell(pfIn));
      bEOF= true;
      break;
    }
    //fprintf(stderr, "%.4s: %d Bytes\n", chunk.title, chunk.size);
    if (!strncmp(chunk.title, "data", 4)){
      //fprintf(stderr, "Data found.\n");

      /* bepaal het werkelijk aantal monsters */
      int iOffData= ftell(pfIn);
      fseek(pfIn, 0, SEEK_END);
      long nSamples= (ftell(pfIn) - iOffData)/(wfIn.channels * sizeof(short));
      fprintf(stderr, "Total of %ld samples %s found (mm:ss = %ld:%.2ld).\n",
        nSamples,
	(wfIn.channels == 2 ? "(stereo)" : "(mono)"),
	(nSamples / wfIn.rate) / 60, (nSamples / wfIn.rate) % 60);
      fseek(pfIn, iOffData, SEEK_SET);

      /* Schrijf nu de header voor de output-.wav */
      if (pfOut){
	fwrite(&rh, sizeof(RiffHeader), 1, pfOut); // dit is de riff-header
	fwrite(&chunkFmt, sizeof(Chunk), 1, pfOut); // header van 'fmt '-chunk
	fwrite(&wfIn, chunkFmt.size, 1, pfOut); // format-data zelf
	fwrite(&chunk, sizeof(Chunk), 1, pfOut); // dit is de header vd data-chunk
      }
      BandpassFilter filterL;
      BandpassFilter filterR;
      short aiRawIn[2]; // 16 bits stereo
      int iSample= 0;

      fseek(pfIn, iOffData, SEEK_SET);

      /* Nu schrijven met correctie */
      iSample= 0;
      fprintf(stderr, "Writing filtered signal...\n");
      while(fread(&aiRawIn[0], sizeof(short), 2, pfIn) == 2){
        short aiRawOut[2]; // 16 bits stereo
	/* Ontstoring */
        aiRawOut[0]= filterL.get(aiRawIn[0]);
        aiRawOut[1]= filterR.get(aiRawIn[1]);
	if (pfOut){
          fwrite(&aiRawOut[0], sizeof(short), 2, pfOut);
	} else{
	  printf("%d\t%d\t%d\t%d\n", aiRawIn[0], aiRawOut[0], aiRawIn[1], aiRawOut[1]);
	}
        iSample++;
	if (iSample % (nSamples / 100) == 0){
	  fprintf(stderr, "\r%.2d%%", (int)(.5 + 100.0 * iSample / nSamples));
	}
      }
      fprintf(stderr, " completed.\n");
      break;
    } else{
      char* acBuf= new char[chunk.size];
      fread(acBuf, chunk.size, 1, pfIn);
    }
  }

  if (pfOut){
  fclose(pfOut);
  }
  fclose(pfIn);

  return 0;
} // einde main