annotate procedures.cpp @ 13:de3961f74f30 tip

Add Linux/gcc Makefile; build fix

/*
    Harmonic sinusoidal modelling and tools

    C++ code package for harmonic sinusoidal modelling and relevant signal processing.
    Centre for Digital Music, Queen Mary, University of London.
    This file copyright 2011 Wen Xue.

    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.
*/
//---------------------------------------------------------------------------

#include <math.h>
#include <string.h>
#include <stddef.h>
#include "procedures.h"
#include "matrix.h"
#include "opt.h"
#include "sinest.h"

/** \file procedures.h */

//---------------------------------------------------------------------------
//TGMM methods

//method TGMM::TGMM: default constructor
TGMM::TGMM()
{
  p=0, m=dev=0;
}//TGMM

//method GMM:~TGMM: default destructor
TGMM::~TGMM()
{
  ReleaseGMM(p, m, dev)
};

//---------------------------------------------------------------------------
//TFSpans methods

//method TTFSpans: default constructor
TTFSpans::TTFSpans()
{
  Count=0;
  Capacity=100;
  Items=new TTFSpan[Capacity];
}//TTFSpans

//method ~TTFSpans: default destructor
TTFSpans::~TTFSpans()
{
  delete[] Items;
}//~TTFSpans

/**
  method Add: add a new span to the list

  In: ATFSpan: the new span to add
*/
void TTFSpans::Add(TTFSpan& ATFSpan)
{
  if (Count==Capacity)
  {
    int OldCapacity=Capacity;
    Capacity+=50;
    TTFSpan* NewItems=new TTFSpan[Capacity];
    memcpy(NewItems, Items, sizeof(TTFSpan)*OldCapacity);
    delete[] Items;
    Items=NewItems;
  }
  Items[Count]=ATFSpan;
  Count++;
}//Add

/**
  method Clear: discard the current content without freeing memory.
*/
void TTFSpans::Clear()
{
  Count=0;
}//Clear

/**
  method Delete: delete a span from current list

  In: Index: index to the span to delete
*/
int TTFSpans::Delete(int Index)
{
  if (Index<0 || Index>=Count)
    return 0;
  memmove(&Items[Index], &Items[Index+1], sizeof(TTFSpan)*(Count-1-Index));
  Count--;
  return 1;
}//Delete

//---------------------------------------------------------------------------
//SpecTrack methods

/**
  method TSpecTrack::Add: adds a SpecPeak to the track.

  In: APeak: the SpecPeak to add.
*/
int TSpecTrack::Add(TSpecPeak& APeak)
{
  if (Count>=Capacity)
  {
    Peaks=(TSpecPeak*)realloc(Peaks, sizeof(TSpecPeak)*(Capacity*2));
    Capacity*=2;
  }
  int ind=LocatePeak(APeak);
  if (ind<0)
  {
    InsertPeak(APeak, -ind-1);
    ind=-ind-1;
  }

  int t=APeak.t;
  double f=APeak.f;
  if (Count==1) t1=t2=t, fmin=fmax=f;
  else
  {
    if (t<t1) t1=t;
    else if (t>t2) t2=t;
    if (f<fmin) fmin=f;
    else if (f>fmax) fmax=f;
  }
  return ind;
}//Add

/**
  method TSpecTrack::TSpecTrack: creates a SpecTrack with an inital capacity.

  In: ACapacity: initial capacity, i.e. the number SpecPeak's to allocate storage space for.
*/
TSpecTrack::TSpecTrack(int ACapacity)
{
  Count=0;
  Capacity=ACapacity;
  Peaks=new TSpecPeak[Capacity];
}//TSpecTrack

//method TSpecTrack::~TSpecTrack: default destructor.
TSpecTrack::~TSpecTrack()
{
  delete[] Peaks;
}//TSpecTrack

/**
  method InsertPeak: inserts a new SpecPeak into the track at a given index. Internal use only.

  In: APeak: the SpecPeak to insert.
      index: the position in the list to place the new SpecPeak. Original SpecPeak's at and after this
        position are shifted by 1 posiiton.
*/
void TSpecTrack::InsertPeak(TSpecPeak& APeak, int index)
{
  memmove(&Peaks[index+1], &Peaks[index], sizeof(TSpecPeak)*(Count-index));
  Peaks[index]=APeak;
  Count++;
}//InsertPeak

/**
  method TSpecTrack::LocatePeak: looks for a SpecPeak in the track that has the same time (t) as APeak.

  In: APeak: a SpecPeak

  Returns the index in this track of the first SpecPeak that has the same time stamp as APeak. However,
  if there is no peak with that time stamp, the method returns -1 if APeaks comes before the first
  SpecPeak, -2 if between 1st and 2nd SpecPeak's, -3 if between 2nd and 3rd SpecPeak's, etc.
*/
int TSpecTrack::LocatePeak(TSpecPeak& APeak)
{
  if (APeak.t<Peaks[0].t) return -1;
  if (APeak.t>Peaks[Count-1].t) return -Count-1;

  if (APeak.t==Peaks[0].t) return 0;
  else if (APeak.t==Peaks[Count-1].t) return Count-1;

  int a=0, b=Count-1, c=(a+b)/2;
  while (a<c)
  {
    if (APeak.t==Peaks[c].t) return c;
    else if (APeak.t<Peaks[c].t) {b=c; c=(a+b)/2;}
    else {a=c; c=(a+b)/2;}
  }
  return -a-2;
}//LocatePeak

//---------------------------------------------------------------------------
/**
  function: ACPower: AC power

  In: data[Count]: a signal

  Returns the power of its AC content.
*/
double ACPower(double* data, int Count, void*)
{
  if (Count<=0) return 0;
  double power=0, avg=0, tmp;
  for (int i=0; i<Count; i++)
  {
    tmp=*(data++);
    power+=tmp*tmp;
    avg+=tmp;
  }
  power=(power-avg*avg/Count)/Count;
  return power;
}//ACPower

//---------------------------------------------------------------------------
/**
  function Add: vector addition

  In: dest[Count], source[Count]: two vectors
  Out: dest[Count]: their sum

  No return value.
*/
void Add(double* dest, double* source, int Count)
{
  for (int i=0; i<Count; i++) *(dest++)+=*(source++);
}//Add

/**
  function Add: vector addition

  In: addend[count], adder[count]: two vectors
  Out: out[count]: their sum

  No return value.
*/
void Add(double* out, double* addend, double* adder, int count)
{
  for (int i=0; i<count; i++) *(out++)=*(addend++)+*(adder++);
}//Add

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

/**
  function ApplyWindow: applies window function to signal buffer.

  In: Buffer[Size]: signal to be windowed
      Weight[Size]: the window
  Out: OutBuffer[Size]: windowed signal

  No return value;
*/
void ApplyWindow(double* OutBuffer, double* Buffer, double* Weights, int Size)
{
  for (int i=0; i<Size; i++) *(OutBuffer++)=*(Buffer++)**(Weights++);
}//ApplyWindow

//---------------------------------------------------------------------------
/**
  function Avg: average

  In: data[Count]: a data array

  Returns the average of the array.
*/
double Avg(double* data, int Count, void*)
{
  if (Count<=0) return 0;
  double avg=0;
  for (int i=0; i<Count; i++) avg+=*(data++);
  avg/=Count;
  return avg;
}//Avg

//---------------------------------------------------------------------------
/**
  function AvgFilter: get slow-varying wave trace by averaging

  In: data[Count]: input signal
      HWid: half the size of the averaging window
  Out: datout[Count]: the slow-varying part of data[].

  No return value.
*/
void AvgFilter(double* dataout, double* data, int Count, int HWid)
{
  double sum=0;

  dataout[0]=data[0];

  for (int i=1; i<=HWid; i++)
  {
    sum+=data[2*i-1]+data[2*i];
    dataout[i]=sum/(2*i+1);
  }

  for (int i=HWid+1; i<Count-HWid; i++)
  {
    sum=sum+data[i+HWid]-data[i-HWid-1];
    dataout[i]=sum/(2*HWid+1);
  }

  for (int i=Count-HWid; i<Count; i++)
  {
    sum=sum-data[2*i-Count-1]-data[2*i-Count];
    dataout[i]=sum/(2*(Count-i)-1);
  }
}//AvgFilter

//---------------------------------------------------------------------------
/**
  function CalculateSpectrogram: computes the spectrogram of a signal

  In: data[Count]: the time-domain signal
      start, end: start and end points marking the section for which the spectrogram is to be computed
      Wid, Offst: frame size and hop size
      Window: window function
      amp: a pre-amplifier
      half: specifies if the spectral values at Wid/2 are to be retried
  Out: Spec[][Wid/2] or Spec[][Wid/2+1]: amplitude spectrogram
       ph[][][Wid/2] or Ph[][Wid/2+1]: phase spectrogram

  No return value. The caller is repsonse to arrange storage spance of output buffers.
*/
void CalculateSpectrogram(double* data, int Count, int start, int end, int Wid, int Offst, double* Window, double** Spec, double** Ph, double amp, bool half)
{
  AllocateFFTBuffer(Wid, fft, w, x);

  int Fr=(end-start-Wid)/Offst+1;

  for (int i=0; i<Fr; i++)
  {
    RFFTCW(&data[i*Offst+start], Window, 0, 0, Log2(Wid), w, x);

    if (Spec)
    {
      for (int j=0; j<Wid/2; j++)
        Spec[i][j]=sqrt(x[j].x*x[j].x+x[j].y*x[j].y)*amp;
      if (half)
        Spec[i][Wid/2]=sqrt(x[Wid/2].x*x[Wid/2].x+x[Wid/2].y*x[Wid/2].y)*amp;
    }
    if (Ph)
    {
      for (int j=0; j<=Wid/2; j++)
        Ph[i][j]=Atan2(x[j].y, x[j].x);
      if (half)
        Ph[i][Wid/2]=Atan2(x[Wid/2].y, x[Wid/2].x);
    }
  }
  FreeFFTBuffer(fft);
}//CalculateSpectrogram

//---------------------------------------------------------------------------
/**
  function Conv: simple convolution

  In: in1[N1], in2[N2]: two sequences
  Out: out[N1+N2-1]: their convolution

  No return value.
*/
void Conv(double* out, int N1, double* in1, int N2, double* in2)
{
  int N=N1+N1-1;
  memset(out, 0, sizeof(double)*N);
  for (int n1=0; n1<N1; n1++)
    for (int n2=0; n2<N2; n2++)
      out[n1+n2]+=in1[n1]*in2[n2];
}//Conv

//---------------------------------------------------------------------------
/**
  function Correlation: computes correlation coefficient of 2 vectors a & b, equals cos(aOb).

  In: a[Count], b[Count]: two vectors

  Returns their correlation coefficient.
*/
double Correlation(double* a, double* b, int Count)
{
  double aa=0, bb=0, ab=0;
  for (int i=0; i<Count; i++)
  {
    aa+=*a**a;
    bb+=*b**b;
    ab+=*(a++)**(b++);
  }
  return ab/sqrt(aa*bb);
}//Correlation

//---------------------------------------------------------------------------
/**
  function DCAmplitude: DC amplitude

  In: data[Count]: a signal

  Returns its DC amplitude (=AC amplitude without DC removing)
*/
double DCAmplitude(double* data, int Count, void*)
{
  if (Count<=0) return 0;
  double power=0, tmp;
  for (int i=0; i<Count; i++)
  {
    tmp=*(data++);
    power+=tmp*tmp;
  }
  power/=Count;
  return sqrt(2*power);
}//DCAmplitude

