xue@11: /*
xue@11:     Harmonic sinusoidal modelling and tools
xue@11: 
xue@11:     C++ code package for harmonic sinusoidal modelling and relevant signal processing.
xue@11:     Centre for Digital Music, Queen Mary, University of London.
xue@11:     This file copyright 2011 Wen Xue.
xue@11: 
xue@11:     This program is free software; you can redistribute it and/or
xue@11:     modify it under the terms of the GNU General Public License as
xue@11:     published by the Free Software Foundation; either version 2 of the
xue@11:     License, or (at your option) any later version.
xue@11: */
xue@1: //---------------------------------------------------------------------------
xue@1: 
xue@1: #include <stddef.h>
Chris@2: #include "sinest.h"
xue@1: #include "fft.h"
xue@1: #include "opt.h"
Chris@2: #include "sinsyn.h"
xue@1: #include "splines.h"
Chris@2: #include "windowfunctions.h"
xue@1: 
Chris@5: /** \file sinest.h */
Chris@5: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function dsincd_unn: derivative of unnormalized discrete sinc function
xue@1: 
xue@1:   In: x, scale N
xue@1: 
xue@1:   Returns the derivative of sincd_unn(x, N)
xue@1: */
xue@1: double dsincd_unn(double x, int N)
xue@1: {
xue@1:   double r=0;
xue@1:   double omg=M_PI*x;
xue@1:   double domg=omg/N;
xue@1:   if (fabs(x)>1e-6)
xue@1:   {
xue@1:     r=M_PI*(cos(omg)-sin(omg)*cos(domg)/sin(domg)/N)/sin(domg);
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     if (domg!=0)
xue@1:     {
xue@1:       double sindomg=sin(domg);
xue@1:       r=-omg*omg*omg*(1-1.0/(1.0*N*N))/3*M_PI/N/sindomg/sindomg;
xue@1:     }
xue@1:     else
xue@1:       r=0;
xue@1:   }
xue@1:   return r;
xue@1: }//dsincd_unn
xue@1: 
Chris@5: /**
xue@1:   function ddsincd_unn: 2nd-order derivative of unnormalized discrete sinc function
xue@1: 
xue@1:   In: x, scale (equivalently, window size) N
xue@1: 
xue@1:   Returns the 2nd-order derivative of sincd_unn(x, N)
xue@1: */
xue@1: double ddsincd_unn(double x, int N)
xue@1: {
xue@1:   double r=0;
xue@1:   double omg=M_PI*x;
xue@1:   double domg=omg/N;
xue@1:   double PI2=M_PI*M_PI;
xue@1:   double NN=1.0/N/N-1;
xue@1:   if (domg==0)
xue@1:   {
xue@1:     r=PI2*N*NN/3;
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     if (fabs(x)>1e-5)
xue@1:     {
xue@1:       r=sin(domg)*cos(omg)-sin(omg)*cos(domg)/N;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       r=omg*omg*omg/N*NN/3;
xue@1:     }
xue@1:     double ss=sin(omg)*NN;
xue@1:     r=-2.0/N*cos(domg)*r/sin(domg)/sin(domg)+ss;
xue@1:     r=r*PI2/sin(domg);
xue@1:   }
xue@1:   return r;
xue@1: }//ddsincd_unn
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function Window: calculates the cosine-family-windowed spectrum of a complex sinusoid on [0:N-1] at
xue@1:   frequency f bins with zero central phase.
xue@1: 
xue@1:   In: f: frequency, in bins
xue@1:       N: window size
xue@1:       M, c[]: cosine-family window decomposition coefficients
xue@1:   Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2.
xue@1: 
xue@1:   Returns pointer to x. x is created anew if x=0 is specified on start.
xue@1: */
xue@1: cdouble* Window(cdouble* x, double f, int N, int M, double* c, int K1, int K2)
xue@1: {
xue@1:   if (K1<0) K1=0;
xue@1:   if (K2>N/2-1) K2=N/2-1;
xue@1: 
xue@1:   if (!x) x=new cdouble[K2-K1+1];
xue@1:   memset(x, 0, sizeof(cdouble)*(K2-K1+1));
xue@1: 
xue@1:   for (int l=K1-M; l<=K2+M; l++)
xue@1:   {
xue@1:     double ang=(f-l)*M_PI;
xue@1:     double omg=ang/N;
xue@1: 		long double si, co, sinn=sin(ang);
xue@1: 		si=sin(omg), co=cos(omg);
xue@1: 		double sa=(ang==0)?N:(sinn/si);
xue@1:     double saco=sa*co;
xue@1: 
xue@1:     int k1=l-M, k2=l+M;
xue@1:     if (k1<K1) k1=K1;
xue@1:     if (k2>K2) k2=K2;
xue@1: 
xue@1:     for (int k=k1; k<=k2; k++)
xue@1:     {
xue@1:       int m=k-l, kt=k-K1;
xue@1:       if (m<0) m=-m;
xue@1:       if (k%2)
xue@1:       {
xue@1:         x[kt].x-=c[m]*saco;
xue@1:         x[kt].y+=c[m]*sinn;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         x[kt].x+=c[m]*saco;
xue@1:         x[kt].y-=c[m]*sinn;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1:   return x;
xue@1: }//Window
xue@1: 
Chris@5: /**
xue@1:   function dWindow: calculates the cosine-family-windowed spectrum and its derivative of a complex
xue@1:   sinusoid on [0:N-1] at frequency f bins with zero central phase.
xue@1: 
xue@1:   In: f: frequency, in bins
xue@1:       N: window size
xue@1:       M, c[]: cosine-family window decomposition coefficients
xue@1:   Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
xue@1:        dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void dWindow(cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
xue@1: {
xue@1:   if (K1<0) K1=0;
xue@1:   if (K2>N/2-1) K2=N/2-1;
xue@1:   memset(x, 0, sizeof(cdouble)*(K2-K1+1));
xue@1:   memset(dx, 0, sizeof(cdouble)*(K2-K1+1));
xue@1: 
xue@1:   for (int l=K1-M; l<=K2+M; l++)
xue@1:   {
xue@1:     double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
xue@1: 		long double si, co, sinn=sin(Omg), cosn=cos(Omg);
xue@1: 		si=sin(omg), co=cos(omg);
xue@1: 		double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N);
xue@1:     double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, cosnpi=cosn*M_PI;
xue@1: 
xue@1:     int k1=l-M, k2=l+M;
xue@1:     if (k1<K1) k1=K1;
xue@1:     if (k2>K2) k2=K2;
xue@1: 
xue@1:     for (int k=k1; k<=k2; k++)
xue@1:     {
xue@1:       int m=k-l, kt=k-K1;
xue@1:       if (m<0) m=-m;
xue@1:       if (k%2)
xue@1:       {
xue@1:         x[kt].x-=c[m]*saco;
xue@1:         x[kt].y+=c[m]*sinn;
xue@1:         dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
xue@1:         dx[kt].y+=c[m]*cosnpi;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         x[kt].x+=c[m]*saco;
xue@1:         x[kt].y-=c[m]*sinn;
xue@1:         dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
xue@1:         dx[kt].y-=c[m]*cosnpi;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: }//dWindow
xue@1: 
Chris@5: /**
xue@1:   function ddWindow: calculates the cosine-family-windowed spectrum and its 1st and 2nd derivatives of
xue@1:   a complex sinusoid on [0:N-1] at frequency f bins with zero central phase.
xue@1: 
xue@1:   In: f: frequency, in bins
xue@1:       N: window size
xue@1:       M, c[]: cosine-family window decomposition coefficients
xue@1:   Out: x[0...K2-K1] containing the spectrum at bins K1, ..., K2,
xue@1:        dx[0...K2-K1] containing the derivative spectrum at bins K1, ..., K2
xue@1:        ddx[0...K2-K1] containing the 2nd-order derivative spectrum at bins K1, ..., K2
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void ddWindow(cdouble* ddx, cdouble* dx, cdouble* x, double f, int N, int M, double* c, int K1, int K2)
xue@1: {
xue@1:   if (K1<0) K1=0;
xue@1:   if (K2>N/2-1) K2=N/2-1;
xue@1:   memset(x, 0, sizeof(cdouble)*(K2-K1+1));
xue@1:   memset(dx, 0, sizeof(cdouble)*(K2-K1+1));
xue@1:   memset(ddx, 0, sizeof(cdouble)*(K2-K1+1));
xue@1: 
xue@1:   for (int l=K1-M; l<=K2+M; l++)
xue@1:   {
xue@1:     double ang=(f-l), Omg=ang*M_PI, omg=Omg/N;
xue@1:     long double si, co, sinn=sin(Omg), cosn=cos(Omg);
xue@1:     si=sin(omg), co=cos(omg);
xue@1:     double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
xue@1:     double saco=sa*co, dsaco=dsa*co, sinnpi_n=sinn*M_PI/N, sinnpipi=sinn*M_PI*M_PI, cosnpi=cosn*M_PI,
xue@1:            cosnpipi_n=cosnpi*M_PI/N, sipi_n=si*M_PI/N;
xue@1: 
xue@1:     int k1=l-M, k2=l+M;
xue@1:     if (k1<K1) k1=K1;
xue@1:     if (k2>K2) k2=K2;
xue@1: 
xue@1:     for (int k=k1; k<=k2; k++)
xue@1:     {
xue@1:       int m=k-l, kt=k-K1;
xue@1:       if (m<0) m=-m;
xue@1:       if (k%2)
xue@1:       {
xue@1:         x[kt].x-=c[m]*saco;
xue@1:         x[kt].y+=c[m]*sinn;
xue@1:         dx[kt].x-=c[m]*(-sinnpi_n+dsaco);
xue@1:         dx[kt].y+=c[m]*cosnpi;
xue@1:         ddx[kt].x-=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
xue@1:         ddx[kt].y-=c[m]*sinnpipi;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         x[kt].x+=c[m]*saco;
xue@1:         x[kt].y-=c[m]*sinn;
xue@1:         dx[kt].x+=c[m]*(-sinnpi_n+dsaco);
xue@1:         dx[kt].y-=c[m]*cosnpi;
xue@1:         ddx[kt].x+=c[m]*(-cosnpipi_n+ddsa*co-dsa*sipi_n);
xue@1:         ddx[kt].y+=c[m]*sinnpipi;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: }//ddWindow
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function IPWindow: computes the truncated inner product of a windowed spectrum with that of a sinusoid
xue@1:   at reference frequency f.
xue@1: 
xue@1:   In: x[0:N-1]: input spectrum
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:       returnamplitude: specifies return value, true for amplitude, false for angle
xue@1: 
xue@1:   Returns the amplitude or phase of the inner product, as specified by $returnamplitude. The return
xue@1:   value is interpreted as the actual amplitude/phase of a sinusoid being estimated at f.
xue@1: */
xue@1: double IPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, bool returnamplitude)
xue@1: {
xue@1:   cdouble r=IPWindowC(f, x, N, M, c, iH2, K1, K2);
xue@1:   double result;
xue@1:   if (returnamplitude) result=sqrt(r.x*r.x+r.y*r.y);
xue@1: 	else result=arg(r);
xue@1:   return result;
xue@1: }//IPWindow
xue@1: //wrapper function
xue@1: double IPWindow(double f, void* params)
xue@1: {
xue@1:   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;
xue@1:   return IPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, true);
xue@1: }//IPWindow
xue@1: 
Chris@5: /**
xue@1:   function ddIPWindow: computes the norm of the truncated inner product of a windowed spectrum with
xue@1:   that of a sinusoid at reference frequency f, as well as its 1st and 2nd derivatives.
xue@1: 
xue@1:   In: x[0:N-1]: input spectrum
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: ipwindow and dipwindow: the truncated inner product norm and its derivative
xue@1: 
xue@1:   Returns the 2nd derivative of the norm of the truncated inner product.
xue@1: */
xue@1: double ddIPWindow(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2, double& dipwindow, double& ipwindow)
xue@1: {
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2], *lx=&x[K1];
xue@1: 	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
xue@1: 	cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
xue@1: 	delete[] w;
xue@1: 
xue@1: 	double R2=~r,
xue@1: 				 R=sqrt(R2),
xue@1: 				 dR2=2*(r.x*dr.x+r.y*dr.y),
xue@1: 				 dR=dR2/(2*R),
xue@1: 				 ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr),
xue@1: 				 ddR=(R*ddR2-dR2*dR)/(2*R2);
xue@1: 	ipwindow=R*iH2;
xue@1: 	dipwindow=dR*iH2;
xue@1: 	return ddR*iH2;
xue@1: }//ddIPWindow
xue@1: //wrapper function
xue@1: double ddIPWindow(double f, void* params)
xue@1: {
xue@1: 	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;
xue@1: 	return ddIPWindow(f, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dipwindow, p->ipwindow);
xue@1: }//ddIPWindow
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function IPWindowC: computes the truncated inner product of a windowed spectrum with that of a
xue@1:   sinusoid at reference frequency f.
xue@1: 
xue@1:   In: x[0:N-1]: input spectrum
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1: 
xue@1:   Returns the inner product. The return value is interpreted as the actual amplitude-phase factor of a
xue@1:   sinusoid being estimated at f.
xue@1: */
xue@1: cdouble IPWindowC(double f, cdouble* x, int N, int M, double* c, double iH2, int K1, int K2)
xue@1: {
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K];
xue@1: 	cdouble *lx=&x[K1], result=0;
xue@1: 	Window(w, f, N, M, c, K1, K2);
xue@1: 	for (int k=0; k<K; k++) result+=lx[k]^w[k];
xue@1: 	delete[] w;
xue@1: 	result*=iH2;
xue@1: 	return result;
xue@1: }//IPWindowC
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function sIPWindow: computes the total energy of truncated inner products between multiple windowed
xue@1:   spectra and that of a sinusoid at a reference frequency f. This does not consider phase alignment
xue@1:   between the spectra, supposedly measured at a sequence of known instants.
xue@1: 
xue@1:   In: x[L][N]: input spectra
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)
xue@1: 
xue@1:   Returns the energy of the vector of inner products.
xue@1: */
xue@1: double sIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, cdouble* lmd)
xue@1: {
xue@1: 	double sip=0;
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K];
xue@1: 	Window(w, f, N, M, c, K1, K2);
xue@1: 	for (int l=0; l<L; l++)
xue@1: 	{
xue@1: 		cdouble *lx=&x[l][K1];
xue@1: 		cdouble r=Inner(K, lx, w);
xue@1: 		if (lmd) lmd[l]=r*iH2;
xue@1: 		sip+=~r;
xue@1: 	}
xue@1: 	sip*=iH2;
xue@1: 	delete[] w;
xue@1: 	return sip;
xue@1: }//sIPWindow
xue@1: //wrapper function
xue@1: double sIPWindow(double f, void* params)
xue@1: {
xue@1: 	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;
xue@1: 	return sIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->lmd);
xue@1: }//sIPWindow
xue@1: 
Chris@5: /**
xue@1:   function dsIPWindow: computes the total energy of truncated inner products between multiple windowed
xue@1:   spectra and that of a sinusoid at a reference frequency f, as well as its derivative. This does not
xue@1:   consider phase synchronization between the spectra, supposedly measured at a sequence of known
xue@1:   instants.
xue@1: 
xue@1:   In: x[L][N]: input spectra
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: sip, the energy of the vector of inner products.
xue@1: 
xue@1:   Returns the derivative of the energy of the vector of inner products.
xue@1: */
xue@1: double dsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& sip)
xue@1: {
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K*2], *dw=&w[K];
xue@1: 	dWindow(dw, w, f, N, M, c, K1, K2);
xue@1: 	double dsip; sip=0;
xue@1: 	for (int l=0; l<L; l++)
xue@1: 	{
xue@1: 		cdouble* lx=&x[l][K1];
xue@1: 		cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
xue@1: 		double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y);
xue@1: 		sip+=R2, dsip+=dR2;
xue@1: 	}
xue@1: 	sip*=iH2, dsip*=iH2;
xue@1: 	delete[] w;
xue@1: 	return dsip;
xue@1: }//dsIPWindow
xue@1: //wrapper function
xue@1: double dsIPWindow(double f, void* params)
xue@1: {
xue@1: 	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;
xue@1: 	return dsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
xue@1: }//dsIPWindow
xue@1: 
Chris@5: /**
xue@1:   function dsdIPWindow_unn: computes the energy of unnormalized truncated inner products between a given
xue@1:   windowed spectrum and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd
xue@1:   derivatives. "Unnormalized" indicates that the inner product cannot be taken as the actual amplitude-
xue@1:   phase factor of a sinusoid, but deviate from that by an unspecified factor.
xue@1: 
xue@1:   In: x[N]: input spectrum
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: sipwindow and dsipwindow, the energy and its derivative of the unnormalized inner product.
xue@1: 
xue@1:   Returns the 2nd derivative of the inner product.
xue@1: */
xue@1: double ddsIPWindow_unn(double f, cdouble* x, int N, int M, double* c, int K1, int K2, double& dsipwindow, double& sipwindow, cdouble* w_unn)
xue@1: {
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 
xue@1: 	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
xue@1: 
xue@1: 	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
xue@1: 
xue@1: 	double rr=0, ri=0, drr=0, dri=0, ddrr=0, ddri=0;
xue@1:   cdouble *lx=&x[K1];
xue@1:   for (int k=0; k<K; k++)
xue@1:   {
xue@1: 		rr+=lx[k].x*w[k].x+lx[k].y*w[k].y;
xue@1:     ri+=lx[k].y*w[k].x-lx[k].x*w[k].y;
xue@1:     drr+=lx[k].x*dw[k].x+lx[k].y*dw[k].y;
xue@1:     dri+=lx[k].y*dw[k].x-lx[k].x*dw[k].y;
xue@1: 		ddrr+=lx[k].x*ddw[k].x+lx[k].y*ddw[k].y;
xue@1: 		ddri+=lx[k].y*ddw[k].x-lx[k].x*ddw[k].y;
xue@1: 	}
xue@1: 	delete[] w;
xue@1: 
xue@1: 	double R2=rr*rr+ri*ri,
xue@1:          dR2=2*(rr*drr+ri*dri),
xue@1:          ddR2=2*(rr*ddrr+ri*ddri+drr*drr+dri*dri);
xue@1: 	sipwindow=R2;
xue@1:   dsipwindow=dR2;
xue@1:   if (w_unn) w_unn->x=rr, w_unn->y=ri;
xue@1: 	return ddR2;
xue@1: }//ddsIPWindow_unn
xue@1: 
Chris@5: /**
xue@1:   function ddsIPWindow: computes the total energy of truncated inner products between multiple windowed
xue@1:   spectra and that of a sinusoid at a reference frequency f, as well as its 1st and 2nd derivatives.
xue@1:   This does not consider phase synchronization between the spectra, supposedly measured at a sequence
xue@1:   of known instants.
xue@1: 
xue@1:   In: x[L][N]: input spectra
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: sip and dsip, the energy of the vector of inner products and its derivative.
xue@1: 
xue@1:   Returns the 2nd derivative of the energy of the vector of inner products.
xue@1: */
xue@1: double ddsIPWindow(double f, int L, cdouble** x, int N, int M, double* c, double iH2, int K1, int K2, double& dsip, double& sip)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1:   int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
xue@1: 	ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
xue@1:   double ddsip=0; dsip=sip=0;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1: 		cdouble* lx=&x[l][K1];
xue@1: 		cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
xue@1: 		double R2=~r, dR2=2*(r.x*dr.x+r.y*dr.y), ddR2=2*(r.x*ddr.x+r.y*ddr.y+~dr);
xue@1:     sip+=R2, dsip+=dR2, ddsip+=ddR2;
xue@1: 	}
xue@1:   sip*=iH2, dsip*=iH2, ddsip*=iH2;
xue@1: 	delete[] w;
xue@1:   return ddsip;
xue@1: }//ddsIPWindow
xue@1: //wrapper function
xue@1: double ddsIPWindow(double f, void* params)
xue@1: {
xue@1:   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;
xue@1: 	return ddsIPWindow(f, p->Fr, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->dsip, p->sip);
xue@1: }//ddsIPWindow
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function sIPWindowC: computes the total energy of truncated inner products between multiple frames of
xue@1:   a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f.
xue@1: 
xue@1:   In: x[L][N]: the spectrogram
xue@1:       offst_rel: frame offset, relative to frame size
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: lmd[L]: the actual individual inner products representing actual ampltiude-phase factors (optional)
xue@1: 
xue@1:   Returns the energy of the vector of inner products.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 	cdouble *w=new cdouble[K];
xue@1: 	double Cr=0;
xue@1: 	cdouble Cc=0;
xue@1: 	Window(w, f, N, M, c, K1, K2);
xue@1: 	for (int l=0; l<L; l++)
xue@1: 	{
xue@1: 		cdouble *lx=&x[l][K1];
xue@1: 		cdouble r=Inner(K, lx, w);
xue@1: 		Cr+=~r;
xue@1: 		double ph=-4*M_PI*f*offst_rel*l;
xue@1: 		cdouble r2=r*r;
xue@1: 		Cc+=r2.rotate(ph);
xue@1: 		if (lmd) lmd[l]=r;
xue@1: 	}
xue@1: 	delete[] w;
xue@1: 	double result=0.5*iH2*(Cr+abs(Cc));
xue@1: 	if (lmd)
xue@1: 	{
xue@1: 		double absCc=abs(Cc), hiH2=0.5*iH2;
xue@1: 		cdouble ej2ph=Cc/absCc;
xue@1: 		for (int l=0; l<L; l++)
xue@1: 		{
xue@1:       double ph=4*M_PI*f*offst_rel*l;
xue@1: 			lmd[l]=hiH2*(lmd[l]+(ej2ph**lmd[l]).rotate(ph));
xue@1:     }
xue@1: 	}
xue@1:   return result;
xue@1: }//sIPWindowC
xue@1: //wrapper function
xue@1: double sIPWindowC(double f, void* params)
xue@1: {
xue@1:   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;
xue@1: 	return sIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2);
xue@1: }//sIPWindowC
xue@1: 
Chris@5: /**
xue@1:   function dsIPWindowC: computes the total energy of truncated inner products between multiple frames of
xue@1:   a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f, together
xue@1:   with its derivative.
xue@1: 
xue@1:   In: x[L][N]: the spectrogram
xue@1:       offst_rel: frame offset, relative to frame size
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: sip: energy of the vector of the inner products
xue@1: 
xue@1:   Returns the 1st derivative of the energy of the vector of inner products.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 
xue@1: 	cdouble *w=new cdouble[K*2], *dw=&w[K];
xue@1:   dWindow(dw, w, f, N, M, c, K1, K2);
xue@1: 	double Cr=0, dCr=0;
xue@1:   cdouble Cc=0, dCc=0;
xue@1: 	for (int l=0; l<L; l++)
xue@1:   {
xue@1: 		cdouble *lx=&x[l][K1];
xue@1:     cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw);
xue@1: 		Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y);
xue@1:     int two=2;
xue@1: 		cdouble r2=r*r, dr2=r*dr*two;
xue@1:     double lag=-4*M_PI*offst_rel*l, ph=lag*f;
xue@1: 		Cc=Cc+cdouble(r2).rotate(ph), dCc=dCc+(dr2+cdouble(0,lag)*r2).rotate(ph);
xue@1:   }
xue@1: 	double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y);
xue@1:   double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1);
xue@1: 	sip=0.5*iH2*(Cr+Cc1);
xue@1:   double dsip=0.5*iH2*(dCr+dCc1);
xue@1: 	delete[] w;
xue@1: 	return dsip;
xue@1: }//dsIPWindowC
xue@1: //wrapper function
xue@1: double dsIPWindowC(double f, void* params)
xue@1: {
xue@1:   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;
xue@1: 	return dsIPWindowC(f, p->L, p->offst_rel, p->x, p->N, p->M, p->c, p->iH2, p->k1, p->k2, p->sip);
xue@1: }//dsIPWindowC
xue@1: 
Chris@5: /**
xue@1:   function ddsIPWindowC: computes the total energy of truncated inner products between multiple frames
xue@1:   of a spectrogram and multiple frames of a spectrogram of a sinusoid at a reference frequency f,
xue@1:   together with its 1st and 2nd derivatives.
xue@1: 
xue@1:   In: x[L][N]: the spectrogram
xue@1:       offst_rel: frame offset, relative to frame size
xue@1:       f: reference frequency, in bins
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       K1, K2: spectrum truncation bounds, in bins, inclusive
xue@1:   Out: sipwindow, dsipwindow: energy of the vector of the inner products and its derivative
xue@1: 
xue@1:   Returns the 2nd derivative of the energy of the vector of inner products.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=N/2) K2=N/2-1;
xue@1: 	int K=K2-K1+1;
xue@1: 
xue@1: 	cdouble *w=new cdouble[K*3], *dw=&w[K], *ddw=&w[K*2];
xue@1:   ddWindow(ddw, dw, w, f, N, M, c, K1, K2);
xue@1: 	double Cr=0, dCr=0, ddCr=0;
xue@1:   cdouble Cc=0, dCc=0, ddCc=0;
xue@1: 	for (int l=0; l<L; l++)
xue@1:   {
xue@1: 		cdouble *lx=&x[l][K1];
xue@1:     cdouble r=Inner(K, lx, w), dr=Inner(K, lx, dw), ddr=Inner(K, lx, ddw);
xue@1: 		Cr+=~r; dCr+=2*(r.x*dr.x+r.y*dr.y); ddCr+=2*(r.x*ddr.x+r.y*ddr.y+~dr);
xue@1:     int two=2;
xue@1: 		cdouble r2=r*r, dr2=r*dr*two, ddr2=(dr*dr+r*ddr)*two;
xue@1:     double lag=-4*M_PI*offst_rel*l, ph=lag*f;
xue@1: 		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);
xue@1:   }
xue@1: 	double Cc2=~Cc, dCc2=2*(Cc.x*dCc.x+Cc.y*dCc.y), ddCc2=2*(Cc.x*ddCc.x+Cc.y*ddCc.y+~dCc);
xue@1:   double Cc1=sqrt(Cc2), dCc1=dCc2/(2*Cc1), ddCc1=(Cc1*ddCc2-dCc2*dCc1)/(2*Cc2);
xue@1: 	sipwindow=0.5*iH2*(Cr+Cc1);
xue@1:   dsipwindow=0.5*iH2*(dCr+dCc1);
xue@1: 	double ddsipwindow=0.5*iH2*(ddCr+ddCc1);
xue@1: 	delete[] w;
xue@1: 	return ddsipwindow;
xue@1: }//ddsIPWindowC
xue@1: //wrapper function
xue@1: double ddsIPWindowC(double f, void* params)
xue@1: {
xue@1: 	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;
xue@1:   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);
xue@1: }//ddsIPWindowC
xue@1: 
xue@1: //--------------------------------------------------------------------------
xue@1: /*
xue@1: 	Least-square-error sinusoid detection function
xue@1: 
xue@1: 	version1: picking the highest peak and take measurement of a single sinusoid
xue@1: 	version2: given a rough peak location and take measurement of a single sinusoid
xue@1: 
xue@1:   Complex spectrum x is calculated using N data points windowed by a window function that is specified
xue@1:   by the parameter set (M, c, iH2). c[0:M] is	provided according to Table 3 in the transfer report, on
xue@1:   pp.11. iH2 is simply 1/H2, where H2 can be calculated using formula (2.17) on pp.12.
xue@1: 
xue@1:   f & epf are given/returned in bins.
xue@1: 
xue@1:   Further reading: "Least-square-error estimation of sinusoids.pdf"
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function LSESinusoid: LSE estimation of the predominant stationary sinusoid.
xue@1: 
xue@1:   In: x[N]: windowed spectrum
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: a and pp: amplitude and phase estimates
xue@1: 
xue@1:   Returns the frequency estimate, in bins.
xue@1: */
xue@1: double LSESinusoid(cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
xue@1: {
xue@1:   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;
Chris@3:   int dfshift=offsetof(l_hx, dhxpeak);
xue@1: 
xue@1:   int inp;
xue@1:   double minp=0;
xue@1:   for (int i=0; i<N; i++)
xue@1:   {
xue@1:     double lf=i, tmp;
xue@1:     p.k1=ceil(lf-B); if (p.k1<0) p.k1=0;
xue@1:     p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:     tmp=IPWindow(lf, &p);
xue@1:     if (minp<tmp) inp=i, minp=tmp;
xue@1:   }
xue@1: 
xue@1:   double f=inp;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
xue@1:   if (tmp==-1)
xue@1:   {
xue@1:     Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
xue@1:   }
xue@1:   else
xue@1:     a=p.hxpeak;
xue@1:   pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
xue@1:   return f;
xue@1: }//LSESinusoid
xue@1: 
xue@1: /*function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency.
xue@1: 
xue@1:   In: x[N]: windowed spectrum
xue@1:       f: initial frequency, in bins
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: f, a and pp: frequency, amplitude and phase estimates
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void LSESinusoid(double& f, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
xue@1: {
xue@1:   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};
Chris@3:   int dfshift=offsetof(l_hx, dhxpeak);
xue@1: 
xue@1:   double inp=f;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
xue@1:   if (tmp==-1)
xue@1:   {
xue@1:     Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
xue@1:   }
xue@1:   else
xue@1:     a=p.hxpeak;
xue@1:   pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
xue@1: }//LSESinusoid
xue@1: 
Chris@5: /**
xue@1:   function LSESinusoid: LSE estimation of stationary sinusoid predominant within [f1, f2].
xue@1: 
xue@1:   In: x[N]: windowed spectrum
xue@1:       [f1, f2]: frequency range
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: a and pp: amplitude and phase estimates
xue@1: 
xue@1:   Returns the frequency estimate, in bins.
xue@1: */
xue@1: double LSESinusoid(int f1, int f2, cdouble* x, int N, double B, int M, double* c, double iH2, double& a, double& pp, double epf)
xue@1: {
xue@1:   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};
Chris@3:   int dfshift=offsetof(l_hx, dhxpeak);
xue@1: 
xue@1:   int inp;
xue@1:   double minp=0;
xue@1:   for (int i=f1; i<f2; i++)
xue@1:   {
xue@1:     double lf=i, tmp;
xue@1:     p.k1=ceil(lf-B); if (p.k1<0) p.k1=0;
xue@1:     p.k2=floor(lf+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:     tmp=IPWindow(lf, &p);
xue@1:     if (minp<tmp) inp=i, minp=tmp;
xue@1:   }
xue@1: 
xue@1:   double f=inp;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double tmp=Newton(f, ddIPWindow, &p, dfshift, epf);
xue@1:   if (tmp==-1)
xue@1:   {
xue@1:     Search1Dmax(f, &p, IPWindow, inp-1, inp+1, &a, epf);
xue@1:   }
xue@1:   else
xue@1:     a=p.hxpeak;
xue@1:   pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
xue@1:   return f;
xue@1: }//LSESinusoid
xue@1: 
Chris@5: /**
xue@1:   function LSESinusoid: LSE estimation of stationary sinusoid near a given initial frequency within [f1,
xue@1:   f2].
xue@1: 
xue@1:   In: x[N]: windowed spectrum
xue@1:       f: initial frequency, in bins
xue@1:       [f1, f2]: frequency range
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: f, a and pp: frequency, amplitude and phase estimates
xue@1: 
xue@1:   Returns 1 if managed to find a sinusoid, 0 if not, upon which $a and $pp are estimated at the initial
xue@1:   f.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   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;
Chris@3:   int dfshift=offsetof(l_hx, dhxpeak);
xue@1: 
xue@1:   int result=0;
xue@1:   double inp=f;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double tmp=Newton(f, ddIPWindow, &p, dfshift, epf, 100, 1e-256, f1, f2);
xue@1:   if (tmp!=-1 && f>f1 && f<f2)
xue@1:   {
xue@1:     result=1;
xue@1:     a=p.hxpeak;
xue@1:     pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
xue@1:   }
xue@1:   else
xue@1:   {
xue@1:     Search1DmaxEx(f, &p, IPWindow, f1, f2, &a, epf);
xue@1:     if (f<=f1 || f>=f2)
xue@1:     {
xue@1:       f=inp;
xue@1:       cdouble r=IPWindowC(f, x, N, M, c, iH2, p.k1, p.k2);
xue@1:       a=abs(r);
xue@1:       pp=arg(r);
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       result=1;
xue@1:       pp=IPWindow(f, x, N, M, c, iH2, p.k1, p.k2, false);
xue@1:     }
xue@1:   }
xue@1:   return result;
xue@1: }//LSESinusoid
xue@1: 
Chris@5: /**
xue@1:   function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
xue@1:   considering phase-frequency consistency across frames.
xue@1: 
xue@1:   In: x[Fr][N]: spectrogram
xue@1:       f: initial frequency, in bins
xue@1:       [f1, f2]: frequency range
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates
xue@1: 
xue@1:   Returns an error bound of the frequency estimate.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   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};
Chris@3:   int dfshift=offsetof(l_ip1, dsip), fshift=offsetof(l_ip1, sip);
xue@1: 
xue@1:   double inp=f;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double errf=Newton1dmax(f, f1, f2, ddsIPWindow, &p, dfshift, fshift, dsIPWindow, dfshift, epf);
xue@1:   if (errf<0) errf=Search1Dmax(f, &p, sIPWindow, f1, f2, a, epf);
xue@1:   if (a || ph)
xue@1:   {
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       cdouble r=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);
xue@1:       if (a) a[fr]=abs(r);
xue@1:       if (ph) ph[fr]=arg(r);
xue@1:     }
xue@1:   }
xue@1:   return errf;
xue@1: }//LSESinusoidMP
xue@1: 
Chris@5: /**
xue@1:   function LSESinusoidMP: LSE estimation of a stationary sinusoid from multi-frames spectrogram without
xue@1:   considering phase-frequency consistency across frames.
xue@1: 
xue@1:   In: x[Fr][N]: spectrogram
xue@1:       f: initial frequency, in bins
xue@1:       [f1, f2]: frequency range
xue@1:       B: spectral truncation half width, in bins.
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:       epf: frequency error tolerance, in bins
xue@1:   Out: f, a[Fr] and ph[Fr]: frequency, amplitudes and phase angles estimates
xue@1: 
xue@1:   Returns an error bound of the frequency estimate. Although the frequencies are estimated assuming
xue@1:   cross-frame frequency-phase consistency, the final output phase angles are reestimated independently
xue@1:   for each frame using the frequency estimate.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   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;}
xue@1:     p={N, 0, 0, M, c,iH2, Fr, Offst*1.0/N, x, 0, 0};
Chris@3:     int dfshift=offsetof(l_ip, sdip), fshift=offsetof(l_ip, sip);
xue@1: 
xue@1:   double inp=f;
xue@1:   p.k1=ceil(inp-B); if (p.k1<0) p.k1=0;
xue@1:   p.k2=floor(inp+B); if (p.k2>=p.N/2) p.k2=p.N/2-1;
xue@1:   double errf=Newton1dmax(f, f1, f2, ddsIPWindowC, &p, dfshift, fshift, dsIPWindowC, dfshift, epf);
xue@1:   if (errf<0) errf=Search1Dmax(f, &p, sIPWindowC, f1, f2, a, epf);
xue@1:   if (a || ph)
xue@1:   {
xue@1:     cdouble* lmd=new cdouble[Fr];
xue@1:     sIPWindowC(f, Fr, Offst*1.0/N, x, N, M, c, iH2, p.k1, p.k2, lmd);
xue@1:     for (int fr=0; fr<Fr; fr++)
xue@1:     {
xue@1:       lmd[fr]=IPWindowC(f, x[fr], N, M, c, iH2, p.k1, p.k2);
xue@1: 
xue@1:       if (a) a[fr]=abs(lmd[fr]);
xue@1:       if (ph) ph[fr]=arg(lmd[fr]);
xue@1:     }
xue@1:     delete[] lmd;
xue@1:   }
xue@1:   return errf;
xue@1: }//LSESinusoidMPC
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
xue@1:   suppression index of eps, i.e. the least square error is minimized with an additional eps*||lmd||^2
xue@1:   term.
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I]: frequencies
xue@1:       M, c[]: cosine-family window specification parameters
xue@1:       K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
xue@1:       eps: energy suppression factor
xue@1:   Out: lmd[I]: amplitude-phase factors
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int K1, int K2, int M, double* c, double eps)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
xue@1:   MList* List=new MList;
xue@1:   cdouble** Allocate2L(cdouble, I, K, wt, List);
xue@1:   for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
xue@1:   cdouble** whw=MultiplyXcXt(I, K, wt, List);
xue@1:   cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
xue@1:   for (int i=0; i<I; i++) whw[i][i]+=eps;
xue@1:   GECP(I, lmd, whw, whx);
xue@1:   delete List;
xue@1: }//IPMulti
xue@1: 
Chris@5: /**
xue@1:   function IPMulti: least square estimation of multiple sinusoids, given their frequencies and an energy
xue@1:   suppression index of eps, and optionally returns residue and sensitivity indicators for each sinusoid.
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I]: frequencies
xue@1:       M, c[]: cosine-family window specification parameters
xue@1:       K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
xue@1:       eps: energy suppression factor
xue@1:   Out: lmd[I]: amplitude-phase factors
xue@1:        sens[I]: sensitivity indicators
xue@1:        r1[I]: residue indicators, measured by correlating residue with sinusoid spectra, optional
xue@1: 
xue@1:   No return value. Sensibitily is computed BEFORE applying eps.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
xue@1:   MList* List=new MList;
xue@1:   cdouble** Allocate2L(cdouble, I, K, wt, List);
xue@1:   for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
xue@1:   cdouble** whw=MultiplyXcXt(I, K, wt, List);
xue@1: 
xue@1: 	//*computes sensitivity if required
xue@1:   if (sens)
xue@1:   {
xue@1:     cdouble** iwhw=Copy(I, whw, List);
xue@1:     GICP(I, iwhw);
xue@1:     cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
xue@1:     for (int i=0; i<I; i++)
xue@1:     {
xue@1: 			sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
xue@1:     }
xue@1: 	} //*/
xue@1:   cdouble* whx=MultiplyXcy(I, K, wt, &x[K1], List);
xue@1:   for (int i=0; i<I; i++) whw[i][i]+=eps;
xue@1:   GECP(I, lmd, whw, whx);
xue@1:   //compute residue if required
xue@1:   if (r1)
xue@1:   {
xue@1:     cdouble* wlmd=MultiplyXty(K, I, wt, lmd, List); //reconstruct
xue@1:     for (int k=0; k<K; k++) wlmd[k]=wlmd[k]-x[K1+k]; //-residue
xue@1:     for (int i=0; i<I; i++) //r1[i]=Inner(K, wlmd, wt[i]).abs(); //-residue weighted by window
xue@1:     {
xue@1:       r1[i]=0;
xue@1:       for (int k=0; k<K; k++) r1[i]+=abs(wlmd[k])*abs(wt[i][k]);
xue@1:     }
xue@1:   }
xue@1:   delete List;
xue@1: }//IPMulti
xue@1: 
Chris@5: /**
xue@1:   function IPMultiSens: computes the sensitivity of the least square estimation of multiple sinusoids given
xue@1:   their frequencies .
xue@1: 
xue@1:   In: f[I]: frequencies
xue@1:       M, c[]: cosine-family window specification parameters
xue@1:       K1, K2: spectral truncation range, i.e. bins outside [K1, K2] are ignored
xue@1:       eps: energy suppression factor
xue@1:   Out: sens[I]: sensitivity indicators
xue@1: 
xue@1:   No return value. Sensibility is computed AFTER applying eps
xue@1: */
xue@1: void IPMultiSens(int I, double* f, int Wid, int K1, int K2, int M, double* c, double* sens, double eps)
xue@1: {
xue@1:   if (K1<0) K1=0; if (K2>=Wid/2) K2=Wid/2-1; int K=K2-K1+1;
xue@1:   MList* List=new MList;
xue@1:   cdouble** Allocate2L(cdouble, I, K, wt, List);
xue@1:   for (int i=0; i<I; i++) Window(wt[i], f[i], Wid, M, c, K1, K2);
xue@1: 
xue@1:   cdouble** whw=MultiplyXcXt(I, K, wt, List);
xue@1:   for (int i=0; i<I; i++) whw[i][i]+=eps;
xue@1: 
xue@1:   cdouble** iwhw=Copy(I, whw, List);
xue@1:   GICP(I, iwhw);
xue@1:   cdouble** u=MultiplyXYc(I, I, K, iwhw, wt, List);
xue@1:   for (int i=0; i<I; i++)
xue@1: 	{
xue@1:     sens[i]=0; for (int k=0; k<K; k++) sens[i]+=~u[i][k]; sens[i]=sqrt(sens[i]);
xue@1:   }
xue@1:   delete List;
xue@1: }//IPMultiSens
xue@1: 
Chris@5: /**
xue@1:   function IPMulti: least square estimation of multi-sinusoids with GIVEN frequencies. This version
xue@1:   operates in groups at least B bins from each other, rather than LSE all frequencies together.
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I]: frequencies, must be ordered low to high.
xue@1:       B: number of bins beyond which sinusoids are treated as non-interfering
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:   Out: lmd[I]: amplitude-phase factors
xue@1: 
xue@1:   Returns 0.
xue@1: */
xue@1: double IPMulti(int I, double* f, cdouble* lmd, cdouble* x, int Wid, int M, double* c, double iH2, int B)
xue@1: {
xue@1:   int i=0, ist=0;
xue@1:   double Bw=B;
xue@1:   while (i<I)
xue@1:   {
xue@1:     if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
xue@1:     {
xue@1:       if (i==I-1) i++;
xue@1:       //process frequencies from ist to i-1
xue@1:       if (i-1==ist) //one sinusoid
xue@1:       {
xue@1:         double fb=f[ist]; int K1=floor(fb-B+0.5), K2=floor(fb+B+0.5);
xue@1:         lmd[ist]=IPWindowC(fb, x, Wid, M, c, iH2, K1, K2);
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         MList* List=new MList;
xue@1:         int N=i-ist, K1=floor(f[ist]-B+0.5), K2=floor(f[i-1]+B+0.5), K=K2-K1+1;
xue@1:         cdouble** Allocate2L(cdouble, N, K, wt, List);
xue@1:         for (int n=0; n<N; n++) Window(wt[n], f[ist+n], Wid, M, c, K1, K2);
xue@1:         cdouble* whx=MultiplyXcy(N, K, wt, &x[K1], List); //w*'x=(wt*)x
xue@1:         cdouble** whw=MultiplyXcXt(N, K, wt, List);
xue@1:           /*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);
xue@1:           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);*/
xue@1:         GECP(N, &lmd[ist], whw, whx); //solving complex linear system (w*'w)a=w*'x
xue@1:         delete List;
xue@1:       }
xue@1:       ist=i;
xue@1:     }
xue@1:     i++;
xue@1:   }
xue@1:   return 0;
xue@1: }//IPMulti
xue@1: 
Chris@5: /**
xue@1:   function IPMulti_Direct: LSE estimation of multiple sinusoids given frequencies AND PHASES (direct
xue@1:   method)
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I], ph[I]: frequencies and phase angles.
xue@1:       B: spectral truncation half width, in bins; sinusoids over 3B bins apart are regarded non-interfering
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:   Out: a[I]: amplitudes
xue@1: 
xue@1:   Returns square norm of the residue.
xue@1: */
xue@1: double IPMulti_Direct(int I, double* f, double* ph, double* a, cdouble* x, int Wid, int M, double* c, double iH2, int B)
xue@1: {
xue@1: 	MList* List=new MList;
xue@1:   int i=0, ist=0, hWid=Wid/2;
xue@1:   cdouble* r=Copy(hWid, x, List); //to store the residue
xue@1: 
xue@1:   double Bw=3.0*B;
xue@1:   while (i<I)
xue@1:   {
xue@1:     if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
xue@1:     {
xue@1:       if (i==I-1) i++;
xue@1: 
xue@1:       //process frequencies from ist to i-1
xue@1:       if (i-1==ist) //one sinusoid
xue@1:       {
xue@1:         double fb=f[ist];
xue@1:         cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
xue@1:         for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
xue@1:         double ip=Inner(2*hWid, (double*)x, (double*)w);
xue@1:         a[ist]=ip*iH2;
xue@1:         MultiAdd(hWid, r, r, w, -a[ist]);
xue@1:         delete[] w;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         int N=i-ist;
xue@1:         cdouble** Allocate2L(cdouble, N, hWid, wt, List);
xue@1:         for (int n=0; n<N; n++)
xue@1:         {
xue@1:           Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
xue@1:           for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
xue@1:         }
xue@1:         double* whxr=MultiplyXy(N, hWid*2, (double**)wt, (double*)x, List); //w*'x=(wt*)x
xue@1:         double** whwr=MultiplyXXt(N, hWid*2, (double**)wt, List);
xue@1:         GECP(N, &a[ist], whwr, whxr); //solving complex linear system (w*'w)a=w*'x
xue@1:         for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
xue@1:       }
xue@1:       ist=i;
xue@1:     }
xue@1:     i++;
xue@1:   }
xue@1:   double result=Inner(hWid, r, r).x;
xue@1:   delete List;
xue@1:   return result;
xue@1: }//IPMulti_Direct
xue@1: 
Chris@5: /**
xue@1:   function IPMulti_GS: LSE estimation of multiple sinusoids given frequencies AND PHASES (Gram-Schmidt method)
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I], ph[I]: frequencies and phase angles.
xue@1:       B: spectral truncation, in bins; sinusoids over 3B bins apart are regarded non-interfering
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:   Out: a[I]: amplitudes
xue@1: 
xue@1:   Returns square norm of the residue.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   MList* List=new MList;
xue@1:   int i=0, ist=0, hWid=Wid/2;
xue@1:   cdouble* r=Copy(hWid, x, List); //to store the residue
xue@1:   double Bw=3.0*B;
xue@1:   while (i<I)
xue@1:   {
xue@1:     if ((i>0 && f[i]-f[i-1]>Bw) || i==I-1)
xue@1:     {
xue@1:       if (i==I-1) i++;
xue@1: 
xue@1:       //process frequencies from ist to i-1
xue@1:       if (i-1==ist) //one sinusoid
xue@1:       {
xue@1:         double fb=f[ist];
xue@1:         cdouble* w=Window(0, fb, Wid, M, c, 0, hWid-1);
xue@1:         for (int k=0; k<hWid; k++) w[k].rotate(ph[ist]);
xue@1:         double ip=Inner(2*hWid, (double*)x, (double*)w);
xue@1:         a[ist]=ip*iH2;
xue@1:         MultiAdd(hWid, r, r, w, -a[ist]);
xue@1:         delete[] w;
xue@1:       }
xue@1:       else
xue@1:       {
xue@1:         int N=i-ist;
xue@1:         cdouble** Allocate2L(cdouble, N, hWid, wt, List);
xue@1:         Alloc2L(N, N, L, List); Alloc2L(N, hWid*2, Q, List);
xue@1:         for (int n=0; n<N; n++)
xue@1:         {
xue@1:           Window(wt[n], f[ist+n], Wid, M, c, 0, hWid-1);
xue@1:           for (int k=0; k<hWid; k++) wt[n][k].rotate(ph[ist+n]);
xue@1:         }
xue@1:         LQ_GS(N, hWid*2, (double**)wt, L, Q);
xue@1:         double* atl=MultiplyxYt(N, hWid*2, (double*)x, Q, List);
xue@1:         GExL(N, &a[ist], L, atl);
xue@1:         for (int n=0; n<N; n++) MultiAdd(hWid, r, r, wt[n], -a[ist+n]);
xue@1:       }
xue@1:       ist=i;
xue@1:     }
xue@1:     i++;
xue@1:   }
xue@1:   double result=Inner(hWid, r, r).x;
xue@1:   delete List;
xue@1: 	return result;
xue@1: }//IPMulti_GS
xue@1: 
Chris@5: /**
xue@1:   function IPMulti: LSE estimation of I sinusoids given frequency and phase and J sinusoids given
xue@1:   frequency only
xue@1: 
xue@1:   In: x[Wid]: spectrum
xue@1:       f[I+J], ph[I]: frequencies and phase angles
xue@1:       M, c[], iH2: cosine-family window specification parameters
xue@1:   Out: a[I+J]: amplitudes
xue@1:        ph[I:I+J-1]: phase angles not given on start
xue@1:        wt[I+2J][hWid], Q[I+2J][hWid], L[I+2J][I+2J]: internal w matrix and its LQ factorization, optional
xue@1: 
xue@1:   Returns the residue vector, newly created and registered to RetList, if specified. On start a[] should
xue@1:   have valid storage no less than I+2J.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   MList* List=new MList;
xue@1:   int hWid=Wid/2;
xue@1:   cdouble* r=Copy(hWid, x, RetList); //to store the residue
xue@1:   if (!wt){Allocate2L(cdouble, I+J*2, hWid, wt, List);}
xue@1:   if (!Q){Allocate2L(cdouble, I+J*2, hWid, Q, List);}
xue@1:   if (!L){Allocate2L(double, I+J*2, I+J*2, L, List);}
xue@1:   memset(wt[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
xue@1:   memset(Q[0], 0, sizeof(cdouble)*(I+J*2)*hWid);
xue@1:   memset(L[0], 0, sizeof(double)*(I+J*2)*(I+J*2));
xue@1: 
xue@1:   //*The direct form
xue@1:   for (int i=0; i<I; i++)
xue@1:   {
xue@1:     Window(wt[i], f[i], Wid, M, c, 0, hWid-1);
xue@1:     for (int k=0; k<hWid; k++) wt[i][k].rotate(ph[i]);
xue@1:   }
xue@1:   for (int j=0; j<J; j++)
xue@1:   {
xue@1:     cdouble *w1=wt[I+j*2], *w2=wt[I+j*2+1];
xue@1:     Window(w1, f[I+j], Wid, M, c, 0, hWid-1);
xue@1:     for (int k=0; k<hWid; k++) w2[k].y=w1[k].x, w2[k].x=-w1[k].y;
xue@1:   }
xue@1: 
xue@1:   LQ_GS(I+J*2, hWid*2, (double**)wt, L, (double**)Q);
xue@1:   double *atl=MultiplyxYt(I+J*2, hWid*2, (double*)x, (double**)Q, List);
xue@1:   GExL(I+J*2, a, L, atl);
xue@1:   
xue@1:   for (int i=0; i<I+J*2; i++) MultiAdd(hWid, r, r, wt[i], -a[i]);
xue@1:   for (int j=0; j<J; j++)
xue@1:   {
xue@1:     double xx=a[I+j*2], yy=a[I+j*2+1];
xue@1:     a[I+j]=sqrt(xx*xx+yy*yy);
xue@1:     ph[I+j]=atan2(yy, xx);
xue@1:   }
xue@1:   delete List;
xue@1:   return r;
xue@1: }//IPMulti
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Routines for estimation two sinusoids with 1 fixed and 1 flexible frequency
xue@1: 
xue@1:     Further reading: "LSE estimation for 2 sinusoids with 1 at a fixed frequency.pdf"
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function WindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
xue@1:   sinusoids at frequencies f1 and f2, df=f1-f2.
xue@1: 
xue@1:   In: df: frequency difference, in bins
xue@1:       N: DFT size
xue@1:       M, d[]: cosine-family window specification parameters (see "further reading").
xue@1:   Out: w[0], the inner product, optional
xue@1: 
xue@1:   Returns square norm of the inner product.
xue@1: */
xue@1: double WindowDuo(double df, int N, double* d, int M, cdouble* w)
xue@1: {
xue@1: 	double wr=0, wi=0;
xue@1: 	for (int m=-2*M; m<=2*M; m++)
xue@1: 	{
xue@1: 		double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
xue@1: 		double si=sin(omg), co=cos(omg), sinn=sin(Omg);
xue@1: 		double sa=(ang==0)?N:(sinn/si);
xue@1: 		double dm; if (m<0) dm=d[-m]; else dm=d[m];
xue@1: 		wr+=dm*sa*co, wi+=-dm*sinn;
xue@1: 	}
xue@1: 	wr*=N, wi*=N;
xue@1: 	if (w) w->x=wr, w->y=wi;
xue@1: 	double result=wr*wr+wi*wi;
xue@1: 	return result;
xue@1: }//WindowDuo
xue@1: 
Chris@5: /**
xue@1:   function ddWindowDuo: calcualtes the square norm of the inner product between windowed spectra of two
xue@1:   sinusoids at frequencies f1 and f2, df=f1-f2, with its 1st and 2nd derivatives
xue@1: 
xue@1:   In: df: frequency difference, in bins
xue@1:       N: DFT size
xue@1:       M, d[]: cosine-family window specification parameters (see "further reading" for d[]).
xue@1:   Out: w[0], the inner product, optional
xue@1:        window, dwindow: square norm and its derivative, of the inner product
xue@1: 
xue@1:   Returns 2nd derivative of the square norm of the inner product.
xue@1: */
xue@1: double ddWindowDuo(double df, int N, double* d, int M, double& dwindow, double& window, cdouble* w)
xue@1: {
xue@1: 	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;
xue@1: 	for (int m=-2*M; m<=2*M; m++)
xue@1: 	{
xue@1: 		double ang=df+m, Omg=ang*M_PI, omg=Omg/N;
xue@1: 		double si=sin(omg), co=cos(omg), sinn=sin(Omg), cosn=cos(Omg);
xue@1: 		double sa=(ang==0)?N:(sinn/si), dsa=dsincd_unn(ang, N), ddsa=ddsincd_unn(ang, N);
xue@1: 		double dm; if (m<0) dm=d[-m]; else dm=d[m];
xue@1: 		wr+=dm*sa*co, wi+=-dm*sinn;
xue@1: 		dwr+=dm*(dsa*co-PI_N*sinn), dwi+=-dm*M_PI*cosn;
xue@1: 		ddwr+=dm*(ddsa*co-PI_N*dsa*si-PIPI_N*cosn), ddwi+=dm*PIPI*sinn;
xue@1: 	}
xue@1:   wr*=N, wi*=N, dwr*=N, dwi*=N, ddwr*=N, ddwi*=N;
xue@1:   window=wr*wr+wi*wi;
xue@1:   dwindow=2*(wr*dwr+wi*dwi);
xue@1:   if (w) w->x=wr, w->y=wi;
xue@1: 	double ddwindow=2*(wr*ddwr+dwr*dwr+wi*ddwi+dwi*dwi);
xue@1:   return ddwindow;
xue@1: }//ddWindowDuo
xue@1: 
Chris@5: /**
xue@1:   function sIPWindowDuo: calculates the square norm of the orthogonal projection of a windowed spectrum
xue@1:   onto the linear span of the windowed spectra of two sinusoids at reference frequencies f1 and f2.
xue@1: 
xue@1:   In: x[N]: spectrum
xue@1:       f1, f2: reference frequencies.
xue@1:       M, c[], d[], iH2: cosine-family window specification parameters.
xue@1:       K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.
xue@1:   Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors
xue@1: 
xue@1:   Returns the square norm of the orthogonal projection.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	int K=K2-K1+1;
xue@1:   cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
xue@1:   Window(w1, f1, N, M, c, K1, K2);
xue@1:   double w1w1=0;
xue@1:   for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
xue@1:   for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];
xue@1: 	Window(w1, f2, N, M, c, K1, K2);
xue@1:   cdouble r1w2=0, w12; for (int k=0; k<K; k++) r1w2+=(r1[k]^w1[k]);
xue@1:   double w=WindowDuo(f1-f2, N, d, M, &w12);
xue@1:   double v=1.0/iH2-w*iH2;
xue@1:   double result=~xw1/w1w1+~r1w2/v;
xue@1: 	cdouble mu2=r1w2/v;
xue@1: 	lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;
xue@1:   delete[] w1;
xue@1:   return result;
xue@1: }//sIPWindowDuo
xue@1: //wrapper function
xue@1: double sIPWindowDuo(double f2, void* params)
xue@1: {
xue@1: 	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;
xue@1: 	cdouble r1, r2;
xue@1: 	return sIPWindowDuo(p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
xue@1: }//sIPWindowDuo
xue@1: 
Chris@5: /**
xue@1:   function ddsIPWindowDuo: calculates the square norm, and its 1st and 2nd derivatives against f2,, of
xue@1:   the orthogonal projection of a windowed spectrum onto the linear span of the windowed spectra of two
xue@1:   sinusoids at reference frequencies f1 and f2.
xue@1: 
xue@1:   In: x[N]: spectrum
xue@1:       f1, f2: reference frequencies.
xue@1:       M, c[], d[], iH2: cosine-family window specification parameters.
xue@1:       K1, K2: spectrum truncation range, i.e. bins outside [K1, K2] are ignored.
xue@1: 
xue@1:   Out: lmd1, lmd2: projection coefficients, interpreted as actual amplitude-phase factors
xue@1:        ddsip[3]: the 2nd, 1st and 0th derivatives (against f2) of the square norm.
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   int K=K2-K1+1;
xue@1: 	cdouble xw1=0, *lx=&x[K1], *w1=new cdouble[K*2], *r1=&w1[K];
xue@1:   Window(w1, f1, N, M, c, K1, K2);
xue@1:   double w1w1=0;
xue@1:   for (int k=0; k<K; k++) xw1+=(lx[k]^w1[k]), w1w1+=~w1[k]; cdouble mu1=xw1/w1w1;
xue@1:   for (int k=0; k<K; k++) r1[k]=lx[k]-mu1*w1[k];
xue@1: 
xue@1:   cdouble r1w2, w12;
xue@1:   double u, du, ddu=ddsIPWindow_unn(f2, &r1[-K1], N, M, c, K1, K2, du, u, &r1w2);
xue@1: 	double w, dw, ddw=ddWindowDuo(f1-f2, N, d, M, dw, w, &w12); dw=-dw;
xue@1:   double v=1.0/iH2-w*iH2, dv=-iH2*dw, ddv=-iH2*ddw;
xue@1: 	double iv=1.0/v;//, div=-dv*iv*iv, ddiv=(2*dv*dv-v*ddv)*iv*iv*iv;
xue@1: 
xue@1:   ddsip2[2]=~xw1/w1w1+u*iv;
xue@1:   ddsip2[1]=iv*(du-iv*u*dv);
xue@1:   ddsip2[0]=iv*(ddu-iv*(u*ddv+2*du*dv-2*iv*u*dv*dv));
xue@1: 
xue@1:   cdouble mu2=r1w2*iv;
xue@1: 	lmd2=mu2; lmd1=mu1-(mu2^w12)*iH2;
xue@1: 
xue@1:   delete[] w1;
xue@1: }//ddsIPWindowDuo
xue@1: //wrapper function
xue@1: double ddsIPWindowDuo(double f2, void* params)
xue@1: {
xue@1: 	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;
xue@1:   double ddsip2[3]; cdouble r1, r2;
xue@1:   ddsIPWindowDuo(ddsip2, p->f1, f2, p->x, p->N, p->c, p->d, p->M, p->iH2, p->k1, p->k2, r1, r2);
xue@1:   p->dipwindow=ddsip2[1], p->ipwindow=ddsip2[2];
xue@1:   return ddsip2[0];
xue@1: }//ddsIPWindowDuo
xue@1: 
Chris@5: /**
xue@1:   function LSEDuo: least-square estimation of two sinusoids of which one has a fixed frequency
xue@1: 
xue@1:   In: x[N]: the windowed spectrum
xue@1:       f1: the fixed frequency
xue@1:       f2: initial value of the flexible frequency
xue@1:       fmin, fmax: search range for f2, the flexible frequency
xue@1:       B: spectral truncation half width
xue@1:       M, c[], d[], iH2:
xue@1:       epf: frequency error tolerance
xue@1:   Out: f2: frequency estimate
xue@1:        lmd1, lmd2: amplitude-phase factor estimates
xue@1:   Returns 1 if managed to find a good f2, 0 if not, upon which the initial f2 is used for estimating
xue@1: 
xue@1:   amplitudes and phase angles.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   int result=0;
xue@1:   double inp=f2;
xue@1:   int k1=ceil(inp-B); if (k1<0) k1=0;
xue@1:   int k2=floor(inp+B); if (k2>=N/2) k2=N/2-1;
xue@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};
Chris@3:   int dfshift=offsetof(l_hx, dipwindow);// fshift=int(&((l_hx*)0)->ipwindow);
xue@1: 
xue@1:   double tmp=Newton(f2, ddsIPWindowDuo, &p, dfshift, epf, 100, 1e-256, fmin, fmax);
xue@1:   if (tmp!=-1 && f2>fmin && f2<fmax) result=1;
xue@1:   else
xue@1:   {
xue@1:     Search1DmaxEx(f2, &p, sIPWindowDuo, fmin, fmax, NULL, epf);
xue@1:     if (f2<=fmin || f2>=fmax) f2=inp;
xue@1:     else result=1;
xue@1:   }
xue@1:   sIPWindowDuo(f1, f2, x, N, c, d, M, iH2, k1, k2, r1, r2);
xue@1:   return result;
xue@1: }//LSEDuo
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Time-frequency reassignment sinusoid estimation routines.
xue@1: 
xue@1:     Further reading: A. R?bel, ¡°Estimating partial frequency and frequency slope using reassignment
xue@1:     operators,¡± in Proc. ICMC¡¯02. G?teborg. 2002.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function CDFTW: single-frequency windowed DTFT, centre-aligned
xue@1: 
xue@1:   In: data[Wid]: waveform data x
xue@1:       win[Wid+1]: window function
xue@1:       k: frequency, in bins, where bin=1/Wid
xue@1:   Out: X: DTFT of xw at frequency k bins
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void CDFTW(cdouble& X, double k, int Wid, cdouble* data, double* win)
xue@1: {
xue@1:   X=0;
xue@1:   int hWid=Wid/2;
xue@1:   for (int i=0; i<Wid; i++)
xue@1:   {
xue@1:     cdouble tmp=data[i]*win[Wid-i];
xue@1:     double ph=-2*M_PI*(i-hWid)*k/Wid;
xue@1:     tmp.rotate(ph);
xue@1:     X+=tmp;
xue@1:   }
xue@1: }//CDFTW
xue@1: 
Chris@5: /**
xue@1:   function CuDFTW: single-frequency windowed DTFT of t*data[t], centre-aligned
xue@1: 
xue@1:   In: data[Wid]: waveform data x
xue@1:       wid[Wid+1]: window function
xue@1:       k: frequency, in bins
xue@1:   Out: X: DTFT of txw at frequency k bins
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void CuDFTW(cdouble& X, int k, int Wid, cdouble* data, double* win)
xue@1: {
xue@1:   X=0;
xue@1:   int hWid=Wid/2;
xue@1:   for (int i=0; i<Wid; i++)
xue@1: 	{
xue@1: 		double tw=((i-hWid)*win[Wid-i]);
xue@1: 		cdouble tmp=data[i]*tw;
xue@1:     double ph=-2*M_PI*(i-hWid)*k/Wid;
xue@1:     tmp.rotate(ph);
xue@1: 		X+=tmp;
xue@1:   }
xue@1: }//CuDFTW
xue@1: 
Chris@5: /**
xue@1:   function TFReas: time-frequency reassignment
xue@1: 
xue@1:   In: data[Wid]: waveform data
xue@1:       win[Wid+1], dwin[Wid+1], ddwin[Wid+1]: window function and its derivatives
xue@1:       f, t: initial digital frequency and time
xue@1:   Out: f, t: reassigned digital frequency and time
xue@1:        fslope: estimate of frequency derivative
xue@1:        plogaslope[0]: estimate of the derivative of logarithmic amplitude, optional
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void TFReas(double& f, double& t, double& fslope, int Wid, cdouble* data, double* win, double* dwin, double* ddwin, double* plogaslope)
xue@1: {
xue@1: 	int fi=floor(f*Wid+0.5);
xue@1: 
xue@1: 	cdouble x, xt, xw;
xue@1:   CDFTW(x, fi, Wid, data, win);
xue@1:   CuDFTW(xw, fi, Wid, data, win); xt.x=xw.y; xw.y=-xw.x; xw.x=xt.x;
xue@1:   CDFTW(xt, fi, Wid, data, dwin);
xue@1:   double px=~x;
xue@1:   t=t-(xw.y*x.x-xw.x*x.y)/px;
xue@1: 	f=1.0*fi/Wid+(xt.y*x.x-xt.x*x.y)/px/(2*M_PI);
xue@1: 	if (plogaslope) plogaslope[0]=-(xt.x*x.x+xt.y*x.y)/px;
xue@1:   cdouble xtt, xtw;
xue@1:   CuDFTW(xtw, fi, Wid, data, dwin); xtt.x=xtw.y; xtw.y=-xtw.x; xtw.x=xtt.x;
xue@1:   CDFTW(xtt, fi, Wid, data, ddwin);
xue@1:   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,
xue@1:          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;
xue@1:   if (dtdt!=0) fslope=dwdt/dtdt/(2*M_PI);
xue@1:   else fslope=0;
xue@1: } //TFReas*/
xue@1: 
Chris@5: /**
xue@1:   function TFReas: sinusoid estimation using reassignment method
xue@1: 
xue@1:   In: data[Wid]: waveform data
xue@1:       w[Wid+1], dw[Wid+1], ddw[Wid+1]: window function and its derivatives
xue@1:       win[Wid]: window function used for estimating amplitude and phase by projection onto a chirp
xue@1:       t: time for which the parameters are estimated
xue@1:       f: initial frequency at t
xue@1:   Out: f, a, ph: digital frequency, amplitude and phase angle estimated at t
xue@1:        fslope: frequency derivative estimate
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void TFReas(double& f, double t, double& a, double& ph, double& fslope, int Wid, cdouble* data, double* w, double* dw, double* ddw, double* win)
xue@1: {
xue@1: 	double localt=t, logaslope;
xue@1: 	TFReas(f, localt, fslope, Wid, data, w, dw, ddw, &logaslope);
xue@1: 
xue@1: 	if (logaslope*Wid>6) logaslope=6.0/Wid;
xue@1: 	else if (logaslope*Wid<-6) logaslope=-6.0/Wid;
xue@1: 
xue@1: 	f=f+fslope*(t-localt); //obtain frequency estimate at t
xue@1: 
xue@1: 	cdouble x=0;
xue@1: 	if (win==0)
xue@1: 	{
xue@1: 		for (int n=0; n<Wid; n++)
xue@1: 		{
xue@1: 			double ni=n-t;
xue@1: 			cdouble tmp=data[n];
xue@1: 			double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
xue@1: 			tmp.rotate(p);
xue@1: 			x+=tmp;
xue@1: 		}
xue@1: 		a=abs(x)/Wid;
xue@1: 	}
xue@1: 	else
xue@1: 	{
xue@1: 		double sumwin=0;
xue@1: 		for (int n=0; n<Wid; n++)
xue@1: 		{
xue@1: 			double ni=n-t;
xue@1: 			cdouble tmp=data[n]*win[n];
xue@1: 			double p=-2*M_PI*(f+0.5*fslope*ni)*ni;
xue@1: 			tmp.rotate(p);
xue@1: 			x+=tmp; sumwin+=win[n];
xue@1: 		}
xue@1: 		a=abs(x)/sumwin;
xue@1: 	}
xue@1: 	ph=arg(x);
xue@1: }//TFReas
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Routines for additive and multiplicative reestimation of sinusoids.
xue@1: 
xue@1:     Further reading: Wen X. and M. Sandler, "Additive and multiplicative reestimation schemes
xue@1:     for the sinusoid modeling of audio," in Proc. EUSIPCO'09, Glasgow, 2009.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function AdditiveUpdate: additive reestimation of time-varying sinusoid
xue@1: 
xue@1:   In: x[Count]: waveform data
xue@1:       Wid, Offst: frame size and hop
xue@1:       fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
xue@1:       das[Count]: initial estimate of amplitude derivative
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
xue@1:   Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void AdditiveUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
xue@1: {
xue@1:   int HWid=Wid/2, Fr=(Count-Wid)/Offst+1;
xue@1: 
xue@1: 	for (int fr=0; fr<Fr; fr++)
xue@1: 	{
xue@1: 		int i=HWid+Offst*fr;
xue@1:     if (fs[i]<0 || fs[i]>0.5){}
xue@1: 	}
xue@1: 
xue@1: 	cdouble *y=new cdouble[Count];
xue@1: 	double *lf=new double[Count*4], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3];
xue@1: 
xue@1: 	__int16* ref=new __int16[Count];
xue@1: 	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);
xue@1: 	memcpy(lf, fs, sizeof(double)*Count);
xue@1: 	BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, ref, reserved, LogA);
xue@1: 
xue@1:   //merge and interpolate
xue@1:   double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
xue@1:        *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
xue@1:        *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
xue@1:   for (int fr=0; fr<Fr; fr++)
xue@1:   {
xue@1: 		int i=HWid+Offst*fr;
xue@1:     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];
xue@1: 		xs[fr]=i;
xue@1: 		if (fabs(f-g)*Wid>1)
xue@1:     {
xue@1:       afr[fr]=a, pfr[fr]=fai, ffr[fr]=f;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       double rr=a*cos(fai)+b*cos(thet);
xue@1:       double ii=a*sin(fai)+b*sin(thet);
xue@1: 			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));
xue@1:       afr[fr]=sqrt(rr*rr+ii*ii);
xue@1: 			pfr[fr]=atan2(ii, rr);
xue@1: 		}
xue@1: 		if (LogA) afr[fr]=log(afr[fr]);
xue@1: 	}
xue@1: 	CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
xue@1: 	CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
xue@7:   for (int fr=0; fr<Fr-1; fr++) Sinusoid(Offst, &fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
xue@7:   double tmpph=pfr[0]; Sinusoid_direct(&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], tmpph, LogA);
xue@7:   ShiftTrinomial(xs[Fr-1]-xs[Fr-2], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
xue@7:   ShiftTrinomial(xs[Fr-1]-xs[Fr-2], aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2]);
xue@7:   tmpph=pfr[Fr-1]; Sinusoid_direct(&fs[int(xs[Fr-1])], &as[int(xs[Fr-1])], &phs[int(xs[Fr-1])], &das[int(xs[Fr-1])], 0, HWid, aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph, LogA);
xue@1: 	delete[] fa;  //*/
xue@1: 	/*
xue@1:   for (int i=0; i<Count; i++)
xue@1:   {
xue@1:     double rr=as[i]*cos(phs[i])+la[i]*cos(lp[i]);
xue@1:     double ii=as[i]*sin(phs[i])+la[i]*sin(lp[i]);
xue@1: 		as[i]=sqrt(rr*rr+ii*ii);
xue@1:     phs[i]=atan2(ii, rr);
xue@1: 	}         //*/
xue@1: 	for (int fr=0; fr<Fr; fr++)
xue@1: 	{
xue@1: 		int i=HWid+Offst*fr;
xue@1:     if (fs[i]<0 || fs[i]>0.5){}
xue@1: 	}
xue@1: 	delete[] y; delete[] lf; delete[] ref;
xue@1: }//AdditiveUpdate
xue@1: 
Chris@5: /**
xue@1:   function AdditiveAnalyzer: sinusoid analyzer with one additive update
xue@1: 
xue@1:   In: x[Count]: waveform data
xue@1:       Wid, Offst: frame size and hop size
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
xue@1:   Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
xue@1:        das[Count]: estimate of amplitude derivative
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
xue@1: 	AdditiveUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved, LogA);
xue@1: }//AdditiveAnalyzer
xue@1: 
Chris@5: /**
xue@1:   function MultiplicativeUpdate: multiplicative reestimation of time-varying sinusoid
xue@1: 
xue@1:   In: x[Count]: waveform data
xue@1:       Wid, Offst: frame size and hop
xue@1:       fs[Count], as[Count], phs[Count]: initial estimate of sinusoid parameters
xue@1:       das[Count]: initial estimate of amplitude derivative
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
xue@1:   Out: fs[Count], as[Count], phs[Count], das[Count]: estimates after additive update
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void MultiplicativeUpdate(double* fs, double* as, double* phs, double* das, cdouble* x, int Count, int Wid, int Offst, TBasicAnalyzer BasicAnalyzer, int reserved, bool LogA)
xue@1: {
xue@1:   int HWid=Wid/2;
xue@1:   cdouble *y=new cdouble[Count];
xue@1:   double *lf=new double[Count*8], *la=&lf[Count], *lp=&lf[Count*2], *lda=&lf[Count*3],
xue@1:          *lf2=&lf[Count*4], *la2=&lf2[Count], *lp2=&lf2[Count*2], *lda2=&lf2[Count*3];
xue@1: 	__int16 *lref=new __int16[Count];
xue@1: 
xue@1: 	for (int i=0; i<Count; i++) y[i]=x[i]*(cdouble(1.0).rotate(-phs[i]+i*0.15*2*M_PI)),
xue@1: 														lref[i]=0.15*Wid;
xue@1: 	BasicAnalyzer(lf, la, lp, lda, y, Count, Wid, Offst, lref, reserved, LogA);
xue@1: 	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;
xue@1: 	BasicAnalyzer(lf2, la2, lp2, lda2, y, Count, Wid, Offst, lref, reserved, LogA);
xue@1: 
xue@1: 		/*
xue@1: 		for (int i=0; i<Count; i++)
xue@1: 		{
xue@1: 			as[i]=la[i]*la2[i];
xue@1: 			phs[i]=phs[i]+lp[i]+lp2[i]-0.3*2*M_PI*i;
xue@1:       fs[i]=fs[i]+lf[i]+lf2[i]-0.3;
xue@1: 		}         //*/
xue@1: 
xue@1: 	//merge
xue@1: 	int Fr=(Count-Wid)/Offst+1;
xue@1: 	double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
xue@1: 				 *aa=&fa[Fr*4], *ab=&aa[Fr], *ac=&aa[Fr*2], *ad=&aa[Fr*3],
xue@1: 				 *xs=&fa[Fr*8], *ffr=&xs[Fr], *afr=&xs[Fr*2], *pfr=&xs[Fr*3];
xue@1: 	for (int fr=0; fr<Fr; fr++)
xue@1: 	{
xue@1: 		int i=HWid+Offst*fr;
xue@1: 		xs[fr]=i;
xue@1: 		afr[fr]=la[i]*la2[i];
xue@1: 		if (LogA) afr[fr]=log(afr[fr]);
xue@1: 		ffr[fr]=fs[i]+lf[i]-0.15+lf2[i]-0.15;
xue@1: 		pfr[fr]=phs[i]+lp[i]+lp2[i]-0.3*i*2*M_PI;
xue@1: 	}
xue@1: 	CubicSpline(Fr-1, fa, fb, fc, fd, xs, ffr, 1, 1);
xue@1: 	CubicSpline(Fr-1, aa, ab, ac, ad, xs, afr, 1, 1);
xue@7:   for (int fr=0; fr<Fr-1; fr++) Sinusoid(Offst, &fs[int(xs[fr])], &as[int(xs[fr])], &phs[int(xs[fr])], &das[int(xs[fr])], aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], pfr[fr], pfr[fr+1], LogA);
xue@7:   double tmpph=pfr[0]; Sinusoid_direct(&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], tmpph, LogA);
xue@7:   ShiftTrinomial(xs[Fr-1]-xs[Fr-2], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
xue@7:   ShiftTrinomial(xs[Fr-1]-xs[Fr-2], aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], aa[Fr-2], ab[Fr-2], ac[Fr-2], ad[Fr-2]);
xue@7:   tmpph=pfr[Fr-1]; Sinusoid_direct(&fs[int(xs[Fr-1])], &as[int(xs[Fr-1])], &phs[int(xs[Fr-1])], &das[int(xs[Fr-1])], 0, HWid, aa[Fr-1], ab[Fr-1], ac[Fr-1], ad[Fr-1], fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph, LogA);
xue@7: 
xue@7:   delete[] fa;  //*/
xue@1: 
xue@1: 	for (int fr=0; fr<Fr; fr++)
xue@1: 	{
xue@1: 		int i=HWid+Offst*fr;
xue@1:     if (fs[i]<0 || fs[i]>0.5){}
xue@1: 	}
xue@1: 
xue@1: 	delete[] y; delete[] lf; delete[] lref;
xue@1: }//MultiplicativeUpdate
xue@1: 
Chris@5: /**
xue@1:   function MultiplicativeAnalyzer: sinusoid analyzer with one multiplicative update
xue@1: 
xue@1:   In: x[Count]: waveform data
xue@1:       Wid, Offst: frame size and hop size
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       ref[Count]: reference frequencies, in bins, used by BasicAnalyzer
xue@1:       BasicAnalyzer: pointer to a sinusoid analyzer
xue@1:       LogA: indicates if amplitudes are interpolated at cubic spline or exponential cubic spline
xue@1:   Out: fs[Count], as[Count], phs[Count]: sinusoid parameter estimates
xue@1:        das[Count]: estimate of amplitude derivative
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	BasicAnalyzer(fs, as, phs, das, x, Count, Wid, Offst, ref, reserved, LogA);
xue@1: 	MultiplicativeUpdate(fs, as, phs, das, x, Count, Wid, Offst, BasicAnalyzer, reserved);
xue@1: }//MultiplicativeAnalyzer
xue@1: 
xue@1: /*
xue@1:   This is an earlier version of the multiplicative method without using a user-provided BasicAnalyzer.
xue@1:   This updates the sinusoid estimates at the selected consecutive FRAMES of x. Only frequency modulation
xue@1:   is included in the multiplier. The first frame (0) is centred at x[Wid/2]. fs, as, and phs are based
xue@1:   on frames rather than samples. Updates include frame frst, but not frame fren.
xue@1: */
xue@1: void MultiplicativeUpdateF(double* fs, double* as, double* phs, __int16* x, int Fr, int frst, int fren, int Wid, int Offst)
xue@1: {
xue@1:   int HWid=Wid/2;
xue@1: 
xue@1:   double *fa=new double[Fr*12], *fb=&fa[Fr], *fc=&fa[Fr*2], *fd=&fa[Fr*3],
xue@1:          *xs=&fa[Fr*8];
xue@1:   for (int fr=0; fr<Fr; fr++) xs[fr]=HWid+Offst*fr;
xue@1:   CubicSpline(Fr-1, fa, fb, fc, fd, xs, fs, 1, 1);
xue@1: 
xue@1:   int dst=Offst*frst, den=Offst*(fren-1)+Wid, dcount=den-dst;
xue@1:   double *f=new double[dcount*2], *ph=&f[dcount];
xue@7:   for (int fr=frst; fr<fren-1; fr++) Sinusoid(Offst, &f[int(xs[fr])-dst], &ph[int(xs[fr])-dst], fa[fr], fb[fr], fc[fr], fd[fr], phs[fr], phs[fr+1]);
xue@7:   if (frst==0)
xue@7:   {
xue@7:     double tmpph=phs[0];
xue@7:     Sinusoid_direct(&f[int(xs[0])-dst], &ph[int(xs[0])-dst], -HWid, 0, fa[0], fb[0], fc[0], fd[0], tmpph);
xue@7:   }
xue@7:   else
xue@7:     Sinusoid(Offst, &f[int(xs[frst-1])-dst], &ph[int(xs[frst-1])-dst], fa[frst-1], fb[frst-1], fc[frst-1], fd[frst-1], phs[frst-1], phs[frst]);
xue@7:   if (fren==Fr)
xue@7:   {
xue@7:     double tmpph=phs[Fr-1];
xue@7:     ShiftTrinomial(Offst, fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], fa[Fr-2], fb[Fr-2], fc[Fr-2], fd[Fr-2]);
xue@7:     Sinusoid_direct(&f[int(xs[Fr-1])-dst], &ph[int(xs[Fr-1])-dst], 0, HWid, fa[Fr-1], fb[Fr-1], fc[Fr-1], fd[Fr-1], tmpph);
xue@7:   }
xue@7:   else
xue@7:     Sinusoid(Offst, &f[int(xs[fren-1])-dst], &ph[int(xs[fren-1])-dst], fa[fren-1], fb[fren-1], fc[fren-1], fd[fren-1], phs[fren-1], phs[fren]);
xue@1: 
xue@1:   cdouble* y=new cdouble[Wid];
xue@1:   AllocateFFTBuffer(Wid, Amp, W, X);
xue@1:   double* win=NewWindow(wtHann, Wid);
xue@1:   int M; double c[10], iH2; windowspec(wtHann, Wid, &M, c, &iH2);
xue@1:   for (int fr=frst; fr<fren; fr++)
xue@1:   {
xue@1:     __int16* lx=&x[Offst*fr];
xue@1:     double* lph=&ph[Offst*(fr-frst)];
xue@1:     for (int i=0; i<Wid; i++) y[i]=cdouble(lx[i]).rotate(-lph[i]+i*0.15*2*M_PI);
xue@11:     CFFTCW(y, win, Amp, 0, Log2(Wid), W, X);
xue@1:     int pf=0.15*Wid, mpf=pf;
xue@1:     for (int k=pf-4; k<=pf+4; k++) if (Amp[k]>Amp[mpf]) mpf=k;
xue@1:     if (mpf>pf-4 && mpf<pf+4) pf=mpf;
xue@1:     double lfs=pf, lphs;
xue@1:     LSESinusoid(lfs, pf-3, pf+3, X, Wid, 3, M, c, iH2, as[fr], lphs, 1e-3);
xue@1:     fs[fr]=fs[fr]+lfs/Wid-0.15;
xue@1:     phs[fr]+=lphs-0.15*Wid*M_PI;
xue@1:       as[fr]*=2;
xue@1:   }
xue@1: 
xue@1:   delete[] y;
xue@1:   delete[] f;
xue@1:   delete[] win;
xue@1:   delete[] fa;
xue@1:   FreeFFTBuffer(Amp);
xue@1: }//MultiplicativeUpdateF
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Earlier reestimation method routines.
xue@1: 
xue@1:     Further reading: Wen X. and M. Sandler, "Evaluating parameters of time-varying
xue@1:     sinusoids by demodulation," in Proc. DAFx'08, Espoo, 2008.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function ReEstFreq: sinusoid reestimation by demodulating frequency.
xue@1: 
xue@1:   In: x[Wid+Offst*(FrCount-1)]: waveform data
xue@1:       FrCount, Wid, Offst: frame count, frame size and hop size
xue@1:       fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
xue@1:       win[Wid]: window function for estimating demodulated sinusoid
xue@1:       M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
xue@1:       Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid, optional
xue@1:       w[Wid/2], ps[Wid], xs[Wid], xc[Wid], fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
xue@1:   Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   int hWid=Wid/2;
xue@1:   //reestimate using frequency track
xue@1:   CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
xue@1:   for (int fr=0; fr<FrCount; fr++)
xue@1:   {
xue@1:     //find ps
xue@1:     if (fr==0)
xue@1:     {
xue@1:       double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0];
xue@1:       for (int j=0; j<Wid; j++)
xue@1:       {
xue@1:          double lx=j-hWid;
xue@1:          ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
xue@1:       }
xue@1: //      memset(ps, 0, sizeof(double)*hWid);
xue@1:     }
xue@1:     else if (fr==FrCount-1)
xue@1:     {
xue@1:       int lfr=FrCount-2;
xue@1:       double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
xue@1:       ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
xue@1:       for (int j=1; j<Wid; j++)
xue@1:       {
xue@1:         ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:       }
xue@1: //        memset(&ps[hWid], 0, sizeof(double)*hWid);
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int lfr=fr-1;
xue@1:       double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
xue@1:       for (int j=1; j<hWid+1; j++)
xue@1:       {
xue@1:         ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:       }
xue@1:       lfr=fr;
xue@1:       lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       for (int j=1; j<hWid; j++)
xue@1:       {
xue@1:         ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:       }
xue@1:     }
xue@1:     double* ldata=&x[fr*Offst];
xue@1:     for (int j=0; j<Wid; j++)
xue@1:     {
xue@1:       xs[j].x=ldata[j]*cos(-ps[j]);
xue@1:       xs[j].y=ldata[j]*sin(-ps[j]);
xue@1:     }
xue@1: 
xue@1:     if (Wids)
xue@1:     {
xue@1:       int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
xue@1:       SetTwiddleFactors(lWid, w);
xue@1:       double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
xue@1:       windowspec(wtHann, lWid, &lM, lc, &liH2);
xue@11:       CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, Log2(lWid), w, xc);
xue@1:       delete[] lwin;
xue@1:       double lf=fbuf[fr]*lWid, la, lp;
xue@1:       LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
xue@1:       if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
xue@1:     }
xue@1:     else
xue@1:     {
xue@11:       CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
xue@1:       double lf=fbuf[fr]*Wid, la, lp;
xue@1:       LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
xue@1:       if (la*2>abuf[fr])
xue@1:         fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
xue@1:     }
xue@1:   }
xue@1: }//ReEstFreq
xue@1: 
Chris@5: /**
xue@1:   function ReEstFreq_2: sinusoid reestimation by demodulating frequency. This is that same as ReEstFreq(...)
xue@1:   except that it calls Sinusoid(...) to synthesize the phase track used for demodulation and that it
xue@1:   does not allow variable window sizes for estimating demodulated sinusoid.
xue@1: 
xue@1:   In: x[Wid+Offst*(FrCount-1)]: waveform data
xue@1:       FrCount, Wid, Offst: frame count, frame size and hop size
xue@1:       fbuf[FrCount], ns[FrCount]: initial frequency estiamtes and their timing
xue@1:       win[Wid]: window function for LSE sinusoid estimation
xue@1:       M, c[], iH2: cosine-family window specification parameters, must be consistent with M, c, iH2
xue@1:       w[Wid/2], xs[Wid], xc[Wid], f3[FrCount-1], f2[FrCount-1], f1[FrCount-1], f0[FrCount-1]: buffers
xue@1:   Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   int hWid=Wid/2;
xue@1:   //reestimate using frequency track
xue@1:   CubicSpline(FrCount-1, f3, f2, f1, f0, ns, fbuf, 1, 1);
xue@1:   double *refcos=(double*)malloc8(sizeof(double)*Wid), *refsin=&refcos[hWid], ph=0, centralph;
xue@1: 
xue@1:   memset(f0, 0, sizeof(double)*FrCount);
xue@1: 
xue@1:     int N=Wid+Offst*(FrCount-1);
xue@1:     double* cosine=new double[N], *sine=new double[N];
xue@7:     CosSin(&cosine[hWid], &sine[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
xue@1:     for (int fr=0; fr<FrCount-1; fr++)
xue@1:     {
xue@1:       int ncentre=hWid+Offst*fr;
xue@7:       if (fr==FrCount-2) CosSin(&cosine[ncentre], &sine[ncentre], 0, Wid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
xue@7:       else CosSin(&cosine[ncentre], &sine[ncentre], 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
xue@1:     }
xue@1:     double err=0;
xue@1:     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;}
xue@1: 
xue@1:   ph=0;
xue@1:   for (int fr=0; fr<FrCount; fr++)
xue@1:   {
xue@1:     double* ldata=&x[fr*Offst-hWid];
xue@1: 
xue@1:     //store first half of demodulated frame to xs[0:hWid-1]
xue@1:     if (fr==0)
xue@1:     {
xue@7:       CosSin(&refcos[hWid], &refsin[hWid], -hWid, 0, f3[0], f2[0], f1[0], f0[0], ph);
xue@1:       for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       ph=0;
xue@7:       CosSin(refcos, refsin, 0, hWid, f3[fr-1], f2[fr-1], f1[fr-1], f0[fr-1], ph);
xue@1:       for (int i=0; i<hWid; i++) xs[i].x=ldata[i]*refcos[i], xs[i].y=-ldata[i]*refsin[i];
xue@1:     }
xue@1: 
xue@1:     //taking care of phase angles
xue@1:     if (fr==FrCount-1) {double tmp=ph; ph=centralph; centralph=tmp;}
xue@1:     else centralph=ph;
xue@1: 
xue@1:     double *lrefcos=&refcos[-hWid], *lrefsin=&refsin[-hWid];
xue@1:     //store second half of demodulated frame to xs[hWid:Wid-1]
xue@1:     if (fr==FrCount-1)
xue@1:     {
xue@7:       CosSin(lrefcos, lrefsin, hWid, Wid, f3[FrCount-2], f2[FrCount-2], f1[FrCount-2], f0[FrCount-2], ph);
xue@1:       for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
xue@1:     }
xue@1:     else
xue@1:     {
xue@7:       CosSin(refcos, refsin, 0, hWid, f3[fr], f2[fr], f1[fr], f0[fr], ph);
xue@1:       for (int i=hWid; i<Wid; i++) xs[i].x=ldata[i]*lrefcos[i], xs[i].y=-ldata[i]*lrefsin[i];
xue@1:     }
xue@1: 
xue@11:     CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
xue@1:     double lf=fbuf[fr]*Wid, la, lp;
xue@1:     LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
xue@1:     if (la*2>abuf[fr])
xue@1:       fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp+centralph;
xue@1:   }
xue@1: }//ReEstFreq_2
xue@1: 
Chris@5: /**
xue@1:   function ReEstFreqAmp: sinusoid reestimation by demodulating frequency and amplitude.
xue@1: 
xue@1:   In: x[Wid+Offst*(FrCount-1)]: waveform data
xue@1:       FrCount, Wid, Offst: frame count, frame size and hop size
xue@1:       fbuf[FrCount], abuf[FrCount], ns[FrCount]: initial frequency and amplitude estiamtes and their
xue@1:         timing
xue@1:       win[Wid]: window function for estimating demodulated sinusoid
xue@1:       M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
xue@1:       Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
xue@1:         optional
xue@1:       w[Wid/2], ps[Wid], xs[Wid], xc[Wid]: buffers
xue@1:       fa[FrCount-1], fb[FrCount-1], fc[FrCount-1], fd[FrCount-1]: buffers
xue@1:       aa[FrCount-1], ab[FrCount-1], ac[FrCount-1], ad[FrCount-1]: buffers
xue@1:   Out: fbuf[FrCount], abuf[FrCount], pbuf[FrCount]: reestimated frequencies, amplitudes and phase angles
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   int hWid=Wid/2;
xue@1:   //reestimate using amplitude and frequency track
xue@1:   CubicSpline(FrCount-1, fa, fb, fc, fd, ns, fbuf, 0, 1);
xue@1:   CubicSpline(FrCount-1, aa, ab, ac, ad, ns, abuf, 0, 1);
xue@1:   for (int fr=0; fr<FrCount; fr++)
xue@1:   {
xue@1:     if (fr==0)
xue@1:     {
xue@1:       double lfd=0, lfc=fc[0], lfb=fb[0], lfa=fa[0],
xue@1:              lad=ad[0], lac=ac[0], lab=ab[0], laa=aa[0];
xue@1:       for (int j=0; j<Wid; j++)
xue@1:       {
xue@1:         double lx=j-hWid;
xue@1:         ps[j]=2*M_PI*lx*(lfd+lx*(lfc/2+lx*(lfb/3+lx*lfa/4)));
xue@1:       }
xue@1:       for (int j=0; j<Wid; j++)
xue@1:       {
xue@1:         double lx=j-hWid;
xue@1:         as[j]=lad+lx*(lac+lx*(lab+lx*laa));
xue@1:       }
xue@1:     }
xue@1:     else if (fr==FrCount-1)
xue@1:     {
xue@1:       int lfr=FrCount-2;
xue@1:       double lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       double lfd=-(hWid*(lfc+hWid*(lfb+hWid*lfa)));
xue@1:       double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
xue@1:       ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
xue@1:       for (int j=1; j<Wid; j++)
xue@1:       {
xue@1:         ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:       }
xue@1:       as[0]=ad[lfr];
xue@1:       for (int j=0; j<Wid; j++)
xue@1:       {
xue@1:         as[j]=lad+j*(lac+j*(lab+j*laa));
xue@1:       }
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       int lfr=fr-1;
xue@1:       double lfd=fd[lfr]-fd[fr], lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       double lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
xue@1:       ps[0]=-2*M_PI*hWid*(lfd+hWid*(lfc/2+hWid*(lfb/3+hWid*lfa/4)));
xue@1:       for (int j=0; j<hWid+1; j++)
xue@1:       {
xue@1:         ps[j]=ps[0]+2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:         as[j]=lad+j*(lac+j*(lab+j*laa));
xue@1:       }
xue@1:       lfr=fr;
xue@1:       lfd=0, lfc=fc[lfr], lfb=fb[lfr], lfa=fa[lfr];
xue@1:       lad=ad[lfr], lac=ac[lfr], lab=ab[lfr], laa=aa[lfr];
xue@1:       for (int j=1; j<hWid; j++)
xue@1:       {
xue@1:         ps[j+hWid]=2*M_PI*j*(lfd+j*(lfc/2+j*(lfb/3+j*lfa/4)));
xue@1:         as[j+hWid]=lad+j*(lac+j*(lab+j*laa));
xue@1:       }
xue@1:     }
xue@1:     double *ldata=&x[fr*Offst];
xue@1:     for (int j=0; j<Wid; j++)
xue@1:     {
xue@1:       double tmp;
xue@1:       if ((fr==0 && j<hWid) || (fr==FrCount-1 && j>=hWid)) tmp=1;
xue@1:       else if (as[hWid]>100*as[j]) tmp=100;
xue@1:       else tmp=as[hWid]/as[j];
xue@1:       tmp=tmp*ldata[j];
xue@1:       xs[j].x=tmp*cos(-ps[j]);
xue@1:       xs[j].y=tmp*sin(-ps[j]);
xue@1:     }
xue@1: 
xue@1:     if (Wids)
xue@1:     {
xue@1:       int lWid=Wids[fr], lhWid=Wids[fr]/2, lM;
xue@1:       SetTwiddleFactors(lWid, w);
xue@1:       double *lwin=NewWindow(wtHann, lWid), lc[4], liH2;
xue@1:       windowspec(wtHann, lWid, &lM, lc, &liH2);
xue@11:       CFFTCW(&xs[hWid-lhWid], lwin, NULL, NULL, Log2(lWid), w, xc);
xue@1:       delete[] lwin;
xue@1:       double lf=fbuf[fr]*lWid, la, lp;
xue@1:       LSESinusoid(lf, lf-3, lf+3, xc, lWid, 3, lM, lc, liH2, la, lp, 1e-3);
xue@1:       if (la*2>abuf[fr]) fbuf[fr]=lf/lWid, abuf[fr]=la*2, pbuf[fr]=lp;
xue@1:     }
xue@1:     else
xue@1:     {
xue@11:       CFFTCW(xs, win, NULL, NULL, Log2(Wid), w, xc);
xue@1:       double lf=fbuf[fr]*Wid, la, lp;
xue@1:       LSESinusoid(lf, lf-3, lf+3, xc, Wid, 3, M, c, iH2, la, lp, 1e-3);
xue@1:       if (la*2>abuf[fr]) fbuf[fr]=lf/Wid, abuf[fr]=la*2, pbuf[fr]=lp;
xue@1:     }
xue@1:   }
xue@1: }//ReEstFreqAmp
xue@1: 
Chris@5: /**
xue@1:   function Reestimate2: iterative demodulation method for sinusoid parameter reestimation.
xue@1: 
xue@1:   In: x[(FrCount-1)*Offst+Wid]: waveform data
xue@1:       FrCount, Wid, Offst: frame count, frame size and hop size
xue@1:       win[Wid]: window function
xue@1:       M, c[], iH2: cosine-family window specification parameters, must be consistent with win[]
xue@1:       Wids[FrCount]: specifies frame sizes for estimating individual frames of demodulated sinusoid,
xue@1:         optional
xue@1:       maxiter: maximal number of iterates
xue@1:       ae[FrCount], fe[FrCount], pe[FrCount]: initial amplitude, frequency and phase estimates
xue@1:   Out: aret[FrCount], fret[FrCount], pret[FrCount]: reestimated amplitudes, frequencies and phase angles
xue@1: 
xue@1:   Returns the number of unused iterates left of the total of maxiter.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   AllocateFFTBuffer(Wid, fft, w, xc);
xue@1:   double convep=1e-4, dif=0, lastdif=0; //convep is the hard-coded threshold that stops the iteration
xue@1:   int iter=1, hWid=Wid/2;
xue@1: 
xue@1:   double *ns=new double[FrCount*12], *as=new double[Wid*5];
xue@1:   double *fbuf=&ns[FrCount], *abuf=&ns[FrCount*2],
xue@1:          *aa=&ns[FrCount*3], *ab=&ns[FrCount*4], *ac=&ns[FrCount*5], *ad=&ns[FrCount*6],
xue@1:          *fa=&ns[FrCount*7], *fb=&ns[FrCount*8], *fc=&ns[FrCount*9], *fd=&ns[FrCount*10],
xue@1:          *pbuf=&ns[FrCount*11];
xue@1:   double *ps=&as[Wid];
xue@1:   cdouble *xs=(cdouble*)&as[Wid*3];
xue@1: 
xue@1:   memcpy(fbuf, fe, sizeof(double)*FrCount);
xue@1:   memcpy(abuf, ae, sizeof(double)*FrCount);
xue@1:   memcpy(pbuf, pe, sizeof(double)*FrCount);
xue@1:   for (int i=0; i<FrCount; i++)
xue@1:   {
xue@1:     ns[i]=hWid+i*Offst;
xue@1:   }
xue@1: 
xue@1:   while (iter<=maxiter)
xue@1:   {
xue@1:     ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
xue@1:     ReEstFreq(FrCount, Wid, Offst, x, fbuf, abuf, pbuf, win, M, c, iH2, w, xc, xs, ps, fa, fb, fc, fd, ns, Wids);
xue@1:     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);
xue@1: 
xue@1:     if (iter>1) lastdif=dif;
xue@1:     dif=0;
xue@1:     if (iter==1)
xue@1:     {
xue@1:       for (int fr=0; fr<FrCount; fr++)
xue@1:       {
xue@1:         if (fabs(abuf[fr])>fabs(ae[fr]))
xue@1:           dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/abuf[fr]);
xue@1:         else
xue@1:           dif+=fabs(fe[fr]-fbuf[fr])*Wid+fabs((ae[fr]-abuf[fr])/ae[fr]);
xue@1:       }
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       for (int fr=0; fr<FrCount; fr++)
xue@1:       {
xue@1:         if (fabs(abuf[fr])>fabs(aret[fr]))
xue@1:           dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/abuf[fr]);
xue@1:         else
xue@1:           dif+=fabs(fret[fr]-fbuf[fr])*Wid+fabs((aret[fr]-abuf[fr])/aret[fr]);
xue@1:       }
xue@1:     }
xue@1:     memcpy(fret, fbuf, sizeof(double)*FrCount);
xue@1:     memcpy(aret, abuf, sizeof(double)*FrCount);
xue@1:     dif/=FrCount;
xue@1:     if (fabs(dif)<convep || (iter>1 && fabs(dif-lastdif)<convep*lastdif)) break;
xue@1:     iter++;
xue@1:   }
xue@1: 
xue@1:   memcpy(pret, pbuf, sizeof(double)*FrCount);
xue@1: 
xue@1:   delete[] ns;
xue@1:   delete[] as;
xue@1:   delete[] fft;
xue@1: 
xue@1:   return maxiter-iter;
xue@1: }//Reestimate2
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Derivative method as proposed in DAFx09
xue@1: 
xue@1:     Further reading: Wen X. and M. Sandler, "Notes on model-based non-stationary sinusoid estimation methods
xue@1:     using derivatives," in Proc. DAFx'09, Como, 2009.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function Derivative: derivative method for estimating amplitude derivative, frequency, and frequency derivative given
xue@1:   signal and its derivatives.
xue@1: 
xue@1:   In: x[Wid], dx[Wid], ddx[Wid]: waveform and its derivatives
xue@1:       win[Wid]: window function
xue@1:       f0: initial digital frequency estimate
xue@1:   Out: f0: new estimate of digital frequency
xue@1:        f1, a1: estimates of frequency and amplitude derivatives
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void Derivative(int Wid, double* win, cdouble* x, cdouble* dx, cdouble* ddx, double& f0, double* f1, double* a0, double* a1, double* ph)
xue@1: {
xue@1:   AllocateFFTBuffer(Wid, fft, W, X);
xue@11:   CFFTCW(x, win, fft, NULL, Log2(Wid), W, X);
xue@1:   int m=f0*Wid, m0=m-10, m1=m+10, hWid=Wid/2;
xue@1:   if (m0<0) m0=0; if (m1>hWid) m1=hWid;
xue@1:   for (int n=m0; n<=m1; n++) if (fft[n]>fft[m]) m=n;
xue@1:   cdouble Sw=0, S1w=0, S2w=0;
xue@1:   for (int n=0; n<Wid; n++)
xue@1:   {
xue@1:     cdouble tmp=x[n]*win[n];
xue@1:     Sw+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
xue@1:     tmp=dx[n]*win[n];
xue@1:     S1w+=tmp.rotate(-2*M_PI*m*(n-hWid)/Wid);
xue@1:   }
xue@1:   double omg0=(S1w/Sw).y;
xue@1:   Sw=0, S1w=0;
xue@1:   for (int n=0; n<Wid; n++)
xue@1:   {
xue@1:     cdouble tmp=x[n]*win[n];
xue@1:     Sw+=tmp.rotate(-omg0*(n-hWid)/Wid);
xue@1:     tmp=dx[n]*win[n];
xue@1:     S1w+=tmp.rotate(-omg0*(n-hWid)/Wid);
xue@1:     tmp=ddx[n]*win[n];
xue@1:     S2w+=tmp.rotate(-omg0*(n-hWid)/Wid);
xue@1:   }
xue@1:   omg0=(S1w/Sw).y;
xue@1:   double miu0=(S1w/Sw).x;
xue@1:   double psi0=(S2w/Sw).y-2*miu0*omg0;
xue@1: 
xue@1:   f0=omg0/(2*M_PI);
xue@1:   *f1=psi0/(2*M_PI);
xue@1:   *a1=miu0;
xue@1: 
xue@1:   FreeFFTBuffer(fft);
xue@1: }//Derivative
xue@1: 
Chris@5: /**
xue@1:   function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency omg,
xue@1:   from x using window w and its derivatives.
xue@1: 
xue@1:   In: x[Wid]: waveform data
xue@1:       w[K+1][Wid]: window functions and its derivatives up to order K
xue@1:       omg: angular frequency
xue@1:   Out: X[K+1]: windowed spectrum and its derivatives up to order K
xue@1: 
xue@1:   No return value. This function is for internal use.
xue@1: */
xue@1: void Xkw(cdouble* X, int K, int Wid, double* x, double** w, double omg)
xue@1: {
xue@1:   int hWid=Wid/2;
xue@1:   //calculate the first row
xue@1:   memset(X, 0, sizeof(cdouble)*(K+1));
xue@1:   for (int i=0; i<Wid; i++)
xue@1:   {
xue@1:     double n=i-hWid;
xue@1:     double ph=omg*n;
xue@1:     for (int k=0; k<=K; k++)
xue@1:     {
xue@1:       cdouble tmp=x[i]*w[k][i];
xue@1:       X[k]+=tmp.rotate(-ph);
xue@1:     }
xue@1:   }
xue@1:   //calculate the rest rows
xue@1:   for (int k=1; k<=K; k++)
xue@1:   {
xue@1:     cdouble *thisX=&X[k], *lastX=&X[k-1];
xue@1: 			for (int kk=K-k; kk>=0; kk--) thisX[kk]=-lastX[kk+1]+cdouble(0, omg)*lastX[kk];
xue@1:   }
xue@1: }//Xkw
xue@1: 
Chris@5: /**
xue@1:   function Xkw: computes windowed spectrum of x and its derivatives up to order K at angular frequency
xue@1:   omg, from x and its derivatives using window w.
xue@1: 
xue@1:   In: x[K+1][Wid]: waveform data and its derivatives up to order K.
xue@1:       w[Wid]: window function
xue@1:       omg: angular frequency
xue@1:   Out: X[K+1]: windowed spectrum and its derivatives up to order K
xue@1: 
xue@1:   No return value. This function is for testing only.
xue@1: */
xue@1: void Xkw(cdouble* X, int K, int Wid, double** x, double* w, double omg)
xue@1: {
xue@1:   int hWid=Wid/2;
xue@1:   memset(X, 0, sizeof(cdouble)*(K+1));
xue@1:   for (int i=0; i<Wid; i++)
xue@1:   {
xue@1:     double n=i-hWid;
xue@1:     double ph=omg*n;
xue@1:     for (int k=0; k<=K; k++)
xue@1:     {
xue@1:       cdouble tmp=x[k][i]*w[i];
xue@1:       X[k]+=tmp.rotate(-ph);
xue@1:     }
xue@1:   }
xue@1: }//Xkw
xue@1: 
Chris@5: /**
xue@1:   function Derivative: derivative method for estimating the model log(s)=h[M]'r[M], by discarding extra
xue@1:   equations
xue@1: 
xue@1:   In: s[Wid]: waveform data
xue@1:       win[][Wid]: window function and its derivatives
xue@1:       h[M], dh[M]: pointers to basis functions and their derivatives
xue@1:       harg: pointer argument to be used by calls to functions in h[] amd dh[].
xue@1:       p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
xue@1:       q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
xue@1:       omg: initial angular frequency
xue@1:   Out: r[M]: estimated coefficients to h[M].
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	int hWid=Wid/2, M1=M-1;
xue@1: 	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
xue@1: 	int Kc=ceil(Kr/2.0); //number of derivatives required
xue@1: 
xue@1: 	//ind marks the 2*M1 real elements of an M1-array of complex unknowns with
xue@1: 	//  numerical indices (0-based) or -1 if it is not a real unknown variable
xue@1: 	//uind marks the Kr real unknowns with their positions in ind
xue@1: 	int *uind=new int[Kr], *ind=new int[2*M1];
xue@1: 	memset(ind, 0, sizeof(int)*2*M1);
xue@1: 	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
xue@1: 	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
xue@1: 	{
xue@1: 		int p=0, up=0;
xue@1: 		while (p<2*M1)
xue@1: 		{
xue@1: 			if (ind[p]>=0)
xue@1: 			{
xue@1: 				uind[up]=p;
xue@1: 				ind[p]=up;
xue@1: 				up++;
xue@1: 			}
xue@1: 			p++;
xue@1: 		}
xue@1: 		if (up!=Kr) throw("");
xue@1: 	}
xue@1: 
xue@1: 	cdouble* Skw=new cdouble[M];
xue@1: 	Xkw(Skw, Kc, Wid, s, win, omg);
xue@1: 
xue@1: 	double* x=new double[Wid];
xue@1: 	cdouble** Allocate2(cdouble, M, Kc, Smkw);
xue@1: 	for (int m=1; m<M; m++)
xue@1: 	{
xue@1: 		for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
xue@1: 		Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
xue@1: 	}
xue@1: 
xue@1: 	//allocate buffer for linear system A(pq)=b
xue@1: 	Alloc2(2*Kc+2, Kr, A); double** AA; double *bb, *pqpq;
xue@1: 	double *b=A[2*Kc], *pq=A[2*Kc+1];
xue@1: 	for (int k=0; k<Kr; k++) b[k]=((double*)(&Skw[1]))[k];
xue@1: 	//		*pq=(double*)(&r[1]);
xue@1: 	for (int k=0; k<Kc; k++) //looping through rows of A
xue@1: 	{
xue@1:     //columns of A includes rows of Smkw corresponding to real unknowns
xue@1: 		for (int m=0; m<M1; m++)
xue@1: 		{
xue@1: 			int lind;
xue@1: 			if ((lind=ind[2*m])>=0) //the real part being unknown
xue@1: 			{
xue@1: 				A[2*k][lind]=Smkw[m+1][k].x;
xue@1: 				A[2*k+1][lind]=Smkw[m+1][k].y;
xue@1: 			}
xue@1: 			if ((lind=ind[2*m+1])>=0) //the imag part being unknown
xue@1: 			{
xue@1: 				A[2*k+1][lind]=Smkw[m+1][k].x;
xue@1: 				A[2*k][lind]=-Smkw[m+1][k].y;
xue@1: 			}
xue@1: 		}
xue@1: 	}
xue@1:   
xue@1: 	bool dropeq=(2*Kc-1==Kr);
xue@1: 	if (dropeq)
xue@1: 	{
xue@1: 		Allocate2(double, Kr+2, Kr, AA);
xue@1: 		bb=AA[Kr], pqpq=AA[Kr+1];
xue@1: 		memcpy(AA[0], A[0], sizeof(double)*Kr*(Kr-1));
xue@1: 		memcpy(AA[Kr-1], A[Kr], sizeof(double)*Kr);
xue@1: 		memcpy(bb, b, sizeof(double)*(Kr-1));
xue@1: 		bb[Kr-1]=((double*)(&Skw[1]))[Kr];
xue@1: 	}
xue@1: 
xue@1: 	double det;
xue@1: 	GECP(Kr, pq, A, b, &det);
xue@1: 	if (dropeq)
xue@1: 	{
xue@1: 		double det2;
xue@1: 		GECP(Kr, pqpq, AA, bb, &det2);
xue@1: 		if (fabs(det2)>fabs(det)) memcpy(pq, pqpq, sizeof(double)*Kr);
xue@1: 		DeAlloc2(AA);
xue@1: 	}
xue@1: 	memset(&r[1], 0, sizeof(double)*M1*2);
xue@1: 	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
xue@1: 
xue@1: 	//estiamte r0
xue@1: 	cdouble e0=0;
xue@1: 	for (int i=0; i<Wid; i++)
xue@1: 	{
xue@1: 		cdouble expo=0;
xue@1: 		double n=i-hWid;
xue@1: 		for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
xue@1: 		cdouble tmp=exp(expo)*win[0][i];
xue@1: 		e0+=tmp.rotate(-omg*n);
xue@1: 	}
xue@1: 	r[0]=log(Skw[0]/e0);
xue@1: 
xue@1: 	delete[] x;
xue@1: 	delete[] Skw;
xue@1: 	delete[] uind;
xue@1: 	delete[] ind;
xue@1: 	DeAlloc2(Smkw);
xue@1: 	DeAlloc2(A);
xue@1: }//Derivative*/
xue@1: 
Chris@5: /**
xue@1:   function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M], least-square
xue@1:   implementation
xue@1: 
xue@1:   In: s[Wid]: waveform data
xue@1:       win[][Wid]: window function and its derivatives
xue@1:       h[M], dh[M]: pointers to basis functions and their derivatives
xue@1:       harg: pointer argument to be used by calls to functions in h[] amd dh[].
xue@1:       K: number of derivatives to take
xue@1:       p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
xue@1:       q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
xue@1:       omg: initial angular frequency
xue@1:   Out: r[M]: estimated coefficients to h[M].
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	int hWid=Wid/2, M1=M-1;
xue@1: 	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
xue@1: 	int Kc=ceil(Kr/2.0); //number of derivatives required
xue@1: 	if (Kc<K) Kc=K;
xue@1: 
xue@1: 	int *uind=new int[Kr], *ind=new int[2*M1];
xue@1: 	memset(ind, 0, sizeof(int)*2*M1);
xue@1: 	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
xue@1: 	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
xue@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("");}
xue@1: 
xue@1: 	//allocate buffer for linear system A(pq)=b
xue@1: 	cdouble* Skw=new cdouble[Kc+1];
xue@1: 	double* x=new double[Wid];
xue@1: 	cdouble** Allocate2(cdouble, M, Kc, Smkw);
xue@1: 
xue@1: 	Alloc2(2*Kc+2, 2*Kc, A);
xue@1: 	double *b=A[2*Kc], *pq=A[2*Kc+1];
xue@1: 
xue@1: 	  Xkw(Skw, Kc, Wid, s, win, omg);
xue@1: 	  for (int m=1; m<M; m++)
xue@1: 	  {
xue@1: 		  for (int i=0; i<Wid; i++) x[i]=dh[m](i-hWid, harg)*s[i];
xue@1: 		  Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
xue@1: 	  }
xue@1: 
xue@1: 	  for (int k=0; k<2*Kc; k++) b[k]=((double*)(&Skw[1]))[k];
xue@1: 		for (int k=0; k<Kc; k++)
xue@1: 		{
xue@1: 		  for (int m=0; m<M1; m++)
xue@1: 		  {
xue@1: 			  int lind;
xue@1: 				if ((lind=ind[2*m])>=0)
xue@1: 			  {
xue@1: 				  A[2*k][lind]=Smkw[m+1][k].x;
xue@1: 				  A[2*k+1][lind]=Smkw[m+1][k].y;
xue@1: 				}
xue@1: 			  if ((lind=ind[2*m+1])>=0)
xue@1: 			  {
xue@1: 				  A[2*k+1][lind]=Smkw[m+1][k].x;
xue@1: 				  A[2*k][lind]=-Smkw[m+1][k].y;
xue@1: 			  }
xue@1: 		  }
xue@1: 	  }
xue@1: 
xue@1: 	if (2*Kc==Kr) GECP(Kr, pq, A, b);
xue@1: 	else LSLinear2(2*Kc, Kr, pq, A, b);
xue@1: 
xue@1: 	memset(&r[1], 0, sizeof(double)*M1*2);
xue@1: 	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
xue@1: 	//estiamte r0
xue@1:   if (r0)
xue@1:   {
xue@1: 		cdouble e0=0;
xue@1:   	for (int i=0; i<Wid; i++)
xue@1: 	  {
xue@1: 		  cdouble expo=0;
xue@1: 			double n=i-hWid;
xue@1:   		for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
xue@1: 	  	cdouble tmp=exp(expo)*win[0][i];
xue@1: 			e0+=tmp.rotate(-omg*n);
xue@1: 		}
xue@1: 		r[0]=log(Skw[0]/e0);
xue@1:   }
xue@1: 	delete[] x;
xue@1: 	delete[] Skw;
xue@1: 	delete[] uind;
xue@1: 	delete[] ind;
xue@1: 	DeAlloc2(Smkw);
xue@1: 	DeAlloc2(A);
xue@1: }//DerivativeLS
xue@1: 
Chris@5: /**
xue@1:   function DerivativeLS: derivative method for estimating the model log(s)=h[M]'r[M] using Fr
xue@1:   measurement points a quarter of Wid apart from each other, implemented by least-square.
xue@1: 
xue@1:   In: s[Wid+(Fr-1)*Wid/4]: waveform data
xue@1:       win[][Wid]: window function and its derivatives
xue@1:       h[M], dh[M]: pointers to basis functions and their derivatives
xue@1:       harg: pointer argument to be used by calls to functions in h[] amd dh[].
xue@1:       Fr: number of measurement points
xue@1:       K: number of derivatives to take at each measurement point
xue@1:       p0[p0s]: zero-constraints on real parts of r, i.e. Re(r[p0[*]]) are constrained to 0.
xue@1:       q0[q0s]: zero-constraints on imaginary parts of r, i.e. Im(r[q0[*]]) are constrained to 0.
xue@1:       omg: initial angular frequency
xue@1:       r0: specifies if r[0] is to be computed.
xue@1:   Out: r[M]: estimated coefficients to h[M].
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1: 	int hWid=Wid/2, qWid=Wid/4, M1=M-1;
xue@1: 	int Kr=(M1)*2-p0s-q0s; //number of real unknowns apart from p0 and q0
xue@1: 	int Kc=ceil(Kr/2.0/Fr); //number of derivatives required
xue@1: 	if (Kc<K) Kc=K;
xue@1: 
xue@1: 	int *uind=new int[Kr], *ind=new int[2*M1];
xue@1: 	memset(ind, 0, sizeof(int)*2*M1);
xue@1: 	for (int p=0; p<p0s; p++) ind[2*(p0[p]-1)]=-1;
xue@1: 	for (int q=0; q<q0s; q++) ind[2*(q0[q]-1)+1]=-1;
xue@1: 	{int p=0, up=0;	while (p<2*M1){if (ind[p]>=0){uind[up]=p;	ind[p]=up; up++;}	p++;}}
xue@1: 
xue@1: 	//allocate buffer for linear system A(pq)=b
xue@1: 	cdouble* Skw=new cdouble[Kc+1], Skw00;
xue@1: 	double* x=new double[Wid];
xue@1: 	cdouble** Allocate2(cdouble, M, Kc, Smkw);
xue@1: 
xue@1: 	Alloc2(2*Fr*Kc, 2*Fr*Kc, A);
xue@1: 	double *pq=new double[2*Fr*Kc], *b=new double[2*Fr*Kc];
xue@1: 
xue@1: 	for (int fr=0; fr<Fr; fr++)
xue@1: 	{
xue@1: 		int Offst=qWid*fr; double* ss=&s[Offst];
xue@1: 
xue@1: 		Xkw(Skw, Kc, Wid, ss, win, omg); if (fr==0) Skw00=Skw[0];
xue@1: 		for (int m=1; m<M; m++)
xue@1: 		{
xue@1: 			for (int i=0; i<Wid; i++) x[i]=dh[m](i+Offst-hWid, harg)*ss[i];
xue@1: 			Xkw(Smkw[m], Kc-1, Wid, x, win, omg);
xue@1: 		}
xue@1: 
xue@1: 		for (int k=0; k<2*Kc; k++) b[2*fr*Kc+k]=((double*)(&Skw[1]))[k];
xue@1: 		for (int k=0; k<Kc; k++)
xue@1: 		{
xue@1: 			for (int m=0; m<M1; m++)
xue@1: 			{
xue@1: 				int lind;
xue@1: 				if ((lind=ind[2*m])>=0)
xue@1: 				{
xue@1: 					A[2*fr*Kc+2*k][lind]=Smkw[m+1][k].x;
xue@1: 					A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].y;
xue@1: 				}
xue@1: 				if ((lind=ind[2*m+1])>=0)
xue@1: 				{
xue@1: 					A[2*fr*Kc+2*k+1][lind]=Smkw[m+1][k].x;
xue@1: 					A[2*fr*Kc+2*k][lind]=-Smkw[m+1][k].y;
xue@1: 				}
xue@1: 			}
xue@1: 		}
xue@1: 	}
xue@1: 	if (2*Fr*Kc==Kr) GECP(Kr, pq, A, b);
xue@1: 	else LSLinear2(2*Fr*Kc, Kr, pq, A, b);
xue@1: 
xue@1: 	memset(&r[1], 0, sizeof(double)*M1*2);
xue@1: 	for (int k=0; k<Kr; k++) ((double*)(&r[1]))[uind[k]]=pq[k];
xue@1: 	//estiamte r0
xue@1: 	if (r0)
xue@1: 	{
xue@1: 		cdouble e0=0;
xue@1: 		for (int i=0; i<Wid; i++)
xue@1: 		{
xue@1: 			cdouble expo=0;
xue@1: 			double n=i-hWid;
xue@1: 			for (int m=1; m<M; m++){double lhm=h[m](n, harg); expo+=r[m]*lhm;}
xue@1: 			cdouble tmp=exp(expo)*win[0][i];
xue@1: 			e0+=tmp.rotate(-omg*n);
xue@1: 		}
xue@1: 		r[0]=log(Skw00/e0);
xue@1:   }
xue@1: 	delete[] x;
xue@1: 	delete[] Skw;
xue@1: 	delete[] uind;
xue@1: 	delete[] ind;
xue@1: 	DeAlloc2(Smkw);
xue@1: 	DeAlloc2(A);
xue@1: 	delete[] pq; delete[] b;
xue@1: }//DerivativeLS
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Abe-Smith sinusoid estimator 2005
xue@1: 
xue@1:     Further reading: M. Abe and J. O. Smith III, ¡°AM/FM rate estimation for time-varying sinusoidal
xue@1:     modeling,¡± in Proc. ICASSP'05, Philadelphia, 2005.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function RDFTW: windowed DTFT at frequency k bins
xue@1: 
xue@1:   In: data[Wid]: waveform data
xue@1:       w[Wid]: window function
xue@1:       k: frequency, in bins
xue@1:   Out: Xr, Xi: real and imaginary parts of the DTFT of xw at frequency k bins
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void RDFTW(double& Xr, double& Xi, double k, int Wid, double* data, double* w)
xue@1: {
xue@1: 	Xr=Xi=0;
xue@1: 	int hWid=Wid/2;
xue@1: 	double* lw=&w[Wid];
xue@1: 	for (int i=0; i<=Wid; i++)
xue@1: 	{
xue@1: 		double tmp;
xue@1: 		tmp=*data**lw;
xue@1: 		data++, lw--;
xue@1: //*
xue@1:     double ph=-2*M_PI*(i-hWid)*k/Wid;
xue@1:     Xr+=tmp*cos(ph);
xue@1:     Xi+=tmp*sin(ph); //*/
xue@1:   }
xue@1: }//RDFTW
xue@1: 
Chris@5: /**
xue@1:   function TFAS05: the Abe-Smith method 2005
xue@1: 
xue@1:   In: data[Wid]: waveform data
xue@1:       w[Wid]: window function
xue@1:       res: resolution of frequency for QIFFT
xue@1:   Out: f, a, ph: frequency, amplitude and phase angle estimates
xue@1:        aesp, fslope: estimates of log amplitude and frequency derivatives
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void TFAS05(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
xue@1: {
xue@1: 	double fi=floor(f*Wid+0.5); //frequency (int) in bins
xue@1: 	double xr0, xi0, xr_1, xi_1, xr1, xi1;
xue@1: 	RDFTW(xr0, xi0, fi, Wid, data, w);
xue@1: 	RDFTW(xr_1, xi_1, fi-res, Wid, data, w);
xue@1: 	RDFTW(xr1, xi1, fi+res, Wid, data, w);
xue@1: 	double winnorm=0; for (int i=0; i<=Wid; i++) winnorm+=w[i];
xue@1: 	double y0=log(sqrt(xr0*xr0+xi0*xi0)/winnorm),
xue@1:          y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm),
xue@1:          y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
xue@1:   double df=0;
xue@1: //*
xue@1:   if (y0<y1)
xue@1:   {
xue@1:     double newfi=fi+res;
xue@1:     while (y0<y1)
xue@1:     {
xue@1:       y_1=y0, xr_1=xr0, xi_1=xi0;
xue@1:       y0=y1, xr0=xr1, xi0=xi1;
xue@1:       newfi+=res;
xue@1:       RDFTW(xr1, xi1, newfi, Wid, data, w);
xue@1:       y1=log(sqrt(xr1*xr1+xi1*xi1)/winnorm);
xue@1:       fi+=res;
xue@1:     }
xue@1:   }
xue@1:   else if(y0<y_1)
xue@1:   {
xue@1:     double newfi=fi-res;
xue@1:     while (y0<y_1)
xue@1:     {
xue@1:       y1=y0, xr1=xr0, xi1=xi0;
xue@1:       y0=y_1, xr0=xr_1, xi0=xi_1;
xue@1:       newfi-=res;
xue@1:       RDFTW(xr_1, xi_1, newfi, Wid, data, w);
xue@1:       y_1=log(sqrt(xr_1*xr_1+xi_1*xi_1)/winnorm);
xue@1:       fi-=res;
xue@1:     }
xue@1:   }    //*/
xue@1: 
xue@1: 	double a2=(y1+y_1)*0.5-y0, a1=(y1-y_1)*0.5, a0=y0;
xue@1:   df=-a1*0.5/a2;
xue@1:   f=fi+df*res; //in bins
xue@1:   double y=a0-0.25*a1*a1/a2;
xue@1:   a=exp(y);
xue@1: 	double ph0=(xi0==0 && xr0==0)?0:atan2(xi0, xr0),
xue@1: 				 ph_1=(xi_1==0 && xr_1==0)?0:atan2(xi_1, xr_1),
xue@1: 				 ph1=(xi1==0 && xr1==0)?0:atan2(xi1, xr1);
xue@1:   if (fabs(ph_1-ph0)>M_PI)
xue@1:   {
xue@1:     if (ph_1-ph0>0) ph_1-=M_PI*2;
xue@1:     else ph_1+=M_PI*2;
xue@1:   }
xue@1:   if (fabs(ph1-ph0)>M_PI)
xue@1:   {
xue@1:     if (ph1-ph0>0) ph1-=M_PI*2;
xue@1:     else ph1+=M_PI*2;
xue@1:   } 
xue@1:   double b2=(ph1+ph_1)*0.5-ph0, b1=(ph1-ph_1)*0.5, b0=ph0;
xue@1:   ph=b0+b1*(df+b2*df);
xue@1:   //now we have the QI estimates
xue@1:   double uff=2*a2, vf=b1+2*b2*df, vff=2*b2;
xue@1: 	double dfdp=Wid/(2*M_PI*res);
xue@1: 	double upp=uff*dfdp*dfdp, vp=vf*dfdp, vpp=vff*dfdp*dfdp;
xue@1: 	double p=-upp*0.5/(upp*upp+vpp*vpp);
xue@1: 	double alf=-2*p*vp, beta=p*vpp/upp;
xue@1: 	//*direct method
xue@1: 	double beta_p=beta/p;
xue@1: 	double feses=f-alf*beta/p /(2*M_PI)*Wid,
xue@1: 				 yeses=y-alf*alf*0.25/p+0.25*log(1+beta_p*beta_p),
xue@1: 				 pheses=ph+alf*alf*beta*0.25/p-0.5*atan(beta_p); //*/
xue@1: 	/*adapted method
xue@1: 	double zt[]={0, 0.995354, 0.169257, 1.393056, 0.442406, -0.717980, -0.251620, 0.177511, 0.158120, -0.503299};
xue@1: 	double delt=res/Wid; double delt0=df*delt;
xue@1: 	beta=zt[3]*beta+zt[4]*delt0*alf;
xue@1: 	alf=(zt[1]+zt[2]*delt*delt)*alf;
xue@1: 	double beta_p=beta/p;
xue@1: 	double feses=f+zt[5]*alf*beta/p /(2*M_PI)*Wid,
xue@1: 				 yeses=y+zt[6]*alf*alf/p+zt[7]*log(1+beta_p*beta_p),
xue@1: 				 pheses=ph+zt[8]*alf*alf*beta/p+zt[9]*atan(beta_p); //*/
xue@1: 	f=feses/Wid, a=exp(yeses), ph=pheses, fslope=2*beta/2/M_PI, aesp=alf;
xue@1: }//TFAS05
xue@1: 
Chris@5: /**
xue@1:   function TFAS05_enh: the Abe-Smith method 2005 enhanced by LSE amplitude and phase estimation
xue@1: 
xue@1:   In: data[Wid]: waveform data
xue@1:       w[Wid]: window function
xue@1:       res: resolution of frequency for QIFFT
xue@1:   Out: f, a, ph: frequency, amplitude and phase angle estimates
xue@1:        aesp, fslope: estimates of log amplitude and frequency derivatives
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void TFAS05_enh(double& f, double& t, double& a, double& ph, double& aesp, double& fslope, int Wid, double* data, double* w, double res)
xue@1: {
xue@1: 	TFAS05(f, t, a, ph, aesp, fslope, Wid, data, w, res);
xue@1: 	double xr=0, xi=0, p, win2=0;
xue@1: 	for (int n=0; n<=Wid; n++)
xue@1: 	{
xue@1: 		double ni=n-Wid/2, tmp=data[n]*w[n]*w[n];//*exp(-aesp*(n-Wid/2));  if (IsInfinite(tmp)) continue;
xue@1: 		p=-2*M_PI*(f+0.5*fslope*ni)*ni;
xue@1: 		xr+=tmp*cos(p);
xue@1: 		xi+=tmp*sin(p);
xue@1: 		win2+=w[n]*w[n];
xue@1: 	}
xue@1: 	a=sqrt(xr*xr+xi*xi)/win2;
xue@1: 	ph=(xr==0 && xi==0)?0:atan2(xi, xr);
xue@1: }//TFAS05_enh
xue@1: //version without returning aesp and fslope
xue@1: void TFAS05_enh(double& f, double& t, double& a, double& ph, int Wid, double* data, double* w, double res)
xue@1: {
xue@1: 	double aesp, fslope;
xue@1: 	TFAS05_enh(f, t, a, ph, aesp, fslope, Wid, data, w, res);
xue@1: }//TFAS05_enh
xue@1: 
xue@1: //---------------------------------------------------------------------------
Chris@5: /**
xue@1:   function DerivativeLSv_AmpPh: estimate the constant-term in the local derivative method. This is used
xue@1:   by the local derivative algorithm, whose implementation is found in the header file as templates.
xue@1: 
xue@1:   In: sv0: inner product <s, v0>, where s is the sinusoid being estimated.
xue@1:       integr_h[M][Wid]: M vectors containing samples of the integral of basis functions h[M].
xue@1:       v0[M]: a test function
xue@1:       lmd[M]: coefficients to h[M]
xue@1: 
xue@1:   Returns coefficient of integr_h[0]=1.
xue@1: */
xue@1: cdouble DerivativeLSv_AmpPh(int Wid, int M, double** integr_h, cdouble* lmd, cdouble* v0, cdouble sv0)
xue@1: {
xue@1:   cdouble e0=0;
xue@1:   for (int n=0; n<Wid; n++)
xue@1:   {
xue@1:     cdouble expo=0;
xue@1:     for (int m=1; m<=M; m++) expo+=lmd[m]*integr_h[m][n];
xue@1:     e0+=exp(expo)**v0[n];
xue@1:   }
xue@1:   return log(sv0/e0);
xue@1: }//DerivativeLSv_AmpPh
xue@1: 
xue@1: //---------------------------------------------------------------------------
xue@1: /*
xue@1:     Piecewise derivative algorithm
xue@1: 
xue@1:     Further reading: Wen X. and M. Sandler, "Spline exponential approximation of time-varying
xue@1:     sinusoids," under review.
xue@1: */
xue@1: 
Chris@5: /**
xue@1:   function setv: computes I test functions v[I] by modulation u[I] to frequency f
xue@1: 
xue@1:   In: u[I+1][Wid], du[I+1][Wid]: base-band test functions and their derivatives
xue@1:       f: carrier frequency
xue@1:   Out: v[I][Wid], dv[I][Wid]: test functions and their derivatives
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void setv(int I, int Wid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
xue@1: {
xue@1:   double fbin=floor(f*Wid+0.5)/Wid;
xue@1:   double omg=fbin*2*M_PI;
xue@1:   cdouble jomg=cdouble(0, omg);
xue@1:   for (int c=0; c<Wid; c++)
xue@1:   {
xue@1:     double t=c;
xue@1:     cdouble rot=polar(1.0, omg*t);
xue@1:     for (int i=0; i<I-1; i++) v[i][c]=u[i][c]*rot;
xue@1:     for (int i=0; i<I-1; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
xue@1:     //Here it is assumed that elements of u[] are modulated at 0, 1, -1, 2, -2, 3, -3, 4, ...;
xue@1:     //if f is under fbin then the closest ones are in order 0, -1, 1, -2, 3, -3, 3, .... This
xue@1:     //makes a difference to the whole of v[] only if I is even.
xue@1:     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];}
xue@1:     else{v[I-1][c]=u[I][c]*rot; dv[I-1][c]=du[I][c]*rot+jomg*v[I-1][c];}
xue@1:   }
xue@1: }//setv
xue@1: 
Chris@5: /**
xue@1:   function setvhalf: computes I half-size test functions v[I] by modulation u[I] to frequency f.
xue@1: 
xue@1:   In: u[I][hWid*2], du[I][Wid*2]: base-band test functions and their derivatives
xue@1:       f: carrier frequency
xue@1:   Out: v[I][hWid], dv[hWid]: half-size test functions and their derivatives
xue@1: 
xue@1:   No return value.
xue@1: */void setvhalf(int I, int hWid, cdouble** v, cdouble** dv, double f, cdouble** u, cdouble** du)
xue@1: {
xue@1:   double fbin=floor(f*hWid)/hWid;
xue@1:   double omg=fbin*2*M_PI;
xue@1:   cdouble jomg=cdouble(0, omg);
xue@1:   for (int c=0; c<hWid; c++)
xue@1:   {
xue@1:     double t=c;
xue@1:     cdouble rot=polar(1.0, omg*t);
xue@1:     for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
xue@1:     for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
xue@1:   }
xue@1: }//setvhalf
xue@1: 
xue@1: //#define ERROR_CHECK
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewise: Piecewise derivative algorithm. In this implementation of the piecewise
xue@1:   method the test functions v are constructed from I "basic" (single-frame) test functions, each
xue@1:   covering the same period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I
xue@1:   test functions are used.
xue@1: 
xue@1:   In: s[LT+1]: waveform data
xue@1:       ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
xue@1:       L, T: number and length of pieces.
xue@1:       N: number of independent coefficients
xue@1:       h[M][T]: piecewise basis functions
xue@1:       A[L][M][N]: L matrices that map independent coefficients onto component coefficients over the L pieces
xue@1:       u[I][2T}, du[I][2T]: base-band test functions
xue@1:       f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
xue@1:       endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: aita[N]: independent coefficients
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
xue@1:   cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
xue@1:   cdouble** Allocate2(cdouble, I, T*2, v);
xue@1:   cdouble** Allocate2(cdouble, I, T*2, dv);
xue@1:   //compute <sr, v>
xue@1:   cdouble*** Allocate3L(cdouble, L_1, I, N, srv, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
xue@1: 
xue@1: #ifdef ERROR_CHECK
xue@1:   cdouble dsv1[128], dsv2[128];
xue@1: #endif
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     double fbin=floor(f[l+1]*T*2)/(T*2.0);
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T*2; c++)
xue@1:     {
xue@1:       double t=c-T;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
xue@1:     for (int i=0; i<I; i++)
xue@1:       for (int m=0; m<M; m++)
xue@1:         shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
xue@1:     //memset(srv[l][0], 0, sizeof(cdouble)*I*N);
xue@1:     MultiplyXY(I, M, N, srv[l], shv1, A[l]);
xue@1:     MultiAddXY(I, M, N, srv[l], shv2, A[l+1]);
xue@1: 
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'>
xue@1:     if (ds)
xue@1:     {
xue@1:       cdouble* lds=&ds[l*T];
xue@1:       for (int i=0; i<I && l*I+1<36; i++)
xue@1:       {
xue@1:         cdouble lsv=Inner(2*T, lds, v[i]); //compute <s', v[i]>
xue@1:         //cdouble* ls=&s[l*T];
xue@1:         //cdouble lsv2=Inner(2*T, ls, dv[i]);
xue@1:         dsv1[l*I+i]=lsv-sv[l][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
xue@1:       }
xue@1: 
xue@1:       //error check: srv[l]*pq=<s',v>
xue@1:       for (int i=0; i<I && l*I+i<36; i++)
xue@1:       {
xue@1:         cdouble lsv=0;
xue@1:         for (int n=0; n<N; n++) lsv+=srv[l][i][n]*aita[n];
xue@1:         dsv2[l*I+i]=lsv-sv[l][i]-dsv1[l*I+i];
xue@1:       }
xue@1:     }
xue@1: #endif
xue@1:   }
xue@1:   L_1=L-1;
xue@1:   if (endmode==1 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     int hT=T/2;
xue@1:     double fbin=floor((f[0]+f[1])*hT)/T;
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T; c++)
xue@1:     {
xue@1:       double t=c-hT;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
xue@1:     MultiplyXY(I, M, N, srv[L_1], shv1, A[0]);
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'>
xue@1:     if (ds)
xue@1:     {
xue@1:       cdouble* lds=&ds[0];
xue@1:       for (int i=0; i<I && L_1*I+1<36; i++)
xue@1:       {
xue@1:         cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
xue@1:         //cdouble* ls=&s[l*T];
xue@1:         //cdouble lsv2=Inner(2*T, ls, dv[i]);
xue@1:         dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
xue@1:       }
xue@1: 
xue@1:       //error check: srv[l]*pq=<s',v>
xue@1:       for (int i=0; i<I && L_1*I+i<36; i++)
xue@1:       {
xue@1:         cdouble lsv=0;
xue@1:         for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
xue@1:         dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
xue@1:       }
xue@1:     }
xue@1: #endif
xue@1:     L_1++;
xue@1:   }
xue@1:   if (endmode==2 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     int hT=T/2;
xue@1:     double fbin=floor((f[L-1]+f[L])*hT)/T;
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T; c++)
xue@1:     {
xue@1:       double t=c-hT;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     //memset(srv[L_1][0], 0, sizeof(cdouble)*I*N);
xue@1:     MultiplyXY(I, M, N, srv[L_1], shv1, A[L-1]);
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'>
xue@1:     if (ds)
xue@1:     {
xue@1:       cdouble* lds=&ds[(L-1)*T];
xue@1:       for (int i=0; i<I && L_1*I+1<36; i++)
xue@1:       {
xue@1:         cdouble lsv=Inner(T, lds, v[i]); //compute <s', v[i]>
xue@1:         //cdouble* ls=&s[l*T];
xue@1:         //cdouble lsv2=Inner(2*T, ls, dv[i]);
xue@1:         dsv1[L_1*I+i]=lsv-sv[L_1][i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
xue@1:       }
xue@1: 
xue@1:       //error check: srv[l]*pq=<s',v>
xue@1:       for (int i=0; i<I && L_1*I+i<36; i++)
xue@1:       {
xue@1:         cdouble lsv=0;
xue@1:         for (int n=0; n<N; n++) lsv+=srv[L_1][i][n]*aita[n];
xue@1:         dsv2[L_1*I+i]=lsv-sv[L_1][i]-dsv1[L_1*I+i];
xue@1:       }
xue@1:     }
xue@1: #endif
xue@1:       L_1++;
xue@1:   }
xue@1: 
xue@1:   if (L_1*2*I==2*N) GECP(N, aita, srv[0], sv[0]);
xue@1:   else LSLinear(L_1*I, N, aita, srv[0], sv[0]);
xue@1: 
xue@1:   delete mlist;
xue@1: }//DerivativePiecewise
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewise2: Piecewise derivative algorithm in which the real and imaginary parts of
xue@1:   the exponent are modelled separately. In this implementation of the piecewise method the test
xue@1:   functions v are constructed from I "basic" (single-frame) test functions, each covering the same
xue@1:   period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
xue@1:   used.
xue@1: 
xue@1:   In: s[LT+1]: waveform data
xue@1:       ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
xue@1:       L, T: number and length of pieces.
xue@1:       N: number of independent coefficients
xue@1:       h[M][T]: piecewise basis functions
xue@1:       A[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
xue@1:       B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
xue@1:       u[I][2T}, du[I][2T]: base-band test functions
xue@1:       f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
xue@1:       endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: p[Np], q[Nq]: independent coefficients
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void DerivativePiecewise2(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** A, double*** B,
xue@1:   int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
xue@1:   cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
xue@1:   cdouble** Allocate2(cdouble, I, T*2, v);
xue@1:   cdouble** Allocate2(cdouble, I, T*2, dv);
xue@1:   //compute <sr, v>
xue@1:   cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
xue@1:   cdouble*** srbv;
xue@1:   if (Np==Nq && B==A) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
xue@1: 
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     double fbin=floor(f[l+1]*T*2)/(T*2.0);
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T*2; c++)
xue@1:     {
xue@1:       double t=c-T;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=du[i][c]*rot+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
xue@1:     for (int i=0; i<I; i++)
xue@1:       for (int m=0; m<M; m++)
xue@1:         shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
xue@1:     memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[l], shv1, A[l]);
xue@1:     MultiAddXY(I, M, Np, srav[l], shv2, A[l+1]);
xue@1:     if (srbv!=srav) //so that either B!=A or Np!=Nq
xue@1:     {
xue@1:       //memset(srbv[l][0], 0, sizeof(cdouble)*I*Nq);
xue@1:       MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
xue@1:       MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
xue@1:     }
xue@1:   }
xue@1:   L_1=L-1;
xue@1:   if (endmode==1 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     int hT=T/2;
xue@1:     double fbin=floor((f[0]+f[1])*hT)/T;
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T; c++)
xue@1:     {
xue@1:       double t=c-hT;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=rot*du[i][c*2]*cdouble(2.0)+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[L_1], shv1, A[0]);
xue@1:     if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
xue@1:     L_1++;
xue@1:   }
xue@1:   if (endmode==2 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     int hT=T/2;
xue@1:     double fbin=floor((f[L-1]+f[L])*hT)/T;
xue@1:     double omg=fbin*2*M_PI;
xue@1:     cdouble jomg=cdouble(0, omg);
xue@1:     for (int c=0; c<T; c++)
xue@1:     {
xue@1:       double t=c-hT;
xue@1:       cdouble rot=polar(1.0, omg*t);
xue@1:       for (int i=0; i<I; i++) v[i][c]=u[i][c*2]*rot;
xue@1:       for (int i=0; i<I; i++) dv[i][c]=cdouble(2.0)*du[i][c*2]*rot+jomg*v[i][c];
xue@1:     }
xue@1: 
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[L_1], shv1, A[L-1]);
xue@1:     if (srbv!=srav)
xue@1:     {
xue@1:       //memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);
xue@1:       MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
xue@1:     }
xue@1:     L_1++;
xue@1:   }
xue@1: 
xue@1:   //real implementation of <sr,v>aita=<s',v>
xue@1:   double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
xue@1:   for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
xue@1:   {
xue@1:     int li=l*I+i, li_H=li+L_1*I;
xue@1:     for (int n=0; n<Np; n++)
xue@1:     {
xue@1:       AM[li][n]=srav[l][i][n].x;
xue@1:       AM[li_H][n]=srav[l][i][n].y;
xue@1:     }
xue@1:     for (int n=0; n<Nq; n++)
xue@1:     {
xue@1:       AM[li][Np+n]=-srbv[l][i][n].y;
xue@1:       AM[li_H][Np+n]=srbv[l][i][n].x;
xue@1:     }
xue@1:   }
xue@1:   //least-square solution of (srv)(aita)=(sv)
xue@1:   double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
xue@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;
xue@1: 
xue@1:   if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
xue@1:   else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);
xue@1: 
xue@1:   memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);
xue@1: 
xue@1:   delete mlist;
xue@1: }//DerivativePiecewise2
xue@1: 
xue@1: /*
xue@1:   Error check: test that ds[LT] equals s[LT] times reconstructed R'. Notice that DA is D time A where D
xue@1:   is a pre-emphasis because p[Np] applies to log amplitude rather than its derivative.
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   double err=0, ene=0, *lamdax=new double[M*2], *lamday=&lamdax[M];
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     MultiplyXy(M, Np, lamdax, DA[l], p);
xue@1:     MultiplyXy(M, Nq, lamday, B[l], q);
xue@1:     for (int t=0; t<T; t++)
xue@1:     {
xue@1:       double drtx=0; for (int m=0; m<M; m++) drtx+=lamdax[m]*h[m][t];
xue@1:       double drty=0; for (int m=0; m<M; m++) drty+=lamday[m]*h[m][t];
xue@1:       cdouble drt=cdouble(drtx, drty);
xue@1:       cdouble eds=ds[l*T+t]-s[l*T+t]*drt;
xue@1:       err+=~eds; ene+=~ds[l*T+t];
xue@1:       if (errds) errds[l*T+t]=eds;
xue@1:     }
xue@1:   }
xue@1:   delete[] lamdax;
xue@1:   return err/ene;
xue@1: }//testds_pqA
xue@1: 
xue@1: /*
xue@1:   Error check: dsv1[I] tests that <s', v[I]> equals -<s, v[I]'>, dsv2[I] tests that <sr, v[I]>*pq=
xue@1:   <s',v[I]>
xue@1: */
xue@1: 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)
xue@1: {
xue@1:   for (int i=0; i<I; i++)
xue@1:   {
xue@1:     cdouble lsv=Inner(TT, dsl, vl[i]); //compute <s', v[i]>
xue@1:     //cdouble* ls=&s[l*T];
xue@1:     dsv1[i]=lsv-svl[i]; //i.e. <s', v[i]>=-<s, v[i]'>+dsv1[lI+i]
xue@1:     //sv[l][i]=lsv;
xue@1:   }
xue@1:   //error check: srv[l]*pq=<s',v>
xue@1:   for (int i=0; i<I; i++)
xue@1:   {
xue@1:     cdouble lsv=0;
xue@1:     for (int n=0; n<Np; n++) lsv+=sravl[i][n]*p[n];
xue@1:     for (int n=0; n<Nq; n++) lsv+=srbvl[i][n]*cdouble(0, q[n]);
xue@1:     dsv2[i]=lsv-svl[i]-dsv1[i];
xue@1:   }
xue@1: }//testdsv
xue@1: 
xue@1: /*
xue@1:   Error check: tests A[MN]x[N1]=b[N1], returns square error
xue@1: */
xue@1: double testlinearsystem(int M, int N, double** A, double* x, double* b)
xue@1: {
xue@1:   double err=0;
xue@1:   for (int m=0; m<M; m++)
xue@1:   {
xue@1:     double errli=Inner(N, A[m], x)-b[m];
xue@1:     err+=errli*errli;
xue@1:   }
xue@1:   return err;
xue@1: }//testlinearsystem
xue@1: 
xue@1: /*
xue@1:   Error check: test the total square norm of <s, v>
xue@1: */
xue@1: double testsv(int L, double* f, int T, cdouble* s, int I, cdouble** u, cdouble** du, int endmode)
xue@1: {
xue@1:   cdouble** Allocate2(cdouble, I, T*2, v);
xue@1:   cdouble** Allocate2(cdouble, I, T*2, dv);
xue@1:   double ene=0;
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setv(I, T*2, v, dv, f[l+1], u, du);
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[l*T];
xue@1:     for (int i=0; i<I; i++)
xue@1:     {
xue@1:       cdouble d=Inner(2*T, ls, v[i]);
xue@1:       ene+=~d;
xue@1:     }
xue@1:   }
xue@1:   if (endmode==1 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
xue@1:     cdouble* ls=&s[0];
xue@1:     for (int i=0; i<I; i++)
xue@1: 
xue@1:       ene+=~Inner(T, ls, v[i]);
xue@1:   }
xue@1:   if (endmode==2 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
xue@1:     cdouble* ls=&s[(L-1)*T];
xue@1:     for (int i=0; i<I; i++)
xue@1:       ene+=~Inner(T, ls, v[i]);
xue@1:   }
xue@1:   DeAlloc2(v); DeAlloc2(dv);
xue@1:   return ene;
xue@1: }//testsv
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewise3: Piecewise derivative algorithm in which the log amplitude and frequeny
xue@1:   are modeled separately as piecewise functions. In this implementation of the piecewise method the test
xue@1:   functions v are constructed from I "basic" (single-frame) test functions, each covering the same
xue@1:   period of 2T, by shifting these I functions by steps of T. A total number of (L-1)I test functions are
xue@1:   used.
xue@1: 
xue@1:   In: s[LT+1]: waveform data
xue@1:       ds[LT+1]: derivative of s[LT], used only if ERROR_CHECK is defined.
xue@1:       L, T: number and length of pieces.
xue@1:       N: number of independent coefficients
xue@1:       h[M][T]: piecewise basis functions
xue@1:       dh[M][T]: derivative of h[M][T], used only if ERROR_CHECK is defined.
xue@1:       DA[L][M][Np]: L matrices that do coefficient mapping (real part) over the L pieces
xue@1:       B[L][M][Nq]: L matrices that do coefficient mapping (imaginary part) over the L pieces
xue@1:       u[I][2T}, du[I][2T]: base-band test functions
xue@1:       f[L+1]: reference frequencies at 0, T, ..., LT, only f[1]...f[L-1] are used
xue@1:       endmode: set to 1 or 3 to apply half-size testing over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: p[Np], q[Nq]: independent coefficients
xue@1: 
xue@1:   No return value.
xue@1: */
xue@1: void DerivativePiecewise3(int Np, double* p, int Nq, double* q, int L, double* f, int T, cdouble* s, double*** DA, double*** B,
xue@1:   int M, double** h, int I, cdouble** u, cdouble** du, int endmode, cdouble* ds, double** dh)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   int L_1=(endmode==0)?(L-1):((endmode==3)?(L+1):L);
xue@1:   cdouble** Allocate2L(cdouble, L_1, I, sv, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, T*2, v, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, T*2, dv, mlist);
xue@1:   //compute <sr, v>
xue@1:   cdouble*** Allocate3L(cdouble, L_1, I, Np, srav, mlist);
xue@1:   cdouble*** srbv;
xue@1:   if (Np==Nq && B==DA) srbv=srav; else {Allocate3L(cdouble, L_1, I, Nq, srbv, mlist);} //same model for amplitude and phase
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv1, mlist);
xue@1:   cdouble** Allocate2L(cdouble, I, M, shv2, mlist);
xue@1: 
xue@1: #ifdef ERROR_CHECK
xue@1:   cdouble dsv1in[128], dsv2in[128];
xue@1: #endif
xue@1: 
xue@1:   for (int l=0; l<L-1; l++)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setv(I, T*2, v, dv, f[l+1], u, du);
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[l*T]; for (int i=0; i<I; i++) sv[l][i]=-Inner(2*T, ls, dv[i]);
xue@1: 
xue@1:     //compute <sr, v> over the lth frame
xue@1:     cdouble *ls1=&s[l*T], *ls2=&s[l*T+T];
xue@1:     for (int i=0; i<I; i++)
xue@1:       for (int m=0; m<M; m++)
xue@1:         shv1[i][m]=Inner(T, ls1, h[m], v[i]), shv2[i][m]=Inner(T, ls2, h[m], &v[i][T]);
xue@1:     memset(srav[l][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[l], shv1, DA[l]);
xue@1:     MultiAddXY(I, M, Np, srav[l], shv2, DA[l+1]);
xue@1:     if (srbv!=srav) //so that either B!=A or Np!=Nq
xue@1:     {
xue@1:       MultiplyXY(I, M, Nq, srbv[l], shv1, B[l]);
xue@1:       MultiAddXY(I, M, Nq, srbv[l], shv2, B[l+1]);
xue@1:     }
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
xue@1:     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]);
xue@1: #endif
xue@1:   }
xue@1:   L_1=L-1;
xue@1:   if (endmode==1 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setvhalf(I, T, v, dv, (f[0]+f[1])/2, u, du);
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[0]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     //memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[L_1], shv1, DA[0]);
xue@1:     if (srbv!=srav) {memset(srbv[L_1][0], 0, sizeof(cdouble)*I*Nq);	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[0]);}
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
xue@1:     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]);
xue@1: #endif
xue@1:     L_1++;
xue@1:   }
xue@1:   if (endmode==2 || endmode==3)
xue@1:   {
xue@1:     //v from u given f[l]
xue@1:     setvhalf(I, T, v, dv, (f[L-1]+f[L])/2, u, du);
xue@1:     //compute -<s, v'> over the lth frame
xue@1:     cdouble* ls=&s[(L-1)*T]; for (int i=0; i<I; i++) sv[L_1][i]=-Inner(T, ls, dv[i]);
xue@1:     //compute <sr, v> over the lth frame
xue@1:     for (int i=0; i<I; i++) for (int m=0; m<M; m++) shv1[i][m]=Inner(T, ls, h[m], v[i]);
xue@1:     memset(srav[L_1][0], 0, sizeof(cdouble)*I*Np);
xue@1:     MultiplyXY(I, M, Np, srav[L_1], shv1, DA[L-1]);
xue@1:     if (srbv!=srav)	MultiplyXY(I, M, Nq, srbv[L_1], shv1, B[L-1]);
xue@1: #ifdef ERROR_CHECK
xue@1:     //error check: <s', v>=-<s, v'> and srv[l]*pq=<s',v>
xue@1:     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]);
xue@1: #endif
xue@1:     L_1++;
xue@1:   }
xue@1: 
xue@1:   //real implementation of <sr,v>aita=<s',v>
xue@1:   double** Allocate2L(double, L_1*I*2, Np+Nq, AM, mlist);
xue@1:   for (int l=0; l<L_1; l++) for (int i=0; i<I; i++)
xue@1:   {
xue@1:     int li=l*I+i, li_H=li+L_1*I;
xue@1:     for (int n=0; n<Np; n++)
xue@1:     {
xue@1:       AM[li][n]=srav[l][i][n].x;
xue@1:       AM[li_H][n]=srav[l][i][n].y;
xue@1:     }
xue@1:     for (int n=0; n<Nq; n++)
xue@1:     {
xue@1:       AM[li][Np+n]=-srbv[l][i][n].y;
xue@1:       AM[li_H][Np+n]=srbv[l][i][n].x;
xue@1:     }
xue@1:   }
xue@1:   //least-square solution of (srv)(aita)=(sv)
xue@1:   double* pq=new double[Np+Nq]; mlist->Add(pq, 1);
xue@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);
xue@1: #ifdef ERROR_CHECK
xue@1:     //tests that AM is invariant to a constant shift of p
xue@1:     double errAM=0, errAM2=0, err1, err2;
xue@1:     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;}
xue@1:     //test square error of the input pq
xue@1:     if (ds)
xue@1:     {
xue@1:       memcpy(pq, p, sizeof(double)*Np); memcpy(&pq[Np], q, sizeof(double)*Nq);
xue@1:       err1=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
xue@1:     }
xue@1:     //test error of s'-sR where R is synthesized from the input pq
xue@1:     double errdsin, errdsvin; cdouble* edsin;
xue@1:     if (ds && dh)
xue@1:     {
xue@1:       edsin=new cdouble[L*T]; mlist->Add(edsin, 1);
xue@1:       errdsin=testds_pqA(Np, p, Nq, q, L, T, s, ds, M, h, dh, DA, B, edsin);
xue@1:       errdsvin=testsv(L, f, T, edsin, I, u, du, endmode);
xue@1:     }
xue@1: #endif
xue@1:   Alloc2L(L_1*I*2, Np+Nq-1, Am, mlist);
xue@1:   for (int l=0; l<L_1*I*2; l++) memcpy(Am[l], &AM[l][1], sizeof(double)*(Np+Nq-1));
xue@1:   pq[0]=0;
xue@1:   //if (L_1*2*I==Np+Nq) GECP(Np+Nq, pq, AM, b);
xue@1:   //else LSLinear(2*L_1*I, Np+Nq, pq, AM, b);
xue@1:   if (L_1*2*I==Np+Nq-1) GECP(Np+Nq-1, &pq[1], Am, b);
xue@1:   else LSLinear(2*L_1*I, Np+Nq-1, &pq[1], Am, b);
xue@1: #ifdef ERROR_CHECK
xue@1:     //test square error of output pq
xue@1:     if (ds) err2=testlinearsystem(L_1*I*2, Np+Nq, AM, pq, b);
xue@1:     //test error of s'-sR of the output pq
xue@1:     double errdsout, errdsvout; cdouble* edsout;
xue@1:     if (ds && dh)
xue@1:     {
xue@1:       edsout=new cdouble[L*T]; mlist->Add(edsout, 1);
xue@1:       errdsout=testds_pqA(Np, pq, Nq, &pq[Np], L, T, s, ds, M, h, dh, DA, B, edsout);
xue@1:       errdsvout=testsv(L, f, T, edsout, I, u, du, endmode);
xue@1:     }
xue@1: #endif
xue@1:   memcpy(p, pq, sizeof(double)*Np); memcpy(q, &pq[Np], sizeof(double)*Nq);
xue@1: 
xue@1:   delete mlist;
xue@1: }//DerivativePiecewise3
xue@1: 
xue@1: //initialization routines for the piecewise derivative method
xue@1: 
Chris@5: /**
xue@1:   function seth: set h[M] to a series of power functions.
xue@1: 
xue@1:   In: M, T.
xue@1:   Out: h[M][T], where h[m] is power function of order m.
xue@1: 
xue@1:   No return value. h is allocated anew and must be freed by caller.
xue@1: */
xue@1: void seth(int M, int T, double**& h, MList* mlist)
xue@1: {
xue@1:  if (M<=0){h=0; return;}
xue@1:  Allocate2L(double, M, T, h, mlist);
xue@1:  double* hm=h[0]; for (int t=0; t<T; t++) hm[t]=1;
xue@1:  for (int m=1; m<M; m++)
xue@1:  {
xue@1:    hm=h[m]; for (int t=0; t<T; t++) hm[t]=pow(t*1.0, m);
xue@1:  }
xue@1: }//seth
xue@1: 
Chris@5: /**
xue@1:   function setdh: set dh[M] to the derivative of a series of power functions.
xue@1: 
xue@1:   In: M, T.
xue@1:   Out: dh[M][T], where dh[m] is derivative of the power function of order m.
xue@1: 
xue@1:   No return value. dh is allocated anew and must be freed by caller.
xue@1: */
xue@1: void setdh(int M, int T, double**& dh, MList* mlist)
xue@1: {
xue@1:  if (M<=0){dh=0; return;}
xue@1:  Allocate2L(double, M, T, dh, mlist);
xue@1:  double* dhm=dh[0]; memset(dhm, 0, sizeof(double)*T);
xue@1:  if (M>1){dhm=dh[1]; for (int t=0; t<T; t++) dhm[t]=1;}
xue@1:  for (int m=2; m<M; m++)
xue@1:  {
xue@1:    dhm=dh[m]; for (int t=0; t<T; t++) dhm[t]=m*pow(t*1.0, m-1);
xue@1:  }
xue@1: }//setdh
xue@1: 
Chris@5: /**
xue@1:   function setdih: set dih[M] to the difference of the integral of a series of power functions.
xue@1: 
xue@1:   In: M, I
xue@1:   Out: dih[M][I], where the accumulation of dih[m] is the integral of the power function of order m.
xue@1: 
xue@1:   No return value. dih is allocated anew and must be freed by caller.
xue@1: */
xue@1: void setdih(int M, int T, double**& dih, MList* mlist)
xue@1: {
xue@1:   if (M<=0){dih=0; return;}
xue@1:   Allocate2L(double, M, T, dih, mlist);
xue@1:   double* dihm=dih[0]; for (int t=0; t<T; t++) dihm[t]=1;
xue@1:   for (int m=1; m<M; m++)
xue@1:   {
xue@1:     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);
xue@1:   }
xue@1: }//setdih
xue@1: 
Chris@5: /**
xue@1:   function sshLinear: sets M and h[M] for the linear spline model
xue@1: 
xue@1:   In: T
xue@1:   Out: M=2, h[2][T] filled out for linear spline model.
xue@1: 
xue@1:   No return value. h is allocated anew and must be freed by caller.
xue@1: */
xue@1: void sshLinear(int T, int& M, double** &h, MList* mlist)
xue@1: {
xue@1:   M=2; Allocate2L(double, M, T, h, mlist);
xue@1:   for (int t=0; t<T; t++) h[0][t]=1, h[1][t]=t;
xue@1: }//sshLinear
xue@1: 
Chris@5: /**
xue@1:   function sdihLinear: sets dih[M] for the linear spline model. For testing only.
xue@1: 
xue@1:   In: T
xue@1:   Out: dih[2][T] filled out for linear spline model.
xue@1: 
xue@1:   No return value. dih is allocated anew and must be freed by caller.
xue@1: */
xue@1: void sdihLinear(int T, double**& dih, MList* mlist)
xue@1: {
xue@1:   Allocate2L(double, 2, T, dih, mlist);
xue@1:   for (int t=0; t<T; t++)	dih[0][t]=1, dih[1][t]=t+0.5;
xue@1: }//sdihLinear
xue@1: 
Chris@5: /**
xue@1:   function sshCubic: sets M and h[M] for cubic spline models.
xue@1: 
xue@1:   In: T
xue@1:   Out: M=4 and h[M] filled out for cubic spline models, including cubic and cubic-Hermite.
xue@1: 
xue@1:   No return value. h is allocated anew and must be freed by caller.
xue@1: */
xue@1: void sshCubic(int T, int& M, double** &h, MList* mlist)
xue@1: {
xue@1:   M=4; Allocate2L(double, M, T, h, mlist);
xue@1:   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;
xue@1: }//sshCubic
xue@1: 
Chris@5: /**
xue@1:   function sdihCubic: sets dih[M] for cubic spline models.
xue@1: 
xue@1:   In: T
xue@1:   Out: dih[4] filled out for cubic spline models.
xue@1: 
xue@1:   No return value. dih is allocated anew and must be freed by caller.
xue@1: */
xue@1: void sdihCubic(int T, double** &dih, MList* mlist)
xue@1: {
xue@1:   Allocate2L(double, 4, T, dih, mlist);
xue@1:   for (int t=0; t<T; t++)
xue@1:   {
xue@1:     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;
xue@1:   }
xue@1: }//sdihCubic*/
xue@1: 
Chris@5: /**
xue@1:   function ssALinearSpline: sets N and A[L] for the linear spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:   Out: N=L+1, A[L][M][N] filled out for the linear spline model
xue@1: 
xue@1:   No return value. A is created anew and bust be freed by caller.
xue@1: */
xue@1: void ssALinearSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   N=L+1;
xue@1:   Allocate3L(double, L, M, N, A, mlist);
xue@1:   memset(A[0][0], 0, sizeof(double)*L*M*N);
xue@1:   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;
xue@1: }//ssALinearSpline
xue@1: 
Chris@5: /**
xue@1:   function ssLinearSpline: sets M, N, h and A for the linear spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:   Out: N and h[][] and A[][][] filled out for the linear spline model
xue@1: 
xue@1:   No reutrn value. A and h are created anew and bust be freed by caller.
xue@1: */
xue@1: void ssLinearSpline(int L, int T, int M, int &N, double** &h, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   seth(M, T, h, mlist);
xue@1:   ssALinearSpline(L, T, M, N, A, mlist);
xue@1: }//ssLinearSpline
xue@1: 
Chris@5: /**
xue@1:   function ssACubicHermite: sets N and A[L] for cubic Hermite spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:   Out: N=2(L+1), A[L][M][N] filled out for the cubic Hermite spline
xue@1: 
xue@1:   No return value. A is created anew and must be freed by caller.
xue@1: */
xue@1: void ssACubicHermite(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   N=2*(L+1);
xue@1:   Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
xue@1:   double iT=1.0/T, iT2=iT*iT, iT3=iT2*iT;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     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;
xue@1:     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;
xue@1:     A[l][1][2*l+1]=1;
xue@1:     A[l][0][2*l]=1;
xue@1:   }
xue@1: }//ssACubicHermite
xue@1: 
Chris@5: /**
xue@1:   function ssLinearSpline: sets M, N, h and A for the cubic Hermite spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:   Out: N and h[][] and A[][][] filled out for the cubic Hermite spline model
xue@1: 
xue@1:   No reutrn value. A and h are created anew and bust be freed by caller.
xue@1: */
xue@1: void ssCubicHermite(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   seth(M, T, h, mlist);
xue@1:   ssACubicHermite(L, T, M, N, A, mlist);
xue@1: }//ssCubicHermite
xue@1: 
Chris@5: /**
xue@1:   function ssACubicSpline: sets N and A[L] for cubic spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:       mode: boundary mode of cubic spline, 0=natural, 1=quadratic run-out, 2=cubic run-out
xue@1:   Out: N=2(L+1), A[L][M][N] filled out for the cubic spline
xue@1: 
xue@1:   No return value. A is created anew and must be freed by caller.
xue@1: */
xue@1: void ssACubicSpline(int L, int T, int M, int& N, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   N=L+1;
xue@1:   Allocate3L(double, L, M, N, A, mlist); memset(A[0][0], 0, sizeof(double)*L*M*N);
xue@1:   Alloc2(L+1, L+1, ML); memset(ML[0], 0, sizeof(double)*(L+1)*(L+1));
xue@1:   Alloc2(L+1, L+1, MR); memset(MR[0], 0, sizeof(double)*(L+1)*(L+1));
xue@1:   //fill in ML and MR. The only difference between various cubic splines are ML.
xue@1:   double _6iT2=6.0/(T*T);
xue@1:   ML[0][0]=ML[L][L]=1;
xue@1:   for (int l=1; l<L; l++) ML[l][l-1]=ML[l][l+1]=1, ML[l][l]=4,
xue@1:     MR[l][l-1]=MR[l][l+1]=_6iT2, MR[l][l]=-2*_6iT2;
xue@1:   if (mode==0){} //no more coefficients are needed for natural cubic spline
xue@1:   else if (mode==1) ML[0][1]=ML[L][L-1]=-1; //setting for quadratic run-out
xue@1:   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
xue@1:   GICP(L+1, ML);
xue@1:   double** MM=MultiplyXY(L+1, ML, ML, MR);
xue@1:   double iT=1.0/T;
xue@1:   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;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     MultiplyXY(4, 2, N, A[l], M42, &MM[l]);
xue@1:     A[l][1][l]-=iT; A[l][1][l+1]+=iT; A[l][0][l]+=1;
xue@1:   }
xue@1:   DeAlloc2(ML); DeAlloc2(MR); DeAlloc2(M42);
xue@1: }//ssACubicSpline
xue@1: 
Chris@5: /**
xue@1:   function ssLinearSpline: sets M, N, h and A for the cubic spline model
xue@1: 
xue@1:   In: L, M, T
xue@1:   Out: N and h[][] and A[][][] filled out for the cubic spline model
xue@1: 
xue@1:   No reutrn value. A and h are created anew and bust be freed by caller.
xue@1: */
xue@1: void ssCubicSpline(int L, int T, int M, int& N, double** &h, double*** &A, MList* mlist, int mode)
xue@1: {
xue@1:   seth(M, T, h, mlist);
xue@1:   ssACubicSpline(L, T, M, N, A, mlist, mode);
xue@1: }//ssCubicSpline
xue@1: 
Chris@5: /**
xue@1:   function setu: sets u[I+1] as base-band windowed Fourier atoms, whose frequencies come in the order of
xue@1:   0, 1, -1, 2, -2, 3, -3, 4, etc, in bins.
xue@1: 
xue@1:   In: I, Wid: number and size of atoms to generate.
xue@1:       WinOrder: order (=vanishing moment) of window function to use (2=Hann, 4=Hann^2, etc.)
xue@1:   Out: u[I+1][Wid], du[I+1]{Wid]: the I+1 atoms and their derivatives.
xue@1: 
xue@1:   No return value. u and du are created anew and must be freed by caller.
xue@1: */
xue@1: void setu(int I, int Wid, cdouble**& u, cdouble**& du, int WinOrder, MList* mlist)
xue@1: {
xue@1:   Allocate2L(cdouble, I+1, Wid, u, mlist);
xue@1:   Allocate2L(cdouble, I+1, Wid, du, mlist);
xue@1: 
xue@1:   double** wins=CosineWindows(WinOrder, Wid, (double**)0, 2);
xue@1:   double omg=2*M_PI/Wid; cdouble jomg=cdouble(0, omg);
xue@1:   for (int t=0; t<Wid; t++)
xue@1:   {
xue@1:     u[0][t]=wins[0][t], du[0][t]=wins[1][t];
xue@1:     int li=1;
xue@1:     for (int i=1; i<=I; i++)
xue@1:     {
xue@1:       cdouble rot=polar(1.0, li*omg*t);
xue@1:       u[i][t]=u[0][t]*rot; du[i][t]=du[0][t]*rot+jomg*li*u[i][t];
xue@1:       li=-li; if (li>0) li++;
xue@1:     }
xue@1:   }
xue@1:   DeAlloc2(wins);
xue@1: }//setu
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewiseI: wrapper for DerivativePiecewise(), doing the initialization ,etc.
xue@1: 
xue@1:   In: L, T: number and length of pieces
xue@1:       s[LT]: waveform signal
xue@1:       ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
xue@1:       f[L+1]: reference frequencies at knots
xue@1:       M: polynomial degree of piecewise approximation
xue@1:       SpecifyA, ssmode: pointer to a function that fills A[L], and mode argument to call it
xue@1:       WinOrder: order(=vanishing moment) of window used for constructing test functions
xue@1:       I: number of test functions per frame.
xue@1:       endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: aita[N]: independent coefficients, where N is specified by SpecifyA.
xue@1: 
xue@1:   No return vlue.
xue@1: */
xue@1: void DerivativePiecewiseI(cdouble* aita, int L, double* f, int T, cdouble* s, int M,
xue@1:   void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssmode,
xue@1:   int WinOrder, int I, int endmode, cdouble* ds)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   cdouble **u, **du;
xue@1:   setu(I, 2*T, u, du, WinOrder, mlist);
xue@1: 
xue@1:   int N; double **h, ***A;
xue@1:   seth(M, T, h, mlist);
xue@1:   SpecifyA(L, T, M, N, A, mlist, ssmode);
xue@1: 
xue@1:   DerivativePiecewise(N, aita, L, f, T, s, A, M, h, I, u, du, endmode, ds);
xue@1:   delete mlist;
xue@1: }//DerivativePiecewiseI
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewiseII: wrapper for DerivativePiecewise2(), doing the initialization ,etc.
xue@1:   This models the derivative of log ampltiude and frequency as separate piecewise polynomials, the first
xue@1:   specified by SpecifyA, the second by SpecifyB.
xue@1: 
xue@1:   In: L, T: number and length of pieces
xue@1:       s[LT]: waveform signal
xue@1:       ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
xue@1:       f[L+1]: reference frequencies at knots
xue@1:       M: polynomial degree of piecewise approximation
xue@1:       SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
xue@1:       SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
xue@1:       WinOrder: order(=vanishing moment) of window used for constructing test functions
xue@1:       I: number of test functions per frame.
xue@1:       endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.
xue@1: 
xue@1:   No reutrn value.
xue@1: */
xue@1: void DerivativePiecewiseII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
xue@1:   void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
xue@1:   void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
xue@1:   int WinOrder, int I, int endmode, cdouble* ds)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   cdouble **u, **du;
xue@1:   setu(I, 2*T, u, du, WinOrder, mlist);
xue@1: 
xue@1:   int Np, Nq;
xue@1:   double **h, ***A, ***B;
xue@1:   seth(M, T, h, mlist);
xue@1:   SpecifyA(L, T, M, Np, A, mlist, ssAmode);
xue@1:   SpecifyB(L, T, M, Nq, B, mlist, ssBmode);
xue@1: 
xue@1:   DerivativePiecewise2(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds);
xue@1: 
xue@1:   delete mlist;
xue@1: }//DerivativePiecewiseII
xue@1: 
Chris@5: /**
xue@1:   function DerivativePiecewiseIII: wrapper for DerivativePiecewise3(), doing the initialization ,etc.
xue@1:   Notice that this time the log amplitude, rather than its derivative, is modeled as a piecewise
xue@1:   polynomial specified by SpecifyA.
xue@1: 
xue@1:   In: L, T: number and length of pieces
xue@1:       s[LT]: waveform signal
xue@1:       ds[LT]: derivative of s[LT], used only when ERROR_CHECK is defined.
xue@1:       f[L+1]: reference frequencies at knots
xue@1:       M: polynomial degree of piecewise approximation
xue@1:       SpecifyA, ssAmode: pointer to a function that fills A[L], and mode argument to call it
xue@1:       SpecifyB, ssBmode: pointer to a function that fills B[L], and mode argument to call it
xue@1:       WinOrder: order(=vanishing moment) of window used for constructing test functions
xue@1:       I: number of test functions per frame.
xue@1:       endmode: set to 1 or 3 to apply half-size frame over [0, T], to 2 or 3 to apply over [LT-T, LT]
xue@1:   Out: p[Np], q[Nq]: independent coefficients, where Np and Nq are specified by SpecifyA and SpecifyB.
xue@1: 
xue@1:   No reutrn value.
xue@1: */
xue@1: void DerivativePiecewiseIII(double* p, double* q, int L, double* f, int T, cdouble* s, int M,
xue@1:   void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int ssAmode,
xue@1:   void (*SpecifyB)(int L, int T, int M, int &N, double*** &B, MList* mlist, int mode), int ssBmode,
xue@1:   int WinOrder, int I, int endmode, cdouble* ds)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   int Np, Nq;
xue@1:   double **h, ***A, ***B, **dh=0;
xue@1:   cdouble **u, **du;
xue@1:   setu(I, T*2, u, du, WinOrder, mlist);
xue@1:   seth(M, T, h, mlist);
xue@1:   if (ds) setdh(M, T, dh, mlist);
xue@1:   SpecifyA(L, T, M, Np, A, mlist, ssAmode);
xue@1:   SpecifyB(L, T, M, Nq, B, mlist, ssBmode);
xue@1:   Alloc2L(M, M, DM, mlist);
xue@1:   memset(DM[0], 0, sizeof(double)*M*M); for (int m=0; m<M-1; m++) DM[m][m+1]=m+1;
xue@1:   double** DA=0;
xue@1: 
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     DA=MultiplyXY(M, M, Np, DA, DM, A[l], mlist);
xue@1:     Copy(M, Np, A[l], DA);
xue@1:   }
xue@1: 
xue@1:   DerivativePiecewise3(Np, p, Nq, q, L, f, T, s, A, B, M, h, I, u, du, endmode, ds, dh);
xue@1: 
xue@1:   delete mlist;
xue@1: }//DerivativePiecewiseIII
xue@1: 
Chris@5: /**
xue@1:   function AmpPhCorrectionExpA: model-preserving amplitude and phase correction in piecewise derivative
xue@1:   method.
xue@1: 
xue@1:   In: aita[N]: inital independent coefficients
xue@1:       L, T: number and size of pieces
xue@1:       sre[LT]: waveform data
xue@1:       h[M][T], dih[M][T]: piecewise basis functions and their difference-integrals
xue@1:       A[L][M][N]: L coefficient mapping matrices
xue@1:       SpecifyA: pointer to the function used for constructing A
xue@1:       WinOrder: order(=vanishing moment) of window used for constructing test functions
xue@1:   Out: aita[N]: corrected independent coefficients
xue@1:        s2[LT]: reconstruct sinusoid BEFORE correction
xue@1: 
xue@1:   Returns the estimate of phase angle at 0.
xue@1: */
xue@1: double AmpPhCorrectionExpA(cdouble* s2, int N, cdouble* aita, int L, int T, cdouble* sre, int M, double** h, double** dih, double*** A,
xue@1:   void (*SpecifyA)(int L, int T, int M, int &N, double*** &A, MList* mlist, int mode), int WinOrder)
xue@1: {
xue@1:   MList* mlist=new MList;
xue@1:   //*amplitude and phase correction
xue@1:   //amplitude is done by updating p, i.e. Re(aita)
xue@1:   double *s2ph=new double[L+1]; mlist->Add(s2ph, 1);
xue@1:   double *phcor=new double[L+1]; mlist->Add(phcor, 1);
xue@1:   cdouble* lamda=new cdouble[M]; mlist->Add(lamda, 1);
xue@1:   double* lamdax=new double[M]; mlist->Add(lamdax, 1);
xue@1:   double* lamday=new double[M]; mlist->Add(lamday, 1);
xue@1:   {
xue@1:   double tmpph=0;
xue@1:   memset(s2ph, 0, sizeof(double)*(L+1));
xue@1:   s2ph[0]=tmpph;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     MultiplyXy(M, N, lamda, A[l], aita); for (int m=0; m<M; m++) lamdax[m]=lamda[m].x, lamday[m]=lamda[m].y;
xue@1:     SinusoidExpA(T, &s2[l*T], M, lamdax, lamday, h, dih, tmpph); s2ph[l+1]=tmpph;
xue@1:   }
xue@1:   double* win=new double[2*T+1]; CosineWindows(WinOrder, 2*T, &win, 1); mlist->Add(win, 1);
xue@1:   for (int l=1; l<L; l++)
xue@1:   {
xue@1:     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]);
xue@1:     cdouble loginn=log(inn);
xue@1:     if (SpecifyA==ssACubicHermite)
xue@1:     {
xue@1:       aita[l*2]+=loginn.x;
xue@1:       s2ph[l]+=loginn.y;
xue@1:       phcor[l]=loginn.y;
xue@1:       if (l==1) aita[0]+=loginn.x, phcor[0]=loginn.y, s2ph[0]+=loginn.y;
xue@1:       if (l==L-1) aita[L*2]+=loginn.x, phcor[L]=loginn.y, s2ph[L]+=loginn.y;
xue@1:     }
xue@1:     else
xue@1:     {
xue@1:       aita[l]+=loginn.x;
xue@1:       s2ph[l]+=loginn.y;
xue@1:       phcor[l]=loginn.y;
xue@1:       if (l==1)
xue@1:       {
xue@1:         inn=Inner(T, sre, &win[T], s2)/Inner(T, s2, &win[T], s2);
xue@1:         loginn=log(inn);
xue@1:         aita[0]+=loginn.x;
xue@1:         s2ph[0]+=loginn.y;
xue@1:         phcor[0]=loginn.y;
xue@1:       }
xue@1:       if (l==L-1)
xue@1:       {
xue@1:         inn=Inner(T, &sre[L*T-T], win, &s2[L*T-T])/Inner(T, &s2[L*T-T], win, &s2[L*T-T]);
xue@1:         loginn=log(inn);
xue@1:         aita[L]+=loginn.x;
xue@1:         s2ph[L]+=loginn.y;
xue@1:         phcor[L]=loginn.y;
xue@1:       }
xue@1:     }
xue@1:   }
xue@1: 
xue@1:   for (int l=1; l<=L; l++)
xue@1:   {
xue@1:     int k=floor((phcor[l]-phcor[l-1])/(2*M_PI)+0.5);
xue@1:     if (k!=0)
xue@1:       phcor[l]+=2*M_PI*k;
xue@1:   }
xue@1:   //*
xue@1:   //now phcor[] contains phase corrector to be interpolated
xue@1:   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));
xue@1:   mlist->Add(b, 1); mlist->Add(zet, 1); mlist->Add(dzet, 1);
xue@1:   double ihT[]={T, T/2.0*T, T/3.0*T*T, T/4.0*T*T*T};
xue@1: 
xue@1:   Alloc2L(L, N, BB, mlist);
xue@1:   //prepare linear system (BB)(zet)=(b)
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     MultiplyxY(N, 4, BB[l], ihT, A[l]);
xue@1:     b[l]=phcor[l+1]-phcor[l];
xue@1:   }
xue@1:   Alloc2L(L, L, copyA, mlist);
xue@1:   if (L+1==N)	for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
xue@1:   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];
xue@1:   double* copyb=Copy(L, b, mlist);
xue@1:   zet[0]=0; GECP(L, &zet[1], copyA, copyb);
xue@1:   if (L+1==N) for (int l=0; l<L; l++) memcpy(copyA[l], &BB[l][1], sizeof(double)*L);
xue@1:   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];
xue@1:   Copy(L, copyb, b); for (int l=0; l<L; l++) copyb[l]-=BB[l][0];
xue@1:   dzet[0]=1; GECP(L, &dzet[1], copyA, copyb);
xue@1: 
xue@1: #ifdef ERROR_CHECK
xue@1:     //Test that (BB)(zet)=b and (BB)(dzet)=b
xue@1:     double* bbzet=MultiplyXy(L, L+1, BB, zet, mlist);
xue@1:     MultiAdd(L, bbzet, bbzet, b, -1);
xue@1:     double err1=Inner(L, bbzet, bbzet);
xue@1:     double* bbdzet=MultiplyXy(L, L+1, BB, dzet, mlist);
xue@1:     MultiAdd(L, bbdzet, bbdzet, b, -1);
xue@1:     double err2=Inner(L, bbdzet, bbdzet);
xue@1:     MultiAdd(L+1, dzet, dzet, zet, -1);
xue@1:     //Test that (BB)dzet=0
xue@1:     MultiplyXy(L, L+1, bbdzet, BB, dzet);
xue@1:     double err3=Inner(L, bbzet, bbzet);
xue@1: #endif
xue@1:   //now that (zet)+(miu)(dzet) is the general solution to (BB)(zet)=b,
xue@1:   //	we look for (miu) that maximizes smoothness
xue@1: 
xue@1:   double innuv=0, innvv=0, lmd0[4], lmdd[4], clmdd[4],
xue@1:     T2=T*T, T3=T2*T, T4=T3*T, T5=T4*T;
xue@1:   for (int l=0; l<L; l++)
xue@1:   {
xue@1:     MultiplyXy(4, L+1, lmd0, A[l], zet);
xue@1:     MultiplyXy(4, L+1, lmdd, A[l], dzet);
xue@1:     clmdd[1]=T*lmdd[1]+T2*lmdd[2]+T3*lmdd[3];
xue@1:     clmdd[2]=T2*lmdd[1]+(4.0/3)*T3*lmdd[2]+1.5*T4*lmdd[3];
xue@1:     clmdd[3]=T3*lmdd[1]+1.5*T4*lmdd[2]+1.8*T5*lmdd[3];
xue@1:     innuv+=Inner(3, &lmd0[1], &clmdd[1]);
xue@1:     innvv+=Inner(3, &lmdd[1], &clmdd[1]);
xue@1:   }
xue@1:   MultiAdd(L+1, zet, zet, dzet, -innuv/innvv);
xue@1: 
xue@1:   if (SpecifyA==ssACubicHermite)
xue@1:     for (int l=0; l<=L; l++) aita[2*l].y+=zet[l];
xue@1:   else
xue@1:     for (int l=0; l<=L; l++) aita[l].y+=zet[l];
xue@1:   //*/
xue@1:   }
xue@1:   double result=s2ph[0];
xue@1:   delete mlist;
xue@1:   return result;
xue@1: }//AmpPhCorrectionExpA
Chris@5: