/****************************************************************************
|                         Digital Audio Processor
|                         =======================
|
| Filename    : DPResample.cc
|
| Object      : None
|
| Description : DPResample functions (for resampling using phase vocoder)
|
| (c) Richard Kent 1996 and (c) Richard Moore
|
| $Id: DPResample.cc,v 1.1 2003/09/10 00:06:25 rk Exp $
|
****************************************************************************/

static char DPResample_cc [] = "$Id: DPResample.cc,v 1.1 2003/09/10 00:06:25 rk Exp $";

#include "DPResample.h"

#define FORWARD 1
#define INVERSE 0

#define RESAMPCHECK 10
// #define USEREGISTERS
// #define SNEAKYCHECK

#define FREEALL                                                             \
  convert (0,0,0,0,0,0,1);                                                  \
  oscbank (0,0,0,0,0,0,0,0,1);                                              \
  unconvert (0,0,0,0,0,0,1);                                                \
  delete [] Wanal;                                                          \
  delete [] Wsyn;                                                           \
  delete [] input;                                                          \
  delete [] buffer;                                                         \
  delete [] channel;                                                        \
  delete [] output

double PII;
double TWOPI;
int framesToWrite;
int framesWritten;

#define ALLOCATEDOUBLE(name,size)                                           \
  name = new double [(size)];                                               \
  for (i=0; i<(size); i++) name [i] = 0.0

int resample (
  DPSample *sample,
  DPSample *newSample,
  int      chan,
  int      R,
  int      N,
  int      Nw,
  int      D,
  int      I,
  double   P,
  double   synt)
{
  int    i;
  int    N2;
  int    Nw2;
  int    in;
  int    on;
  int    eof      = 0;
  int    obank    = 0;
  double *Wanal   = 0;
  double *Wsyn    = 0;
  double *input   = 0;
  double *buffer  = 0;
  double *channel = 0;
  double *output  = 0;
  
  #ifndef NOFORMS
  double percentComp;
  char tempString [20];
  #endif
  
  #ifdef SNEAKYCHECK
  XEvent newEvent;
  #endif
  
  int checkReset = RESAMPCHECK;
  int check      = 1;
  
  framesToWrite =
    newSample->getRangeEnd () - newSample->getRangeStart ();
  framesWritten = 0;
  
  PII   = 4.0 * atan (1.0);
  TWOPI = 8.0 * atan (1.0);
  if (P < 0.999999999 || P > 1.000000001) obank = 1;
  N2    = N >> 1;
  Nw2   = Nw >> 1;
  
  ALLOCATEDOUBLE (Wanal,Nw);
  ALLOCATEDOUBLE (Wsyn,Nw);
  ALLOCATEDOUBLE (input,Nw);
  ALLOCATEDOUBLE (buffer,N);
  ALLOCATEDOUBLE (channel,N+2);
  ALLOCATEDOUBLE (output,Nw);
  
  if (!Wanal || !Wsyn || !input || !buffer || !channel || !output)
  {
    delete [] Wanal;
    delete [] Wsyn;
    delete [] input;
    delete [] buffer;
    delete [] channel;
    delete [] output;
    return 0;
  }
  
  shiftin   (sample,chan,0,0,0,1);
  shiftout  (newSample,chan,0,0,0,0,1);
  convert   (0,0,0,0,0,1,0);
  oscbank   (0,0,0,0,0,0,0,1,0);
  unconvert (0,0,0,0,0,1,0);
  
  makewindows (Wanal,Wsyn,Nw,N,I);
  
  in = -Nw;
  on = (in * I) / D;
  
  while (!eof)
  {
    in += D;
    on += I;
    eof = shiftin (sample,chan,input,Nw,D,0);
    fold (input,Wanal,Nw,buffer,N,in);
    rfft (buffer,N2,FORWARD);
    if (!convert (buffer,channel,N2,D,R,0,0))
    {
      FREEALL;
      return 0;
    }
    if (obank)
    {
      if (!oscbank (channel,N2,R,I,output,P,synt,0,0))
      {
        FREEALL;
        return 0;
      }
      shiftout (newSample,chan,output,Nw,I,(in+Nw2+D),0);
    }
    else
    {
      if (!unconvert (channel,buffer,N2,I,R,0,0))
      {
        FREEALL;
        return 0;
      }
      rfft (buffer,N2,INVERSE);
      overlapadd (buffer,N,Wsyn,output,Nw,on);
      shiftout (newSample,chan,output,Nw,I,on,0);
    }
    
    if (!--check)
    {
      check = checkReset;
      #ifndef NOFORMS
      percentComp = 100.0 * framesWritten / framesToWrite;
      fl_set_slider_value (stretchForm->percentCompSlider,percentComp);
      sprintf (tempString,"%.0f",percentComp);
      fl_set_object_label (stretchForm->percentComp,tempString);

      #ifdef SNEAKYCHECK
      if (XCheckWindowEvent (fl_display,
    	stretchForm->stretchForm->window,~(int)0,&newEvent))
      {
    	XPutBackEvent (fl_display,&newEvent);
    	fl_check_forms ();
      }
      #else
      fl_check_forms ();
      #endif // SNEAKYCHECK

      if (stretchCancel)
      {
    	FREEALL;
    	return 0;
      }
      #endif // NOFORMS
    }
  }
  FREEALL;
  return 1;
}