/**
  function DCPower: DC power

  In: data[Count]: a signal

  Returns its DC power.
*/
double DCPower(double* data, int Count, void*)
{
  if (Count<=0) return 0;
  double power=0, tmp;
  for (int i=0; i<Count; i++)
  {
    tmp=*(data++);
    power+=tmp*tmp;
  }
  power/=Count;
  return power;
}//DCPower

//---------------------------------------------------------------------------
/**
  DCT: discrete cosine transform, direct computation. For fast DCT, see fft.cpp.

  In: input[N]: a signal
  Out: output[N]: its DCT

  No return value.
*/
void DCT( double* output, double* input, int N)
{
  double Wn;

  for (int n=0; n<N; n++)
  {
    output[n]=0;
    Wn=n*M_PI/2/N;
    for (int k=0; k<N; k++)
      output[n]+=input[k]*cos((2*k+1)*Wn);
    if (n==0) output[n]*=1.4142135623730950488016887242097/N;
    else output[n]*=2.0/N;
  }
}//DCT

/**
  function IDCT: inverse discrete cosine transform, direct computation. For fast IDCT, see fft.cpp.

  In: input[N]: a signal
  Out: output[N]: its IDCT

  No return value.
*/
void IDCT(double* output, double* input, int N)
{
  for (int k=0; k<N; k++)
  {
    double Wk=(2*k+1)*M_PI/2/N;
    output[k]=input[0]/1.4142135623730950488016887242097;
    for (int n=1; n<N; n++)
      output[k]+=input[n]*cos(n*Wk);
  }
}//IDCT

//---------------------------------------------------------------------------
/**
  function DeDC: removes DC component of a signal

  In: data[Count]: the signal
      HWid: half of averaging window size
  Out: data[Count]: de-DC-ed signal

  No return value.
*/
void DeDC(double* data, int Count, int HWid)
{
  double* data2=new double[Count];
  AvgFilter(data2, data, Count, HWid);
  for (int i=0; i<Count; i++)
    *(data++)-=*(data2++);
  delete[] data2;
}//DeDC

/**
  function DeDC_static: removes DC component statically

  In: data[Count]: the signal
  Out: data[Count]: DC-removed signal

  No return value.
*/
void DeDC_static(double* data, int Count)
{
  double avg=Avg(data, Count, 0);
  for (int i=0; i<Count; i++) *(data++)-=avg;
}//DeDC_static

//---------------------------------------------------------------------------
/**
  function DoubleToInt: converts double-precision floating point array to integer array

  In: in[Count]: the double array
      BytesPerSample: bytes per sample of destination integers
  Out: out[Count]: the integer array

  No return value.
*/
void DoubleToInt(void* out, int BytesPerSample, double* in, int Count)
{
  if (BytesPerSample==1){unsigned char* out8=(unsigned char*)out; for (int k=0; k<Count; k++) *(out8++)=*(in++)+128.5;}
  else {__int16* out16=(__int16*)out; for (int k=0; k<Count; k++) *(out16++)=floor(*(in++)+0.5);}
}//DoubleToInt

/**
  function DoubleToIntAdd: adds double-precision floating point array to integer array

  In: in[Count]: the double array
      out[Count]: the integer array
      BytesPerSample: bytes per sample of destination integers
  Out: out[Count]: the sum of the two arrays

  No return value.
*/
void DoubleToIntAdd(void* out, int BytesPerSample, double* in, int Count)
{
  if (BytesPerSample==1)
  {
    unsigned char* out8=(unsigned char*)out;
    for (int k=0; k<Count; k++){*out8=*out8+*in+128.5; out8++; in++;}
  }
  else
  {
    __int16* out16=(__int16*)out;
    for (int k=0; k<Count; k++){*out16=*out16+floor(*in+0.5); out16++; in++;}
  }
}//DoubleToIntAdd

//---------------------------------------------------------------------------
/**
  DPower: in-frame power variation

  In: data[Count]: a signal

  returns the different between AC powers of its first and second halves.
*/
double DPower(double* data, int Count, void*)
{
  double ene1=ACPower(data, Count/2, 0);
  double ene2=ACPower(&data[Count/2], Count/2, 0);
  return ene2-ene1;
}//DPower

//---------------------------------------------------------------------------
/**
  function Energy: energy

  In: data[Count]: a signal

  Returns its total energy
*/
double Energy(double* data, int Count)
{
  double result=0;
  for (int i=0; i<Count; i++) result+=data[i]*data[i];
  return result;
}//Energy

//---------------------------------------------------------------------------
/**
  function ExpOnsetFilter: onset filter with exponential impulse response h(t)=Aexp(-t/Tr)-Bexp(-t/Ta),
  A=1-exp(-1/Tr), B=1-exp(-1/Ta).

  In: data[Count]: signal to filter
      Tr, Ta: time constants of h(t)
  Out: dataout[Count]: filtered signal, normalized by multiplying a factor.

  Returns the normalization factor. Identical data and dataout is allowed.
*/
double ExpOnsetFilter(double* dataout, double* data, int Count, double Tr, double Ta)
{
  double FA=0, FB=0;
  double EA=exp(-1.0/Tr), EB=exp(-1.0/Ta);
  double A=1-EA, B=1-EB;
  double NormFactor=1/sqrt((1-EA)*(1-EA)/(1-EA*EA)+(1-EB)*(1-EB)/(1-EB*EB)-2*(1-EA)*(1-EB)/(1-EA*EB));
  for (int i=0; i<Count; i++)
  {
    FA=FA*EA+*data;
    FB=FB*EB+*(data++);
    *(dataout++)=(A*FA-B*FB)*NormFactor;
  }
  return NormFactor;
}//ExpOnsetFilter

/**
  function ExpOnsetFilter_balanced: exponential onset filter without starting step response. It
  extends the input signal at the front end by bal*Ta samples by repeating the its value at 0, then
  applies the onset filter on the extended signal instead.

  In: data[Count]: signal to filter
      Tr, Ta: time constants to the impulse response of onset filter, see ExpOnsetFilter().
      bal: balancing factor
  Out: dataout[Count]: filtered signal, normalized by multiplying a factor.

  Returns the normalization factor. Identical data and dataout is allowed.
*/
double ExpOnsetFilter_balanced(double* dataout, double* data, int Count, double Tr, double Ta, int bal)
{
  double* tmpdata=new double[int(Count+bal*Ta)];
  double* ltmpdata=tmpdata;
  for (int i=0; i<bal*Ta; i++) *(ltmpdata++)=data[0];
  memcpy(ltmpdata, data, sizeof(double)*Count);
  double result=ExpOnsetFilter(tmpdata, tmpdata, bal*Ta+Count, Tr, Ta);
  memcpy(dataout, ltmpdata, sizeof(double)*Count);
  delete[] tmpdata;
  return result;
}//ExpOnsetFilter_balanced

//---------------------------------------------------------------------------
/**
  function ExtractLinearComponent: Legendre linear component

  In: data[Count+1]: a signal
  Out: dataout[Count+1]: its Legendre linear component, optional.

  Returns the coefficient to the linear component.
*/
double ExtractLinearComponent(double* dataout, double* data, int Count)
{
  double tmp=0;
  int N=Count*2;
  for (int n=0; n<=Count; n++) tmp+=n**(data++);
  tmp=tmp*24/N/(N+1)/(N+2);
  if (dataout)
    for (int n=0; n<=Count; n++) *(dataout++)=tmp*n;
  return tmp;
}//ExtractLinearComponent

//---------------------------------------------------------------------------
/**
  function FFTConv: fast convolution of two series by FFT overlap-add. In an overlap-add scheme it is
  assumed that one of the convolvends is short compared to the other one, which can be potentially
  infinitely long. The long convolvend is devided into short segments, each of which is convolved with
  the short convolvend, the results of which are then assembled into the final result. The minimal delay
  from input to output is the amount of overlap, which is the size of the short convolvend minus 1.

  In: source1[size1]: convolvend
      source2[size2]: second convolvend
      zero: position of first point in convoluton result, relative to main output buffer.
      pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result.
  Out: dest[size1]: the middle part of convolution result
       pre_buffer[-zero]: now updated by adding beginning part of the convolution result
       post_buffer[size2+zero]: end part of the convolution result

  No return value. Identical dest and source1 allowed.

  The convolution result has length size1+size2 (counting one trailing zero). If zero lies in the range
  between -size2 and 0, then the first -zero samples are added to pre_buffer[], next size1 samples are
  saved to dest[], and the last size2+zero sampled are saved to post_buffer[]; if not, the middle size1
  samples are saved to dest[], while pre_buffer[] and post_buffer[] are not used.
*/
void FFTConv(double* dest, double* source1, int size1, double* source2, int size2, int zero, double* pre_buffer, double* post_buffer)
{
  int order=Log2(size2-1)+1+1;
  int Wid=1<<order;
  int HWid=Wid/2;
  int Fr=size1/HWid;
  int res=size1-HWid*Fr;
  bool trunc=false;
  if (zero<-size2+1 || zero>0)  zero=-size2/2, trunc=true;
  if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true;

  AllocateFFTBuffer(Wid, fft, w, x1);
  int* hbitinv=CreateBitInvTable(order-1);
  cdouble* x2=new cdouble[Wid];
  double* tmp=new double[HWid];
  memset(tmp, 0, sizeof(double)*HWid);

  memcpy(fft, source2, sizeof(double)*size2);
  memset(&fft[size2], 0, sizeof(double)*(Wid-size2));
  RFFTC(fft, 0, 0, order, w, x2, hbitinv);

  double r1, r2, i1, i2;
  int ind, ind_;
  for (int i=0; i<Fr; i++)
  {
    memcpy(fft, &source1[i*HWid], sizeof(double)*HWid);
    memset(&fft[HWid], 0, sizeof(double)*HWid);

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);

    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);
    for (int j=0; j<HWid; j++) tmp[j]+=fft[j];

    ind=i*HWid+zero; //(i+1)*HWid<=size1
    ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1
    if (ind<0)
    {
      if (!trunc)
        memdoubleadd(pre_buffer, tmp, -ind);
      memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind));
    }
    else
      memcpy(&dest[ind], tmp, sizeof(double)*HWid);
    memcpy(tmp, &fft[HWid], sizeof(double)*HWid);
  }

  if (res>0)
  {
    memcpy(fft, &source1[Fr*HWid], sizeof(double)*res);
    memset(&fft[res], 0, sizeof(double)*(Wid-res));

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);

    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);
    for (int j=0; j<HWid; j++)
      tmp[j]+=fft[j];

    ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1
    ind_=ind+HWid;  //ind_=size1 -res+zero+HWid
    if (ind<0)
    {
      if (!trunc)
        memdoubleadd(pre_buffer, tmp, -ind);
      memcpy(dest, &tmp[-ind], sizeof(double)*(HWid+ind));
    }
    else if (ind_>size1)
    {
      memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind));
      if (!trunc && post_buffer)
      {
        if (ind_>size1+size2+zero)
          memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero));
        else
          memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1));
      }
    }
    else
      memcpy(&dest[ind], tmp, sizeof(double)*HWid);
    memcpy(tmp, &fft[HWid], sizeof(double)*HWid);
    Fr++;
  }

  ind=Fr*HWid+zero;
  ind_=ind+HWid;

  if (ind<size1)
  {
    if (ind_>size1)
    {
      memcpy(&dest[ind], tmp, sizeof(double)*(size1-ind));
      if (!trunc && post_buffer)
      {
        if (ind_>size1+size2+zero)
          memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(size2+zero));
        else
          memcpy(post_buffer, &tmp[size1-ind], sizeof(double)*(ind_-size1));
      }
    }
    else
      memcpy(&dest[ind], tmp, sizeof(double)*HWid);
  }
  else //ind>=size1 => ind_>=size1+size2+zero
  {
    if (!trunc && post_buffer)
      memcpy(&post_buffer[ind-size1], tmp, sizeof(double)*(size1+size2+zero-ind));
  }

  FreeFFTBuffer(fft);
  delete[] x2;
  delete[] tmp;
  delete[] hbitinv;
}//FFTConv

