view hssf.cpp @ 11:977f541d6683

GPL and cosmetic changes

/*
    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 "hssf.h"
#include "arrayalloc.h"
#include "matrix.h"
#include "vibrato.h"

/** \file hssf.h */

//---------------------------------------------------------------------------
//method TSF::TSF: default constructor.
TSF::TSF()
{
	memset(this, 0, sizeof(TSF));
}//TSF

//method TSF::~TSF: default destructor.
TSF::~TSF()
{
	if (h) DeAlloc2(h);
	if (b) DeAlloc2(b);
	delete[] lp;
	delete[] F0;
	free(avgh);
	free(avgb);
}//~TSF

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

/**
  method TSF::AllocateP: allocates or reallocates storage space whose size depends on P, using the
  interal P.
*/
void TSF::AllocateP()
{
  delete[] lp;
  lp=new double[P+2];
}//AllocateP

/**
  method TSF::AllocateSF: allocates or reallocates storage space for source-filter coefficients.
*/
void TSF::AllocateSF()
{
  if (h) DeAlloc2(h); int k=1.0/F; Allocate2(double, L, (k+2), h); memset(h[0], 0, sizeof(double)*(L)*(k+2));
  if (b) DeAlloc2(b); Allocate2(double, L, M, b);	memset(b[0], 0, sizeof(double)*(L)*M);
  avgh=(double*)realloc(avgh, sizeof(double)*(k+2)); avgb=(double*)realloc(avgb, sizeof(double)*M);
}//AllocateSF

/**
  method TSF::Duplicate: copies the complete contents from another SF object

  In: SF: source object
*/
void TSF::Duplicate(TSF& SF)
{
	TSF::~TSF();
  memcpy(this, &SF, sizeof(TSF)); lp=F0=avgh=avgb=0, h=b=0;
  AllocateL(L); memcpy(F0, SF.F0, sizeof(double)*(L+2)*6);
  AllocateP(); memcpy(lp, SF.lp, sizeof(double)*(P+2));
  AllocateSF(); int SFL=ceil(lp[P-1])-ceil(lp[0]);
  for (int l=0; l<SFL; l++) memcpy(h[l], SF.h[l], sizeof(double)*(K+2)), memcpy(b[l], SF.b[l], sizeof(double)*M);
  memcpy(avgh, SF.avgh, sizeof(double)*(K+2)); memcpy(avgb, SF.avgb, sizeof(double)*M);
}//Duplicate

/**
  method TSF::LoadFromFileHandle: reads SF object from file
*/
void TSF::LoadFromFileHandle(FILE* f)
{
  fread(&M, sizeof(int), 1, f);
  fread(&L, sizeof(int), 1, f);
  fread(&P, sizeof(int), 1, f);
  fread(&offst, sizeof(double), 1, f);
  AllocateL(L); AllocateP();
  fread(F0C, sizeof(double), L, f);
  fread(logA0C, sizeof(double), L, f);
  fread(logA0D, sizeof(double), L, f);
  fread(lp, sizeof(double), P, f);
  LoadSFFromFileHandle(f);
}//LoadFromFileHandle

/**
  method TSF::LoadSFFromFileHandle: reads SF coefficients from file
*/
void TSF::LoadSFFromFileHandle(FILE* f)
{
  fread(&K, sizeof(int), 1, f);
  fread(&FScaleMode, sizeof(int), 1, f);
  fread(&F, sizeof(double), 1, f);
  fread(&Fs, sizeof(double), 1, f);
  AllocateSF();
  for (int l=0; l<L; l++) fread(b[l], sizeof(double), M, f);
  for (int l=0; l<L; l++) fread(h[l], sizeof(double), K+2, f);
  fread(avgb, sizeof(double), M, f);
  fread(avgh, sizeof(double), K+2, f);
}//LoadSFFromFileHandle

/**
  method TSF::LogAF: average filter response

  In: f: frequency

  Returns the average response of the filter model at f
*/
double TSF::LogAF(double f)
{
	if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
	int k=floor(f/F);
	double f_plus=f/F-k;
	if (k<K+1) return avgh[k]*(1-f_plus)+avgh[k+1]*f_plus;
	else return avgh[K+1];
}//LogAF

/**
  method TSF::LogAF: filter response

  In: f: frequency
      fr: frame index

  Returns the response of the filter model at f for frame fr
*/
double TSF::LogAF(double f, int fr)
{
	int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
	int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1;
	if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
	int k=floor(f/F);
	double f_plus=f/F-k;
	if (k<K+1) return h[l][k]*(1-f_plus)+h[l][k+1]*f_plus;
	else return h[l][K+1];
}//LogAF

/**
  method TSF::LogAS: source response

  In: m: partial index
      fr: frame index

  Returns response of the source model for partial m at frame fr
*/
double TSF::LogAS(int m, int fr)
{
	int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
	int l=fr-lp0; if (l<0) l=0; else if (l>=lpp-lp0) l=lpp-lp0-1;
	return b[l][m];
}//LogAS

