annotate sinest.cpp @ 5:5f3c32dc6e17

* Adjust comment syntax to permit Doxygen to generate HTML documentation; add Doxyfile

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

#include <stddef.h>
#include "sinest.h"
#include "fft.h"
#include "opt.h"
#include "sinsyn.h"
#include "splines.h"
#include "windowfunctions.h"

/** \file sinest.h */

//---------------------------------------------------------------------------
/**
  function dsincd_unn: derivative of unnormalized discrete sinc function

  In: x, scale N

  Returns the derivative of sincd_unn(x, N)
*/
double dsincd_unn(double x, int N)
{
  double r=0;
  double omg=M_PI*x;
  double domg=omg/N;
  if (fabs(x)>1e-6)
  {
    r=M_PI*(cos(omg)-sin(omg)*cos(domg)/sin(domg)/N)/sin(domg);
  }
  else
  {
    if (domg!=0)
    {
      double sindomg=sin(domg);
      r=-omg*omg*omg*(1-1.0/(1.0*N*N))/3*M_PI/N/sindomg/sindomg;
    }
    else
      r=0;
  }
  return r;
}//dsincd_unn

/**
  function ddsincd_unn: 2nd-order derivative of unnormalized discrete sinc function

  In: x, scale (equivalently, window size) N

  Returns the 2nd-order derivative of sincd_unn(x, N)
*/
double ddsincd_unn(double x, int N)
{
  double r=0;
  double omg=M_PI*x;
  double domg=omg/N;
  double PI2=M_PI*M_PI;
  double NN=1.0/N/N-1;
  if (domg==0)
  {
    r=PI2*N*NN/3;
  }
  else
  {
    if (fabs(x)>1e-5)
    {
      r=sin(domg)*cos(omg)-sin(omg)*cos(domg)/N;
    }
    else
    {
      r=omg*omg*omg/N*NN/3;
    }
    double ss=sin(omg)*NN;
    r=-2.0/N*cos(domg)*r/sin(domg)/sin(domg)+ss;
    r=r*PI2/sin(domg);
  }
  return r;
}//ddsincd_unn

//---------------------------------------------------------------------------
/**
  function Window: calculates the cosine-family-windowed spectrum of a complex sinusoid on [0:N-1] at
  frequency f bins with zero central phase.

  In: f: frequency, in bins
      N: window size
      M, c[]: cosine-family window decomposition coefficients
  Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2.

  Returns pointer to x. x is created anew if x=0 is specified on start.
*/
cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2)
{
  if (K1<0) K1=0;
  if (K2>N/2-1) K2=N/2-1;

  if (!x) x=new cdouble[K2-K1+1];
  memset(x, 0, sizeof(cdouble)*(K2-K1+1));

  for (int l=K1-M; l<=K2+M; l++)
  {
    double ang=(f-l)*M_PI;
    double omg=ang/N;
		long double si, co, sinn=sin(ang);
		si=sin(omg), co=cos(omg);
		double sa=(ang==0)?N:(sinn/si);
    double saco=sa*co;

    int k1=l-M, k2=l+M;
    if (k1<K1) k1=K1;
    if (k2>K2) k2=K2;

    for (int k=k1; k<=k2; k++)
    {
      int m=k-l, kt=k-K1;
      if (m<0) m=-m;
      if (k%2)
      {
        x[kt].x-=c[m]*saco;
        x[kt].y+=c[m]*sinn;
      }
      else
      {
        x[kt].x+=c[m]*saco;
        x[kt].y-=c[m]*sinn;
      }
    }
  }
  return x;
}//Window

/**
  function dWindow: calculates the cosine-family-windowed spectrum and its derivative of a complex
  sinusoid on [0:N-1] at frequency f bins with zero central phase.

  In: f: frequency, in bins
      N: window size
      M, c[]: cosine-family window decomposition coefficients
  Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
       dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2

  No return value.
*/
void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
{
  if (K1<0) K1=0;
  if (K2>N/2-1) K2=N/2-1;
  memset(x, 0, sizeof(cdouble)*(K2-K1+1));
  memset(dx, 0, sizeof(cdouble)*(K2-K1+1));

  for (int l=K1-M; l<=K2+M; l++)
  {
    double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
		long double si, co, sinn=sin(Omg), cosn=cos(Omg);
		si=sin(omg), co=cos(omg);
		double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N);
    double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, cosnpi=cosn*M_PI;

    int k1=l-M, k2=l+M;
    if (k1<K1) k1=K1;
    if (k2>K2) k2=K2;

    for (int k=k1; k<=k2; k++)
    {
      int m=k-l, kt=k-K1;
      if (m<0) m=-m;
      if (k%2)
      {
        x[kt].x-=c[m]*saco;
        x[kt].y+=c[m]*sinn;
        dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
        dx[kt].y+=c[m]*cosnpi;
      }
      else
      {
        x[kt].x+=c[m]*saco;
        x[kt].y-=c[m]*sinn;
        dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
        dx[kt].y-=c[m]*cosnpi;
      }
    }
  }
}//dWindow

/**
  function ddWindow: calculates the cosine-family-windowed spectrum and its 1st and 2nd derivatives of
  a complex sinusoid on [0:N-1] at frequency f bins with zero central phase.

  In: f: frequency, in bins
      N: window size
      M, c[]: cosine-family window decomposition coefficients
  Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
       dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2
       ddx[0...K2-K1] containing the 2nd-order derivative spectrum at bins K1, ..., K2

  No return value.
*/
void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
{
  if (K1<0) K1=0;
  if (K2>N/2-1) K2=N/2-1;
  memset(x, 0, sizeof(cdouble)*(K2-K1+1));
  memset(dx, 0, sizeof(cdouble)*(K2-K1+1));
  memset(ddx, 0, sizeof(cdouble)*(K2-K1+1));

  for (int l=K1-M; l<=K2+M; l++)
  {
    double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
    long double si, co, sinn=sin(Omg), cosn=cos(Omg);
    si=sin(omg), co=cos(omg);
    double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
    double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, sinnpipi=sinn*M_PI*M_PI, cosnpi=cosn*M_PI,
           cosnpipi_n=cosnpi*M_PI/N, sipi_n=si*M_PI/N;

    int k1=l-M, k2=l+M;
    if (k1<K1) k1=K1;
    if (k2>K2) k2=K2;

    for (int k=k1; k<=k2; k++)
    {
      int m=k-l, kt=k-K1;
      if (m<0) m=-m;
      if (k%2)
      {
        x[kt].x-=c[m]*saco;
        x[kt].y+=c[m]*sinn;
        dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
        dx[kt].y+=c[m]*cosnpi;
        ddx[kt].x-=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
        ddx[kt].y-=c[m]*sinnpipi;
      }
      else
      {
        x[kt].x+=c[m]*saco;
        x[kt].y-=c[m]*sinn;
        dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
        dx[kt].y-=c[m]*cosnpi;
        ddx[kt].x+=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
        ddx[kt].y+=c[m]*sinnpipi;
      }
    }
  }
}//ddWindow

//---------------------------------------------------------------------------
/**
  function IPWindow: computes the truncated inner product of a windowed spectrum with that of a sinusoid
  at reference frequency f.

  In: x[0:N-1]: input spectrum
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
      returnamplitude: specifies return value, true for amplitude, false for angle

  Returns the amplitude or phase of the inner product, as specified by $returnamplitude. The return
  value is interpreted as the actual amplitude/phase of a sinusoid being estimated at f.
*/
double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude)
{
  cdouble r=IPWindowC(f, x, N, M, c, iH2, K1, K2);
  double result;
  if (returnamplitude) result=sqrt(r.x*r.x+r.y*r.y);
	else result=arg(r);
  return result;
}//IPWindow
//wrapper function
double IPWindow(double f, void* params)
{
  struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
  return IPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, true);
}//IPWindow

/**
  function ddIPWindow: computes the norm of the truncated inner product of a windowed spectrum with
  that of a sinusoid at reference frequency f, as well as its 1st and 2nd derivatives.

  In: x[0:N-1]: input spectrum
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: ipwindow and dipwindow: the truncated inner product norm and its derivative

  Returns the 2nd derivative of the norm of the truncated inner product.
*/
double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow)
{
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;
	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2], *lx=&x[K1];
	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
	cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
	delete[] w;

	double R2=~r,
				 R=sqrt(R2),
				 dR2=2*(r.x*dr.x+r.y*dr.y),
				 dR=dR2/(2*R),
				 ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr),
				 ddR=(R*ddR2-dR2*dR)/(2*R2);
	ipwindow=R*iH2;
	dipwindow=dR*iH2;
	return ddR*iH2;
}//ddIPWindow
//wrapper function
double ddIPWindow(double f, void* params)
{
	struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
	return ddIPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow);
}//ddIPWindow

//---------------------------------------------------------------------------
/**
  function IPWindowC: computes the truncated inner product of a windowed spectrum with that of a
  sinusoid at reference frequency f.

  In: x[0:N-1]: input spectrum
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive

  Returns the inner product. The return value is interpreted as the actual amplitude-phase factor of a
  sinusoid being estimated at f.
*/
cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2)
{
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;
	cdouble *w=new cdouble[K];
	cdouble *lx=&x[K1], result=0;
	Window(w, f, N, M, c, K1, K2);
	for (int k=0; k<K; k++) result+=lx[k]^w[k];
	delete[] w;
	result*=iH2;
	return result;
}//IPWindowC

//---------------------------------------------------------------------------
/**
  function sIPWindow: computes the total energy of truncated inner products between multiple windowed
  spectra and that of a sinusoid at a reference frequency f. This does not consider phase alignment
  between the spectra, supposedly measured at a sequence of known instants.

  In: x[L][N]: input spectra
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)

  Returns the energy of the vector of inner products.
*/
double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd)
{
	double sip=0;
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;
	cdouble *w=new cdouble[K];
	Window(w, f, N, M, c, K1, K2);
	for (int l=0; l<L; l++)
	{
		cdouble *lx=&x[l][K1];
		cdouble r=Inner(K, lx, w);
		if (lmd) lmd[l]=r*iH2;
		sip+=~r;
	}
	sip*=iH2;
	delete[] w;
	return sip;
}//sIPWindow
//wrapper function
double sIPWindow(double f, void* params)
{
	struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dipwindow; double ipwindow; cdouble* lmd;} *p=(l_ip *)params;
	return sIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->lmd);
}//sIPWindow

/**
  function dsIPWindow: computes the total energy of truncated inner products between multiple windowed
  spectra and that of a sinusoid at a reference frequency f, as well as its derivative. This does not
  consider phase synchronization between the spectra, supposedly measured at a sequence of known
  instants.

  In: x[L][N]: input spectra
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: sip, the energy of the vector of inner products.

  Returns the derivative of the energy of the vector of inner products.
*/
double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip)
{
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;
	cdouble *w=new cdouble[K*2], *dw=&w[K];
	dWindow(dw, w, f, N, M, c, K1, K2);
	double dsip; sip=0;
	for (int l=0; l<L; l++)
	{
		cdouble* lx=&x[l][K1];
		cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
		double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y);
		sip+=R2, dsip+=dR2;
	}
	sip*=iH2, dsip*=iH2;
	delete[] w;
	return dsip;
}//dsIPWindow
//wrapper function
double dsIPWindow(double f, void* params)
{
	struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double sip;} *p=(l_ip1 *)params;
	return dsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
}//dsIPWindow

/**
  function dsdIPWindow_unn: computes the energy of unnormalized truncated inner products between a given
  windowed spectrum and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd
  derivatives. "Unnormalized" indicates that the inner product cannot be taken as the actual amplitude-
  phase factor of a sinusoid, but deviate from that by an unspecified factor.

  In: x[N]: input spectrum
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: sipwindow and dsipwindow, the energy and its derivative of the unnormalized inner product.

  Returns the 2nd derivative of the inner product.
*/
double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn)
{
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;

	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];

	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);

	double rr=0, ri=0, drr=0, dri=0, ddrr=0, ddri=0;
  cdouble *lx=&x[K1];
  for (int k=0; k<K; k++)
  {
		rr+=lx[k].x*w[k].x+lx[k].y*w[k].y;
    ri+=lx[k].y*w[k].x-lx[k].x*w[k].y;
    drr+=lx[k].x*dw[k].x+lx[k].y*dw[k].y;
    dri+=lx[k].y*dw[k].x-lx[k].x*dw[k].y;
		ddrr+=lx[k].x*ddw[k].x+lx[k].y*ddw[k].y;
		ddri+=lx[k].y*ddw[k].x-lx[k].x*ddw[k].y;
	}
	delete[] w;

	double R2=rr*rr+ri*ri,
         dR2=2*(rr*drr+ri*dri),
         ddR2=2*(rr*ddrr+ri*ddri+drr*drr+dri*dri);
	sipwindow=R2;
  dsipwindow=dR2;
  if (w_unn) w_unn->x=rr, w_unn->y=ri;
	return ddR2;
}//ddsIPWindow_unn

/**
  function ddsIPWindow: computes the total energy of truncated inner products between multiple windowed
  spectra and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd derivatives.
  This does not consider phase synchronization between the spectra, supposedly measured at a sequence
  of known instants.

  In: x[L][N]: input spectra
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: sip and dsip, the energy of the vector of inner products and its derivative.

  Returns the 2nd derivative of the energy of the vector of inner products.
*/
double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip)
{
  if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
  int K=K2-K1+1;
	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
  double ddsip=0; dsip=sip=0;
  for (int l=0; l<L; l++)
  {
		cdouble* lx=&x[l][K1];
		cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
		double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y), ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr);
    sip+=R2, dsip+=dR2, ddsip+=ddR2;
	}
  sip*=iH2, dsip*=iH2, ddsip*=iH2;
	delete[] w;
  return ddsip;
}//ddsIPWindow
//wrapper function
double ddsIPWindow(double f, void* params)
{
  struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int Fr; cdouble** x; double dsip; double sip;} *p=(l_ip1 *)params;
	return ddsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dsip, p->sip);
}//ddsIPWindow

//---------------------------------------------------------------------------
/**
  function sIPWindowC: computes the total energy of truncated inner products between multiple frames of
  a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f.

  In: x[L][N]: the spectrogram
      offst_rel: frame offset, relative to frame size
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)

  Returns the energy of the vector of inner products.
*/
double sIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd)
{
	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;
	cdouble *w=new cdouble[K];
	double Cr=0;
	cdouble Cc=0;
	Window(w, f, N, M, c, K1, K2);
	for (int l=0; l<L; l++)
	{
		cdouble *lx=&x[l][K1];
		cdouble r=Inner(K, lx, w);
		Cr+=~r;
		double ph=-4*M_PI*f*offst_rel*l;
		cdouble r2=r*r;
		Cc+=r2.rotate(ph);
		if (lmd) lmd[l]=r;
	}
	delete[] w;
	double result=0.5*iH2*(Cr+abs(Cc));
	if (lmd)
	{
		double absCc=abs(Cc), hiH2=0.5*iH2;
		cdouble ej2ph=Cc/absCc;
		for (int l=0; l<L; l++)
		{
      double ph=4*M_PI*f*offst_rel*l;
			lmd[l]=hiH2*(lmd[l]+(ej2ph**lmd[l]).rotate(ph));
    }
	}
  return result;
}//sIPWindowC
//wrapper function
double sIPWindowC(double f, void* params)
{
  struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
	return sIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2);
}//sIPWindowC

/**
  function dsIPWindowC: computes the total energy of truncated inner products between multiple frames of
  a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, together
  with its derivative.

  In: x[L][N]: the spectrogram
      offst_rel: frame offset, relative to frame size
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: sip: energy of the vector of the inner products

  Returns the 1st derivative of the energy of the vector of inner products.
*/
double dsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip)
{
  if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;

	cdouble *w=new cdouble[K*2], *dw=&w[K];
  dWindow(dw, w, f, N, M, c, K1, K2);
	double Cr=0, dCr=0;
  cdouble Cc=0, dCc=0;
	for (int l=0; l<L; l++)
  {
		cdouble *lx=&x[l][K1];
    cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
		Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y);
    int two=2;
		cdouble r2=r*r, dr2=r*dr*two;
    double lag=-4*M_PI*offst_rel*l, ph=lag*f;
		Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph);
  }
	double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y);
  double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1);
	sip=0.5*iH2*(Cr+Cc1);
  double dsip=0.5*iH2*(dCr+dCc1);
	delete[] w;
	return dsip;
}//dsIPWindowC
//wrapper function
double dsIPWindowC(double f, void* params)
{
  struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sip;} *p=(l_ip *)params;
	return dsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
}//dsIPWindowC