/**
  function FFTConv: the simplified version using two output buffers instead of three. This is almost
  equivalent to setting zero=-size2 in the three-output-buffer version (so that post_buffer no longer
  exists), except that this version requires size2 (renamed HWid) be a power of 2, and pre_buffer point
  to the END of the storage (so that pre_buffer=dest automatically connects the two buffers in a
  continuous memory block).

  In: source1[size1]: convolvend
      source2[HWid]: second convolved, HWid be a power of 2
      pre_buffer[-HWid:-1]: buffer hosting values to be overlap-added to the start of the result.
  Out: dest[size1]: main output buffer, now hosting end part of the result (after HWid samples).
       pre_buffer[-HWid:-1]: now updated by added the start of the result

  No return value.
*/
void FFTConv(double* dest, double* source1, int size1, double* source2, int HWid, double* pre_buffer)
{
  int Wid=HWid*2;
  int order=Log2(Wid);
  int Fr=size1/HWid;
  int res=size1-HWid*Fr;

  AllocateFFTBuffer(Wid, fft, w, x1);
  cdouble *x2=new cdouble[Wid];
  double *tmp=new double[HWid];
  int* hbitinv=CreateBitInvTable(order-1);

  memcpy(fft, source2, sizeof(double)*HWid);
  memset(&fft[HWid], 0, sizeof(double)*HWid);
  RFFTC(fft, 0, 0, order, w, x2, hbitinv);

  double r1, r2, i1, i2;
  for (int i=0; i<Fr; i++)
  {
    memcpy(fft, &source1[i*HWid], sizeof(double)*HWid);
    memset(&fft[HWid], 0, sizeof(double)*HWid);

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);

    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);

    if (i==0)
    {
      if (pre_buffer!=NULL)
      {
        double* destl=&pre_buffer[-HWid+1];
        for (int j=0; j<HWid-1; j++) destl[j]+=fft[j];
      }
    }
    else
    {
      for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j];
      memcpy(&dest[(i-1)*HWid], tmp, sizeof(double)*HWid);
    }
    memcpy(tmp, &fft[HWid-1], sizeof(double)*HWid);
  }

  if (res>0)
  {
    if (Fr==0) memset(tmp, 0, sizeof(double)*HWid);

    memcpy(fft, &source1[Fr*HWid], sizeof(double)*res);
    memset(&fft[res], 0, sizeof(double)*(Wid-res));

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);
    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);

    if (Fr==0)
    {
      if (pre_buffer!=NULL)
      {
        double* destl=&pre_buffer[-HWid+1];
        for (int j=0; j<HWid-1; j++) destl[j]+=fft[j];
      }
    }
    else
    {
      for (int j=0; j<HWid-1; j++) tmp[j+1]+=fft[j];
      memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid);
    }

    memcpy(&dest[Fr*HWid], &fft[HWid-1], sizeof(double)*res);
  }
  else
    memcpy(&dest[(Fr-1)*HWid], tmp, sizeof(double)*HWid);

  FreeFFTBuffer(fft);
  delete[] x2; delete[] tmp; delete[] hbitinv;
}//FFTConv

/**
  function FFTConv: fast convolution of two series by FFT overlap-add. Same as the three-output-buffer
  version above but using integer output buffers as well as integer source1.

  In: source1[size1]: convolvend
      bps: bytes per sample of integer units in source1[].
      source2[size2]: second convolvend
      zero: position of first point in convoluton result, relative to main output buffer.
      pre_buffer[-zero]: buffer hosting values to be overlap-added to the start of the result.
  Out: dest[size1]: the middle part of convolution result
       pre_buffer[-zero]: now updated by adding beginning part of the convolution result
       post_buffer[size2+zero]: end part of the convolution result

  No return value. Identical dest and source1 allowed.
*/
void FFTConv(unsigned char* dest, unsigned char* source1, int bps, int size1, double* source2, int size2, int zero, unsigned char* pre_buffer, unsigned char* post_buffer)
{
  int order=Log2(size2-1)+1+1;
  int Wid=1<<order;
  int HWid=Wid/2;
  int Fr=size1/HWid;
  int res=size1-HWid*Fr;
  bool trunc=false;
  if (zero<-size2+1 || zero>0)  zero=-size2/2, trunc=true;
  if (pre_buffer==NULL || (post_buffer==NULL && size2+zero!=0)) trunc=true;

  AllocateFFTBuffer(Wid, fft, w, x1);
  cdouble* x2=new cdouble[Wid];
  double* tmp=new double[HWid];
  memset(tmp, 0, sizeof(double)*HWid);
  int* hbitinv=CreateBitInvTable(order-1);

  memcpy(fft, source2, sizeof(double)*size2);
  memset(&fft[size2], 0, sizeof(double)*(Wid-size2));
  RFFTC(fft, 0, 0, order, w, x2, hbitinv);

  double r1, r2, i1, i2;
  int ind, ind_;
  for (int i=0; i<Fr; i++)
  {
    IntToDouble(fft, &source1[i*HWid*bps], bps, HWid);
    memset(&fft[HWid], 0, sizeof(double)*HWid);

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);

    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);
    for (int j=0; j<HWid; j++) tmp[j]+=fft[j];

    ind=i*HWid+zero; //(i+1)*HWid<=size1
    ind_=ind+HWid; //ind_=(i+1)*HWid+zero<=size1
    if (ind<0)
    {
      if (!trunc)
        DoubleToIntAdd(pre_buffer, bps, tmp, -ind);
      DoubleToInt(dest, bps, &tmp[-ind], HWid+ind);
    }
    else
      DoubleToInt(&dest[ind*bps], bps, tmp, HWid);
    memcpy(tmp, &fft[HWid], sizeof(double)*HWid);
  }

  if (res>0)
  {
    IntToDouble(fft, &source1[Fr*HWid*bps], bps, res);
    memset(&fft[res], 0, sizeof(double)*(Wid-res));

    RFFTC(fft, 0, 0, order, w, x1, hbitinv);

    for (int j=0; j<Wid; j++)
    {
      r1=x1[j].x, r2=x2[j].x, i1=x1[j].y, i2=x2[j].y;
      x1[j].x=r1*r2-i1*i2;
      x1[j].y=r1*i2+r2*i1;
    }
    CIFFTR(x1, order, w, fft, hbitinv);
    for (int j=0; j<HWid; j++)
      tmp[j]+=fft[j];

    ind=Fr*HWid+zero; //Fr*HWid=size1-res, ind=size1-res+zero<size1
    ind_=ind+HWid;  //ind_=size1 -res+zero+HWid
    if (ind<0)
    {
      if (!trunc)
        DoubleToIntAdd(pre_buffer, bps, tmp, -ind);
      DoubleToInt(dest, bps, &tmp[-ind], HWid+ind);
    }
    else if (ind_>size1)
    {
      DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind);
      if (!trunc && post_buffer)
      {
        if (ind_>size1+size2+zero)
          DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero);
        else
          DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1);
      }
    }
    else
      DoubleToInt(&dest[ind*bps], bps, tmp, HWid);
    memcpy(tmp, &fft[HWid], sizeof(double)*HWid);
    Fr++;
  }

  ind=Fr*HWid+zero;
  ind_=ind+HWid;

  if (ind<size1)
  {
    if (ind_>size1)
    {
      DoubleToInt(&dest[ind*bps], bps, tmp, size1-ind);
      if (!trunc && post_buffer)
      {
        if (ind_>size1+size2+zero)
          DoubleToInt(post_buffer, bps, &tmp[size1-ind], size2+zero);
        else
          DoubleToInt(post_buffer, bps, &tmp[size1-ind], ind_-size1);
      }
    }
    else
      DoubleToInt(&dest[ind*bps], bps, tmp, HWid);
  }
  else //ind>=size1 => ind_>=size1+size2+zero
  {
    if (!trunc && post_buffer)
      DoubleToInt(&post_buffer[(ind-size1)*bps], bps, tmp, size1+size2+zero-ind);
  }

  FreeFFTBuffer(fft);
  delete[] x2;
  delete[] tmp;
  delete[] hbitinv;
}//FFTConv

//---------------------------------------------------------------------------
/**
  function FFTFilter: FFT with cosine-window overlap-add: This FFT filter is not an actural LTI system,
  but an block processing with overlap-add. In this function the blocks are overlapped by 50% and summed
  up with Hann windowing.

  In: data[Count]: input data
      Wid: DFT size
      On, Off: cut-off frequencies of FFT filter. On<Off: band-pass; On>Off: band-stop.
  Out: dataout[Count]: filtered data

  No return value. Identical data and dataout allowed
*/
void FFTFilter(double* dataout, double* data, int Count, int Wid, int On, int Off)
{
  int Order=Log2(Wid);
  int HWid=Wid/2;
  int Fr=(Count-Wid)/HWid+1;
  AllocateFFTBuffer(Wid, ldata, w, x);

  double* win=new double[Wid];
  for (int i=0; i<Wid; i++) win[i]=sqrt((1-cos(2*M_PI*i/Wid))/2);
  double* tmpdata=new double[HWid];
  memset(tmpdata, 0, HWid*sizeof(double));

  for (int i=0; i<Fr; i++)
  {
    memcpy(ldata, &data[i*HWid], Wid*sizeof(double));
    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*win[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*win[k];

    RFFTC(ldata, NULL, NULL, Order, w, x, 0);

    if (On<Off) //band pass: keep [On, Off) and set other bins to zero
    {
      memset(x, 0, On*sizeof(cdouble));
      if (On>=1)
        memset(&x[Wid-On+1], 0, (On-1)*sizeof(cdouble));
      if (Off*2<=Wid)
        memset(&x[Off], 0, (Wid-Off*2+1)*sizeof(cdouble));
    }
    else //band stop: set [Off, On) to zero.
    {
      memset(&x[Off], 0, sizeof(cdouble)*(On-Off));
      memset(&x[Wid-On+1], 0, sizeof(double)*(On-Off));
    }

    CIFFTR(x, Order, w, ldata);

    if (i>0) for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*win[k];
    for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*win[k];

    memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double));
    for (int k=0; k<HWid; k++) dataout[i*HWid+k]+=ldata[k];
    memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
  }

  memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double));
  memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double));

  delete[] win;
  delete[] tmpdata;
  FreeFFTBuffer(ldata);
}//FFTFilter

