annotate vibrato.cpp @ 13:de3961f74f30 tip

Add Linux/gcc Makefile; build fix

/*
    Harmonic sinusoidal modelling and tools

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

    This program is free software; you can redistribute it and/or
    modify it under the terms of the GNU General Public License as
    published by the Free Software Foundation; either version 2 of the
    License, or (at your option) any later version.
*/
//---------------------------------------------------------------------------

#include <math.h>
#include <string.h>
#include "vibrato.h"
#include "arrayalloc.h"
#include "fft.h"
#include "hssf.h"
#include "matrix.h"
#include "splines.h"
#include "xcomplex.h"

/** \file vibrato.h */

//---------------------------------------------------------------------------
//method TVo::TVo: default constructor
TVo::TVo()
{
	memset(this, 0, sizeof(TVo));
	afres=8192;
}//TVo

//method TVo::~TVo: default destructor
TVo::~TVo()
{
  free(peakfr);
	delete[] lp;
  delete[] F0;
	if (fxr) DeAlloc2(fxr);
	if (fres) DeAlloc2(fres);
	if (LogASp) DeAlloc2(LogASp);
}//~TVo

/**
  method TVo::AllocateL: allocates or reallocates storage space whose size depends on L. This uses an
  external L value for allocation and updates L itself.
*/
void TVo::AllocateL(int AnL)
{
  L=AnL;
  delete[] F0;
  F0=new double[(L+2)*4];
  F0C=&F0[L+2], F0D=&F0[(L+2)*2], A0C=&F0[(L+2)*3];
}//AllocateL

/**
  method TVo::AllocateP: allocates or reallocates storage space whose size depends on P, using the
  interal P.
*/
void TVo::AllocateP()
{
  peakfr=(int*)realloc(peakfr, sizeof(int)*P);
  delete[] lp; lp=new double[(P+2)*3];
  Dp=&lp[P+2];
  Kp=(int*)&lp[(P+2)*2];
  if (LogASp) DeAlloc2(LogASp); Allocate2(double, P, M, LogASp);
}//AllocateP

/**
  method TVo::AllocatePK: allocates or reallocates storage space whose size depends on both P and K.
*/
void TVo::AllocatePK()
{
  if (fxr) DeAlloc2(fxr);
  Allocate2(double, P, 2*K+1, fxr);
  memset(fxr[0], 0, sizeof(double)*P*(2*K+1));
  if (fres) DeAlloc2(fres);
  Allocate2(double, P, K, fres);
}//AllocatePK

/**
  method TVo::GetOldP: returns the number of "equivalent" cycles of the whole duration. Although P-1
  cycles are delineated by lp[P], the parts before lp[0] and after lp[P-1] are also a part of the
  harmonic sinusoid being modeled and therefore should be taken into account when necessary.
*/
double TVo::GetOldP()
{
  return P-1+lp[0]/(lp[1]-lp[0])+(L-lp[P-1])/(lp[P-1]-lp[P-2]);
}//GetOldP

/**
  methodTVo::LoadFromFileHandle: reads TVo object from a file stream.
*/
void TVo::LoadFromFileHandle(FILE* file)
{
  fread(&M, sizeof(int), 1, file);
  fread(&L, sizeof(int), 1, file);
  fread(&P, sizeof(int), 1, file);
  fread(&K, sizeof(int), 1, file);
  fread(&h, sizeof(double), 1, file);
  fread(&afres, sizeof(int), 1, file);
  AllocateL(L); AllocateP(); AllocatePK();
  fread(F0C, sizeof(double), L, file);
  fread(A0C, sizeof(double), L, file);
  fread(LogAF, sizeof(double), 4096, file);
  fread(peakfr, sizeof(int), P, file);
  fread(lp, sizeof(double), P, file);
  fread(Dp, sizeof(double), P, file);
  fread(Kp, sizeof(int), P, file);
  fread(fxr[0], sizeof(double), P*(2*K+1), file);
  fread(LogASp[0], sizeof(double), P*M, file);
}//LoadFromFileHandle

/**
  method TVo::ReAllocateL: reallocates storage space whose size depends on L, and transfer the contents.
  This uses an external L value for allocation but does not update L itself.
*/
void TVo::ReAllocateL(int newL)
{
  double* newF0=new double[(newL+2)*4];
  F0C=&newF0[newL+2], F0D=&newF0[(newL+2)*2], A0C=&newF0[(newL+2)*3];
  memcpy(A0C, &F0[(L+2)*3], sizeof(double)*(L+2));
  memcpy(F0D, &F0[(L+2)*2], sizeof(double)*(L+2));
  memcpy(F0C, &F0[L+2], sizeof(double)*(L+2));
  memcpy(newF0, F0, sizeof(double)*(L+2));
  delete[] F0;
  F0=newF0;
}//ReAllocateL

/**
  method TVo::SaveToFile: saves a TVo object to a file.

  In: filename: path to the destination file.
*/
void TVo::SaveToFile(char* filename)
{
  FILE* file=fopen(filename, "wb");
  if (file)
  {
    SaveToFileHandle(file);
    fclose(file);
  }
  else
  {
    throw(strcat((char*)"Cannot open file ", filename));
  }
}//SaveToFile

/**
  method TVo::SaveToFileHandle: saves a TVo object to an open file stream.
*/
void TVo::SaveToFileHandle(FILE* file)
{
  fwrite(&M, sizeof(int), 1, file);
  fwrite(&L, sizeof(int), 1, file);
  fwrite(&P, sizeof(int), 1, file);
  fwrite(&K, sizeof(int), 1, file);
	fwrite(&h, sizeof(double), 1, file);
  fwrite(&afres, sizeof(int), 1, file);
	fwrite(F0C, sizeof(double), L, file);
  fwrite(A0C, sizeof(double), L, file);
	fwrite(LogAF, sizeof(double), 4096, file);
  fwrite(peakfr, sizeof(int), P, file);
  fwrite(lp, sizeof(double), P, file);
	fwrite(Dp, sizeof(double), P, file);
  fwrite(Kp, sizeof(int), P, file);
  fwrite(fxr[0], sizeof(double), P*(2*K+1), file);
	fwrite(LogASp[0], sizeof(double), P*M, file);
}//SaveToFileHandle

//---------------------------------------------------------------------------
//functions