/**
  function ddsIPWindowC: computes the total energy of truncated inner products between multiple frames
  of a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f,
  together with its 1st and 2nd derivatives.

  In: x[L][N]: the spectrogram
      offst_rel: frame offset, relative to frame size
      f: reference frequency, in bins
      M, c[], iH2: cosine-family window specification parameters
      K1, K2: spectrum truncation bounds, in bins, inclusive
  Out: sipwindow, dsipwindow: energy of the vector of the inner products and its derivative

  Returns the 2nd derivative of the energy of the vector of inner products.
*/
double ddsIPWindowC(double f, int L, double offst_rel, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsipwindow, double& sipwindow)
{
  if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
	int K=K2-K1+1;

	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
  ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
	double Cr=0, dCr=0, ddCr=0;
  cdouble Cc=0, dCc=0, ddCc=0;
	for (int l=0; l<L; l++)
  {
		cdouble *lx=&x[l][K1];
    cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
		Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); ddCr+=2*(r.x*ddr.x+r.y*ddr.y+~dr);
    int two=2;
		cdouble r2=r*r, dr2=r*dr*two, ddr2=(dr*dr+r*ddr)*two;
    double lag=-4*M_PI*offst_rel*l, ph=lag*f;
		Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph), ddCc=ddCc+(ddr2+cdouble(0,2*lag)*dr2-r2*lag*lag).rotate(ph);
  }
	double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y), ddCc2=2*(Cc.x*ddCc.x+Cc.y*ddCc.y+~dCc);
  double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1), ddCc1=(Cc1*ddCc2-dCc2*dCc1)/(2*Cc2);
	sipwindow=0.5*iH2*(Cr+Cc1);
  dsipwindow=0.5*iH2*(dCr+dCc1);
	double ddsipwindow=0.5*iH2*(ddCr+ddCc1);
	delete[] w;
	return ddsipwindow;
}//ddsIPWindowC
//wrapper function
double ddsIPWindowC(double f, void* params)
{
	struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double dipwindow; double ipwindow;} *p=(l_ip *)params;
  return ddsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow);
}//ddsIPWindowC

//--------------------------------------------------------------------------
/*
	Least-square-error sinusoid detection function

	version1: picking the highest peak and take measurement of a single sinusoid
	version2: given a rough peak location and take measurement of a single sinusoid

  Complex spectrum x is calculated using N data points windowed by a window function that is specified
  by the parameter set (M, c, iH2). c[0:M] is	provided according to Table 3 in the transfer report, on
  pp.11. iH2 is simply 1/H2, where H2 can be calculated using formula (2.17) on pp.12.

  f & epf are given/returned in bins.

  Further reading: "Least-square-error estimation of sinusoids.pdf"
*/

/**
  function LSESinusoid: LSE estimation of the predominant stationary sinusoid.

  In: x[N]: windowed spectrum
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: a and pp: amplitude and phase estimates

  Returns the frequency estimate, in bins.
*/
double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
{
  struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0}; //(l_hx *)&params;
  int dfshift=offsetof(l_hx, dhxpeak);

  int inp;
  double minp=0;
  for (int i=0; i<N; i++)
  {
    double lf=i, tmp;
    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;
    tmp=IPWindow(lf, &p);
    if (minp<tmp) inp=i, minp=tmp;
  }

  double f=inp;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
  if (tmp==-1)
  {
    Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
  }
  else
    a=p.hxpeak;
  pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
  return f;
}//LSESinusoid

/*function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency.

  In: x[N]: windowed spectrum
      f: initial frequency, in bins
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: f, a and pp: frequency, amplitude and phase estimates

  No return value.
*/
void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
{
  struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};
  int dfshift=offsetof(l_hx, dhxpeak);

  double inp=f;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
  if (tmp==-1)
  {
    Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
  }
  else
    a=p.hxpeak;
  pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
}//LSESinusoid

/**
  function LSESinusoid: LSE estimation of stationary sinusoid predominant within [f1, f2].

  In: x[N]: windowed spectrum
      [f1, f2]: frequency range
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: a and pp: amplitude and phase estimates

  Returns the frequency estimate, in bins.
*/
double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
{
  struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};
  int dfshift=offsetof(l_hx, dhxpeak);

  int inp;
  double minp=0;
  for (int i=f1; i<f2; i++)
  {
    double lf=i, tmp;
    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;
    tmp=IPWindow(lf, &p);
    if (minp<tmp) inp=i, minp=tmp;
  }

  double f=inp;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
  if (tmp==-1)
  {
    Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
  }
  else
    a=p.hxpeak;
  pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
  return f;
}//LSESinusoid

/**
  function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency within [f1,
  f2].

  In: x[N]: windowed spectrum
      f: initial frequency, in bins
      [f1, f2]: frequency range
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: f, a and pp: frequency, amplitude and phase estimates

  Returns 1 if managed to find a sinusoid, 0 if not, upon which $a and $pp are estimated at the initial
  f.
*/
int LSESinusoid(double& f, double f1, double f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
{
  struct l_hx {int N; int k1; int k2; int M; double* c; double iH2; cdouble* x; double dhxpeak; double hxpeak;} p={N, 0, 0, M, c, iH2, x, 0, 0};//(l_hx *)&params;
  int dfshift=offsetof(l_hx, dhxpeak);

  int result=0;
  double inp=f;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double tmp=Newton(f, ddIPWindow, &p, dfshift, epf, 100, 1e-256, f1, f2);
  if (tmp!=-1 && f>f1 && f<f2)
  {
    result=1;
    a=p.hxpeak;
    pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
  }
  else
  {
    Search1DmaxEx(f, &p, IPWindow, f1, f2, &a, epf);
    if (f<=f1 || f>=f2)
    {
      f=inp;
      cdouble r=IPWindowC(f, x, N, M, c, iH2, p.k1, p.k2);
      a=abs(r);
      pp=arg(r);
    }
    else
    {
      result=1;
      pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
    }
  }
  return result;
}//LSESinusoid

/**
  function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
  considering phase-frequency consistency across frames.

  In: x[Fr][N]: spectrogram
      f: initial frequency, in bins
      [f1, f2]: frequency range
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates

  Returns an error bound of the frequency estimate.
*/
double LSESinusoidMP(double& f, double f1, double f2, cdouble** x, int Fr, int N, double B, int M, double* c, double iH2, double* a, double* ph, double epf)
{
  struct l_ip1 {int N; int k1; int k2; int M; double* c; double iH2; int L; cdouble** x; double dsip; double sip; cdouble* lmd;} p={N, 0, 0, M, c,iH2, Fr, x, 0, 0, 0};
  int dfshift=offsetof(l_ip1, dsip), fshift=offsetof(l_ip1, sip);

  double inp=f;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double errf=Newton1dmax(f, f1, f2, ddsIPWindow, &p, dfshift, fshift, dsIPWindow, dfshift, epf);
  if (errf<0) errf=Search1Dmax(f, &p, sIPWindow, f1, f2, a, epf);
  if (a || ph)
  {
    for (int fr=0; fr<Fr; fr++)
    {
      cdouble r=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);
      if (a) a[fr]=abs(r);
      if (ph) ph[fr]=arg(r);
    }
  }
  return errf;
}//LSESinusoidMP

/**
  function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
  considering phase-frequency consistency across frames.

  In: x[Fr][N]: spectrogram
      f: initial frequency, in bins
      [f1, f2]: frequency range
      B: spectral truncation half width, in bins.
      M, c[], iH2: cosine-family window specification parameters
      epf: frequency error tolerance, in bins
  Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates

  Returns an error bound of the frequency estimate. Although the frequencies are estimated assuming
  cross-frame frequency-phase consistency, the final output phase angles are reestimated independently
  for each frame using the frequency estimate.
*/
double LSESinusoidMPC(double& f, double f1, double f2, cdouble** x, int Fr, int N, int Offst, double B, int M, double* c, double iH2, double* a, double* ph, double epf)
{
  struct l_ip {int N; int k1; int k2; int M; double* c; double iH2; int L; double offst_rel; cdouble** x; double sdip; double sip;}
    p={N, 0, 0, M, c,iH2, Fr, Offst*1.0/N, x, 0, 0};
    int dfshift=offsetof(l_ip, sdip), fshift=offsetof(l_ip, sip);

  double inp=f;
  p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
  p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
  double errf=Newton1dmax(f, f1, f2, ddsIPWindowC, &p, dfshift, fshift, dsIPWindowC, dfshift, epf);
  if (errf<0) errf=Search1Dmax(f, &p, sIPWindowC, f1, f2, a, epf);
  if (a || ph)
  {
    cdouble* lmd=new cdouble[Fr];
    sIPWindowC(f, Fr, Offst*1.0/N, x, N, M, c, iH2, p.k1, p.k2, lmd);
    for (int fr=0; fr<Fr; fr++)
    {
      lmd[fr]=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);

      if (a) a[fr]=abs(lmd[fr]);
      if (ph) ph[fr]=arg(lmd[fr]);
    }
    delete[] lmd;
  }
  return errf;
}//LSESinusoidMPC

//---------------------------------------------------------------------------
/**
  function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
  suppression index of eps, i.e. the least square error is minimized with an additional eps*||lmd||^2
  term.

  In: x[Wid]: spectrum
      f[I]: frequencies
      M, c[]: cosine-family window specification parameters
      K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
      eps: energy suppression factor
  Out: lmd[I]: amplitude-phase factors

  No return value.
*/
void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps)
{
  if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
  MList* List=new MList;
  cdouble** Allocate2L(cdouble, I, K, wt, List);
  for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
  cdouble** whw=MultiplyXcXt(I, K, wt, List);
  cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
  for (int i=0; i<I; i++) whw[i][i]+=eps;
  GECP(I, lmd, whw, whx);
  delete List;
}//IPMulti

/**
  function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
  suppression index of eps, and optionally returns residue and sensitivity indicators for each sinusoid.

  In: x[Wid]: spectrum
      f[I]: frequencies
      M, c[]: cosine-family window specification parameters
      K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
      eps: energy suppression factor
  Out: lmd[I]: amplitude-phase factors
       sens[I]: sensitivity indicators
       r1[I]: residue indicators, measured by correlating residue with sinusoid spectra, optional

  No return value. Sensibitily is computed BEFORE applying eps.
*/
void IPMulti(int I, double* f, cdouble* lmd, cfloat* x, int Wid, int K1, int K2, int M, double* c, double eps, double* sens, double* r1)
{
  if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
  MList* List=new MList;
  cdouble** Allocate2L(cdouble, I, K, wt, List);
  for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
  cdouble** whw=MultiplyXcXt(I, K, wt, List);

	//*computes sensitivity if required
  if (sens)
  {
    cdouble** iwhw=Copy(I, whw, List);
    GICP(I, iwhw);
    cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
    for (int i=0; i<I; i++)
    {
			sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
    }
	} //*/
  cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
  for (int i=0; i<I; i++) whw[i][i]+=eps;
  GECP(I, lmd, whw, whx);
  //compute residue if required
  if (r1)
  {
    cdouble* wlmd=MultiplyXty(K, I, wt, lmd, List); //reconstruct
    for (int k=0; k<K; k++) wlmd[k]=wlmd[k]-x[K1+k]; //-residue
    for (int i=0; i<I; i++) //r1[i]=Inner(K, wlmd, wt[i]).abs(); //-residue weighted by window
    {
      r1[i]=0;
      for (int k=0; k<K; k++) r1[i]+=abs(wlmd[k])*abs(wt[i][k]);
    }
  }
  delete List;
}//IPMulti

/**
  function IPMultiSens: computes the sensitivity of the least square estimation of multiple sinusoids given
  their frequencies .

  In: f[I]: frequencies
      M, c[]: cosine-family window specification parameters
      K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
      eps: energy suppression factor
  Out: sens[I]: sensitivity indicators

  No return value. Sensibility is computed AFTER applying eps
*/
void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps)
{
  if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
  MList* List=new MList;
  cdouble** Allocate2L(cdouble, I, K, wt, List);
  for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);

  cdouble** whw=MultiplyXcXt(I, K, wt, List);
  for (int i=0; i<I; i++) whw[i][i]+=eps;

  cdouble** iwhw=Copy(I, whw, List);
  GICP(I, iwhw);
  cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
  for (int i=0; i<I; i++)
	{
    sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
  }
  delete List;
}//IPMultiSens

/**
  function IPMulti: least square estimation of multi-sinusoids with GIVEN frequencies. This version
  operates in groups at least B bins from each other, rather than LSE all frequencies together.

  In: x[Wid]: spectrum
      f[I]: frequencies, must be ordered low to high.
      B: number of bins beyond which sinusoids are treated as non-interfering
      M, c[], iH2: cosine-family window specification parameters
  Out: lmd[I]: amplitude-phase factors

  Returns 0.
*/
double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B)
{
  int i=0, ist=0;
  double Bw=B;
  while (i<I)
  {
    if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
    {
      if (i==I-1) i++;
      //process frequencies from ist to i-1
      if (i-1==ist) //one sinusoid
      {
        double fb=f[ist]; int K1=floor(fb-B+0.5), K2=floor(fb+B+0.5);
        lmd[ist]=IPWindowC(fb, x, Wid, M, c, iH2, K1, K2);
      }
      else
      {
        MList* List=new MList;
        int N=i-ist, K1=floor(f[ist]-B+0.5), K2=floor(f[i-1]+B+0.5), K=K2-K1+1;
        cdouble** Allocate2L(cdouble, N, K, wt, List);
        for (int n=0; n<N; n++) Window(wt[n], f[ist+n], Wid, M, c, K1, K2);
        cdouble* whx=MultiplyXcy(N, K, wt, &x[K1], List); //w*'x=(wt*)x
        cdouble** whw=MultiplyXcXt(N, K, wt, List);
          /*debug cdouble** C=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** C2=SubMatrix(0, whw, 1, 4, 1, 4, List); cdouble** Bh=SubMatrix(0, whw, 1, 4, 0, 1, List); cdouble* Y2=SubVector(0, whx, 1, 4);
          cdouble x2[4]; cdouble x1=lmd[ist], Bhx1[4], dx2[4]; for (int j=0; j<4; j++) Bhx1[j]=x1^Bh[j][0]; GECP(4, x2, C, Y2); GECP(4, dx2, C2, Bhx1);*/
        GECP(N, &lmd[ist], whw, whx); //solving complex linear system (w*'w)a=w*'x
        delete List;
      }
      ist=i;
    }
    i++;
  }
  return 0;
}//IPMulti

/**
  function IPMulti_Direct: LSE estimation of multiple sinusoids given frequencies AND PHASES (direct
  method)

  In: x[Wid]: spectrum
      f[I], ph[I]: frequencies and phase angles.
      B: spectral truncation half width, in bins; sinusoids over 3B bins apart are regarded non-interfering
      M, c[], iH2: cosine-family window specification parameters
  Out: a[I]: amplitudes

  Returns square norm of the residue.
*/
double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B)
{
	MList* List=new MList;
  int i=0, ist=0, hWid=Wid/2;
  cdouble* r=Copy(hWid, x, List); //to store the residue

  double Bw=3.0*B;
  while (i<I)
  {
    if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
    {
      if (i==I-1) i++;

      //process frequencies from ist to i-1
      if (i-1==ist) //one sinusoid
      {
        double fb=f[ist];
        cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
        for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
        double ip=Inner(2*hWid, (double*)x, (double*)w);
        a[ist]=ip*iH2;
        MultiAdd(hWid, r, r, w, -a[ist]);
        delete[] w;
      }
      else
      {
        int N=i-ist;
        cdouble** Allocate2L(cdouble, N, hWid, wt, List);
        for (int n=0; n<N; n++)
        {
          Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
          for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
        }
        double* whxr=MultiplyXy(N, hWid*2, (double**)wt, (double*)x, List); //w*'x=(wt*)x
        double** whwr=MultiplyXXt(N, hWid*2, (double**)wt, List);
        GECP(N, &a[ist], whwr, whxr); //solving complex linear system (w*'w)a=w*'x
        for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
      }
      ist=i;
    }
    i++;
  }
  double result=Inner(hWid, r, r).x;
  delete List;
  return result;
}//IPMulti_Direct

