view SinSyn.cpp @ 1:6422640a802f

first upload

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


#include "align8.h"
#include "SinSyn.h"
#include "splines.h"

//---------------------------------------------------------------------------
/*
  function Sinuoid:	original McAuley-Quatieri synthesizer interpolation between two measurement points.

  In: T: length from measurement point 1 to measurement point 2
      a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
      a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
      ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
  Out: data[T]: output buffer
       a[T], f[T], p[T]: resynthesized amplitude, frequency and phase

  No return value.
*/
void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, double* a, double* f, double* p, bool ad)
{
  int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
  double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
  double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
  double la=a1, da=(a2-a1)/T;
  if (ad)
    for (int t=0; t<T; t++)
    {
      double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
      data[t]+=la*cos(lp);
      a[t]=la;
      p[t]=lp;
      f[t]=lf;
      la=la+da;
    }
  else
    for (int t=0; t<T; t++)
    {
      double lp=pd+t*(pc+t*(pa+t*pb)), lf=(pc+2*pa*t+3*pb*t*t)/2/M_PI;
      data[t]=la*cos(lp);
      a[t]=la;
      p[t]=lp;
      f[t]=lf;
      la=la+da;
    }
}//Sinusoid

/*
  function Sinuoid:	original McAuley-Quatieri synthesizer interpolation between two measurement points,
  without returning interpolated sinusoid parameters.

  In: T: length from measurement point 1 to measurement point 2
      a1, f1, p2: amplitude, frequency and phase angle at measurement point 1
      a2, f2, p2: amplitude, frequency and phase angle at measurement point 2
      ad: specifies if the resynthesized sinusoid is to be added to or to replace the contents of output buffer
  Out: data[T]: output buffer

  No return value.
*/
void Sinusoid(double* data, int T, double a1, double a2, double f1, double f2, double p1, double p2, bool ad)
{
  int M=floor(((p1-p2)/M_PI+(f1+f2)*T)/2.0+0.5);
  double b1=p2-p1-2*M_PI*(f1*T-M), b2=2*M_PI*(f2-f1);
  double pa=(3*b1/T-b2)/T, pb=(-2*b1/T+b2)/T/T, pc=2*M_PI*f1, pd=p1;
  double la=a1, da=(a2-a1)/T;
  if (ad)
    for (int t=0; t<T; t++)
    {
      data[t]+=la*cos(pd+t*(pc+t*(pa+t*pb)));
      la=la+da;
    }
  else
    for (int t=0; t<T; t++)
    {
      data[t]=la*cos(pd+t*(pc+t*(pa+t*pb)));
      la=la+da;
    }
}//Sinusoid

//---------------------------------------------------------------------------
/*
  function Sinusoid_direct: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
  amplitude and frequency, direct implementation.

  In: CountSt, CountEn
      aa, ab, ac, ad: trinomial coefficients of amplitude
      fa, fb, fc, fd: trinomial coefficients of frequency
      p1: initial phase angle at 0 (NOT at CountSt)
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.
       p1: phase angle at CountEn

  No return value.
*/
void Sinusoid_direct(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
  double fa, double fb, double fc, double fd, double &p1, bool add)
{
  int i; double a, ph;
  for (i=CountSt; i<CountEn; i++)
  {
     a=ad+i*(ac+i*(ab+i*aa));
     ph=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
    if (add) data[i]+=a*cos(ph);
    else data[i]=a*cos(ph);
  }
  p1=p1+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)));
}//Sinusoid