/**
  method TSF::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 TSF::ReAllocateL(int newL)
{
  double* newF0=new double[(newL+2)*4]; F0C=&newF0[newL+2], F0D=&newF0[(newL+2)*2], logA0C=&newF0[(newL+2)*3], logA0=&newF0[(L+2)*4], logA0D=&F0[(L+2)*5];
  memcpy(logA0C, &F0[(L+2)*3], sizeof(double)*(L+2));
  memcpy(logA0D, &F0[(L+2)*5], 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 TSF::SaveSFToFileHandle: writes SF coefficients to file
*/
void TSF::SaveSFToFileHandle(FILE* f)
{
  fwrite(&K, sizeof(int), 1, f);
  fwrite(&FScaleMode, sizeof(int), 1, f);
  fwrite(&F, sizeof(double), 1, f);
  fwrite(&Fs, sizeof(double), 1, f);
  for (int l=0; l<L; l++) fwrite(b[l], sizeof(double), M, f);
  for (int l=0; l<L; l++) fwrite(h[l], sizeof(double), K+2, f);
  fwrite(avgb, sizeof(double), M, f);
  fwrite(avgh, sizeof(double), K+2, f);
}//SaveSFToFileHandle

/**
  method TSF::SaveToFile: save SF object to file

  In: filename: full path of destination file
*/
void TSF::SaveToFile(char* filename)
{
  FILE* file;
  if ((file=fopen(filename, "wb"))!=NULL)
  {
    SaveToFileHandle(file); fclose(file);
  }
}//SaveToFile

/**
  method TSF::SaveToFileHandle: writes SF object to file
*/
void TSF::SaveToFileHandle(FILE* f)
{
  fwrite(&M, sizeof(int), 1, f);
  fwrite(&L, sizeof(int), 1, f);
  fwrite(&P, sizeof(int), 1, f);
  fwrite(&offst, sizeof(double), 1, f);
  fwrite(F0C, sizeof(double), L, f);
  fwrite(logA0C, sizeof(double), L, f);
  fwrite(logA0D, sizeof(double), L, f);
  fwrite(lp, sizeof(double), P, f);
  SaveSFToFileHandle(f);
}//SaveToFileHandle

/**
  method TSF::ShiftFilterByDB: adds a given number of dBs to the filter model.

  In: dB: amount to add.
*/
void TSF::ShiftFilterByDB(double dB)
{
  for (int l=0; l<L; l++) for (int k=0; k<K+2; k++) h[l][k]+=dB;
	for (int k=0; k<K+2; k++) avgh[k]+=dB;
}//ShiftFilterByDB

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


/**
  function AnalyzeSF: wrapper function.

  In: HS: a harmonic sinusoid
      sps: sampling rate ("samples per second")
      offst: hop size, the interval between adjacent harmonic atoms of HS
      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: SF: a TSF object estimated from HS.
       cyclefrcount: number of cycles
       cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle

  No return value.
*/
void AnalyzeSF(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, double offst, int* cyclefrcount,
      int SFMode, double SFF, int SFFScale, double SFtheta)
{
  AnalyzeSF_1(HS, SF, sps, offst);
  AnalyzeSF_2(HS, SF, cyclefrs, cyclefs, sps, cyclefrcount, SFMode, SFF, SFFScale, SFtheta);
}//AnalyzeSF

/**
  function AnalyzeSF_1: first stage of source-filter model estimation, in which the duration of a HS is
  segmented into what is equivalent to "cycles" in vibrato analysis.

  In: HS: a harmonic sinusoid
      sps: sampling rate ("samples per second")
      offst: hop size, the interval between adjacent harmonic atoms of HS
  Out: SF: a TSF object partially updated, particularly lp[P].

  No return value.
*/
void AnalyzeSF_1(THS& HS, TSF& SF, double sps, double offst)
{
  int M=HS.M, Fr=HS.Fr;
  if (M<=0 || Fr<=0) return;
  atom** Partials=HS.Partials;

  //basic descriptor: offst
  SF.offst=offst;

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

  double* A0=SF.logA0;
  //find the amplitude and pitch tracks
  SF.F0max=0, SF.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;
    }
    SF.F0[fr]=suma2p/suma2;
    if (SF.F0max<SF.F0[fr]) SF.F0max=SF.F0[fr];
    if (SF.F0min>SF.F0[fr]) SF.F0min=SF.F0[fr];
    A0[fr]=suma2;
  }

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

  //rough estimation of rate
  double* f0d=new double[Fr-1]; for (int i=0; i<Fr-1; i++) f0d[i]=SF.F0[i+1]-SF.F0[i];
  RateAndReg(SF.rate, SF.regularity, f0d, 0, Fr-2, 8, sps, offst);
  delete[] f0d;
  //find peaks of the pitch track
  int* peakfr=new int[Fr/2];
  double periodinframe=sps/(SF.rate*offst), *dummy=(double*)0xFFFFFFFF;
  FindPeaks(peakfr, SF.P, SF.F0, Fr, periodinframe, dummy);
  //Basic descriptor: lp[]
  SF.AllocateP();
  for (int p=0; p<SF.P; p++)
  {
    double startfr; QIE(&SF.F0[peakfr[p]], startfr); SF.lp[p]=startfr+peakfr[p];
  }
  delete[] peakfr;
}//AnalyzeSF_1

