#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <assert.h>
#include "paramlist.hpp"
#include "Equ.h"

int _Unwind_Resume_or_Rethrow;
int _Unwind_RaiseException;
int _Unwind_GetLanguageSpecificData;
int _Unwind_Resume;
int _Unwind_DeleteException;
int _Unwind_GetTextRelBase;
int _Unwind_SetIP;
int _Unwind_GetDataRelBase;
int _Unwind_GetRegionStart;
int _Unwind_SetGR;
int _Unwind_GetIPInfo;

void rfft(int n,int isign,REAL x[]);

#define PI 3.1415926535897932384626433832795

#if ENABLE_INT
#define RINT(x) ((x) >= 0 ? ((int)((x) + 0.5)) : ((int)((x) - 0.5)))
#endif

#define DITHERLEN 65536

#define M 15
static REAL fact[M+1];
static REAL aa = 96;
static REAL iza = 0;

// play -c 2 -r 44100 -fs -sw
#define NCH 2

#define NBANDS 17
static REAL bands[] = {
  65.406392,92.498606,130.81278,184.99721,261.62557,369.99442,523.25113,
  739.9884 ,1046.5023,1479.9768,2093.0045,2959.9536,4186.0091,5919.9072,
  8372.0181,11839.814,16744.036
};

static REAL alpha(REAL a)
{
  if (a <= 21) return 0;
  if (a <= 50) return 0.5842*pow(a-21,0.4)+0.07886*(a-21);
  return 0.1102*(a-8.7);
}

static REAL izero(REAL x)
{
  REAL ret = 1;
  int m;

  for(m=1;m<=M;m++)
    {
      REAL t;
      t = pow(x/2,m)/fact[m];
      ret += t*t;
    }

  return ret;
}

extern "C" void equ_init(SuperEqState *state, int wb)
{
  int i,j;

  if (state->lires1 != NULL)   free(state->lires1);
  if (state->lires2 != NULL)   free(state->lires2);
  if (state->rires1 != NULL)   free(state->rires1);
  if (state->rires2 != NULL)   free(state->rires2);
  if (state->irest != NULL)    free(state->irest);
  if (state->fsamples != NULL) free(state->fsamples);
#if ENABLE_INT
  if (state->inbuf != NULL)    free(state->inbuf);
#endif
#if ENABLE_REAL
  if (state->finbuf != NULL)    free(state->finbuf);
#endif
  if (state->outbuf != NULL)   free(state->outbuf);
  if (state->ditherbuf != NULL) free(state->ditherbuf);


  memset (state, 0, sizeof (SuperEqState));
  state->enable = 1;

  state->winlen = (1 << (wb-1))-1;
  state->winlenbit = wb;
  state->tabsize  = 1 << wb;

  state->lires1   = (REAL *)malloc(sizeof(REAL)*state->tabsize);
  state->lires2   = (REAL *)malloc(sizeof(REAL)*state->tabsize);
  state->rires1   = (REAL *)malloc(sizeof(REAL)*state->tabsize);
  state->rires2   = (REAL *)malloc(sizeof(REAL)*state->tabsize);
  state->irest    = (REAL *)malloc(sizeof(REAL)*state->tabsize);
  state->fsamples = (REAL *)malloc(sizeof(REAL)*state->tabsize);
#if ENABLE_INT
  state->inbuf    = (short *)calloc(state->winlen*NCH,sizeof(int));
#endif
#if ENABLE_REAL
  state->finbuf    = (REAL *)calloc(state->winlen*NCH,sizeof(REAL));
#endif
  state->outbuf   = (REAL *)calloc(state->tabsize*NCH,sizeof(REAL));
  state->ditherbuf = (REAL *)malloc(sizeof(REAL)*DITHERLEN);

  state->lires = state->lires1;
  state->rires = state->rires1;
  state->cur_ires = 1;
  state->chg_ires = 1;

  for(i=0;i<DITHERLEN;i++)
	state->ditherbuf[i] = (float(rand())/RAND_MAX-0.5);

  if (fact[0] < 1) {
      for(i=0;i<=M;i++)
      {
          fact[i] = 1;
          for(j=1;j<=i;j++) fact[i] *= j;
      }
      iza = izero(alpha(aa));
  }
}