/**
  function FFTFilterOLA: FFTFilter with overlap-add support. This is a true LTI filter whose impulse
  response is constructed using IFFT. The filtering is implemented by fast convolution.

  In: data[Count]: input data
      Wid: FFT size
      On, Off: cut-off frequencies, in bins, of the filter
      pre_buffer[Wid]: buffer hosting sampled to be added with the start of output
  Out: dataout[Count]: main output buffer, hosting the last $Count samples of output.
       pre_buffer[Wid]: now updated by adding the first Wid samples of output

  No return value. The complete output contains Count+Wid effective samples (including final 0); firt
  $Wid are added to pre_buffer[], next Count samples saved to dataout[].
*/
void FFTFilterOLA(double* dataout, double* data, int Count, int Wid, int On, int Off, double* pre_buffer)
{
  AllocateFFTBuffer(Wid, spec, w, x);
  memset(x, 0, sizeof(cdouble)*Wid);
  for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2);
  CIFFTR(x, Log2(Wid), w, spec);
  FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL);
  FreeFFTBuffer(spec);
}//FFTFilterOLA
//version for integer input and output, where BytesPerSample specifies the integer format.
void FFTFilterOLA(unsigned char* dataout, unsigned char* data, int BytesPerSample, int Count, int Wid, int On, int Off, unsigned char* pre_buffer)
{
  AllocateFFTBuffer(Wid, spec, w, x);
  memset(x, 0, sizeof(cdouble)*Wid);
  for (int i=On+1; i<Off; i++) x[i].x=x[Wid-i].x=1-2*(i%2);
  CIFFTR(x, Log2(Wid), w, spec);
  FFTConv(dataout, data, BytesPerSample, Count, spec, Wid, -Wid, pre_buffer, NULL);
  FreeFFTBuffer(spec);
}//FFTFilterOLA

/**
  function FFTFilterOLA: FFT filter with overlap-add support.

  In: data[Count]: input data
      amp[0:HWid]: amplitude response
      ph[0:HWid]: phase response, where ph[0]=ph[HWid]=0;
      pre_buffer[Wid]: buffer hosting sampled to be added to the beginning of the output
  Out: dataout[Count]: main output buffer, hosting the middle $Count samples of output.
       pre_buffer[Wid]: now updated by adding the first Wid/2 samples of output

  No return value.
*/
void FFTFilterOLA(double* dataout, double* data, int Count, double* amp, double* ph, int Wid, double* pre_buffer)
{
  int HWid=Wid/2;
  AllocateFFTBuffer(Wid, spec, w, x);
  x[0].x=amp[0], x[0].y=0;
  for (int i=1; i<HWid; i++)
  {
    x[i].x=x[Wid-i].x=amp[i]*cos(ph[i]);
    x[i].y=amp[i]*sin(ph[i]);
    x[Wid-i].y=-x[i].y;
  }
  x[HWid].x=amp[HWid], x[HWid].y=0;
  CIFFTR(x, Log2(Wid), w, spec);
  FFTConv(dataout, data, Count, spec, Wid, -Wid, pre_buffer, NULL);
  FreeFFTBuffer(spec);
}//FFTFilterOLA

/**
  function FFTMask: masks a band of a signal with noise

  In: data[Count]: input signal
      DigiOn, DigiOff: cut-off frequences of the band to mask
      maskcoef: masking noise amplifier. If set to 1 than the mask level is set to the highest signal
        level in the masking band.
  Out: dataout[Count]: output data

  No return value.
*/
double FFTMask(double* dataout, double* data, int Count, int Wid, double DigiOn, double DigiOff, double maskcoef)
{
  int Order=Log2(Wid);
  int HWid=Wid/2;
  int Fr=(Count-Wid)/HWid+1;
  int On=Wid*DigiOn, Off=Wid*DigiOff;
  AllocateFFTBuffer(Wid, ldata, w, x);

  double* winhann=new double[Wid];
  double* winhamm=new double[Wid];
  for (int i=0; i<Wid; i++)
    {winhamm[i]=0.54-0.46*cos(2*M_PI*i/Wid); winhann[i]=(1-cos(2*M_PI*i/Wid))/2/winhamm[i];}
  double* tmpdata=new double[HWid];
  memset(tmpdata, 0, HWid*sizeof(double));
  double max, randfi;

  max=0;
  for (int i=0; i<Fr; i++)
  {
    memcpy(ldata, &data[i*HWid], Wid*sizeof(double));
    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*winhamm[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*winhamm[k];

    RFFTC(ldata, ldata, NULL, Order, w, x, 0);

    for (int k=On; k<Off; k++)
    {
      x[k].x=x[Wid-k].x=x[k].y=x[Wid-k].y=0;
      if (max<ldata[k]) max=ldata[k];
    }

    CIFFTR(x, Order, w, ldata);

    if (i>0)
      for (int k=0; k<HWid; k++) ldata[k]=ldata[k]*winhann[k];
    for (int k=HWid; k<Wid; k++) ldata[k]=ldata[k]*winhann[k];

    for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k];
    memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double));
    memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
  }
  memcpy(&dataout[Fr*HWid], tmpdata, HWid*sizeof(double));

  max*=maskcoef;

  for (int i=0; i<Wid; i++)
    winhann[i]=winhann[i]*winhamm[i];

  for (int i=0; i<Fr; i++)
  {
    memset(x, 0, sizeof(cdouble)*Wid);
    for (int k=On; k<Off; k++)
    {
      randfi=rand()*M_PI*2/RAND_MAX;
      x[k].x=x[Wid-k].x=max*cos(randfi);
      x[k].y=max*sin(randfi);
      x[Wid-k].y=-x[k].y;
    }

    CIFFTR(x, Order, w, ldata);

    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*winhann[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*winhann[k];

    for (int k=0; k<Wid; k++) dataout[i*HWid+k]+=ldata[k];
  }

  memset(&dataout[Fr*HWid+HWid], 0, (Count-Fr*HWid-HWid)*sizeof(double));

  delete[] winhann;
  delete[] winhamm;
  delete[] tmpdata;
  FreeFFTBuffer(ldata);

  return max;
}//FFTMask

//---------------------------------------------------------------------------
/**
  function FindInc: find the element in ordered list data that is closest to value.

  In: data[Count]: a ordered list
      value: the value to locate in the list

  Returns the index of the element in the sorted list which is closest to $value.
*/
int FindInc(double value, double* data, int Count)
{
  if (value>=data[Count-1]) return Count-1;
  if (value<data[0]) return 0;
  int end=InsertInc(value, data, Count, false);
  if (fabs(value-data[end-1])<fabs(value-data[end])) return end-1;
  else return end;
}//FindInc

//---------------------------------------------------------------------------
/**
  function Gaussian: Gaussian function

  In: Vector[Dim]: a vector
      Mean[Dim]: mean of Gaussian function
      Dev[Fim]: diagonal autocorrelation matrix of Gaussian function

  Returns the value of Gaussian function at Vector[].
*/
double Gaussian(int Dim, double* Vector, double* Mean, double* Dev)
{
  double bmt=0, tmp;
  for (int dim=0; dim<Dim; dim++)
  {
    tmp=Vector[dim]-Mean[dim];
    bmt+=tmp*tmp/Dev[dim];
  }
  bmt=-bmt/2;
  tmp=log(Dev[0]);
  for (int dim=1; dim<Dim; dim++) tmp+=log(Dev[dim]);
  bmt-=tmp/2;
  bmt-=Dim*log(M_PI*2)/2;
  bmt=exp(bmt);
  return bmt;
}//Gaussian


//---------------------------------------------------------------------------
/**
  function Hamming: calculates the amplitude spectrum of Hamming window at a given frequency

  In: f: frequency
      T: size of Hamming window

  Returns the amplitude spectrum at specified frequency.
*/
double Hamming(double f, double T)
{
  double omg0=2*M_PI/T;
  double omg=f*2*M_PI;
  cdouble c1, c2, c3;
  cdouble nj(0, -1);
  cdouble pj(0, 1);
  double a=0.54, b=0.46;

  cdouble c=1.0-exp(nj*T*omg);
  double half=0.5;

  if (fabs(omg)<1e-100)
    c1=a*T;
  else
    c1=a*c/(pj*omg);

  if (fabs(omg+omg0)<1e-100)
    c2=b*0.5*T;
  else
    c2=c*b*half/(nj*cdouble(omg+omg0));

  if (fabs(omg-omg0)<1e-100)
    c3=b*0.5*T;
  else
    c3=b*c*half/(nj*cdouble(omg-omg0));

  c=c1+c2+c3;
  return abs(c);
}//Hamming*/

//---------------------------------------------------------------------------
/**
  function HannSq: computes the square norm of Hann window spectrum (window-size-normalized)

  In: x: frequency, in bins
      N: size of Hann window

  Return the square norm.
*/
double HannSq(double x, double N)
{
  double re, im;
  double pim=M_PI*x;
  double pimf=pim/N;
  double pif=M_PI/N;

  double sinpim=sin(pim);
  double sinpimf=sin(pimf);
  double sinpimplus1f=sin(pimf+pif);
  double sinpimminus1f=sin(pimf-pif);

  double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf;

  if (sinpimf==0)
    spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0;
  else if (sinpimplus1f==0)
    spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0;
  else if (sinpimminus1f==0)
    spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0;
  else
    spmdivbyspmf=sinpim/sinpimf, spmdivbyspmpf=sinpim/sinpimplus1f, spmdivbyspmmf=sinpim/sinpimminus1f;

  re=0.5*spmdivbyspmf-0.25*cos(pif)*(spmdivbyspmpf+spmdivbyspmmf);
  im=0.25*sin(pif)*(-spmdivbyspmpf+spmdivbyspmmf);

  return (re*re+im*im)/(N*N);
}//HannSq

/**
  function Hann: computes the Hann window amplitude spectrum (window-size-normalized).

  In: x: frequency, in bins
      N: size of Hann window

  Return the amplitude spectrum evaluated at x. Maximum 0.5 is reached at x=0. Time 2 to normalize
  maximum to 1.
*/
double Hann(double x, double N)
{
  double pim=M_PI*x;
  double pif=M_PI/N;
  double pimf=pif*x;

  double sinpim=sin(pim);
  double tanpimf=tan(pimf);
  double tanpimplus1f=tan(pimf+pif);
  double tanpimminus1f=tan(pimf-pif);

  double spmdivbyspmf, spmdivbyspmpf, spmdivbyspmmf;

  if (fabs(tanpimf)<1e-10)
    spmdivbyspmf=N, spmdivbyspmpf=spmdivbyspmmf=0;
  else if (fabs(tanpimplus1f)<1e-10)
    spmdivbyspmpf=-N, spmdivbyspmf=spmdivbyspmmf=0;
  else if (fabs(tanpimminus1f)<1e-10)
    spmdivbyspmmf=-N, spmdivbyspmf=spmdivbyspmpf=0;
  else
    spmdivbyspmf=sinpim/tanpimf, spmdivbyspmpf=sinpim/tanpimplus1f, spmdivbyspmmf=sinpim/tanpimminus1f;

  double result=0.5*spmdivbyspmf-0.25*(spmdivbyspmpf+spmdivbyspmmf);

  return result/N;
}//HannC