void makewindows (
  double A [],
  double S [],
  int    Nw,
  int    N,
  int    I)
{
  int i;
  double sum;
  
  for (i=0; i<Nw; i++)
  {
    A [i] = S [i] = 0.54 - 0.46 * cos (TWOPI * i / (Nw - 1));
  }
  
  if (Nw > N)
  {
    double x;
    
    x = - (Nw - 1) / 2.0;

    for (i=0; i<Nw; i++,x+=1.0)
    {
      if (x != 0.0)
      {
        A [i] *= N * sin (PII * x / N) / (PII * x);
        S [i] *= I * sin (PII * x / I) / (PII * x);
      }
    }
  }
  
  for (sum=i=0; i<Nw; i++)
  {
    sum += A [i];
  }
  
  for (i=0; i<Nw; i++)
  {
    double afac = 2.0 / sum;
    double sfac = Nw > N ? 1.0 / afac : afac;
    
    A [i] *= afac;
    S [i] *= sfac;
  }
  
  if (Nw <= N)
  {
    for (sum=i=0; i<Nw; i+=I)
    {
      sum += S [i] * S [i];
    }
    
    for (sum=1.0/sum,i=0; i<Nw; i++)
    {
      S [i] *= sum;
    }
  }
}

int shiftin (
  DPSample *sample,
  int      channel,
  double   A [],
  int      N,
  int      D,
  int      initialise)
{
  int i;
  static int valid;
  static int frame;
  static int endFrame;
  
  if (initialise)
  {
    valid    = -1;
    frame    = sample->getRangeStart ();
    endFrame = sample->getRangeEnd ();
    return 0;
  }
  
  if (valid < 0) valid = N;
  
  for (i=0; i<N-D; i++)
  {
    A [i] = A [i+D];
  }

  if (valid == N)
  {
    for (i=N-D; i<N; i++)
    {
      if (frame >= endFrame)
      {
        valid = i;
        break;
      }
      else
        A [i] = sample->getFrameDb (frame++,channel);
    }
  }
  
  if (valid < N)
  {
    for (i=valid; i<N; i++)
    {
      A [i] = 0.0;
    }
    valid -= D;
  }
  
  return (valid <= 0);
}

void fold (
  double I [],
  double W [],
  int    Nw,
  double O [],
  int    N,
  int    n)
{
  int i;
  
  for (i=0; i<N; i++)
  {
    O [i] = 0.0;
  }
  
  while (n<0)
  {
    n += N;
  }
  
  n %= N;
  
  for (i=0; i<Nw; i++)
  {
    O [n] += I [i] * W [i];
    if (++n == N) n = 0;
  }
}