/**
  function IPMulti_GS: LSE estimation of multiple sinusoids given frequencies AND PHASES (Gram-Schmidt method)

  In: x[Wid]: spectrum
      f[I], ph[I]: frequencies and phase angles.
      B: spectral truncation, in bins; sinusoids over 3B bins apart are regarded non-interfering
      M, c[], iH2: cosine-family window specification parameters
  Out: a[I]: amplitudes

  Returns square norm of the residue.
*/
double IPMulti_GS(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B, double** L, double** Q)
{
  MList* List=new MList;
  int i=0, ist=0, hWid=Wid/2;
  cdouble* r=Copy(hWid, x, List); //to store the residue
  double Bw=3.0*B;
  while (i<I)
  {
    if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
    {
      if (i==I-1) i++;

      //process frequencies from ist to i-1
      if (i-1==ist) //one sinusoid
      {
        double fb=f[ist];
        cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
        for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
        double ip=Inner(2*hWid, (double*)x, (double*)w);
        a[ist]=ip*iH2;
        MultiAdd(hWid, r, r, w, -a[ist]);
        delete[] w;
      }
      else
      {
        int N=i-ist;
        cdouble** Allocate2L(cdouble, N, hWid, wt, List);
        Alloc2L(N, N, L, List); Alloc2L(N, hWid*2, Q, List);
        for (int n=0; n<N; n++)
        {
          Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
          for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
        }
        LQ_GS(N, hWid*2, (double**)wt, L, Q);
        double* atl=MultiplyxYt(N, hWid*2, (double*)x, Q, List);
        GExL(N, &a[ist], L, atl);
        for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
      }
      ist=i;
    }
    i++;
  }
  double result=Inner(hWid, r, r).x;
  delete List;
	return result;
}//IPMulti_GS

/**
  function IPMulti: LSE estimation of I sinusoids given frequency and phase and J sinusoids given
  frequency only

  In: x[Wid]: spectrum
      f[I+J], ph[I]: frequencies and phase angles
      M, c[], iH2: cosine-family window specification parameters
  Out: a[I+J]: amplitudes
       ph[I:I+J-1]: phase angles not given on start
       wt[I+2J][hWid], Q[I+2J][hWid], L[I+2J][I+2J]: internal w matrix and its LQ factorization, optional

  Returns the residue vector, newly created and registered to RetList, if specified. On start a[] should
  have valid storage no less than I+2J.
*/
cdouble* IPMulti(int I, int J, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, cdouble** wt, cdouble** Q, double** L, MList* RetList)
{
  MList* List=new MList;
  int hWid=Wid/2;
  cdouble* r=Copy(hWid, x, RetList); //to store the residue
  if (!wt){Allocate2L(cdouble, I+J*2, hWid, wt, List);}
  if (!Q){Allocate2L(cdouble, I+J*2, hWid, Q, List);}
  if (!L){Allocate2L(double, I+J*2, I+J*2, L, List);}
  memset(wt[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
  memset(Q[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
  memset(L[0], 0, sizeof(double)*(I+J*2)*(I+J*2));

  //*The direct form
  for (int i=0; i<I; i++)
  {
    Window(wt[i], f[i], Wid, M, c, 0, hWid-1);
    for (int k=0; k<hWid; k++) wt[i][k].rotate(ph[i]);
  }
  for (int j=0; j<J; j++)
  {
    cdouble *w1=wt[I+j*2], *w2=wt[I+j*2+1];
    Window(w1, f[I+j], Wid, M, c, 0, hWid-1);
    for (int k=0; k<hWid; k++) w2[k].y=w1[k].x, w2[k].x=-w1[k].y;
  }

  LQ_GS(I+J*2, hWid*2, (double**)wt, L, (double**)Q);
  double *atl=MultiplyxYt(I+J*2, hWid*2, (double*)x, (double**)Q, List);
  GExL(I+J*2, a, L, atl);
  
  for (int i=0; i<I+J*2; i++) MultiAdd(hWid, r, r, wt[i], -a[i]);
  for (int j=0; j<J; j++)
  {
    double xx=a[I+j*2], yy=a[I+j*2+1];
    a[I+j]=sqrt(xx*xx+yy*yy);
    ph[I+j]=atan2(yy, xx);
  }
  delete List;
  return r;
}//IPMulti

//---------------------------------------------------------------------------
/*
    Routines for estimation two sinusoids with 1 fixed and 1 flexible frequency

    Further reading: "LSE estimation for 2 sinusoids with 1 at a fixed frequency.pdf"
*/

/**
  function WindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
  sinusoids at frequencies f1 and f2, df=f1-f2.

  In: df: frequency difference, in bins
      N: DFT size
      M, d[]: cosine-family window specification parameters (see "further reading").
  Out: w[0], the inner product, optional

  Returns square norm of the inner product.
*/
double WindowDuo(double df, int N, double* d, int M, cdouble* w)
{
	double wr=0, wi=0;
	for (int m=-2*M; m<=2*M; m++)
	{
		double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
		double si=sin(omg), co=cos(omg), sinn=sin(Omg);
		double sa=(ang==0)?N:(sinn/si);
		double dm; if (m<0) dm=d[-m]; else dm=d[m];
		wr+=dm*sa*co, wi+=-dm*sinn;
	}
	wr*=N, wi*=N;
	if (w) w->x=wr, w->y=wi;
	double result=wr*wr+wi*wi;
	return result;
}//WindowDuo

/**
  function ddWindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
  sinusoids at frequencies f1 and f2, df=f1-f2, with its 1st and 2nd derivatives

  In: df: frequency difference, in bins
      N: DFT size
      M, d[]: cosine-family window specification parameters (see "further reading" for d[]).
  Out: w[0], the inner product, optional
       window, dwindow: square norm and its derivative, of the inner product

  Returns 2nd derivative of the square norm of the inner product.
*/
double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w)
{
	double wr=0, wi=0, dwr=0, dwi=0, ddwr=0, ddwi=0, PI_N=M_PI/N, PIPI_N=PI_N*M_PI, PIPI=M_PI*M_PI;
	for (int m=-2*M; m<=2*M; m++)
	{
		double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
		double si=sin(omg), co=cos(omg), sinn=sin(Omg), cosn=cos(Omg);
		double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
		double dm; if (m<0) dm=d[-m]; else dm=d[m];
		wr+=dm*sa*co, wi+=-dm*sinn;
		dwr+=dm*(dsa*co-PI_N*sinn), dwi+=-dm*M_PI*cosn;
		ddwr+=dm*(ddsa*co-PI_N*dsa*si-PIPI_N*cosn), ddwi+=dm*PIPI*sinn;
	}
  wr*=N, wi*=N, dwr*=N, dwi*=N, ddwr*=N, ddwi*=N;
  window=wr*wr+wi*wi;
  dwindow=2*(wr*dwr+wi*dwi);
  if (w) w->x=wr, w->y=wi;
	double ddwindow=2*(wr*ddwr+dwr*dwr+wi*ddwi+dwi*dwi);
  return ddwindow;
}//ddWindowDuo

/**
  function sIPWindowDuo: calculates the square norm of the orthogonal projection of a windowed spectrum
  onto the linear span of the windowed spectra of two sinusoids at reference frequencies f1 and f2.

  In: x[N]: spectrum
      f1, f2: reference frequencies.
      M, c[], d[], iH2: cosine-family window specification parameters.
      K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.
  Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors

  Returns the square norm of the orthogonal projection.
*/
double sIPWindowDuo(double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2)
{
	int K=K2-K1+1;
  cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
  Window(w1, f1, N, M, c, K1, K2);
  double w1w1=0;
  for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
  for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];
	Window(w1, f2, N, M, c, K1, K2);
  cdouble r1w2=0, w12; for (int k=0; k<K; k++) r1w2+=(r1[k]^w1[k]);
  double w=WindowDuo(f1-f2, N, d, M, &w12);
  double v=1.0/iH2-w*iH2;
  double result=~xw1/w1w1+~r1w2/v;
	cdouble mu2=r1w2/v;
	lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;
  delete[] w1;
  return result;
}//sIPWindowDuo
//wrapper function
double sIPWindowDuo(double f2, void* params)
{
	struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params;
	cdouble r1, r2;
	return sIPWindowDuo(p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
}//sIPWindowDuo

/**
  function ddsIPWindowDuo: calculates the square norm, and its 1st and 2nd derivatives against f2,, of
  the orthogonal projection of a windowed spectrum onto the linear span of the windowed spectra of two
  sinusoids at reference frequencies f1 and f2.

  In: x[N]: spectrum
      f1, f2: reference frequencies.
      M, c[], d[], iH2: cosine-family window specification parameters.
      K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.

  Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors
       ddsip[3]: the 2nd, 1st and 0th derivatives (against f2) of the square norm.

  No return value.
*/
void ddsIPWindowDuo(double* ddsip2, double f1, double f2, cdouble* x, int N, double* c, double* d, int M, double iH2, int K1, int K2, cdouble& lmd1, cdouble& lmd2)
{
  int K=K2-K1+1;
	cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
  Window(w1, f1, N, M, c, K1, K2);
  double w1w1=0;
  for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
  for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];

  cdouble r1w2, w12;
  double u, du, ddu=ddsIPWindow_unn(f2, &r1[-K1], N, M, c, K1, K2, du, u, &r1w2);
	double w, dw, ddw=ddWindowDuo(f1-f2, N, d, M, dw, w, &w12); dw=-dw;
  double v=1.0/iH2-w*iH2, dv=-iH2*dw, ddv=-iH2*ddw;
	double iv=1.0/v;//, div=-dv*iv*iv, ddiv=(2*dv*dv-v*ddv)*iv*iv*iv;

  ddsip2[2]=~xw1/w1w1+u*iv;
  ddsip2[1]=iv*(du-iv*u*dv);
  ddsip2[0]=iv*(ddu-iv*(u*ddv+2*du*dv-2*iv*u*dv*dv));

  cdouble mu2=r1w2*iv;
	lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;

  delete[] w1;
}//ddsIPWindowDuo
//wrapper function
double ddsIPWindowDuo(double f2, void* params)
{
	struct l_ip {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} *p=(l_ip *)params;
  double ddsip2[3]; cdouble r1, r2;
  ddsIPWindowDuo(ddsip2, p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
  p->dipwindow=ddsip2[1], p->ipwindow=ddsip2[2];
  return ddsip2[0];
}//ddsIPWindowDuo

/**
  function LSEDuo: least-square estimation of two sinusoids of which one has a fixed frequency

  In: x[N]: the windowed spectrum
      f1: the fixed frequency
      f2: initial value of the flexible frequency
      fmin, fmax: search range for f2, the flexible frequency
      B: spectral truncation half width
      M, c[], d[], iH2:
      epf: frequency error tolerance
  Out: f2: frequency estimate
       lmd1, lmd2: amplitude-phase factor estimates
  Returns 1 if managed to find a good f2, 0 if not, upon which the initial f2 is used for estimating

  amplitudes and phase angles.
*/
int LSEDuo(double& f2, double fmin, double fmax, double f1, cdouble* x, int N, double B, double* c, double* d, int M, double iH2, cdouble& r1, cdouble &r2, double epf)
{
  int result=0;
  double inp=f2;
  int k1=ceil(inp-B); if (k1<0) k1=0;
  int k2=floor(inp+B); if (k2>=N/2) k2=N/2-1;
  struct l_hx {int N; int k1; int k2; double* c; double* d; int M; double iH2; cdouble* x; double f1; double dipwindow; double ipwindow;} p={N, k1, k2, c, d, M, iH2, x, f1, 0, 0};
  int dfshift=offsetof(l_hx, dipwindow);// fshift=int(&((l_hx*)0)->ipwindow);

  double tmp=Newton(f2, ddsIPWindowDuo, &p, dfshift, epf, 100, 1e-256, fmin, fmax);
  if (tmp!=-1 && f2>fmin && f2<fmax) result=1;
  else
  {
    Search1DmaxEx(f2, &p, sIPWindowDuo, fmin, fmax, NULL, epf);
    if (f2<=fmin || f2>=fmax) f2=inp;
    else result=1;
  }
  sIPWindowDuo(f1, f2, x, N, c, d, M, iH2, k1, k2, r1, r2);
  return result;
}//LSEDuo

//---------------------------------------------------------------------------
/*
    Time-frequency reassignment sinusoid estimation routines.

    Further reading: A. R?bel, ¡°Estimating partial frequency and frequency slope using reassignment
    operators,¡± in Proc. ICMC¡¯02. G?teborg. 2002.
*/

/**
  function CDFTW: single-frequency windowed DTFT, centre-aligned

  In: data[Wid]: waveform data x
      win[Wid+1]: window function
      k: frequency, in bins, where bin=1/Wid
  Out: X: DTFT of xw at frequency k bins

  No return value.
*/
void CDFTW(cdouble& X, double k, int Wid, cdouble* data, double* win)
{
  X=0;
  int hWid=Wid/2;
  for (int i=0; i<Wid; i++)
  {
    cdouble tmp=data[i]*win[Wid-i];
    double ph=-2*M_PI*(i-hWid)*k/Wid;
    tmp.rotate(ph);
    X+=tmp;
  }
}//CDFTW

/**
  function CuDFTW: single-frequency windowed DTFT of t*data[t], centre-aligned

  In: data[Wid]: waveform data x
      wid[Wid+1]: window function
      k: frequency, in bins
  Out: X: DTFT of txw at frequency k bins

  No return value.
*/
void CuDFTW(cdouble& X, int k, int Wid, cdouble* data, double* win)
{
  X=0;
  int hWid=Wid/2;
  for (int i=0; i<Wid; i++)
	{
		double tw=((i-hWid)*win[Wid-i]);
		cdouble tmp=data[i]*tw;
    double ph=-2*M_PI*(i-hWid)*k/Wid;
    tmp.rotate(ph);
		X+=tmp;
  }
}//CuDFTW

/**
  function TFReas: time-frequency reassignment

  In: data[Wid]: waveform data
      win[Wid+1], dwin[Wid+1], ddwin[Wid+1]: window function and its derivatives
      f, t: initial digital frequency and time
  Out: f, t: reassigned digital frequency and time
       fslope: estimate of frequency derivative
       plogaslope[0]: estimate of the derivative of logarithmic amplitude, optional

  No return value.
*/
void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope)
{
	int fi=floor(f*Wid+0.5);

	cdouble x, xt, xw;
  CDFTW(x, fi, Wid, data, win);
  CuDFTW(xw, fi, Wid, data, win); xt.x=xw.y; xw.y=-xw.x; xw.x=xt.x;
  CDFTW(xt, fi, Wid, data, dwin);
  double px=~x;
  t=t-(xw.y*x.x-xw.x*x.y)/px;
	f=1.0*fi/Wid+(xt.y*x.x-xt.x*x.y)/px/(2*M_PI);
	if (plogaslope) plogaslope[0]=-(xt.x*x.x+xt.y*x.y)/px;
  cdouble xtt, xtw;
  CuDFTW(xtw, fi, Wid, data, dwin); xtt.x=xtw.y; xtw.y=-xtw.x; xtw.x=xtt.x;
  CDFTW(xtt, fi, Wid, data, ddwin);
  double dtdt=-(xtw.y*x.x-xtw.x*x.y)/px+((xt.y*x.x-xt.x*x.y)*(xw.x*x.x+xw.y*x.y)+(xt.x*x.x+xt.y*x.y)*(xw.y*x.x-xw.x*x.y))/px/px,
         dwdt=(xtt.y*x.x-xtt.x*x.y)/px-2*(xt.x*x.x+xt.y*x.y)*(xt.y*x.x-xt.x*x.y)/px/px;
  if (dtdt!=0) fslope=dwdt/dtdt/(2*M_PI);
  else fslope=0;
} //TFReas*/

/**
  function TFReas: sinusoid estimation using reassignment method

  In: data[Wid]: waveform data
      w[Wid+1], dw[Wid+1], ddw[Wid+1]: window function and its derivatives
      win[Wid]: window function used for estimating amplitude and phase by projection onto a chirp
      t: time for which the parameters are estimated
      f: initial frequency at t
  Out: f, a, ph: digital frequency, amplitude and phase angle estimated at t
       fslope: frequency derivative estimate

  No return value.
*/
void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win)
{
	double localt=t, logaslope;
	TFReas(f, localt, fslope, Wid, data, w, dw, ddw, &logaslope);

	if (logaslope*Wid>6) logaslope=6.0/Wid;
	else if (logaslope*Wid<-6) logaslope=-6.0/Wid;

	f=f+fslope*(t-localt); //obtain frequency estimate at t

	cdouble x=0;
	if (win==0)
	{
		for (int n=0; n<Wid; n++)
		{
			double ni=n-t;
			cdouble tmp=data[n];
			double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
			tmp.rotate(p);
			x+=tmp;
		}
		a=abs(x)/Wid;
	}
	else
	{
		double sumwin=0;
		for (int n=0; n<Wid; n++)
		{
			double ni=n-t;
			cdouble tmp=data[n]*win[n];
			double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
			tmp.rotate(p);
			x+=tmp; sumwin+=win[n];
		}
		a=abs(x)/sumwin;
	}
	ph=arg(x);
}//TFReas

//---------------------------------------------------------------------------
/*
    Routines for additive and multiplicative reestimation of sinusoids.

    Further reading: Wen X. and M. Sandler, "Additive and multiplicative reestimation schemes
    for the sinusoid modeling of audio," in Proc. EUSIPCO'09, Glasgow, 2009.
*/

/**
  function AdditiveUpdate: additive reestimation of time-varying sinusoid

  In: x[Count]: waveform data
      Wid, Offst: frame size and hop
      fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
      das[Count]: initial estimate of amplitude derivative
      BasicAnalyzer: pointer to a sinusoid analyzer
      LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
  Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update

  No return value.
*/
void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
{
  int HWid=Wid/2, Fr=(Count-Wid)/Offst+1;

	for (int fr=0; fr<Fr; fr++)
	{
		int i=HWid+Offst*fr;
    if (fs[i]<0 || fs[i]>0.5){}
	}

	cdouble *y=new cdouble[Count];
	double *lf=new double[Count*4], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3];

	__int16* ref=new __int16[Count];
	for (int i=0; i<Count; i++) y[i]=x[i].x-as[i]*cos(phs[i]), ref[i]=floor(fs[i]*Wid+0.5);
	memcpy(lf, fs, sizeof(double)*Count);
	BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, ref, reserved, LogA);

  //merge and interpolate
  double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
       *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
       *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
  for (int fr=0; fr<Fr; fr++)
  {
		int i=HWid+Offst*fr;
    double a=as[i], b=la[i], fai=phs[i], thet=lp[i], f=fs[i], g=lf[i], delt=fai-thet, da=das[i], db=lda[i];
		xs[fr]=i;
		if (fabs(f-g)*Wid>1)
    {
      afr[fr]=a, pfr[fr]=fai, ffr[fr]=f;
    }
    else
    {
      double rr=a*cos(fai)+b*cos(thet);
      double ii=a*sin(fai)+b*sin(thet);
			ffr[fr]=(a*f*(a+b*cos(delt))+b*g*(b+a*cos(delt))+(da*b-a*db)*sin(delt)/(2*M_PI))/(a*a+b*b+2*a*b*cos(delt));
      afr[fr]=sqrt(rr*rr+ii*ii);
			pfr[fr]=atan2(ii, rr);
		}
		if (LogA) afr[fr]=log(afr[fr]);
	}
	CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
	CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
	for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
	Sinusoid(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], pfr[0], pfr[1], LogA);
	Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA);
	delete[] fa;  //*/
	/*
  for (int i=0; i<Count; i++)
  {
    double rr=as[i]*cos(phs[i])+la[i]*cos(lp[i]);
    double ii=as[i]*sin(phs[i])+la[i]*sin(lp[i]);
		as[i]=sqrt(rr*rr+ii*ii);
    phs[i]=atan2(ii, rr);
	}         //*/
	for (int fr=0; fr<Fr; fr++)
	{
		int i=HWid+Offst*fr;
    if (fs[i]<0 || fs[i]>0.5){}
	}
	delete[] y; delete[] lf; delete[] ref;
}//AdditiveUpdate