// -(N-1)/2 <= n <= (N-1)/2
static REAL win(REAL n,int N)
{
  return izero(alpha(aa)*sqrt(1-4*n*n/((N-1)*(N-1))))/iza;
}

static REAL sinc(REAL x)
{
  return x == 0 ? 1 : sin(x)/x;
}

static REAL hn_lpf(int n,REAL f,REAL fs)
{
  REAL t = 1/fs;
  REAL omega = 2*PI*f;
  return 2*f*t*sinc(n*omega*t);
}

static REAL hn_imp(int n)
{
  return n == 0 ? 1.0 : 0.0;
}

static REAL hn(int n,paramlist &param2,REAL fs)
{
  paramlistelm *e;
  REAL ret,lhn;

  lhn = hn_lpf(n,param2.elm->upper,fs);
  ret = param2.elm->gain*lhn;

  for(e=param2.elm->next;e->next != NULL && e->upper < fs/2;e = e->next)
    {
      REAL lhn2 = hn_lpf(n,e->upper,fs);
      ret += e->gain*(lhn2-lhn);
      lhn = lhn2;
    }

  ret += e->gain*(hn_imp(n)-lhn);
  
  return ret;
}

void process_param(REAL *bc,paramlist *param,paramlist &param2,REAL fs,int ch)
{
  paramlistelm **pp,*p,*e,*e2;
  int i;

  delete param2.elm;
  param2.elm = NULL;

  for(i=0,pp=&param2.elm;i<=NBANDS;i++,pp = &(*pp)->next)
  {
    (*pp) = new paramlistelm;
	(*pp)->lower = i == 0        ?  0 : bands[i-1];
	(*pp)->upper = i == NBANDS-1 ? fs : bands[i  ];
	(*pp)->gain  = bc[i];
  }
  
  for(e = param->elm;e != NULL;e = e->next)
  {
	if ((ch == 0 && !e->left) || (ch == 1 && !e->right)) continue;
	if (e->lower >= e->upper) continue;

	for(p=param2.elm;p != NULL;p = p->next)
		if (p->upper > e->lower) break;

	while(p != NULL && p->lower < e->upper)
	{
		if (e->lower <= p->lower && p->upper <= e->upper) {
			p->gain *= pow(10,e->gain/20);
			p = p->next;
			continue;
		}
		if (p->lower < e->lower && e->upper < p->upper) {
			e2 = new paramlistelm;
			e2->lower = e->upper;
			e2->upper = p->upper;
			e2->gain  = p->gain;
			e2->next  = p->next;
			p->next   = e2;

			e2 = new paramlistelm;
			e2->lower = e->lower;
			e2->upper = e->upper;
			e2->gain  = p->gain * pow(10,e->gain/20);
			e2->next  = p->next;
			p->next   = e2;

			p->upper  = e->lower;

			p = p->next->next->next;
			continue;
		}
		if (p->lower < e->lower) {
			e2 = new paramlistelm;
			e2->lower = e->lower;
			e2->upper = p->upper;
			e2->gain  = p->gain * pow(10,e->gain/20);
			e2->next  = p->next;
			p->next   = e2;

			p->upper  = e->lower;
			p = p->next->next;
			continue;
		}
		if (e->upper < p->upper) {
			e2 = new paramlistelm;
			e2->lower = e->upper;
			e2->upper = p->upper;
			e2->gain  = p->gain;
			e2->next  = p->next;
			p->next   = e2;

			p->upper  = e->upper;
			p->gain   = p->gain * pow(10,e->gain/20);
			p = p->next->next;
			continue;
		}
		abort();
	}
  }
}