/**
  function AnalyzeV: calculates all basic and non-basic descriptors of a vibrato from a harmonic
  sinusoid

  In: HS: harmonic sinusoid to compute vibrato representation from
      sps: sampling rate
      h: hop size
      SFMode: specifies source-filter estimation criterion, 0=FB (filter bank), 1=SV (slow variation)
      SFF: filter response sampling interval
      SFScale: set if filter sampling uses mel scale
      SFtheta: balancing factor of amplitude and frequency variations, needed for SV approach
  Out: V: a TVo object hosting vibrato descriptors (vibrato representation) of HS
       cyclefrcount: number of cycles between cycle boundaries, equals V.P-1.
       cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: time (in frames) and F0 centroid of cycles. These are
         knots from which V.F0C[] is interpolated.
       peaky[V.P]: shape score of cycle peaks

  No return value.
*/
void AnalyzeV(THS& HS, TVo& V, double*& peaky, double*& cyclefrs, double*& cyclefs, double sps, double h, int* cyclefrcount,
      int SFMode, double SFF, int SFFScale, double SFtheta)
{
  int M=HS.M, Fr=HS.Fr;
  if (M<=0 || Fr<=0) return;
  atom** Partials=HS.Partials;

  //basic descriptor: h
  V.h=h;

  //demodulating pitch
  //Basic descriptor: L
  V.AllocateL(Fr);

  double* AA=new double[Fr];
  //find the pitch track
  V.F0max=0, V.F0min=1;
  for (int fr=0; fr<Fr; fr++)
  {
    double suma2=0, suma2p=0;
    for (int m=0; m<M; m++)
    {
      double a=Partials[m][fr].a, p=Partials[m][fr].f/(m+1);
      if (p<=0) break;
      suma2+=a*a, suma2p+=a*a*p;
    }
    V.F0[fr]=suma2p/suma2;
    if (V.F0max<V.F0[fr]) V.F0max=V.F0[fr];
    if (V.F0min>V.F0[fr]) V.F0min=V.F0[fr];
    AA[fr]=suma2;
  }

  //Basic descriptor: M
  V.M=0.45/V.F0max;
  if (V.M>M) V.M=M;

  //rough estimation of rate
  double* f0d=new double[Fr-1]; for (int i=0; i<Fr-1; i++) f0d[i]=V.F0[i+1]-V.F0[i];
  RateAndReg(V.rate, V.regularity, f0d, 0, Fr-2, 8, sps, h);
  delete[] f0d;

  //find peaks of the pitch track
  free(V.peakfr); V.peakfr=(int*)malloc(sizeof(int)*Fr/2);
  double periodinframe=sps/(V.rate*h);
  FindPeaks(V.peakfr, V.P, V.F0, Fr, periodinframe, peaky);
  if (V.P>=1) //de-modulation
  {
    //Basic descriptor: F0C[]
    DeFM(V.F0C, V.F0, AA, V.P, V.peakfr, Fr, V.F0Overall, *cyclefrcount, cyclefrs, cyclefs, true);
    //Basic descriptor: A0C[]
    DeAM(V.A0C, AA, V.P, V.peakfr, Fr);
  }

  V.F0Cmax=0; V.F0Dmax=-1; V.F0Cmin=1; V.F0Dmin=1;
  for (int fr=0; fr<Fr; fr++)
  {
    if (V.F0Cmax<V.F0C[fr]) V.F0Cmax=V.F0C[fr];
    if (V.F0Cmin>V.F0C[fr]) V.F0Cmin=V.F0C[fr];
    V.F0D[fr]=V.F0[fr]-V.F0C[fr];
  }

  V.D=0; int cyclestart=(V.P>=2)?V.peakfr[0]:0, cycleend=(V.P>=2)?V.peakfr[V.P-1]:Fr;
  for (int fr=cyclestart; fr<cycleend; fr++)
  {
    double lf0d=V.F0D[fr];
    if (V.F0Dmax<lf0d) V.F0Dmax=lf0d;
    if (V.F0Dmin>lf0d) V.F0Dmin=lf0d;
    V.D+=lf0d*lf0d;
  }
  V.D=sqrt(V.D/(cycleend-cyclestart));
  delete[] AA;

  //Calculate rate and regularity
  //find the section used for calculating regularity and rate
  {
    int frst=0, fren=Fr-1;
    while (frst<fren && V.F0D[frst]*V.F0D[frst+1]>0) frst++;
    while (fren>frst && V.F0D[fren]*V.F0D[fren-1]>0) fren--;

    RateAndReg(V.rate, V.regularity, V.F0D, frst, fren, 8, sps, h);
  }

  //Basic descriptor: P
  //Basic descriptor: peakfr[]
  periodinframe=sps/(V.rate*h);
  FindPeaks(V.peakfr, V.P, V.F0D, Fr, periodinframe, peaky);

  //Basic descriptor: lp[]
  V.AllocateP();
  for (int p=0; p<V.P; p++)
  {
    double startfr; QIE(&V.F0D[V.peakfr[p]], startfr); V.lp[p]=startfr+V.peakfr[p];
  }

  //Basic descriptor: LogAF[]
  //Source-filter modeling
  if (SFMode==0)
    S_F(V.afres, V.LogAF, V.LogAS, Fr, M, Partials, V.A0C, V.lp, V.P, V.F0Overall);
  else
    S_F_SV(V.afres, V.LogAF, V.LogAS, Fr, M, Partials, V.A0C, V.lp, V.P, SFF, SFFScale, SFtheta, sps);


  //the alternative: find K only
  V.K=50;
  if (V.K>periodinframe/2) V.K=periodinframe/2;

  //single-cycle analyses
  //Basic descriptor: Dp[]
  V.AllocatePK();
  for (int p=1; p<V.P; p++)
  {
    //int frst=ceil(V.lp[p-1]), freninc=floor(V.lp[p]);
    int frst=V.peakfr[p-1], freninc=V.peakfr[p];
    V.Dp[p]=0; for (int fr=frst; fr<=freninc; fr++) V.Dp[p]+=V.F0D[fr]*V.F0D[fr]; V.Dp[p]=sqrt(V.Dp[p]/(freninc-frst+1));
    double* f0e=new double[freninc-frst+1];
    for (int fr=0; fr<=freninc-frst; fr++) f0e[fr]=V.F0D[frst+fr]/V.Dp[p];
    V.Kp[p]=V.K; FS_QR(V.Kp[p], V.fxr[p], &f0e[0], freninc-frst, V.lp[p]-V.lp[p-1], V.lp[p-1]-frst, V.fres[p]);
    delete[] f0e;

    memset(V.LogASp[p], 0, sizeof(double)*V.M);
    for (int m=0; m<V.M; m++)
    {
      int ccount=0;
      for (int fr=frst; fr<=freninc; fr++)
      {
        double f=Partials[m][fr].f*V.afres;
        if (f<=0) continue;
        int intf=floor(f);
        V.LogASp[p][m]=V.LogASp[p][m]+log(Partials[m][fr].a/V.A0C[fr])-(V.LogAF[intf]*(intf+1-f)+V.LogAF[intf+1]*(f-intf));
        ccount++;
      }
      if (ccount>0) V.LogASp[p][m]/=ccount;
      else V.LogASp[p][m]=0;
    }
  }
  memset(V.FXR, 0, sizeof(double)*2*V.K); memset(V.FRes, 0, sizeof(double)*V.K);
  double sumdp=0; for (int p=1; p<V.P; p++) sumdp+=V.Dp[p], V.FXR[0]+=V.fxr[p][0]*V.Dp[p], V.FRes[0]+=V.fres[p][0]*V.Dp[p]; V.FXR[0]/=sumdp, V.FRes[0]/=sumdp;
  for (int k=1; k<V.K; k++)
  {
    double sumdp=0;
    for (int p=1; p<V.P; p++)
    {
      if (k<V.Kp[p])
      {
        V.FXR[2*k-1]+=V.fxr[p][2*k-1]*V.Dp[p];
        V.FXR[2*k]+=V.fxr[p][2*k]*V.Dp[p];
        V.FRes[k]+=V.fres[p][k]*V.Dp[p];
        sumdp+=V.Dp[p];
      }
    }
    V.FXR[2*k-1]/=sumdp, V.FXR[2*k]/=sumdp, V.FRes[k]/=sumdp;
  }
}//AnalyzeV