/**
  function AdditiveAnalyzer: sinusoid analyzer with one additive update

  In: x[Count]: waveform data
      Wid, Offst: frame size and hop size
      BasicAnalyzer: pointer to a sinusoid analyzer
      ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
      BasicAnalyzer: pointer to a sinusoid analyzer
      LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
  Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
       das[Count]: estimate of amplitude derivative

  No return value.
*/
void AdditiveAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
{
	BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
	AdditiveUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved, LogA);
}//AdditiveAnalyzer

/**
  function MultiplicativeUpdate: multiplicative reestimation of time-varying sinusoid

  In: x[Count]: waveform data
      Wid, Offst: frame size and hop
      fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
      das[Count]: initial estimate of amplitude derivative
      BasicAnalyzer: pointer to a sinusoid analyzer
      LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
  Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update

  No return value.
*/
void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
{
  int HWid=Wid/2;
  cdouble *y=new cdouble[Count];
  double *lf=new double[Count*8], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3],
         *lf2=&lf[Count*4], *la2=&lf2[Count], *lp2=&lf2[Count*2], *lda2=&lf2[Count*3];
	__int16 *lref=new __int16[Count];

	for (int i=0; i<Count; i++) y[i]=x[i]*(cdouble(1.0).rotate(-phs[i]+i*0.15*2*M_PI)),
														lref[i]=0.15*Wid;
	BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, lref, reserved, LogA);
	for (int i=0; i<Count; i++) y[i]=y[i]*(cdouble(1.0/la[i]).rotate(-lp[i]+i*0.15*2*M_PI)), lref[i]=0.15*Wid;
	BasicAnalyzer(lf2, la2, lp2, lda2, y, Count, Wid, Offst, lref, reserved, LogA);

		/*
		for (int i=0; i<Count; i++)
		{
			as[i]=la[i]*la2[i];
			phs[i]=phs[i]+lp[i]+lp2[i]-0.3*2*M_PI*i;
      fs[i]=fs[i]+lf[i]+lf2[i]-0.3;
		}         //*/

	//merge
	int Fr=(Count-Wid)/Offst+1;
	double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
				 *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
				 *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
	for (int fr=0; fr<Fr; fr++)
	{
		int i=HWid+Offst*fr;
		xs[fr]=i;
		afr[fr]=la[i]*la2[i];
		if (LogA) afr[fr]=log(afr[fr]);
		ffr[fr]=fs[i]+lf[i]-0.15+lf2[i]-0.15;
		pfr[fr]=phs[i]+lp[i]+lp2[i]-0.3*i*2*M_PI;
	}
	CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
	CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
	for (int fr=0; fr<Fr-1; fr++) Sinusoid(&fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], 0, Offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
	Sinusoid(&fs[int(xs[0])], &as[int(xs[0])], &phs[int(xs[0])], &das[int(xs[0])], -HWid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], pfr[0], pfr[1], LogA);
	Sinusoid(&fs[int(xs[Fr-2])], &as[int(xs[Fr-2])], &phs[int(xs[Fr-2])], &das[int(xs[Fr-2])], Offst, Offst+HWid, aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2], pfr[Fr-2], pfr[Fr-1], LogA);
	delete[] fa;  //*/

	for (int fr=0; fr<Fr; fr++)
	{
		int i=HWid+Offst*fr;
    if (fs[i]<0 || fs[i]>0.5){}
	}

	delete[] y; delete[] lf; delete[] lref;
}//MultiplicativeUpdate

/**
  function MultiplicativeAnalyzer: sinusoid analyzer with one multiplicative update

  In: x[Count]: waveform data
      Wid, Offst: frame size and hop size
      BasicAnalyzer: pointer to a sinusoid analyzer
      ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
      BasicAnalyzer: pointer to a sinusoid analyzer
      LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
  Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
       das[Count]: estimate of amplitude derivative

  No return value.
*/
void MultiplicativeAnalyzer(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, __int16* ref, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
{
	BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
	MultiplicativeUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved);
}//MultiplicativeAnalyzer

/*
  This is an earlier version of the multiplicative method without using a user-provided BasicAnalyzer.
  This updates the sinusoid estimates at the selected consecutive FRAMES of x. Only frequency modulation
  is included in the multiplier. The first frame (0) is centred at x[Wid/2]. fs, as, and phs are based
  on frames rather than samples. Updates include frame frst, but not frame fren.
*/
void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst)
{
  int HWid=Wid/2;

  double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
         *xs=&fa[Fr*8];
  for (int fr=0; fr<Fr; fr++) xs[fr]=HWid+Offst*fr;
  CubicSpline(Fr-1, fa, fb, fc, fd, xs, fs, 1, 1);

  int dst=Offst*frst, den=Offst*(fren-1)+Wid, dcount=den-dst;
  double *f=new double[dcount*2], *ph=&f[dcount];
  for (int fr=frst; fr<fren-1; fr++) Sinusoid(&f[int(xs[fr])-dst], &ph[int(xs[fr])-dst], 0, Offst, fa[fr], fb[fr], fc[fr], fd[fr], phs[fr], phs[fr+1]);
  if (frst==0) Sinusoid(&f[int(xs[0])-dst], &ph[int(xs[0])-dst], -HWid, 0, fa[0], fb[0], fc[0], fd[0], phs[0], phs[1]);
  else Sinusoid(&f[int(xs[frst-1])-dst], &ph[int(xs[frst-1])-dst], 0, Offst, fa[frst-1], fb[frst-1], fc[frst-1], fd[frst-1], phs[frst-1], phs[frst]);
  if (fren==Fr) Sinusoid(&f[int(xs[fren-2])-dst], &ph[int(xs[fren-2])-dst], Offst, Offst+HWid, fa[fren-2], fb[fren-2], fc[fren-2], fd[fren-2], phs[fren-2], phs[fren-1]);
  else Sinusoid(&f[int(xs[fren-1])-dst], &ph[int(xs[fren-1])-dst], 0, Offst, fa[fren-1], fb[fren-1], fc[fren-1], fd[fren-1], phs[fren-1], phs[fren]);

  cdouble* y=new cdouble[Wid];
  AllocateFFTBuffer(Wid, Amp, W, X);
  double* win=NewWindow(wtHann, Wid);
  int M; double c[10], iH2; windowspec(wtHann, Wid, &M, c, &iH2);
  for (int fr=frst; fr<fren; fr++)
  {
    __int16* lx=&x[Offst*fr];
    double* lph=&ph[Offst*(fr-frst)];
    for (int i=0; i<Wid; i++) y[i]=cdouble(lx[i]).rotate(-lph[i]+i*0.15*2*M_PI);
    CFFTCW(y, win, Amp, 0, log2(Wid), W, X);
    int pf=0.15*Wid, mpf=pf;
    for (int k=pf-4; k<=pf+4; k++) if (Amp[k]>Amp[mpf]) mpf=k;
    if (mpf>pf-4 && mpf<pf+4) pf=mpf;
    double lfs=pf, lphs;
    LSESinusoid(lfs, pf-3, pf+3, X, Wid, 3, M, c, iH2, as[fr], lphs, 1e-3);
    fs[fr]=fs[fr]+lfs/Wid-0.15;
    phs[fr]+=lphs-0.15*Wid*M_PI;
      as[fr]*=2;
  }

  delete[] y;
  delete[] f;
  delete[] win;
  delete[] fa;
  FreeFFTBuffer(Amp);
}//MultiplicativeUpdateF

//---------------------------------------------------------------------------
/*
    Earlier reestimation method routines.

    Further reading: Wen X. and M. Sandler, "Evaluating parameters of time-varying
    sinusoids by demodulation," in Proc. DAFx'08, Espoo, 2008.
*/

/**
  function ReEstFreq: sinusoid reestimation by demodulating frequency.

  In: x[Wid+Offst*(FrCount-1)]: waveform data
      FrCount, Wid, Offst: frame count, frame size and hop size
      fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
      win[Wid]: window function for estimating demodulated sinusoid
      M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
      Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, optional
      w[Wid/2], ps[Wid], xs[Wid], xc[Wid], fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
  Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles

  No return value.
*/
void ReEstFreq(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* fa, double* fb, double* fc, double* fd, double* ns, int* Wids)
{
  int hWid=Wid/2;
  //reestimate using frequency track
  CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
  for (int fr=0; fr<FrCount; fr++)
  {
    //find ps
    if (fr==0)
    {
      double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0];
      for (int j=0; j<Wid; j++)
      {
         double lx=j-hWid;
         ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
      }
//      memset(ps, 0, sizeof(double)*hWid);
    }
    else if (fr==FrCount-1)
    {
      int lfr=FrCount-2;
      double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
      ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
      for (int j=1; j<Wid; j++)
      {
        ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
      }
//        memset(&ps[hWid], 0, sizeof(double)*hWid);
    }
    else
    {
      int lfr=fr-1;
      double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
      for (int j=1; j<hWid+1; j++)
      {
        ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
      }
      lfr=fr;
      lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      for (int j=1; j<hWid; j++)
      {
        ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
      }
    }
    double* ldata=&x[fr*Offst];
    for (int j=0; j<Wid; j++)
    {
      xs[j].x=ldata[j]*cos(-ps[j]);
      xs[j].y=ldata[j]*sin(-ps[j]);
    }

    if (Wids)
    {
      int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
      SetTwiddleFactors(lWid, w);
      double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
      windowspec(wtHann, lWid, &lM, lc, &liH2);
      CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc);
      delete[] lwin;
      double lf=fbuf[fr]*lWid, la, lp;
      LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
      if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
    }
    else
    {
      CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc);
      double lf=fbuf[fr]*Wid, la, lp;
      LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
      if (la*2>abuf[fr])
        fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
    }
  }
}//ReEstFreq

