/*******************************************************************

dnr is fft-based wav-file denoiser
Copyright (C) 1999 Matti Koskinen mjkoskin@sci.fi

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
*********************************************************************/

#include "snd.h"

#define MAXFFTLEN 4096
#define LOG10 2.302585093
#define TWOPI M_PI*2


static snd_plug *dnr_plug = NULL;
static double *idata1=NULL,*odata1=NULL,*cplx=NULL;
static double *re=NULL;
static double sintbl[16384];
static double lc1,lc2,yt[2],lpfc1,lpfc2,ytl,level,attack,fs1;
static int curr,wsize, samplep=0,samples_ready = 0;
static int first = 1;

static double lowp(double d, int ch) {
double y;
    y = lc1 * d + lc2 * yt[ch];
    yt[ch] = y;
    return y;
}

static double filt(double d) {
double y;
    y = lpfc1 * d + lpfc2 * ytl;
    ytl = y;
    return y;
}

static void lpcoeffs(double sr, double lpf) {
  double b;
  
  if(lpf!=1.0) {
    b = 2.0 - cos(lpf*2*M_PI/sr);
    lc2 = b - sqrt(b*b-1.0);
    lc1 = 1.0 - lc2;
  } else {
    lc2 = 0.0;
    lc1 = 1.0;
  }

}

static void lpflpfcoeffs(double sr, double lpf) {
  double b;
  
  b = 2.0 - cos(lpf*2*M_PI/sr);
  lpfc2 = b - sqrt(b*b-1.0);
  lpfc1 = 1.0 - lpfc2;

}

static void initfft(int nsamples) {
  double theta;
  int i;

  for (i=0; i<=nsamples/4; i++)
    {
      theta=TWOPI*i/( (double) nsamples);
      sintbl[i]=sin(theta);
    }
}

static void fft(double *x,double *y,int n,double *sintbl)
{
  double s1,s2,al,ak,bl,bk,tr,ti;
  int i,j,k,l,m,nt,nq,nh,nd,it,id,ir;
  nt=n-1;
  nq=nt/4+1;
  nh=nq+nq;
  j=0;
  for(i=0;i<nt-2;i++)
    {
      if (j>i)
	{
	  tr=x[j];
	  x[j]=x[i];
	  x[i]=tr;
	  ti=y[j];
	  y[j]=y[i];
	  y[i]=ti;
	}
      k=nh;
      while(j>=k)
	{
	  j=j-k;
	  k=k/2;
	}
      j=j+k;
    }
  id=1;
  ir=1;
  m=0;
  it=0;
  do
    {
      i=id;
      id=id+id;
      for(j=0;j<i;j++)
	{
	  s2=(-sintbl[m]);
	  s1=sintbl[nq-m];
	  if (j>=ir)
	    {
	      m=m-it;
	      s1=(-s1);
	    }
	  else
	    {
	      m=m+it;
	    }
	  for(nd=j;nd<=nt;nd+=id)
	    {
	      ak=x[nd];
	      bk=y[nd];
	      l=nd+i;
	      al=x[l];
	      bl=y[l];
	      tr=s1*al-s2*bl;
	      ti=s2*al+s1*bl;
	      x[l]=ak-tr;
	      y[l]=bk-ti;
	      x[nd]=ak+tr;
	      y[nd]=bk+ti;
	    }
	}
      ir=i;
      it=nq/i;
    }
  while(i<nh);
}

static void hamming(double *data,int len)
{
  int i;
  for(i=0;i<len;i++)
    data[i] = data[i]*(0.54+0.46*cos(2*M_PI*i/len));
}

static double corner(double sr, double* fftd, int len, double lim) {
  double fc0;
  int i;

  for(i=len-1;i>=0;i--) {
    if(fftd[i] > lim)
      break;
  }
  fc0 = sr / len / 2.0;
  if(i >  0)
    return fc0*i;
  else
    return 0.0;
}

static int dnr_init(snd_state *ss)
{
  printf("dnr_init\n");
  return(PLUG_OK);
}

static int dnr_start_channel(chan_info *cp) {
printf("dnr_start_channel\n");
fflush(stdout);
  re = (double *)malloc(wsize*sizeof(double));
  idata1 = (double *)malloc(wsize*sizeof(double));
  odata1 = (double *)malloc(wsize*sizeof(double));
  cplx = (double *)malloc(wsize*sizeof(double)); 
  initfft(wsize);
  return (PLUG_OK);
}