/**
  function copeak: measures the similarity of F0 between frst and fren to a cosine cycle 0~2pi.

  In: F0[peakfr[frst]:peakfr[fren]]
      periodinframe: period of the cosine reference, in frames

  Returns the correlation coefficient.
*/
double copeak(double* F0, int* peakfr, int frst, int fren, double periodinframe)
{
  double ef0=0, rfs=0, rff=0, rss=0; int frcount=0;
  for (int fr=peakfr[frst]; fr<=peakfr[fren]; fr++) ef0+=F0[fr], frcount++; ef0/=frcount;
  for (int fr=peakfr[frst]; fr<=peakfr[fren]; fr++)
  {
    double s=cos(2*M_PI*(fr-peakfr[frst])/periodinframe), f=F0[fr];
    rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s;
  }
  return rfs/sqrt(rff*rss);
}//copeak

/**
  function CorrectFS: time-shifts Fourier series so that maximum is reached at 0. This is done by
  finding the actual peak numerically then shifting it to 0.

  In: c[2K-1]: Fourier series coefficients, in the order of 0r, 1i, 1r, 2i, 2r, ..., so that the series
        is expanded as c[0]+c[1]sinx+c[2]cosx+c[3]sin2x+c[4]cos2x+...c[2K-2]cos(K-1)x
      epx: a small positive value for convergence test for finding the peak
      maxiter: maximal number of iterates for finding the peak
  Out: c[2K-1]: updated Fourier series coefficients.

  No return value.
*/
void CorrectFS(int K, double* c, double epx=1e-8, int maxiter=10)
{
  double y=0, kb=0, kka=0, th2pi=0, dth2pi=0, my, mth2pi;
  epx=2*M_PI*epx;
  int iter=0;
  for (int k=1; k<K; k++) y+=c[2*k], kb+=k*c[2*k-1], kka+=k*k*c[2*k]; my=y;
  dth2pi=(kka>0)?(kb/kka):(kb*0.1);
  while (iter<maxiter && fabs(dth2pi)>epx)
  {
    iter++;
    th2pi+=dth2pi;
    kb=kka=y=0;
    for (int k=1; k<K; k++)
    {
      double cc=cos(k*th2pi), ss=sin(k*th2pi);
      y+=(c[2*k]*cc+c[2*k-1]*ss);
      kb+=k*(-c[2*k]*ss+c[2*k-1]*cc);
      kka+=k*k*(c[2*k]*cc+c[2*k-1]*ss);
    }
    if (y>my) my=y, mth2pi=th2pi;
    dth2pi=(kka>0)?(kb/kka):(kb*0.1);
  }
  if (iter>=maxiter || th2pi!=mth2pi)
  {
    th2pi=mth2pi;
  }
  for (int k=1; k<K; k++)
  {
    double cc=cos(k*th2pi), ss=sin(k*th2pi);
    double tmp=c[2*k]*cc+c[2*k-1]*ss;
    c[2*k-1]=-c[2*k]*ss+c[2*k-1]*cc; c[2*k]=tmp;
  }
}//CorrectFS

/**
  function DeAM: amplitude demodulation based on given segmentation into cycles

  In: A1[Fr]: original amplitude
      peakfr[npfr]: cycle boundaries (literally, "frame indices of frequency modulation peaks")
  Out: A2[Fr]: demodulated amplitude

  No return value.
*/
void DeAM(double* A2, double* A1, int npfr, int* peakfr, int Fr)
{
  if (npfr==1) {memcpy(A2, A1, sizeof(double)*Fr); return;}
  double *frs=new double[npfr*2], *a=&frs[npfr];
  for (int i=0; i<npfr-1; i++)
  {
    int frst=peakfr[i], fren=peakfr[i+1];
    a[i]=(A1[frst]+A1[fren])*0.5;
    frs[i]=(frst*A1[frst]+fren*A1[fren])*0.5;
    for (int fr=frst+1; fr<fren; fr++) a[i]+=A1[fr], frs[i]+=fr*A1[fr];
    frs[i]/=a[i]; a[i]=log(sqrt(a[i]/(fren-frst)));
  }
  if (npfr==2)
  {
    A2[0]=exp(a[0]);
    for (int fr=1; fr<Fr; fr++) A2[fr]=A2[0];
  }
  else if (npfr==3)
  {
    double alp=(a[1]-a[0])/(frs[1]-frs[0]);
    for (int fr=0; fr<Fr; fr++) A2[fr]=exp(a[0]+(fr-frs[0])*alp);
  }
  else if (npfr==4)
  {
    double x0=frs[0], x1=frs[1], x2=frs[2], y0=log(a[0]), y1=log(a[1]), y2=log(a[2]);
    double ix0_12=y0/((x0-x1)*(x0-x2)), ix1_02=y1/((x1-x0)*(x1-x2)), ix2_01=y2/((x2-x0)*(x2-x1));
    double aa=ix0_12+ix1_02+ix2_01,
           b=-(x1+x2)*ix0_12-(x0+x2)*ix1_02-(x0+x1)*ix2_01,
           c=x1*x2*ix0_12+x0*x2*ix1_02+x0*x1*ix2_01;
    for (int fr=0; fr<Fr; fr++) A2[fr]=exp(c+fr*(b+fr*aa));
  }
  else
  {
    double *fa=new double[npfr*4], *fb=&fa[npfr], *fc=&fa[npfr*2], *fd=&fa[npfr*3];
    CubicSpline(npfr-2, fa, fb, fc, fd, frs, a, 0, 1, A2, 1, 0, Fr-1);
    for (int fr=0; fr<Fr; fr++) A2[fr]=exp(A2[fr]);
    delete[] fa;
  }
  delete[] frs;
}//DeAM

/**
  function DeAM: wrapper function using floating-point cycle boundaries

  In: A1[Fr]: original amplitude
      lp[npfr]: cycle boundaries
  Out: A2[Fr]: demodulated amplitude

  No return value.
*/
void DeAM(double* A2, double* A1, int npfr, double* lp, int Fr)
{
  int* peakfr=(int*)malloc(sizeof(int)*npfr);
  for (int i=0; i<npfr; i++) peakfr[i]=ceil(lp[i]);
  DeAM(A2, A1, npfr, peakfr, Fr);
  free(peakfr);
}//DeAM