/*
  function Sinusoid: synthesizes sinusoid over [CountSt, CountEn) from tronomial coefficients of
  amplitude and frequency, recursive implementation.

  In: CountSt, CountEn
      a3, a2, a1, a0: trinomial coefficients of amplitude
      f3, f2, f1, f0: trinomial coefficients of frequency
      ph: initial phase angle at 0 (NOT at CountSt)
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.
       ph: phase angle at CountEn

  No return value. This function requires 8-byte stack alignment for optimal speed.
*/
void Sinusoid(double* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
  double f3, double f2, double f1, double f0, double &ph, bool add)
{
  int i;
  double a, da, dda, ddda, dph, ddph, dddph, ddddph,
         sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
         p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp;
  if (CountSt==0)
  {
    a=a0; da=a1+a2+a3; dda=2*a2+6*a3; ddda=6*a3;
    dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
  }
  else
  {
    a=a0+CountSt*(a1+CountSt*(a2+CountSt*a3));
    da=a1+a2+a3+CountSt*(2*a2+3*a3+CountSt*3*a3);
    dda=2*a2+6*a3+CountSt*6*a3; ddda=6*a3;
    ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
    dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
    ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
    dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
  }
  sph=sin(ph), cph=cos(ph);
  sdph=sin(dph), cdph=cos(dph);
  sddph=sin(ddph), cddph=cos(ddph);
  sdddph=sin(dddph), cdddph=cos(dddph);
  sddddph=sin(ddddph), cddddph=cos(ddddph);
  if (add)
  {
    for (i=CountSt; i<CountEn; i++)
    {
      data[i]+=a*cph;
      a=a+da; da=da+dda; dda=dda+ddda;
      tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
    }
  }
  else
  {
    for (i=CountSt; i<CountEn; i++)
    {
      data[i]=a*cph;
      a=a+da; da=da+dda; dda=dda+ddda;
      tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
    }
  }
  ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
}

/*
  function SinusoidExp: synthesizes complex sinusoid whose derivative log amplitude and frequency are
  trinomials

  In: CountSt, CountEn
      a3, a2, a1, a0: trinomial coefficients for the derivative of log amplitude
      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
      ea, ph: initial log amplitude and phase angle at 0
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.
       ea, ph: log amplitude and phase angle at CountEn.

  No return value.
*/
void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
	double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
{
	double e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
         p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
	if (add) for (int i=CountSt; i<CountEn; i++)
	{
		double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
		data[i]+=exp(cdouble(lea, lph));
	}
	else for (int i=CountSt; i<CountEn; i++)
	{
		double lea=e0+i*(e1+i*(e2+i*(e3+i*e4)));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
		data[i]=exp(cdouble(lea, lph));
	}
	ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
}//SinusoidExp

/*
  function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
  This version also synthesizes its derivative.

  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
      lamda[M]: coefficients of h[M]
      tmpexp: inital logarithm at 0
  Out: s[T], ds[T]: synthesized sinusoid and its derivative
       tmpexp: logarithm at T

  No return value.
*/
void SinusoidExp(int T, cdouble* s, cdouble* ds, int M, cdouble* lamda, double** h, double** dih, cdouble& tmpexp)
{
	for (int t=0; t<T; t++)
	{
		s[t]=exp(tmpexp);
		cdouble dexp=0, dR=0;
		for (int m=0; m<M; m++)	dexp+=lamda[m]*dih[m][t], dR+=lamda[m]*h[m][t];
		tmpexp+=dexp;
		ds[t]=s[t]*dR;
	}
}//SinusoidExp

/*
  function SinusoidExp: synthesizes complex sinusoid piece whose derivative logarithm is h[M]'lamda[M].
  This version does not synthesize its derivative.

  In: dih[M][T]: difference of integrals of basis functions h[M]
      lamda[M]: coefficients of h[M]
      tmpexp: inital logarithm at 0
  Out: s[T]: synthesized sinusoid
       tmpexp: logarithm at T

  No return value.
*/
void SinusoidExp(int T, cdouble* s, int M, cdouble* lamda, double** dih, cdouble& tmpexp)
{
	for (int t=0; t<T; t++)
	{
		s[t]=exp(tmpexp);
		cdouble dexp=0;
		for (int m=0; m<M; m++)	dexp+=lamda[m]*dih[m][t];
		tmpexp+=dexp;
	}
}//SinusoidExp