/**
  function ReEstFreq_2: sinusoid reestimation by demodulating frequency. This is that same as ReEstFreq(...)
  except that it calls Sinusoid(...) to synthesize the phase track used for demodulation and that it
  does not allow variable window sizes for estimating demodulated sinusoid.

  In: x[Wid+Offst*(FrCount-1)]: waveform data
      FrCount, Wid, Offst: frame count, frame size and hop size
      fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
      win[Wid]: window function for LSE sinusoid estimation
      M, c[], iH2: cosine-family window specification parameters, must be consistent with M, c, iH2
      w[Wid/2], xs[Wid], xc[Wid], f3[FrCount-1], f2[FrCount-1], f1[FrCount-1], f0[FrCount-1]: buffers
  Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles

  No return value.
*/
void ReEstFreq_2(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* f3, double* f2, double* f1, double* f0, double* ns)
{
  int hWid=Wid/2;
  //reestimate using frequency track
  CubicSpline(FrCount-1, f3, f2, f1, f0, ns, fbuf, 1, 1);
  double *refcos=(double*)malloc8(sizeof(double)*Wid), *refsin=&refcos[hWid], ph=0, centralph;

  memset(f0, 0, sizeof(double)*FrCount);

    int N=Wid+Offst*(FrCount-1);
    double* cosine=new double[N], *sine=new double[N];
    Sinusoid(&cosine[hWid], &sine[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
    for (int fr=0; fr<FrCount-1; fr++)
    {
      int ncentre=hWid+Offst*fr;
      if (fr==FrCount-2) Sinusoid(&cosine[ncentre], &sine[ncentre], 0, Wid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
      else Sinusoid(&cosine[ncentre], &sine[ncentre], 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
    }
    double err=0;
    for (int n=0; n<N; n++) {double tmp=cosine[n]-x[n-hWid]; err+=tmp*tmp; tmp=cosine[n]*cosine[n]+sine[n]*sine[n]-1; err+=tmp*tmp;}

  ph=0;
  for (int fr=0; fr<FrCount; fr++)
  {
    double* ldata=&x[fr*Offst-hWid];

    //store first half of demodulated frame to xs[0:hWid-1]
    if (fr==0)
    {
      Sinusoid(&refcos[hWid], &refsin[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
      for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
    }
    else
    {
      ph=0;
      Sinusoid(refcos, refsin, 0, hWid, f3[fr-1], f2[fr-1], f1[fr-1], f0[fr-1], ph);
      for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
    }

    //taking care of phase angles
    if (fr==FrCount-1) {double tmp=ph; ph=centralph; centralph=tmp;}
    else centralph=ph;

    double *lrefcos=&refcos[-hWid], *lrefsin=&refsin[-hWid];
    //store second half of demodulated frame to xs[hWid:Wid-1]
    if (fr==FrCount-1)
    {
      Sinusoid(lrefcos, lrefsin, hWid, Wid, f3[FrCount-2], f2[FrCount-2], f1[FrCount-2], f0[FrCount-2], ph);
      for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
    }
    else
    {
      Sinusoid(refcos, refsin, 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
      for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
    }

    CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc);
    double lf=fbuf[fr]*Wid, la, lp;
    LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
    if (la*2>abuf[fr])
      fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp+centralph;
  }
}//ReEstFreq_2

/**
  function ReEstFreqAmp: sinusoid reestimation by demodulating frequency and amplitude.

  In: x[Wid+Offst*(FrCount-1)]: waveform data
      FrCount, Wid, Offst: frame count, frame size and hop size
      fbuf[FrCount], abuf[FrCount], ns[FrCount]: initial frequency and amplitude estiamtes and their
        timing
      win[Wid]: window function for estimating demodulated sinusoid
      M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
      Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
        optional
      w[Wid/2], ps[Wid], xs[Wid], xc[Wid]: buffers
      fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
      aa[FrCount-1], ab[FrCount-1], ac[FrCount-1], ad[FrCount-1]: buffers
  Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles

  No return value.
*/
void ReEstFreqAmp(int FrCount, int Wid, int Offst, double* x, double* fbuf, double* abuf, double* pbuf, double* win, int M, double* c, double iH2, cdouble* w, cdouble* xc, cdouble* xs, double* ps, double* as, double* fa, double* fb, double* fc, double* fd, double* aa, double* ab, double* ac, double* ad, double* ns, int* Wids)
{
  int hWid=Wid/2;
  //reestimate using amplitude and frequency track
  CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
  CubicSpline(FrCount-1, aa, ab, ac, ad, ns, abuf, 0, 1);
  for (int fr=0; fr<FrCount; fr++)
  {
    if (fr==0)
    {
      double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0],
             lad=ad[0], lac=ac[0], lab=ab[0], laa=aa[0];
      for (int j=0; j<Wid; j++)
      {
        double lx=j-hWid;
        ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
      }
      for (int j=0; j<Wid; j++)
      {
        double lx=j-hWid;
        as[j]=lad+lx*(lac+lx*(lab+lx*laa));
      }
    }
    else if (fr==FrCount-1)
    {
      int lfr=FrCount-2;
      double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
      double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
      ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
      for (int j=1; j<Wid; j++)
      {
        ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
      }
      as[0]=ad[lfr];
      for (int j=0; j<Wid; j++)
      {
        as[j]=lad+j*(lac+j*(lab+j*laa));
      }
    }
    else
    {
      int lfr=fr-1;
      double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
      ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
      for (int j=0; j<hWid+1; j++)
      {
        ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
        as[j]=lad+j*(lac+j*(lab+j*laa));
      }
      lfr=fr;
      lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
      lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
      for (int j=1; j<hWid; j++)
      {
        ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
        as[j+hWid]=lad+j*(lac+j*(lab+j*laa));
      }
    }
    double *ldata=&x[fr*Offst];
    for (int j=0; j<Wid; j++)
    {
      double tmp;
      if ((fr==0 && j<hWid) || (fr==FrCount-1 && j>=hWid)) tmp=1;
      else if (as[hWid]>100*as[j]) tmp=100;
      else tmp=as[hWid]/as[j];
      tmp=tmp*ldata[j];
      xs[j].x=tmp*cos(-ps[j]);
      xs[j].y=tmp*sin(-ps[j]);
    }

    if (Wids)
    {
      int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
      SetTwiddleFactors(lWid, w);
      double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
      windowspec(wtHann, lWid, &lM, lc, &liH2);
      CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, log2(lWid), w, xc);
      delete[] lwin;
      double lf=fbuf[fr]*lWid, la, lp;
      LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
      if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
    }
    else
    {
      CFFTCW(xs, win, NULL, NULL, log2(Wid), w, xc);
      double lf=fbuf[fr]*Wid, la, lp;
      LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
      if (la*2>abuf[fr]) fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
    }
  }
}//ReEstFreqAmp

/**
  function Reestimate2: iterative demodulation method for sinusoid parameter reestimation.

  In: x[(FrCount-1)*Offst+Wid]: waveform data
      FrCount, Wid, Offst: frame count, frame size and hop size
      win[Wid]: window function
      M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
      Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
        optional
      maxiter: maximal number of iterates
      ae[FrCount], fe[FrCount], pe[FrCount]: initial amplitude, frequency and phase estimates
  Out: aret[FrCount], fret[FrCount], pret[FrCount]: reestimated amplitudes, frequencies and phase angles

  Returns the number of unused iterates left of the total of maxiter.
*/
int Reestimate2(int FrCount, int Wid, int Offst, double* win, int M, double* c, double iH2, double* x, double* ae, double* fe, double* pe, double* aret, double* fret, double *pret, int maxiter, int* Wids)
{
  AllocateFFTBuffer(Wid, fft, w, xc);
  double convep=1e-4, dif=0, lastdif=0; //convep is the hard-coded threshold that stops the iteration
  int iter=1, hWid=Wid/2;

  double *ns=new double[FrCount*12], *as=new double[Wid*5];
  double *fbuf=&ns[FrCount], *abuf=&ns[FrCount*2],
         *aa=&ns[FrCount*3], *ab=&ns[FrCount*4], *ac=&ns[FrCount*5], *ad=&ns[FrCount*6],
         *fa=&ns[FrCount*7], *fb=&ns[FrCount*8], *fc=&ns[FrCount*9], *fd=&ns[FrCount*10],
         *pbuf=&ns[FrCount*11];
  double *ps=&as[Wid];
  cdouble *xs=(cdouble*)&as[Wid*3];

  memcpy(fbuf, fe, sizeof(double)*FrCount);
  memcpy(abuf, ae, sizeof(double)*FrCount);
  memcpy(pbuf, pe, sizeof(double)*FrCount);
  for (int i=0; i<FrCount; i++)
  {
    ns[i]=hWid+i*Offst;
  }

  while (iter<=maxiter)
  {
    ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
    ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
    ReEstFreqAmp(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, as, fa, fb, fc, fd, aa, ab, ac, ad, ns, Wids);

    if (iter>1) lastdif=dif;
    dif=0;
    if (iter==1)
    {
      for (int fr=0; fr<FrCount; fr++)
      {
        if (fabs(abuf[fr])>fabs(ae[fr]))
          dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/abuf[fr]);
        else
          dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/ae[fr]);
      }
    }
    else
    {
      for (int fr=0; fr<FrCount; fr++)
      {
        if (fabs(abuf[fr])>fabs(aret[fr]))
          dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/abuf[fr]);
        else
          dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/aret[fr]);
      }
    }
    memcpy(fret, fbuf, sizeof(double)*FrCount);
    memcpy(aret, abuf, sizeof(double)*FrCount);
    dif/=FrCount;
    if (fabs(dif)<convep || (iter>1 && fabs(dif-lastdif)<convep*lastdif)) break;
    iter++;
  }

  memcpy(pret, pbuf, sizeof(double)*FrCount);

  delete[] ns;
  delete[] as;
  delete[] fft;

  return maxiter-iter;
}//Reestimate2

//---------------------------------------------------------------------------
/*
    Derivative method as proposed in DAFx09

    Further reading: Wen X. and M. Sandler, "Notes on model-based non-stationary sinusoid estimation methods
    using derivatives," in Proc. DAFx'09, Como, 2009.
*/

/**
  function Derivative: derivative method for estimating amplitude derivative, frequency, and frequency derivative given
  signal and its derivatives.

  In: x[Wid], dx[Wid], ddx[Wid]: waveform and its derivatives
      win[Wid]: window function
      f0: initial digital frequency estimate
  Out: f0: new estimate of digital frequency
       f1, a1: estimates of frequency and amplitude derivatives

  No return value.
*/
void Derivative(int Wid, double* win, cdouble* x, cdouble* dx, cdouble* ddx, double& f0, double* f1, double* a0, double* a1, double* ph)
{
  AllocateFFTBuffer(Wid, fft, W, X);
  CFFTCW(x, win, fft, NULL, log2(Wid), W, X);
  int m=f0*Wid, m0=m-10, m1=m+10, hWid=Wid/2;
  if (m0<0) m0=0; if (m1>hWid) m1=hWid;
  for (int n=m0; n<=m1; n++) if (fft[n]>fft[m]) m=n;
  cdouble Sw=0, S1w=0, S2w=0;
  for (int n=0; n<Wid; n++)
  {
    cdouble tmp=x[n]*win[n];
    Sw+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
    tmp=dx[n]*win[n];
    S1w+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
  }
  double omg0=(S1w/Sw).y;
  Sw=0, S1w=0;
  for (int n=0; n<Wid; n++)
  {
    cdouble tmp=x[n]*win[n];
    Sw+=tmp.rotate(-omg0*(n-hWid)/Wid);
    tmp=dx[n]*win[n];
    S1w+=tmp.rotate(-omg0*(n-hWid)/Wid);
    tmp=ddx[n]*win[n];
    S2w+=tmp.rotate(-omg0*(n-hWid)/Wid);
  }
  omg0=(S1w/Sw).y;
  double miu0=(S1w/Sw).x;
  double psi0=(S2w/Sw).y-2*miu0*omg0;

  f0=omg0/(2*M_PI);
  *f1=psi0/(2*M_PI);
  *a1=miu0;

  FreeFFTBuffer(fft);
}//Derivative

/**
  function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency omg,
  from x using window w and its derivatives.

  In: x[Wid]: waveform data
      w[K+1][Wid]: window functions and its derivatives up to order K
      omg: angular frequency
  Out: X[K+1]: windowed spectrum and its derivatives up to order K

  No return value. This function is for internal use.
*/
void Xkw(cdouble* X, int K, int Wid, double* x, double** w, double omg)
{
  int hWid=Wid/2;
  //calculate the first row
  memset(X, 0, sizeof(cdouble)*(K+1));
  for (int i=0; i<Wid; i++)
  {
    double n=i-hWid;
    double ph=omg*n;
    for (int k=0; k<=K; k++)
    {
      cdouble tmp=x[i]*w[k][i];
      X[k]+=tmp.rotate(-ph);
    }
  }
  //calculate the rest rows
  for (int k=1; k<=K; k++)
  {
    cdouble *thisX=&X[k], *lastX=&X[k-1];
			for (int kk=K-k; kk>=0; kk--) thisX[kk]=-lastX[kk+1]+cdouble(0, omg)*lastX[kk];
  }
}//Xkw

/**
  function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency
  omg, from x and its derivatives using window w.

  In: x[K+1][Wid]: waveform data and its derivatives up to order K.
      w[Wid]: window function
      omg: angular frequency
  Out: X[K+1]: windowed spectrum and its derivatives up to order K

  No return value. This function is for testing only.
*/
void Xkw(cdouble* X, int K, int Wid, double** x, double* w, double omg)
{
  int hWid=Wid/2;
  memset(X, 0, sizeof(cdouble)*(K+1));
  for (int i=0; i<Wid; i++)
  {
    double n=i-hWid;
    double ph=omg*n;
    for (int k=0; k<=K; k++)
    {
      cdouble tmp=x[k][i]*w[i];
      X[k]+=tmp.rotate(-ph);
    }
  }
}//Xkw

/**
  function Derivative: derivative method for estimating the model log(s)=h[M]'r[M], by discarding extra
  equations

  In: s[Wid]: waveform data
      win[][Wid]: window function and its derivatives
      h[M], dh[M]: pointers to basis functions and their derivatives
      harg: pointer argument to be used by calls to functions in h[] amd dh[].
      p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
      q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
      omg: initial angular frequency
  Out: r[M]: estimated coefficients to h[M].

  No return value.
*/
void Derivative(int M, double (**h)(double t, void*), double (**dh)(double t, void*), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg)
{
	int hWid=Wid/2, M1=M-1;
	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
	int Kc=ceil(Kr/2.0); //number of derivatives required

	//ind marks the 2*M1 real elements of an M1-array of complex unknowns with
	//  numerical indices (0-based) or -1 if it is not a real unknown variable
	//uind marks the Kr real unknowns with their positions in ind
	int *uind=new int[Kr], *ind=new int[2*M1];
	memset(ind, 0, sizeof(int)*2*M1);
	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
	{
		int p=0, up=0;
		while (p<2*M1)
		{
			if (ind[p]>=0)
			{
				uind[up]=p;
				ind[p]=up;
				up++;
			}
			p++;
		}
		if (up!=Kr) throw("");
	}

	cdouble* Skw=new cdouble[M];
	Xkw(Skw, Kc, Wid, s, win, omg);

	double* x=new double[Wid];
	cdouble** Allocate2(cdouble, M, Kc, Smkw);
	for (int m=1; m<M; m++)
	{
		for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
		Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
	}

	//allocate buffer for linear system A(pq)=b
	Alloc2(2*Kc+2, Kr, A); double** AA; double *bb, *pqpq;
	double *b=A[2*Kc], *pq=A[2*Kc+1];
	for (int k=0; k<Kr; k++) b[k]=((double*)(&Skw[1]))[k];
	//		*pq=(double*)(&r[1]);
	for (int k=0; k<Kc; k++) //looping through rows of A
	{
    //columns of A includes rows of Smkw corresponding to real unknowns
		for (int m=0; m<M1; m++)
		{
			int lind;
			if ((lind=ind[2*m])>=0) //the real part being unknown
			{
				A[2*k][lind]=Smkw[m+1][k].x;
				A[2*k+1][lind]=Smkw[m+1][k].y;
			}
			if ((lind=ind[2*m+1])>=0) //the imag part being unknown
			{
				A[2*k+1][lind]=Smkw[m+1][k].x;
				A[2*k][lind]=-Smkw[m+1][k].y;
			}
		}
	}
  
	bool dropeq=(2*Kc-1==Kr);
	if (dropeq)
	{
		Allocate2(double, Kr+2, Kr, AA);
		bb=AA[Kr], pqpq=AA[Kr+1];
		memcpy(AA[0], A[0], sizeof(double)*Kr*(Kr-1));
		memcpy(AA[Kr-1], A[Kr], sizeof(double)*Kr);
		memcpy(bb, b, sizeof(double)*(Kr-1));
		bb[Kr-1]=((double*)(&Skw[1]))[Kr];
	}

	double det;
	GECP(Kr, pq, A, b, &det);
	if (dropeq)
	{
		double det2;
		GECP(Kr, pqpq, AA, bb, &det2);
		if (fabs(det2)>fabs(det)) memcpy(pq, pqpq, sizeof(double)*Kr);
		DeAlloc2(AA);
	}
	memset(&r[1], 0, sizeof(double)*M1*2);
	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];

	//estiamte r0
	cdouble e0=0;
	for (int i=0; i<Wid; i++)
	{
		cdouble expo=0;
		double n=i-hWid;
		for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
		cdouble tmp=exp(expo)*win[0][i];
		e0+=tmp.rotate(-omg*n);
	}
	r[0]=log(Skw[0]/e0);

	delete[] x;
	delete[] Skw;
	delete[] uind;
	delete[] ind;
	DeAlloc2(Smkw);
	DeAlloc2(A);
}//Derivative*/

/**
  function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M], least-square
  implementation

  In: s[Wid]: waveform data
      win[][Wid]: window function and its derivatives
      h[M], dh[M]: pointers to basis functions and their derivatives
      harg: pointer argument to be used by calls to functions in h[] amd dh[].
      K: number of derivatives to take
      p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
      q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
      omg: initial angular frequency
  Out: r[M]: estimated coefficients to h[M].

  No return value.
*/
void DerivativeLS(int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0)
{
	int hWid=Wid/2, M1=M-1;
	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
	int Kc=ceil(Kr/2.0); //number of derivatives required
	if (Kc<K) Kc=K;

	int *uind=new int[Kr], *ind=new int[2*M1];
	memset(ind, 0, sizeof(int)*2*M1);
	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
	{int p=0, up=0; while (p<2*M1){if (ind[p]>=0){uind[up]=p;	ind[p]=up; up++;}	p++;} if (up!=Kr) throw("");}

	//allocate buffer for linear system A(pq)=b
	cdouble* Skw=new cdouble[Kc+1];
	double* x=new double[Wid];
	cdouble** Allocate2(cdouble, M, Kc, Smkw);

	Alloc2(2*Kc+2, 2*Kc, A);
	double *b=A[2*Kc], *pq=A[2*Kc+1];

	  Xkw(Skw, Kc, Wid, s, win, omg);
	  for (int m=1; m<M; m++)
	  {
		  for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
		  Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
	  }

	  for (int k=0; k<2*Kc; k++) b[k]=((double*)(&Skw[1]))[k];
		for (int k=0; k<Kc; k++)
		{
		  for (int m=0; m<M1; m++)
		  {
			  int lind;
				if ((lind=ind[2*m])>=0)
			  {
				  A[2*k][lind]=Smkw[m+1][k].x;
				  A[2*k+1][lind]=Smkw[m+1][k].y;
				}
			  if ((lind=ind[2*m+1])>=0)
			  {
				  A[2*k+1][lind]=Smkw[m+1][k].x;
				  A[2*k][lind]=-Smkw[m+1][k].y;
			  }
		  }
	  }

	if (2*Kc==Kr) GECP(Kr, pq, A, b);
	else LSLinear2(2*Kc, Kr, pq, A, b);

	memset(&r[1], 0, sizeof(double)*M1*2);
	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
	//estiamte r0
  if (r0)
  {
		cdouble e0=0;
  	for (int i=0; i<Wid; i++)
	  {
		  cdouble expo=0;
			double n=i-hWid;
  		for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
	  	cdouble tmp=exp(expo)*win[0][i];
			e0+=tmp.rotate(-omg*n);
		}
		r[0]=log(Skw[0]/e0);
  }
	delete[] x;
	delete[] Skw;
	delete[] uind;
	delete[] ind;
	DeAlloc2(Smkw);
	DeAlloc2(A);
}//DerivativeLS