/**
  function HxPeak2: fine spectral peak detection. This does detection and high-precision LSE estimation
  in one go. However, since in practise most peaks are spurious, LSE estimation is not necessary on
  them. Accordingly, HxPeak2 is deprecated in favour of faster but coarser peak picking methods, such as
  QIFFT, which leaves fine estimation to a later stage of processing.

  In: F, dF, ddF: pointers to functions that compute LSE peak energy for, plus its 1st and 2nd
        derivatives against, a given frequency.
      params: pointer to a data structure (l_hx) hosting input data fed to F, dF, and ddF
      (st, en): frequency range, in bins, to search for peaks in
      epf: convergence detection threshold
  Out: hps[return value]: peak frequencies
       vps[return value]; peak amplitudes

  Returns the number of peaks detected.
*/
int HxPeak2(double*& hps, double*& vhps, double (*F)(double, void*), double (*dF)(double, void*), double(*ddF)(double, void*), void* params, double st, double en, double epf)
{
  struct l_hx {int N; union {double B; struct {int k1; int k2;};}; cdouble* x; double dhxpeak; double hxpeak;} *p=(l_hx *)params;
  int dfshift=offsetof(l_hx, dhxpeak);
  int fshift=offsetof(l_hx, hxpeak);
  double B=p->B;
  int count=0;

  int den=ceil(en), dst=floor(st);
  if (den-dst<3) den++, dst--;
  if (den-dst<3) den++, dst--;
  if (dst<1) dst=1;

  double step=0.5;
  int num=(den-dst)/step+1;
  bool allochps=false, allocvhps=false;
  if (hps==NULL) allochps=true, hps=new double[num];
  if (vhps==NULL) allocvhps=true, vhps=new double[num];

  {
    double* inp=new double[num];
    for (int i=0; i<num; i++)
    {
      double lf=dst+step*i;
      p->k1=ceil(lf-B); if (p->k1<0) p->k1=0;
      p->k2=floor(lf+B); if (p->k2>=p->N/2) p->k2=p->N/2-1;
      inp[i]=F(lf, params);
    }

    for (int i=1; i<num-1; i++)
    {
      if (inp[i]>=inp[i-1] && inp[i]>=inp[i+1])  //inp[i]=F(dst+step*i)
      {
        if (inp[i]==inp[i-1] && inp[i]==inp[i+1]) continue;
        double fa=dst+step*(i-1), fb=dst+step*(i+1);
        double ff=dst+step*i;
        p->k1=ceil(ff-B); if (p->k1<0) p->k1=0;
        p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1;

        double tmp=Newton1dmax(ff, fa, fb, ddF, params, dfshift, fshift, dF, dfshift, epf);

        //although we have selected inp[i] to be a local maximum, different truncation
        //  of local spectrum implies it may not hold as the truncation of inp[i] is
        //  used for recalculating inp[i-1] and inp[i+1] in init_Newton method. In this
        //  case we retry the sub-maximal frequency to see if it becomes a local maximum
        //  when the spectrum is truncated to centre on it.

        if (tmp==-0.5 || tmp==-0.7) //y(fa)<=y(ff)<y(fb) or y(ff)<y(fa)<y(fb)
        {
          tmp=Newton1dmax(fb, ff, 2*fb-ff, ddF, params, dfshift, fshift, dF, dfshift, epf);
          if (tmp==-0.5 || tmp==-0.7) continue;
          /*
          double ff2=(ff+fb)/2;
          p->k1=ceil(ff2-B); if (p->k1<0) p->k1=0;
          p->k2=floor(ff2+B); if (p->k2>=p->N/2) p->k2=p->N/2-1;
          tmp=Newton1dmax(ff2, ff, fb, ddF, params, dfshift, fshift, dF, dfshift, epf);
          p->k1=ceil(ff-B); if (p->k1<0) p->k1=0;
          p->k2=floor(ff+B); if (p->k2>=p->N/2) p->k2=p->N/2-1;    */
        }
        else if (tmp==-0.6 || tmp==-0.8) //y(fb)<=y(ff)<y(fa)
        {
          tmp=Newton1dmax(fa, 2*fa-ff, ff, ddF, params, dfshift, fshift, dF, dfshift, epf);
          if (tmp==-0.6 || tmp==-0.8) continue;
        }
        if (tmp<0 /*tmp==-0.5 || tmp==-0.6 || tmp==-1 || tmp==-2 || tmp==-3*/)
        {
          Search1Dmax(ff, params, F, dst+step*(i-1), dst+step*(i+1), &vhps[count], epf);
        }
        else
        {
          vhps[count]=p->hxpeak;
        }
        if (ff>=st && ff<=en && ff>dst+step*(i-0.99) && ff<dst+step*(i+0.99))
        {
//          if (count==0 || fabs(tmp-hps[count-1])>0.1)
//          {
            hps[count]=ff;
            count++;
//          }
        }
      }
    }
    delete[] inp;
  }

  if (allochps) hps=(double*)realloc(hps, sizeof(double)*count);
  if (allocvhps) vhps=(double*)realloc(vhps, sizeof(double)*count);
  return count;
}//HxPeak2

//---------------------------------------------------------------------------
/**
  function InsertDec: inserts value into sorted decreasing list

  In: data[Count]: a sorted decreasing list.
      value: the value to be added
  Out: data[Count]: the list with $value inserted if the latter is larger than its last entry, in which
        case the original last entry is discarded.

  Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to
  data[Count-1].
*/
int InsertDec(int value, int* data, int Count)
{
  if (Count<=0) return -1;
  if (value<=data[Count-1]) return -1;
  if (value>data[0])
  {
    memmove(&data[1], &data[0], sizeof(int)*(Count-1));
    data[0]=value;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)>=value>D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value<=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)>=value>D(end)
  memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1));
  data[end]=value;
  return end;
}//InsertDec
//the double version
int InsertDec(double value, double* data, int Count)
{
  if (Count<=0) return -1;
  if (value<=data[Count-1]) return -1;
  if (value>data[0])
  {
    memmove(&data[1], &data[0], sizeof(double)*(Count-1));
    data[0]=value;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)>=value>D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value<=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)>=value>D(end)
  memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1));
  data[end]=value;
  return end;
}//InsertDec

/**
  function InsertDec: inserts value and attached integer into sorted decreasing list

  In: data[Count]: a sorted decreasing list
      indices[Count]: a list of integers attached to entries of data[]
      value, index: the value to be added and its attached integer
  Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is larger than
        the last entry of data[], in which case the original last entries are discarded.

  Returns the index where $value is located in data[], or -1 if $value is smaller than or equal to
  data[Count-1].
*/
int InsertDec(double value, int index, double* data, int* indices, int Count)
{
  if (Count<=0) return -1;
  if (value<=data[Count-1]) return -1;
  if (value>data[0])
  {
    memmove(&data[1], data, sizeof(double)*(Count-1));
    memmove(&indices[1], indices, sizeof(int)*(Count-1));
    data[0]=value, indices[0]=index;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)>=value>D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value<=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)>=value>D(end)
  memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1));
  memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1));
  data[end]=value, indices[end]=index;
  return end;
}//InsertDec

/**
  InsertInc: inserts value into sorted increasing list.

  In: data[Count]: a sorted increasing list.
      Capacity: maximal size of data[]
      value: the value to be added
      Compare: pointer to function that compare two values
  Out: data[Count]: the list with $value inserted. If the original list is full (Count=Capacity) then
        either $value, or the last entry of data[], whichever is larger, is discarded.

  Returns the index where $value is located in data[], or -1 if it is not inserted, which happens if
  Count=Capacity and $value is larger than or equal to the last entry in data[Capacity].
*/
int InsertInc(void* value, void** data, int Count, int Capacity, int (*Compare)(void*, void*))
{
  if (Capacity<=0) return -1;
  if (Count>Capacity) Count=Capacity;

//Compare(A,B)<0 if A<B, =0 if A=B, >0 if A>B
  int PosToInsert;
  if (Count==0) PosToInsert=0;
  else if (Compare(value, data[Count-1])>0) PosToInsert=Count;
  else if (Compare(value, data[0])<0) PosToInsert=0;
  else
  {
    //now Count>=2
    int head=0, end=Count-1, mid;

    //D(head)<=value<D(end)
    while (end-head>1)
    {
      mid=(head+end)/2;
      if (Compare(value, data[mid])>=0) head=mid;
      else end=mid;
    }
    //D(head=end-1)<=value<D(end)
    PosToInsert=end;
  }

  if (Count<Capacity)
  {
    memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert));
    data[PosToInsert]=value;
  }
  else //Count==Capacity
  {
    if (PosToInsert>=Capacity) return -1;
    memmove(&data[PosToInsert+1], &data[PosToInsert], sizeof(void*)*(Count-PosToInsert-1));
    data[PosToInsert]=value;
  }
  return PosToInsert;
}//InsertInc

/**
  function InsertInc: inserts value into sorted increasing list

  In: data[Count]: a sorted increasing list.
      value: the value to be added
      doinsert: specifies whether the actually insertion is to be performed
  Out: data[Count]: the list with $value inserted if the latter is smaller than its last entry, in which
        case the original last entry of data[] is discarded.

  Returns the index where $value is located in data[], or -1 if value is larger than or equal to
  data[Count-1].
*/
int InsertInc(double value, double* data, int Count, bool doinsert)
{
  if (Count<=0) return -1;
  if (value>=data[Count-1]) return -1;
  if (value<data[0])
  {
    memmove(&data[1], &data[0], sizeof(double)*(Count-1));
    if (doinsert) data[0]=value;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)<=value<D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value>=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)<=value<D(end)
  if (doinsert)
  {
    memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1));
    data[end]=value;
  }
  return end;
}//InsertInc
//version where data[] is int.
int InsertInc(double value, int* data, int Count, bool doinsert)
{
  if (Count<=0) return -1;
  if (value>=data[Count-1]) return -1;
  if (value<data[0])
  {
    memmove(&data[1], &data[0], sizeof(int)*(Count-1));
    if (doinsert) data[0]=value;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)<=value<D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value>=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)<=value<D(end)
  if (doinsert)
  {
    memmove(&data[end+1], &data[end], sizeof(int)*(Count-end-1));
    data[end]=value;
  }
  return end;
}//InsertInc

/**
  function InsertInc: inserts value and attached integer into sorted increasing list

  In: data[Count]: a sorted increasing list
      indices[Count]: a list of integers attached to entries of data[]
      value, index: the value to be added and its attached integer
  Out: data[Count], indices[Count]: the lists with $value and $index inserted if $value is smaller than
        the last entry of data[], in which case the original last entries are discarded.

  Returns the index where $value is located in data[], or -1 if $value is larger than or equal to
  data[Count-1].
*/
int InsertInc(double value, int index, double* data, int* indices, int Count)
{
  if (Count<=0) return -1;
  if (value>=data[Count-1]) return -1;
  if (value<data[0])
  {
    memmove(&data[1], data, sizeof(double)*(Count-1));
    memmove(&indices[1], indices, sizeof(int)*(Count-1));
    data[0]=value, indices[0]=index;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)>=value>D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value>=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)>=value>D(end)
  memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1));
  memmove(&indices[end+1], &indices[end], sizeof(int)*(Count-end-1));
  data[end]=value, indices[end]=index;
  return end;
}//InsertInc
//version where indices[] is double-precision floating point.
int InsertInc(double value, double index, double* data, double* indices, int Count)
{
  if (Count<=0) return -1;
  if (value>=data[Count-1]) return -1;
  if (value<data[0])
  {
    memmove(&data[1], data, sizeof(double)*(Count-1));
    memmove(&indices[1], indices, sizeof(double)*(Count-1));
    data[0]=value, indices[0]=index;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)>=value>D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value>=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)>=value>D(end)
  memmove(&data[end+1], &data[end], sizeof(double)*(Count-end-1));
  memmove(&indices[end+1], &indices[end], sizeof(double)*(Count-end-1));
  data[end]=value, indices[end]=index;
  return end;
}//InsertInc