/*
  function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials

  In: CountSt, CountEn
      a3, a2, a1, a0: trinomial coefficients for log amplitude
      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
      ph: initial phase angle at 0
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.
       ph: phase angle at CountEn.

  No return value.
*/
void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
	double omg3, double omg2, double omg1, double omg0, double &ph, bool add)
{
  double p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
	if (add) for (int i=CountSt; i<CountEn; i++)
	{
		double lea=a0+i*(a1+i*(a2+i*a3));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
		data[i]+=exp(cdouble(lea, lph));
	}
	else for (int i=CountSt; i<CountEn; i++)
	{
		double lea=a0+i*(a1+i*(a2+i*a3));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
		data[i]=exp(cdouble(lea, lph));
	}
	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
}//SinusoidExpA

/*
  function SinusoidExpA: synthesizes complex sinusoid whose log amplitude and frequency are trinomials
  with phase angle specified at both ends.

  In: CountSt, CountEn
      a3, a2, a1, a0: trinomial coefficients for log amplitude
      omg3, omg2, omg1, omg0: trinomial coefficients for angular frequency
      ph0, ph2: phase angles at 0 and CountEn.
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.

  No return value.
*/
void SinusoidExpA(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
	double omg3, double omg2, double omg1, double omg0, double ph0, double ph2, bool add)
{
  double p0=ph0, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4;
	double pend=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));

	int k=floor((pend-ph2)/2/M_PI+0.5);
	double d=ph2-pend+2*M_PI*k;
	double _p=-2*d/CountEn/CountEn/CountEn;
	double _q=3*d/CountEn/CountEn;

	if (add) for (int i=CountSt; i<CountEn; i++)
	{
		double lea=a0+i*(a1+i*(a2+i*a3));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
    data[i]+=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
	}
	else for (int i=CountSt; i<CountEn; i++)
	{
		double lea=a0+i*(a1+i*(a2+i*a3));
		double lph=p0+i*(p1+i*(p2+i*(p3+i*p4)));
		data[i]=exp(cdouble(lea, lph+(i*i*(_q+i*_p))));
	}
}//SinusoidExpA

/*
  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
  frequency is h[M]'q[M]. This version also synthesizes its derivative.

  In: h[M][T], dh[M][T], dih[M][T]: basis functions and their derivatives and difference-integrals
      p[M], q[M]: real and imaginary parts of coefficients of h[M]
      tmpph: inital phase angle at 0
  Out: s[T], ds[T]: synthesized sinusoid and its derivative
       tmpph: phase angle at T

  No return value.
*/
void SinusoidExpA(int T, cdouble* s, cdouble* ds, int M, double* p, double* q, double** h, double** dh, double** dih, double& tmpph)
{
	for (int t=0; t<T; t++)
	{
		double e=0, dph=0, drr=0, dri=0;
		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t], drr+=p[m]*dh[m][t], dri+=q[m]*h[m][t];
		s[t]=exp(cdouble(e, tmpph));
		ds[t]=s[t]*cdouble(drr, dri);
		tmpph+=dph;
	}
}//SinusoidExpA

/*
  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
  frequency is h[M]'q[M]. This version does not synthesize its derivative.

  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
      p[M], q[M]: real and imaginary parts of coefficients of h[M]
      tmpph: inital phase angle at 0
  Out: s[T]: synthesized sinusoid
       tmpph: phase angle at T

  No return value.
*/
void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double& tmpph)
{
	for (int t=0; t<T; t++)
	{
		double e=0, dph=0;
		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
		s[t]=exp(cdouble(e, tmpph));
		tmpph+=dph;
	}
}//SinusoidExpA