static void process_window(void) {
  int i;
  double c1,c2,lpf;
  c1 = fs1 / wsize; /* set up lpf filtering */
  lpflpfcoeffs(c1,1.0/attack);
  for(i=0;i<wsize;i++) {
    cplx[i] = 0.0;
    re[i] = idata1[i];
  }
  hamming(re,wsize);
  fft(re,cplx,wsize,sintbl); /* fft input */
  for(i=0;i<wsize/2;i++) {
    re[i] = sqrt(re[i]*re[i]+cplx[i]*cplx[i]); 
    re[i] = log(re[i])/LOG10; 
  }
  lpf = corner(fs1,re,wsize/2,level); /* find highest freq */
  lpf = filt(lpf); /* filter this to avoid abrupt changes */
  lpcoeffs(fs1,lpf); /* set low-pass */
  for(i=0;i<wsize;i++) {
    odata1[i] = lowp(idata1[i],0); /* filter input */
  }
  curr = 0;
  samples_ready = wsize;
}

static int dnr_read_sample (int val, int end_of_channel) {
  int i;
  idata1[curr] = (double)val;
  curr++;
  if(end_of_channel) {
    for(i=curr;i<wsize;i++)
      idata1[i] = 0.0;
    curr = wsize;
  }
  if(curr >= wsize) 
    process_window();
  return(PLUG_OK);
}

static int dnr_write_sample(int *val) {

  if(samples_ready > 0) {
    (*val) = odata1[samplep];
    samplep++;
    if(samplep == samples_ready) {
      samplep = 0;
      samples_ready = 0;
    }
    return(PLUG_OK);
  }
  else
    return(PLUG_NO_DATA);
}

static int dnr_quit(snd_state *ss)
{
printf("dnr_quit\n");
  if(idata1)
    free(idata1);
  if(odata1)
    free(odata1);
  if(cplx)
    free(cplx);
  if(re)
    free(re);
  idata1=NULL;
  odata1=NULL;
  cplx=NULL;
  re=NULL;
  return(PLUG_OK);
}

static int dnr_end_channel(chan_info *cp) {
printf("dnr_end_channel\n");
fflush(stdout);
  dnr_quit(cp->state);
  return(PLUG_OK);
}

SCM g_dnr(SCM s_srate, SCM s_wsize, SCM s_level, SCM s_attack) {
  
  fs1 = gh_scm2double(s_srate); /* mandatory */

  if(gh_number_p(s_wsize))
     wsize = gh_scm2int(s_wsize);
  else
    wsize = 128;

  if(gh_number_p(s_level))
    level = gh_scm2double(s_level);
  else
    level = 3.0;

  if(gh_number_p(s_attack))
    attack = gh_scm2double(s_attack);
  else
    attack = 0.9;
printf("g_dnr %f %d %f %f\n",fs1,wsize,level,attack);
 if (dnr_plug == NULL)
    {
      dnr_plug = (snd_plug *)calloc(1,sizeof(snd_plug));
      dnr_plug->init = dnr_init;
      dnr_plug->start_channel = dnr_start_channel;
      dnr_plug->end_channel = dnr_end_channel;
      dnr_plug->read_sample = dnr_read_sample;
      dnr_plug->write_sample = dnr_write_sample;
      dnr_plug->quit = dnr_quit;
      dnr_plug->name = "dnr";
      dnr_plug->documentation = "denoise tries to reduce noise";
      dnr_plug->error = NULL;
      dnr_plug->edit_name = NULL;
      scm_sysintern("dnr-plug",gh_ulong2scm((unsigned long)dnr_plug));
    }
  if (dnr_plug->edit_name) free(dnr_plug->edit_name);
  dnr_plug->edit_name = (char *)calloc(128,sizeof(char));
  sprintf(dnr_plug->edit_name,"(dnr %f %d %f %f)",fs1,wsize,level,attack);

  /* now invoke the denoiser */
  gh_eval_str("(call-plug dnr-plug)");
  return(SCM_BOOL_F);
}

void init_dnr(void)
{
  /* define "dnr" in Guile to call g_denoise and take 1 mandatory arg (srate) 
     and 3 optional arguments */
  gh_new_procedure("dnr",g_dnr,1,3,0);
}

/* Linux or SGI:
 *   cc anoi.c -c -DHAVE_GUILE=3 -o anoi.o 
 *   ld -shared anoi.o -o anoi.so
 * Sun:
 *   gcc anoi.c -c -DHAVE_GUILE=3 -o anoi.o 
 *   ld -G anoi.o -o anoi.so
 * 
 * then in Snd (this could obviously be in your Snd initialization file)
 *
 *  (define lib (dynamic-link "/space/home/bil/cl/anoi.so"))
 *     -> prints #<unspecified> in listener if all went well
 *     -> if you get can't map libguile.so set the environment variable LD_LIBRARY_PATH to its directory
 *  (dynamic-call "init_dnr" lib)
 *     -> also prints #unspecified> 
 *
 * and in the listener
 *
 *  (dnr 8000.0)
 */