/**
  function DeFM: frequency demodulation based on given segmentation into cycles

  In: f1[Fr], AA[Fr]: original frequency and partial-independent amplitude
      peakfr[npfr]: cycle boundaries (literally, "frame indices of frequency modulation peaks")
      furthursmoothing: specifies if a second round demodulation is to be performed for more smooth
        carrier
  Out: f2[Fr]: demodulated frequency
       f[fcount], frs[fcount]: frequency and time (in frames) of carrier knots, optional
       fct: frequency centroid

  No return value.
*/
void DeFM(double* f2, double* f1, double* AA, int npfr, int* peakfr, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing)
{
  double ac=0;
  fct=0;

  if (npfr==1)
  {
    memcpy(f2, f1, sizeof(double)*Fr);
    for (int fr=0; fr<Fr; fr++) ac+=AA[fr], fct+=f1[fr]*AA[fr]; fct/=ac;
    return;
  }

  bool localfrs=(frs==(double*)0xFFFFFFFF), localf=(f==(double*)0xFFFFFFFF);
  if (!localfrs) delete[] frs;
  if (!localf) delete[] f;
  frs=new double[npfr]; f=new double[npfr];
  for (int i=0; i<npfr-1; i++)
  {
    int frst=peakfr[i], fren=peakfr[i+1];
    f[i]=(f1[frst]*AA[frst]+f1[fren]*AA[fren])*0.5;
    frs[i]=(frst*AA[frst]+fren*AA[fren])*0.5;
    double lrec=(AA[frst]+AA[fren])*0.5;
    for (int fr=frst+1; fr<fren; fr++) f[i]+=f1[fr]*AA[fr], frs[i]+=fr*AA[fr], lrec+=AA[fr];
    ac+=lrec;
    fct+=f[i];
    f[i]/=lrec, frs[i]/=lrec;
  }
  fct/=ac;

  if (furthersmoothing)
  {
    //further smoothing
    int newnpfr=1;
    for (int pfr=1; pfr<npfr-2; pfr++)
    {
      if ((f[pfr]>f[pfr-1] && f[pfr]>f[pfr+1]) || (f[pfr]<f[pfr-1] && f[pfr]<f[pfr+1]))
      {
        //this is a peak or valley
        if ((pfr+1<npfr-2 && f[pfr+1]>f[pfr] && f[pfr+1]>f[pfr+2]) || (f[pfr+1]<f[pfr] && f[pfr+1]<f[pfr+2]))
        {
          frs[newnpfr]=0.5*(frs[pfr]+frs[pfr+1]), f[newnpfr]=0.5*(f[pfr]+f[pfr+1]);
          newnpfr++;
        }
      }
      else
      {
        frs[newnpfr]=frs[pfr], f[newnpfr]=f[pfr];
        newnpfr++;
      }
    }
    frs[newnpfr]=frs[npfr-2], f[newnpfr]=f[npfr-2]; newnpfr++; newnpfr++;
    npfr=newnpfr;
  }

  Smooth_Interpolate(f2, Fr, npfr, f, frs);
  fcount=npfr-1;

  if (localfrs) delete[] frs;
  if (localf) delete[] f;
}//DeFM

/**
  function DeFM: wrapper function using floating-point cycle boundaries

  In: f1[Fr], AA[Fr]: original frequency and partial-independent amplitude
      lp[npfr]: cycle boundaries
      furthursmoothing: specifies if a second round demodulation is to be performed for more smooth
        carrier
  Out: f2[Fr]: demodulated frequency
       f[fcount], frs[fcount]: frequency and time (in frames) of carrier knots, optional
       fct: frequency centroid

  No return value.
*/
void DeFM(double* f2, double* f1, double* AA, int npfr, double* lp, int Fr, double& fct, int& fcount, double* &frs, double* &f, bool furthersmoothing)
{
  int* peakfr=(int*)malloc(sizeof(int)*npfr);
  for (int i=0; i<npfr; i++) peakfr[i]=ceil(lp[i]);
  DeFM(f2, f1, AA, npfr, peakfr, Fr, fct, fcount, frs, f, furthersmoothing);
  free(peakfr);
}//DeFM

/**
  function FindPeaks: find modulation peaks of signal F0[] roughly periodical at $periodinframe

  In: F0[Fr]: modulated signal
      periodinframe: reference modulation period
  Out: npfr, peakfr[npfr]: modulation peaks
       peaky[npfr]: shape score for the peaks measuring their similarity to cosine peak, optional

  No return value.
*/
void FindPeaks(int* peakfr, int& npfr, double* F0, int Fr, double periodinframe, double*& peaky)
{
  npfr=0;
  for (int fr=1; fr<Fr-1; fr++)
  {
    if (F0[fr]>F0[fr-1] && F0[fr]>F0[fr+1])
    {
      peakfr[npfr++]=fr;
    }
  }

  bool localpeaky=(peaky==(double*)0xFFFFFFFF); if (!localpeaky) delete[] peaky;

  peaky=new double[npfr];
  for (int pfr=0; pfr<npfr; pfr++)
  {
    double rfs=0, rff=0, rss=0;
    if (peakfr[pfr]>periodinframe/2)
    {
      double ef0=0; int frcount=0; for (int fr=peakfr[pfr]-periodinframe/2; fr<peakfr[pfr]; fr++) ef0+=F0[fr], frcount++; ef0/=frcount;
      for (int fr=peakfr[pfr]-periodinframe/2; fr<peakfr[pfr]; fr++)
      {
        double s=cos(2*M_PI*(fr-peakfr[pfr])/periodinframe), f=F0[fr];
        rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s;
      }
    }
    if (peakfr[pfr]<Fr-periodinframe/2)
    {
      double ef0=0; int frcount=0; for (int fr=peakfr[pfr]; fr<peakfr[pfr]+periodinframe/2; fr++) ef0+=F0[fr], frcount++; ef0/=frcount;
      for (int fr=peakfr[pfr]; fr<peakfr[pfr]+periodinframe/2; fr++)
      {
        double s=cos(2*M_PI*(fr-peakfr[pfr])/periodinframe), f=F0[fr];
        rfs+=(f-ef0)*s; rff+=(f-ef0)*(f-ef0); rss+=s*s;
      }
    }
    peaky[pfr]=rfs/sqrt(rff*rss);
  }
  int pst=0; for (int pfr=1; pfr<npfr; pfr++) if (peaky[pst]<peaky[pfr]) pst=pfr;
//    return;
  //*
  int pen=pst+1, pfr=pst+1;
  while (pfr<npfr)
  {
    int impfr=-1;
    while (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*0.5) pfr++;
    if (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*1.5)
    {
      double mpfr=peaky[pfr]+copeak(F0, peakfr, pen-1, pfr, periodinframe);
      impfr=pfr; pfr++;
      while (pfr<npfr && peakfr[pfr]<peakfr[pen-1]+periodinframe*1.5)
      {
        double mp=peaky[pfr]+copeak(F0, peakfr, pen-1, pfr, periodinframe);
        if (mp>mpfr) impfr=pfr, mpfr=mp;
        pfr++;
      }
    }
    else
    {
      while (pfr<npfr)
      {
        if (peaky[pfr]>0) {impfr=pfr; break;}
        pfr++;
      }
    }
    if (impfr>=0) peakfr[pen++]=peakfr[impfr];
  }
  pst--; pfr=pst;
  while (pfr>=0)
  {
    int impfr=-1;
    while (pfr>=0 && peakfr[pfr]>peakfr[pst+1]-periodinframe*0.5) pfr--;
    if (pfr>=0 && peakfr[pfr]>=peakfr[pst+1]-periodinframe*1.5)
    {
      double mpfr=peaky[pfr]+copeak(F0, peakfr, pfr, pst+1, periodinframe);
      impfr=pfr; pfr--;
      while (pfr>=0 && peakfr[pfr]>=peakfr[pst+1]-periodinframe*1.5)
      {
        double mp=peaky[pfr]+copeak(F0, peakfr, pfr, pst+1, periodinframe);
        if (mp>mpfr) impfr=pfr, mpfr=mp;
        pfr--;
      }
    }
    else
    {
      while (pfr>=0)
      {
        if (peaky[pfr]>0) {impfr=pfr; break;}
        pfr--;
      }
    }
    if (impfr>=0) peakfr[pst--]=peakfr[impfr];
  }
  pst++;
  npfr=pen-pst;
  memmove(peakfr, &peakfr[pst], sizeof(int)*npfr); //*/
  if (localpeaky) delete[] peaky;
}//FindPeaks