/*
  function SinusoidExpA: synthesizes complex sinusoid piece whose log amplitude is h[M]'p[M] and
  frequency is h[M]'q[M] with phase angle specified at both ends. This version does not synthesize its
  derivative.

  In: h[M][T], dih[M][T]: basis functions and their difference-integrals
      p[M], q[M]: real and imaginary parts of coefficients of h[M]
      ph1, ph2: phase angles at 0 and T.
  Out: s[T]: synthesized sinusoid

  No return value.
*/
void SinusoidExpA(int T, cdouble* s, int M, double* p, double* q, double** h, double** dih, double ph1, double ph2)
{
	double pend=ph1;
	for (int t=0; t<T; t++)
	{
		double dph=0;
		for (int m=0; m<M; m++) dph+=q[m]*dih[m][t];
		pend+=dph;
	}

	int k=floor((pend-ph2)/2/M_PI+0.5);
	double d=ph2-pend+2*M_PI*k;
	double _p=-2*d/T/T/T;
	double _q=3*d/T/T;

	double ph=ph1;
	for (int t=0; t<T; t++)
	{
		double e=0, dph=0;
		for (int m=0; m<M; m++) e+=p[m]*h[m][t], dph+=q[m]*dih[m][t];
		if (e>300) e=300;
		if (e<-300) e=-300;
		s[t]=exp(cdouble(e, ph+(t*t*(_q+t*_p))));
		ph+=dph;
	}
}//SinusoidExpA

/*
//This is not used any longer as the recursion does not seem to help saving computation with all its overheads.
void SinusoidExp(cdouble* data, int CountSt, int CountEn, double a3, double a2, double a1, double a0,
	double omg3, double omg2, double omg1, double omg0, double &ea, double &ph, bool add)
{
  int i;
	double dea, ddea, dddea, ddddea,
				 dph, ddph, dddph, ddddph,
				 sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
				 e0=ea, e1=a0, e2=0.5*a1, e3=a2/3, e4=a3/4,
				 p0=ph, p1=omg0, p2=0.5*omg1, p3=omg2/3, p4=omg3/4, tmp;
	if (CountSt==0)
  {
		dea=e1+e2+e3+e4; ddea=2*e2+6*e3+14*e4; dddea=6*e3+36*e4; ddddea=24*e3;
		dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
  }
  else
  {
		ea=e0+CountSt*(e1+CountSt*(e2+CountSt*(e3+CountSt*e4)));
		dea=e1+e2+e3+e4+CountSt*(2*e2+3*e3+4*e4+CountSt*(3*e3+6*e4+CountSt*4*e4));
		ddea=2*e2+6*e3+14*e4+CountSt*(6*e3+24*e4+CountSt*12*e4);
		dddea=6*e3+36*e4+CountSt*24*e4; ddddea=24*e4;
		ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
		dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
		ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
		dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
	}
  sph=sin(ph), cph=cos(ph);
  sdph=sin(dph), cdph=cos(dph);
  sddph=sin(ddph), cddph=cos(ddph);
  sdddph=sin(dddph), cdddph=cos(dddph);
	sddddph=sin(ddddph), cddddph=cos(ddddph);
  if (add)
  {
    for (i=CountSt; i<CountEn; i++)
    {
			data[i]+=exp(ea)*cdouble(cph, sph);
			ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
			tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
			tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
			tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
			tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
		}
	}
	else
	{
		for (i=CountSt; i<CountEn; i++)
		{
			data[i]=exp(ea)*cdouble(cph, sph);
			ea=ea+dea; dea=dea+ddea; ddea=ddea+dddea; dddea+dddea+ddddea;
			tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
			tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
			tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
			tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
		}
	}
	ea=e0+CountEn*(e1+CountEn*(e2+CountEn*(e3+CountEn*e4)));
	ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
}   //*/