/**
  function AnalyzeSF_2: second stage of source-filter model estimation, in which the HS is demodulated
  and its source-filter model is estimated.

  In: HS: a harmonic sinusoid
      SF: a TSF object with valid segmentation data (lp[P]) of HS
      sps: sampling rate ("samples per second")
      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: SF: the TSF object fully estimated.
       cyclefrcount: number of cycles
       cyclefrs[cyclefrcount], cyclefs[cyclefrcount]: average time (in frames) and frequency of each cycle

  No return value.
*/
void AnalyzeSF_2(THS& HS, TSF& SF, double*& cyclefrs, double*& cyclefs, double sps, 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;

  if (SF.P>=1) //de-modulation
  {
    //Basic descriptor: F0C[]
    DeFM(SF.F0C, SF.F0, SF.logA0, SF.P, SF.lp, Fr, SF.F0Overall, *cyclefrcount, cyclefrs, cyclefs);
    //Basic descriptor: A0C[]
    double *A0C=SF.logA0C, *logA0C=SF.logA0C, *logA0D=SF.logA0D, *logA0=SF.logA0;
    DeAM(A0C, SF.logA0, SF.P, SF.lp, Fr);
    for (int fr=0; fr<Fr; fr++) logA0C[fr]=log(A0C[fr]);
    //Basic descriptor: logA0D[]
    int hM=M/2;
    for (int fr=0; fr<Fr; fr++)
    {
      double loga0d=0;
      for (int m=0; m<hM; m++) loga0d+=log(Partials[m][fr].a);
      logA0[fr]=loga0d/hM;
      logA0D[fr]=logA0[fr]-logA0C[fr];
    }
  }
  //Basic descriptor: F0D[]
  SF.F0Cmax=0; SF.F0Dmax=-1; SF.F0Cmin=1; SF.F0Dmin=1;
  for (int fr=0; fr<Fr; fr++)
  {
    if (SF.F0Cmax<SF.F0C[fr]) SF.F0Cmax=SF.F0C[fr];
    if (SF.F0Cmin>SF.F0C[fr]) SF.F0Cmin=SF.F0C[fr];
    SF.F0D[fr]=SF.F0[fr]-SF.F0C[fr];
  }

  //Basic descriptor: b, h
  //Source-filter modeling
  {
    SF.F=SFF; SF.Fs=sps; SF.AllocateSF();
    int M=SF.M; double **h=SF.h, **b=SF.b;// *avgh=SF.avgh, *avgb=SF.avgb;

    double *logA0C=useA0?SF.logA0:SF.logA0C;
    if (SFMode==0){
      S_F_FB(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, sps);}
    else if (SFMode==1){
      S_F_SV(M, Partials, logA0C, SF.lp, SF.P, SF.K, h, SF.avgh, b, SF.avgb, SFF, SFFScale, SFtheta, sps);}

    SF.FScaleMode=SFFScale;
//		int K=SF.K, effL=ceil(SF.lp[SF.P-1])-ceil(SF.lp[0]);
//		for (int k=0; k<K+2; k++){double sumh=0; for (int l=0; l<effL; l++) sumh+=h[l][k]; avgh[k]=sumh/effL;}
//		for (int m=0; m<M; m++){double sumb=0; for (int l=0; l<effL; l++) sumb+=b[l][m]; avgb[m]=sumb/effL;}
  }
}//AnalyzeSF_2

/**
  function I3: integrates the product of 3 linear functions (0, a*)-(T, b*) (*=1, 2 or 3) over (0, T).
  See "further reading", pp.12 eq.(37).

  In: T, a1, a2, a3, b1, b2, b3

  Returns the definite integral.
*/
double I3(double T, double a1, double a2, double a3, double b1, double b2, double b3)
{
  return T*(a1*(a2*(3*a3+b3)+b2*(a3+b3))+b1*(a2*(a3+b3)+b2*(a3+3*b3)))/12;
}//I3

/**
  function P_Ax: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic
  coefficient of filter coefficients x[L][K+2] towards the totoal variation. See "further reading",
  pp.8, (29a).

  In: x[L][K+2]: filter coefficients
      f[L][M]: partial frequencies
      F: filter response sampling interval
      theta: balancing factor of amplitude and frequency variations
  Out: d[L][K+2]: Ax.

  Returns inner product of x[][] and d[][].
*/
double P_Ax(double** d, int L, int M, int K, double** x, double** f, double F, double theta)
{
  double **dFtr=d;
  Alloc2(L, K+2, dSrc);
  double DelFtr=P2_DelFtr(dFtr, L, K, x, F);
  double DelSrc=P3_DelSrc(dSrc, L, M, K, x, f, F);
  Multiply(L, K+2, dFtr, dFtr, (1-theta)/F);
  MultiAdd(L, K+2, d, dFtr, dSrc, theta);
  DeAlloc2(dSrc);
  return DelFtr*(1-theta)/F+DelSrc*theta;
}//P_Ax