/**
  function FS: Fourier series decomposition

  In: data[frst:fren]: signal to decompose
      period: period of Fourier series decomposition
      shift: amount of original shift of Fourier components (from frst to frst-shift)
      K: number of Fourier components requested
  Out: K: number of Fourier components returned
       FXR[K], FXI[K]: real and imaginary parts of Fourier series coefficients
       FRES[K]: decomposition residues

  No return value.
*/
void FS(int& K, double* FXR, double* FXI, double* FRES, double* data, int frst, int fren, double period, double shift)
{
  if (K>period/2) K=period/2;
  int len=fren-frst+1;
  double omg0=2*M_PI/period, ilen=1.0/len;

  //for (int fr=frst; fr<=fren; fr++) data[fr]=cos(2*M_PI*fr/period+1);

  double* res=new double[len]; memcpy(res, &data[frst], sizeof(double)*len);
  double* rec=new double[len];
  double resene=0; for (int i=0; i<len; i++) resene+=res[i]*res[i]; double ene=resene;

  for (int k=0; k<K; k++)
  {
//      double eer=0, eei=0;
    double xr=0, xi=0, omg=k*omg0;
    for (int fr=frst; fr<=fren; fr++)
    {
      double ph=omg*(fr-shift);
      double c=cos(ph), s=sin(ph);
      xr+=data[fr]*c, xi+=data[fr]*s;
//        eer+=c*c, eei+=s*s;
    }
    xr*=ilen, xi*=ilen;
//      if (eer>0) xr/=eer;
//      if (eei>0) xi/=eei;
    FXR[k]=xr, FXI[k]=xi;
    double lresene=0;
    for (int fr=frst; fr<=fren; fr++)
    {
      double ph=omg*(fr-shift);
//        rec[fr-frst]=xr*cos(ph)+xi*sin(ph);
      if (k==0) rec[fr-frst]=xr*cos(ph)+xi*sin(ph);
      else rec[fr-frst]=2*(xr*cos(ph)+xi*sin(ph));
      res[fr-frst]-=rec[fr-frst];
      lresene+=res[fr-frst]*res[fr-frst];
    }
    resene=lresene;
    FRES[k]=resene/ene;
  }
  delete[] rec;
  delete[] res;
}//FS

/**
  function FS_QR: Fourier series decomposition with QR orthogonalization. Since the Fourier series is
  applied on finite-length discrate signal, the Fourier components are no longer orthogonal to each
  other. A decreasing residue can be guaranteed by applying QR (or any other) orthogonalization process
  to the Fourier components before decomposition.

  In: data[0:Fr]: signal to decompose
      period: period of Fourier series decomposition
      shift: amount of original shift of Fourier components (from 0 to -shift)
      K: number of Fourier components requested
  Out: K: number of Fourier components returned
       FXR[2K-1]: Fourier series coefficients, in the order of 0r, 1i, 1r, 2i, 2r, ....
       FRES[K]: decomposition residues, optional

  No return value.
*/
void FS_QR(int& K, double* FXR, double* data, int Fr, double period, double shift, double* FRES)
{
  int len=Fr+1;
  if (K>period/2) K=period/2;
  if (K>Fr/2) K=Fr/2;
  if (K>len/2) K=len/2;
  if (K<=0) return;
  memset(FXR, 0, sizeof(double)*(2*K-1));

  double omg0=2*M_PI/period, ene, resene;

  if (FRES)
  {
    ene=0; for (int i=0; i<len; i++) ene+=data[i]*data[i]; resene=ene;
  }

  /*debug only
  double *res=new double[len*4], *rec=&res[len];
  memcpy(res, data, sizeof(double)*len); memset(rec, 0, sizeof(double)*len);
  double resene2=resene;//*/

  int NK=K*2-1;

  Alloc2(len, NK, A); Alloc2(len, len, Q); Alloc2(len, NK, R);
  for (int fr=0; fr<=Fr; fr++) A[fr][0]=1;
  for (int k=1; k<(NK+1)/2; k++)
  {
    double omg=k*omg0;
    for (int fr=0; fr<=Fr; fr++){double ph=omg*(fr-shift); A[fr][2*k-1]=2*sin(ph), A[fr][2*k]=2*cos(ph);}
  }
  QR_householder(len, NK, A, Q, R);///////////////////
  GIUT(NK, R);
  for (int k=0; k<NK; k++)
  {
    double xr=0;
    for (int fr=0; fr<=Fr; fr++)
    {
      double c=Q[fr][k];
      xr+=data[fr]*c;
    }
    for (int kk=0; kk<=k; kk++) FXR[kk]+=xr*R[kk][k];
    /*debug only
    double lresene=0;
    for (int fr=0; fr<=Fr; fr++)
    {
      rec[fr]=0; for (int kk=0; kk<=k; kk++) rec[fr]+=FXR[kk]*A[fr][kk];
      res[fr]=data[fr]-rec[fr];
      lresene+=res[fr]*res[fr];
    }
    resene2=lresene;   */

    if (FRES)
    {
      resene-=xr*xr;
      if (k%2==0) FRES[k/2]=resene/ene;
    }
  }
  /* debug only
  delete[] res; //*/
  DeAlloc2(A); DeAlloc2(Q); DeAlloc2(R);
}//FS_QR

/**
  function InterpolateV: adjusts modulation rate to $rate times the current value. Since TVo is based on
  individual cycles, this operation involves finding new cycle boundaries and recomputing other single-
  cycle descriptors by interpolation. This is used for time stretching and cycle rate adjustment.

  In: V: a TVo object to adjust
      newP: number of total cycles after adjustment.
      rate: amount of adjustment.
  Out: another TVo object with new cycle boundaries and single-cycle descriptors interpoated from V

  Returns pointer to the output TVo. This function does not affect the content in V.
*/
TVo* InterpolateV(double newP, double rate, TVo& V)
{
  double Pst=-V.lp[0]/(V.lp[1]-V.lp[0]);
  double oldP=V.P-1+(V.L-V.lp[V.P-1])/(V.lp[V.P-1]-V.lp[V.P-2])-Pst;

  TVo* newV=new TVo;
  memcpy(newV, &V, sizeof(TVo));

  newV->F0C=0; newV->A0C=0; newV->peakfr=0; newV->Dp=0; newV->lp=0, newV->Kp=0;
  newV->fxr=0; newV->LogASp=0; newV->F0=0; newV->F0D=0; newV->fres=0;

  int inewP=ceil(newP+Pst);
  newV->P=inewP;

  newV->peakfr=(int*)malloc(sizeof(int)*inewP);
  newV->AllocateP(); newV->AllocatePK();

  newV->lp[0]=V.lp[0]/rate;
  newV->peakfr[0]=floor(newV->lp[0]+0.5);
  for (int p=1; p<inewP; p++)
  {
    double rp=1+(p-1)*(oldP-2+Pst)/(newP-2+Pst);
    int ip=floor(rp); rp=rp-ip;
    if (ip+1>=V.P) ip=V.P-1, rp=0;
    if (fabs(rp)<1e-16)
    {
      newV->lp[p]=newV->lp[p-1]+(V.lp[ip]-V.lp[ip-1])/rate;
      newV->Dp[p]=V.Dp[ip];
      newV->Kp[p]=V.Kp[ip];
      memcpy(newV->fxr[p], V.fxr[ip], sizeof(double)*2*newV->Kp[p]);
      memcpy(newV->LogASp[p], V.LogASp[ip], sizeof(double)*V.M);
    }
    else
    {
      double fv1=1.0/(V.lp[ip]-V.lp[ip-1]), fv2=1.0/(V.lp[ip+1]-V.lp[ip]);
      double fv=rate*(fv1*(1-rp)+fv2*rp);
      newV->lp[p]=newV->lp[p-1]+1.0/fv;
      newV->Dp[p]=V.Dp[ip]*(1-rp)+V.Dp[ip+1]*rp;
      int minK=V.Kp[ip]; if (minK>V.Kp[ip+1]) minK=V.Kp[ip+1];
      for (int k=0; k<2*minK-1; k++) newV->fxr[p][k]=V.fxr[ip][k]*(1-rp)+V.fxr[ip+1][k]*rp;
      for (int m=0; m<V.M; m++) newV->LogASp[p][m]=V.LogASp[ip][m]*(1-rp)+V.LogASp[ip+1][m]*rp;
      newV->Kp[p]=minK;
    }
    newV->peakfr[p]=floor(newV->lp[p]+0.5);
    memset(newV->fres[0], 0, sizeof(double)*newV->P*V.K);
  }
  int newL=newV->lp[inewP-1]+(newP+Pst-(inewP-1))*(V.lp[V.P-1]-V.lp[V.P-2])/rate+0.5;
  newV->AllocateL(newL);

  //F0C and A0C
  if (V.L!=newL)
  {
    for (int l=0; l<newL; l++)
    {
      double rl=1.0*l*(V.L-1)/(newL-1);
      int il=floor(rl); rl-=il;
      newV->F0C[l]=V.F0C[il]*(1-rl)+V.F0C[il+1]*rl;
      newV->A0C[l]=V.A0C[il]*(1-rl)+V.A0C[il+1]*rl;
    }
  }
  else
  {
    memcpy(newV->F0C, V.F0C, sizeof(double)*V.L);
    memcpy(newV->A0C, V.A0C, sizeof(double)*V.L);
//    memcpy(newV->F0, V.F0, sizeof(double)*V.L);
//    memcpy(newV->F0D, V.F0D, sizeof(double)*V.L);
  }
  return newV;
}//InterpoalteV