int convert (
  double S [],
  double C [],
  int    N2,
  int    D,
  int    R,
  int    initialise,
  int    remove)
{
  static int    first;
  static double *lastphase;
  static double fundamental;
  static double factor;

  double  phase;
  double  phasediff;
  double  a;
  double  b;
  int     real;
  int     imag;
  int     amp;
  int     freq;
  int     i;
  
  if (initialise)
  {
    first       = 1;
    lastphase   = 0;
    fundamental = 0.0;
    factor      = 0.0;
    return 1;
  }
  
  if (remove)
  {
    delete [] lastphase;
    lastphase = 0;
    return 1;
  }
  
  if (first)
  {
    first = 0;
    ALLOCATEDOUBLE (lastphase,N2+1);
    fundamental = (double) R / (N2 << 1);
    factor = R / (D * TWOPI);
    if (!lastphase) return 0;
  }
    
  for (i=0; i<=N2; i++)
  {
    imag = freq = (real = amp = i << 1) + 1;
    a           = (i == N2 ? S [1] : S [real]);
    b           = (i == 0 || i == N2 ? 0.0 : S [imag]);
    C [amp]     = sqrt (a*a + b*b);
    
    if (C [amp] == 0.0)
    {
      phasediff = 0.0;
    }
    else
    {
      phasediff = (phase = -atan2 (b,a)) - lastphase [i];
      lastphase [i] = phase;
      
      while (phasediff > PII)
      {
        phasediff -= TWOPI;
      }
      
      while (phasediff < -PII)
      {
        phasediff += TWOPI;
      }
    }
    
    C [freq] = phasediff * factor + i * fundamental;
  }
  return 1;
}

int oscbank (
  double C [],
  int    N,
  int    R,
  int    I,
  double O [],
  double P,
  double synt,
  int    initialise,
  int    remove)
{
  static int    NP;
  static int    L;
  static int    first;
  static double Iinv;
  static double *lastamp;
  static double *lastfreq;
  static double *index;
  static double *table;
  static double Pinc;
  
  int i;
  int amp;
  int freq;
  int n;
  int chan;
  
  if (initialise)
  {
    NP       = 0;
    L        = 8192;
    first    = 1;
    Iinv     = 0.0;
    lastamp  = 0;
    lastfreq = 0;
    index    = 0;
    table    = 0;
    Pinc     = 0.0;
    return 1;
  }
  
  if (remove)
  {
    delete [] lastamp;
    delete [] lastfreq;
    delete [] index;
    delete [] table;
    lastamp  = 0;
    lastfreq = 0;
    index    = 0;
    table    = 0;
    return 1;
  }
  
  if (first)
  {
    first = 0;
    ALLOCATEDOUBLE (lastamp,N+1);
    ALLOCATEDOUBLE (lastfreq,N+1);
    ALLOCATEDOUBLE (index,N+1);
    ALLOCATEDOUBLE (table,L);
    if (!lastamp || !lastfreq || !index || !table)
    {
      delete [] lastamp;
      delete [] lastfreq;
      delete [] index;
      delete [] table;
      lastamp  = 0;
      lastfreq = 0;
      index    = 0;
      table    = 0;
      return 0;
    }
    
    for (n=0; n<L; n++)
    {
      table [n] = N * cos (TWOPI * n / L);
    }
    
    if (P > 1.0)
      NP = (int) (N / P);
    else
      NP = N;
    
    Iinv = 1.0 / I;
    Pinc = P * L / R;
  }
  
  for (chan=0; chan<NP; chan++)
  {
    #ifdef USEREGISTERS
    register double a;
    register double ainc;
    register double f;
    register double finc;
    register double address;
    #else
    double a;
    double ainc;
    double f;
    double finc;
    double address;
    #endif
    
    freq = (amp = chan << 1) + 1;
    C [freq] *= Pinc;
    finc = (C [freq] - (f = lastfreq [chan])) * Iinv;
    
    if (C [amp] < synt) C [amp] = 0.0;
    
    ainc = (C [amp] - (a = lastamp [chan])) * Iinv;
    address = index [chan];
    
    if (ainc != 0.0 || a != 0.0)
    {
      for (n=0; n<I; n++)
      {
        O [n] += a * table [(int) address];
        address += f;

        while (address >= L)
        {
          address -= L;
        }

        while (address < 0)
        {
          address += L;
        }

        a += ainc;
        f += finc;
      }
    }
    
    lastamp [chan]  = C [amp];
    lastfreq [chan] = C [freq];
    index [chan]    = address;
  }
  return 1;
}