/**
  function InsertIncApp: inserts value into flexible-length sorted increasing list

  In: data[Count]: a sorted increasing list.
      value: the value to be added
  Out: data[Count+1]: the list with $value inserted.

  Returns the index where $value is located in data[], or -1 if Count<0. data[] must have Count+1
  storage units on calling.
*/
int InsertIncApp(double value, double* data, int Count)
{
  if (Count<0) return -1;
  if (Count==0){data[0]=value; return 0;}
  if (value>=data[Count-1]){data[Count]=value; return Count;}
  if (value<data[0])
  {
    memmove(&data[1], &data[0], sizeof(double)*Count);
    data[0]=value;
    return 0;
  }

  //now Count>=2
  int head=0, end=Count-1, mid;

  //D(head)<=value<D(end)
  while (end-head>1)
  {
    mid=(head+end)/2;
    if (value>=data[mid]) head=mid;
    else end=mid;
  }

  //D(head=end-1)<=value<D(end)
  memmove(&data[end+1], &data[end], sizeof(double)*(Count-end));
  data[end]=value;

  return end;
}//InsertIncApp

//---------------------------------------------------------------------------
/**
  function InstantFreq; calculates instantaneous frequency from spectrum, evaluated at bin k
  
  In: x[hwid]: spectrum with scale 2hwid
      k: reference frequency, in bins
      mode: must be 1.
  
  Returns an instantaneous frequency near bin k.
*/
double InstantFreq(int k, int hwid, cdouble* x, int mode)
{
  double result;
  switch(mode)
  {
    //mode 1: the phase vocoder method, based on J. Brown, where the spectrogram
    //  MUST be calculated using rectangular window
    case 1:
    {
      if (k<1) k=1;
      if (k>hwid-2) k=hwid-2;
      double hr=0.5*(x[k].x-0.5*(x[k+1].x+x[k-1].x)), hi=0.5*(x[k].y-0.5*(x[k+1].y+x[k-1].y));
      double ph0=Atan2(hi, hr);
      double c=cos(M_PI/hwid), s=sin(M_PI/hwid);
      hr=0.5*(x[k].x-0.5*(x[k+1].x*c-x[k+1].y*s+x[k-1].x*c+x[k-1].y*s));
      hi=0.5*(x[k].y-0.5*(x[k+1].y*c+x[k+1].x*s+x[k-1].y*c-x[k-1].x*s));
      double ph1=Atan2(hi, hr);
      result=(ph1-ph0)/(2*M_PI);
      if (result<-0.5) result+=1;
      if (result>0.5) result-=1;
      result+=k*0.5/hwid;
      break;
    }
    case 2:
      break;
  }
  return result;
}//InstantFreq

/**
  function InstantFreq; calculates "frequency spectrum", a sequence of frequencies evaluated at DFT bins
  
  In: x[hwid]: spectrum with scale 2hwid
      mode: must be 1.
  Out: freqspec[hwid]: the frequency spectrum
    
  No return value.
*/
void InstantFreq(double* freqspec, int hwid, cdouble* x, int mode)
{
  for (int i=0; i<hwid; i++)
    freqspec[i]=InstantFreq(i, hwid, x, mode);
}//InstantFreq

//---------------------------------------------------------------------------
/**
  function IntToDouble: copy content of integer array to double array

  In: in: pointer to integer array
      BytesPerSample: number of bytes each integer takes
      Count: size of integer array, in integers
  Out: vector out[Count].

  No return value.

  This version is currently commented out in favour of the version implemented in QuickSpec.cpp which
  supports 24-bit integers.
*//*
void IntToDouble(double* out, void* in, int BytesPerSample, int Count)
{
  if (BytesPerSample==1){unsigned char* in8=(unsigned char*)in; for (int k=0; k<Count; k++) *(out++)=*(in8++)-128.0;}
  else {__int16* in16=(__int16*)in; for (int k=0; k<Count; k++) *(out++)=*(in16++);}
}//IntToDouble*/

//---------------------------------------------------------------------------
/**
  function IPHannC: inner product with Hann window spectrum

  In: x[N]: spectrum
      f: reference frequency
      K1, K2: spectral truncation bounds

  Returns the absolute value of the inner product of x[K1:K2] with the corresponding band of the
  spectrum of a sinusoid at frequency f.
*/
double IPHannC(double f, cdouble* x, int N, int K1, int K2)
{
  int M; double c[4], iH2;
  windowspec(wtHann, N, &M, c, &iH2);
  return abs(IPWindowC(f, x, N, M, c, iH2, K1, K2));
}//IPHannC


//---------------------------------------------------------------------------
/**
  function lse: linear regression y=ax+b

  In: x[Count], y[Count]: input points
  Out: a, b: LSE estimation of coefficients in y=ax+b

  No return value.
*/
void lse(double* x, double* y, int Count, double& a, double& b)
{
  double sx=0, sy=0, sxx=0, sxy=0;
  for (int i=0; i<Count; i++)
  {
    sx+=x[i];
    sy+=y[i];
    sxx+=x[i]*x[i];
    sxy+=x[i]*y[i];
  }
  b=(sxx*sy-sx*sxy)/(Count*sxx-sx*sx);
  a=(sy-Count*b)/sx;
}//lse

//--------------------------------------------------------------------------
/**
  memdoubleadd: vector addition

  In: dest[count], source[count]: addends
  Out: dest[count]: sum

  No return value.
*/
void memdoubleadd(double* dest, double* source, int count)
{
  for (int i=0; i<count; i++){*dest=*dest+*source; dest++; source++;}
}//memdoubleadd

//--------------------------------------------------------------------------
/**
  function Mel: converts frequency in Hz to frequency in mel.

  In: f: frequency, in Hz

  Returns the frequency measured on mel scale.
*/
double Mel(double f)
{
  return 1127.01048*log(1+f/700);
}//Mel

/**
  function InvMel: converts frequency in mel to frequency in Hz.

  In: f: frequency, in mel.

  Returns the frequency in Hz.
*/
double InvMel(double mel)
{
  return 700*(exp(mel/1127.01048)-1);
}//InvMel

/**
  function MFCC: calculates MFCC.

  In: Data[FrameWidth]: data
      NumBands: number of frequency bands on mel scale
      Bands[3*NumBands]: mel frequency bands, comes as $NumBands triples, each containing the lower,
        middle and high frequencies, in bins, of one band, from which a weighting window is created to
        weight individual bins.
      Ceps_Order: number of MFC coefficients (i.e. DCT coefficients)
      W, X: FFT buffers
  Out: C[Ceps_Order]: MFCC
       Amps[NumBands]: log spectrum on MF bands

  No return value. Use MFCCPrepareBands() to retrieve Bands[].
*/
void MFCC(int FrameWidth, int NumBands, int Ceps_Order, double* Data, double* Bands, double* C, double* Amps, cdouble* W, cdouble* X)
{
  double tmp, b2s, b2c, b2e;

  RFFTC(Data, 0, 0, Log2(FrameWidth), W, X, 0);
  for (int i=0; i<=FrameWidth/2; i++) Amps[i]=X[i].x*X[i].x+X[i].y*X[i].y;
  
  for (int i=0; i<NumBands; i++)
  {
    tmp=0;
    b2s=Bands[3*i], b2c=Bands[3*i+1], b2e=Bands[3*i+2];

    for (int j=ceil(b2s); j<ceil(b2c); j++)
      tmp+=Amps[j]*(j-b2s)/(b2c-b2s);
    for (int j=ceil(b2c); j<b2e; j++)
      tmp+=Amps[j]*(b2e-j)/(b2e-b2c);

    if (tmp<3.7200759760208359629596958038631e-44)
      Amps[i]=-100;
    else
      Amps[i]=log(tmp);
  }

  for (int i=0; i<Ceps_Order; i++)
  {
    tmp=Amps[0]*cos(M_PI*(i+1)/2/NumBands);
    for (int j=1; j<NumBands; j++)
      tmp+=Amps[j]*cos(M_PI*(i+0.5)*(j+0.5)/NumBands);
    C[i]=tmp;
  }
}//MFCC

/**
  function MFCCPrepareBands: returns a array of OVERLAPPING bands given in triples, whose 1st and 3rd
  entries are the start and end of a band, in bins, and the 2nd is a middle frequency.

  In: SamplesPerSec: sampling rate
      NumberOfBins: FFT size
      NumberOfBands: number of mel-frequency bands

  Returns pointer to the array of triples.
*/
double* MFCCPrepareBands(int NumberOfBands, int SamplesPerSec, int NumberOfBins)
{
  double* Bands=new double[NumberOfBands*3];
  double naqfreq=SamplesPerSec/2.0; //naqvist freq
  double binwid=SamplesPerSec*1.0/NumberOfBins;
  double naqmel=Mel(naqfreq);
  double b=naqmel/(NumberOfBands+1);

  Bands[0]=0;
  Bands[1]=InvMel(b)/binwid;
  Bands[2]=InvMel(b*2)/binwid;
  for (int i=1; i<NumberOfBands; i++)
  {
    Bands[3*i]=Bands[3*i-2];
    Bands[3*i+1]=Bands[3*i-1];
    Bands[3*i+2]=InvMel(b*(i+2))/binwid;
  }
  return Bands;
}//MFCCPrepareBands

//---------------------------------------------------------------------------
/**
  function Multi: vector-constant multiplication

  In: data[count]: a vector
      mul: a constant
  Out: data[count]: their product

  No return value.
*/
void Multi(double* data, int count, double mul)
{
  for (int i=0; i<count; i++){*data=*data*mul; data++;}
}//Multi

/**
  function Multi: vector-constant multiplication

  In: in[count]: a vector
      mul: a constant
  Out: out[count]: their product

  No return value.
*/
void Multi(double* out, double* in, int count, double mul)
{
  for (int i=0; i<count; i++) *(out++)=*(in++)*mul;
}//Multi

/**
  function Multi: vector-constant multiply-addition

  In: in[count], adder[count]: vectors
      mul: a constant
  Out: out[count]: in[]+adder[]*mul

  No return value.
*/
void MultiAdd(double* out, double* in, double* adder, int count, double mul)
{
  for (int i=0; i<count; i++) *(out++)=*(in++)+*(adder++)*mul;
}//MultiAdd

//---------------------------------------------------------------------------
/**
  function NearestPeak: finds a peak value in an array that is nearest to a given index

  In: data[count]: an array
      anindex: an index

  Returns the index to a peak of data[] that is closest to anindex. In case of two cloest indices,
  returns the index to the higher peak of the two.
*/
int NearestPeak(double* data, int count, int anindex)
{
  int upind=anindex, downind=anindex;
  if (anindex<1) anindex=1;
  if (anindex>count-2) anindex=count-2;

  if (data[anindex]>data[anindex-1] && data[anindex]>data[anindex+1]) return anindex;

  if (data[anindex]<data[anindex-1])
    while (downind>0 && data[downind-1]>data[downind]) downind--;
  if (data[anindex]<data[anindex+1])
    while (upind<count-1 && data[upind]<data[upind+1]) upind++;

  if (upind==anindex) return downind;
  if (downind==anindex) return upind;

  if (anindex-downind<upind-anindex) return downind;
  else if (anindex-downind>upind-anindex) return upind;
  else if (data[upind]<data[downind]) return downind;
  else return upind;
}//NearestPeak