//vibrato rate boundaries, in Hz
#define MAXMOD 15.0
#define MINMOD 3.0

/**
  function RateAndReg: evaluates modulation rate and regularity

  In: data[frst:fren]: modulated signal, time measured in frames
      padrate: padding rate used for inverse FFT
      sps: sampling frequency
      offst: hop size
  Out: rate, regularity: rate and regularity

  No return value.
*/
void RateAndReg(double& rate, double& regularity, double* data, int frst, int fren, int padrate, double sps, double offst)
{
  rate=0; regularity=0;
  //find the section used for calculating regularity and rate
  int frlen=fren-frst+1, frlenp=1<<(int(Log2(fren))+2), frlenpp=frlenp*padrate;
  double *lf=new double[frlenpp*4];
  cdouble *w=(cdouble*)&lf[frlenpp];
  cdouble *x=(cdouble*)&lf[frlenpp*2];
  SetTwiddleFactors(frlenp, w);

  memset(lf, 0, sizeof(double)*frlenp);
  memcpy(lf, &data[frst], sizeof(double)*frlen);
//    for (int i=0; i<frlen; i++) lf[i]=data[frst+i]-edata;

    double* rdata=new double[50];
    for (int i=0; i<50; i++)
    {
      rdata[i]=0; for (int j=0; j<fren-i; j++) rdata[i]+=data[j]*data[j+i];
    }


  RFFTC(lf, NULL, NULL, Log2(frlenp), w, x, 0);
//  xr[0]=0;
  for (int i=0; i<frlenp/2; i++) {x[i].x=x[i].x*x[i].x+x[i].y*x[i].y; x[i].y=0;}
  memset(&x[frlenp/2], 0, sizeof(cdouble)*(frlenpp-frlenp/2));

  SetTwiddleFactors(frlenpp, w);
  CIFFTR(x, Log2(frlenpp), w, lf);

  delete[] rdata;

  double minpeak=sps/(offst*MAXMOD), maxpeak=sps/(offst*MINMOD);

  //find 2nd highest peak at interval padrate
  int imaxlf=padrate*minpeak;
  while (imaxlf<frlenpp && imaxlf<padrate*maxpeak && lf[imaxlf]<=lf[imaxlf-padrate]) imaxlf+=padrate;
  double maxlf=lf[imaxlf];
  for (int i=imaxlf/padrate; i<frlen/2; i++) if (maxlf<lf[i*padrate]) maxlf=lf[i*padrate], imaxlf=i*padrate;
  //locate the peak at interval 1
  int ii=imaxlf; for (int i=ii-padrate+1; i<ii+padrate; i++) if (maxlf<lf[i]) maxlf=lf[i], imaxlf=i;

  double a_=0.5*(lf[imaxlf-1]+lf[imaxlf+1])-maxlf, b_=0.5*(lf[imaxlf+1]-lf[imaxlf-1]);
  double period=imaxlf-0.5*b_/a_;
  period=period/padrate; //period in frames
  rate=sps/(period*offst); //rate in hz
  regularity=(maxlf-0.25*b_*b_/a_)/lf[0]*(fren-frst)/(fren-frst-period);

  delete[] lf;
}//RateAndReg

/**
  function RegularizeV: synthesize a regular vibrato from the basic descriptors.

  In: V: a TVo object hosting vibrato descriptors
      sps: sampling rate
      h: hop size
  Out: HS: a harmonic sinusoid

  No return value.
*/
void RegularizeV(THS& HS, TVo& V, double sps, double h)
{
  int K=V.Kp[1]; for (int p=2; p<V.P; p++) if (K>V.Kp[p]) K=V.Kp[p];
  double D=0, *fxr=new double[2*K-1];
  memset(fxr, 0, sizeof(double)*(2*K-1));
  for (int p=1; p<V.P; p++)
  {
    D+=V.Dp[p]; for (int k=0; k<2*K-1; k++) fxr[k]+=V.fxr[p][k]*V.Dp[p];
  }
  //uncomment the following line if re-normalization needed
  //double alfa=fxr[0]*fxr[0]; for (int k=1; k<2*K-1; k++) alfa+=2*fxr[k]*fxr[k]; alfa=sqrt(alfa); D=alfa;
  for (int k=0; k<2*K-1; k++) fxr[k]/=D;
  D/=(V.P-1);
  V.D=D;

  atom** Partials=HS.Partials;
  for (int l=0; l<V.L; l++)
  {
    double theta=V.rate*l*V.h/sps;//-0.25;
    //f0(theta)
    double f0=fxr[0]; for (int k=1; k<K; k++) f0=f0+2*(fxr[2*k-1]*sin(2*M_PI*k*theta)+fxr[2*k]*cos(2*M_PI*k*theta));
    V.F0D[l]=D*f0; V.F0[l]=V.F0C[l]+V.F0D[l];
    for (int m=0; m<V.M; m++)
    {
      if (Partials[m][l].t<0) Partials[m][l].t=Partials[0][l].t;
      Partials[m][l].f=(m+1)*V.F0[l];
      if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].f=-1, Partials[m][l].a=0;
      else Partials[m][l].a=V.A0C[l]*exp(V.LogAS[m]+V.LogAF[int(Partials[m][l].f*V.afres)]);
    }
  }

  delete[] fxr;
}//RegularizeV