/*
  function Sinusoid: recursive cos-sin generator with trinomial frequency

  In: CountSt, CountEn
      f3, f2, f1, f0: trinomial coefficients of frequency
      ph: initial phase angle at 0 (NOT at CountSt)
  Out: datar[CountSt:CountEn-1], datai[CountSt:CountEn-1]: synthesized pair of cosine and sine functions
       ph: phase angle at CountEn

  No return value.
*/
void Sinusoid(double* datar, double* datai, int CountSt, int CountEn, double f3, double f2, double f1, double f0, double &ph)
{
  int i;
  double dph, ddph, dddph, ddddph,
         sph, cph, sdph, cdph, sddph, cddph, sdddph, cdddph, sddddph, cddddph,
         p0=ph, p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4, tmp;
  if (CountSt==0)
  {
    dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
  }
  else
  {
    ph=p0+CountSt*(p1+CountSt*(p2+CountSt*(p3+CountSt*p4)));
    dph=p1+p2+p3+p4+CountSt*(2*p2+3*p3+4*p4+CountSt*(3*p3+6*p4+CountSt*4*p4));
    ddph=2*p2+6*p3+14*p4+CountSt*(6*p3+24*p4+CountSt*12*p4);
    dddph=6*p3+36*p4+CountSt*24*p4; ddddph=24*p4;
  }
  sph=sin(ph), cph=cos(ph);
  sdph=sin(dph), cdph=cos(dph);
  sddph=sin(ddph), cddph=cos(ddph);
  sdddph=sin(dddph), cdddph=cos(dddph);
  sddddph=sin(ddddph), cddddph=cos(ddddph);

  for (i=CountSt; i<CountEn; i++)
  {
    datar[i]=cph; datai[i]=sph;
    tmp=cph*cdph-sph*sdph; sph=sph*cdph+cph*sdph; cph=tmp;
    tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
    tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
    tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
  }
  ph=p0+CountEn*(p1+CountEn*(p2+CountEn*(p3+CountEn*p4)));
}//Sinusoid*/