int unconvert (
  double C [],
  double S [],
  int    N2,
  int    I,
  int    R,
  int    initialise,
  int    remove)
{
  static int    first;
  static double *lastphase;
  static double fundamental;
  static double factor;

  double mag;
  double phase;
  int    i;
  int    real;
  int    imag;
  int    amp;
  int    freq;
  
  if (initialise)
  {
    first       = 1;
    lastphase   = 0;
    fundamental = 0.0;
    factor      = 0.0;
    return 1;
  }
  
  if (remove)
  {
    delete [] lastphase;
    lastphase = 0;
    return 1;
  }
  
  if (first)
  {
    first = 0;
    ALLOCATEDOUBLE (lastphase,N2+1);
    fundamental = (double) R / (N2 << 1);
    factor = TWOPI * I / R;
    if (!lastphase) return 0;
  }
  
  for (i=0; i<=N2; i++)
  {
    imag = freq = (real = amp = i << 1) + 1;
    if (i == N2) real = 1;
    mag = C [amp];
    lastphase [i] += C [freq] - i * fundamental;
    phase = lastphase [i] * factor;
    S [real] = mag * cos (phase);
    if (i != N2)
    {
      S [imag] = -mag * sin (phase);
    }
  }
  return 1;
}

void overlapadd (
  double I [],
  int    N,
  double W [],
  double O [],
  int    Nw,
  int    n)
{
  int i;
  
  while (n < 0)
  {
    n += N;
  }
  n %= N;
  for (i=0; i<Nw; i++)
  {
    O [i] += I [n] * W [i];
    if (++n == N) n = 0;
  }
}

void shiftout (
  DPSample *newSample,
  int      channel,
  double   A [],
  int      N,
  int      I,
  int      n,
  int      initialise)
{
  static int frame;
  static int endFrame;

  int i;
  
  if (initialise)
  {
    frame    = newSample->getRangeStart ();
    endFrame = newSample->getRangeEnd ();
    return;
  }
  
  if (n >= 0)
  {
    for (i=0; i<I; i++)
    {
      if (frame < endFrame)
      {
        newSample->setFrameDb (frame++,channel,A [i]);
        framesWritten++;
      }
    }
  }
  
  for (i=0; i<N-I; i++)
  {
    A [i] = A [i+I];
  }
  
  for (i=N-I; i<N; i++)
  {
    A [i] = 0.0;
  }
}