/**
  function QIE: computes extremum using quadratic interpolation

  In: y[-1:1]: three points to interpolate from. It is assumed y[0] is larger (or smaller) than both
        y[-1] and y[1].

  Returns the extremum of y.
*/
double QIE(double* y)
{
  double a=0.5*(y[1]+y[-1])-y[0], b=0.5*(y[1]-y[-1]);
  return y[0]-0.25*b*b/a;
}//QIE

/**
  function QIE: computes extremum using quadratic interpolation

  In: y[-1:1]: three points to interpolate from. It is assumed y[0] is larger (or smaller) than both
        y[-1] and y[1].
  Out: x: the argument value that yields extremum of y(x)

  Returns the extremum of y.
*/
double QIE(double* y, double& x)
{
  double a=0.5*(y[1]+y[-1])-y[0], b=0.5*(y[1]-y[-1]);
  x=-0.5*b/a;
  return y[0]-0.25*b*b/a;
}//QIE

/**
  function S_F: original source-filter estimation used in AES124.

  In: afres: filter response resolution
      Partials[M][Fr]: HS partials
      A0C[Fr]: partial-independent amplitude carrier
      lp[P]: cycle boundaries
      F0Overall: average fundamental frequency
  Out: LogAF[afres/2]: filter response
       LogAS[M]: source amplitudes

  Returns 0.
*/
double S_F(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F0Overall)
{
    //SOURCE-FILTER estimation routines
    //derivative of log A(f), f is sampled at 1/8192 (5hz), averaged over (-15hz, 15hz)
    double *DAF=new double[afres], *NAF=&DAF[afres/2];
    memset(DAF, 0, sizeof(double)*afres);
    double avgwidth1, avgwidth2;
    for (int fr=lp[1]; fr<lp[P-1]; fr++) for (int m=0; m<M; m++)
    {
      if (Partials[m][fr].f>0 && Partials[m][fr-1].f>0 && Partials[m][fr+1].f>0)
      {
        double f_1=Partials[m][fr-1].f, f0=Partials[m][fr].f, f1=Partials[m][fr+1].f, a_1=Partials[m][fr-1].a/A0C[fr-1], a1=Partials[m][fr+1].a/A0C[fr+1];
        if ((f0-f_1)*(f0-f1)<0)
        {
          double dlogaf_df=(log(a1)-log(a_1))/(f1-f_1); //calculate derivative of log

          //this derivative contributes within a frequency interval of |f1-f_1|
          if (f1<f_1) {double tmp=f1; f1=f_1; f_1=tmp;}
          int startbin=ceil(f_1*afres), endbin=floor(f1*afres);

            if (startbin<0) startbin=0;
            avgwidth1=(f0-f_1)*afres, avgwidth2=(f1-f0)*afres;
            for (int i=startbin; i<=endbin; i++)
            {
              double fi=i-f0*afres, w;
              if (fi<0) w=1-fabs(fi)/avgwidth1; else w=1-fabs(fi)/avgwidth2;
              DAF[i]+=dlogaf_df*w, NAF[i]+=w;
            }
        }
      }
    }
    for (int i=0; i<afres/2; i++) if (NAF[i]>0) DAF[i]=DAF[i]/NAF[i]; //else NAF[i]=0;
    int i=0, nbands=0, bandsstart[100], bandsendinc[100]; double mininaf=3;
    while (i<afres/2)
    {
      while (i<afres/2 && NAF[i]<mininaf) i++; if (i>=afres/2) break;
      bandsstart[nbands]=i;
      while (i<afres/2 && NAF[i]>0) i++; if (i>=afres/2) {bandsendinc[nbands++]=i-1; break;}
      if (NAF[i]<=0) while (NAF[i]<mininaf) i--;
      bandsendinc[nbands++]=i++;
    }

  if (nbands==0)
  {
    memset(LogAF, 0, sizeof(double)*afres/2);
  }
  else
  {
    //integrate DAF over the fundamental band, with AF(F0Overall)=1
    i=F0Overall*afres;
    double theta=F0Overall*afres-i;
    LogAF[i]=-0.5*theta*(DAF[i]+DAF[i]*(1-theta)+DAF[i+1]*theta)/afres; i--;
    while (i>=bandsstart[0]){LogAF[i]=LogAF[i+1]-0.5*(DAF[i]+DAF[i+1])/afres; i--;}
    i=F0Overall*afres+1;
    LogAF[i]=0.5*(1-theta)*(DAF[i]+DAF[i-1]*(1-theta)+DAF[i]*theta)/afres; i++;
    while (i<=bandsendinc[0]){LogAF[i]=LogAF[i-1]+0.5*(DAF[i]+DAF[i-1])/afres; i++;}

    int band=1;
    while (band<nbands)
    {
      //integrate DAF over band-th band, first ignore the blank band
      LogAF[bandsstart[band]]=LogAF[bandsendinc[band-1]];
      for (int i=bandsstart[band]+1; i<=bandsendinc[band]; i++) LogAF[i]=LogAF[i-1]+0.5*(DAF[i]+DAF[i-1])/afres;
      //then look for the shift required between bands
      double cshift=0; int ccount=0;
      for (int fr=lp[1]; fr<lp[P-1]; fr++)
      {
        double f=Partials[band-1][fr].f*afres;
        if (bandsendinc[band-1]>bandsstart[band-1] && (f<bandsstart[band-1] || f>bandsendinc[band-1])) continue;
        int intf=floor(f);
        double logaflast=LogAF[intf]+(DAF[intf]+0.5*(DAF[intf+1]-DAF[intf])*(f-intf))*(f-intf)/afres;
        f=Partials[band][fr].f*afres;
        if (bandsendinc[band]>bandsstart[band] && (f<bandsstart[band] || f>bandsendinc[band])) continue;
        intf=floor(f);
        double logafthis=LogAF[intf]+(DAF[intf]+0.5*(DAF[intf+1]-DAF[intf])*(f-intf))*(f-intf)/afres;
        cshift=cshift+log(Partials[band][fr].a*(band+1)/(Partials[band-1][fr].a*band))+logaflast-logafthis;
        ccount++;
      }
      if (ccount!=0) cshift/=ccount;
      else cshift=0;
      //apply the shift
      for (int i=bandsstart[band]; i<=bandsendinc[band]; i++) LogAF[i]+=cshift;
      //make up the blank band
      int lastend=bandsendinc[band-1], thisstart=bandsstart[band];
      for (int i=lastend+1; i<thisstart; i++) LogAF[i]=LogAF[lastend]+cshift*(i-lastend)/(thisstart-lastend);
      band++;
    }

    int tmpband=bandsstart[0]; for (int i=0; i<tmpband; i++) LogAF[i]=LogAF[tmpband];
    tmpband=bandsendinc[nbands-1]; for (int i=tmpband; i<afres/2; i++) LogAF[i]=LogAF[tmpband];
  }
  delete[] DAF;

    //Here LogAF contains the filter model, new get the source model
    memset(LogAS, 0, sizeof(double)*100);
    for (int m=0; m<M; m++)
    {
      int ccount=0;
      for (int fr=lp[1]; fr<lp[P-1]; fr++)
      {
        double f=Partials[m][fr].f*afres;
        if (f<=0) continue;
        int intf=floor(f);
        LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf));
        ccount++;
      }
      if (ccount>0) LogAS[m]/=ccount;
      else LogAS[m]=0;
    }
  return 0;
}//S_F