/*
  function Sinusoids: recursive harmonic multi-sinusoid generator

  In: st, en
      M: number of partials
      a3[M], a2[M], a1[M], a0[M]: trinomial coefficients for partial amplitudes
      f3, f2, f1, f0: trinomial coefficients for fundamental frequency
      ph[M]: partial phases at 0.
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[st:en-1]: output buffer.
       ph[M]: partial phases at en.

  No return value.
*/
void Sinusoids(int M, double* data, int st, int en, double* a3, double* a2, double* a1, double* a0, double f3, double f2, double f1, double f0, double* ph, bool add)
{
  double dph, ddph, dddph, ddddph;
  double sdph, cdph, cdph2, sddph, cddph, sdddph, cdddph, sddddph, cddddph, sdmph, cdmph, sdm_1ph, cdm_1ph;
  double p0, p1, p2, p3, p4, tmp, tmp2;
  double *a=(double*)malloc8(sizeof(double)*M*6), *da=&a[M], *dda=&a[M*2], *ddda=&a[M*3],
         *sph=&a[M*4], *cph=&a[M*5];

  for (int m=0; m<M; m++)
  {
    p0=ph[m], p1=2*M_PI*f0, p2=M_PI*f1, p3=2.0*M_PI*f2/3, p4=2.0*M_PI*f3/4;
    if (st==0)
    {
      a[m]=a0[m]; da[m]=a1[m]+a2[m]+a3[m]; dda[m]=2*a2[m]+6*a3[m]; ddda[m]=6*a3[m];
    }
    else
    {
      a[m]=a0[m]+st*(a1[m]+st*(a2[m]+st*a3[m]));
      da[m]=a1[m]+a2[m]+a3[m]+st*(2*a2[m]+3*a3[m]+st*3*a3[m]);
      dda[m]=2*a2[m]+6*a3[m]+st*6*a3[m]; ddda[m]=6*a3[m];
      ph[m]=p0+st*(p1+st*(p2+st*(p3+st*p4)));
    }
    sph[m]=sin(ph[m]), cph[m]=cos(ph[m]);
    ph[m]=p0+(m+1)*en*(p1+en*(p2+en*(p3+en*p4)));
  }

    if (st==0)
    {
      dph=p1+p2+p3+p4; ddph=2*p2+6*p3+14*p4; dddph=6*p3+36*p4; ddddph=24*p4;
    }
    else
    {
      dph=p1+p2+p3+p4+st*(2*p2+3*p3+4*p4+st*(3*p3+6*p4+st*4*p4));
      ddph=2*p2+6*p3+14*p4+st*(6*p3+24*p4+st*12*p4);
      dddph=6*p3+36*p4+st*24*p4; ddddph=24*p4;
    }
    sdph=sin(dph), cdph=cos(dph);
    sddph=sin(ddph), cddph=cos(ddph);
    sdddph=sin(dddph), cdddph=cos(dddph);
    sddddph=sin(ddddph), cddddph=cos(ddddph);

  if (add)
  {
    for (int i=st; i<en; i++)
    {
      data[i]+=a[0]*cph[0]; a[0]+=da[0]; da[0]+=dda[0]; dda[0]+=ddda[0];
      tmp=cph[0]*cdph-sph[0]*sdph; sph[0]=sph[0]*cdph+cph[0]*sdph; cph[0]=tmp;
      cdm_1ph=1, sdm_1ph=0, cdmph=cdph, sdmph=sdph, cdph2=2*cdph;

      for (int m=1; m<M; m++)
      {
        data[i]+=a[m]*cph[m]; a[m]+=da[m]; da[m]+=dda[m]; dda[m]+=ddda[m];
//        asm{mov ecx,m} asm{mov eax,a} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,cph} asm{fld qword ptr [edx+ecx*8]} asm{fmul st(0),st(1)} asm{mov edx,data} asm{mov ebx,i} asm{fadd qword ptr [edx+ebx*8]} asm{fstp qword ptr [edx+ebx*8]} asm{mov edx,da} asm{fld qword ptr [edx+ecx*8]} asm{fadd st(1),st(0)} asm{mov ebx,dda} asm{fld qword ptr [ebx+ecx*8]} asm{fadd st(1),st(0)} asm{mov edi,ddda} asm{fadd qword ptr [edi+ecx*8]} asm{fstp qword ptr [ebx+ecx*8]} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]}
        tmp=cdmph, tmp2=sdmph;
        cdmph=cdmph*cdph2-cdm_1ph; sdmph=sdmph*cdph2-sdm_1ph;
        cdm_1ph=tmp, sdm_1ph=tmp2;

        tmp=cph[m]*cdmph-sph[m]*sdmph; sph[m]=sph[m]*cdmph+cph[m]*sdmph; cph[m]=tmp;
//        asm{mov ecx,m} asm{mov eax,cph} asm{fld qword ptr [eax+ecx*8]} asm{mov edx,sph} asm{fld qword ptr [edx+ecx*8]} asm{fld st(1)} asm{fmul sdmph} asm{fld st(1)} asm{fmul sdmph} asm{fld cdmph} asm{fmul st(4),st(0)} asm{fmulp st(3),st(0)} asm{fsubp st(3),st(0)} asm{faddp} asm{fstp qword ptr [edx+ecx*8]} asm{fstp qword ptr [eax+ecx*8]}
      }

      tmp=cdph*cddph-sdph*sddph; sdph=sdph*cddph+cdph*sddph; cdph=tmp;
      tmp=cddph*cdddph-sddph*sdddph; sddph=sddph*cdddph+cddph*sdddph; cddph=tmp;
      tmp=cdddph*cddddph-sdddph*sddddph; sdddph=sdddph*cddddph+cdddph*sddddph; cdddph=tmp;
    }
  }
  else
  {
  }
  free8(a);
}//Sinusoids*/

/*
  function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients.

  In: CountSt, CountEn
      aa, ab, ac, ad: trinomial coefficients of amplitude.
      fa, fb, fc, fd: trinomial coefficients of frequency.
      ph0, ph2: phase angles at 0 and CountEn.
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: data[CountSt:CountEn-1]: output buffer.

  No return value.
*/
void Sinusoid(double* data, int CountSt, int CountEn, double aa, double ab, double ac, double ad,
  double fa, double fb, double fc, double fd, double ph0, double ph2, bool add)
{
  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
  int k=floor((pend-ph2)/2/M_PI+0.5);
  double d=ph2-pend+2*M_PI*k;
  double p=-2*d/CountEn/CountEn/CountEn;
  double q=3*d/CountEn/CountEn, a, ph;
  for (int i=CountSt; i<CountEn; i++)
  {
    a=ad+i*(ac+i*(ab+i*aa)); if (a<0) a=0;
    ph=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
    if (add) data[i]+=a*cos(ph);
    else data[i]=a*cos(ph);
  }
}//Sinusoid