extern "C" void equ_makeTable(SuperEqState *state, REAL *lbc,REAL *rbc,void *_param,REAL fs)
{
    paramlist *param = (paramlist *)_param;
  int i,cires = state->cur_ires;
  REAL *nires;

  if (fs <= 0) return;

  paramlist param2;

  // L

  process_param(lbc,param,param2,fs,0);
  
  for(i=0;i<state->winlen;i++)
    state->irest[i] = hn(i-state->winlen/2,param2,fs)*win(i-state->winlen/2,state->winlen);

  for(;i<state->tabsize;i++)
    state->irest[i] = 0;

  rfft(state->tabsize,1,state->irest);

  nires = cires == 1 ? state->lires2 : state->lires1;

  for(i=0;i<state->tabsize;i++)
    nires[i] = state->irest[i];

  process_param(rbc,param,param2,fs,1);

  // R
  
  for(i=0;i<state->winlen;i++)
    state->irest[i] = hn(i-state->winlen/2,param2,fs)*win(i-state->winlen/2,state->winlen);

  for(;i<state->tabsize;i++)
    state->irest[i] = 0;

  rfft(state->tabsize,1,state->irest);

  nires = cires == 1 ? state->rires2 : state->rires1;

  for(i=0;i<state->tabsize;i++)
    nires[i] = state->irest[i];
   
  //
  
  state->chg_ires = cires == 1 ? 2 : 1;
}

extern "C" void equ_quit(SuperEqState *state)
{
  free(state->lires1);
  free(state->lires2);
  free(state->rires1);
  free(state->rires2);
  free(state->irest);
  free(state->fsamples);
#if ENABLE_INT
  free(state->inbuf);
#endif
#if ENABLE_REAL
  free(state->finbuf);
#endif
  free(state->outbuf);
  free(state->ditherbuf);

  state->lires1   = NULL;
  state->lires2   = NULL;
  state->rires1   = NULL;
  state->rires2   = NULL;
  state->irest    = NULL;
  state->fsamples = NULL;
#if ENABLE_INT
  state->inbuf    = NULL;
#endif
#if ENABLE_REAL
  state->finbuf    = NULL;
#endif
  state->outbuf   = NULL;

  rfft(0,0,NULL);
}

extern "C" void equ_clearbuf(SuperEqState *state)
{
	int i;

	state->nbufsamples = 0;
	for(i=0;i<state->tabsize*NCH;i++) state->outbuf[i] = 0;
}