//---------------------------------------------------------------------------
/**
  function NegativeExp: fits the curve y=1-x^lmd.

  In: x[Count], y[Count]: sample points to fit, x[0]=0, x[Count-1]=1, y[0]=1, y[Count-1]=0
  Out: lmd: coefficient of y=1-x^lmd.

  Returns rms fitting error.
*/
double NegativeExp(double* x, double* y, int Count, double& lmd, int sample, double step, double eps, int maxiter)
{
  lmd=0;
  for (int i=1; i<Count-1; i++)
  {
    if (y[i]<1)
      lmd+=log(1-y[i])/log(x[i]);
    else
      lmd+=-50/log(x[i]);
  }
  lmd/=(Count-2);

//lmd has been initialized
//coming up will be the recursive calculation of lmd by lgg

  int iter=0;
  double laste, lastdel, e=0, del=0, tmp;
  do
  {
    iter++;
    laste=e;
    lastdel=del;
    e=0, del=0;
    for (int i=1; i<Count-1; i+=sample)
    {
      tmp=pow(x[i], lmd);
      e=e+(y[i]+tmp-1)*(y[i]+tmp-1);
      del=del+(y[i]+tmp-1)*tmp*log(x[i]);
    }
    if (laste && e>laste) lmd+=step*lastdel, step/=2;
    else lmd+=-step*sample*del;
  }
  while (e && iter<=maxiter && (!laste || fabs(laste-e)/e>eps));
  return sqrt(e/Count);
}//NegativeExp

//---------------------------------------------------------------------------
/**
  function: NL: noise level, calculated on 5% of total frames with least energy

  In: data[Count]:
      Wid: window width for power level estimation

  Returns noise level, in rms.
*/
double NL(double* data, int Count, int Wid)
{
  int Fr=Count/Wid;
  int Num=Fr/20+1;
  double* ene=new double[Num], tmp;
  for (int i=0; i<Num; i++) ene[i]=1e30;
  for (int i=0; i<Fr; i++)
  {
    tmp=DCPower(&data[i*Wid], Wid, 0);
    InsertInc(tmp, ene, Num);
  }
  double result=Avg(ene, Num, 0);
  delete[] ene;
  result=sqrt(result);
  return result;
}//NL

//---------------------------------------------------------------------------
/**
  function Normalize: normalizes data to [-Maxi, Maxi], without zero shift

  In: data[Count]: data to be normalized
      Maxi: destination maximal absolute value
  Out: data[Count]: normalized data

  Returns the original maximal absolute value.
*/
double Normalize(double* data, int Count, double Maxi)
{
  double max=0;
  double* ldata=data;
  for (int i=0; i<Count; i++)
  {
    if (*ldata>max) max=*ldata;
    else if (-*ldata>max) max=-*ldata;
    ldata++;
  }
  if (max>0)
  {
    Maxi=Maxi/max;
    for (int i=0; i<Count; i++) *(data++)*=Maxi;
  }
  return max;
}//Normalize

/**
  function Normalize2: normalizes data to a specified Euclidian norm

  In: data[Count]: data to normalize
      Norm: destination Euclidian norm
  Out: data[Count]: normalized data.

  Returns the original Euclidian norm.
*/
double Normalize2(double* data, int Count, double Norm)
{
  double norm=0;
  for (int i=0; i<Count; i++) norm+=data[i]*data[i];
  norm=sqrt(norm);
  double mul=norm/Norm;
  if (mul!=0) for (int i=0; i<Count; i++) data[i]/=mul;
  return norm;
}//Normalize2

//---------------------------------------------------------------------------
/**
  function PhaseSpan: computes the unwrapped phase variation across the Nyquist range

  In: data[Count]: time-domain data
      aparams: a fftparams structure

  Returns the difference between unwrapped phase angles at 0 and Nyquist frequency.
*/
double PhaseSpan(double* data, int Count, void* aparams)
{
  int Pad=1;
  fftparams* params=(fftparams*)aparams;
  double* Arg=new double[Count*Pad];
  AllocateFFTBuffer(Count*Pad, Amp, w, x);
  memset(Amp, 0, sizeof(double)*Count*Pad);
  memcpy(&Amp[Count*(Pad-1)/2], data, sizeof(double)*Count);
  ApplyWindow(Amp, Amp, params->Amp, Count);
  RFFTC(Amp, Amp, Arg, Log2(Count*Pad), w, x, 0);

  SmoothPhase(Arg, Count*Pad/2+1);
  double result=Arg[Count*Pad/2]-Arg[0];
  delete[] Arg;
  FreeFFTBuffer(Amp);
  return result;
}//PhaseSpan

//---------------------------------------------------------------------------
/**
  function PolyFit: least square polynomial fitting y=sum(i){a[i]*x^i}

  In: x[N], y[N]: sample points
      P: order of polynomial, P<N
  Out: a[P+1]: coefficients of polynomial

  No return value.
*/
void PolyFit(int P, double* a, int N, double* x, double* y)
{
  Alloc2(P+1, P+1, aa);
  double ai0, bi, *bb=new double[P+1], *s=new double[N], *r=new double[N];
  aa[0][0]=N; bi=0; for (int i=0; i<N; i++) s[i]=1, r[i]=y[i], bi+=y[i]; bb[0]=bi;

  for (int i=1; i<=P; i++)
  {
    ai0=bi=0; for (int j=0; j<N; j++) {s[j]*=x[j], r[j]*=x[j]; ai0+=s[j], bi+=r[j];}
    for (int j=0; j<=i; j++) aa[i-j][j]=ai0; bb[i]=bi;
  }
  for (int i=P+1; i<=2*P; i++)
  {
    ai0=0; for (int j=0; j<N; j++) {s[j]*=x[j]; ai0+=s[j];}
    for (int j=i-P; j<=P; j++) aa[i-j][j]=ai0;
  }
  GESCP(P+1, a, aa, bb);
  DeAlloc2(aa); delete[] bb; delete[] s; delete[] r;
}//PolyFit

//---------------------------------------------------------------------------
/**
  function Pow: vector power function

  In: data[Count]: a vector
      ex: expontnet
  Out: data[Count]: point-wise $ex-th power of data[]

  No return value.
*/
void Pow(double* data, int Count, double ex)
{
  for (int i=0; i<Count; i++)
    data[i]=pow(data[i], ex);
}//Power

//---------------------------------------------------------------------------
/**
  Rectify: semi-wave rectification

  In: data[Count]: data to rectify
      th: rectification threshold: values below th are rectified to th
  Out: data[Count]: recitified data

  Returns number of preserved (i.e. not rectified) samples.
*/
int Rectify(double* data, int Count, double th)
{
  int Result=0;
  for (int i=0; i<Count; i++)
  {
    if (data[i]<=th) data[i]=th;
    else Result++;
  }
  return Result;
}//Rectify

//---------------------------------------------------------------------------
/**
  function Res: minimum absolute residue.

  In: in: a number
      mod: modulus

  Returns the minimal absolute residue of $in devided by $mod.
*/
double Res(double in, double mod)
{
  int i=in/mod;
  in=in-i*mod;
  if (in>mod/2) in-=mod;
  if (in<-mod/2) in+=mod;
  return in;
}//Res

//---------------------------------------------------------------------------
/**
  function Romberg: Romberg algorithm for numerical integration

  In: f: function to integrate
      params: extra argument for calling f
      a, b: integration boundaries
      n: depth of sampling

  Returns the integral of f(*, params) over [a, b].
*/
double Romberg(int n, double(*f)(double, void*), double a, double b, void* params)
{
  int np=1;
  double* r1=new double[n+1];
  double* r2=new double[n+1];
  double h=b-a, *swp;
  r1[1]=h*(f(a, params)+f(b, params))/2;
  for (int i=2; i<=n; i++)
  {
    double akh=a+0.5*h; r2[1]=f(akh, params);
    for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h
    r2[1]=0.5*(r1[1]+h*r2[1]);
    double fj=4;
    for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;}  //fj=4^(j-1)
    h/=2; np*=2;
    swp=r1; r1=r2; r2=swp;
  }
  h=r1[n];
  delete[] r1;
  delete[] r2;
  return h;
}//Romberg

/**
  function Romberg: Romberg algorithm for numerical integration, may return before specified depth on
  convergence.

  In: f: function to integrate
      params: extra argument for calling f
      a, b: integration boundaries
      n: depth of sampling
      ep: convergence test threshold

  Returns the integral of f(*, params) over [a, b].
*/
double Romberg(double(*f)(double, void*), double a, double b, void* params, int n, double ep)
{
  int i, np=1;
  double* r1=new double[n+1];
  double* r2=new double[n+1];
  double h=b-a, *swp;
  r1[1]=h*(f(a, params)+f(b, params))/2;
  bool mep=false;
  for (i=2; i<=n; i++)
  {
    double akh=a+0.5*h; r2[1]=f(akh, params);
    for (int k=2; k<=np; k++) {akh+=h; r2[1]+=f(akh, params);} //akh=a+(k-0.5)h
    r2[1]=0.5*(r1[1]+h*r2[1]);
    double fj=4;
    for (int j=2; j<=i; j++) {r2[j]=(fj*r2[j-1]-r1[j-1])/(fj-1); fj*=4;}  //fj=4^(j-1)

    if (fabs(r2[i]-r1[i-1])<ep)
    {
      if (mep) break;
      else mep=true;
    }
    else mep=false;

    h/=2; np*=2;
    swp=r1; r1=r2; r2=swp;
  }
  if (i<=n) h=r2[i];
  else h=r1[n];
  delete[] r1;
  delete[] r2;
  return h;
}//Romberg

//---------------------------------------------------------------------------
//analog and digital sinc functions

//sinca(0)=1, sincd(0)=N, sinca(1)=sincd(1)=0.
/**
  function sinca: analog sinc function.

  In: x: frequency

  Returns sinc(x)=sin(pi*x)/(pi*x), sinca(0)=1, sinca(1)=0
*/
double sinca(double x)
{
  if (x==0) return 1;
  return sin(M_PI*x)/(M_PI*x);
}//sinca

/**
  function sincd_unn: unnormalized discrete sinc function

  In: x: frequency
      N: scale (window size, DFT size)

  Returns sinc(x)=sin(pi*x)/sin(pi*x/N), sincd(0)=N, sincd(1)=0.
*/
double sincd_unn(double x, int N)
{
  if (x==0) return N;
  return sin(M_PI*x)/sin(M_PI*x/N);
}//sincd

//---------------------------------------------------------------------------
/**
  SmoothPhase: phase unwrapping on module mpi*PI, 2PI by default

  In: Arg[Count]: phase angles to unwrap
      mpi: unwrapping modulus, in pi's
  Out: Arg[Count]: unwrapped phase

  Returns the amount of unwrap, in pi's, of the last phase angle
*/
double SmoothPhase(double* Arg, int Count, int mpi)
{
  double m2pi=mpi*M_PI;
  for (int i=1; i<Count-1; i++)
    Arg[i]=Arg[i-1]+Res(Arg[i]-Arg[i-1], m2pi);
  double tmp=Res(Arg[Count-1]-Arg[Count-2], m2pi);
  double result=(Arg[Count-1]-Arg[Count-2]-tmp)/m2pi;
  Arg[Count-1]=Arg[Count-2]+tmp;

  return result;
}//SmoothPhase