/*
  function Sinusoid: synthesizes sinusoid piece from trinomial frequency and amplitude coefficients,
  returning sinusoid coefficients instead of waveform.

  In: CountSt, CountEn
      aa, ab, ac, ad: trinomial coefficients of amplitude (or log amplitude if LogA=true)
      fa, fb, fc, fd: trinomial coefficients of frequency.
      ph0, ph2: phase angles at 0 and CountEn.
      LogA: specifies whether log amplitude or amplitude is a trinomial
  Out: f[CountSt:CountEn-1], a[CountSt:CountEn-1], ph[CountSt:CountEn-1]: synthesized sinusoid parameters
       da[CountSt:CountEn-1]: derivative of synthesized amplitude, optional

  No return value.
*/
void Sinusoid(double* f, double* a, double* ph, double* da,	int CountSt, int CountEn, double aa, double ab,
  double ac, double ad,	double fa, double fb, double fc, double fd, double ph0, double ph2, bool LogA)
{
  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
  int k=floor((pend-ph2)/2/M_PI+0.5);
  double d=ph2-pend+2*M_PI*k;
  double p=-2*d/CountEn/CountEn/CountEn;
  double q=3*d/CountEn/CountEn;
	if (LogA) for (int i=CountSt; i<CountEn; i++)
	{
		a[i]=exp(ad+i*(ac+i*(ab+i*aa)));
		if (da) da[i]=a[i]*(ac+i*(2*ab+i*3*aa));
		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
	}
	else for (int i=CountSt; i<CountEn; i++)
	{
		a[i]=ad+i*(ac+i*(ab+i*aa));
		if (da) da[i]=ac+i*(2*ab+i*3*aa);
		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
	}
}//Sinusoid

/*
  function Sinusoid: generates trinomial frequency and phase with phase correction.

  In: CountSt, CountEn
      fa, fb, fc, fd: trinomial coefficients of frequency.
      ph0, ph2: phase angles at 0 and CountEn.
  Out: f[CountSt:CountEn-1], ph[CountSt:CountEn-1]: output buffers holding frequency and phase.

  No return value.
*/
void Sinusoid(double* f, double* ph, int CountSt, int CountEn, double fa, double fb,
  double fc, double fd, double ph0, double ph2)
{
  double pend=ph0+2*M_PI*CountEn*(fd+CountEn*(fc/2+CountEn*(fb/3+CountEn*fa/4)));
  int k=floor((pend-ph2)/2/M_PI+0.5);
  double d=ph2-pend+2*M_PI*k;
  double p=-2*d/CountEn/CountEn/CountEn;
  double q=3*d/CountEn/CountEn;
	for (int i=CountSt; i<CountEn; i++)
	{
		f[i]=fd+i*(fc+i*(fb+i*fa))+i*(2*q+3*i*p)/(2*M_PI);
    ph[i]=ph0+2*M_PI*i*(fd+i*((fc/2)+i*((fb/3)+i*fa/4)))+i*i*(q+i*p);
	}
}//Sinusoid