/**
  function P_Ax_cf: calculate the 2-D vector Ax for a given x, where A is a matrix hosting the parabolic
  coefficient of time-unvarying filter coefficients x[K+2] towards totoal source variation.

  In: x[K+2]: filter coefficients
      f[L][M]: partial frequencies
      F: filter response sampling interval
  Out: d[K+2]: Ax.

  Returns inner product of x[] and d[].
*/
double P_Ax_cf(double* d, int L, int M, int K, double* x, double** f, double F)
{
  double xAx=0;
  for (int l=0; l<L; l++) memset(d, 0, sizeof(double)*(K+2));
  for (int l=0; l<L-1; l++)
    for (int m=0; m<M; m++)
    {
      double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
      if (f0f<=0 || f1f<=0) continue;
      int k0=floor(f0f), k1=floor(f1f);
      double f0=f0f-k0, f1=f1f-k1;
      double g0=1-f0, g1=1-f1;
      double A=-x[k1]*g1-x[k1+1]*f1+x[k0]*g0+x[k0+1]*f0;
      d[k1]-=2*A*g1;
      d[k1+1]-=2*A*f1;
      d[k0]+=2*A*g0;
      d[k0+1]+=2*A*f0;
      xAx+=A*A;
    }
  return xAx;
}//P_Ax_cf

/**
  function P1_b: internal procedure P1 of slow-variation SF estimator. See "further reading", pp.8.

  In: a[L][M], f[L][M]: partial amplitudes and frequencies
      F: filter response sampling interval
      theta: balancing factor of amplitude and frequency variations
  Out: b[L][K+2]: linear coefficients of filter coefficients h[L][K+2] towards the total variation.

  No return value.
*/
void P1_b(double** b, int L, int M, int K, double** a, double** f, double F, double theta)
{
  for (int l=0; l<L; l++) memset(b[l], 0, sizeof(double)*(K+2));
  for (int l=0; l<L-1; l++)
  {
    for (int m=0; m<M; m++)
    {
      double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
      if (f0f<=0 || f1f<=0) continue;
      int k0=floor(f0f), k1=floor(f1f);
      double f0=f0f-k0, f1=f1f-k1;
      double g0=1-f0, g1=1-f1;
      double delta=a[l+1][m]-a[l][m];
      b[l+1][k1]+=g1*delta, b[l+1][k1+1]+=f1*delta;
      b[l][k0]-=g0*delta, b[l][k0+1]-=f0*delta;
    }
  }
  Multiply(L, K+2, b, b, theta);
}//P1_b

/**
  function P1_b_cf: internal procedure P1 of slow-variation SF estimator, constant-filter version.

  In: a[L][M], f[L][M]: partial amplitudes and frequencies
      F: filter response sampling interval
  Out: b[K+2]: linear coefficients of filter coefficients h[K+2] towards total source variation.

  No return value.
*/
void P1_b_cf(double* b, int L, int M, int K, double** a, double** f, double F)
{
  memset(b, 0, sizeof(double)*(K+2));
  for (int l=0; l<L-1; l++)
  {
    for (int m=0; m<M; m++)
    {
      double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
      if (f0f<=0 || f1f<=0) continue;
      int k0=floor(f0f), k1=floor(f1f);
      double f0=f0f-k0, f1=f1f-k1;
      double g0=1-f0, g1=1-f1;
      double delta=a[l+1][m]-a[l][m];
      b[k1]+=g1*delta, b[k1+1]+=f1*delta;
      b[k0]-=g0*delta, b[k0+1]-=f0*delta;
    }
  }
  Multiply(K+2, b, b, 2);
}//P1_b_cf

/**
  function P2_DelFtr: internal procedure P2 of slow-variation SF estimator. See "further reading", pp.8.

  In: x[L][K+2]: filter coefficients
      F: filter response sampling interval
  Out: d[L][K+2]: derivative of total filter variation against filter coefficients.

  Returns the inner product of x[][] and d[][].
*/
double P2_DelFtr(double** d, int L, int K, double** x, double F)
{
  double xAx=0;
  for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2));
  for (int l=0; l<L-1; l++)
    for (int k=0; k<K+1; k++)
    {
      try{
      double A0=x[l+1][k]-x[l][k], A1=x[l+1][k+1]-x[l][k+1];
      d[l+1][k]+=(2*A0+A1);
      d[l][k]-=(2*A0+A1);
      d[l+1][k+1]+=(2*A1+A0);
      d[l][k+1]-=(2*A1+A0);
      xAx+=A0*A0+A1*A1+A0*A1;
      }
      catch(...){}
    }
  Multiply(L, K+2, d, d, F/3);
  return xAx*F/3;
}//P2_DelFtr

/**
  function P3_DelSec: internal procedure P3 of slow-variation SF estimator. See "further reading", pp.9.

  In: x[L][K+2]: filter coefficients
      f[L][M]: partial frequencies
      F: filter response sampling interval
  Out: d[L][K+2]: derivative of total source variation against filter coefficients.

  Returns the inner product of x[][] and d[][].
*/
double P3_DelSrc(double** d, int L, int M, int K, double** x, double** f, double F)
{
  double xAx=0;
  for (int l=0; l<L; l++) memset(d[l], 0, sizeof(double)*(K+2));
  for (int l=0; l<L-1; l++)
    for (int m=0; m<M; m++)
    {
      double f0f=f[l][m]/F, f1f=f[l+1][m]/F;
      if (f0f<=0 || f1f<=0) continue;
      int k0=floor(f0f), k1=floor(f1f);
      double f0=f0f-k0, f1=f1f-k1;
      double g0=1-f0, g1=1-f1;
      double A=-x[l+1][k1]*g1-x[l+1][k1+1]*f1+x[l][k0]*g0+x[l][k0+1]*f0;
      d[l+1][k1]-=2*A*g1;
      d[l+1][k1+1]-=2*A*f1;
      d[l][k0]+=2*A*g0;
      d[l][k0+1]+=2*A*f0;
      xAx+=A*A;
    }
  return xAx;
}//P3_DelSec