//---------------------------------------------------------------------------
//the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)).

/**
  StiffB: computes stiffness coefficient from fundamental and another partial frequency based on the
  stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)).

  In: f0: fundamental frequency
      m: 1-based partial index
      fm: frequency of partial m

  Returns stiffness coefficient B.
*/
double StiffB(double f0, double fm, int m)
{
  double f2=fm/m/f0;
  return (f2*f2-1)/(m*m-1);
}//StiffB

//StiffF: partial frequency of a stiff string
/**
  StiffFm: computes a partial frequency from fundamental frequency and partial index based on the stiff
  string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)).

  In: f0: fundamental frequency
      m: 1-based partial index
      B: stiffness coefficient

  Returns frequency of the m-th partial.
*/
double StiffFm(double f0, int m, double B)
{
  return m*f0*sqrt(1+B*(m*m-1));
}//StiffFm

/**
  StiffF0: computes fundamental frequency from another partial frequency and stiffness coefficient based
  on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)).

  In: m: 1-based partial index
      fm: frequency of partial m
      B: stiffness coefficient

  Returns the fundamental frequency.
*/
double StiffF0(double fm, int m, double B)
{
  return fm/m/sqrt(1+B*(m*m-1));
}//StiffF0

/**
  StiffM: computes 1-based partial index from partial frequency, fundamental frequency and stiffness
  coefficient based on the stiff string partial frequency model f[m]=mf[1]*sqrt(1+B(m*m-1)).

  In: f0: fundamental freqency
      fm: a partial frequency
      B: stiffness coefficient

  Returns the 1-based partial index which will generate the specified partial frequency from the model
  which, however, does not have to be an integer in this function.
*/
double StiffM(double f0, double fm, double B)
{
  if (B<1e-14) return fm/f0;
  double b1=B-1, ff=fm/f0;
  double delta=b1*b1+4*B*ff*ff;
  if (delta<0)
    return sqrt(b1/2/B);
  else
    return sqrt((b1+sqrt(delta))/2/B);
}//StiffMd

//---------------------------------------------------------------------------
/**
  TFFilter: time-frequency filtering with Hann-windowed overlap-add.

  In: data[Count]: input data
      Spans: time-frequency spans
      wt, windp: type and extra parameter of FFT window
      Sps: sampling rate
      TOffst: optional offset applied to all time values in Spans, set to Spans timing of of data[0].
      Pass: set to pass T-F content covered by Spans, clear to stop T-F content covered by Spans
  Out: dataout[Count]: filtered data

  No return value. Identical data and dataout allowed.
*/
void TFFilter(double* data, double* dataout, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst)
{
  int HWid=Wid/2;
  int Fr=Count/HWid-1;
  int Order=Log2(Wid);

  int** lspan=new int*[Fr];
  double* lxspan=new double[Fr];

  lspan[0]=new int[Fr*Wid];
  for (int i=1; i<Fr; i++)
    lspan[i]=&lspan[0][i*Wid];

  //fill local filter span table
  if (Pass)
    memset(lspan[0], 0, sizeof(int)*Fr*Wid);
  else
    for (int i=0; i<Fr; i++)
      for (int j=0; j<Wid; j++)
        lspan[i][j]=1;

  for (int i=0; i<Spans->Count; i++)
  {
    int lx1, lx2, ly1, ly2;
    lx1=(Spans->Items[i].T1-TOffst)/HWid-1;
    lx2=(Spans->Items[i].T2-1-TOffst)/HWid;
    ly1=Spans->Items[i].F1*2/Sps*HWid+0.001;
    ly2=Spans->Items[i].F2*2/Sps*HWid+1;
    if (lx1<0) lx1=0;
    if (lx2>=Fr) lx2=Fr-1;
    if (ly1<0) ly1=0;
    if (ly1>HWid) ly1=HWid;
    if (Pass)
      for (int x=lx1; x<=lx2; x++)
        for (int y=ly1; y<=ly2; y++)
          lspan[x][y]=1;
    else
      for (int x=lx1; x<=lx2; x++)
        for (int y=ly1; y<=ly2; y++)
          lspan[x][y]=0;
  }
  for (int i=0; i<Fr; i++)
  {
    lxspan[i]=0;
    for (int j=0; j<=HWid; j++)
    {
      if (lspan[i][j])
        lxspan[i]=lxspan[i]+1;
    }
    lxspan[i]/=(HWid+1);
  }
  double* winf=NewWindow(wt, Wid, 0, &windp);
  double* wini=NewWindow(wtHann, Wid, NULL, NULL);
  for (int i=0; i<Wid; i++)
    if (winf[i]!=0) wini[i]=wini[i]/winf[i];
  AllocateFFTBuffer(Wid, ldata, w, x);
  double* tmpdata=new double[HWid];
  memset(tmpdata, 0, HWid*sizeof(double));

  for (int i=0; i<Fr; i++)
  {
    if (lxspan[i]==0)
    {
      memcpy(&dataout[i*HWid], tmpdata, sizeof(double)*HWid);
      memset(tmpdata, 0, sizeof(double)*HWid);
      continue;
    }
    if (lxspan[i]==1)
    {
      memcpy(ldata, &data[i*HWid], Wid*sizeof(double));
      if (i>0)
        for (int k=0; k<HWid; k++)
          ldata[k]=ldata[k]*winf[k]*wini[k];
      for (int k=HWid; k<Wid; k++)
        ldata[k]=ldata[k]*winf[k]*wini[k];

      memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double));
      for (int k=0; k<HWid; k++)
        dataout[i*HWid+k]+=ldata[k];
      memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
      continue;
    }
    memcpy(ldata, &data[i*HWid], Wid*sizeof(double));
    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*winf[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*winf[k];

    RFFTC(ldata, NULL, NULL, Order, w, x, 0);

    if (!lspan[i][0]) x[0].x=x[0].y=0;
    for (int j=1; j<=HWid; j++)
      if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0;

    CIFFTR(x, Order, w, ldata);

    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*wini[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*wini[k];

    memcpy(&dataout[i*HWid], tmpdata, HWid*sizeof(double));
    for (int k=0; k<HWid; k++)
      dataout[i*HWid+k]+=ldata[k];
    memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
  }
  memcpy(&dataout[Fr*HWid], tmpdata, sizeof(double)*HWid);
  memset(&dataout[Fr*HWid+HWid], 0, sizeof(double)*(Count-Fr*HWid-HWid));

  FreeFFTBuffer(ldata);
  delete[] lspan[0];
  delete[] lspan;
  delete[] lxspan;
  delete[] tmpdata;
  delete[] winf;
  delete[] wini;
}//TFFilter
//version on integer data, where BytesPerSample specified the integer format.
void TFFilter(void* data, void* dataout, int BytesPerSample, int Count, int Wid, TTFSpans* Spans, bool Pass, WindowType wt, double windp, int Sps, int TOffst)
{
  int HWid=Wid/2;
  int Fr=Count/HWid-1;
  int Order=Log2(Wid);

  int** lspan=new int*[Fr];
  double* lxspan=new double[Fr];

  lspan[0]=new int[Fr*Wid];
  for (int i=1; i<Fr; i++)
    lspan[i]=&lspan[0][i*Wid];

  //fill local filter span table
  if (Pass)
    memset(lspan[0], 0, sizeof(int)*Fr*Wid);
  else
    for (int i=0; i<Fr; i++)
      for (int j=0; j<Wid; j++)
        lspan[i][j]=1;

  for (int i=0; i<Spans->Count; i++)
  {
    int lx1, lx2, ly1, ly2;
    lx1=(Spans->Items[i].T1-TOffst)/HWid-1;
    lx2=(Spans->Items[i].T2-1-TOffst)/HWid;
    ly1=Spans->Items[i].F1*2/Sps*HWid+0.001;
    ly2=Spans->Items[i].F2*2/Sps*HWid+1;
    if (lx1<0) lx1=0;
    if (lx2>=Fr) lx2=Fr-1;
    if (ly1<0) ly1=0;
    if (ly1>HWid) ly1=HWid;
    if (Pass)
      for (int x=lx1; x<=lx2; x++)
        for (int y=ly1; y<=ly2; y++)
          lspan[x][y]=1;
    else
      for (int x=lx1; x<=lx2; x++)
        for (int y=ly1; y<=ly2; y++)
          lspan[x][y]=0;
  }
  for (int i=0; i<Fr; i++)
  {
    lxspan[i]=0;
    for (int j=0; j<=HWid; j++)
    {
      if (lspan[i][j])
        lxspan[i]=lxspan[i]+1;
    }
    lxspan[i]/=(HWid+1);
  }
  double* winf=NewWindow(wt, Wid, 0, &windp);
  double* wini=NewWindow(wtHann, Wid, NULL, NULL);
  for (int i=0; i<Wid; i++)
    if (winf[i]!=0) wini[i]=wini[i]/winf[i];
  AllocateFFTBuffer(Wid, ldata, w, x);
  double* tmpdata=new double[HWid];
  memset(tmpdata, 0, HWid*sizeof(double));

  for (int i=0; i<Fr; i++)
  {
    if (lxspan[i]==0)
    {
      DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid);
      memset(tmpdata, 0, sizeof(double)*HWid);
      continue;
    }
    if (lxspan[i]==1)
    {
      IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid);
      if (i>0)
        for (int k=0; k<HWid; k++)
          ldata[k]=ldata[k]*winf[k]*wini[k];
      for (int k=HWid; k<Wid; k++)
        ldata[k]=ldata[k]*winf[k]*wini[k];

      for (int k=0; k<HWid; k++) tmpdata[k]+=ldata[k];
      DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid);
      memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
      continue;
    }
    IntToDouble(ldata, &((char*)data)[i*HWid*BytesPerSample], BytesPerSample, Wid);
    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*winf[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*winf[k];

    RFFTC(ldata, NULL, NULL, Order, w, x, 0);

    if (!lspan[i][0]) x[0].x=x[0].y=0;
    for (int j=1; j<=HWid; j++)
      if (!lspan[i][j]) x[j].x=x[Wid-j].x=x[j].y=x[Wid-j].y=0;

    CIFFTR(x, Order, w, ldata);

    if (i>0)
      for (int k=0; k<HWid; k++)
        ldata[k]=ldata[k]*wini[k];
    for (int k=HWid; k<Wid; k++)
      ldata[k]=ldata[k]*wini[k];


    for (int k=0; k<HWid; k++)
      tmpdata[k]+=ldata[k];
    DoubleToInt(&((char*)dataout)[i*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid);
    memcpy(tmpdata, &ldata[HWid], HWid*sizeof(double));
  }
  DoubleToInt(&((char*)dataout)[Fr*HWid*BytesPerSample], BytesPerSample, tmpdata, HWid);
  memset(&((char*)dataout)[(Fr*HWid+HWid)*BytesPerSample], 0, BytesPerSample*(Count-Fr*HWid-HWid));

  FreeFFTBuffer(ldata);

  delete[] lspan[0];
  delete[] lspan;
  delete[] lxspan;
  delete[] tmpdata;
  delete[] winf;
  delete[] wini;
}//TFFilter