#if ENABLE_REAL
extern "C" int equ_modifySamples_float (SuperEqState *state, char *buf,int nsamples,int nch)
{
  int i,p,ch;
  REAL *ires;
  float amax = 1.0f;
  float amin = -1.0f;
  static float hm1 = 0, hm2 = 0;

  if (state->chg_ires) {
	  state->cur_ires = state->chg_ires;
	  state->lires = state->cur_ires == 1 ? state->lires1 : state->lires2;
	  state->rires = state->cur_ires == 1 ? state->rires1 : state->rires2;
	  state->chg_ires = 0;
  }

  p = 0;

  while(state->nbufsamples+nsamples >= state->winlen)
    {
		for(i=0;i<(state->winlen-state->nbufsamples)*nch;i++)
			{
                state->finbuf[state->nbufsamples*nch+i] = ((float *)buf)[i+p*nch];
				float s = state->outbuf[state->nbufsamples*nch+i];
				//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
				if (s < amin) s = amin;
				if (amax < s) s = amax;
				((float *)buf)[i+p*nch] = s;
			}
		for(i=state->winlen*nch;i<state->tabsize*nch;i++)
			state->outbuf[i-state->winlen*nch] = state->outbuf[i];


      p += state->winlen-state->nbufsamples;
      nsamples -= state->winlen-state->nbufsamples;
      state->nbufsamples = 0;

      for(ch=0;ch<nch;ch++)
		{
			ires = ch == 0 ? state->lires : state->rires;

				for(i=0;i<state->winlen;i++)
					state->fsamples[i] = state->finbuf[nch*i+ch];

			for(i=state->winlen;i<state->tabsize;i++)
				state->fsamples[i] = 0;

			if (state->enable) {
				rfft(state->tabsize,1,state->fsamples);

				state->fsamples[0] = ires[0]*state->fsamples[0];
				state->fsamples[1] = ires[1]*state->fsamples[1]; 
			
				for(i=1;i<state->tabsize/2;i++)
					{
						REAL re,im;

						re = ires[i*2  ]*state->fsamples[i*2] - ires[i*2+1]*state->fsamples[i*2+1];
						im = ires[i*2+1]*state->fsamples[i*2] + ires[i*2  ]*state->fsamples[i*2+1];

						state->fsamples[i*2  ] = re;
						state->fsamples[i*2+1] = im;
					}

				rfft(state->tabsize,-1,state->fsamples);
			} else {
				for(i=state->winlen-1+state->winlen/2;i>=state->winlen/2;i--) state->fsamples[i] = state->fsamples[i-state->winlen/2]*state->tabsize/2;
				for(;i>=0;i--) state->fsamples[i] = 0;
			}

			for(i=0;i<state->winlen;i++) state->outbuf[i*nch+ch] += state->fsamples[i]/state->tabsize*2;

			for(i=state->winlen;i<state->tabsize;i++) state->outbuf[i*nch+ch] = state->fsamples[i]/state->tabsize*2;
		}
    }

		for(i=0;i<nsamples*nch;i++)
			{
				state->finbuf[state->nbufsamples*nch+i] = ((float *)buf)[i+p*nch];
				float s = state->outbuf[state->nbufsamples*nch+i];
				if (state->dither) {
					float u;
					s -= hm1;
					u = s;
//					s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					hm1 = s - u;
					((float *)buf)[i+p*nch] = s;
				} else {
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					((float *)buf)[i+p*nch] = s;
				}
			}

  p += nsamples;
  state->nbufsamples += nsamples;

  return p;
}
#endif