/**
  function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M] using Fr
  measurement points a quarter of Wid apart from each other, implemented by least-square.

  In: s[Wid+(Fr-1)*Wid/4]: waveform data
      win[][Wid]: window function and its derivatives
      h[M], dh[M]: pointers to basis functions and their derivatives
      harg: pointer argument to be used by calls to functions in h[] amd dh[].
      Fr: number of measurement points
      K: number of derivatives to take at each measurement point
      p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
      q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
      omg: initial angular frequency
      r0: specifies if r[0] is to be computed.
  Out: r[M]: estimated coefficients to h[M].

  No return value.
*/
void DerivativeLS(int Fr, int K, int M, double (**h)(double t, void* harg), double (**dh)(double t, void* harg), cdouble* r, int p0s, int* p0, int q0s, int* q0, int Wid, double* s, double** win, double omg, void* harg, bool r0)
{
	int hWid=Wid/2, qWid=Wid/4, M1=M-1;
	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
	int Kc=ceil(Kr/2.0/Fr); //number of derivatives required
	if (Kc<K) Kc=K;

	int *uind=new int[Kr], *ind=new int[2*M1];
	memset(ind, 0, sizeof(int)*2*M1);
	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
	{int p=0, up=0;	while (p<2*M1){if (ind[p]>=0){uind[up]=p;	ind[p]=up; up++;}	p++;}}

	//allocate buffer for linear system A(pq)=b
	cdouble* Skw=new cdouble[Kc+1], Skw00;
	double* x=new double[Wid];
	cdouble** Allocate2(cdouble, M, Kc, Smkw);

	Alloc2(2*Fr*Kc, 2*Fr*Kc, A);
	double *pq=new double[2*Fr*Kc], *b=new double[2*Fr*Kc];

	for (int fr=0; fr<Fr; fr++)
	{
		int Offst=qWid*fr; double* ss=&s[Offst];

		Xkw(Skw, Kc, Wid, ss, win, omg); if (fr==0) Skw00=Skw[0];
		for (int m=1; m<M; m++)
		{
			for (int i=0; i<Wid; i++) x[i]=dh[m](i+Offst-hWid, harg)*ss[i];
			Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
		}

		for (int k=0; k<2*Kc; k++) b[2*fr*Kc+k]=((double*)(&Skw[1]))[k];
		for (int k=0; k<Kc; k++)
		{
			for (int m=0; m<M1; m++)
			{
				int lind;
				if ((lind=ind[2*m])>=0)
				{
					A[2*fr*Kc+2*k][lind]=Smkw[m+1][k].x;
					A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].y;
				}
				if ((lind=ind[2*m+1])>=0)
				{
					A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].x;
					A[2*fr*Kc+2*k][lind]=-Smkw[m+1][k].y;
				}
			}
		}
	}
	if (2*Fr*Kc==Kr) GECP(Kr, pq, A, b);
	else LSLinear2(2*Fr*Kc, Kr, pq, A, b);

	memset(&r[1], 0, sizeof(double)*M1*2);
	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
	//estiamte r0
	if (r0)
	{
		cdouble e0=0;
		for (int i=0; i<Wid; i++)
		{
			cdouble expo=0;
			double n=i-hWid;
			for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
			cdouble tmp=exp(expo)*win[0][i];
			e0+=tmp.rotate(-omg*n);
		}
		r[0]=log(Skw00/e0);
  }
	delete[] x;
	delete[] Skw;
	delete[] uind;
	delete[] ind;
	DeAlloc2(Smkw);
	DeAlloc2(A);
	delete[] pq; delete[] b;
}//DerivativeLS

//---------------------------------------------------------------------------
/*
    Abe-Smith sinusoid estimator 2005

    Further reading: M. Abe and J. O. Smith III, ¡°AM/FM rate estimation for time-varying sinusoidal
    modeling,¡± in Proc. ICASSP'05, Philadelphia, 2005.
*/

/**
  function RDFTW: windowed DTFT at frequency k bins

  In: data[Wid]: waveform data
      w[Wid]: window function
      k: frequency, in bins
  Out: Xr, Xi: real and imaginary parts of the DTFT of xw at frequency k bins

  No return value.
*/
void RDFTW(double& Xr, double& Xi, double k, int Wid, double* data, double* w)
{
	Xr=Xi=0;
	int hWid=Wid/2;
	double* lw=&w[Wid];
	for (int i=0; i<=Wid; i++)
	{
		double tmp;
		tmp=*data**lw;
		data++, lw--;
//*
    double ph=-2*M_PI*(i-hWid)*k/Wid;
    Xr+=tmp*cos(ph);
    Xi+=tmp*sin(ph); //*/
  }
}//RDFTW

/**
  function TFAS05: the Abe-Smith method 2005

  In: data[Wid]: waveform data
      w[Wid]: window function
      res: resolution of frequency for QIFFT
  Out: f, a, ph: frequency, amplitude and phase angle estimates
       aesp, fslope: estimates of log amplitude and frequency derivatives

  No return value.
*/
void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
{
	double fi=floor(f*Wid+0.5); //frequency (int) in bins
	double xr0, xi0, xr_1, xi_1, xr1, xi1;
	RDFTW(xr0, xi0, fi, Wid, data, w);
	RDFTW(xr_1, xi_1, fi-res, Wid, data, w);
	RDFTW(xr1, xi1, fi+res, Wid, data, w);
	double winnorm=0; for (int i=0; i<=Wid; i++) winnorm+=w[i];
	double y0=log(sqrt(xr0*xr0+xi0*xi0)/winnorm),
         y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm),
         y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
  double df=0;
//*
  if (y0<y1)
  {
    double newfi=fi+res;
    while (y0<y1)
    {
      y_1=y0, xr_1=xr0, xi_1=xi0;
      y0=y1, xr0=xr1, xi0=xi1;
      newfi+=res;
      RDFTW(xr1, xi1, newfi, Wid, data, w);
      y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
      fi+=res;
    }
  }
  else if(y0<y_1)
  {
    double newfi=fi-res;
    while (y0<y_1)
    {
      y1=y0, xr1=xr0, xi1=xi0;
      y0=y_1, xr0=xr_1, xi0=xi_1;
      newfi-=res;
      RDFTW(xr_1, xi_1, newfi, Wid, data, w);
      y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm);
      fi-=res;
    }
  }    //*/

	double a2=(y1+y_1)*0.5-y0, a1=(y1-y_1)*0.5, a0=y0;
  df=-a1*0.5/a2;
  f=fi+df*res; //in bins
  double y=a0-0.25*a1*a1/a2;
  a=exp(y);
	double ph0=(xi0==0 && xr0==0)?0:atan2(xi0, xr0),
				 ph_1=(xi_1==0 && xr_1==0)?0:atan2(xi_1, xr_1),
				 ph1=(xi1==0 && xr1==0)?0:atan2(xi1, xr1);
  if (fabs(ph_1-ph0)>M_PI)
  {
    if (ph_1-ph0>0) ph_1-=M_PI*2;
    else ph_1+=M_PI*2;
  }
  if (fabs(ph1-ph0)>M_PI)
  {
    if (ph1-ph0>0) ph1-=M_PI*2;
    else ph1+=M_PI*2;
  } 
  double b2=(ph1+ph_1)*0.5-ph0, b1=(ph1-ph_1)*0.5, b0=ph0;
  ph=b0+b1*(df+b2*df);
  //now we have the QI estimates
  double uff=2*a2, vf=b1+2*b2*df, vff=2*b2;
	double dfdp=Wid/(2*M_PI*res);
	double upp=uff*dfdp*dfdp, vp=vf*dfdp, vpp=vff*dfdp*dfdp;
	double p=-upp*0.5/(upp*upp+vpp*vpp);
	double alf=-2*p*vp, beta=p*vpp/upp;
	//*direct method
	double beta_p=beta/p;
	double feses=f-alf*beta/p /(2*M_PI)*Wid,
				 yeses=y-alf*alf*0.25/p+0.25*log(1+beta_p*beta_p),
				 pheses=ph+alf*alf*beta*0.25/p-0.5*atan(beta_p); //*/
	/*adapted method
	double zt[]={0, 0.995354, 0.169257, 1.393056, 0.442406, -0.717980, -0.251620, 0.177511, 0.158120, -0.503299};
	double delt=res/Wid; double delt0=df*delt;
	beta=zt[3]*beta+zt[4]*delt0*alf;
	alf=(zt[1]+zt[2]*delt*delt)*alf;
	double beta_p=beta/p;
	double feses=f+zt[5]*alf*beta/p /(2*M_PI)*Wid,
				 yeses=y+zt[6]*alf*alf/p+zt[7]*log(1+beta_p*beta_p),
				 pheses=ph+zt[8]*alf*alf*beta/p+zt[9]*atan(beta_p); //*/
	f=feses/Wid, a=exp(yeses), ph=pheses, fslope=2*beta/2/M_PI, aesp=alf;
}//TFAS05

/**
  function TFAS05_enh: the Abe-Smith method 2005 enhanced by LSE amplitude and phase estimation

  In: data[Wid]: waveform data
      w[Wid]: window function
      res: resolution of frequency for QIFFT
  Out: f, a, ph: frequency, amplitude and phase angle estimates
       aesp, fslope: estimates of log amplitude and frequency derivatives

  No return value.
*/
void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
{
	TFAS05(f, t, a, ph, aesp, fslope, Wid, data, w, res);
	double xr=0, xi=0, p, win2=0;
	for (int n=0; n<=Wid; n++)
	{
		double ni=n-Wid/2, tmp=data[n]*w[n]*w[n];//*exp(-aesp*(n-Wid/2));  if (IsInfinite(tmp)) continue;
		p=-2*M_PI*(f+0.5*fslope*ni)*ni;
		xr+=tmp*cos(p);
		xi+=tmp*sin(p);
		win2+=w[n]*w[n];
	}
	a=sqrt(xr*xr+xi*xi)/win2;
	ph=(xr==0 && xi==0)?0:atan2(xi, xr);
}//TFAS05_enh
//version without returning aesp and fslope
void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res)
{
	double aesp, fslope;
	TFAS05_enh(f, t, a, ph, aesp, fslope, Wid, data, w, res);
}//TFAS05_enh

//---------------------------------------------------------------------------
/**
  function DerivativeLSv_AmpPh: estimate the constant-term in the local derivative method. This is used
  by the local derivative algorithm, whose implementation is found in the header file as templates.

  In: sv0: inner product <s, v0>, where s is the sinusoid being estimated.
      integr_h[M][Wid]: M vectors containing samples of the integral of basis functions h[M].
      v0[M]: a test function
      lmd[M]: coefficients to h[M]

  Returns coefficient of integr_h[0]=1.
*/
cdouble DerivativeLSv_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, cdouble* v0, cdouble sv0)
{
  cdouble e0=0;
  for (int n=0; n<Wid; n++)
  {
    cdouble expo=0;
    for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n];
    e0+=exp(expo)**v0[n];
  }
  return log(sv0/e0);
}//DerivativeLSv_AmpPh

//---------------------------------------------------------------------------
/*
    Piecewise derivative algorithm

    Further reading: Wen X. and M. Sandler, "Spline exponential approximation of time-varying
    sinusoids," under review.
*/

/**
  function setv: computes I test functions v[I] by modulation u[I] to frequency f

  In: u[I+1][Wid], du[I+1][Wid]: base-band test functions and their derivatives
      f: carrier frequency
  Out: v[I][Wid], dv[I][Wid]: test functions and their derivatives

  No return value.
*/
void setv(int I, int Wid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
{
  double fbin=floor(f*Wid+0.5)/Wid;
  double omg=fbin*2*M_PI;
  cdouble jomg=cdouble(0, omg);
  for (int c=0; c<Wid; c++)
  {
    double t=c;
    cdouble rot=polar(1.0, omg*t);
    for (int i=0; i<I-1; i++) v[i][c]=u[i][c]*rot;
    for (int i=0; i<I-1; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
    //Here it is assumed that elements of u[] are modulated at 0, 1, -1, 2, -2, 3, -3, 4, ...;
    //if f is under fbin then the closest ones are in order 0, -1, 1, -2, 3, -3, 3, .... This
    //makes a difference to the whole of v[] only if I is even.
    if (f>=fbin || I%2==1){v[I-1][c]=u[I-1][c]*rot; dv[I-1][c]=du[I-1][c]*rot+jomg*v[I-1][c];}
    else{v[I-1][c]=u[I][c]*rot; dv[I-1][c]=du[I][c]*rot+jomg*v[I-1][c];}
  }
}//setv

/**
  function setvhalf: computes I half-size test functions v[I] by modulation u[I] to frequency f.

  In: u[I][hWid*2], du[I][Wid*2]: base-band test functions and their derivatives
      f: carrier frequency
  Out: v[I][hWid], dv[hWid]: half-size test functions and their derivatives

  No return value.
*/void setvhalf(int I, int hWid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
{
  double fbin=floor(f*hWid)/hWid;
  double omg=fbin*2*M_PI;
  cdouble jomg=cdouble(0, omg);
  for (int c=0; c<hWid; c++)
  {
    double t=c;
    cdouble rot=polar(1.0, omg*t);
    for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
    for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
  }
}//setvhalf

//#define ERROR_CHECK

/**
  function DerivativePiecewise: Piecewise derivative algorithm. In this implementation of the piecewise
  method the test functions v are constructed from I "basic" (single-frame) test functions, each
  covering the same period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I
  test functions are used.

  In: s[LT+1]: waveform data
      ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
      L, T: number and length of pieces.
      N: number of independent coefficients
      h[M][T]: piecewise basis functions
      A[L][M][N]: L matrices that map independent coefficients onto component coefficients over the L pieces
      u[I][2T}, du[I][2T]: base-band test functions
      f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
      endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: aita[N]: independent coefficients

  No return value.
*/
void DerivativePiecewise(int N, cdouble* aita, int L, double* f, int T, cdouble* s, double*** A, int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds)
{
  MList* mlist=new MList;
  int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
  cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
  cdouble** Allocate2(cdouble, I, T*2, v);
  cdouble** Allocate2(cdouble, I, T*2, dv);
  //compute <sr, v>
  cdouble*** Allocate3L(cdouble, L_1, I, N, srv, mlist);
  cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
  cdouble** Allocate2L(cdouble, I, M, shv2, mlist);

#ifdef ERROR_CHECK
  cdouble dsv1[128], dsv2[128];
#endif
  for (int l=0; l<L-1; l++)
  {
    //v from u given f[l]
    double fbin=floor(f[l+1]*T*2)/(T*2.0);
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T*2; c++)
    {
      double t=c-T;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
      for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
    for (int i=0; i<I; i++)
      for (int m=0; m<M; m++)
        shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
    //memset(srv[l][0], 0, sizeof(cdouble)*I*N);
    MultiplyXY(I, M, N, srv[l], shv1, A[l]);
    MultiAddXY(I, M, N, srv[l], shv2, A[l+1]);

#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'>
    if (ds)
    {
      cdouble* lds=&ds[l*T];
      for (int i=0; i<I && l*I+1<36; i++)
      {
        cdouble lsv=Inner(2*T, lds, v[i]); //compute <s', v[i]>
        //cdouble* ls=&s[l*T];
        //cdouble lsv2=Inner(2*T, ls, dv[i]);
        dsv1[l*I+i]=lsv-sv[l][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
      }

      //error check: srv[l]*pq=<s',v>
      for (int i=0; i<I && l*I+i<36; i++)
      {
        cdouble lsv=0;
        for (int n=0; n<N; n++) lsv+=srv[l][i][n]*aita[n];
        dsv2[l*I+i]=lsv-sv[l][i]-dsv1[l*I+i];
      }
    }
#endif
  }
  L_1=L-1;
  if (endmode==1 || endmode==3)
  {
    //v from u given f[l]
    int hT=T/2;
    double fbin=floor((f[0]+f[1])*hT)/T;
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T; c++)
    {
      double t=c-hT;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
      for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
    MultiplyXY(I, M, N, srv[L_1], shv1, A[0]);
#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'>
    if (ds)
    {
      cdouble* lds=&ds[0];
      for (int i=0; i<I && L_1*I+1<36; i++)
      {
        cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
        //cdouble* ls=&s[l*T];
        //cdouble lsv2=Inner(2*T, ls, dv[i]);
        dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
      }

      //error check: srv[l]*pq=<s',v>
      for (int i=0; i<I && L_1*I+i<36; i++)
      {
        cdouble lsv=0;
        for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
        dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
      }
    }
#endif
    L_1++;
  }
  if (endmode==2 || endmode==3)
  {
    //v from u given f[l]
    int hT=T/2;
    double fbin=floor((f[L-1]+f[L])*hT)/T;
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T; c++)
    {
      double t=c-hT;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
      for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
    MultiplyXY(I, M, N, srv[L_1], shv1, A[L-1]);
#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'>
    if (ds)
    {
      cdouble* lds=&ds[(L-1)*T];
      for (int i=0; i<I && L_1*I+1<36; i++)
      {
        cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
        //cdouble* ls=&s[l*T];
        //cdouble lsv2=Inner(2*T, ls, dv[i]);
        dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
      }

      //error check: srv[l]*pq=<s',v>
      for (int i=0; i<I && L_1*I+i<36; i++)
      {
        cdouble lsv=0;
        for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
        dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
      }
    }
#endif
      L_1++;
  }

  if (L_1*2*I==2*N) GECP(N, aita, srv[0], sv[0]);
  else LSLinear(L_1*I, N, aita, srv[0], sv[0]);

  delete mlist;
}//DerivativePiecewise

/**
  function DerivativePiecewise2: Piecewise derivative algorithm in which the real and imaginary parts of
  the exponent are modelled separately. In this implementation of the piecewise method the test
  functions v are constructed from I "basic" (single-frame) test functions, each covering the same
  period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
  used.

  In: s[LT+1]: waveform data
      ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
      L, T: number and length of pieces.
      N: number of independent coefficients
      h[M][T]: piecewise basis functions
      A[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
      B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
      u[I][2T}, du[I][2T]: base-band test functions
      f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
      endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: p[Np], q[Nq]: independent coefficients

  No return value.
*/
void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B,
  int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds)
{
  MList* mlist=new MList;
  int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
  cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
  cdouble** Allocate2(cdouble, I, T*2, v);
  cdouble** Allocate2(cdouble, I, T*2, dv);
  //compute <sr, v>
  cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
  cdouble*** srbv;
  if (Np==Nq && B==A) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
  cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
  cdouble** Allocate2L(cdouble, I, M, shv2, mlist);

  for (int l=0; l<L-1; l++)
  {
    //v from u given f[l]
    double fbin=floor(f[l+1]*T*2)/(T*2.0);
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T*2; c++)
    {
      double t=c-T;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
      for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
    for (int i=0; i<I; i++)
      for (int m=0; m<M; m++)
        shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
    memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[l], shv1, A[l]);
    MultiAddXY(I, M, Np, srav[l], shv2, A[l+1]);
    if (srbv!=srav) //so that either B!=A or Np!=Nq
    {
      //memset(srbv[l][0], 0, sizeof(cdouble)*I*Nq);
      MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
      MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
    }
  }
  L_1=L-1;
  if (endmode==1 || endmode==3)
  {
    //v from u given f[l]
    int hT=T/2;
    double fbin=floor((f[0]+f[1])*hT)/T;
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T; c++)
    {
      double t=c-hT;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
      for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[L_1], shv1, A[0]);
    if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
    L_1++;
  }
  if (endmode==2 || endmode==3)
  {
    //v from u given f[l]
    int hT=T/2;
    double fbin=floor((f[L-1]+f[L])*hT)/T;
    double omg=fbin*2*M_PI;
    cdouble jomg=cdouble(0, omg);
    for (int c=0; c<T; c++)
    {
      double t=c-hT;
      cdouble rot=polar(1.0, omg*t);
      for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
      for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
    }

    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[L_1], shv1, A[L-1]);
    if (srbv!=srav)
    {
      //memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);
      MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
    }
    L_1++;
  }

  //real implementation of <sr,v>aita=<s',v>
  double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
  for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
  {
    int li=l*I+i, li_H=li+L_1*I;
    for (int n=0; n<Np; n++)
    {
      AM[li][n]=srav[l][i][n].x;
      AM[li_H][n]=srav[l][i][n].y;
    }
    for (int n=0; n<Nq; n++)
    {
      AM[li][Np+n]=-srbv[l][i][n].y;
      AM[li_H][Np+n]=srbv[l][i][n].x;
    }
  }
  //least-square solution of (srv)(aita)=(sv)
  double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
  double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y;

  if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
  else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);

  memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);

  delete mlist;
}//DerivativePiecewise2