/**
  function S_F_b: computes source coefficients given amplitudes and filter coefficients

  In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
      lp[P]: temporal segmentation points, in frames.
      logA0C[Fr]: amplitude carrier
      h[L][K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]).
      F: filter response sampling interval
      FScaleMode: set if mel scale is used as frequency scale
      Fs: sampling frequency, effective only when FScaleMode is set.
  Out: b[L][M]: source coefficients for L frames starting from ceil(lp[0]).
       avgb[L]: average source coefficents for these L frames

  No return value.
*/
void S_F_b(int M, atom** Partials, double* logA0C, double* lp, int P, int K, double** h, double** b, double* avgb, double F, int FScaleMode, double Fs)
{
  int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
  int L=lpp-lp0;
  for (int l=0; l<L; l++)
  {
    int fr=lp0+l;
    double loga0=logA0C[fr];
    for (int m=0; m<M; m++)
    {
      double f=Partials[m][fr].f;
      if (FScaleMode) f=log(1+f*Fs/700)/log(1+Fs/700);
      if (f>0)
      {
        double loga=log(Partials[m][fr].a);
        loga-=loga0;
        double ff=f/F;
        int klm=floor(ff);
        double f0=ff-klm;
        double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
        b[l][m]=loga-hlm;
      }
      else b[l][m]=-100;
    }
  }
  //	for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
  for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}
}//S_F_b

/**
  function S_F_b: wrapper function

  In: Partials: HS partials
      SF: TSF object containing valid filter coefficients
  Out: SF: TSF object with source coefficients updated

  No return value.
*/
void S_F_b(TSF& SF, atom** Partials)
{
  S_F_b(SF.M, Partials, useA0?SF.logA0:SF.logA0C, SF.lp, SF.P, SF.K, SF.h, SF.b, SF.avgb, SF.F, SF.FScaleMode, SF.Fs);
}//S_F_b

/**
  function S_F_b: filter-bank method for estimating source-filter model. This is a wrapper function of
  SF_FB() that transfers and scales values, etc.

  In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
      lp[P]: temporal segmentation points, in frames.
      logA0C[Fr]: amplitude carrier
      F: filter response sampling interval
      FScaleMode: set if mel scale is used as frequency scale
      Fs: sampling frequency, effective only when FScaleMode is set.
  Out: K
       h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
       b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages

  Returns 0.
*/
double S_F_FB(int M, atom** Partials, double* logA0C, double* lp, int P, int& K, double** h, double* avgh, double** b, double* avgb, double F, int FScaleMode, double Fs)
{
  int lp0=ceil(lp[0]), lpp=ceil(lp[P-1]);
  int L=lpp-lp0;
  Alloc2(L, M, a); Alloc2(L, M, f);
  double fmax=0;
  for (int l=0; l<L; l++)
  {
    int fr=lp0+l;
    double ia0c=exp(-logA0C[fr]);
    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) a[l][m]=Partials[m][fr].a*ia0c; //log(Partials[m][fr].a);
      else a[l][m]=0;
      if (fmax<f[l][m]) fmax=f[l][m];
    }
  }
  K=floor(fmax/F);


  SF_FB(h[0], M, K, &a[0], &f[0], F, 1);
  for (int l=1; l<L-1; l++) SF_FB(h[l], M, K, &a[l], &f[l], F, 0);
  SF_FB(h[L-1], M, K, &a[L-1], &f[L-1], F, -1);

  for (int l=0; l<L; l++)
    for (int m=0; m<M; m++)
    {
      double ff=f[l][m]/F;
      if (ff<=0) b[l][m]=-100;
      else
      {
        int klm=floor(ff);
        double f0=ff-klm;
        double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
        b[l][m]=log(a[l][m])-hlm;
      }
    }

  for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
  for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}

  DeAlloc2(a); DeAlloc2(f);
  return 0;
}//S_F_b

/**
  function S_F_SV: slow-variation method for estimating source-filter model. This is a wrapper function
  of SF_SV() that transfers and scales values, etc.

  In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
      lp[P]: temporal segmentation points, in frames.
      logA0C[Fr]: amplitude carrier
      theta: balancing factor of amplitude and frequency variations
      F: filter response sampling interval
      FScaleMode: set if mel scale is used as frequency scale
      Fs: sampling frequency, effective only when FScaleMode is set.
  Out: K
       h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
       b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
  Returns 0.
*/
double S_F_SV(int M, atom** Partials, double* logA0C, double* lp, int P, int& K, double** h, double* avgh, double** b, double* avgb, double F, int FScaleMode, double theta, double Fs)
{
  int lp0=ceil(lp[0]), lpp=ceil(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];
    }
  }
  K=floor(fmax/F);

  for (int l=0; l<L; l++){double loga0=logA0C[lp0+l];	for (int m=0; m<M; m++) loga[l][m]-=loga0;}

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

  for (int l=0; l<L; l++)
    for (int m=0; m<M; m++)
    {
      double ff=f[l][m]/F;
      if (ff<=0) continue;
      int klm=floor(ff);
      double f0=ff-klm;
      double hlm=h[l][klm]*(1-f0)+h[l][klm+1]*f0;
      b[l][m]=loga[l][m]-hlm;
    }

  for (int k=0; k<K+2; k++){avgh[k]=0; for (int l=0; l<L; l++) avgh[k]+=h[l][k]; avgh[k]/=L;}
  for (int m=0; m<M; m++){avgb[m]=0; for (int l=0; l<L; l++) avgb[m]+=b[l][m]; avgb[m]/=L;}

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