/*
  function SynthesizeSinusoid: synthesizes a time-varying sinusoid from a sequence of frequencies and amplitudes

  In: xs[Fr]: measurement points, should be integers although *xs has double type.
      fs[Fr], as[Fr]: sequence of frequencies and amplitudes at xs[Fr]
      phs[0]: initial phase angle at (int)xs[0].
      dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output buffer
  Out: xrec[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.
       phs[Fr]: phase angles at xs[Fr]

  Returns pointer to xrec.
*/
double* SynthesizeSinusoid(double* xrec, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add, bool* terminatetag)
{
  double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
         *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
  CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
  CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
  double ph=phs[0];
  for (int fr=0; fr<Fr-1; fr++)
  {
    phs[fr]=ph;
    ALIGN8(Sinusoid(&xrec[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, add);)
    if (terminatetag && *terminatetag) {delete[] f3; return 0;}
  }
  phs[Fr-1]=ph;
  ALIGN8(Sinusoid(&xrec[(int)xs[Fr-2]-dst], xs[Fr-1]-xs[Fr-2], den-xs[Fr-2], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2], ph, add);
         Sinusoid(&xrec[(int)xs[0]-dst], dst-xs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], phs[0], add);)
  delete[] f3;
  return xrec;
}//SynthesizeSinusoid

/*
  function ShiftTrinomial: shifts the origin of a trinomial from 0 to T

  In: a3, a2, a1, a0.
  Out: b3, b2, b1, b0, so that a3*x^3+a2*x^2+a1*x+a0=b3(x-T)^3+b2(x-T)^2+b1(x-T)+b0

  No return value.
*/
void ShiftTrinomial(double T, double& b3, double& b2, double& b1, double& b0, double a3, double a2, double a1, double a0)
{
  b3=a3;
  b2=a2+T*3*b3;
  b1=a1+T*(2*b2-T*3*b3);
  b0=a0+T*(b1-T*(b2-T*b3));
}//ShiftTrinomial

/*
  function SynthesizeSinusoidP: synthesizes a time-varying sinusoid from a sequence of frequencies,
  amplitudes and phase angles

  In: xs[Fr]: measurement points, should be integers although *xs has double type.
      fs[Fr], as[Fr], phs[Fr]: sequence of frequencies, amplitudes and phase angles at xs[Fr]
      dst, den: start and end time of synthesis, dst<=xs[0], den>=xs[Fr-1]
      add: specifies if the resynthesized sinusoid is to be added to or to replace the content of output
        buffer
  Out: xrecm[0:den-dst-1]: output buffer hosting synthesized sinusoid from dst to den.

  Returns pointer to xrecm.
*/
double* SynthesizeSinusoidP(double* xrecm, int dst, int den, double* phs, int Fr, double* xs, double* fs, double* as, bool add)
{
  double *f3=new double[Fr*8], *f2=&f3[Fr], *f1=&f3[Fr*2], *f0=&f3[Fr*3],
         *a3=&f3[Fr*4], *a2=&a3[Fr], *a1=&a3[Fr*2], *a0=&a3[Fr*3];
  CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
  CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
  for (int fr=0; fr<Fr-1; fr++) Sinusoid(&xrecm[(int)xs[fr]-dst], 0, xs[fr+1]-xs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], phs[fr], phs[fr+1], add);
  double tmpph=phs[0]; Sinusoid(&xrecm[(int)xs[0]-dst], dst-xs[0], 0, 0, 0, 0, a0[0], f3[0], f2[0], f1[0], f0[0], tmpph, add);
  //extend the trinomials on [xs[Fr-2], xs[Fr-1]) based at xs[Fr-2] to beyond xs[Fr-1] based at xs[Fr-1].
  tmpph=phs[Fr-1];
  ShiftTrinomial(xs[Fr-1]-xs[Fr-2], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], f3[Fr-2], f2[Fr-2], f1[Fr-2], f0[Fr-2]);
  ShiftTrinomial(xs[Fr-1]-xs[Fr-2], a3[Fr-1], a2[Fr-1], a1[Fr-1], a0[Fr-1], a3[Fr-2], a2[Fr-2], a1[Fr-2], a0[Fr-2]);
  Sinusoid(&xrecm[(int)xs[Fr-1]-dst], 0, den-xs[Fr-1], 0, 0, 0, a0[Fr-1], f3[Fr-1], f2[Fr-1], f1[Fr-1], f0[Fr-1], tmpph, add);
  delete[] f3;
  return xrecm;
}//SynthesizeSinusoidP