#if ENABLE_INT
extern "C" int equ_modifySamples(char *buf,int nsamples,int nch,int bps)
{
  int i,p,ch;
  REAL *ires;
  int amax =  (1 << (bps-1))-1;
  int amin = -(1 << (bps-1));
  static float hm1 = 0, hm2 = 0;

  if (chg_ires) {
	  cur_ires = chg_ires;
	  lires = cur_ires == 1 ? lires1 : lires2;
	  rires = cur_ires == 1 ? rires1 : rires2;
	  chg_ires = 0;
  }

  p = 0;

  while(nbufsamples+nsamples >= winlen)
    {
	  switch(bps)
	  {
	  case 8:
		for(i=0;i<(winlen-nbufsamples)*nch;i++)
			{
				inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
				float s = outbuf[nbufsamples*nch+i];
				if (dither) {
					float u;
					s -= hm1;
					u = s;
					s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					s = RINT(s);
					hm1 = s - u;
					((unsigned char *)buf)[i+p*nch] = s + 0x80;
				} else {
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					((unsigned char *)buf)[i+p*nch] = RINT(s) + 0x80;
				}
			}
		for(i=winlen*nch;i<tabsize*nch;i++)
			outbuf[i-winlen*nch] = outbuf[i];

		break;

	  case 16:
		for(i=0;i<(winlen-nbufsamples)*nch;i++)
			{
				inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
				float s = outbuf[nbufsamples*nch+i];
				if (dither) {
					float u;
					s -= hm1;
					u = s;
					s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					s = RINT(s);
					hm1 = s - u;
					((short *)buf)[i+p*nch] = s;
				} else {
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					((short *)buf)[i+p*nch] = RINT(s);
				}
			}
		for(i=winlen*nch;i<tabsize*nch;i++)
			outbuf[i-winlen*nch] = outbuf[i];

		break;

	  case 24:
		for(i=0;i<(winlen-nbufsamples)*nch;i++)
			{
				((int *)inbuf)[nbufsamples*nch+i] =
					(((unsigned char *)buf)[(i+p*nch)*3  ]      ) +
					(((unsigned char *)buf)[(i+p*nch)*3+1] <<  8) +
					(((  signed char *)buf)[(i+p*nch)*3+2] << 16) ;

				float s = outbuf[nbufsamples*nch+i];
				//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
				if (s < amin) s = amin;
				if (amax < s) s = amax;
				int s2 = RINT(s);
				((signed char *)buf)[(i+p*nch)*3  ] = s2 & 255; s2 >>= 8;
				((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
				((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
			}
		for(i=winlen*nch;i<tabsize*nch;i++)
			outbuf[i-winlen*nch] = outbuf[i];

		break;

	  default:
		assert(0);
	  }

      p += winlen-nbufsamples;
      nsamples -= winlen-nbufsamples;
      nbufsamples = 0;

      for(ch=0;ch<nch;ch++)
		{
			ires = ch == 0 ? lires : rires;

			if (bps == 24) {
				for(i=0;i<winlen;i++)
					fsamples[i] = ((int *)inbuf)[nch*i+ch];
			} else {
				for(i=0;i<winlen;i++)
					fsamples[i] = inbuf[nch*i+ch];
			}

			for(i=winlen;i<tabsize;i++)
				fsamples[i] = 0;

			if (enable) {
				rfft(tabsize,1,fsamples);

				fsamples[0] = ires[0]*fsamples[0];
				fsamples[1] = ires[1]*fsamples[1]; 
			
				for(i=1;i<tabsize/2;i++)
					{
						REAL re,im;

						re = ires[i*2  ]*fsamples[i*2] - ires[i*2+1]*fsamples[i*2+1];
						im = ires[i*2+1]*fsamples[i*2] + ires[i*2  ]*fsamples[i*2+1];

						fsamples[i*2  ] = re;
						fsamples[i*2+1] = im;
					}

				rfft(tabsize,-1,fsamples);
			} else {
				for(i=winlen-1+winlen/2;i>=winlen/2;i--) fsamples[i] = fsamples[i-winlen/2]*tabsize/2;
				for(;i>=0;i--) fsamples[i] = 0;
			}

			for(i=0;i<winlen;i++) outbuf[i*nch+ch] += fsamples[i]/tabsize*2;

			for(i=winlen;i<tabsize;i++) outbuf[i*nch+ch] = fsamples[i]/tabsize*2;
		}
    }

	switch(bps)
	  {
	  case 8:
		for(i=0;i<nsamples*nch;i++)
			{
				inbuf[nbufsamples*nch+i] = ((unsigned char *)buf)[i+p*nch] - 0x80;
				float s = outbuf[nbufsamples*nch+i];
				if (dither) {
					float u;
					s -= hm1;
					u = s;
					s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					s = RINT(s);
					hm1 = s - u;
					((unsigned char *)buf)[i+p*nch] = s + 0x80;
				} else {
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					((unsigned char *)buf)[i+p*nch] = RINT(s) + 0x80;
				}
			}
		break;

	  case 16:
		for(i=0;i<nsamples*nch;i++)
			{
				inbuf[nbufsamples*nch+i] = ((short *)buf)[i+p*nch];
				float s = outbuf[nbufsamples*nch+i];
				if (dither) {
					float u;
					s -= hm1;
					u = s;
					s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					s = RINT(s);
					hm1 = s - u;
					((short *)buf)[i+p*nch] = s;
				} else {
					if (s < amin) s = amin;
					if (amax < s) s = amax;
					((short *)buf)[i+p*nch] = RINT(s);
				}
			}
		break;

	  case 24:
		for(i=0;i<nsamples*nch;i++)
			{
				((int *)inbuf)[nbufsamples*nch+i] =
					(((unsigned char *)buf)[(i+p*nch)*3  ]      ) +
					(((unsigned char *)buf)[(i+p*nch)*3+1] <<  8) +
					(((  signed char *)buf)[(i+p*nch)*3+2] << 16) ;

				float s = outbuf[nbufsamples*nch+i];
				//if (dither) s += ditherbuf[(ditherptr++) & (DITHERLEN-1)];
				if (s < amin) s = amin;
				if (amax < s) s = amax;
				int s2 = RINT(s);
				((signed char *)buf)[(i+p*nch)*3  ] = s2 & 255; s2 >>= 8;
				((signed char *)buf)[(i+p*nch)*3+1] = s2 & 255; s2 >>= 8;
				((signed char *)buf)[(i+p*nch)*3+2] = s2 & 255;
			}
		break;

	  default:
		assert(0);
	}

  p += nsamples;
  nbufsamples += nsamples;

  return p;
}
#endif

#if 0
void usage(void)
{
  fprintf(stderr,"Ouch!\n");
}

int main(int argc,char **argv)
{
  FILE *fpi,*fpo;
  char buf[576*2*2];

  static REAL bc[] =
  {1.0,  0,1.0,  0,1.0,  0,1.0,  0,1.0,  0,1.0,  0,1.0,  0,1.0,  0,1.0,  0};

  init(14);
  makeTable(bc,44100);

  if (argc != 3 && argc != 4) exit(-1);

  fpi = fopen(argv[1],"r");
  fpo = fopen(argv[2],"w");

  if (!fpi || !fpo) exit(-1);

  /* generate wav header */

  {
    short word;
    int dword;

    fwrite("RIFF",4,1,fpo);
    dword = 0;
    fwrite(&dword,4,1,fpo);

    fwrite("WAVEfmt ",8,1,fpo);
    dword = 16;
    fwrite(&dword,4,1,fpo);
    word = 1;
    fwrite(&word,2,1,fpo);  /* format category, PCM */
    word = 2;
    fwrite(&word,2,1,fpo);  /* channels */
    dword = 44100;
    fwrite(&dword,4,1,fpo); /* sampling rate */
    dword = 44100*2*2;
    fwrite(&dword,4,1,fpo); /* bytes per sec */
    word = 4;
    fwrite(&word,2,1,fpo);  /* block alignment */
    word = 16;
    fwrite(&word,2,1,fpo);  /* ??? */

    fwrite("data",4,1,fpo);
    dword = 0;
    fwrite(&dword,4,1,fpo);
  }

  preamp = 65536;
  maxamp = 0;

  if (argc == 4) {
    preamp = 32767*65536/atoi(argv[3]);
    fprintf(stderr,"preamp = %d\n",preamp);
  }

  for(;;)
    {
      int n,m;

      n = fread(buf,1,576*2*2,fpi);
      if (n <= 0) break;
      m = modifySamples((short *)buf,n/4,2);
      fwrite(buf,4,m,fpo);
    }

#if 0
  for(;;)
    {
      int n = flushbuf((short *)buf,576);
      if (n == 0) break;
      fwrite(buf,4,n,fpo);
    }
#endif

  {
    short word;
    int dword;
    int len = ftell(fpo);

    fseek(fpo,4,SEEK_SET);
    dword = len-8;
    fwrite(&dword,4,1,fpo);

    fseek(fpo,40,SEEK_SET);
    dword = len-44;
    fwrite(&dword,4,1,fpo);
  }

  if (maxamp != 0) {
    fprintf(stderr,"maxamp = %d\n",maxamp);
  }

  quit();
}
#endif

extern "C" void *paramlist_alloc (void) {
    return (void *)(new paramlist);
}
extern "C" void paramlist_free (void *pl) {
    delete ((paramlist *)pl);
}