/*
  Error check: test that ds[LT] equals s[LT] times reconstructed R'. Notice that DA is D time A where D
  is a pre-emphasis because p[Np] applies to log amplitude rather than its derivative.
*/
double testds_pqA(int Np, double* p, int Nq, double* q, int L, int T, cdouble* s, cdouble* ds, int M, double** h, double** dh, double*** DA, double*** B, cdouble* errds=0)
{
  double err=0, ene=0, *lamdax=new double[M*2], *lamday=&lamdax[M];
  for (int l=0; l<L; l++)
  {
    MultiplyXy(M, Np, lamdax, DA[l], p);
    MultiplyXy(M, Nq, lamday, B[l], q);
    for (int t=0; t<T; t++)
    {
      double drtx=0; for (int m=0; m<M; m++) drtx+=lamdax[m]*h[m][t];
      double drty=0; for (int m=0; m<M; m++) drty+=lamday[m]*h[m][t];
      cdouble drt=cdouble(drtx, drty);
      cdouble eds=ds[l*T+t]-s[l*T+t]*drt;
      err+=~eds; ene+=~ds[l*T+t];
      if (errds) errds[l*T+t]=eds;
    }
  }
  delete[] lamdax;
  return err/ene;
}//testds_pqA

/*
  Error check: dsv1[I] tests that <s', v[I]> equals -<s, v[I]'>, dsv2[I] tests that <sr, v[I]>*pq=
  <s',v[I]>
*/
void testdsv(cdouble* dsv1, cdouble* dsv2, int Np, double* p, int Nq, double* q, int TT, cdouble* dsl, int I, cdouble** vl, cdouble* svl, cdouble** sravl, cdouble** srbvl)
{
  for (int i=0; i<I; i++)
  {
    cdouble lsv=Inner(TT, dsl, vl[i]); //compute <s', v[i]>
    //cdouble* ls=&s[l*T];
    dsv1[i]=lsv-svl[i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
    //sv[l][i]=lsv;
  }
  //error check: srv[l]*pq=<s',v>
  for (int i=0; i<I; i++)
  {
    cdouble lsv=0;
    for (int n=0; n<Np; n++) lsv+=sravl[i][n]*p[n];
    for (int n=0; n<Nq; n++) lsv+=srbvl[i][n]*cdouble(0, q[n]);
    dsv2[i]=lsv-svl[i]-dsv1[i];
  }
}//testdsv

/*
  Error check: tests A[MN]x[N1]=b[N1], returns square error
*/
double testlinearsystem(int M, int N, double** A, double* x, double* b)
{
  double err=0;
  for (int m=0; m<M; m++)
  {
    double errli=Inner(N, A[m], x)-b[m];
    err+=errli*errli;
  }
  return err;
}//testlinearsystem

/*
  Error check: test the total square norm of <s, v>
*/
double testsv(int L, double* f, int T, cdouble* s, int I, cdouble** u, cdouble** du, int endmode)
{
  cdouble** Allocate2(cdouble, I, T*2, v);
  cdouble** Allocate2(cdouble, I, T*2, dv);
  double ene=0;
  for (int l=0; l<L-1; l++)
  {
    //v from u given f[l]
    setv(I, T*2, v, dv, f[l+1], u, du);
    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[l*T];
    for (int i=0; i<I; i++)
    {
      cdouble d=Inner(2*T, ls, v[i]);
      ene+=~d;
    }
  }
  if (endmode==1 || endmode==3)
  {
    //v from u given f[l]
    setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
    cdouble* ls=&s[0];
    for (int i=0; i<I; i++)

      ene+=~Inner(T, ls, v[i]);
  }
  if (endmode==2 || endmode==3)
  {
    //v from u given f[l]
    setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
    cdouble* ls=&s[(L-1)*T];
    for (int i=0; i<I; i++)
      ene+=~Inner(T, ls, v[i]);
  }
  DeAlloc2(v); DeAlloc2(dv);
  return ene;
}//testsv

/**
  function DerivativePiecewise3: Piecewise derivative algorithm in which the log amplitude and frequeny
  are modeled separately as piecewise functions. In this implementation of the piecewise method the test
  functions v are constructed from I "basic" (single-frame) test functions, each covering the same
  period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
  used.

  In: s[LT+1]: waveform data
      ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
      L, T: number and length of pieces.
      N: number of independent coefficients
      h[M][T]: piecewise basis functions
      dh[M][T]: derivative of h[M][T], used only if ERROR_CHECK is defined.
      DA[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
      B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
      u[I][2T}, du[I][2T]: base-band test functions
      f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
      endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: p[Np], q[Nq]: independent coefficients

  No return value.
*/
void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B,
  int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds, double** dh)
{
  MList* mlist=new MList;
  int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
  cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
  cdouble** Allocate2L(cdouble, I, T*2, v, mlist);
  cdouble** Allocate2L(cdouble, I, T*2, dv, mlist);
  //compute <sr, v>
  cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
  cdouble*** srbv;
  if (Np==Nq && B==DA) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
  cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
  cdouble** Allocate2L(cdouble, I, M, shv2, mlist);

#ifdef ERROR_CHECK
  cdouble dsv1in[128], dsv2in[128];
#endif

  for (int l=0; l<L-1; l++)
  {
    //v from u given f[l]
    setv(I, T*2, v, dv, f[l+1], u, du);
    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);

    //compute <sr, v> over the lth frame
    cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
    for (int i=0; i<I; i++)
      for (int m=0; m<M; m++)
        shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
    memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[l], shv1, DA[l]);
    MultiAddXY(I, M, Np, srav[l], shv2, DA[l+1]);
    if (srbv!=srav) //so that either B!=A or Np!=Nq
    {
      MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
      MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
    }
#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
    if (ds) testdsv(&dsv1in[l*I], &dsv2in[l*I], Np, p, Nq, q, T*2, &ds[l*T], I, v, sv[l], srav[l], srbv[l]);
#endif
  }
  L_1=L-1;
  if (endmode==1 || endmode==3)
  {
    //v from u given f[l]
    setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[L_1], shv1, DA[0]);
    if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
    if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[0], I, v, sv[L_1], srav[L_1], srbv[L_1]);
#endif
    L_1++;
  }
  if (endmode==2 || endmode==3)
  {
    //v from u given f[l]
    setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
    //compute -<s, v'> over the lth frame
    cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
    //compute <sr, v> over the lth frame
    for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
    memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
    MultiplyXY(I, M, Np, srav[L_1], shv1, DA[L-1]);
    if (srbv!=srav)	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
#ifdef ERROR_CHECK
    //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
    if (ds) testdsv(&dsv1in[L_1*I], &dsv2in[L_1*I], Np, p, Nq, q, T, &ds[(L-1)*T], I, v, sv[L_1], srav[L_1], srbv[L_1]);
#endif
    L_1++;
  }

  //real implementation of <sr,v>aita=<s',v>
  double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
  for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
  {
    int li=l*I+i, li_H=li+L_1*I;
    for (int n=0; n<Np; n++)
    {
      AM[li][n]=srav[l][i][n].x;
      AM[li_H][n]=srav[l][i][n].y;
    }
    for (int n=0; n<Nq; n++)
    {
      AM[li][Np+n]=-srbv[l][i][n].y;
      AM[li_H][Np+n]=srbv[l][i][n].x;
    }
  }
  //least-square solution of (srv)(aita)=(sv)
  double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
  double* b=new double[2*L_1*I]; for (int i=0; i<L_1*I; i++) b[i]=sv[0][i].x, b[i+L_1*I]=sv[0][i].y; mlist->Add(b, 1);
#ifdef ERROR_CHECK
    //tests that AM is invariant to a constant shift of p
    double errAM=0, errAM2=0, err1, err2;
    for (int l=0; l<L_1*I; l++){double errli=0; for (int n=0; n<Np; n++) errli+=AM[l][n]; errAM+=errli*errli; errli=0; for (int n=0; n<Np; n+=2) errli+=AM[l][n]; errAM2+=errli*errli;}
    //test square error of the input pq
    if (ds)
    {
      memcpy(pq, p, sizeof(double)*Np); memcpy(&pq[Np], q, sizeof(double)*Nq);
      err1=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
    }
    //test error of s'-sR where R is synthesized from the input pq
    double errdsin, errdsvin; cdouble* edsin;
    if (ds && dh)
    {
      edsin=new cdouble[L*T]; mlist->Add(edsin, 1);
      errdsin=testds_pqA(Np, p, Nq, q, L, T, s, ds, M, h, dh, DA, B, edsin);
      errdsvin=testsv(L, f, T, edsin, I, u, du, endmode);
    }
#endif
  Alloc2L(L_1*I*2, Np+Nq-1, Am, mlist);
  for (int l=0; l<L_1*I*2; l++) memcpy(Am[l], &AM[l][1], sizeof(double)*(Np+Nq-1));
  pq[0]=0;
  //if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
  //else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);
  if (L_1*2*I==Np+Nq-1) GECP(Np+Nq-1, &pq[1], Am, b);
  else LSLinear(2*L_1*I, Np+Nq-1, &pq[1], Am, b);
#ifdef ERROR_CHECK
    //test square error of output pq
    if (ds) err2=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
    //test error of s'-sR of the output pq
    double errdsout, errdsvout; cdouble* edsout;
    if (ds && dh)
    {
      edsout=new cdouble[L*T]; mlist->Add(edsout, 1);
      errdsout=testds_pqA(Np, pq, Nq, &pq[Np], L, T, s, ds, M, h, dh, DA, B, edsout);
      errdsvout=testsv(L, f, T, edsout, I, u, du, endmode);
    }
#endif
  memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);

  delete mlist;
}//DerivativePiecewise3

//initialization routines for the piecewise derivative method

/**
  function seth: set h[M] to a series of power functions.

  In: M, T.
  Out: h[M][T], where h[m] is power function of order m.

  No return value. h is allocated anew and must be freed by caller.
*/
void seth(int M, int T, double**& h, MList* mlist)
{
 if (M<=0){h=0; return;}
 Allocate2L(double, M, T, h, mlist);
 double* hm=h[0]; for (int t=0; t<T; t++) hm[t]=1;
 for (int m=1; m<M; m++)
 {
   hm=h[m]; for (int t=0; t<T; t++) hm[t]=pow(t*1.0, m);
 }
}//seth

/**
  function setdh: set dh[M] to the derivative of a series of power functions.

  In: M, T.
  Out: dh[M][T], where dh[m] is derivative of the power function of order m.

  No return value. dh is allocated anew and must be freed by caller.
*/
void setdh(int M, int T, double**& dh, MList* mlist)
{
 if (M<=0){dh=0; return;}
 Allocate2L(double, M, T, dh, mlist);
 double* dhm=dh[0]; memset(dhm, 0, sizeof(double)*T);
 if (M>1){dhm=dh[1]; for (int t=0; t<T; t++) dhm[t]=1;}
 for (int m=2; m<M; m++)
 {
   dhm=dh[m]; for (int t=0; t<T; t++) dhm[t]=m*pow(t*1.0, m-1);
 }
}//setdh

/**
  function setdih: set dih[M] to the difference of the integral of a series of power functions.

  In: M, I
  Out: dih[M][I], where the accumulation of dih[m] is the integral of the power function of order m.

  No return value. dih is allocated anew and must be freed by caller.
*/
void setdih(int M, int T, double**& dih, MList* mlist)
{
  if (M<=0){dih=0; return;}
  Allocate2L(double, M, T, dih, mlist);
  double* dihm=dih[0]; for (int t=0; t<T; t++) dihm[t]=1;
  for (int m=1; m<M; m++)
  {
    dihm=dih[m]; for (int t=0; t<T; t++)	dihm[t]=(pow(t+1.0, m+1)-pow(t*1.0, m+1))/(m+1);
  }
}//setdih

/**
  function sshLinear: sets M and h[M] for the linear spline model

  In: T
  Out: M=2, h[2][T] filled out for linear spline model.

  No return value. h is allocated anew and must be freed by caller.
*/
void sshLinear(int T, int& M, double** &h, MList* mlist)
{
  M=2; Allocate2L(double, M, T, h, mlist);
  for (int t=0; t<T; t++) h[0][t]=1, h[1][t]=t;
}//sshLinear

/**
  function sdihLinear: sets dih[M] for the linear spline model. For testing only.

  In: T
  Out: dih[2][T] filled out for linear spline model.

  No return value. dih is allocated anew and must be freed by caller.
*/
void sdihLinear(int T, double**& dih, MList* mlist)
{
  Allocate2L(double, 2, T, dih, mlist);
  for (int t=0; t<T; t++)	dih[0][t]=1, dih[1][t]=t+0.5;
}//sdihLinear

/**
  function sshCubic: sets M and h[M] for cubic spline models.

  In: T
  Out: M=4 and h[M] filled out for cubic spline models, including cubic and cubic-Hermite.

  No return value. h is allocated anew and must be freed by caller.
*/
void sshCubic(int T, int& M, double** &h, MList* mlist)
{
  M=4; Allocate2L(double, M, T, h, mlist);
  for (int t=0; t<T; t++) h[3][t]=t*t*t, h[2][t]=t*t, h[1][t]=t, h[0][t]=1;
}//sshCubic

/**
  function sdihCubic: sets dih[M] for cubic spline models.

  In: T
  Out: dih[4] filled out for cubic spline models.

  No return value. dih is allocated anew and must be freed by caller.
*/
void sdihCubic(int T, double** &dih, MList* mlist)
{
  Allocate2L(double, 4, T, dih, mlist);
  for (int t=0; t<T; t++)
  {
    dih[3][t]=t*(t*(t+1.5)+1)+0.25, dih[2][t]=t*(t+1)+1.0/3, dih[1][t]=t+0.5, dih[0][t]=1;
  }
}//sdihCubic*/