/**
  function S_F_SV_cf: slow-variation method for estimating source-(constant)filter model. This is a
  wrapper function of SF_SV_cf() that transfers and scales values, as well as converting results to
  source filter format used by the vibrato class TVo.

  In: Partials[M][Fr]: HS partials. Fr is not specified but is assumed no less then lp[P-1].
      lp[P]: temporal segmentation points, in frames.
      A0C[Fr]: amplitude carrier (linear, not dB)
      F: filter response sampling interval
      FScaleMode: set if mel scale is used as frequency scale
      Fs: sampling frequency, effective only when FScaleMode is set.
  Out: K
       h[L][K+2], aveh[K+2]: filter coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
       b[L][M], avgb[M]: source coefficients for L frames from ceil(lp[0]) to ceil(lp[P-1]), and their averages
       LogAF[afres/2]: TVo-format filter response
       LogAS[M]: TVo-format source response

  Returns 0.
*/
double S_F_SV_cf(int afres, double* LogAF, double* LogAS, int Fr, int M, atom** Partials, double* A0C, double* lp, int P, int& K, double** h, double** b, double F, int FScaleMode, double Fs)
{
  int lp0=ceil(lp[0]), lpp=ceil(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++)
    {
      try{
      f[l][m]=Partials[m][fr].f;
      }
      catch(...)
      {
        m=m;
      }
      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];
    }
  }
  K=floor(fmax/F);

  SF_SV_cf(h[0], b, L, M, K, loga, f, F, 1e-6, K+2);
  for (int l=0; l<L; l++)
  {
    if (l>0) memcpy(h[l], h[0], sizeof(double)*(K+2));
    double loga0=log(A0C[l]);
    for (int m=0; m<M; m++) b[l][m]-=loga0;
  }

  double* lh=new double[K+2]; memset(lh, 0, sizeof(double)*(K+2));
  for (int k=0; k<K+2; k++)
  {
    for (int l=0; l<L; l++) lh[k]+=h[l][k];
    lh[k]/=L;
  }
  for (int i=0; i<afres/2; i++)
  {
    double freq=i*1.0/afres;
    if (FScaleMode) freq=log(1+freq*Fs/700)/log(1+Fs/700);
    int k=floor(freq/F);
    double f_plus=freq/F-k;
    if (k<K+1) LogAF[i]=lh[k]*(1-f_plus)+lh[k+1]*f_plus;
    else LogAF[i]=lh[K+1];
  }
  delete[] lh;

  for (int m=0; m<M; m++)
  {
    int ccount=0;
    for (int fr=lp0; fr<lpp; 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);
  return 0;
}//S_F_SV_cf