/**
  function S_F_SV: slow-variation source-filter estimation used in AES126, adapted from TSF to TVo.

  In: afres: filter response resolution
      Partials[M][Fr]: HS partials
      A0C[Fr]: partial-independent amplitude carrier
      lp[P]: cycle boundaries
      F: filter response sampling interval
      FScaleMode: set if filter sampling uses mel scale
      theta: balancing factor of amplitude and frequency variations, needed for SV approach
  Out: LogAF[afres/2]: filter response
       LogAS[M]: source amplitudes

  Returns 0.
*/
double S_F_SV(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, double F, int FScaleMode, double theta, double Fs)
{
  int lp0=lp[1], lpp=lp[P-1];
  int L=lpp-lp0;
  Alloc2(L, M, loga); Alloc2(L, M, f);
  double fmax=0;
  for (int l=0; l<L; l++)
  {
    int fr=lp0+l;
    for (int m=0; m<M; m++)
    {
      f[l][m]=Partials[m][fr].f;
      if (FScaleMode) f[l][m]=log(1+f[l][m]*Fs/700)/log(1+Fs/700);
      if (f[l][m]>0) loga[l][m]=log(Partials[m][fr].a);
      else loga[l][m]=-100;
      if (fmax<f[l][m]) fmax=f[l][m];
    }
  }
  int K=floor(fmax/F);

  Alloc2(L, K+2, h); memset(h[0], 0, sizeof(double)*L*(K+2));

  SF_SV(h, L, M, K, loga, f, F, theta, 1e-6, L*(K+2));

  for (int k=0; k<K+2; k++)
  {
    for (int l=1; l<L; l++) h[0][k]+=h[l][k];
    h[0][k]/=L;
  }

  for (int i=0; i<afres/2; i++)
  {
    double freq=i*1.0/afres;
    int k=floor(freq/F);
    double f_plus=freq/F-k;
    if (k<K+1) LogAF[i]=h[0][k]*(1-f_plus)+h[0][k+1]*f_plus;
    else LogAF[i]=h[0][K+1];
  }

  memset(LogAS, 0, sizeof(double)*100);
  for (int m=0; m<M; m++)
  {
    int ccount=0;
    for (int fr=lp[1]; fr<lp[P-1]; fr++)
    {
      double f=Partials[m][fr].f*afres;
      if (f<=0) continue;
      int intf=floor(f);
      LogAS[m]=LogAS[m]+log(Partials[m][fr].a/A0C[fr])-(LogAF[intf]*(intf+1-f)+LogAF[intf+1]*(f-intf));
      ccount++;
    }
    if (ccount>0) LogAS[m]/=ccount;
    else LogAS[m]=0;
  }

  DeAlloc2(loga); DeAlloc2(f); DeAlloc2(h);
  return 0;
}//S_F_SV

/**
  function SynthesizeV: synthesizes a harmonic sinusoid from vibrato descriptors

  In: V: a TVo object hosting vibrato descriptors
      sps: sampling rate
      UseK: maximal number of Fourier series components to use to construct frequency modulator
  Out: HS: a harmonic sinusoid

  No return value.
*/
void SynthesizeV(THS* HS, TVo* V, double sps, int UseK)
{
  int HSt0=HS->Partials[0][0].t, HSs0=HS->Partials[0][0].s, L=V->L, M=V->M,
      P=V->P, *Kp=V->Kp, K=V->K, afres=V->afres;
  double **fxr=V->fxr, *lp=V->lp, **LogASp=V->LogASp, *LogAF=V->LogAF, *Dp=V->Dp,
      *F0C=V->F0C, *F0D=V->F0D, *F0=V->F0, *A0C=V->A0C, h=V->h;
  HS->Resize(M, L);
  double *gamp=new double[4*P], *gamm=&gamp[P];

  Alloc2(P, K*2, newfxr);
  int* newKp=new int[P];
  for (int p=1; p<P; p++)
  {
    memcpy(newfxr[p], fxr[p], sizeof(double)*2*K);
    newKp[p]=Kp[p]; if (UseK>=2 && newKp[p]>UseK) newKp[p]=UseK;
    CorrectFS(newKp[p], newfxr[p]);
  }

  double f0, f1;
  for (int p=1; p<P-1; p++)
  {
    if (p==1)
    {
      f0=newfxr[p][0]; for (int k=1; k<newKp[p]; k++) f0=f0+2*newfxr[p][2*k];
    }
    else f0=f1;
    f1=newfxr[p+1][0]; for (int k=1; k<newKp[p+1]; k++) f1=f1+2*newfxr[p+1][2*k];
    gamp[p]=(Dp[p+1]*f1-Dp[p]*f0)/2; gamm[p+1]=-gamp[p]; //additive correction
  }
  gamm[1]=-gamp[1], gamp[P-1]=-gamm[P-1]; //additive correction
  atom** Partials=HS->Partials;
  for (int p=0; p<P; p++)
  {
    double l0, lrange;
    int lst, len, ilp;
    if (p==0) lst=0, len=ceil(lp[0]), ilp=1;
    else if (p==P-1) lst=ceil(lp[p-1]), len=L, ilp=p-1;
    else lst=ceil(lp[p-1]), len=ceil(lp[p]), ilp=p;
    l0=lp[ilp-1], lrange=lp[ilp]-l0;
    for (int l=lst; l<len; l++)
    {
      Partials[0][l].s=HSs0;
      Partials[0][l].t=HSt0+h*l;
      double theta=(l-l0)/lrange;
      double f0=newfxr[ilp][0]; for (int k=1; k<newKp[ilp]-1; k++) f0=f0+2*(newfxr[ilp][2*k-1]*sin(2*M_PI*k*theta)+newfxr[ilp][2*k]*cos(2*M_PI*k*theta));
      F0D[l]=f0*Dp[ilp]+gamm[ilp]*(1-theta)+gamp[ilp]*theta;   //additive correction
//        V.F0D[l]=f0*V.Dp[lp];   //no correction
      F0[l]=F0C[l]+F0D[l];
      for (int m=0; m<M; m++)
      {
        Partials[m][l].s=HSs0;
        Partials[m][l].t=Partials[0][l].t;
        Partials[m][l].f=(m+1)*F0[l];
        if (Partials[m][l].f>0.5 || Partials[m][l].f<0) Partials[m][l].a=0;
        else if (ilp==1) Partials[m][l].a=A0C[l]*exp(LogASp[1][m]*(1-0.5*theta)+LogASp[2][m]*0.5*theta+LogAF[int(Partials[m][l].f*afres)]);
        else if (ilp==P-1) Partials[m][l].a=A0C[l]*exp(LogASp[ilp][m]*0.5*(1+theta)+LogASp[ilp-1][m]*0.5*(1-theta)+LogAF[int(Partials[m][l].f*afres)]);
        else
//					  Partials[m][l].a=A0C[l]*exp(LogASp[ilp][m]*0.5+LogASp[ilp-1][m]*0.5*(1-theta)+LogASp[ilp+1][m]*0.5*theta+LogAF[int(Partials[m][l].f*afres)]);
          Partials[m][l].a=A0C[l]*exp(LogASp[p][m]+LogAF[int(Partials[m][l].f*afres)]);
      }
    }
  }
  delete[] gamp;
  DeAlloc2(newfxr);
  delete[] newKp;
//    VibratoDemoForm->Label13->Caption=L;
}//SynthesizeV