/**
  function ssALinearSpline: sets N and A[L] for the linear spline model

  In: L, M, T
  Out: N=L+1, A[L][M][N] filled out for the linear spline model

  No return value. A is created anew and bust be freed by caller.
*/
void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
{
  N=L+1;
  Allocate3L(double, L, M, N, A, mlist);
  memset(A[0][0], 0, sizeof(double)*L*M*N);
  double iT=1.0/T; for (int l=0; l<L; l++) A[l][0][l]=1, A[l][1][l]=-iT, A[l][1][l+1]=iT;
}//ssALinearSpline

/**
  function ssLinearSpline: sets M, N, h and A for the linear spline model

  In: L, M, T
  Out: N and h[][] and A[][][] filled out for the linear spline model

  No reutrn value. A and h are created anew and bust be freed by caller.
*/
void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode)
{
  seth(M, T, h, mlist);
  ssALinearSpline(L, T, M, N, A, mlist);
}//ssLinearSpline

/**
  function ssACubicHermite: sets N and A[L] for cubic Hermite spline model

  In: L, M, T
  Out: N=2(L+1), A[L][M][N] filled out for the cubic Hermite spline

  No return value. A is created anew and must be freed by caller.
*/
void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
{
  N=2*(L+1);
  Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
  double iT=1.0/T, iT2=iT*iT, iT3=iT2*iT;
  for (int l=0; l<L; l++)
  {
    A[l][3][2*l]=2*iT3; A[l][3][2*l+2]=-2*iT3; A[l][3][2*l+1]=A[l][3][2*l+3]=iT2;
    A[l][2][2*l]=-3*iT2; A[l][2][2*l+1]=-2*iT; A[l][2][2*l+2]=3*iT2; A[l][2][2*l+3]=-iT;
    A[l][1][2*l+1]=1;
    A[l][0][2*l]=1;
  }
}//ssACubicHermite

/**
  function ssLinearSpline: sets M, N, h and A for the cubic Hermite spline model

  In: L, M, T
  Out: N and h[][] and A[][][] filled out for the cubic Hermite spline model

  No reutrn value. A and h are created anew and bust be freed by caller.
*/
void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
{
  seth(M, T, h, mlist);
  ssACubicHermite(L, T, M, N, A, mlist);
}//ssCubicHermite

/**
  function ssACubicSpline: sets N and A[L] for cubic spline model

  In: L, M, T
      mode: boundary mode of cubic spline, 0=natural, 1=quadratic run-out, 2=cubic run-out
  Out: N=2(L+1), A[L][M][N] filled out for the cubic spline

  No return value. A is created anew and must be freed by caller.
*/
void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
{
  N=L+1;
  Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
  Alloc2(L+1, L+1, ML); memset(ML[0], 0, sizeof(double)*(L+1)*(L+1));
  Alloc2(L+1, L+1, MR); memset(MR[0], 0, sizeof(double)*(L+1)*(L+1));
  //fill in ML and MR. The only difference between various cubic splines are ML.
  double _6iT2=6.0/(T*T);
  ML[0][0]=ML[L][L]=1;
  for (int l=1; l<L; l++) ML[l][l-1]=ML[l][l+1]=1, ML[l][l]=4,
    MR[l][l-1]=MR[l][l+1]=_6iT2, MR[l][l]=-2*_6iT2;
  if (mode==0){} //no more coefficients are needed for natural cubic spline
  else if (mode==1) ML[0][1]=ML[L][L-1]=-1; //setting for quadratic run-out
  else if (mode==2) ML[0][1]=ML[L][L-1]=-2, ML[0][2]=ML[L][L-2]=1; //setting for cubic run-out
  GICP(L+1, ML);
  double** MM=MultiplyXY(L+1, ML, ML, MR);
  double iT=1.0/T;
  Alloc2(4, 2, M42); M42[3][0]=-1.0/6/T, M42[3][1]=1.0/6/T, M42[2][0]=0.5, M42[2][1]=M42[0][0]=M42[0][1]=0, M42[1][0]=-T/3.0, M42[1][1]=-T/6.0;
  for (int l=0; l<L; l++)
  {
    MultiplyXY(4, 2, N, A[l], M42, &MM[l]);
    A[l][1][l]-=iT; A[l][1][l+1]+=iT; A[l][0][l]+=1;
  }
  DeAlloc2(ML); DeAlloc2(MR); DeAlloc2(M42);
}//ssACubicSpline

/**
  function ssLinearSpline: sets M, N, h and A for the cubic spline model

  In: L, M, T
  Out: N and h[][] and A[][][] filled out for the cubic spline model

  No reutrn value. A and h are created anew and bust be freed by caller.
*/
void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
{
  seth(M, T, h, mlist);
  ssACubicSpline(L, T, M, N, A, mlist, mode);
}//ssCubicSpline

/**
  function setu: sets u[I+1] as base-band windowed Fourier atoms, whose frequencies come in the order of
  0, 1, -1, 2, -2, 3, -3, 4, etc, in bins.

  In: I, Wid: number and size of atoms to generate.
      WinOrder: order (=vanishing moment) of window function to use (2=Hann, 4=Hann^2, etc.)
  Out: u[I+1][Wid], du[I+1]{Wid]: the I+1 atoms and their derivatives.

  No return value. u and du are created anew and must be freed by caller.
*/
void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder, MList* mlist)
{
  Allocate2L(cdouble, I+1, Wid, u, mlist);
  Allocate2L(cdouble, I+1, Wid, du, mlist);

  double** wins=CosineWindows(WinOrder, Wid, (double**)0, 2);
  double omg=2*M_PI/Wid; cdouble jomg=cdouble(0, omg);
  for (int t=0; t<Wid; t++)
  {
    u[0][t]=wins[0][t], du[0][t]=wins[1][t];
    int li=1;
    for (int i=1; i<=I; i++)
    {
      cdouble rot=polar(1.0, li*omg*t);
      u[i][t]=u[0][t]*rot; du[i][t]=du[0][t]*rot+jomg*li*u[i][t];
      li=-li; if (li>0) li++;
    }
  }
  DeAlloc2(wins);
}//setu

/**
  function DerivativePiecewiseI: wrapper for DerivativePiecewise(), doing the initialization ,etc.

  In: L, T: number and length of pieces
      s[LT]: waveform signal
      ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
      f[L+1]: reference frequencies at knots
      M: polynomial degree of piecewise approximation
      SpecifyA, ssmode: pointer to a function that fills A[L], and mode argument to call it
      WinOrder: order(=vanishing moment) of window used for constructing test functions
      I: number of test functions per frame.
      endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: aita[N]: independent coefficients, where N is specified by SpecifyA.

  No return vlue.
*/
void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M,
  void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode,
  int WinOrder, int I, int endmode, cdouble* ds)
{
  MList* mlist=new MList;
  cdouble **u, **du;
  setu(I, 2*T, u, du, WinOrder, mlist);

  int N; double **h, ***A;
  seth(M, T, h, mlist);
  SpecifyA(L, T, M, N, A, mlist, ssmode);

  DerivativePiecewise(N, aita, L, f, T, s, A, M, h, I, u, du, endmode, ds);
  delete mlist;
}//DerivativePiecewiseI

/**
  function DerivativePiecewiseII: wrapper for DerivativePiecewise2(), doing the initialization ,etc.
  This models the derivative of log ampltiude and frequency as separate piecewise polynomials, the first
  specified by SpecifyA, the second by SpecifyB.

  In: L, T: number and length of pieces
      s[LT]: waveform signal
      ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
      f[L+1]: reference frequencies at knots
      M: polynomial degree of piecewise approximation
      SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
      SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
      WinOrder: order(=vanishing moment) of window used for constructing test functions
      I: number of test functions per frame.
      endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.

  No reutrn value.
*/
void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
  void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
  void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
  int WinOrder, int I, int endmode, cdouble* ds)
{
  MList* mlist=new MList;
  cdouble **u, **du;
  setu(I, 2*T, u, du, WinOrder, mlist);

  int Np, Nq;
  double **h, ***A, ***B;
  seth(M, T, h, mlist);
  SpecifyA(L, T, M, Np, A, mlist, ssAmode);
  SpecifyB(L, T, M, Nq, B, mlist, ssBmode);

  DerivativePiecewise2(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds);

  delete mlist;
}//DerivativePiecewiseII

/**
  function DerivativePiecewiseIII: wrapper for DerivativePiecewise3(), doing the initialization ,etc.
  Notice that this time the log amplitude, rather than its derivative, is modeled as a piecewise
  polynomial specified by SpecifyA.

  In: L, T: number and length of pieces
      s[LT]: waveform signal
      ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
      f[L+1]: reference frequencies at knots
      M: polynomial degree of piecewise approximation
      SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
      SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
      WinOrder: order(=vanishing moment) of window used for constructing test functions
      I: number of test functions per frame.
      endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
  Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.

  No reutrn value.
*/
void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
  void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
  void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
  int WinOrder, int I, int endmode, cdouble* ds)
{
  MList* mlist=new MList;
  int Np, Nq;
  double **h, ***A, ***B, **dh=0;
  cdouble **u, **du;
  setu(I, T*2, u, du, WinOrder, mlist);
  seth(M, T, h, mlist);
  if (ds) setdh(M, T, dh, mlist);
  SpecifyA(L, T, M, Np, A, mlist, ssAmode);
  SpecifyB(L, T, M, Nq, B, mlist, ssBmode);
  Alloc2L(M, M, DM, mlist);
  memset(DM[0], 0, sizeof(double)*M*M); for (int m=0; m<M-1; m++) DM[m][m+1]=m+1;
  double** DA=0;

  for (int l=0; l<L; l++)
  {
    DA=MultiplyXY(M, M, Np, DA, DM, A[l], mlist);
    Copy(M, Np, A[l], DA);
  }

  DerivativePiecewise3(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds, dh);

  delete mlist;
}//DerivativePiecewiseIII

/**
  function AmpPhCorrectionExpA: model-preserving amplitude and phase correction in piecewise derivative
  method.

  In: aita[N]: inital independent coefficients
      L, T: number and size of pieces
      sre[LT]: waveform data
      h[M][T], dih[M][T]: piecewise basis functions and their difference-integrals
      A[L][M][N]: L coefficient mapping matrices
      SpecifyA: pointer to the function used for constructing A
      WinOrder: order(=vanishing moment) of window used for constructing test functions
  Out: aita[N]: corrected independent coefficients
       s2[LT]: reconstruct sinusoid BEFORE correction

  Returns the estimate of phase angle at 0.
*/
double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A,
  void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder)
{
  MList* mlist=new MList;
  //*amplitude and phase correction
  //amplitude is done by updating p, i.e. Re(aita)
  double *s2ph=new double[L+1]; mlist->Add(s2ph, 1);
  double *phcor=new double[L+1]; mlist->Add(phcor, 1);
  cdouble* lamda=new cdouble[M]; mlist->Add(lamda, 1);
  double* lamdax=new double[M]; mlist->Add(lamdax, 1);
  double* lamday=new double[M]; mlist->Add(lamday, 1);
  {
  double tmpph=0;
  memset(s2ph, 0, sizeof(double)*(L+1));
  s2ph[0]=tmpph;
  for (int l=0; l<L; l++)
  {
    MultiplyXy(M, N, lamda, A[l], aita); for (int m=0; m<M; m++) lamdax[m]=lamda[m].x, lamday[m]=lamda[m].y;
    SinusoidExpA(T, &s2[l*T], M, lamdax, lamday, h, dih, tmpph); s2ph[l+1]=tmpph;
  }
  double* win=new double[2*T+1]; CosineWindows(WinOrder, 2*T, &win, 1); mlist->Add(win, 1);
  for (int l=1; l<L; l++)
  {
    cdouble inn=Inner(2*T, &sre[l*T-T], win, &s2[l*T-T])/Inner(2*T, &s2[l*T-T], win, &s2[l*T-T]);
    cdouble loginn=log(inn);
    if (SpecifyA==ssACubicHermite)
    {
      aita[l*2]+=loginn.x;
      s2ph[l]+=loginn.y;
      phcor[l]=loginn.y;
      if (l==1) aita[0]+=loginn.x, phcor[0]=loginn.y, s2ph[0]+=loginn.y;
      if (l==L-1) aita[L*2]+=loginn.x, phcor[L]=loginn.y, s2ph[L]+=loginn.y;
    }
    else
    {
      aita[l]+=loginn.x;
      s2ph[l]+=loginn.y;
      phcor[l]=loginn.y;
      if (l==1)
      {
        inn=Inner(T, sre, &win[T], s2)/Inner(T, s2, &win[T], s2);
        loginn=log(inn);
        aita[0]+=loginn.x;
        s2ph[0]+=loginn.y;
        phcor[0]=loginn.y;
      }
      if (l==L-1)
      {
        inn=Inner(T, &sre[L*T-T], win, &s2[L*T-T])/Inner(T, &s2[L*T-T], win, &s2[L*T-T]);
        loginn=log(inn);
        aita[L]+=loginn.x;
        s2ph[L]+=loginn.y;
        phcor[L]=loginn.y;
      }
    }
  }

  for (int l=1; l<=L; l++)
  {
    int k=floor((phcor[l]-phcor[l-1])/(2*M_PI)+0.5);
    if (k!=0)
      phcor[l]+=2*M_PI*k;
  }
  //*
  //now phcor[] contains phase corrector to be interpolated
  double *b=new double[L], *zet=new double[L+1], *dzet=new double[L+1]; memset(zet, 0, sizeof(double)*(L+1)); memset(dzet, 0, sizeof(double)*(L+1));
  mlist->Add(b, 1); mlist->Add(zet, 1); mlist->Add(dzet, 1);
  double ihT[]={T, T/2.0*T, T/3.0*T*T, T/4.0*T*T*T};

  Alloc2L(L, N, BB, mlist);
  //prepare linear system (BB)(zet)=(b)
  for (int l=0; l<L; l++)
  {
    MultiplyxY(N, 4, BB[l], ihT, A[l]);
    b[l]=phcor[l+1]-phcor[l];
  }
  Alloc2L(L, L, copyA, mlist);
  if (L+1==N)	for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
  else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2];
  double* copyb=Copy(L, b, mlist);
  zet[0]=0; GECP(L, &zet[1], copyA, copyb);
  if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
  else if (L+1==N/2) for (int l=0; l<L; l++) for (int k=0; k<L; k++) copyA[l][k]=BB[l][2*k+2];
  Copy(L, copyb, b); for (int l=0; l<L; l++) copyb[l]-=BB[l][0];
  dzet[0]=1; GECP(L, &dzet[1], copyA, copyb);

#ifdef ERROR_CHECK
    //Test that (BB)(zet)=b and (BB)(dzet)=b
    double* bbzet=MultiplyXy(L, L+1, BB, zet, mlist);
    MultiAdd(L, bbzet, bbzet, b, -1);
    double err1=Inner(L, bbzet, bbzet);
    double* bbdzet=MultiplyXy(L, L+1, BB, dzet, mlist);
    MultiAdd(L, bbdzet, bbdzet, b, -1);
    double err2=Inner(L, bbdzet, bbdzet);
    MultiAdd(L+1, dzet, dzet, zet, -1);
    //Test that (BB)dzet=0
    MultiplyXy(L, L+1, bbdzet, BB, dzet);
    double err3=Inner(L, bbzet, bbzet);
#endif
  //now that (zet)+(miu)(dzet) is the general solution to (BB)(zet)=b,
  //	we look for (miu) that maximizes smoothness

  double innuv=0, innvv=0, lmd0[4], lmdd[4], clmdd[4],
    T2=T*T, T3=T2*T, T4=T3*T, T5=T4*T;
  for (int l=0; l<L; l++)
  {
    MultiplyXy(4, L+1, lmd0, A[l], zet);
    MultiplyXy(4, L+1, lmdd, A[l], dzet);
    clmdd[1]=T*lmdd[1]+T2*lmdd[2]+T3*lmdd[3];
    clmdd[2]=T2*lmdd[1]+(4.0/3)*T3*lmdd[2]+1.5*T4*lmdd[3];
    clmdd[3]=T3*lmdd[1]+1.5*T4*lmdd[2]+1.8*T5*lmdd[3];
    innuv+=Inner(3, &lmd0[1], &clmdd[1]);
    innvv+=Inner(3, &lmdd[1], &clmdd[1]);
  }
  MultiAdd(L+1, zet, zet, dzet, -innuv/innvv);

  if (SpecifyA==ssACubicHermite)
    for (int l=0; l<=L; l++) aita[2*l].y+=zet[l];
  else
    for (int l=0; l<=L; l++) aita[l].y+=zet[l];
  //*/
  }
  double result=s2ph[0];
  delete mlist;
  return result;
}//AmpPhCorrectionExpA