/**
  function SF_FB: computes filter coefficients with filter-bank method: single frame. See "further
  reading", pp.12, P5.

  In: al[][M], fl[][M]: partial amplitudes and frequencies, al[0] and fl[0] are of current frame
      F: filter response sampling interval
      LMode: 1: current frame is the first frame; -1: the last frame; 0: neither first nor last frame
  Out: hl[K+2]: filter coefficients integrated at current frame

  Returns K.
*/
int SF_FB(double* hl, int M, int K, double** al, double** fl, double F, int LMode)
{
  memset(hl, 0, sizeof(double)*(K+2));
  double* norm=new double[K+2]; memset(norm, 0, sizeof(double)*(K+2));
  for (int m=0; m<M; m++)
  {
    int dfsgn, k, K1;
    if (LMode!=1)
    {
      if (fl[0][m]-fl[-1][m]==0)
      {
        double a0, wk, wk_, a1;
        int k=floor(fl[0][m]/F);
        a0=al[-1][m], a1=al[0][m];
        wk=1-(fl[0][m]/F-k), wk_=1-wk;
        double dh=(a0+2*a1)/6.0;
        hl[k]+=wk*dh;   //I3(1, wk, 0, a0, wk, 1, a1);
        hl[k+1]+=wk_*dh;//I3(1, wk_, 0, a0, wk_, 1, a1);
        norm[k]+=wk*0.5;//I3(1, wk, 0, 1, wk, 1, 1);
        norm[k+1]+=wk_*0.5;//I3(1, wk_, 0, 1, wk_, 1, 1);
      }
      else
      {
        double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_;
        dfsgn=Sign(fl[0][m]-fl[-1][m]);
        if (dfsgn>0) K1=ceil(fl[-1][m]/F); else K1=floor(fl[-1][m]/F);
        t0=-1; k=K1;
        if (fl[-1][m]>0 && fl[0][m]>0) do
        {
          t1=-1+(k*F-fl[-1][m])/(fl[0][m]-fl[-1][m]);
          if (t1>0) t1=0;

          if (t0==-1){a0=al[-1][m],	u0=0,	w0k=1-fabs(fl[-1][m]/F-k); w0k_=1-w0k;}
          else{a0=al[0][m]+t0*(al[0][m]-al[-1][m]),	u0=1+t0, w0k=0, w0k_=1;}
          if (t1==0){a1=al[0][m], u1=1, w1k=1-fabs(fl[0][m]/F-k); w1k_=1-w1k;}
          else{a1=al[0][m]+t1*(al[0][m]-al[-1][m]), u1=1+t1, w1k=1, w1k_=0;}

          hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1);
          hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1);
          norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1);
          norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1);

          t0=t1;
          k+=dfsgn;
        }
        while (t1<0);
      }
    }

    if (LMode!=-1)
    {
      double a0, wk, wk_, a1;
      if (fl[1][m]-fl[0][m]==0)
      {
        int k=floor(fl[0][m]/F);
        a0=al[0][m], a1=al[1][m];
        wk=1-fabs(fl[0][m]/F-k), wk_=1-wk;
        double dh=(2*a0+a1)/6.0;
        hl[k]+=wk*dh;   //I3(1, wk, 1, a0, wk, 0, a1);
        hl[k+1]+=wk_*dh;//I3(1, wk_, 1, a0, wk_, 0, a1);
        norm[k]+=wk*0.5;//I3(1, wk, 1, 1, wk, 0, 1);
        norm[k+1]+=wk_*0.5;//I3(1, wk_, 1, 1, wk_, 0, 1);
      }
      else
      {
        double t0, t1, a0, u0, w0k, w0k_, a1, u1, w1k, w1k_;
        dfsgn=Sign(fl[1][m]-fl[0][m]);
        if (dfsgn>0) K1=ceil(fl[0][m]/F);	else K1=floor(fl[0][m]/F);
        t0=0; k=K1;
        if (fl[0][m]>0 && fl[1][m]>0)	do
        {
          try{
          t1=(k*F-fl[0][m])/(fl[1][m]-fl[0][m]);}
          catch(...){}
          if (t1>1) t1=1;

          if (t0==0){a0=al[0][m], u0=1, w0k=1-fabs(fl[0][m]/F-k); w0k_=1-w0k;}
          else{a0=al[0][m]+t0*(al[1][m]-al[0][m]), u0=1-t0, w0k=1, w0k_=0;}
          if (t1==1){a1=al[1][m], u1=0, w1k=1-fabs(fl[1][m]/F-k); w1k_=1-w1k;}
          else{a1=al[0][m]+t1*(al[1][m]-al[0][m]), u1=1-t1, w1k=0, w1k_=1;}

          hl[k]+=I3(t1-t0, w0k, u0, a0, w1k, u1, a1);
          hl[k-dfsgn]+=I3(t1-t0, w0k_, u0, a0, w1k_, u1, a1);
          norm[k]+=I3(t1-t0, w0k, u0, 1, w1k, u1, 1);
          norm[k-dfsgn]+=I3(t1-t0, w0k_, u0, 1, w1k_, u1, 1);

          t0=t1;
          k+=dfsgn;
        }
        while (t1<1);
      }
    }
  }

  int mink=0; while (mink<=K+1 && norm[mink]==0) mink++;
  int maxk=K+1; while (maxk>=0 && norm[maxk]==0) maxk--;
  int lastk=mink, nextk=-1;
  for (int k=mink; k<=maxk; k++)
  {
    if (k==nextk) lastk=k;
    else if (norm[k]!=0) hl[k]=log(hl[k]/norm[k]), lastk=k;
    else
    {
      if (k>nextk){nextk=k+1; while (norm[nextk]==0) nextk++; hl[nextk]=log(hl[nextk]/norm[nextk]);}
      hl[k]=(hl[lastk]*(nextk-k)+hl[nextk]*(k-lastk))/(nextk-lastk);
    }
  }
  for (int k=0; k<mink; k++) hl[k]=hl[mink];
  for (int k=maxk+1; k<K+2; k++) hl[k]=hl[maxk];

  delete[] norm;
  return K;
}//SF_FB