void rfft (
  double x [],
  int    N,
  int    forward)
{
  double c1;
  double c2;
  double h1r;
  double h1i;
  double h2r;
  double h2i;
  double wr;
  double wi;
  double wpr;
  double wpi;
  double temp;
  double theta;
  double xr;
  double xi;
  int    i;
  int    i1;
  int    i2;
  int    i3;
  int    i4;
  int    N2p1;
  
  theta = PII / N;
  wr = 1.0;
  wi = 0.0;
  c1 = 0.5;
  
  if (forward)
  {
    c2 = -0.5;
    cfft (x,N,forward);
    xr = x [0];
    xi = x [1];
  }
  else
  {
    c2    = 0.5;
    theta = -theta;
    xr    = x [1];
    xi    = 0.0;
    x [1] = 0.0;
  }
  
  temp = sin (0.5 * theta);
  wpr  = -2.0 * (temp * temp);
  wpi  = sin (theta);
  N2p1 = (N << 1) + 1;
  
  for (i=0; i<=N>>1; i++)
  {
    i1 = i << 1;
    i2 = i1 + 1;
    i3 = N2p1 - i2;
    i4 = i3 + 1;
    
    if (i == 0)
    {
      h1r =  c1 * (x [i1] + xr);
      h1i =  c1 * (x [i2] - xi);
      h2r = -c2 * (x [i2] + xi);
      h2i =  c2 * (x [i1] - xr);
      
      x [i1] = h1r + wr * h2r - wi * h2i;
      x [i2] = h1i + wr * h2i + wi * h2r;
      
      xr =  h1r - wr * h2r + wi * h2i;
      xi = -h1i + wr * h2i + wi * h2r;
    }
    else
    {
      h1r =  c1 * (x [i1] + x [i3]);
      h1i =  c1 * (x [i2] - x [i4]);
      h2r = -c2 * (x [i2] + x [i4]);
      h2i =  c2 * (x [i1] - x [i3]);
      
      x [i1] =  h1r + wr * h2r - wi * h2i;
      x [i2] =  h1i + wr * h2i + wi * h2r;
      x [i3] =  h1r - wr * h2r + wi * h2i;
      x [i4] = -h1i + wr * h2i + wi * h2r;
    }
    
    wr = (temp = wr) * wpr - wi * wpi + wr;
    wi = wi * wpr + temp * wpi + wi;
  }
  
  if (forward)
    x [1] = xr;
  else
    cfft (x,N,forward);
}

void cfft (
  double x [],
  int    NC,
  int    forward )
{
  double wr;
  double wi;
  double wpr;
  double wpi;
  double theta;
  double scale;
  double temp;
  int    mmax;
  int    ND;
  int    m;
  int    i;
  int    j;
  int    delta;
  
  ND = NC << 1;
  bitreverse (x,ND);
  
  delta = 0;
  for (mmax=2; mmax<ND; mmax=delta)
  {
    delta = mmax << 1;
    theta = TWOPI / (forward ? mmax : -mmax);
    temp = sin (0.5 * theta);
    wpr = -2.0 * (temp * temp);
    wpi = sin (theta);
    wr = 1.0;
    wi = 0.0;
    
    for (m=0; m<mmax; m+=2)
    {
      #ifdef USEREGISTERS
      register double rtemp;
      register double itemp;
      #else
      double rtemp;
      double itemp;
      #endif
      
      for (i=m; i<ND; i+=delta)
      {
        j        = i + mmax;
        rtemp    = wr * x [j]   - wi * x [j+1];
        itemp    = wr * x [j+1] + wi * x [j];
        x [j]    = x [i] - rtemp;
        x [j+1]  = x [i+1] - itemp;
        x [i]   += rtemp;
        x [i+1] += itemp;
      }
      
      wr = (rtemp = wr) * wpr - wi * wpi + wr;
      wi = wi * wpr + rtemp * wpi + wi;
    }
  }
  
  scale = forward ? 1.0 / ND : 2.0;
  for (i=0; i<ND; i++)
  {
    x [i] *= scale;
  }
}

void bitreverse (
  double x [],
  int    N)
{
  double rtemp;
  double itemp;
  int    i;
  int    j;
  int    m;
  
  m = 0;
  for (i=j=0; i<N; i+=2,j+=m)
  {
    if (j>i)
    {
      rtemp   = x [j];
      itemp   = x [j+1];
      x [j]   = x [i];
      x [j+1] = x [i+1];
      x [i]   = rtemp;
      x [i+1] = itemp;
    }
    
    for (m=N>>1; m>=2 && j>=m; m>>=1)
    {
      j -= m;
    }
  }
}

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