/**
  function SF_SV: CG method for estimation source-filter model using slow-variation criterion. See
  "further reading", pp.9, boxed algorithm P4.

  In: a[L][M], f[L][M]: partial amplitudes and frequencies
      F: filter response sampling interval
      theta: balancing factor of amplitude and frequency variations
      ep: convergence test threshold
      maxiter: maximal number of iterates
  Out: h[L][K+2]: filter coefficients

  Returns K.
*/
int SF_SV(double** h, int L, int M, int K, double** a, double** f, double F, double theta, double ep, int maxiter)
{
  Alloc2(L*7, K+2, b);
  double **Ax=&b[L], **d=&Ax[L], **r=&d[L], **q=&r[L];
  for (int l=0; l<L; l++) memset(h[l], 0, sizeof(double)*(K+2));
  //calcualte b
  P1_b(b, L, M, K, a, f, F, theta);
  //calculate Ah
  //double hAh=
  P_Ax(Ax, L, M, K, h, f, F, theta);
  //double **t_Ax=&b[L*5], **t_h=&b[L*6], epdel=1e-10, t_err=0;

  //double bh=Inner(L, K+2, b, h);  //debug
  //double Del=0.5*hAh-bh;          //debug

  MultiAdd(L, K+2, r, b, Ax, -1);
  Copy(L, K+2, d, r);
  double delta=Inner(L, K+2, r, r);
  ep=ep*delta;

  int iter=0;
  while(iter<maxiter && delta>ep)
  {
    P_Ax(q, L, M, K, d, f, F, theta);
    /*debug point: watch if hAh-bh keeps decreasing
    double lastdel=Del;
    hAh=P_Ax(t_Ax, L, M, K, h, f, F, theta);
    bh=Inner(L, K+2, b, h);
    Del=hAh-bh;
    if (Del>lastdel)
    {lastdel=Del;} //set breakpoint here*/
    double alpha=delta/Inner(L, K+2, d, q);
    MultiAdd(L, K+2, h, h, d, alpha);
    if (iter%50==0 && false){P_Ax(Ax, L, M, K, h, f, F, theta); MultiAdd(L, K+2, r, b, Ax, -1);}
    else MultiAdd(L, K+2, r, r, q, -alpha);
    double deltanew=Inner(L, K+2, r, r);
    MultiAdd(L, K+2, d, r, d, deltanew/delta);
    delta=deltanew;
    iter++;
  }

  DeAlloc2(b);
  return K;
}//SF_SV

/**
  function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion.

  In: a[L][M], f[L][M]: partial amplitudes and frequencies
      F: filter response sampling interval
      ep: convergence test threshold
      maxiter: maximal number of iterates
  Out: h[K+2]: filter coefficients

  Returns K.
*/
int SF_SV_cf(double* h, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter)
{
  double* b=new double[(K+2)*7];
  double *Ax=&b[K+2], *d=&Ax[K+2], *r=&d[K+2], *q=&r[K+2];

  //calcualte b
  P1_b_cf(b, L, M, K, a, f, F);
  //calculate Ah
  //double hAh=
  P_Ax_cf(Ax, L, M, K, h, f, F);

  MultiAdd(K+2, r, b, Ax, -1);
  Copy(K+2, d, r);
  double delta=Inner(K+2, r, r);
  ep=ep*delta;

  int iter=0;
  while(iter<maxiter && delta>ep)
  {
    P_Ax_cf(q, L, M, K, d, f, F);
    double alpha=delta/Inner(K+2, d, q);
    MultiAdd(K+2, h, h, d, alpha);
    if (iter%50==0 && false){P_Ax_cf(Ax, L, M, K, h, f, F); MultiAdd(K+2, r, b, Ax, -1);}
    else MultiAdd(K+2, r, r, q, -alpha);
    double deltanew=Inner(K+2, r, r);
    MultiAdd(K+2, d, r, d, deltanew/delta);
    delta=deltanew;
    iter++;
  }

  delete[] b;
  return K;
}//SF_SV_cf

/**
  function SF_SV_cf: CG method for estimation source-(constant)filter model by slow-variation criterion.

  In: a[L][M], f[L][M]: partial amplitudes and frequencies
      F: filter response sampling interval
      ep: convergence test threshold
      maxiter: maximal number of iterates
  Out: h[K+2]: filter coefficients
       b[L][M]: source coefficients

  No reutrn value.
*/
void SF_SV_cf(double* h, double** b, int L, int M, int K, double** a, double** f, double F, double ep, int maxiter)
{
  memset(h, 0, sizeof(double)*(K+2));
  SF_SV_cf(h, L, M, K, a, f, F, ep, maxiter);
  for (int l=0; l<L; l++)
  for (int m=0; m<M; m++)
  {
    double ff=f[l][m]/F;
    if (ff<=0) continue;
    int klm=floor(ff);
    double f0=ff-klm;
    double hlm=h[klm]*(1-f0)+h[klm+1]*f0;
    b[l][m]=a[l][m]-hlm;
  }
}//SF_SV_cf

/**
  function Sign: sign function

  In: x: a floating-point number

  Returns 0 if x=0, 1 if x>0, -1 if x<0.
*/
int Sign(double x)
{
  if (x==0) return 0;
  else if (x>0) return 1;
  else return -1;
}//Sign

/**
  function SynthesizeSF: synthesizes a harmonic sinusoid representation from a TSF object.

  In: SF: a TSF object
      sps: sampling rate ("samples per second")
  Out: HS: teh synthesized HS.

  No return value.
*/
void SynthesizeSF(THS* HS, TSF* SF, double sps)
{
  int t0=HS->Partials[0][0].t, s0=HS->Partials[0][0].s, L=SF->L, M=SF->M;
  double *F0C=SF->F0C, *F0D=SF->F0D, *F0=SF->F0, offst=SF->offst;
  double *logA0C=useA0?SF->logA0:SF->logA0C;
  HS->Resize(M, L);

  atom** Partials=HS->Partials;

  for (int l=0; l<L; l++)
  {
    double tl=t0+offst*l;
    F0[l]=F0C[l]+F0D[l];
    for (int m=0; m<M; m++)
    {
      Partials[m][l].s=s0;
      Partials[m][l].t=tl;
      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 Partials[m][l].a=exp(logA0C[l]+SF->LogAS(m,l)+SF->LogAF(Partials[m][l].f,l));
    }
  }
}//SynthesizeSF