view hs.cpp @ 1:6422640a802f

first upload

//---------------------------------------------------------------------------
#include <math.h>
#include "hs.h"
#include "align8.h"
#include "arrayalloc.h"
#include "procedures.h"
#include "SinEst.h"
#include "SinSyn.h"
#include "splines.h"
#include "xcomplex.h"
#ifdef I
#undef I
#endif

//---------------------------------------------------------------------------
//stiffcandid methods

/*
  method stiffcandid::stiffcandid: creates a harmonic atom with minimal info, i.e. fundamantal frequency
  between 0 and Nyqvist frequency, stiffness coefficient between 0 and STIFF_B_MAX.

  In: Wid: DFT size (frame size, frequency resolution)
*/
stiffcandid::stiffcandid(int Wid)
{
  F=new double[6];
  G=&F[3];
  double Fm=Wid*Wid/4; //NyqvistF^2
  F[0]=0, G[0]=0;
  F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
  F[2]=Fm, G[2]=0;
  N=3;
  P=0;
  s=0;
  p=NULL;
  f=NULL;
}//stiffcandid

/*
  method stiffcandid::stiffcandid: creates a harmonic atom with a frequency range for the $ap-th partial,
  without actually taking this partial as "known".

  In; Wid: DFT size
      ap: partial index
      af, errf: centre and half-width of the frequency range of the ap-th partial, in bins
*/
stiffcandid::stiffcandid(int Wid, int ap, double af, double errf)
{
  F=new double[10];
  G=&F[5];
  double Fm=Wid*Wid/4;
  F[0]=0, G[0]=0;
  F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
  F[2]=Fm, G[2]=0;
  N=3;
  P=0;
  s=0;
  p=NULL;
  f=NULL;
  double k=ap*ap-1;
  double g1=(af-errf)/ap, g2=(af+errf)/ap;
  if (g1<0) g1=0;
  g1=g1*g1, g2=g2*g2;
  //apply constraints F+kG-g2<0 and -F-kG+g1<0
  cutcvpoly(N, F, G, 1, k, -g2);
  cutcvpoly(N, F, G, -1, -k, g1);
}//stiffcandid

/*
  method stiffcandid::stiffcandid: creates a harmonic atom from one atom.

  In; Wid: DFT size
      ap: partial index of the atom.
      af, errf: centre and half-width of the frequency range of the atom, in bins.
      ds: contribution to candidate score from this atom.
*/
stiffcandid::stiffcandid(int Wid, int ap, double af, double errf, double ds)
{
  //the starting polygon implements the trivial constraints 0<f0<NyqvistF,
  //  0<B<Bmax. in terms of F and G, 0<F<NF^2, 0<G<FBmax. this is a triangle
  //  with vertices (0, 0), (NF^2, Bmax*NF^2), (NF^2, 0)
  F=new double[12];
  G=&F[5], f=&F[10], p=(int*)&F[11];
  double Fm=Wid*Wid/4; //NyqvistF^2
  F[0]=0, G[0]=0;
  F[1]=Fm, G[1]=STIFF_B_MAX*Fm;
  F[2]=Fm, G[2]=0;

  N=3;
  P=1;
  p[0]=ap;
  f[0]=af;
  s=ds;

  double k=ap*ap-1;
  double g1=(af-errf)/ap, g2=(af+errf)/ap;
  if (g1<0) g1=0;
  g1=g1*g1, g2=g2*g2;
  //apply constraints F+kG-g2<0 and -F-kG+g1<0
  cutcvpoly(N, F, G, 1, k, -g2);
  cutcvpoly(N, F, G, -1, -k, g1);
}//stiffcandid

/*
  method stiffcandid::stiffcandid: creates a harmonic atom by adding a new partial to an existing
  harmonic atom.

  In; prev: initial harmonic atom
      ap: partial index of new atom
      af, errf: centre and half-width of the frequency range of the new atom, in bins.
      ds: contribution to candidate score from this atom.
*/
stiffcandid::stiffcandid(stiffcandid* prev, int ap, double af, double errf, double ds)
{
  N=prev->N, P=prev->P, s=prev->s;
  int Np2=N+2, Pp1=P+1;
  F=new double[(Np2+Pp1)*2];
  G=&F[Np2]; f=&G[Np2]; p=(int*)&f[Pp1];
  memcpy(F, prev->F, sizeof(double)*N);
  memcpy(G, prev->G, sizeof(double)*N);
  if (P>0)
  {
    memcpy(f, prev->f, sizeof(double)*P);
    memcpy(p, prev->p, sizeof(int)*P);
  }
  //see if partial ap is already involved
  int i=0; while (i<P && p[i]!=ap) i++;
  //if it is not:
  if (i>=P)
  {
    p[P]=ap;
    f[P]=af;
    s+=ds;
    P++;

    double k=ap*ap-1;
    double g1=(af-errf)/ap, g2=(af+errf)/ap;
    if (g1<0) g1=0;
    g1=g1*g1, g2=g2*g2;
    //apply constraints F+kG-g2<0 and -F-kG+g1<0
    cutcvpoly(N, F, G, 1, k, -g2);
    cutcvpoly(N, F, G, -1, -k, g1);
  }
  //otherwise, the new candid is simply a copy of prev
}//stiffcandid

//method stiffcandid::~stiffcandid: destructor
stiffcandid::~stiffcandid()
{
  delete[] F;
}//~stiffcandid


//---------------------------------------------------------------------------
//THS methods

//method THS::THS: default constructor
THS::THS()
{
  memset(this, 0, sizeof(THS));
  memset(BufM, 0, sizeof(void*)*128);
}//THS

/*
  method THS::THS: creates an HS with given number of partials and frames.

  In: aM, aFr: number of partials and frames
*/
THS::THS(int aM, int aFr)
{
  memset(this, 0, sizeof(THS));
  M=aM; Fr=aFr; Allocate2(atom, M, Fr, Partials);
  memset(Partials[0], 0, sizeof(atom)*M*Fr);
  memset(BufM, 0, sizeof(void*)*128);
}//THS

/*
  method THS::THS: creates a duplicate HS. This does not duplicate contents of BufM.

  In: HS: the HS to duplicate
*/
THS::THS(THS* HS)
{
  memcpy(this, HS, sizeof(THS));
  Allocate2(atom, M, Fr, Partials);
  for (int m=0; m<M; m++) memcpy(Partials[m], HS->Partials[m], sizeof(atom)*Fr);
  Allocate2(double, M, st_count, startamp);
  if (st_count) for (int m=0; m<M; m++) memcpy(startamp[m], HS->startamp[m], sizeof(double)*st_count);
  memset(BufM, 0, sizeof(void*)*128);
}//THS

/*
  method THS::THS: create a new HS which is a trunction of an existing HS.

  In: HS: the exinting HS from which a sub-HS is to be created
      start, end: truncation points. Atoms of *HS beyond these points are discarded.
*/
THS::THS(THS* HS, double start, double end)
{
  memset(this, 0, sizeof(THS));
  M=HS->M; Fr=HS->Fr; isconstf=HS->isconstf;
  int frst=0, fren=Fr;
  while (HS->Partials[0][frst].t<start && frst<fren) frst++;
  while (HS->Partials[0][fren-1].t>end && fren>frst) fren--;
  Fr=fren-frst;
  Allocate2(atom, M, Fr, Partials);
  for (int m=0; m<M; m++)
  {
    memcpy(Partials[m], &HS->Partials[m][frst], sizeof(atom)*Fr);
    for (int fr=0; fr<Fr; fr++) Partials[m][fr].t-=start;
  }
  if (frst>0){DeAlloc2(startamp); startamp=0;}
}//THS

//method THS::~THS: default descructor.
THS::~THS()
{
  DeAlloc2(Partials);
  DeAlloc2(startamp);
  ClearBufM();
}//~THS

/*
  method THS::ClearBufM: free buffers pointed to by members of BufM.
*/
void THS::ClearBufM()
{
  for (int m=0; m<128; m++) delete[] BufM[m];
  memset(BufM, 0, sizeof(void*)*128);
}//ClearBufM

/*
  method THS::EndPos: the end position of an HS.

  Returns the time of the last frame of the first partial.
*/
int THS::EndPos()
{
  return Partials[0][Fr-1].t;
}//EndPos

/*
  method THS::EndPosEx: the extended end position of an HS.

  Returns the time later than the last frame of the first partial by the interval between the first and
  second frames of that partial.
*/
int THS::EndPosEx()
{
  return Partials[0][Fr-1].t+Partials[0][1].t-Partials[0][0].t;
}//EndPosEx

/*
  method THS::ReadAtomFromStream: reads an atom from a stream.

  Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
*/
int THS::ReadAtomFromStream(TStream* Stream, atom* atm)
{
  int StartPosition=Stream->Position, atomlen; char s[4];
  if (Stream->Read(s, 4)!=4) goto ret0;
  if (strcmp(s, "ATM")) goto ret0;
  if (Stream->Read(&atomlen, sizeof(int))!=sizeof(int)) goto ret0;
  if (Stream->Read(atm, atomlen)!=atomlen) goto ret0;
  return atomlen+4+sizeof(int);
ret0:
  Stream->Position=StartPosition;
  return 0;
}//ReadAtomFromStream

/*
  method THS::ReadFromStream: reads an HS from a stream.

  Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
*/
int THS::ReadFromStream(TStream* Stream)
{
  int StartPosition=Stream->Position, lenevt; char s[4];
  if (Stream->Read(s, 4)!=4) goto ret0;
  if (strcmp(s, "EVT")) goto ret0;
  if (Stream->Read(&lenevt, sizeof(int))!=sizeof(int)) goto ret0;
  if (!ReadHdrFromStream(Stream)) goto ret0;
  Resize(M, Fr, false, true);
  for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) if (!ReadAtomFromStream(Stream, &Partials[m][fr])) goto ret0;
  lenevt=lenevt+4+sizeof(int);
  if (lenevt!=Stream->Position-StartPosition) goto ret0;
  return lenevt;
ret0:
  Stream->Position=StartPosition;
  return 0;
}//ReadFromStream

/*
  method THS::ReadHdrFromStream: reads header from a stream into this HS.

  Returns the number of bytes read, or 0 on failure. The stream pointer is shifted by this value.
*/
int THS::ReadHdrFromStream(TStream* Stream)
{
  int StartPosition=Stream->Position, hdrlen, tags; char s[4];
  if (Stream->Read(s, 4)!=4) goto ret0;
  if (strcmp(s, "HDR")) goto ret0;
  if (Stream->Read(&hdrlen, sizeof(int))!=sizeof(int)) goto ret0;
  if (Stream->Read(&Channel, sizeof(int))!=sizeof(int)) goto ret0;
  if (Stream->Read(&M, sizeof(int))!=sizeof(int)) goto ret0;
  if (Stream->Read(&Fr, sizeof(int))!=sizeof(int)) goto ret0;
  if (Stream->Read(&tags, sizeof(int))!=sizeof(int)) goto ret0;
  isconstf=tags&HS_CONSTF;
  hdrlen=hdrlen+4+sizeof(int); //expected read count
  if (hdrlen!=Stream->Position-StartPosition) goto ret0;
  return hdrlen;
ret0:
  Stream->Position=StartPosition;
  return 0;
}//ReadHdrFromStream

/*
  method THS::Resize: change the number of partials or frames of an HS structure. Unless forcealloc is
  set to true, this allocates new buffers to host HS data only when the new dimensions are larger than
  current ones.

  In: aM, aFr: new number of partials and frames
      copydata: specifies if HS data is to be copied to new buffers.
      forcealloc: specifies if new buffers are to be allocated in all cases.
*/
void THS::Resize(int aM, int aFr, bool copydata, bool forcealloc)
{
  if (forcealloc || M<aM || Fr<aFr)
  {
    atom** Allocate2(atom, aM, aFr, NewPartials);
    if (copydata)
    {
      int minM=(M<aM)?M:aM, minFr=(Fr<aFr)?Fr:aFr;
      for (int m=0; m<minM; m++) memcpy(NewPartials[m], Partials[m], sizeof(atom)*minFr);
    }
    DeAlloc2(Partials);
    Partials=NewPartials;
  }
  if (forcealloc || M<aM)
  {
    double** Allocate2(double, aM, st_count, newstartamp);
    if (copydata)
    {
      for (int m=0; m<M; m++) memcpy(newstartamp[m], startamp[m], sizeof(double)*st_count);
    }
    DeAlloc2(startamp);
    startamp=newstartamp;
  }
  M=aM, Fr=aFr;
}//Resize

/*
  method THS::StartPos: the start position of an HS.

  Returns the time of the first frame of the first partial.
*/
int THS::StartPos()
{
  return Partials[0][0].t;
}//StartPos

/*
  method THS::StdOffst: standard hop size of an HS.
  Returns the interval between the timing of the first and second frames of the first partial, which is
  the "standard" hop size if all measurement points are uniformly positioned.
*/
int THS::StdOffst()
{
  return Partials[0][1].t-Partials[0][0].t;
}//StdOffst

/*
  method THS::WriteAtomToStream: writes an atom to a stream. An atom is stored in a stream as an RIFF
  chunk identified by "ATM\0".

  Returns the number of bytes written. The stream pointer is shifted by this value.
*/
int THS::WriteAtomToStream(TStream* Stream, atom* atm)
{
  Stream->Write((void*)"ATM", 4);
  int atomlen=sizeof(atom);
  Stream->Write(&atomlen, sizeof(int));
  atomlen=Stream->Write(atm, atomlen);
  return atomlen+4+sizeof(int);
}//WriteAtomToStream

/*
  method THS::WriteHdrToStream: writes header to a stream. HS header is stored in a stream as an RIFF
  chunk, identified by "HDR\0".

  Returns the number of bytes written. The stream pointer is shifted by this value.
*/
int THS::WriteHdrToStream(TStream* Stream)
{
  Stream->Write((void*)"HDR", 4);
  int hdrlen=0, tags=0;
  if (isconstf) tags=tags|HS_CONSTF;
  Stream->Write(&hdrlen, sizeof(int));
  Stream->Write(&Channel, sizeof(int)); hdrlen+=sizeof(int);
  Stream->Write(&M, sizeof(int));  hdrlen+=sizeof(int);
  Stream->Write(&Fr, sizeof(int)); hdrlen+=sizeof(int);
  Stream->Write(&tags, sizeof(int)); hdrlen+=sizeof(int);
  Stream->Seek(-hdrlen-sizeof(int), soFromCurrent); Stream->Write(&hdrlen, sizeof(int));
  Stream->Seek(hdrlen, soFromCurrent);
  return hdrlen+4+sizeof(int);
}//WriteHdrToStream

/*
  method THS::WriteToStream: write an HS to a stream. An HS is stored in a stream as an RIFF chunk
  identified by "EVT\0". Its chunk data contains an HS header chunk followed by M*Fr atom chunks, in
  rising order of m then fr.

  Returns the number of bytes written. The stream pointer is shifted by this value.
*/
int THS::WriteToStream(TStream* Stream)
{
  Stream->Write((void*)"EVT", 4);
  int lenevt=0;
  Stream->Write(&lenevt, sizeof(int));
  lenevt+=WriteHdrToStream(Stream);

  for (int m=0; m<M; m++) for (int fr=0; fr<Fr; fr++) lenevt+=WriteAtomToStream(Stream, &Partials[m][fr]);
  Stream->Seek(-lenevt-sizeof(int), soFromCurrent);
  Stream->Write(&lenevt, sizeof(int));
  Stream->Seek(lenevt, soFromCurrent);
  return lenevt+4+sizeof(int);
}//WriteToStream


//---------------------------------------------------------------------------
//TPolygon methods

/*
  method TPolygon::TPolygon: create an empty polygon with a storage capacity

  In: cap: number of vertices to allocate memory for.
*/
TPolygon::TPolygon(int cap)
{
  X=(double*)malloc8(sizeof(double)*cap*2);
  Y=&X[cap];
}//TPolygon

/*
  method TPolygon::TPolygon: create a duplicate polygon.

  In: cap: number of vertices to allocate memory for.
      R: the polygon to duplicate.

  If cap is smaller than the number of vertices of R, the latter is used for memory allocation.
*/
TPolygon::TPolygon(int cap, TPolygon* R)
{
  if (cap<R->N) cap=R->N; X=(double*)malloc8(sizeof(double)*cap*2); Y=&X[cap]; N=R->N;
  memcpy(X, R->X, sizeof(double)*R->N); memcpy(Y, R->Y, sizeof(double)*R->N);
}//TPolygon

//method TPolygon::~TPolygon: default destructor.
TPolygon::~TPolygon()
{
  free8(X);
}//~TPolygon

//---------------------------------------------------------------------------
//TTempAtom methods

/*
  method TTempAtom::TTempAtom: initializes an empty atom with a fundamental frequency range.

  In: af, ef: centre and half width of the fundamental frequency
      maxB: upper bound of stiffness coefficient
*/
TTempAtom::TTempAtom(double af, double ef, double maxB)
{
  Prev=NULL; pind=f=a=s=acce=0;
  R=new TPolygon(4);
  InitializeR(R, af, ef, maxB);
}//TTempAtom

/*
  method TTempAtom::TTempAtom: initialize an empty atom with a frequency range for a certain partial.

  In: apin: partial index
      af, ef: centre and half width of the fundamental frequency
      maxB: upper bound of stiffness coefficient
*/
TTempAtom::TTempAtom(int apin, double af, double ef, double maxB)
{
  Prev=NULL; pind=f=a=s=acce=0;
  R=new TPolygon(4);
  InitializeR(R, apin, af, ef, maxB);
}//TTempAtom

/*
  method TTempAtom::TTempAtom: initialize an empty atom whose F-G polygon is the extension of AR

In: AR: the F-G polygon to extend
      delf1, delf2: amount of extension, in semitones, of fundamental frequency
      minf: minimal fundamental frequency: extension will not go below this value
*/
TTempAtom::TTempAtom(TPolygon* AR, double delf1, double delf2, double minf)
{
  Prev=NULL; pind=f=a=s=acce=0;
  R=new TPolygon(AR->N+2);
  R->N=AR->N;
  memcpy(R->X, AR->X, sizeof(double)*AR->N); memcpy(R->Y, AR->Y, sizeof(double)*AR->N);
  ExtendR(R, delf1, delf2, minf);
}//TTempAtom

/*
  method TTempAtom::TTempAtom: initialize a atom as the only partial.

  In: apind: partial index
      af, ef: partial frequency and its error range
      aa, as: partial amplitude and measurement scale
      maxB: stiffness coefficient upper bound
*/
TTempAtom::TTempAtom(int apind, double af, double ef, double aa, double as, double maxB)
{
  Prev=NULL; pind=apind; f=af; a=aa; s=as; acce=aa*aa;
  R=new TPolygon(4);
  InitializeR(R, apind, af, ef, maxB);
}//TTempAtom

/*
  method TTempAtom::TTempAtom: creates a duplicate atom. R can be duplicated or snatched according to
  DupR.

  In: APrev: the atom to duplicate
      DupR: specifies if the newly created atom is to have its F-G polygon duplicated from that of APrev
        or to snatch the F-G polygon from APrev (in the latter case APrev losts its polygon).
*/
TTempAtom::TTempAtom(TTempAtom* APrev, bool DupR)
{
  Prev=APrev; pind=APrev->pind; f=APrev->f; a=APrev->a; s=APrev->s; acce=APrev->acce;
  TPolygon* PrevR=APrev->R;
  if (DupR)
  {//duplicate R
    R=new TPolygon(PrevR->N);
    R->N=PrevR->N; memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N); memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N);
  }
  else
  {//snatch R from APrev
    R=APrev->R;
    APrev->R=0;
  }
}//TTempAtom

/*
  method TTempAtom::TTempAtom:: initializes an atom by adding a new partial to an existing group.

  In: APrev: the existing atom.
      apind: partial index
      af, ef: partial frequency and its error range
      aa, as: partial amplitude and measurement scale
      updateR: specifies if the F-G polygon is updated using (af, ef).
*/
TTempAtom::TTempAtom(TTempAtom* APrev, int apind, double af, double ef, double aa, double as, bool updateR)
{
  Prev=APrev; pind=apind; f=af; a=aa;
  s=as; acce=APrev->acce+aa*aa;
  TPolygon* PrevR=APrev->R;
  R=new TPolygon(PrevR->N+2);                         //  create R
  R->N=PrevR->N;                                      //
  memcpy(R->X, PrevR->X, sizeof(double)*PrevR->N);    // copy R
  memcpy(R->Y, PrevR->Y, sizeof(double)*PrevR->N);    //
  if (updateR) CutR(R, apind, af, ef);
}//TTempAtom

//method TTempAtom::~TTempAtom: default destructor
TTempAtom::~TTempAtom()
{
  delete R;
}//~TTempAtom


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

/*
  function areaandcentroid: calculates the area and centroid of a convex polygon.

  In: x[N], y[N]: x- and y-coordinates of vertices of a polygon
  Out: A: area
       (cx, cy): coordinate of the centroid

  No return value.
*/
void areaandcentroid(double& A, double& cx, double& cy, int N, double* x, double* y)
{
  if (N==0) {A=0;}
  else if (N==1) {A=0; cx=x[0], cy=y[0];}
  else if (N==2) {A=0; cx=(x[0]+x[1])/2, cy=(y[0]+y[1])/2;}
  A=cx=cy=0;
  for (int i=0; i<N; i++)
  {
    double xi1, yi1; //x[i+1], y[i+1], index modular N
    if (i+1==N) xi1=x[0], yi1=y[0];
    else xi1=x[i+1], yi1=y[i+1];
    double tmp=x[i]*yi1-xi1*y[i];
    A+=tmp;
    cx+=(x[i]+xi1)*tmp;
    cy+=(y[i]+yi1)*tmp;
  }
  A/=2;
  cx=cx/6/A;
  cy=cy/6/A;
}//areaandcentroid

/*
  function AtomsToPartials: sort a list of atoms as a number of partials. It is assumed that the atoms
  are simply those from a harmonic sinusoid in arbitrary order with uniform timing and partial index
  clearly marked, so that it is easy to sort them into a list of partials. However, the sorting process
  does not check for harmonicity, missing or duplicate atoms, uniformity of timing, etc.

  In: part[k]: a list of atoms
      offst: interval between adjacent measurement points (hop size)
  Out: M, Fr, partials[M][Fr]: a list of partials containing the atoms.

  No return value. partials[][] is allocated anew and must be freed by caller.
*/
void AtomsToPartials(int k, atom* part, int& M, int& Fr, atom**& partials, int offst)
{
  if (k>0)
  {
    int t1, t2, i=0;
    while (i<k && part[i].f<=0) i++;
    if (i<k)
    {
      t1=part[i].t, t2=part[i].t, M=part[i].pin; i++;
      while (i<k)
      {
        if (part[i].f>0)
        {
          if (M<part[i].pin) M=part[i].pin;
          if (t1>part[i].t) t1=part[i].t;
          if (t2<part[i].t) t2=part[i].t;
        }
        i++;
      }
      Fr=(t2-t1)/offst+1;
      Allocate2(atom, M, Fr, partials); memset(partials[0], 0, sizeof(atom)*M*Fr);
      for (int i=0; i<k; i++)
        if (part[i].f>0)
        {
          int fr=(part[i].t-t1)/offst;
          int pp=part[i].pin;
          partials[pp-1][fr]=part[i];
        }
    }
    else M=0, Fr=0;
  }
  else M=0, Fr=0;
}//AtomsToPartials

/*
  function conta: a continuity measure between two set of amplitudes

  In: a1[N], a2[N]: the amplitudes

  Return the continuity measure, between 0 and 1
*/
double conta(int N, double* a1, double* a2)
{
  double e11=0, e22=0, e12=0;
  for (int n=0; n<N; n++)
  {
    double la1=(a1[n]>0)?a1[n]:0, la2=(a2[n]>0)?a2[n]:0;
    if (la1>1e10) la1=la2;
    e11+=la1*la1;
    e12+=la1*la2;
    e22+=la2*la2;
  }
  return 2*e12/(e11+e22);
}//conta

/*
  function cutcvpoly: severs a polygon by a straight line

  In: x[N], y[N]: x- and y-coordinates of vertices of a polygon, starting from the leftmost (x[0]=
        min x[n]) point clockwise; in case of two leftmost points, x[0] is the upper one and x[N-1] is
        the lower one.
      A, B, C: coefficients of a straight line Ax+By+C=0.
      protect: specifies what to do if the whole polygon satisfy Ax+By+C>0, true=do noting,
        false=eliminate.
  Out: N, x[N], y[N]: the polygon severed by the line, retaining that half for which Ax+By+C<=0.

  No return value.
*/
void cutcvpoly(int& N, double* x, double* y, double A, double B, double C, bool protect)
{
  int n=0, ns;
  while (n<N && A*x[n]+B*y[n]+C<=0) n++;
  if (n==N) //all points satisfies the constraint, do nothing
  {}
  else if (n>0) //points 0, 1, ..., n-1 satisfies the constraint, point n does not
  {
    ns=n;
    n++;
    while (n<N && A*x[n]+B*y[n]+C>0) n++;
    //points 0, ..., ns-1 and n, ..., N-1 satisfy the constraint,
    //points ns, ..., n-1 do not satisfy the constraint,
    //  ns>0, n>ns, however, it's possible that n-1=ns
    int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe
    double xs, ys, xe, ye;

    //find intersection of x:y[ns-1:ns] and A:B:C
    double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns];
    double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0
    if (z1==0) pts=0; //as point ns-1 IS point s
    else
    {
      pts=1, xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1);
    }

    //find intersection of x:y[n-1:n] and A:B:C
    x1=x[n-1], y1=y[n-1];
    if (n==N) x2=x[0], y2=y[0];
    else x2=x[n], y2=y[n];
    z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0
    if (z2==0) pte=0; //as point n is point e
    else
    {
      pte=1, xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1);
    }

    //the new polygon is formed by points 0, 1, ..., ns-1, (s), (e), n, ..., N-1
    memmove(&x[ns+pts+pte], &x[n], sizeof(double)*(N-n));
    memmove(&y[ns+pts+pte], &y[n], sizeof(double)*(N-n));
    if (pts) x[ns]=xs, y[ns]=ys;
    if (pte) x[ns+pts]=xe, y[ns+pts]=ye;
    N=ns+pts+pte+N-n;
  }
  else  //n=0, point 0 does not satisfy the constraint
  {
    n++;
    while (n<N && A*x[n]+B*y[n]+C>0) n++;
    if (n==N) //no point satisfies the constraint
    {
      if (!protect) N=0;
    }
    else
    {
      //points 0, 1, ..., n-1 do not satisfy the constraint, point n does
      ns=n;
      n++;
      while (n<N && A*x[n]+B*y[n]+C<=0) n++;
      //points 0, ..., ns-1 and n, ..., N-1 do not satisfy constraint,
      //points ns, ..., n-1 satisfy the constraint
      //  ns>0, n>ns, however ns can be equal to n-1
      int pts, pte; //indicates (by 0/1) whether or now a new point is to be added for xs/xe
      double xs, ys, xe, ye;

      //find intersection of x:y[ns-1:ns] and A:B:C
      double x1=x[ns-1], x2=x[ns], y1=y[ns-1], y2=y[ns];
      double z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1>0, z2<=0
      if (z2==0) pts=0; //as point ns IS point s
      else pts=1;
      xs=(x1*z2-x2*z1)/(z2-z1), ys=(y1*z2-y2*z1)/(z2-z1);

      //find intersection of x:y[n-1:n] and A:B:C
      x1=x[n-1], y1=y[n-1];
      if (n==N) x2=x[0], y2=y[0];
      else x2=x[n], y2=y[n];
      z1=A*x1+B*y1+C, z2=A*x2+B*y2+C; //z1<=0, z2>0
      if (z1==0) pte=0; //as point n-1 is point e
      else pte=1;
      xe=(x1*z2-x2*z1)/(z2-z1), ye=(y1*z2-y2*z1)/(z2-z1);

      //the new polygon is formed of points (s), ns, ..., n-1, (e)
      if (xs<=xe)
      {
        //the point sequence comes as (s), ns, ..., n-1, (e)
        memmove(&x[pts], &x[ns], sizeof(double)*(n-ns));
        memmove(&y[pts], &y[ns], sizeof(double)*(n-ns));
        if (pts) x[0]=xs, y[0]=ys;
        N=n-ns+pts+pte;
        if (pte) x[N-1]=xe, y[N-1]=ye;
      }
      else //xe<xs
      {
        //the sequence comes as e, (s), ns, ..., n-2 if pte=0 (notice point e<->n-1)
        //or e, (s), ns, ..., n-1 if pte=1
        if (pte==0)
        {
          memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns-1));
          memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns-1));
          if (pts) x[1]=xs, y[1]=ys;
        }
        else
        {
          memmove(&x[pts+1], &x[ns], sizeof(double)*(n-ns));
          memmove(&y[pts+1], &y[ns], sizeof(double)*(n-ns));
          if (pts) x[1]=xs, y[1]=ys;
        }
        x[0]=xe, y[0]=ye;
        N=n-ns+pts+pte;
      }
    }
  }
}//cutcvpoly

/*
  funciton CutR: update an F-G polygon with an new partial

  In: R: F-G polygon
      apind: partial index
      af, ef: partial frequency and error bound
      protect: specifies what to do if the new partial has no intersection with R, true=do noting,
        false=eliminate R
  Out: R: the updated F-G polygon

  No return value.
*/
void CutR(TPolygon* R, int apind, double af, double ef, bool protect)
{
  double k=apind*apind-1;
  double g1=(af-ef)/apind, g2=(af+ef)/apind;
  if (g1<0) g1=0;
  g1=g1*g1, g2=g2*g2;
  //apply constraints F+kG-g2<0 and -F-kG+g1<0
  cutcvpoly(R->N, R->X, R->Y, 1, k, -g2, protect);          //  update R
  cutcvpoly(R->N, R->X, R->Y, -1, -k, g1, protect);         //
}//CutR

/*
  function DeleteByHalf: reduces a list of stiffcandid objects by half, retaining those with higher s
  and delete the others, freeing their memory.

  In: cands[pcs]: the list of stiffcandid objects
  Out: cands[return value]: the list of retained ones

  Returns the size of the new list.
*/
int DeleteByHalf(stiffcandid** cands, int pcs)
{
  int newpcs=pcs/2;
  double* tmp=new double[newpcs];
  memset(tmp, 0, sizeof(double)*newpcs);
  for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs);
  int k=0;
  for (int j=0; j<pcs; j++)
  {
    if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j];
    else delete cands[j];
  }
  return k;
}//DeleteByHalf

/*
  function DeleteByHalf: reduces a list of TTempAtom objects by half, retaining those with higher s and
  delete the others, freeing their memory.

  In: cands[pcs]: the list of TTempAtom objects
  Out: cands[return value]: the list of retained ones

  Returns the size of the new list.
*/
int DeleteByHalf(TTempAtom** cands, int pcs)
{
  int newpcs=pcs/2;
  double* tmp=new double[newpcs];
  memset(tmp, 0, sizeof(double)*newpcs);
  for (int j=0; j<pcs; j++) InsertDec(cands[j]->s, tmp, newpcs);
  int k=0;
  for (int j=0; j<pcs; j++)
  {
    if (cands[j]->s>=tmp[newpcs-1]) cands[k++]=cands[j];
    else delete cands[j];
  }
  delete[] tmp;
  return k;
}//DeleteByHalf

/*
  function ds0: atom score 0 - energy

  In: a: atom amplitude

  Returns a^2, or s[0]+a^2 is s is not NULL, as atom score.
*/
double ds0(double a, void* s)
{
  if (s) return	*(double*)s+a*a;
  else return a*a;
}//ds0

/*
  function ds2: atom score 1 - cross-correlation coefficient with another HA, e.g. from previous frame

  In: a: atom amplitude
      params: pointer to dsprams1 structure supplying other inputs.
  Out: cross-correlation coefficient as atom score.

  Returns the cross-correlation coefficient.
*/
double ds1(double a, void* params)
{
  dsparams1* lparams=(dsparams1*)params;
  double hs=lparams->lastene+lparams->currentacce, hsaa=hs+a*a;
  if (lparams->p<=lparams->lastP) return (lparams->s*hs+a*lparams->lastvfp[lparams->p-1])/hsaa;
  else return (lparams->s*hs+a*a)/hsaa;
}//ds1

/*
  function ExBStiff: finds the minimal and maximal values of stiffness coefficient B given a F-G polygon

  In: F[N], G[N]: vertices of a F-G polygon
  Out: Bmin, Bmax: minimal and maximal values of the stiffness coefficient

  No reutrn value.
*/
void ExBStiff(double& Bmin, double& Bmax, int N, double* F, double* G)
{
  Bmax=G[0]/F[0];
  Bmin=Bmax;
  for (int i=1; i<N; i++)
  {
    double vi=G[i]/F[i];
    if (Bmin>vi) Bmin=vi;
    else if (Bmax<vi) Bmax=vi;
  }
}//ExBStiff

/*
  function ExFmStiff: finds the minimal and maximal frequecies of partial m given a F-G polygon

  In: F[N], G[N]: vertices of a F-G polygon
      m: partial index, fundamental=1
  Out: Fmin, Fmax: minimal and maximal frequencies of partial m

  No return value.
*/
void ExFmStiff(double& Fmin, double& Fmax, int m, int N, double* F, double* G)
{
  int k=m*m-1;
  double vmax=F[0]+k*G[0];
  double vmin=vmax;
  for (int i=1; i<N; i++)
  {
    double vi=F[i]+k*G[i];
    if (vmin>vi) vmin=vi;
    else if (vmax<vi) vmax=vi;
  }
  testnn(vmin); Fmin=m*sqrt(vmin);
  testnn(vmax); Fmax=m*sqrt(vmax);
}//ExFmStiff

/*
  function ExtendR: extends a F-G polygon on the f axis (to allow a larger pitch range)

  In: R: the F-G polygon
      delp1: amount of extension to the low end, in semitones
      delpe: amount of extension to the high end, in semitones
      minf: minimal fundamental frequency
  Out: R: the extended F-G polygon

  No return value.
*/
void ExtendR(TPolygon* R, double delp1, double delp2, double minf)
{
  double mdelf1=pow(2, -delp1/12), mdelf2=pow(2, delp2/12);
  int N=R->N;
  double *G=R->Y, *F=R->X, slope, prevslope, f, ftmp;
  if (N==0) {}
  else if (N==1)
  {
    R->N=2;
    slope=G[0]/F[0];
    testnn(F[0]); f=sqrt(F[0]);
    ftmp=f*mdelf2;
    F[1]=ftmp*ftmp;
    ftmp=f*mdelf1;
    if (ftmp<minf) ftmp=minf;
    F[0]=ftmp*ftmp;
    G[0]=F[0]*slope;
    G[1]=F[1]*slope;
  }
  else if (N==2)
  {
    prevslope=G[0]/F[0];
    slope=G[1]/F[1];
    if (fabs((slope-prevslope)/prevslope)<1e-10)
    {
      testnn(F[0]); ftmp=sqrt(F[0])*mdelf1; if (ftmp<0) ftmp=0; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
      testnn(F[1]); ftmp=sqrt(F[1])*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*slope;
    }
    else
    {
      if (prevslope>slope)
      {
        R->N=4;
        testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[3]=ftmp*ftmp; G[3]=F[3]*slope;
        ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope;
        testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
        ftmp=f*mdelf2; F[1]=ftmp*ftmp; G[1]=F[1]*prevslope;
        if (F[0]==0 && F[3]==0) R->N=3;
      }
      else
      {
        R->N=4;
        testnn(F[1]); f=sqrt(F[1]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[1]=ftmp*ftmp; G[1]=F[1]*slope;
        ftmp=f*mdelf2; F[2]=ftmp*ftmp; G[2]=F[2]*slope;
        testnn(F[0]); f=sqrt(F[0]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[0]=ftmp*ftmp; G[0]=F[0]*prevslope;
        ftmp=f*mdelf2; F[4]=ftmp*ftmp; G[4]=F[4]*prevslope;
        if (F[0]==0 && F[1]==0) {R->N=3; F[1]=F[2]; F[2]=F[3]; G[1]=G[2]; G[3]=G[3];}
      }
    }
  }
  else
  {
    //find the minimal and maximal angles of R
    //maximal slope is taken at Ms if Mt=1, or Ms-1 and Ms if Mt=0
    //minimal slope is taken at ms if mt=1, or ms-1 and ms if mt=0
    int Ms, ms, Mt=-1, mt=-1;
    double prevslope=G[0]/F[0], dslope;
    for (int n=1; n<N; n++)
    {
      double slope=G[n]/F[n];
      dslope=atan(slope)-atan(prevslope);
      if (Mt==-1)
      {
        if (dslope<-1e-10) Ms=n-1, Mt=1;
        else if (dslope<1e-10) Ms=n, Mt=0;
      }
      else if (mt==-1)
      {
        if (dslope>1e-10) ms=n-1, mt=1;
        else if (dslope>-1e-10) ms=n, mt=0;
      }
      prevslope=slope;
    }
    if (mt==-1)
    {
      slope=G[0]/F[0];
      dslope=atan(slope)-atan(prevslope);
      if (dslope>1e-10) ms=N-1, mt=1;
      else if (dslope>-1e-10) ms=0, mt=0;
    }
    R->N=N+mt+Mt;
    int newn=R->N-1;
    if (ms==0 && mt==1)
    {
      slope=G[0]/F[0];
      testnn(F[0]); ftmp=sqrt(F[0])*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
      mt=-1;
    }
    else if (ms==0)
      mt=-1;
    for (int n=N-1; n>=0; n--)
    {
      if (mt!=-1)
      {
        slope=G[n]/F[n];
        testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
        if (ms==n && mt==1)
        {
          ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
          mt=-1;
        }
        else if (ms==n) //mt=0
          mt=-1;
      }
      else if (Mt!=-1)
      {
        slope=G[n]/F[n];
        testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf2; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
        if (Ms==n && Mt==1)
        {
          ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
          Mt=-1;
        }
        else if (Ms==n)
          Mt=-1;
      }
      else
      {
        slope=G[n]/F[n];
        testnn(F[n]); f=sqrt(F[n]); ftmp=f*mdelf1; if (ftmp<minf) ftmp=minf; F[newn]=ftmp*ftmp; G[newn]=F[newn]*slope; newn--;
      }
    }
    //merge multiple vertices at the origin
    N=R->N;
    while (N>0 && F[N-1]==0) N--;
    R->N=N;
    int n=1;
    while (n<N && F[n]==0) n++;
    if (n!=1)
    {
      R->N=N-(n-1);
      memcpy(&F[1], &F[n], sizeof(double)*(N-n));
      memcpy(&G[1], &G[n], sizeof(double)*(N-n));
    }
  }
}//ExtendR

/*
  function FGFrom2: solves the following equation system given m[2], f[2], norm[2]. This is interpreted
  as searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied, where
  k[*]=m[*]^2-1.

    m[0](F+k[0]G)^0.5-f[0]     m[1](F+k[1]G)^0.5-f[1]
   ------------------------ = ------------------------ >0
            norm[0]                    norm[1]

   F=F0+lmd*dF
   G=G0+lmd*dG

  In: (F0, G0): search origin
      (dF, dG): search direction
      m[2], f[2], norm[2]: coefficients in the first equation
  Out: F[return value], G[return value]: solutions.

  Returns the number of solutions for (F, G).
*/
int FGFrom2(double* F, double* G, double F0, double dF, double G0, double dG, int* m, double* f, double* norm)
{
  double m1=m[0]/norm[0], m2=m[1]/norm[1],
         f1=f[0]/norm[0], f2=f[1]/norm[1],
         k1=m[0]*m[0]-1, k2=m[1]*m[1]-1;
  double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG),
         B=2*m1*(f2-f1),
         C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2;
  if (A==0)
  {
    double x=-C/B;
    if (x<0) return 0;
    else if (m1*x-f1<0) return 0;
    else
    {
      double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
      F[0]=F0+lmd*dF;
      G[0]=G0+lmd*dG;
      return 1;
    }
  }
  else
  {
    double delta=B*B-4*A*C;
    if (delta<0) return 0;
    else if (delta==0)
    {
      double x=-B/2/A;
      if (x<0) return 0;
      else if (m1*x-f1<0) return 0;
      else
      {
        double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[0]=F0+lmd*dF;
        G[0]=G0+lmd*dG;
        return 1;
      }
    }
    else
    {
      int roots=0;
      double x=(-B+sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else
      {
        double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[0]=F0+lmd*dF;
        G[0]=G0+lmd*dG;
        roots++;
      }
      x=(-B-sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else
      {
        double lmd=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[roots]=F0+lmd*dF;
        G[roots]=G0+lmd*dG;
        roots++;
      }
      return roots;
    }
  }
}//FGFrom2

/*
  function FGFrom2: solves the following equation system given m[2], f[2], k[2]. This is interpreted as
  searching from (F0, G0) down direction (dF, dG) until the first equation is satisfied. Functionally
  this is the same as the version using norm[2], with m[] anf f[] normalized by norm[] beforehand so
  that norm[] is no longer needed. However, k[2] cannot be computed from normalized m[2] so that it must
  be specified explicitly.

   m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] > 0

   F=F0+lmd*dF
   G=G0+lmd*dG

  In: (F0, G0): search origin
      (dF, dG): search direction
      m[2], f[2], k[2]: coefficients in the first equation
  Out: F[return value], G[return value]: solutions.

  Returns the number of solutions for (F, G).
*/
int FGFrom2(double* F, double* G, double* lmd, double F0, double dF, double G0, double dG, double* m, double* f, int* k)
{
  double m1=m[0], m2=m[1],
         f1=f[0], f2=f[1],
         k1=k[0], k2=k[1];
  double A=m1*m1-m2*m2*(dF+k2*dG)/(dF+k1*dG),
         B=2*m1*(f2-f1),
         C=(f2-f1)*(f2-f1)-(k1-k2)*(F0*dG-G0*dF)/(dF+k1*dG)*m2*m2;
  //x is obtained by solving Axx+Bx+C=0
  if (fabs(A)<1e-6) //A==0
  {
    double x=-C/B;
    if (x<0) return 0;
    else if (m1*x-f1<0) return 0;
    else
    {
      lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
      F[0]=F0+lmd[0]*dF;
      G[0]=G0+lmd[0]*dG;
      return 1;
    }
  }
  else
  {
    int roots=0;
    double delta=B*B-4*A*C;
    if (delta<0) return 0;
    else if (delta==0)
    {
      double x=-B/2/A;
      if (x<0) return 0;
      else if (m1*x-f1<0) return 0;
      else if ((m1*x-f1+f2)*m2<0) {}
      else
      {
        lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[0]=F0+lmd[0]*dF;
        G[0]=G0+lmd[0]*dG;
        roots++;
      }
      return roots;
    }
    else
    {
      int roots=0;
      double x=(-B+sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else if ((m1*x-f1+f2)*m2<0) {}
      else
      {
        lmd[0]=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[0]=F0+lmd[0]*dF;
        G[0]=G0+lmd[0]*dG;
        roots++;
      }
      x=(-B-sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else if ((m1*x-f1+f2)*m2<0) {}
      else
      {
        lmd[roots]=(x*x-(F0+k1*G0))/(dF+k1*dG);
        F[roots]=F0+lmd[roots]*dF;
        G[roots]=G0+lmd[roots]*dG;
        roots++;
      }
      return roots;
    }
  }
}//FGFrom2

/*
  function FGFrom3: solves the following equation system given m[3], f[3], norm[3].

    m[0](F+k[0]G)^0.5-f[0]       m[1](F+k[1]G)^0.5-f[1]       m[2](F+k[2]G)^0.5-f[2]
   ------------------------  =  ------------------------  =  ------------------------ > 0
           norm[0]                      norm[1]                      norm[2]

  In: m[3], f[3], norm[3]: coefficients in the above
  Out: F[return value], G[return value]: solutions.

  Returns the number of solutions for (F, G).
*/
int FGFrom3(double* F, double* G, int* m, double* f, double* norm)
{
  double m1=m[0]/norm[0], m2=m[1]/norm[1], m3=m[2]/norm[2],
         f1=f[0]/norm[0], f2=f[1]/norm[1], f3=f[2]/norm[2],
         k1=m[0]*m[0]-1, k2=m[1]*m[1]-1, k3=m[2]*m[2]-1;
  double h21=k2-k1, h31=k3-k1;             //h21 and h31
  double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13'
  double a=m2*m2*h13*h21-m3*m3*h12*h31;
  if (a==0)
  {
    double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1),
           b=h13*(f2-f1)-h12*(f3-f1);
    x=x/(2*m1*b);
    if (x<0) return 0; //x must the square root of something
    else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
    else
    {
      if (h21!=0)
      {
        G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21);
      }
      else
      {
        G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31);
      }
      F[0]=x*x-k1*G[0];
      return 1;
    }
  }
  else
  {
    double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a;
    a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a;
    double A=h12,
           B=2*m1*(f2-f1)-m2*m2*h21*a,
           C=(f2-f1)*(f2-f1)-m2*m2*h21*b;
    double delta=B*B-4*A*C;
    if (delta<0) return 0;
    else if (delta==0)
    {
      double x=-B/2/A;
      if (x<0) return 0; //x must the square root of something
      else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
      else
      {
        G[0]=a*x+b;
        F[0]=x*x-k1*G[0];
        return 1;
      }
    }
    else
    {
      int roots=0;
      double x=(-B+sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else
      {
        G[0]=a*x+b;
        F[0]=x*x-k1*G[0];
        roots++;
      }
      x=(-B-sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else
      {
        G[roots]=a*x+b;
        F[roots]=x*x-k1*G[roots];
        roots++;
      }
      return roots;
    }
  }
}//FGFrom3

/*
  function FGFrom3: solves the following equation system given m[3], f[3], k[3]. Functionally this is
  the same as the version using norm[3], with m[] anf f[] normalized by norm[] beforehand so that norm[]
  is no longer needed. However, k[3] cannot be computed from normalized m[3] so that it must be
  specified explicitly.

  m[0](F+k[0]G)^0.5-f[0] = m[1](F+k[1]G)^0.5-f[1] = m[2](F+k[2]G)^0.5-f[2] >0

  In: m[3], f[3], k[3]: coefficients in the above
  Out: F[return value], G[return value]: solutions.

  Returns the number of solutions for (F, G).
*/
int FGFrom3(double* F, double* G, double* e, double* m, double* f, int* k)
{
  double m1=m[0], m2=m[1], m3=m[2],
         f1=f[0], f2=f[1], f3=f[2],
         k1=k[0], k2=k[1], k3=k[2];
  double h21=k2-k1, h31=k3-k1;             //h21 and h31
  double h12=m1*m1-m2*m2, h13=m1*m1-m3*m3; //h12' and h13'
  double a=m2*m2*h13*h21-m3*m3*h12*h31;
  if (a==0)
  {
    //a==0 implies two the e's are conjugates
    double x=h12*(f3-f1)*(f3-f1)-h13*(f2-f1)*(f2-f1),
           b=h13*(f2-f1)-h12*(f3-f1);
    x=x/(2*m1*b);
    if (x<0) return 0; //x must the square root of something
    else if (m1*x-f1<-1e-6) return 0; //discarded because we are solving e1=e2=e3>0
    else if ((m1*x-f1+f2)*m2<0) return 0;
    else if ((m1*x-f1+f3)*m3<0) return 0;
    else
    {
      if (h21!=0)
      {
        G[0]=(h12*x*x+2*m1*(f2-f1)*x+(f2-f1)*(f2-f1))/(m2*m2*h21);
      }
      else
      {
        G[0]=(h13*x*x+2*m1*(f3-f1)*x+(f3-f1)*(f3-f1))/(m3*m3*h31);
      }
      F[0]=x*x-k1*G[0];
      double tmp=F[0]+G[0]*k[0]; testnn(tmp);
      e[0]=m1*sqrt(tmp)-f[0];
      return 1;
    }
  }
  else
  {
    double b=(h13*(f2-f1)*(f2-f1)-h12*(f3-f1)*(f3-f1))/a;
    a=2*m1*(h13*(f2-f1)-h12*(f3-f1))/a;
    double A=h12,
           B=2*m1*(f2-f1)-m2*m2*h21*a,
           C=(f2-f1)*(f2-f1)-m2*m2*h21*b;
    double delta=B*B-4*A*C;
    if (delta<0) return 0;
    else if (delta==0)
    {
      int roots=0;
      double x=-B/2/A;
      if (x<0) return 0; //x must the square root of something
      else if (m1*x-f1<0) return 0; //discarded because we are solving e1=e2=e3>0
      else if ((m1*x-f1+f2)*m2<0) return 0;
      else if ((m1*x-f1+f3)*m3<0) return 0;
      else
      {
        G[0]=a*x+b;
        F[0]=x*x-k1*G[0];
        double tmp=F[0]+G[0]*k[0]; testnn(tmp);
        e[0]=m1*sqrt(tmp)-f[0];
        roots++;
      }
      return roots;
    }
    else
    {
      int roots=0;
      double x=(-B+sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else if ((m1*x-f1+f2)*m2<0) {}
      else if ((m1*x-f1+f3)*m3<0) {}
      else
      {
        G[0]=a*x+b;
        F[0]=x*x-k1*G[0];
        double tmp=F[0]+G[0]*k1; testnn(tmp);
        e[0]=m1*sqrt(tmp)-f1;
        roots++;
      }
      x=(-B-sqrt(delta))/2/A;
      if (x<0) {}
      else if (m1*x-f1<0) {}
      else if ((m1*x-f1+f2)*m2<0) {}
      else if ((m1*x-f1+f3)*m3<0) {}
      else
      {
        G[roots]=a*x+b;
        F[roots]=x*x-k1*G[roots];
        double tmp=F[roots]+G[roots]*k1; testnn(tmp);
        e[roots]=m1*sqrt(tmp)-f1;
        roots++;
      }
      return roots;
    }
  }
}//FGFrom3

/*
  function FindNote: harmonic sinusoid tracking from a starting point in time-frequency plane forward
  and backward.

  In: _t, _f: start time and frequency
      frst, fren: tracking range, in frames
      Spec: spectrogram
      wid, offst: atom scale and hop size, must be consistent with Spec
      settings: note match settings
      brake: tracking termination threshold
  Out: M, Fr, partials[M][Fr]: HS partials

  Returns 0 if tracking is done by FindNoteF(), 1 if tracking is done by FindNoteConst()
*/
int FindNote(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings)
{
  double brake=0.02;
  if (settings.delp==0) return FindNoteConst(_t, _f, M, Fr, partials, frst, fren, wid, offst, Spec, settings, brake*5);

  atom* part=new atom[(fren-frst)*settings.maxp];
  double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype};

  int trst=floor((_t-wid/2)*1.0/offst+0.5);
  if (trst<0) trst=0;

  TPolygon* R=new TPolygon(1024); R->N=0;

  cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst);
  double f0=_f*wid;
  cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i];

  settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true;
  double B, starts=NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0, ds0);

  int k=0;
  if (starts>0)
  {
    int startp=NMResultToAtoms(settings.maxp, part, trst*offst+wid/2, wid, results);
    settings.pin0asanchor=false;
    double* startvfp=new double[startp]; memcpy(startvfp, vfpp, sizeof(double)*startp);
    k=startp;
    k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, fren, wid, offst, Spec, settings, brake);
    k+=FindNoteF(&part[k], starts, R, startp, startvfp, trst, frst-1, wid, offst, Spec, settings, brake);
    delete[] startvfp;
  }
  delete R; delete[] x;

  AtomsToPartials(k, part, M, Fr, partials, offst);
  delete[] part;

  //	ReEst1(M, frst, fren, partials, WV->Data16[channel], wid, offst);

  return 0;
}//Findnote*/

/*
  function FindNoteConst: constant-pitch harmonic sinusoid tracking

  In: _t, _f: start time and frequency
      frst, fren: tracking range, in frames
      Spec: spectrogram
      wid, offst: atom scale and hop size, must be consistent with Spec
      settings: note match settings
      brake: tracking termination threshold
  Out: M, Fr, partials[M][Fr]: HS partials

  Returns 1.
*/
int FindNoteConst(int _t, double _f, int& M, int& Fr, atom**& partials, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake)
{
  atom* part=new atom[(fren-frst)*settings.maxp];
  double fpp[1024]; double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768]; NMResults results={fpp, vfpp, pfpp, ptype};

  //trst: the frame index to start tracking
  int trst=floor((_t-wid/2)*1.0/offst+0.5), maxp=settings.maxp;
  if (trst<0) trst=0;

  //find a note candidate for the starting frame at _t
  TPolygon* R=new TPolygon(1024); R->N=0;
  cmplx<QSPEC_FORMAT>* spec=Spec->Spec(trst);
  double f0=_f*wid;
  cdouble *x=new cdouble[wid/2+1]; for (int i=0; i<=wid/2; i++) x[i]=spec[i];

  settings.forcepin0=true; settings.pcount=0; settings.pin0asanchor=true;
  double B, *starts=new double[maxp];
  NoteMatchStiff3(R, f0, B, 1, &x, wid, 0, &settings, &results, 0, 0);

  //read the energy vector of this candidate HA to starts[]. starts[] will contain the highest single-frame
  //energy of each partial during the tracking.
  int P=0; while(P<maxp && f0*(P+1)*sqrt(1+B*((P+1)*(P+1)-1))<wid/2) P++;
  for (int m=0; m<P; m++) starts[m]=results.vfp[m]*results.vfp[m];
  int atomcount=0;

  cdouble* ipw=new cdouble[maxp];

  //find the ends of tracking by constant-pitch extension of the starting HA until
  //at a frame at least half of the partials fall below starts[]*brake.

  //first find end of the note by forward extension from the frame at _t
  int fr1=trst;
  while (fr1<fren)
  {
    spec=Spec->Spec(fr1);
    for (int i=0; i<=wid/2; i++) x[i]=spec[i];
    double ls=0;
    for (int m=0; m<P; m++)
    {
      double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1));
      int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB);
      if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1;
      ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2);
      ls+=~ipw[m];
      if (starts[m]<~ipw[m]) starts[m]=~ipw[m];
    }
    double sumstart=0, sumstop=0;
    for (int m=0; m<P; m++)
    {
      if (~ipw[m]<starts[m]*brake) sumstop+=1;// sqrt(starts[m]);
      sumstart+=1; //sqrt(starts[m]);
    }
    double lstop=sumstop/sumstart;
    if (lstop>0.5) break;

    fr1++;
  }
  //note find the start of note by backward extension from the frame at _t
  int fr0=trst-1;
  while(fr0>frst)
  {
    spec=Spec->Spec(fr0);
    for (int i=0; i<=wid/2; i++) x[i]=spec[i];
    double ls=0;
    for (int m=0; m<P; m++)
    {
      double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1));
      int K1=floor(fm-settings.hB), K2=ceil(fm+settings.hB);
      if (K1<0) K1=0; if (K2>=wid/2) K2=wid/2-1;
      ipw[m]=IPWindowC(fm, x, wid, settings.M, settings.c, settings.iH2, K1, K2);
      ls+=~ipw[m];
      if (starts[m]<~ipw[m]) starts[m]=~ipw[m];
    }

    double sumstart=0, sumstop=0;
    for (int m=0; m<P; m++)
    {
      if (~ipw[m]<starts[m]*brake) sumstop+=1; //sqrt(starts[m]);
      sumstart+=1; //sqrt(starts[m]);
    }
    double lstop=sumstop/sumstart;
    if (lstop>0.5) {fr0++; break;}

    fr0--;
  }

  //now fr0 and fr1 are the first and last (excl.) frame indices
  Fr=fr1-fr0;
  cdouble* ipfr=new cdouble[Fr];
  double *as=new double[Fr*2], *phs=&as[Fr];
  cdouble** Allocate2(cdouble, Fr, wid/2+1, xx);
  for (int fr=0; fr<Fr; fr++)
  {
    spec=Spec->Spec(fr0+fr);
    for (int i=0; i<=wid/2; i++) xx[fr][i]=spec[i];
  }

  //reestimate partial frequencies, amplitudes and phase angles using all frames. In case of frequency estimation
  //failure, the original frequency is used and all atoms of that partial are typed "atInfered".
  for (int m=0; m<P; m++)
  {
    double fm=f0*(m+1)*sqrt(1+B*((m+1)*(m+1)-1)), dfm=(settings.delm<1)?settings.delm:1;
    double errf=LSESinusoidMP(fm, fm-dfm, fm+dfm, xx, Fr, wid, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6);
  //    LSESinusoidMPC(fm, fm-1, fm+1, xx, Fr, wid, offst, 3, settings.M, settings.c, settings.iH2, as, phs, 1e-6);
    atomtype type=(errf<0)?atInfered:atPeak;
    for (int fr=0; fr<Fr; fr++)
    {
      double tfr=(fr0+fr)*offst+wid/2;
      atom tmpatom={tfr, wid, fm/wid, as[fr], phs[fr], m+1, type, 0};
      part[atomcount++]=tmpatom;
    }
  }

  //Tag the atom at initial input (_t, _f) as anchor.
  AtomsToPartials(atomcount, part, M, Fr, partials, offst);
  partials[settings.pin0-1][trst-fr0].type=atAnchor;
  delete[] ipfr;
  delete[] ipw;
  delete[] part;
  delete R; delete[] x; delete[] as; DeAlloc2(xx);
  delete[] starts;
  return 1;
}//FindNoteConst

/*
  function FindNoteF: forward harmonic sinusoid tracking starting from a given harmonic atom until an
  endpoint is detected or a search boundary is reached.

  In: starts: harmonic atom score of the start HA
      startvfp[startp]: amplitudes of partials of start HA
      R: F-G polygon of the start HA
      frst, frst: frame index of the start and end frames of tracking.
      Spec: spectrogram
      wid, offst: atom scale and hop size, must be consistent with Spec
      settings: note match settings
      brake: tracking termination threshold.
  Out: part[return value]: list of atoms tracked.
       starts: maximal HA score of tracked frames. Its product with brake is used as the tracking threshold.

  Returns the number of atoms found.
*/
int FindNoteF(atom* part, double& starts, TPolygon* R, int startp, double* startvfp, int frst, int fren, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings, double brake)
{
  settings.forcepin0=false, settings.pin0=0;

  int k=0, maxp=settings.maxp;

  TPolygon* RR=new TPolygon(1024, R);
  int lastp=startp;
  double *lastvfp=new double[settings.maxp]; memcpy(lastvfp, startvfp, sizeof(double)*startp);

  cdouble *x=new cdouble[wid/2+1];
  double *fpp=new double[maxp*4], *vfpp=&fpp[maxp], *pfpp=&fpp[maxp*2]; atomtype* ptype=(atomtype*)&fpp[maxp*3];
  NMResults results={fpp, vfpp, pfpp, ptype};
  int step=(fren>frst)?1:(-1);
  for (int h=frst+step; (h-fren)*step<0; h+=step)
  {
    cmplx<QSPEC_FORMAT>* spec=Spec->Spec(h);
    for (int i=0; i<=wid/2; i++) x[i]=spec[i];
    double f0=0, B=0;
    double tmp=NoteMatchStiff3(RR, f0, B, 1, &x, wid, 0, &settings, &results, lastp, lastvfp);
    if (starts<tmp) starts=tmp;
    if (tmp<starts*brake) break;  //end condition: power drops by ??dB

    lastp=NMResultToAtoms(settings.maxp, &part[k], h*offst+wid/2, wid, results); k+=lastp;
    memcpy(lastvfp, vfpp, sizeof(double)*lastp);
  }
  delete RR; delete[] lastvfp; delete[] x; delete[] fpp;
  return k;
}//FindNoteF

/*
  function FindNoteFB: forward-backward harmonic sinusid tracking.

  In: frst, fren: index of start and end frames
      Rst, Ren: F-G polygons of start and end harmonic atoms
      vfpst[M], vfpen[M]: amplitude vectors of start and end harmonic atoms
      Spec: spectrogram
      wid, offst: atom scale and hop size, must be consistent with Spec
      settings: note match settings
  Out: partials[0:fren-frst][M], hosting tracked HS between frst and fren (inc).

  Returns 0 if successful, 1 if failure in pitch tracking stage (no feasible HA candidate at some frame).
  On start partials[0:fren-frst][M] shall be prepared to receive fren-frst+1 harmonic atoms
*/
int FindNoteFB(int frst, TPolygon* Rst, double* vfpst, int fren, TPolygon* Ren, double* vfpen, int M, atom** partials, int wid, int offst, TQuickSpectrogram* Spec, NMSettings settings)
{           //*
  if (frst>fren) return -1;
  int result=0;
  if (settings.maxp>M) settings.maxp=M;
  else M=settings.maxp;
  int Fr=fren-frst+1;
  double B, fpp[1024], delm=settings.delm, delp=settings.delp, iH2=settings.iH2;
  double *vfpp=&fpp[256], *pfpp=&fpp[512]; atomtype *ptype=(atomtype*)&fpp[768];
  int maxpitch=50;

  NMResults results={fpp, vfpp, pfpp, ptype};

  cmplx<QSPEC_FORMAT>* spec;

  cdouble *x=new cdouble[wid/2+1];
  TPolygon **Ra=new TPolygon*[maxpitch], **Rb=new TPolygon*[maxpitch], ***R; memset(Ra, 0, sizeof(TPolygon*)*maxpitch), memset(Rb, 0, sizeof(TPolygon*)*maxpitch);
  double *sca=new double[maxpitch], *scb=new double[maxpitch], **sc; sca[0]=scb[0]=0;
  double** Allocate2(double, maxpitch, M, vfpa); memcpy(vfpa[0], vfpst, sizeof(double)*M);
  double** Allocate2(double, maxpitch, M, vfpb); memcpy(vfpb[0], vfpen, sizeof(double)*M);
  double*** vfp;

  double** Allocate2(double, Fr, wid/2, fps);
  double** Allocate2(double, Fr, wid/2, vps);
  int* pc=new int[Fr];

  Ra[0]=new TPolygon(M*2+4, Rst);
  Rb[0]=new TPolygon(M*2+4, Ren);
  int pitchcounta=1, pitchcountb=1, pitchcount;

  //pitch tracking
  int** Allocate2(int, Fr, maxpitch, prev);
  int** Allocate2(int, Fr, maxpitch, pitches);
  int* pitchres=new int[Fr];
  int fra=frst, frb=fren, fr;
  while (fra<frb-1)
  {
    if (pitchcounta<pitchcountb){fra++; fr=fra; pitchcount=pitchcounta; vfp=&vfpa; sc=&sca; R=&Ra;}
    else {frb--; fr=frb; pitchcount=pitchcountb; vfp=&vfpb; sc=&scb, R=&Rb;}
    int fr_frst=fr-frst;

    int absfr=(partials[0][fr].t-wid/2)/offst;
    spec=Spec->Spec(absfr); for (int i=0; i<=wid/2; i++) x[i]=spec[i];
    pc[fr_frst]=QuickPeaks(fps[fr_frst], vps[fr_frst], wid, x, settings.M, settings.c, iH2, 0.0005);
    NoteMatchStiff3FB(pitchcount, *R, *vfp, *sc, pitches[fr_frst], prev[fr_frst], pc[fr_frst], fps[fr_frst], vps[fr_frst], x, wid, maxpitch, &settings);
    if (fr==fra) pitchcounta=pitchcount;
    else pitchcountb=pitchcount;
    if (pitchcount<=0)
    {result=1; goto cleanup;}
  }
  {
   //now fra=frb-1
    int fra_frst=fra-frst, frb_frst=frb-frst, maxpitcha=0, maxpitchb=0;
    double maxs=0;
    for (int i=0; i<pitchcounta; i++)
    {
      double f0a, f0b;
      if (fra==frst) f0a=partials[0][frst].f*wid;
      else
      {
        int peak0a=((__int16*)&pitches[fra_frst][i])[0], pin0a=((__int16*)&pitches[fra_frst][i])[1];
        f0a=fps[fra_frst][peak0a]/pin0a;
      }
      for (int j=0; j<pitchcountb; j++)
      {
        if (frb==fren) f0b=partials[0][fren].f*wid;
        else
        {
          int peak0b=((__int16*)&pitches[frb_frst][j])[0], pin0b=((__int16*)&pitches[frb_frst][j])[1];
          f0b=fps[frb_frst][peak0b]/pin0b;
        }
        double pow2delp=pow(2, delp/12);
        if (f0b<f0a*pow2delp+delm && f0b>f0a/pow2delp-delm)
        {
          double s=sca[i]+scb[j]+conta(M, vfpa[i], vfpb[j]);
          if (maxs<s) maxs=s, maxpitcha=i, maxpitchb=j;
        }
      }
    }
    //get pitch tracking result
    pitchres[fra_frst]=pitches[fra_frst][maxpitcha];
    for (int fr=fra; fr>frst; fr--)
    {
      maxpitcha=prev[fr-frst][maxpitcha];
      pitchres[fr-frst-1]=pitches[fr-frst-1][maxpitcha];
    }
    pitchres[frb_frst]=pitches[frb_frst][maxpitchb];
    for (int fr=frb; fr<fren; fr++)
    {
      maxpitchb=prev[fr-frst][maxpitchb];
      pitchres[fr-frst+1]=pitches[fr-frst+1][maxpitchb];
    }
  }
{
  double f0s[1024]; memset(f0s, 0, sizeof(double)*1024);
  for (int i=frst+1; i<fren; i++)
  {
    int pitch=pitchres[i-frst];
    int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1];
    f0s[i]=fps[i-frst][peak0]/pin0;
  }
  f0s[frst]=sqrt(Rst->X[0]), f0s[fren]=sqrt(Ren->X[0]);

  //partial tracking
  int lastp=0; while (lastp<M && vfpst[lastp]>0) lastp++;
  double* lastvfp=new double[M]; memcpy(lastvfp, vfpst, sizeof(double)*M);
  delete Ra[0]; Ra[0]=new TPolygon(M*2+4, Rst);
  for (int h=frst+1; h<fren; h++)
  {
    int absfr=(partials[0][h].t-wid/2)/offst;
    int h_frst=h-frst, pitch=pitchres[h_frst];
    int peak0=((__int16*)&pitch)[0], pin0=((__int16*)&pitch)[1];
    spec=Spec->Spec(absfr);
    double f0=fpp[0];//, B=Form11->BEdit->Text.ToDouble();
    //double deltaf=f0*(pow(2, settings.delp/12)-1);
    for (int i=0; i<=wid/2; i++) x[i]=spec[i];
    memset(fpp, 0, sizeof(double)*1024);
    //settings.epf0=deltaf,
    settings.forcepin0=true; settings.pin0=pin0; f0=fps[h_frst][peak0]; settings.pin0asanchor=false;
    if (NoteMatchStiff3(Ra[0], f0, B, pc[h_frst], fps[h_frst], vps[h_frst], 1, &x, wid, 0, &settings, &results, lastp, lastvfp)>0)
    {
      lastp=NMResultToPartials(M, h, partials, partials[0][h].t, wid, results);
      memcpy(lastvfp, vfpp, sizeof(double)*lastp);
    }
  }
  delete[] lastvfp;
}
cleanup:
  delete[] x;
  for (int i=0; i<pitchcounta; i++) delete Ra[i]; delete[] Ra;
  for (int i=0; i<pitchcountb; i++) delete Rb[i]; delete[] Rb;
  delete[] sca; delete[] scb;
  DeAlloc2(vfpa); DeAlloc2(vfpb);
  DeAlloc2(fps); DeAlloc2(vps);
  DeAlloc2(pitches); DeAlloc2(prev);
  delete[] pitchres; delete[] pc;
  return result;  //*/
}//FindNoteFB

/*
  function GetResultsSingleFr: used by NoteMatchStiff3 to obtain final note match results after harmonic
  grouping of peaks with rough frequency estimates, including a screening of found peaks based on shape
  and consistency with other peak frequencies, reestimating sinusoid parameters using LSE, estimating
  atoms without peaks by inferring their frequencies, and translating internal peak type to atom type.

  In: R0: F-G polygon of primitive (=rough estimates) partial frequencies
      x[N]: spectrum
      ps[P], fs[P]; partial indices and frequencies, may miss partials
      rsrs[P]: peak shape factors, used for evaluating peak validity
      psb[P]: peak type flags, related to atom::ptype.
      settings: note match settings
      numsam: number of partials to evaluate (=number of atoms to return)
  Out: results: note match results as a NMResult structure
       f0, B: fundamental frequency and stiffness coefficient
       Rret: F-G polygon of reestimated set of partial frequencies

  Return the total energy of the harmonic atom.
*/
double GetResultsSingleFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, cdouble* x, int N, int* psb, int numsam, NMSettings* settings, NMResults* results)
{
  Rret->N=0;
  double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB;
  int M=settings->M, maxp=settings->maxp;
  double *fp=results->fp; memset(fp, 0, sizeof(double)*maxp);

  double result=0;
  if (P>0)
  {
    double *vfp=results->vfp, *pfp=results->pfp;
    atomtype *ptype=results->ptype; //memset(ptype, 0, sizeof(int)*maxp);

      double *f1=new double[(maxp+1)*4], *f2=&f1[maxp+1], *ft=&f2[maxp+1], *fdep=&ft[maxp+1], tmpa, cF, cG;
      areaandcentroid(tmpa, cF, cG, R0->N, R0->X, R0->Y);
      for (int p=1; p<=maxp; p++) ExFmStiff(f1[p], f2[p], p, R0->N, R0->X, R0->Y), ft[p]=p*sqrt(cF+(p*p-1)*cG);
      //sort peaks by rsr and departure from model so that most reliable ones are found at the start
      double* values=new double[P]; int* indices=new int[P];
      for (int i=0; i<P; i++)
      {
        values[i]=1e200;
        int lp=ps[i]; double lf=fs[i];
        if (lf>=f1[lp] && lf<=f2[lp]) fdep[lp]=0;
        else if (lf<f1[lp]) fdep[lp]=f1[lp]-lf;
        else if (lf>f2[lp]) fdep[lp]=lf-f2[lp];
        double tmpv=(fdep[lp]>0.5)?(fdep[lp]-0.5)*2:0;
        tmpv+=rsrs?rsrs[i]:0;
        tmpv*=pow(lp, 0.2);
        InsertInc(tmpv, i, values, indices, i+1);
      }

    for (int i=0; i<P; i++)
    {
      int ind=indices[i];
      int lp=ps[ind]; //partial index
        //Get LSE estimation of atoms
        double lf=fs[ind], f1, f2, la, lph;//, lrsr=rsrs?rsrs[ind]:0
        if (lp==0 || lf==0) continue;
        ExFmStiff(f1, f2, lp, R0->N, R0->X, R0->Y);
        LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf);
          //lrsr=PeakShapeC(lf, 1, N, &x, 6, M, c, iH2);
        if (Rret->N>0) ExFmStiff(f1, f2, lp, Rret->N, Rret->X, Rret->Y);
        if (Rret->N==0 || (lf>=f1 && lf<=f2))
        {
          fp[lp-1]=lf;
          vfp[lp-1]=la;
          pfp[lp-1]=lph;
          if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) ptype[lp-1]=atAnchor; //0 for anchor points
          else ptype[lp-1]=atPeak; //1 for local maximum
          //update R using found partails with amplitude>1
          if (Rret->N==0) InitializeR(Rret, lp, lf, delm, maxB);
          else if (la>1) CutR(Rret, lp, lf, delm, true);
        }
    }
    //*
    for (int p=1; p<=maxp; p++)
    {
      if (fp[p-1]>0)
      {
        double f1, f2;
        ExFmStiff(f1, f2, p, Rret->N, Rret->X, Rret->Y);
        if (fp[p-1]<f1-0.3 || fp[p-1]>f2+0.3)
          fp[p-1]=0;
      }
    }// */


    //estimate f0 and B
    double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
    areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
    testnn(cF); f0=sqrt(cF); B=cG/cF;

    //Get LSE estimates for unfound partials
    for (int i=0; i<numsam; i++)
    {
      if (fp[i]==0) //no peak is found for this partial in lcand
      {
        int m=i+1;
        double tmp=cF+(m*m-1)*cG; testnn(tmp);
        double lf=m*sqrt(tmp);
        if (lf<N/2.1)
        {
          fp[i]=lf;
          ptype[i]=atInfered; //2 for non peak
          cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB);
          vfp[i]=sqrt(r.x*r.x+r.y*r.y);
          pfp[i]=Atan2(r.y, r.x);//*/
        }
      }
    }
    if (f0>0) for (int i=0; i<numsam; i++) if (fp[i]>0) result+=vfp[i]*vfp[i];

      delete[] f1; delete[] values; delete[] indices;
  }
  return result;
}//GetResultsSingleFr

/*
  function GetResultsMultiFr: the constant-pitch multi-frame version of GetResultsSingleFr.

  In: x[Fr][N]: spectrogram
      ps[P], fs[P]; partial indices and frequencies, may miss partials
      psb[P]: peak type flags, related to atom::ptype.
      settings: note match settings
      forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
      numsam: number of partials to evaluate (=number of atoms to return)
  Out: results[Fr]: note match results as Fr NMResult structures
       f0, B: fundamental frequency and stiffness coefficient
       Rret: F-G polygon of reestimated set of partial frequencies

  Return the total energy of the harmonic sinusoid.
*/
double GetResultsMultiFr(double& f0, double& B, TPolygon* Rret, TPolygon* R0, int P, int* ps, double* fs, double* rsrs, int Fr, cdouble** x, int N, int offst, int* psb, int numsam, NMSettings* settings, NMResults* results, int forceinputlocalfr)
{
  Rret->N=0;
  double delm=settings->delm, *c=settings->c, iH2=settings->iH2, epf=settings->epf, maxB=settings->maxB, hB=settings->hB;// *pinf=settings->pinf;

  int M=settings->M, maxp=settings->maxp, pincount=settings->pcount, *pin=settings->pin, *pinfr=settings->pinfr;
//	double *fp=results->fp, *vfp=results->vfp, *pfp=results->pfp;
//	int *ptype=results->ptype;
  for (int fr=0; fr<Fr; fr++)	memset(results[fr].fp, 0, sizeof(double)*maxp);
  int* pclass=new int[maxp+1]; memset(pclass, 0, sizeof(int)*(maxp+1));
  for (int i=0; i<pincount; i++) if (pinfr[i]>=0) pclass[pin[i]]=1;
  if (forceinputlocalfr>=0) pclass[settings->pin0]=1;
  double result=0;
  if (P>0)
  {
    double tmpa, cF, cG;
    //found atoms
    double *as=new double[Fr*2], *phs=&as[Fr];
    for (int i=P-1; i>=0; i--)
    {
      int lp=ps[i];
      double lf=fs[i];
      if (lp==0 || lf==0) continue;

      if (!pclass[lp])
      {
        //Get LSE estimation of atoms
        double startlf=lf;
        LSESinusoidMP(lf, lf-1, lf+1, x, Fr, N, 3, M, c, iH2, as, phs, epf);
        //LSESinusoidMPC(lf, lf-1, lf+1, x, Fr, N, offst, 3, M, c, iH2, as, phs, epf);
        if (fabs(lf-startlf)>0.6)
        {
          lf=startlf;
          for (int fr=0; fr<Fr; fr++)
          {
            cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB);
            as[fr]=abs(r);
            phs[fr]=arg(r);
          }
        }
      }
      else //frequencies of local anchor atoms are not re-estimated
      {
        for (int fr=0; fr<Fr; fr++)
        {
          cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, lf-settings->hB, lf+settings->hB);
          as[fr]=abs(r);
          phs[fr]=arg(r);
        }
      }

        //LSESinusoid(lf, f1-delm, f2+delm, x, N, 3, M, c, iH2, la, lph, epf);
        //fp[lp-1]=lf; vfp[lp-1]=la; pfp[lp-1]=lph;
      for (int fr=0; fr<Fr; fr++) results[fr].fp[lp-1]=lf, results[fr].vfp[lp-1]=as[fr], results[fr].pfp[lp-1]=phs[fr];
      if (psb[lp]==1 || (psb[lp]==2 && settings->pin0asanchor)) for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atAnchor; //0 for anchor points
      else for (int fr=0; fr<Fr; fr++) results[fr].ptype[lp-1]=atPeak; //1 for local maximum
      //update R using found partails with amplitude>1
      if (Rret->N==0)
      {
      //temporary treatment: +0.5 should be +rsr or something similar
        InitializeR(Rret, lp, lf, delm+0.5, maxB);
        areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
      }
      else //if (la>1)
      {
        CutR(Rret, lp, lf, delm+0.5, true);
        areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
      }
    }
    //estimate f0 and B
    double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
    areaandcentroid(tmpa, cF, cG, Rret->N, Rret->X, Rret->Y); //minimaxstiff(cF, cG, P, ps, fs, norm, R->N, R->X, R->Y);
    testnn(cF); f0=sqrt(cF); B=cG/cF;

    //Get LSE estimates for unfound partials
    for (int i=0; i<numsam; i++)
    {
      if (results[0].fp[i]==0) //no peak is found for this partial in lcand
      {
        int m=i+1;
        double tmp=cF+(m*m-1)*cG; testnn(tmp);
        double lf=m*sqrt(tmp);
        if (lf<N/2.1)
        {
          for (int fr=0; fr<Fr; fr++)
          {
            results[fr].fp[i]=lf, results[fr].ptype[i]=atInfered; //2 for non peak
            int k1=ceil(lf-hB); if (k1<0) k1=0;
            int k2=floor(lf+hB); if (k2>=N/2) k2=N/2-1;
            cdouble r=IPWindowC(lf, x[fr], N, M, c, iH2, k1, k2);
            results[fr].vfp[i]=abs(r);
            results[fr].pfp[i]=arg(r);
          }
        }
      }
    }
    delete[] as;
    if (f0>0) for (int fr=0; fr<Fr; fr++) for (int i=0; i<numsam; i++) if (results[fr].fp[i]>0) result+=results[fr].vfp[i]*results[fr].vfp[i];
    result/=Fr;
  }
  delete[] pclass;
  return result;
}//GetResultsMultiFr

/*
  function InitailizeR: initializes a F-G polygon with a fundamental frequency range and stiffness coefficient bound

  In: af, ef: centre and half width of the fundamental frequency range
      maxB: maximal value of stiffness coefficient (the minimal is set to 0)
  Out: R: the initialized F-G polygon.

  No reutrn value.
*/
void InitializeR(TPolygon* R, double af, double ef, double maxB)
{
  R->N=4;
  double *X=R->X, *Y=R->Y;
  double g1=af-ef, g2=af+ef;
  if (g1<0) g1=0;
  g1=g1*g1, g2=g2*g2;
  X[0]=X[3]=g1, X[1]=X[2]=g2;
  Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0;
}//InitializeR

/*
  function InitialzeR: initializes a F-G polygon with a frequency range for a given partial and
  stiffness coefficient bound

  In: apind: partial index
      af, ef: centre and half width of the frequency range of the apind-th partial
      maxB; maximal value of stiffness coefficient (the minimal is set to 0)
  Out: R: the initialized F-G polygon.

  No return value.
*/
void InitializeR(TPolygon* R, int apind, double af, double ef, double maxB)
{
  R->N=4;
  double *X=R->X, *Y=R->Y;
  double k=apind*apind-1;
  double g1=(af-ef)/apind, g2=(af+ef)/apind;
  if (g1<0) g1=0;
  g1=g1*g1, g2=g2*g2;
  double kb1=1+k*maxB;
  X[0]=g1/kb1, X[1]=g2/kb1, X[2]=g2, X[3]=g1;
  Y[0]=X[0]*maxB, Y[1]=X[1]*maxB, Y[2]=Y[3]=0;
}//InitializeR

/*
  function maximalminimum: finds the point within a polygon that maximizes its minimal distance to the
  sides.

  In: sx[N], sy[N]: x- and y-coordinates of vertices of a polygon
  Out: (x, y): point within the polygon with maximal minimal distance to the sides

  Returns the maximial minimal distance. A circle centred at (x, y) with the return value as the radius
  is the maximum inscribed circle of the polygon.
*/
double maximalminimum(double& x, double& y, int N, double* sx, double* sy)
{
  //at the beginning let (x, y) be (sx[0], sy[0]),  then the mininum distance d is 0.
  double dm=0;
  x=sx[0], y=sy[0];
  double *A=new double[N*3];
  double *B=&A[N], *C=&A[N*2];
  //calcualte equations of all sides A[k]x+B[k]y+C[k]=0, with signs adjusted so that for
  //  any (x, y) within the polygon, A[k]x+B[k]y+C[k]>0. A[k]^2+B[k]^2=1.
  for (int n=0; n<N; n++)
  {
    double x1=sx[n], y1=sy[n], x2, y2, AA, BB, CC;
    if (n+1!=N) x2=sx[n+1], y2=sy[n+1];
    else x2=sx[0], y2=sy[0];
    double dx=x1-x2, dy=y1-y2;
    double ds=sqrt(dx*dx+dy*dy);
    AA=dy/ds, BB=-dx/ds;
    CC=-AA*x1-BB*y1;
    //adjust signs
    if (n+2<N) x2=sx[n+2], y2=sy[n+2];
    else x2=sx[n+2-N], y2=sy[n+2-N];
    if (AA*x2+BB*y2+CC<0) A[n]=-AA, B[n]=-BB, C[n]=-CC;
    else A[n]=AA, B[n]=BB, C[n]=CC;
  }

  //during the whole process (x, y) is always equal-distance to at least two sides,
  //  namely l1--(l1+1) and l2--(l2+1). there equations are A1x+B1y+C1=0 and A2x+B2y+C2=0,
  //  where A^2+B^2=1.
  int l1=0, l2=N-1;

  double a, b;
    b=A[l1]-A[l2], a=B[l2]-B[l1];
    double ds=sqrt(a*a+b*b);
    a/=ds, b/=ds;
    //the line (x+at, y+bt) passes (x, y), and points on this line are equal-distance to l1 and l2.
    //along this line at point (x, y), we have dx=ads, dy=bds
    //now find the signs so that dm increases as ds>0
    double ddmds=A[l1]*a+B[l1]*b;
    if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;

  while (true)
  {
    //now the vector starting from (x, y) pointing in (a, b) is equi-distance-to-l1-and-l2 and
    //  dm-increasing. actually at s from (x,y), d=dm+ddmds*s.

    //it is now guaranteed that the distance of (x, y) to l1 (=l2) is smaller than to any other
    //  sides. along direction (A, B) the distance of (x, y) to l1 (=l2) is increasing and
    //  the distance to at least one other sides is increasing, so that at some value of s the
    //  distances equal. we find the smallest s>0 where this happens.
    int l3=-1;
    double s=-1;
    for (int n=0; n<N; n++)
    {
      if (n==l1 || n==l2) continue;
      //distance of (x,y) to the side
      double ldm=A[n]*x+B[n]*y+C[n]; //ldm>dm
      double dldmds=A[n]*a+B[n]*b;
      //so ld=ldm+lddmds*s, the equality is dm+ddmds*s=ldm+lddmds*s
      if (ddmds-dldmds>0)
      {
        ds=(ldm-dm)/(ddmds-dldmds);
        if (l3==-1) l3=n, s=ds;
        else if (ds<s) l3=n, s=ds;
      }
    }
    if (ddmds==0) s/=2;
    x=x+a*s, y=y+b*s;
    dm=A[l1]*x+B[l1]*y+C[l1];
//    Form1->Canvas->Ellipse(x-dm, y-dm, x+dm, y+dm);
    //(x, y) is equal-distance to l1, l2 and l3
    //try use l3 to substitute l2
    b=A[l1]-A[l3], a=B[l3]-B[l1];
    ds=sqrt(a*a+b*b);
    a/=ds, b/=ds;
    ddmds=A[l1]*a+B[l1]*b;
    if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;
    if (ddmds==0 || A[l2]*a+B[l2]*b>0)
    {
      l2=l3;
    }
    else  //l1<-l3 fails, then try use l3 to substitute l2
    {
      b=A[l3]-A[l2], a=B[l2]-B[l3];
      ds=sqrt(a*a+b*b);
      a/=ds, b/=ds;
      ddmds=A[l3]*a+B[l3]*b;
      if (ddmds<0) a=-a, b=-b, ddmds=-ddmds;
      if (ddmds==0 || A[l1]*a+B[l1]*b>0)
      {
        l1=l3;
      }
      else break;
    }
  }

  delete[] A;
  return dm;
}//maximalminimum

/*
  function minimaxstiff: finds the point in polygon (N; F, G) that minimizes the maximal value of the
  function

        | m[l]sqrt(F+(m[l]^2-1)*G)-f[l] |
  e_l = | ----------------------------- | regarding l=0, ..., L-1
        |            norm[l]            |

  In: _m[L], _f[L], norm[L]: coefficients in the above
      F[N], G[N]: vertices of a F-G polygon
  Out: (F1, G1): the mini-maximum.

  Returns the minimized maximum value.

  Further reading: Wen X,  "Estimating model parameters", Ch.3.2.6 from "Harmonic sinusoid modeling of
  tonal music events", PhD thesis, University of London, 2007.
*/
double minimaxstiff(double& F1, double& G1, int L, int* _m, double* _f, double* norm, int N, double* F, double* G)
{
  if (L==0 || N<=2)
  {
    F1=F[0], G1=G[0];
    return 0;
  }
  //normalizing
  double* m=(double*)malloc(sizeof(double)*L*6);//new double[L*6];
  double* f=&m[L*2];
  int* k=(int*)&m[L*4];
  for (int l=0; l<L; l++)
  {
    k[2*l]=_m[l]*_m[l]-1;
    k[2*l+1]=k[2*l];
    m[2*l]=_m[l]/norm[l];
    m[2*l+1]=-m[2*l];
    f[2*l]=_f[l]/norm[l];
    f[2*l+1]=-f[2*l];
  }
  //From this point on the L distance functions with absolute value is replace by 2L distance functions
  L*=2;
  double* vmnl=new double[N*2];
  int* mnl=(int*)&vmnl[N];
start:
  //Initialize (F0, G0) to be the polygon vertex that has the minimal max_l(e_l)
  //  maxn: the vertex index
  //  maxl: the l that maximizes e_l at that vertex
  //  maxsg: the sign of e_l before taking the abs. value
  int nc=-1, nd;
  int l1=-1, l2=0, l3=0;
  double vmax=0;

  for (int n=0; n<N; n++)
  {
    int lmax=-1;
    double lvmax=0;
    for (int l=0; l<L; l++)
    {
      double tmp=F[n]+k[l]*G[n]; testnn(tmp);
      double e=m[l]*sqrt(tmp)-f[l];
      if (e>lvmax) lvmax=e, lmax=l;
    }
    mnl[n]=lmax, vmnl[n]=lvmax;
    if (n==0)
    {
      vmax=lvmax, nc=n, l1=lmax;
    }
    else
    {
      if (lvmax<vmax) vmax=lvmax, nc=n, l1=lmax;
    }
  }
  double F0=F[nc], G0=G[nc];


//    start searching the the minimal maximum from (F0, G0)
//
//    Each searching step starts from (F0, G0), ends at (F1, G1)
//
//    Starting conditions of one step:
//
//      (F0, G0) can be
//        (1)inside polygon R;
//        (2)on one side of R (nc:(nc+1) gives the side)
//        (3)a vertex of R  (nc being the vertex index)
//
//      The maximum at (F0, G0) can be
//        (1)vmax=e1=e2=e3>...; (l1, l2, l3)
//        (2)vmax=e1=e2>...; (l1, l2)
//        (3)vmax=e1>....  (l1)
//
//      More complication arise if we have more than 3 equal maxima, i.e.
//        vmax=e1=e2=e3=e4>=.... CURRENTLY WE ASSUME THIS NEVER HAPPENS.
//
//      These are also the ending conditions of one step.
//
//    There are  types of basic searching steps, i.e.
//
//      (1) e1=e2 search: starting with e1=e2, search down the decreasing direction
//          of e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1)
//          =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about
//          to go out of R.
//          Arguments: l1, l2, (F0, G0, vmax)
//      (2) e1!=e2 search: starting with e1=e2 and (F0, G0) being on one side, search
//          down the side in the decreasing direction of both e1 and e1 until at point
//          (F1, G1) there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or
//          at a the search reaches a vertex of R.
//          Arguments: l1, l2, dF, dG, (F0, G0, vmax)
//      (3) e1 free search: starting with e1 being the only maximum, search down the decreasing
//          direction of e1 until at point (F1, G1) there is another l2 so that
//          e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about
//          to go out of R.
//          Arguments: l1, dF, dG, (F0, G0, vmax)
//      (4) e1 side search: starting with e1 being the only maximum, search down the
//          a side of R in the decreasing direction of e1 until at point (F1, G1) there
//          is another l2 so that e1=e2, or until point (F1, G1) the search reaches
//          a vertex.
//          Arguments: l1, destimation vertex nd, (F0, G0, vmax)
//
//    At the beginning of each searching step we check the conditions to see which
//    basic step to perform, and provide the starting conditions. At the very start of
//    the search, (F0, G0) is a vertex of R, e_l1 being the maximum at this point.
//

  int condpos=3; //inside, on side, vertex
  int condmax=3; //triple max, duo max, solo max
  int searchmode;

  bool minimax=false; //set to true when the minimal maximum is met

  int iter=L*2;
  while (!minimax && iter>0)
  {
    iter--;
    double m1=m[l1], m2=m[l2], m3=m[l3], k1=k[l1], k2=k[l2], k3=k[l3];
    double tmp, tmp1, tmp2, tmp3;

    switch (condmax)
    {
      case 1:
        tmp=F0+k3*G0; testnn(tmp);
        tmp3=sqrt(tmp);
      case 2:
        tmp=F0+k2*G0; testnn(tmp)
        tmp2=sqrt(tmp);
      case 3:
        tmp=F0+k1*G0; testnn(tmp);
        tmp1=sqrt(tmp);
    }
    double dF, dG;
    int n0=(nc==0)?(N-1):(nc-1), n1=(nc==N-1)?0:(nc+1);
    double x0, y0, x1, y1;
    if (n0>=0) x0=F[nc]-F[n0], y0=G[nc]-G[n0];
    if (nc>=0) x1=F[n1]-F[nc], y1=G[n1]-G[nc];

    if (condpos==1) //(F0, G0) being inside polygon
    {
      if (condmax==1) //e1=e2=e3 being the maximum
      {
        //vmax holds the maximum
        //l1, l2, l3

        //now choose a searching direction, either e1=e2 or e1=e3 or e2=e3.
        //  choose e1=e2 if (e3-e1) decreases along the decreasing direction of e1=e2
        //  choose e1=e3 if (e2-e1) decreases along the decreasing direction of e1=e3
        //  choose e2=e3 if (e1-e2) decreases along the decreasing directino of e2=e3
        //  if no condition is satisfied, then (F0, G0) is the minimal maximum.
        //calculate the decreasing direction of e1=e3 as (dF, dG)
        double den=m1*m3*(k1-k3)/2;
        dF=-(k1*m1*tmp3-k3*m3*tmp1)/den,
        dG=(m1*tmp3-m3*tmp1)/den;
        //the negative gradient of e2-e1 is calculated as (gF, gG)
        double gF=-(m2/tmp2-m1/tmp1)/2,
               gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
        if (dF*gF+dG*gG>0) //so that e2-e1 decreases in the decreasing direction of e1=e3
        {
          l2=l3;
          searchmode=1;
        }
        else
        {
          //calculate the decreasing direction of e2=e3 as (dF, dG)
          den=m2*m3*(k2-k3)/2;
          dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
          dG=(m2*tmp3-m3*tmp2)/den;
          //calculate the negative gradient of e1-e2 as (gF, gG)
          gF=-gF, gG=-gG;
          if (dF*gF+dG*gG>0) //so that e1-e2 decreases in the decreasing direction of e2=e3
          {
            l1=l3;
            searchmode=1;
          }
          else
          {
            //calculate the decreasing direction of e1=e2 as (dF, dG)
            den=m1*m2*(k1-k2)/2;
            dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
            dG=(m1*tmp2-m2*tmp1)/den;
            //calculate the negative gradient of (e3-e1) as (gF, gG)
            gF=-(m3/tmp3-m1/tmp1)/2,
            gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
            if (dF*gF+dG*gG>0) //so that e3-e1 decreases in the decreasing direction of e1=e2
            {
              searchmode=1;
            }
            else
            {
              F1=F0, G1=G0;
              searchmode=0; //no search
              minimax=true; //quit loop
            }
          }
        }
      } //
      else if (condmax==2) //e1=e2 being the maximum
      {
        //vmax holds the maximum
        //l1, l2
        searchmode=1;
      }
      else if (condmax==3) //e1 being the maximum
      {
        //the negative gradient of e1
        dF=-0.5*m1/tmp1;
        dG=k1*dF;
        searchmode=3;
      }
    }
    else if (condpos==2) //(F0, G0) being on side nc:(nc+1)
    {
      //the vector nc->(nc+1) as (x1, y1)
      if (condmax==1) //e1=e2=e3 being the maximum
      {
        //This case rarely happens.

        //First see if a e1=e2 search is possible
        //calculate the decreasing direction of e1=e3 as (dF, dG)
        double den=m1*m3*(k1-k3)/2;
        double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den;
        //the negative gradient of e2-e1 is calculated as (gF, gG)
        double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
        if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3
        {                  //~~~~~~~~~~~~~and this direction points inward
          l2=l3;
          searchmode=1;
        }
        else
        {
          //calculate the decreasing direction of e2=e3 as (dF, dG)
          den=m2*m3*(k2-k3)/2;
          dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
          dG=(m2*tmp3-m3*tmp2)/den;
          //calculate the negative gradient of e1-e2 as (gF, gG)
          gF=-gF, gG=-gG;
          if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3
          {
            l1=l3;
            searchmode=1;
          }
          else
          {
            //calculate the decreasing direction of e1=e2 as (dF, dG)
            den=m1*m2*(k1-k2)/2;
            dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
            dG=(m1*tmp2-m2*tmp1)/den;
            //calculate the negative gradient of (e3-e1) as (gF, gG)
            gF=-(m3/tmp3-m1/tmp1)/2,
            gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
            if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2
            {
              searchmode=1;
            }
            else
            {
              //see the possibility of a e1!=e2 search
              //calcualte the dot product of the gradients and (x1, y1)
              double d1=m1/2/tmp1*(x1+k1*y1),
                     d2=m2/2/tmp2*(x1+k2*y1),
                     d3=m3/2/tmp3*(x1+k3*y1);
              //we can prove that if there is a direction pointing inward R in which
              //  e1, e2, e2 decrease, and another direction pointing outside R in
              //  which e1, e2, e3 decrease, then on one direction along the side
              //  all the three decrease. (Even more, this direction must be inside
              //  the <180 angle formed by the two directions.)
              //
              //  On the contrary, if there is a direction
              //  in which all the three decrease, with two equal, it has to point
              //  outward for the program to get here. Then if along neither direction
              //  of side R can the three all descend, then there doesn't exist any
              //  direction inward R in which the three descend. In that case we
              //  have a minimal maximum at (F0, G0).
              if (d1*d2<=0 || d1*d3<=0 || d2*d3<=0) //so that the three don't decrease in the same direction
              {
                F1=F0, G1=G0;
                searchmode=0; //no search
                minimax=true; //quit loop
              }
              else
              {
                if (d1>0) //so that d2>0, d3>0, all three decrease in the direction -(x1, y1) towards nc
                {
                  if (d1>d2 && d1>d3) //e1 decreases fastest
                  {
                    l1=l3; //keep e2, e3
                  }
                  else if (d2>=d1 && d2>d3) //e2 decreases fastest
                  {
                    l2=l3;  //keep e1, e3
                  }
                  else //d3>=d1 && d3>=d2, e3 decreases fastest
                  {
                    //keep e1, e2
                  }
                  nd=nc;
                }
                else //d1<0, d2<0, d3<0, all three decrease in the direction (x1, y1)
                {
                  if (d1<d2 && d1<d3) //e1 decreases fastest
                  {
                    l1=l3;
                  }
                  else if (d2<=d1 && d2<d3) //e2 decreases fastest
                  {
                    l2=l3;
                  }
                  else //d3<=d1 && d3<=d2
                  {
                    //keep e1, e2
                  }
                  nd=n1;
                }
                searchmode=2;
              }
            }
          }
        }
      }
      else if (condmax==2)  //e1=e2 being the maximum
      {
        //first see if the decreasing direction of e1=e2 points to the inside
        //calculate the decreasing direction of e1=e2 as (dF, dG)
        double den=m1*m2*(k1-k2)/2;
        dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
        dG=(m1*tmp2-m2*tmp1)/den;

        if (x1*dG-y1*dF<0) //so that (dF, dG) points inward R
        {
          searchmode=1;
        }
        else
        {
          //calcualte the dot product of the gradients and (x1, y1)
          double d1=m1/2/tmp1*(x1+k1*y1),
                 d2=m2/2/tmp2*(x1+k2*y1);
          if (d1*d2<=0) //so that along the side e1, e2 descend in opposite directions
          {
            F1=F0, G1=G0;
            searchmode=0;
            minimax=true;
          }
          else
          {
            if (d1>0) //so that both decrease in direction -(x1, y1)
              nd=nc;
            else
              nd=n1;
            searchmode=2;
          }
        }
      }
      else if (condmax==3)  //e1 being the maximum
      {
        //calculate the negative gradient of e1 as (dF, dG)
        dF=-0.5*m1/tmp1;
        dG=k1*dF;

        if (x1*dG-y1*dF<0) //so the gradient points inward R
          searchmode=3;
        else
        {
          //calculate the dot product of the gradient and (x1, y1)
          double d1=m1/2/tmp1*(x1+k1*y1);
          if (d1>0) //so that e1 decreases in direction -(x1, y1)
          {
            nd=nc;
            searchmode=4;
          }
          else if (d1<0)
          {
            nd=n1;
            searchmode=4;
          }
          else //so that e1 does not change along side nc:(nc+1)
          {
            F1=F0, G1=G0;
            searchmode=0;
            minimax=true;
          }
        }
      }
    }
    else //condpos==3, (F0, G0) being vertex nc
    {
      //the vector nc->(nc+1) as (x1, y1)
      //the vector (nc-1)->nc as (x0, y0)

      if (condmax==1) //e1=e2=e3 being the maximum
      {
        //This case rarely happens.

        //First see if a e1=e2 search is possible
        //calculate the decreasing direction of e1=e3 as (dF, dG)
        double den=m1*m3*(k1-k3)/2;
        double dF=-(k1*m1*tmp3-k3*m3*tmp1)/den, dG=(m1*tmp3-m3*tmp1)/den;
        //the negative gradient of e2-e1 is calculated as (gF, gG)
        double gF=-(m2/tmp2-m1/tmp1)/2, gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
        if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e2-e1 decreases in the decreasing direction of e1=e3
        {                  //~~~~~~~~~~~~~and this direction points inward
          l2=l3;
          searchmode=1;
        }
        else
        {
          //calculate the decreasing direction of e2=e3 as (dF, dG)
          den=m2*m3*(k2-k3)/2;
          dF=-(k2*m2*tmp3-k3*m3*tmp2)/den,
          dG=(m2*tmp3-m3*tmp2)/den;
          //calculate the negative gradient of e1-e2 as (gF, gG)
          gF=-gF, gG=-gG;
          if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e1-e2 decreases in the decreasing direction of e2=e3
          {
            l1=l3;
            searchmode=1;
          }
          else
          {
            //calculate the decreasing direction of e1=e2 as (dF, dG)
            den=m1*m2*(k1-k2)/2;
            dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
            dG=(m1*tmp2-m2*tmp1)/den;
            //calculate the negative gradient of (e3-e1) as (gF, gG)
            gF=-(m3/tmp3-m1/tmp1)/2,
            gG=-(k3*m3/tmp3-k1*m1/tmp1)/2;
            if (dF*gF+dG*gG>0 && x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so that e3-e1 decreases in the decreasing direction of e1=e2
            {
              searchmode=1;
            }
            else
            {
              //see the possibility of a e1!=e2 search
              //calcualte the dot product of the gradients and (x1, y1)
              double d1=m1/2/tmp1*(x1+k1*y1),
                     d2=m2/2/tmp2*(x1+k2*y1),
                     d3=m3/2/tmp3*(x1+k3*y1);
              //we can also prove that if there is a direction pointing inward R in which
              //  e1, e2, e2 decrease, and another direction pointing outside R in
              //  which e1, e2, e3 decrease, all the three decrease either on nc->(nc+1)
              //  direction along side nc:(nc+1), or on nc->(nc-1) direction along side
              //  nc:(nc-1).
              //
              //  On the contrary, if there is a direction
              //  in which all the three decrease, with two equal, it has to point
              //  outward for the program to get here. Then if along neither direction
              //  of side R can the three all descend, then there doesn't exist any
              //  direction inward R in which the three descend. In that case we
              //  have a minimal maximum at (F0, G0).
              if (d1<0 && d2<0 && d3<0) //so that all the three decrease in the direction (x1, y1)
              {
                if (d1<d2 && d1<d3) //e1 decreases fastest
                {
                  l1=l3;
                }
                else if (d2<=d1 && d2<d3) //e2 decreases fastest
                {
                  l2=l3;
                }
                else //d3<=d1 && d3<=d2
                {
                  //keep e1, e2
                }
                nd=n1;
                searchmode=2;
              }
              else
              {
                d1=m1/2/tmp1*(x0+k1*y0),
                d2=m2/2/tmp2*(x0+k2*y0),
                d3=m3/2/tmp3*(x0+k3*y0);
                if (d1>0 && d2>0 && d3>0) //so that all the three decrease in the direction -(x0, y0)
                {
                  if (d1>d2 && d1>d3) //e1 decreases fastest
                  {
                    l1=l3; //keep e2, e3
                  }
                  else if (d2>=d1 && d2>d3) //e2 decreases fastest
                  {
                    l2=l3;  //keep e1, e3
                  }
                  else //d3>=d1 && d3>=d2, e3 decreases fastest
                  {
                    //keep e1, e2
                  }
                  nd=n0;
                  searchmode=2;
                }
                else
                {
                  F1=F0, G1=G0;
                  searchmode=0;
                  minimax=true;
                }
              }
            }
          }
        }
      }
      else if (condmax==2) //e1=e2 being the maximum
      {
        //first see if the decreasing direction of e1=e2 points to the inside
        //calculate the decreasing direction of e1=e2 as (dF, dG)
        double den=m1*m2*(k1-k2)/2;
        dF=-(k1*m1*tmp2-k2*m2*tmp1)/den,
        dG=(m1*tmp2-m2*tmp1)/den;

        if (x1*dG-y1*dF<0 && x0*dG-y1*dF<0) //so that (dF, dG) points inward R
        {
          searchmode=1;
        }
        else
        {
          //calcualte the dot product of the gradients and (x1, y1)
          double d1=m1/2/tmp1*(x1+k1*y1),
                 d2=m2/2/tmp2*(x1+k2*y1);
          if (d1<0 && d2<0) //so that along side nc:(nc+1) e1, e2 descend in direction (x1, y1)
          {
            nd=n1;
            searchmode=2;
          }
          else
          {
            d1=m1/2/tmp1*(x0+k1*y0),
            d2=m2/2/tmp2*(x0+k2*y0);
            if (d1>0 && d2>0) //so that slong the side (nc-1):nc e1, e2 decend in direction -(x0, y0)
            {
              nd=n0;
              searchmode=2;
            }
            else
            {
              F1=F0, G1=G0;
              searchmode=0;
              minimax=true;
            }
          }
        }
      }
      else //condmax==3, e1 being the solo maximum
      {
        //calculate the negative gradient of e1 as (dF, dG)
        dF=-0.5*m1/tmp1;
        dG=k1*dF;

        if (x1*dG-y1*dF<0 && x0*dG-y0*dF<0) //so the gradient points inward R
          searchmode=3;
        else
        {
          //calculate the dot product of the gradient and (x1, y1)
          double d1=m1/2/tmp1*(x1+k1*y1),
                 d0=-m1/2/tmp1*(x0+k1*y0);
          if (d1<d0) //so that e1 decreases in direction (x1, y1) faster than in -(x0, y0)
          {
            if (d1<0)
            {
              nd=n1;
              searchmode=4;
            }
            else
            {
              F1=F0, G1=G0;
              searchmode=0;
              minimax=true;
            }
          }
          else //so that e1 decreases in direction -(x0, y0) faster than in (x1, y1)
          {
            if (d0<0)
            {
              nd=n0;
              searchmode=4;
            }
            else
            {
              F1=F0, G1=G0;
              searchmode=0;
              minimax=true;
            }
          }
        }
      }
    }
    //  the conditioning stage ends here
    //  the searching step starts here
    if (searchmode==0)
    {}
    else if (searchmode==1)
    {
      //Search mode 1: e1=e2 search
      //  starting with e1=e2, search down the decreasing direction of
      //  e1=e2 until at point (F1, G1) there is another l3 so that e1(F1, G1)
      //  =e2(F1, G1)=e3(F1, G1), or until at point (F1, G1) the search is about
      //  to go out of R.
      //Arguments: l1, l2, (F0, G0, vmax)
      double tmp=F0+k[l1]*G0; testnn(tmp);
      double e1=m[l1]*sqrt(tmp)-f[l1];
      tmp=F0+k[l2]*G0; testnn(tmp);
      double e2=m[l2]*sqrt(tmp)-f[l2];
      if (fabs(e1-e2)>1e-4)
      {
//        int kk=1;
        goto start;
      }

      //find intersections of e1=e2 with other ek's
      l3=-1;
    loopl3:
      double e=-1;
      for (int l=0; l<L; l++)
      {
        if (l==l1 || l==l2) continue;
        double lF1[2], lG1[2], lE[2];
        double lm[3]={m[l1], m[l2], m[l]};
        double lf[3]={f[l1], f[l2], f[l]};
        int lk[3]={k[l1], k[l2], k[l]};
        int r=FGFrom3(lF1, lG1, lE, lm, lf, lk);
        for (int i=0; i<r; i++)
        {
          if (lE[i]>e && lE[i]<vmax) l3=l, e=lE[i], F1=lF1[i], G1=lG1[i];
        }
      }
      //l3 shall be >=0 at this point
      //find intersections of e1=e2 with polygon sides
      if (l3<0)
      {
        ///int kkk=1;
        goto loopl3;
      }
      nc=-1;
      double F2, G2;
      for (int n=0; n<N; n++)
      {
        int nn=(n==N-1)?0:(n+1);
        double xn1=F[nn]-F[n], yn1=G[nn]-G[n], xn2=F1-F[n], yn2=G1-G[n];
        if (xn1*yn2-xn2*yn1>=0) //so that (F1, G1) is on or out of side n:(n+1)
        {
          nc=n;
          break;
        }
      }

      if (nc<0) //(F1, G1) being inside R
      {
        vmax=e;
        condpos=1;
        condmax=1;
        //l3 shall be >=0 at this point, so l1, l2, l3 are all ok
      }
      else //(F1, G1) being outside nc:(nc+1) //(F2, G2) being on side nc:(nc+1)
      {
        //find where e1=e2 crosses a side of R between (F0, G0) and (F1, G1)
        double lF2[2], lG2[2], lmd[2];
        double lm[2]={m[l1], m[l2]};
        double lf[2]={f[l1], f[l2]};
        int lk[2]={k[l1], k[l2]};

        int ncc=-1;
        while (ncc<0)
        {
          n1=(nc==N-1)?0:(nc+1);
          double xn1=F[n1]-F[nc], yn1=G[n1]-G[nc];

          int r=FGFrom2(lF2, lG2, lmd, F[nc], xn1, G[nc], yn1, lm, lf, lk);

          bool once=false;
          for (int i=0; i<r; i++)
          {
            double tmp=lF2[i]+k[l1]*lG2[i]; testnn(tmp);
            double le=m[l1]*sqrt(tmp)-f[l1];
            if (le<=vmax) //this shall be satisfied once
            {
              once=true;
              if (lmd[i]>=0 && lmd[i]<=1) //so that curve e1=e2 does cross the boundry within it
                ncc=nc, F2=lF2[i], G2=lG2[i], e=le;
              else if (lmd[i]>1) //so that the crossing point is out of vertex n1
                nc=n1;
              else //lmd[i]<0, so that the crossing point is out of vertex nc-1
                nc=(nc==0)?(N-1):(nc-1);
            }
          }
          if (!once)
          {
            //int kk=0;
            goto start;
          }
        }
        nc=ncc;

        vmax=e;
        condpos=2;
        if (F1==F2 && G1==G2)
          condmax=1;
        else
          condmax=2;
        F1=F2, G1=G2;
      }
    }
    else if (searchmode==2)
    {
      //  Search mode 2 (e1!=e2 search): starting with e1=e2, and a linear equation
      //  constraint, search down the decreasing direction until at point (F1, G1)
      //  there is a l3 so that e3(F1, G1)=max(e1, e2)(F1, G1), or at point (F1, G1)
      //  the search is about to go out of R.
      //  Arguments: l1, l2, dF, dG, (F0, G0, vmax)

      //The searching direction (dF, dG) is always along some polygon side
      //  If conpos==2, it is along nc:(nc+1)
      //  If conpos==3, it is along nc:(nc+1) or (nc-1):nc

      //

      //first check whether e1>e2 or e2>e1 down (dF, dG)
      dF=F[nd]-F0, dG=G[nd]-G0;
      //
      //the negative gradient of e2-e1 is calculated as (gF, gG)
      double gF=-(m2/tmp2-m1/tmp1)/2,
             gG=-(k2*m2/tmp2-k1*m1/tmp1)/2;
      if (gF*dF+gG*dG>0) //e2-e1 decrease down direction (dF, dG), so that e1>e2
      {}
      else //e1<e2 down (dF, dG)
      {
        int ll=l2; l2=l1; l1=ll;
        m1=m[l1], m2=m[l2], k1=k[l1], k2=k[l2];
        tmp=F0+k1*G0; testnn(tmp); tmp1=sqrt(tmp);
        tmp=F0+k2*G0; testnn(tmp); tmp2=sqrt(tmp);
      }
      //now e1>e2 down (dF, dG) from (F0, G0)
      tmp=F[nd]+k1*G[nd]; testnn(tmp);
      double e1=m1*sqrt(tmp)-f[l1];
      tmp=F[nd]+k2*G[nd]; testnn(tmp);
      double e2=m2*sqrt(tmp)-f[l2], FEx, GEx, eEx;
      int maxlmd=1;
      if (e1>=e2)
      {
        // then e1 is larger than e2 for the whole searching interval
        FEx=F[nd], GEx=G[nd];
        eEx=e1;
      }
      else // the they swap somewhere in between, now find it and make it maxlmd
      {
        double lF1[2], lG1[2], lmd[2];
        double lm[2]={m[l1], m[l2]};
        double lf[2]={f[l1], f[l2]};
        int lk[2]={k[l1], k[l2]};
        int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
        //process the results
        if (r==2) //must be so
        {
          if (lmd[0]<lmd[1]) lmd[0]=lmd[1], FEx=lF1[1], GEx=lG1[1];
          else FEx=lF1[0], GEx=lG1[0];
        }
        if (lmd[0]>0 && lmd[0]<1) //must be so
          maxlmd=lmd[0];
        tmp=FEx+k1*GEx; testnn(tmp);
        eEx=m1*sqrt(tmp)-f[l1];
      }

      l3=-1;
//      int l4;
      double e, llmd=maxlmd;
      int *tpl=new int[L], ctpl=0;
      for (int l=0; l<L; l++)
      {
        if (l==l1 || l==l2) continue;
        tmp=FEx+k[l]*GEx; testnn(tmp);
        double le=m[l]*sqrt(tmp)-f[l];
        if (le<eEx)
        {
          tpl[ctpl]=l;
          ctpl++;
          continue;
        }
        //solve for the equation system e1(F1, G1)=el(F1, G1), with (F1, G1) on nc:n1
        double lF1[2], lG1[2], lmd[2];
        double lm[2]={m[l1], m[l]};
        double lf[2]={f[l1], f[l]};
        int lk[2]={k[l1], k[l]};
        int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
        //process the results
        for (int i=0; i<r; i++)
        {
          if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction
          if (lmd[i]<llmd)
            l3=l, F1=lF1[i], G1=lG1[i], llmd=lmd[i];
        }
      }
      if (ctpl==L-2) //so that at (FEx, GEx) e1 is maximal, equivalent to "l3<0"
      {}
      else
      {
        //at this point F1 and G1 must have valid values already
        tmp=F1+k[l1]*G1; testnn(tmp);
        e=m[l1]*sqrt(tmp)-f[l1];
        //test if e1 is maximal at (F1, G1)
        bool lmax=true;
        for (int tp=0; tp<ctpl; tp++)
        {
          tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
          double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
          if (le>e) {lmax=false; break;}
        }
        if (!lmax)
        {
          for (int tp=0; tp<ctpl; tp++)
          {
            double lF1[2], lG1[2], lmd[2];
            double lm[2]={m[l1], m[tpl[tp]]};
            double lf[2]={f[l1], f[tpl[tp]]};
            int lk[2]={k[l1], k[tpl[tp]]};
            int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
            //process the results
            for (int i=0; i<r; i++)
            {
              if (lmd[i]<0 || lmd[i]>maxlmd) continue; //the solution is in the wrong direction
              if (lmd[i]<=llmd)
                l3=tpl[tp], F1=lF1[i], G1=lG1[i], llmd=lmd[i];
            }
          }
          tmp=F1+k[l1]*G1; testnn(tmp);
          e=m[l1]*sqrt(tmp)-f[l1];
        }
      }
      delete[] tpl;
      if (l3>=0) //the solution is found in the right direction
      {
        vmax=e;
        if (llmd==1) //(F1, G1) is a vertex of R
        {
          nc=nd;
          condpos=3;
          condmax=2;
          l2=l3;
        }
        else
        {
          if (condpos==3 && nd==n0) nc=n0;
          condpos=2;
          condmax=2;
          l2=l3;
        }
      }
      else if (maxlmd<1) //so that e1=e2 remains maximal at maxlmd
      {
        if (condpos==3 && nd==n0) nc=n0;
        condpos=2;
        condmax=2;
        //l1, l2 remain unchanged
        F1=FEx, G1=GEx;
        vmax=eEx;
      }
      else //so that e1 remains maximal at vertex nd
      {
        condpos=3;
        condmax=3;
        nc=nd;
        F1=FEx, G1=GEx; //this shall equal to F0+dF, G0+dG
        vmax=eEx;
      }
    }
    else if (searchmode==3)
    {
      //Search mode 3 (e1 search): starting with e1 being the only maximum, search down
      //  the decreasing direction of e1 until at point (F1, G1) there is another l2 so
      //  that e1(F1, G1)=e2(F1, G1), or until at point (F1, G1) the search is about
      //  to go out of R.
      //
      //  Arguments: l1, dF, dG, (F0, G0, vmax)
      //

      //first calculate the value of lmd from (F0, G0) to get out of R
      double maxlmd=-1, FEx, GEx;
      double fdggdf=-F0*dG+G0*dF;
      nd=-1;
      for (int n=0; n<N; n++)
      {
        if (condpos==2 && n==nc) continue;
        if ((condpos==3 && n==nc) || n==n0) continue;
        int nn=(n==N-1)?0:n+1;
        double xn1=F[n], xn2=F[nn], yn1=G[n], yn2=G[nn];
        double test1=xn1*dG-yn1*dF+fdggdf,
               test2=xn2*dG-yn2*dF+fdggdf;
        if (test1*test2<=0)
        {
          //the two following are equivalent. we use the larger denominator.
          FEx=(xn1*test2-xn2*test1)/(test2-test1);
          GEx=(yn1*test2-yn2*test1)/(test2-test1);
          if (fabs(dF)>fabs(dG)) maxlmd=(FEx-F0)/dF;
          else maxlmd=(GEx-G0)/dG;
          nd=n;
        }
      }

      //maxlmd must be >0 at this point.
      l2=-1;
      tmp=FEx+k[l1]*GEx; testnn(tmp);
      double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=maxlmd;
      int *tpl=new int[L], ctpl=0;
      for (int l=0; l<L; l++)
      {
        if (l==l1) continue;

        //if el does not catch up with e1 at (FEx, GEx), then there is no hope this
        //  happens in between (F0, G0) and (FEx, GEx)
        tmp=FEx+k[l]*GEx; testnn(tmp);
        double le=m[l]*sqrt(tmp)-f[l];
        if (le<eEx)
        {
          tpl[ctpl]=l;
          ctpl++;
          continue;
        }
        //now the existence of (F1, G1) is guaranteed
        double lF1[2], lG1[2], lmd[2];
        double lm[2]={m[l1], m[l]};
        double lf[2]={f[l1], f[l]};
        int lk[2]={k[l1], k[l]};
        int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
        for (int i=0; i<r; i++)
        {
          if (lmd[i]<0 || lmd[i]>maxlmd) continue;
          if (lmd[i]<=llmd) llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i];
        }
      }
      if (ctpl==L-1) //e1 is maximal at eEx, equivalent to "l2<0"
      {}//l2 must equal -1 at this point
      else
      {
        tmp=F1+k[l1]*G1; testnn(tmp);
        e=m[l1]*sqrt(tmp)-f[l1];
        //test if e1 is maximal at (F1, G1)
        //e1 is already maximal of those l's not in tpl[ctpl]
        bool lmax=true;
        for (int tp=0; tp<ctpl; tp++)
        {
          tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
          double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
          if (le>e) {lmax=false; break;}
        }
        if (!lmax)
        {
          for (int tp=0; tp<ctpl; tp++)
          {
            double lF1[2], lG1[2], lmd[2];
            double lm[2]={m[l1], m[tpl[tp]]};
            double lf[2]={f[l1], f[tpl[tp]]};
            int lk[2]={k[l1], k[tpl[tp]]};
            int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
            for (int i=0; i<r; i++)
            {
              if (lmd[i]<0 || lmd[i]>maxlmd) continue;
              if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i];
            }
          }
          tmp=F1+k[l1]*G1; testnn(tmp);
          e=m[l1]*sqrt(tmp)-f[l1];
        }
      }
      delete[] tpl;

      if (l2>=0) //so that e1 meets some other ek during the search
      {
        vmax=e; //this has to equal m[l2]*sqrt(F1+k[l2]*G1)-f[l2]
        if (vmax==eEx)
        {
          condpos=2;
          condmax=2;
          nc=nd;
        }
        else
        {
          condpos=1;
          condmax=2;
        }
      }
      else //l2==-1
      {
        vmax=eEx;
        F1=FEx, G1=GEx;
        condpos=2;
        condmax=3;
        nc=nd;
      }
    }
    else //searchmode==4
    {
      //Search mode 4: a special case of search mode 3 in which the boundry constraint
      //  is simply 0<=lmd<=1.
      dF=F[nd]-F0, dG=G[nd]-G0;
      l2=-1;
      int *tpl=new int[L], ctpl=0;
      tmp=F[nd]+k[l1]*G[nd]; testnn(tmp);
      double e, eEx=m[l1]*sqrt(tmp)-f[l1], llmd=1;
      for (int l=0; l<L; l++)
      {
        if (l==l1) continue;
        //if el does not catch up with e1 at (F[nd], G[nd]), then there is no hope this
        //  happens in between (F0, G0) and vertex nd.
        tmp=F[nd]+k[l]*G[nd]; testnn(tmp);
        double le=m[l]*sqrt(tmp)-f[l];
        if (le<eEx)
        {
          tpl[ctpl]=l;
          ctpl++;
          continue;
        }
        //now the existence of (F1, G1) is guaranteed
        double lF1[2], lG1[2], lmd[2];
        double lm[2]={m[l1], m[l]};
        double lf[2]={f[l1], f[l]};
        int lk[2]={k[l1], k[l]};
        loop1:
        int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
        for (int i=0; i<r; i++)
        {
          if (lmd[i]<0 || lmd[i]>1) continue;
          if (llmd>=lmd[i])
          {
            tmp=lF1[i]+k[l1]*lG1[i]; testnn(tmp);
            double e1=m[l1]*sqrt(tmp)-f[l1];
            tmp=lF1[i]+k[l]*lG1[i]; testnn(tmp);
            double e2=m[l]*sqrt(tmp)-f[l];
            if (fabs(e1-e2)>1e-4)
            {
              //int kk=0;
              goto loop1;
            }
            llmd=lmd[i], l2=l, F1=lF1[i], G1=lG1[i];
          }
        }
      }
      if (ctpl==N-1) //so e1 is maximal at (F[nd], G[nd])
      {}
      else
      {
        tmp=F1+k[l1]*G1; testnn(tmp);
        e=m[l1]*sqrt(tmp)-f[l1];
        //test if e1 is maximal at (F1, G1)
        bool lmax=true;
        for (int tp=0; tp<ctpl; tp++)
        {
          tmp=F1+k[tpl[tp]]*G1; testnn(tmp);
          double le=m[tpl[tp]]*sqrt(tmp)-f[tpl[tp]];
          if (le>e) {lmax=false; break;}
        }
        if (!lmax)
        {
          for (int tp=0; tp<ctpl; tp++)
          {
            double lF1[2], lG1[2], lmd[2];
            double lm[2]={m[l1], m[tpl[tp]]};
            double lf[2]={f[l1], f[tpl[tp]]};
            int lk[2]={k[l1], k[tpl[tp]]};
            int r=FGFrom2(lF1, lG1, lmd, F0, dF, G0, dG, lm, lf, lk);
            for (int i=0; i<r; i++)
            {
              if (lmd[i]<0 || lmd[i]>1) continue;
              if (llmd>=lmd[i]) llmd=lmd[i], l2=tpl[tp], F1=lF1[i], G1=lG1[i];
            }
          }
//          e=m[l1]*sqrt(F1+k[l1]*G1)-f[l1];
        }
      }
      tmp=F1+k[l1]*G1; testnn(tmp);
      e=m[l1]*sqrt(tmp)-f[l1];
      delete[] tpl;
      if (l2>=0)
      {
        vmax=e;
        if (vmax==eEx)
        {
          condpos=3;
          condmax=2;
          nc=nd;
        }
        else
        {
          condpos=2;
          condmax=2;
          //(F1, G1) lies on side n0:nc (nd=n0) or nc:n1 (nd=nc or nd=n1)
          if (nd==n0) nc=n0;
          //else nc=nc;
        }
      }
      else //l2==-1,
      {
        vmax=eEx;
        condpos=3;
        condmax=3;
        F1=F[nd], G1=G[nd];
        nc=nd;  //
      }
    }
    F0=F1, G0=G1;

    if (condmax==1 && ((l1^l2)==1 || (l1^l3)==1 || (l2^l3)==1))
      minimax=true;
    else if (condmax==2 && ((l1^l2)==1))
      minimax=true;
    else if (vmax<1e-6)
      minimax=true;
  }

  free(m);//delete[] m;
  delete[] vmnl;

  return vmax;
}//minimaxstiff*/

/*
  function NMResultToAtoms: converts the note-match result hosted in a NMResults structure, i.e.
  parameters of a harmonic atom, to a list of atoms. The process retrieves atom parameters from low
  partials until a required number of atoms have been retrieved or a non-positive frequency is
  encountered (non-positive frequencies are returned by note-match to indicate high partials for which
  no informative estimates can be obtained).

  In: results: the NMResults structure hosting the harmonic atom
      M: number of atoms to request from &results
      t: time of this harmonic atom
      wid: scale of this harmonic atom with which the note-match was performed
  Out: HP[return value]: list of atoms retrieved from $result.

  Returns the number of atoms retrieved.
*/
int NMResultToAtoms(int M, atom* HP, int t, int wid, NMResults results)
{
  double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp;
  atomtype* ptype=results.ptype;
  for (int i=0; i<M; i++)
  {
    if (fpp[i]<=0) {memset(&HP[i], 0, sizeof(atom)*(M-i)); return i;}
    HP[i].t=t, HP[i].s=wid, HP[i].f=fpp[i]/wid, HP[i].a=vfpp[i];
    HP[i].p=pfpp[i]; HP[i].type=ptype[i]; HP[i].pin=i+1;
  }
  return M;
}//NMResultToAtoms

/*
  function NMResultToPartials: reads atoms of a harmonic atom in a NMResult structure into HS partials.

  In: results: the NMResults structure hosting the harmonic atom
      M: number of atoms to request from &results
      fr: frame index of this harmonic atom
      t: time of this harmonic atom
      wid: scale of this harmonic atom with which the note-match was performed
  Out: Partials[M][]: HS partials whose fr-th frame is updated to the harmonic partial read from $results

  Returns the number of partials retrieved.
*/
int NMResultToPartials(int M, int fr, atom** Partials, int t, int wid, NMResults results)
{
  double *fpp=results.fp, *vfpp=results.vfp, *pfpp=results.pfp;
  atomtype* ptype=results.ptype;
  for (int i=0; i<M; i++)
  {
    atom* part=&Partials[i][fr];
    if (fpp[i]<=0) {for (int j=i; j<M; j++) memset(&Partials[j][fr], 0, sizeof(atom)); return i;}
    part->t=t, part->s=wid, part->f=fpp[i]/wid, part->a=vfpp[i];
    part->p=pfpp[i]; part->type=ptype[i]; part->pin=i+1;
  }
  return M;
}//NMResultToPartials

/*
  function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic
  sinusoid from spectrogram if Fr>1.

  In: x[Fr][N/2+1]: spectrogram
      fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes
      N, offst: atom scale and hop size
      R: initial F-G polygon constraint, optional
      settings: note match settings
      computes: pointer to a function that computes HA score, must be ds0 or ds1
      lastvfp[lastp]: amplitude of the previous harmonic atom
      forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
  Out: results: note match results
       f0, B: fundamental frequency and stiffness coefficient
       R: F-G polygon of harmonic atom

  Returns the total energy of HA or constant-pitch HS.
*/
double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps,
  int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results, int lastp,
  double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr)
{
  double result=0;

  double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
         delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
         *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf,
  int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr,
      pcount=settings->pcount, M=settings->M;

//	int *ptype=results->ptype;

  //calculate the energy of the last frame (hp)
  double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i];
  //fill frequency and amplitude buffers with 0
  //now determine numsam (the maximal number of partials)
  bool inputpartial=(f0>0 && settings->pin0>0);
  if (!inputpartial)
  {
    if (pcount>0) f0=pinf[0], pin0=pin[0];
    else f0=sqrt(R->X[0]), pin0=1;
  }
  int numsam=N*pin0/2.1/f0;
  if (numsam>maxp) numsam=maxp;
  //allocate buffer for candidate atoms
  TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
  //pcs[p] records the number of candidates for partial index p
  //psb[p] = 1: anchor, 2: user input, 0: normal
  int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1];
  memset(psb, 0, sizeof(int)*(numsam+1)*2);
  //if R is not specified, initialize a trivial candidate with maxB
  if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB);
  //if R is, initialize a trivial candidate by extending R by delp
  else cands[0][0]=new TTempAtom(R, delp, delp, minf0);

  int pm=0, pM=0;
  int ind=1; pcs[0]=1;
  //anchor partials: highest priority atoms, must have spectral peaks
  for (int i=0; i<pcount; i++)
  {
    //skip out-of-scope atoms
    if (pin[i]>=numsam) continue;
    //skip user input atom
    if (inputpartial && pin[i]==pin0) continue;

    void* dsparams;
    if (computes==ds0) dsparams=&cands[ind-1][0]->s;
    else
    {
      dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp};
      dsparams=&params;
    }

    if (pinfr[i]>0) //local anchor
    {
      double ls=0;
      for (int fr=0; fr<Fr; fr++)
      {
        cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB);
        ls+=~r;
      }
      ls=sqrt(ls/Fr);
      cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm, ls, computes(ls, dsparams));
    }
    else
    {
      //find the nearest peak to pinf[i]
      while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--;
      if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++;
      cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm, vps[pm], computes(vps[pm], dsparams));
    }
    delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++;
  }
  //harmonic atom grouping fails if anchors conflict
  if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;}

  //user input partial: must have spectral peak
  if (inputpartial)
  {
    //harmonic atom grouping fails if user partial is out of scope
    if (pin0>numsam){f0=0; goto cleanup;}

    TTempAtom* lcand=cands[ind-1][0];
    //feasible frequency range for partial pin0
    double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y);
    f1-=delm, f2+=delm;
    if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
    //forcepin0 being true means this partial have to be the peak nearest to f0
    bool inputfound=false;
    if (forceinputlocalfr>=0)
    {
      int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1;

      cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12];
      for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB);
      for (int k=1; k<k12-1; k++)
        if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1])
        {
          if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k;
        }
      if (pk>0)
      {
        double lf=k1+pk, la, lph, oldlf=lf;
        LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf);
        if (fabs(lf-oldlf)>0.6)
        {
          lf=oldlf;
          cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB);
          la=abs(r); lph=arg(r);
        }
        if (lf>f1 && lf<f2)
        {
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm, la, computes(la, dsparams));
          pcs[ind]=1;
          inputfound=true;
        }
      }
    }
    if (!inputfound && settings->forcepin0)
    { //find the nearest peak to f0
      while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--;
      if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++;
      if (fps[pm]>f1 && fps[pm]<f2)
      {
        void* dsparams;
        if (computes==ds0) dsparams=&lcand->s;
        else
        {
          dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
          dsparams=&params;
        }
        cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm, vps[pm], computes(vps[pm], dsparams));
        pcs[ind]=1;
      }
    }
    else if (!inputfound)
    {
      double epf0=settings->epf0;
      //lpcs records number of candidates found for partial pin0
      int lpcs=0;
      //lcoate peaks with the feasible frequency range, specified by f0 and epf0
      while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++;
      while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--;
      //add peaks as candidates
      for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2)
      {
        void* dsparams;
        if (computes==ds0) dsparams=&lcand->s;
        else
        {
          dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
          dsparams=&params;
        }
        cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm, vps[j], computes(vps[j], dsparams));
        lpcs++;
      }
      pcs[ind]=lpcs;
    }
    //harmonic atom grouping fails if user partial is not found
    if (pcs[ind]==0){f0=0; goto cleanup;}
    else {delete lcand->R; lcand->R=0;}
    psb[pin0]=2; ind++;
  }
  //normal partials (non-anchor, non-user)
  //p: partial index
  {
  int p=0;
  while (ind<=numsam)
  {
    p++;
    //skip anchor or user input partials
    if (psb[p]) continue;
    //loop over all candidates
    for (int i=0; i<pcs[ind-1]; i++)
    {
      //the ith candidate of last partial
      TTempAtom* lcand=cands[ind-1][i];
      //lpcs records number of candidates found for partial p
      int lpcs=0;
      //the feasible frequency range for partial p
      double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
      f1-=delm, f2+=delm; //allow a frequency error for the new partial
      if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
      //locate peaks within this range
      while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
      while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
      //add peaks as candidates
      for (int j=pm; j<=pM; j++)
      {
        if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range
        {
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], computes(vps[j], dsparams));
          lpcs++;
          if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;}
        }
      }
      //if there is no peak found for partial p
      if (lpcs==0)
      {
        cands[ind][pcs[ind]+lpcs]=lcand; lpcs++;
        cands[ind-1][i]=0;
        if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
      }
      else{delete lcand->R; lcand->R=0;}

      pcs[ind]+=lpcs;
    }
    ind++;
  }

  //select the candidate with maximal s
  int maxs=0; double vmax=cands[ind-1][0]->s;
  for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
  TTempAtom* lcand=cands[ind-1][maxs];

  //get results
  //read frequencies of found partials
  int P=0; int* ps=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B
  while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; P++;} lcand=lcand->Prev;}
  //read R of candidate with maximal s as R0
  TPolygon* R0=cands[ind-1][maxs]->R;

  if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, 0, x[0], N, psb, numsam, settings, results);
  else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, 0, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr);

  delete[] ps; delete[] fs;
  }
cleanup:
  for (int i=0; i<=numsam; i++)
    for (int j=0; j<pcs[i]; j++)
      delete cands[i][j];
  DeAlloc2(cands);
  delete[] psb;

  return result;
}//NoteMatchStiff3

/*
  function NoteMatchStiff3: finds harmonic atom from spectrum if Fr=1, or constant-pitch harmonic
  sinusoid from spectrogram if Fr>1. This version uses residue-sinusoid ratio ("rsr") that measures how
  "good" a spectral peak is in terms of its shape. peaks with large rsr[] is likely to be contaminated
  and their frequency estimates are regarded unreliable.

  In: x[Fr][N/2+1]: spectrogram
      N, offst: atom scale and hop size
      fps[pc], vps[pc], rsr[pc]: primitive (rough) peak frequencies, amplitudes and shape factor
      R: initial F-G polygon constraint, optional
      settings: note match settings
      computes: pointer to a function that computes HA score, must be ds0 or ds1
      lastvfp[lastp]: amplitude of the previous harmonic atom
      forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
  Out: results: note match results
       f0, B: fundamental frequency and stiffness coefficient
       R: F-G polygon of harmonic atom

  Returns the total energy of HA or constant-pitch HS.
*/
double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int pc, double* fps, double* vps,
  double* rsr, int Fr, cdouble** x, int N, int offst, NMSettings* settings, NMResults* results,
  int lastp, double* lastvfp, double (*computes)(double a, void* params), int forceinputlocalfr)
{
  double result=0;

  double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
         delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
         *pinf=settings->pinf, hB=settings->hB, epf0=settings->epf0;// epf=settings->epf,
  int maxp=settings->maxp, *pin=settings->pin, pin0=settings->pin0, *pinfr=settings->pinfr,
      pcount=settings->pcount, M=settings->M;

//	int *ptype=results->ptype;

  //calculate the energy of the last frame (hp)
  double lastene=0; for (int i=0; i<lastp; i++) lastene+=lastvfp[i]*lastvfp[i];
  //fill frequency and amplitude buffers with 0
  //now determine numsam (the maximal number of partials)
  bool inputpartial=(f0>0 && settings->pin0>0);
  if (!inputpartial)
  {
    if (pcount>0) f0=pinf[0], pin0=pin[0];
    else f0=sqrt(R->X[0]), pin0=1;
  }
  int numsam=N*pin0/2.1/f0;
  if (numsam>maxp) numsam=maxp;
  //allocate buffer for candidate atoms
  TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
  //pcs[p] records the number of candidates for partial index p
  //psb[p] = 1: anchor, 2: user input, 0: normal
  int *psb=new int[(numsam+1)*2], *pcs=&psb[numsam+1];
  memset(psb, 0, sizeof(int)*(numsam+1)*2);
  //if R is not specified, initialize a trivial candidate with maxB
  if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB);
  //if R is, initialize a trivial candidate by extending R by delp
  else cands[0][0]=new TTempAtom(R, delp, delp, minf0);

  int pm=0, pM=0;
  int ind=1; pcs[0]=1;
  //anchor partials: highest priority atoms, must have spectral peaks
  for (int i=0; i<pcount; i++)
  {
    //skip out-of-scope atoms
    if (pin[i]>=numsam) continue;
    //skip user input atom
    if (inputpartial && pin[i]==pin0) continue;

    void* dsparams;
    if (computes==ds0) dsparams=&cands[ind-1][0]->s;
    else
    {
      dsparams1 params={cands[ind-1][0]->s, lastene, cands[ind-1][0]->acce, pin[i], lastp, lastvfp};
      dsparams=&params;
    }

    if (pinfr[i]>0) //local anchor
    {
      double ls=0, lrsr=PeakShapeC(pinf[i], 1, N, &x[pinfr[i]], hB*2, M, c, iH2);
      for (int fr=0; fr<Fr; fr++)
      {
        cdouble r=IPWindowC(pinf[i], x[fr], N, M, c, iH2, pinf[i]-hB, pinf[i]+hB);
        ls+=~r;
      }
      ls=sqrt(ls/Fr);
      cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], pinf[i], delm+lrsr, ls, computes(ls, dsparams)); cands[ind][0]->rsr=lrsr;
    }
    else
    {
      //find the nearest peak to pinf[i]
      while (pm<pc-1 && fps[pm]<pinf[i]) pm++; while (pm>0 && fps[pm]>=pinf[i]) pm--;
      if (pm<pc-1 && pinf[i]-fps[pm]>fps[pm+1]-pinf[i]) pm++;
      cands[ind][0]=new TTempAtom(cands[ind-1][0], pin[i], fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm];
    }
    delete cands[ind-1][0]->R; cands[ind-1][0]->R=0; psb[pin[i]]=1; pcs[ind]=1; ind++;
  }
  //harmonic atom grouping fails if anchors conflict
  if (cands[ind-1][0]->R->N==0) {f0=0; goto cleanup;}

  //user input partial: must have spectral peak
  if (inputpartial)
  {
    //harmonic atom grouping fails if user partial is out of scope
    if (pin0>numsam){f0=0; goto cleanup;}

    TTempAtom* lcand=cands[ind-1][0];
    //feasible frequency range for partial pin0
    double f1, f2; ExFmStiff(f1, f2, pin0, lcand->R->N, lcand->R->X, lcand->R->Y);
    f1-=delm, f2+=delm;
    if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
    bool inputfound=false;
    if (forceinputlocalfr>=0)
    {
      int k1=floor(f0-epf0-1), k2=ceil(f0+epf0+1), k12=k2-k1+1, pk=-1;

      cdouble *lx=x[forceinputlocalfr], *lr=new cdouble[k12];
      for (int k=0; k<k12; k++) lr[k]=IPWindowC(k1+k, lx, N, M, c, iH2, k1+k-hB, k1+k+hB);
      for (int k=1; k<k12-1; k++)
        if (~lr[k]>~lr[k-1] && ~lr[k]>~lr[k+1])
        {
          if (pk==-1 || fabs(k1+pk-f0)>fabs(k1+k-f0)) pk=k;
        }
      if (pk>0)
      {
        double lf=k1+pk, la, lph, oldlf=lf;
        LSESinusoid(lf, lf-1, lf+1, lx, N, hB, M, c, iH2, la, lph, settings->epf);
        if (fabs(lf-oldlf)>0.6) //by which we judge that the LS result is biased by interference
        {
          lf=oldlf;
          cdouble r=IPWindowC(lf, lx, N, M, c, iH2, lf-hB, lf+hB);
          la=abs(r); lph=arg(r);
        }
        if (lf>f1 && lf<f2)
        {
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          double lrsr=PeakShapeC(lf, 1, N, &x[forceinputlocalfr], hB*2, M, c, iH2);
          cands[ind][0]=new TTempAtom(lcand, pin0, lf, delm+lrsr, la, computes(la, dsparams)); cands[ind][0]->rsr=lrsr;
          pcs[ind]=1;
          inputfound=true;
        }
      }
    }
    //forcepin0 being true means this partial have to be the peak nearest to f0
    if (!inputfound && settings->forcepin0)
    { //find the nearest peak to f0
      while (pm<pc-1 && fps[pm]<f0) pm++; while (pm>0 && fps[pm]>=f0) pm--;
      if (pm<pc-1 && f0-fps[pm]>fps[pm+1]-f0) pm++;
      if (fps[pm]>f1 && fps[pm]<f2)
      {
        void* dsparams;
        if (computes==ds0) dsparams=&lcand->s;
        else
        {
          dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
          dsparams=&params;
        }
        cands[ind][0]=new TTempAtom(lcand, pin0, fps[pm], delm+rsr[pm], vps[pm], computes(vps[pm], dsparams)); cands[ind][0]->rsr=rsr[pm];
        pcs[ind]=1;
      }
    }
    else if (!inputfound)
    {
      double epf0=settings->epf0;
      //lpcs records number of candidates found for partial pin0
      int lpcs=0;
      //lcoate peaks with the feasible frequency range, specified by f0 and epf0
      while (pm>0 && fps[pm]>=f0-epf0) pm--; while (pm<pc-1 && fps[pm]<f0-epf0) pm++;
      while (pM<pc-1 && fps[pM]<=f0+epf0) pM++; while (pM>0 && fps[pM]>f0+epf0) pM--;
      //add peaks as candidates
      for (int j=pm; j<=pM; j++) if (fps[j]>f1 && fps[j]<f2)
      {
        void* dsparams;
        if (computes==ds0) dsparams=&lcand->s;
        else
        {
          dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
          dsparams=&params;
        }
        cands[ind][lpcs]=new TTempAtom(lcand, pin0, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][lpcs]->rsr=rsr[j];
        lpcs++;
      }
      pcs[ind]=lpcs;
    }
    //harmonic atom grouping fails if user partial is not found
    if (pcs[ind]==0){f0=0; goto cleanup;}
    else {delete lcand->R; lcand->R=0;}
    psb[pin0]=2; ind++;
  }
  //normal partials (non-anchor, non-user)
  //p: partial index
  {
  int p=0;
  while (ind<=numsam)
  {
    p++;
    //skip anchor or user input partials
    if (psb[p]) continue;
    //loop over all candidates
    for (int i=0; i<pcs[ind-1]; i++)
    {
      int tightpks=0;
      //the ith candidate of last partial
      TTempAtom* lcand=cands[ind-1][i];
      //lpcs records number of candidates found for partial p
      int lpcs=0;
      //the feasible frequency range for partial p
      double tightf1, tightf2; ExFmStiff(tightf1, tightf2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
      double tightf=(tightf1+tightf2)*0.5, f1=tightf1-delm, f2=tightf2+delm; //allow a frequency error for the new partial
      if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
      //locate peaks within this range
      while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
      while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
      //add peaks as candidates
      for (int j=pm; j<=pM; j++)
      {
        if (fps[j]>f1 && fps[j]<f2) //this peak frequency falls in the frequency range
        {
          if ((fps[j]>tightf1 && fps[j]<tightf2) || fabs(fps[j]-tightf)<0.5) tightpks++; //indicates there are "tight" peaks found for this partial
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm+rsr[j], vps[j], computes(vps[j], dsparams)); cands[ind][pcs[ind]+lpcs]->rsr=rsr[j];
          lpcs++;
          if (pcs[ind]+lpcs>=MAX_CAND) {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
        }
      }
      //if there is no peak found for partial p.
      if (!lpcs)
      {
        //use the lcand directly
        //BUT UPDATE accumulative energy (acce) and s using induced partial
        if (tightf<N/2-hB)
        {
          double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr);
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          double ls=computes(la, dsparams);

          lcand->acce+=la*la;
          lcand->s=ls;
        }
        cands[ind][pcs[ind]+lpcs]=lcand;
        lpcs++;
        cands[ind-1][i]=0;
        if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
      }
      //if there are peaks but no tight peak found for partial p
      else if (!tightpks)
      {
        if (tightf<N/2-hB)
        {
          double la=0; for (int fr=0; fr<Fr; fr++) la+=~IPWindowC(tightf, x[fr], N, M, c, iH2, tightf-hB, tightf+hB); la=sqrt(la/Fr);
          void* dsparams;
          if (computes==ds0) dsparams=&lcand->s;
          else
          {
            dsparams1 params={lcand->s, lastene, lcand->acce, pin0, lastp, lastvfp};
            dsparams=&params;
          }
          double ls=computes(la, dsparams);
          TTempAtom* lnewcand=new TTempAtom(lcand, true);
          lnewcand->f=0; lnewcand->acce+=la*la; lnewcand->s=ls;
          cands[ind][pcs[ind]+lpcs]=lnewcand;
          lpcs++;
          if (pcs[ind]+lpcs>=MAX_CAND)
          {int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
        }
        delete lcand->R; lcand->R=0;
      }
      else{delete lcand->R; lcand->R=0;}

      pcs[ind]+=lpcs;
    }
    ind++;
  }

  //select the candidate with maximal s
  int maxs=0; double vmax=cands[ind-1][0]->s;
  for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
  TTempAtom* lcand=cands[ind-1][maxs];

  //get results
  //read frequencies of found partials
  int P=0; int* ps=new int[maxp]; double* fs=new double[maxp*2], *rsrs=&fs[maxp]; //these are used for evaluating f0 and B
  while (lcand){if (lcand->pind && lcand->f) {ps[P]=lcand->pind; fs[P]=lcand->f; rsrs[P]=lcand->rsr; P++;} lcand=lcand->Prev;}
  //read R of candidate with maximal s as R0
  TPolygon* R0=cands[ind-1][maxs]->R;

  if (Fr==1) result=GetResultsSingleFr(f0, B, R, R0, P, ps, fs, rsrs, x[0], N, psb, numsam, settings, results);
  else result=GetResultsMultiFr(f0, B, R, R0, P, ps, fs, rsrs, Fr, x, N, offst, psb, numsam, settings, results, forceinputlocalfr);

  delete[] ps; delete[] fs;
  }
cleanup:
  for (int i=0; i<=numsam; i++)
    for (int j=0; j<pcs[i]; j++)
      delete cands[i][j];
  DeAlloc2(cands);
  delete[] psb;

  return result;
}//NoteMatchStiff3

/*
  function NoteMatchStiff3: wrapper function of the above that does peak picking and HA grouping (or
  constant-pitch HS finding) in a single call.

  In: x[Fr][N/2+1]: spectrogram
      N, offst: atom scale and hop size
      R: initial F-G polygon constraint, optional
      startfr, validfrrange: centre and half width, in frames, of the interval used for peak picking if Fr>1
      settings: note match settings
      computes: pointer to a function that computes HA score, must be ds0 or ds1
      lastvfp[lastp]: amplitude of the previous harmonic atom
      forceinputlocalfr: specifies if partial settings->pin0 is taken for granted ("pinned")
  Out: results: note match results
       f0, B: fundamental frequency and stiffness coefficient
       R: F-G polygon of harmonic atom

  Returns the total energy of HA or constant-pitch HS.
*/
double NoteMatchStiff3(TPolygon* R, double &f0, double& B, int Fr, cdouble** x, int N, int offst, NMSettings* settings,
         NMResults* results, int lastp, double* lastvfp, double (*computes)(double a, void* params),
         bool forceinputlocalfr, int startfr, int validfrrange)
{
  //find spectral peaks
  double *fps=(double*)malloc8(sizeof(double)*N*2*Fr), *vps=&fps[N/2*Fr], *rsr=&fps[N*Fr];
  int pc;
  if (Fr>1) pc=QuickPeaks(fps, vps, Fr, N, x, startfr, validfrrange, settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr);
  else pc=QuickPeaks(fps, vps, N, x[0], settings->M, settings->c, settings->iH2, 0.0005, -1, -1, settings->hB*2, rsr);
  //if no peaks are present, return 0, indicating empty hp
  if (pc==0){free8(fps); return 0;}

  double result=NoteMatchStiff3(R, f0, B, pc, fps, vps, rsr, Fr, x, N, offst, settings, results, lastp, lastvfp, computes, forceinputlocalfr?startfr:-1);
  free8(fps);
  return result;
}//NoteMatchStiff3

/*
  function NoteMatchStiff3: finds harmonic atom from spectrum given the pitch as a (partial index,
  frequency) pair. This is used internally by NoteMatch3FB().

  In: x[N/2+1]: spectrum
      fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes
      R: initial F-G polygon constraint, optional
      pin0: partial index in the pitch specifier pair
      peak0: index to frequency in fps[] in the pitch specifier pair
      settings: note match settings
  Out: vfp[]: partial amplitudes
       R: F-G polygon of harmonic atom

  Returns the total energy of HA.
*/
double NoteMatchStiff3(TPolygon* R, int peak0, int pin0, cdouble* x, int pc, double* fps, double* vps, int N,
         NMSettings* settings, double* vfp, int** pitchind, int newpc)
{
  double maxB=settings->maxB, minf0=settings->minf0, maxf0=settings->maxf0,
         delm=settings->delm, delp=settings->delp, *c=settings->c, iH2=settings->iH2,
         hB=settings->hB;
  int maxp=settings->maxp, M=settings->M; // pcount=settings->pcount;

  double f0=fps[peak0];

  //determine numsam
  int numsam=N*pin0/2.1/f0; if (numsam>maxp) numsam=maxp;
  if (pin0>=numsam) return 0;
  //pcs[p] contains the number of candidates for partial index p
  TTempAtom*** Allocate2(TTempAtom*, numsam+1, MAX_CAND, cands);
  int *pcs=new int[numsam+1]; memset(pcs, 0, sizeof(int)*(numsam+1));
  if (R->N==0) cands[0][0]=new TTempAtom(1, (minf0+maxf0)*0.5, (maxf0-minf0)*0.5, maxB); //initialization: trivial candidate with maxB
  else cands[0][0]=new TTempAtom(R, delp, delp, minf0); //initialization: trivial candidate by expanding R

  cands[1][0]=new TTempAtom(cands[0][0], pin0, fps[peak0], delm, vps[peak0], vps[peak0]); cands[1][0]->tag[0]=peak0;
  delete cands[0][0]->R; cands[0][0]->R=0;

  int pm=0, pM=0, ind=2, p=0; pcs[0]=pcs[1]=1;

  while (ind<=numsam)
  {
    p++;
    if (p==pin0) continue;
    for (int i=0; i<pcs[ind-1]; i++)
    {
      TTempAtom* lcand=cands[ind-1][i]; //the the ith candidate of last partial
      int lpcs=0;
      {
        double f1, f2; ExFmStiff(f1, f2, p, lcand->R->N, lcand->R->X, lcand->R->Y); //calculate the frequency range to search for peak
        f1-=delm, f2+=delm; //allow a frequency error for the new partial
        if (f1<0) f1=0; if (f1>N/2.1) f1=N/2.1; if (f2>N/2.1) f2=N/2.1;
        while (pm>0 && fps[pm]>=f1) pm--; while (pm<pc-1 && fps[pm]<f1) pm++;
        while (pM<pc-1 && fps[pM]<=f2) pM++; while (pM>0 && fps[pM]>f2) pM--; //an index range for peaks
        for (int j=pm; j<=pM; j++)
        {
          if (fps[j]>f1 && fps[j]<f2 && (p>pin0 || pitchind[j][p]<0))
          {
            //this peak frequency falls in the frequency range
            cands[ind][pcs[ind]+lpcs]=new TTempAtom(lcand, p, fps[j], delm, vps[j], lcand->s+vps[j]); cands[ind][pcs[ind]+lpcs]->tag[0]=j; lpcs++;
            if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom)*lpcs); pcs[ind]=newpcs;}
          }
        }
        if (lpcs==0) //there is no peak found for the partial i, the last level
        {
          cands[ind][pcs[ind]+lpcs]=lcand; lpcs++; //new TTempAtom(lcand, false);
          cands[ind-1][i]=0;
          if (pcs[ind]+lpcs>=MAX_CAND){int newpcs=DeleteByHalf(cands[ind], pcs[ind]); memcpy(&cands[ind][newpcs], &cands[ind][pcs[ind]], sizeof(TTempAtom*)*lpcs); pcs[ind]=newpcs;}
        }
        else
        {
          delete lcand->R; lcand->R=0;
        }
      }
      pcs[ind]+=lpcs;
    }
    ind++;
  }

  //select the candidate with maximal s
  int maxs=0; double vmax=cands[ind-1][0]->s;
  for (int i=1; i<pcs[ind-1]; i++) if (vmax<cands[ind-1][i]->s) maxs=i, vmax=cands[ind-1][i]->s;
  TTempAtom* lcand=cands[ind-1][maxs];

  //get fp, vfp, pfp, ptype
  memset(vfp, 0, sizeof(double)*maxp);

  int P=0; int *ps=new int[maxp], *peaks=new int[maxp]; double* fs=new double[maxp]; //these are used for evaluating f0 and B

  while (lcand){if (lcand->pind) {ps[P]=lcand->pind; fs[P]=lcand->f; peaks[P]=lcand->tag[0]; P++;} lcand=lcand->Prev;}
  R->N=0;

  if (P>0)
  {
    for (int i=P-1; i>=0; i--)
    {
      int lp=ps[i];
      double lf=fs[i];
      vfp[lp-1]=vps[peaks[i]];
      if (R->N==0) InitializeR(R, lp, lf, delm, maxB);
      else if (vfp[lp-1]>1) CutR(R, lp, lf, delm, true);
      if (pitchind[peaks[i]][lp]>=0) {R->N=0; goto cleanup;}
      else pitchind[peaks[i]][lp]=newpc;
    }

    //estimate f0 and B
    double tmpa, cF, cG;
//    double norm[1024]; for (int i=0; i<1024; i++) norm[i]=1;
    areaandcentroid(tmpa, cF, cG, R->N, R->X, R->Y);
    testnn(cF); f0=sqrt(cF); //B=cG/cF;


    for (int i=0; i<numsam; i++)
    {
      if (vfp[i]==0) //no peak is found for this partial in lcand
      {
        int m=i+1;
        double tmp=cF+(m*m-1)*cG; testnn(tmp);
        double lf=m*sqrt(tmp);
        if (lf<N/2.1)
        {
          cdouble r=IPWindowC(lf, x, N, M, c, iH2, lf-hB, lf+hB);
          vfp[i]=-sqrt(r.x*r.x+r.y*r.y);
//          vfp[i]=-IPWindowC(lf, xr, xi, N, M, c, iH2, lf-hB, lf+hB, 0);
        }
      }
    }
  }

cleanup:
  delete[] ps; delete[] fs; delete[] peaks;
  for (int i=0; i<=numsam; i++) for (int j=0; j<pcs[i]; j++) delete cands[i][j];
  DeAlloc2(cands); delete[] pcs;

  double result=0;
  if (f0>0) for (int i=0; i<numsam; i++) result+=vfp[i]*vfp[i];
  return result;
}//NoteMatchStiff3*/

/*
  function NoteMatchStiff3FB: does one dynamic programming step in forward-background pitch tracking of
  harmonic sinusoids. This is used internally by FindNoteFB().

  In: R[pitchcount]: initial F-G polygons associated with pitch candidates at last frame
      vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at last frame
      sc[pitchcount]: accumulated scores associated with pitch candidates at last frame
      fps[pc], vps[pc]: primitive (rough) peak frequencies and amplitudes at this frame
      maxpitch: maximal number of pitch candidates
      x[wid/2+1]: spectrum
      settings: note match settings
  Out: pitchcount: number of pitch candidiate at this frame
       newpitches[pitchcount]: pitch candidates at this frame, whose lower word is peak index, higher word is partial index
       prev[pitchcount]: indices to predecessors of the pitch candidates at this frame
       R[pitchcount]: F-G polygons associated with pitch candidates at this frame
       vfp[pitchcount][maxp]: amplitude vectors associated with pitch candidates at this frame
       sc[pitchcount]: accumulated scores associated with pitch candidates at this frame

  No return value.
*/
void NoteMatchStiff3FB(int& pitchcount, TPolygon**& R, double**& vfp, double*& sc, int* newpitches, int* prev, int pc, double* fps, double* vps, cdouble* x, int wid, int maxpitch, NMSettings* settings)
{
  int maxpin0=6; //this specifies the maximal value that a pitch candidate may have as partial index
  double minf0=settings->minf0, delm=settings->delm, delp=settings->delp;
  int maxp=settings->maxp;

  //pc is the number of peaks at this frame, maxp is the maximal number of partials in the model
  //pitchind is initialized to a sequence of -1
  int** Allocate2(int, pc, maxp+1, pitchind); memset(pitchind[0], 0xff, sizeof(int)*pc*(maxp+1));

  //extend F-G polygons to allow pitch variation
  for (int i=0; i<pitchcount; i++) ExtendR(R[i], delp, delp, minf0);

  double *f1=new double[pitchcount], *f2=new double[pitchcount];
  int pm=0, newpc=0;
  TPolygon** newR=new TPolygon*[maxpitch]; memset(newR, 0, sizeof(TPolygon*)*maxpitch);
  double* newsc=new double[maxpitch]; memset(newsc, 0, sizeof(double)*maxpitch);
  double** Allocate2(double, maxpitch, maxp, newvfp);

  for (int pin0=1; pin0<=maxpin0; pin0++) //pin0: the partial index of a candidate pitch
  {
    //find out the range [pm, pM) of indices into fps[], so that for a * in this range (pin0, fps[*]) may fall
    //in the feasible pitch range succeeding one of the candidate pitches of the previous frame
    for (int i=0; i<pitchcount; i++) {ExFmStiff(f1[i], f2[i], pin0, R[i]->N, R[i]->X, R[i]->Y); f1[i]-=delm, f2[i]+=delm;}
    double f1a=f1[0], f2a=f2[0]; for (int i=1; i<pitchcount; i++) {if (f1a>f1[i]) f1a=f1[i]; if (f2a<f2[i]) f2a=f2[i];}
    while (pm<pc-1 && fps[pm]<f1a) pm++;
    int pM=pm; while (pM<pc-1 && fps[pM]<f2a) pM++;

    for (int p=pm; p<pM; p++) //loop through all peaks in this range
    {
      if (pitchind[p][pin0]>=0) continue; //skip this peak if it is already ...
      int max; double maxs; TPolygon* lnewR=0;

      for (int i=0; i<pitchcount; i++) //loop through candidate pitches of the previous frame
      {
        if (fps[p]>f1[i] && fps[p]<f2[i]) //so that this peak is a feasible successor of the i-th candidate pitch of the previous frame
        {
          if (pitchind[p][pin0]<0) //if this peak has not been registered as a candidate pitch with pin0 being the partial index
          {
            lnewR=new TPolygon(maxp*2+4, R[i]); //create a F-G polygon for (this peak, pin0) pair
            if (newpc==maxpitch)  //maximal number of candidate pitches is reached
            {
              //delete the candidate with the lowest score without checking if this lowest score is below the score
              //of the current pitch candidate (which is not yet computed). of course there is a risk that the new
              //score is even lower so that $maxpitch buffered scores may not be the highest $maxpitch scores, but
              //the (maxpitch-1) highest of the $maxpitch buffered scores should be the highest (maxpitch01) scores.
              int minsc=0; for (int j=0; j<maxpitch; j++) if (newsc[j]<newsc[minsc]) minsc=j;
              delete newR[minsc];
              if (minsc!=newpc-1) {newR[minsc]=newR[newpc-1]; memcpy(newvfp[minsc], newvfp[newpc-1], sizeof(double)*maxp); newsc[minsc]=newsc[newpc-1]; prev[minsc]=prev[newpc-1]; newpitches[minsc]=newpitches[newpc-1];}
            }
            else newpc++;
            //try to find harmonic atom with this candidate pitch
            if (NoteMatchStiff3(lnewR, p, pin0, x, pc, fps, vps, wid, settings, newvfp[newpc-1], pitchind, newpc-1)>0)
            {//if successful, buffer its F-G polygon and save its score
              newR[newpc-1]=lnewR;
              max=i; maxs=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]);
            }
            else
            {//if not, discard this candidate pitch
              delete lnewR; lnewR=0; newpc--;
            }
          }
          else  //if this peak has already been registered as a candidate pitch with pin0 being the partial index
          {
            //compute it score as successor to the i-th candidate of the previous frame
            double ls=sc[i]+conta(maxp, newvfp[newpc-1], vfp[i]);
            //if the score is higher than mark the i-th candidate of previous frame as its predecessor
            if (ls>maxs) maxs=ls, max=i;
          }
        }
      }
      if (lnewR) //i.e. a HA is found for this pitch candidate
      {
        ((__int16*)&newpitches[newpc-1])[0]=p; ((__int16*)&newpitches[newpc-1])[1]=pin0;
        newsc[newpc-1]=maxs; //take note of its score
        prev[newpc-1]=max;  //take note of its predecessor
      }
    }
  }
  DeAlloc2(pitchind);
  delete[] f1; delete[] f2;

  for (int i=0; i<pitchcount; i++) delete R[i]; delete[] R; R=newR;
  for (int i=0; i<newpc; i++) for (int j=0; j<maxp; j++) if (newvfp[i][j]<0) newvfp[i][j]=-newvfp[i][j];
  DeAlloc2(vfp); vfp=newvfp;
  delete[] sc; sc=newsc;
  pitchcount=newpc;
}//NoteMatchStiff3FB

/*
  function PeakShapeC: residue-sinusoid-ratio for a given (hypothesis) sinusoid frequency

  In: x[Fr][N/2+1]: spectrogram
      M, c[], iH2: cosine-family window specifiers
      f: reference frequency, in bins
      B: spectral truncation width

  Returns the residue-sinusoid-ratio.
*/
double PeakShapeC(double f, int Fr, int N, cdouble** x, int B, int M, double* c, double iH2)
{
  cdouble* w=new cdouble[B];
  int fst=floor(f+1-B/2.0);
  if (fst<0) fst=0;
  if (fst+B>N/2) fst=N/2-B;
  Window(w, f, N, M, c, fst, fst+B-1);
  cdouble xx=0, ww=Inner(B, w, w);
  double xw2=0;
  for (int fr=0; fr<Fr; fr++)
    xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]);
  delete[] w;
  if (xx.x==0) return 1;
  return 1-xw2/(xx.x*ww.x);
}//PeakShapeC
//version using cmplx<float> as spectrogram data type
double PeakShapeC(double f, int Fr, int N, cfloat** x, int B, int M, double* c, double iH2)
{
  cdouble* w=new cdouble[B];
  int fst=floor(f+1-B/2.0);
  if (fst<0) fst=0;
  if (fst+B>N/2) fst=N/2-B;
  Window(w, f, N, M, c, fst, fst+B-1);
  cdouble xx=0, ww=Inner(B, w, w);
  double xw2=0;
  for (int fr=0; fr<Fr; fr++)
    xw2+=~Inner(B, &x[fr][fst], w), xx+=Inner(B, &x[fr][fst], &x[fr][fst]);
  delete[] w;
  if (xx.x==0) return 1;
  return 1-xw2/(xx.x*ww.x);
}//PeakShapeC

/*
  function QuickPeaks: finds rough peaks in the spectrum (peak picking)

  In: x[N/2+1]: spectrum
      M, c[], iH2: cosine-family window function specifiers
      mina: minimal amplitude to spot a spectral peak
      [binst, binen): frequency range, in bins, to look for peaks
      B: spectral truncation width
  Out; f[return value], a[return value]: frequencies (in bins) and amplitudes of found peaks
       rsr[return value]: residue-sinusoid-ratio of found peaks, optional

  Returns the number of peaks found. f[] and a[] must be allocated enough space before calling.
*/
int QuickPeaks(double* f, double* a, int N, cdouble* x, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr)
{
  double hB=B*0.5;
  if (binst<2) binst=2;
  if (binst<hB) binst=hB;
  if (binen<0) binen=N/2-1;
  if (binen<0 || binen+hB>N/2-1) binen=N/2-1-hB;
  double a0=~x[binst-1], a1=~x[binst], a2;
  double minA=mina*mina*2/iH2;
  int p=0, n=binst;
  cdouble* w=new cdouble[B];
  while (n<binen)
  {
    a2=~x[n+1];
    if (a1>0 && a1>=a0 && a1>=a2)
    {
      if (a1>minA)
      {
        double A0=sqrt(a0), A1=sqrt(a1), A2=sqrt(a2);
        f[p]=n+(A0-A2)/2/(A0+A2-2*A1);
        int fst=floor(f[p]+1-hB);
        Window(w, f[p], N, M, c, fst, fst+B-1);
        double xw2=~Inner(B, &x[fst], w);
        a[p]=sqrt(xw2)*iH2;
        if (rsr)
        {
          cdouble xx=Inner(B, &x[fst], &x[fst]);
          if (xx.x==0) rsr[p]=1;
          else rsr[p]=1-xw2/xx.x*iH2;
        }
        p++;
      }
    }
    a0=a1;
    a1=a2;
    n++;
  }
  delete[] w;
  return p;
}//QuickPeaks

/*
  function QuickPeaks: finds rough peaks in the spectrogram (peak picking) for constant-frequency
  sinusoids

  In: x[Fr][N/2+1]: spectrogram
      fr0, r0: centre and half width of interval (in frames) to use for peak picking
      M, c[], iH2: cosine-family window function specifiers
      mina: minimal amplitude to spot a spectral peak
      [binst, binen): frequency range, in bins, to look for peaks
      B: spectral truncation width
  Out; f[return value], a[return value]: frequencies (in bins) and summary amplitudes of found peaks
       rsr[return value]: residue-sinusoid-ratio of found peaks, optional

  Returns the number of peaks found. f[] and a[] must be allocated enough space before calling.
*/
int QuickPeaks(double* f, double* a, int Fr, int N, cdouble** x, int fr0, int r0, int M, double* c, double iH2, double mina, int binst, int binen, int B, double* rsr)
{
  double hB=B*0.5;
  int hN=N/2;
  if (binst<hB) binst=hB;
  if (binen<0 || binen>hN-hB) binen=hN-hB;
  double minA=mina*mina*2/iH2;

  double *a0s=new double[hN*3*r0], *a1s=&a0s[hN*r0], *a2s=&a0s[hN*2*r0];
  int *idx=new int[hN*r0], *frc=new int[hN*r0];
  int** Allocate2(int, (hN*r0), (r0*2+1), frs);

  int pc=0;

  int lr=0;
  while (lr<=r0)
  {
    int fr[2]; //indices to frames to process for this lr
    if (lr==0) fr[0]=fr0, fr[1]=-1;
    else
    {
      fr[0]=fr0-lr, fr[1]=fr0+lr;
      if (fr[1]>=Fr) fr[1]=-1;
      if (fr[0]<0) {fr[0]=fr[1]; fr[1]=-1;}
    }
    for (int i=0; i<2; i++)
    {
      if (fr[i]>=0)
      {
        cdouble* xfr=x[fr[i]];
        for (int k=binst; k<binen; k++)
        {
          double a0=~xfr[k-1], a1=~xfr[k], a2=~xfr[k+1];
          if (a1>=a0 && a1>=a2)
          {
            if (a1>minA)
            {
              double A1=sqrt(a1), A2=sqrt(a2), A0=sqrt(a0);
              double lf=k+(A0-A2)/2/(A0+A2-2*A1);
              if (lr==0) //starting frame
              {
                f[pc]=lf;
                a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2;
                idx[pc]=pc; frs[pc][0]=fr[i]; frc[pc]=1;
                pc++;
              }
              else
              {
                int closep, indexp=InsertInc(lf, f, pc, false); //find the closest peak ever found in previous (i.e. closer to fr0) frames
                if (indexp==-1) indexp=pc, closep=pc-1;
                else if (indexp-1>=0 && fabs(lf-f[indexp-1])<fabs(lf-f[indexp])) closep=indexp-1;
                else closep=indexp;

                if (fabs(lf-f[closep])<0.2) //i.e. lf is very close to previous peak at fs[closep]
                {
                  int idxp=idx[closep];
                  a0s[idxp]+=a0, a1s[idxp]+=a1, a2s[idxp]+=a2;
                  frs[idxp][frc[idxp]]=fr[i]; frc[idxp]++;
                  double A0=sqrt(a0s[idxp]), A1=sqrt(a1s[idxp]), A2=sqrt(a2s[idxp]);
                  f[closep]=k+(A0-A2)/2/(A0+A2-2*A1);
                }
                else
                {
                  memmove(&f[indexp+1], &f[indexp], sizeof(double)*(pc-indexp)); f[indexp]=lf;
                  memmove(&idx[indexp+1], &idx[indexp], sizeof(int)*(pc-indexp)); idx[indexp]=pc;
                  a0s[pc]=a0, a1s[pc]=a1, a2s[pc]=a2, frs[pc][0]=fr[i], frc[pc]=1;
                  pc++;
                }
              }
            }
          }
        }
      }
    }
    lr++;
  }

  cdouble* w=new cdouble[B];
  int* frused=new int[Fr];
  for (int p=0; p<pc; p++)
  {
    int fst=floor(f[p]+1-hB);
    memset(frused, 0, sizeof(int)*Fr);
    int idxp=idx[p];

    Window(w, f[p], N, M, c, fst, fst+B-1);
    double xw2=0, xw2r=0, xxr=0;

    for (int ifr=0; ifr<frc[idxp]; ifr++)
    {
      int fr=frs[idxp][ifr];
      frused[fr]=1;
      xw2r+=~Inner(B, &x[fr][fst], w);
      xxr+=Inner(B, &x[fr][fst], &x[fr][fst]).x;
    }
    xw2=xw2r;
    for (int fr=0; fr<Fr; fr++)
      if (!frused[fr]) xw2+=~Inner(B, &x[fr][fst], w);
    a[p]=sqrt(xw2/Fr)*iH2;
    if (xxr==0) rsr[p]=1;
    else rsr[p]=1-xw2r/xxr*iH2;
  }
  delete[] w;
  delete[] frused;
  delete[] a0s;
  delete[] idx;
  delete[] frc;
  DeAlloc2(frs);
  return pc;
}//QuickPeaks

/*
  function ReEstHS1: reestimates a harmonic sinusoid by one multiplicative reestimation using phasor
  multiplier

  In: partials[M][Fr]: HS partials
      [frst, fren): frame (measurement point) range on which to do reestimation
      Data16[]: waveform data
  Out: partials[M][Fr]: updated HS partials

  No return value.
*/
void ReEstHS1(int M, int Fr, int frst, int fren, atom** partials, __int16* Data16)
{
  double* fs=new double[Fr*3], *as=&fs[Fr], *phs=&fs[Fr*2];
  int wid=partials[0][0].s, offst=partials[0][1].t-partials[0][0].t;
  int ldatastart=partials[0][0].t-wid/2;
  __int16* ldata16=&Data16[ldatastart];
  for (int m=0; m<M; m++)
  {
    for (int fr=0; fr<Fr; fr++) {fs[fr]=partials[m][fr].f; if (fs[fr]<=0){delete[] fs; return;} as[fr]=partials[m][fr].a, phs[fr]=partials[m][fr].p;}
    MultiplicativeUpdateF(fs, as, phs, ldata16, Fr, frst, fren, wid, offst);
    for (int fr=0; fr<Fr; fr++) partials[m][fr].f=fs[fr], partials[m][fr].a=as[fr], partials[m][fr].p=phs[fr];
  }
  delete[] fs;
}//ReEstHS1

/*
  function ReEstHS1: wrapper function.

  In: HS: a harmonic sinusoid
      Data16: its waveform data
  Out: HS: updated harmonic sinusoid

  No return value.
*/
void ReEstHS1(THS* HS, __int16* Data16)
{
  ReEstHS1(HS->M, HS->Fr, 0, HS->Fr, HS->Partials, Data16);
}//ReEstHS1

/*
  function SortCandid: inserts a candid object into a listed of candid objects sorted first by f, then
  (for identical f's) by df.

  In: cand: the candid object to insert to the list
      cands[newN]: the sorted list of candid objects
  Out: cands[newN+1]: the sorted list after the insertion

  Returns the index of $cand in the new list.
*/
int SortCandid(candid cand, candid* cands, int newN)
{
  int lnN=newN-1;
  while (lnN>=0 && cands[lnN].f>cand.f) lnN--;
  while (lnN>=0 && cands[lnN].f==cand.f && cands[lnN].df>cand.df) lnN--;
  lnN++; //now insert cantmp as cands[lnN]
  memmove(&cands[lnN+1], &cands[lnN], sizeof(candid)*(newN-lnN));
  cands[lnN]=cand;
  return lnN;
}//SortCandid

/*
  function SynthesisHS: synthesizes a harmonic sinusoid without aligning the phases

  In: partials[M][Fr]: HS partials
      terminatetag: external termination flag. Function SynthesisHS() polls *terminatetag and exits with
      0 when it is set.
  Out: [dst, den): time interval synthesized
       xrec[den-dst]: resynthesized harmonic sinusoid

  Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In
  the first case xrec is created anew with malloc8() and must be freed by caller using free8().
*/
double* SynthesisHS(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag)
{
  int wid=partials[0][0].s, hwid=wid/2;
  double *as=(double*)malloc8(sizeof(double)*Fr*13);
  double *fs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5],
         *a3=&as[Fr*6], *a2=&as[Fr*7], *a1=&as[Fr*8], *a0=&as[Fr*9], *xs=&as[Fr*11];
  int* ixs=(int*)&as[Fr*12];

  dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid;
  double* xrec=(double*)malloc8(sizeof(double)*(den-dst));
  memset(xrec, 0, sizeof(double)*(den-dst));

  for (int m=0; m<M; m++)
  {
    atom* part=partials[m]; bool fzero=false;
    for (int fr=0; fr<Fr; fr++)
    {
      if (part[fr].f<=0){fzero=true; break;}
      ixs[fr]=part[fr].t;
      xs[fr]=part[fr].t;
      as[fr]=part[fr].a*2;
      fs[fr]=part[fr].f;
      if (part[fr].type==atMuted) as[fr]=0;
		}
    if (fzero) break;
    if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;}

    CubicSpline(Fr-1, f3, f2, f1, f0, xs, fs, 1, 1);
    CubicSpline(Fr-1, a3, a2, a1, a0, xs, as, 1, 1);
    double ph=0, ph0=0;
    for (int fr=0; fr<Fr-1; fr++)
    {
      part[fr].p=ph;
      ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[fr], a2[fr], a1[fr], a0[fr], f3[fr], f2[fr], f1[fr], f0[fr], ph, true);)
      if (terminatetag && *terminatetag) {free8(xrec); free8(as); return 0;}
    }
    part[Fr-1].p=ph;
    ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[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, true);
           Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[0], a2[0], a1[0], a0[0], f3[0], f2[0], f1[0], f0[0], ph0, true);)
  }
  free8(as);
  return xrec;
}//SynthesisHS

/*
  function SynthesisHS: synthesizes a perfectly harmonic sinusoid without aligning the phases.
  Frequencies of partials above the fundamental are not used in this synthesis process.

  In: partials[M][Fr]: HS partials
      terminatetag: external termination flag. This function polls *terminatetag and exits with 0 when
      it is set.
  Out: [dst, den) time interval synthesized
       xrec[den-dst]: resynthesized harmonic sinusoid

  Returns pointer to xrec on normal finish, or 0 on external termination by setting $terminatetag. In
  the first case xrec is created anew with malloc8() and must be freed by caller using free8().
*/
double* SynthesisHS2(int M, int Fr, atom** partials, int& dst, int& den, bool* terminatetag)
{
  int wid=partials[0][0].s, hwid=wid/2;
  double *as=(double*)malloc8(sizeof(double)*Fr*7); //*as=new double[Fr*8],
  double *xs=&as[Fr], *f3=&as[Fr*2], *f2=&as[Fr*3], *f1=&as[Fr*4], *f0=&as[Fr*5];
	int *ixs=(int*)&as[Fr*6];
	double** a0=new double*[M*4], **a1=&a0[M], **a2=&a0[M*2], **a3=&a0[M*3];
	a0[0]=(double*)malloc8(sizeof(double)*M*Fr*4); //a0[0]=new double[M*Fr*4];
	for (int m=0; m<M; m++) a0[m]=&a0[0][m*Fr*4], a1[m]=&a0[m][Fr], a2[m]=&a0[m][Fr*2], a3[m]=&a0[m][Fr*3];

  dst=partials[0][0].t-hwid, den=partials[0][Fr-1].t+hwid;
  double* xrec=(double*)malloc8(sizeof(double)*(den-dst));
	memset(xrec, 0, sizeof(double)*(den-dst));
  atom* part=partials[0];

  for (int fr=0; fr<Fr; fr++)
  {
    xs[fr]=ixs[fr]=part[fr].t;
    as[fr]=part[fr].f;
  }
  CubicSpline(Fr-1, f3, f2, f1, f0, xs, as, 1, 1);

  for (int m=0; m<M; m++)
  {
    part=partials[m]; 
    for (int fr=0; fr<Fr; fr++)
    {
      if (part[fr].f<=0){M=m; break;}
      as[fr]=part[fr].a*2;
    }
    if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; return 0;}
    if (m>=M) break;

		CubicSpline(Fr-1, a3[m], a2[m], a1[m], a0[m], xs, as, 1, 1);
  }

  double *ph=(double*)malloc8(sizeof(double)*M*2), *ph0=&ph[M]; memset(ph, 0, sizeof(double)*M*2);
  for (int fr=0; fr<Fr-1; fr++)
  {
//    double *la0=new double[M*4], *la1=&la0[M], *la2=&la0[M*2], *la3=&la0[M*3]; for (int m=0; m<M; m++) la0[m]=a0[m][fr], la1[m]=a1[m][fr], la2[m]=a2[m][fr], la3[m]=a3[m][fr], part[fr].p=ph[m]; Sinusoids(M, &xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], la3, la2, la1, la0, f3[fr], f2[fr], f1[fr], f0[fr], ph, true); delete[] la0;
    for (int m=0; m<M; m++) ALIGN8(Sinusoid(&xrec[ixs[fr]-dst], 0, ixs[fr+1]-ixs[fr], a3[m][fr], a2[m][fr], a1[m][fr], a0[m][fr], (m+1)*f3[fr], (m+1)*f2[fr], (m+1)*f1[fr], (m+1)*f0[fr], ph[m], true);)
  }
	for (int m=0; m<M; m++)
  {
    part[Fr-1].p=ph[m];
    ALIGN8(Sinusoid(&xrec[ixs[Fr-2]-dst], ixs[Fr-1]-ixs[Fr-2], den-ixs[Fr-2], a3[m][Fr-2], a2[m][Fr-2], a1[m][Fr-2], a0[m][Fr-2], (m+1)*f3[Fr-2], (m+1)*f2[Fr-2], (m+1)*f1[Fr-2], (m+1)*f0[Fr-2], ph[m], true);
					 Sinusoid(&xrec[ixs[0]-dst], dst-ixs[0], 0, a3[m][0], a2[m][0], a1[m][0], a0[m][0], (m+1)*f3[0], (m+1)*f2[0], (m+1)*f1[0], (m+1)*f0[0], ph0[m], true);)
    if (terminatetag && *terminatetag) {free8(xrec); free8(as); free8(a0[0]); delete[] a0; free8(ph); return 0;}
  }
  free8(as); free8(ph); free8(a0[0]); delete[] a0;
  return xrec;
}//SynthesisHS2

/*
  function SynthesisHSp: synthesizes a harmonic sinusoid with phase alignment

  In: partials[pm][pfr]: HS partials
      startamp[pm][st_count]: onset amplifiers, optional
      st_start, st_offst: start of and interval between onset amplifying points, optional
  Out: [dst, den): time interval synthesized
       xrec[den-dst]: resynthesized harmonic sinusoid

  Returns pointer to xrec. xrec is created anew with malloc8() and must be freed by caller with free8().
*/
double* SynthesisHSp(int pm, int pfr, atom** partials, int& dst, int& den, double** startamp, int st_start, int st_offst, int st_count)
{
  int wid=partials[0][0].s, hwid=wid/2, offst=partials[0][1].t-partials[0][0].t;
  double *a1=new double[pfr*13];
  double *f1=&a1[pfr], *fa=&a1[pfr*2], *fb=&a1[pfr*3], *fc=&a1[pfr*4], *fd=&a1[pfr*5],
         *aa=&a1[pfr*6], *ab=&a1[pfr*7], *ac=&a1[pfr*8], *ad=&a1[pfr*9], *p1=&a1[pfr*10], *xs=&a1[pfr*11];
  int* ixs=(int*)&a1[pfr*12];

  dst=partials[0][0].t-hwid, den=partials[0][pfr-1].t+hwid;
	double *xrec=(double*)malloc8(sizeof(double)*(den-dst)*2), *xrecm=&xrec[den-dst];
  memset(xrec, 0, sizeof(double)*(den-dst));

  for (int p=0; p<pm; p++)
  {
    atom* part=partials[p];
    bool fzero=false;
    for (int fr=0; fr<pfr; fr++)
    {
      if (part[fr].f<=0)
      {
        fzero=true;
        break;
      }
      ixs[fr]=part[fr].t;
      xs[fr]=part[fr].t;
      a1[fr]=part[fr].a*2;
      f1[fr]=part[fr].f;
      p1[fr]=part[fr].p;
      if (part[fr].type==atMuted) a1[fr]=0;
    }
    if (fzero) break;

		CubicSpline(pfr-1, fa, fb, fc, fd, xs, f1, 1, 1);
    CubicSpline(pfr-1, aa, ab, ac, ad, xs, a1, 1, 1);

    for (int fr=0; fr<pfr-1; fr++) Sinusoid(&xrecm[ixs[fr]-dst], 0, offst, aa[fr], ab[fr], ac[fr], ad[fr], fa[fr], fb[fr], fc[fr], fd[fr], p1[fr], p1[fr+1], false);
//    Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, aa[0], ab[0], ac[0], ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false);
//    Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, aa[pfr-2], ab[pfr-2], ac[pfr-2], ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false);
    Sinusoid(&xrecm[ixs[0]-dst], -hwid, 0, 0, 0, 0, ad[0], fa[0], fb[0], fc[0], fd[0], p1[0], p1[1], false);
    Sinusoid(&xrecm[ixs[pfr-2]-dst], offst, offst+hwid, 0, 0, 0, ad[pfr-2], fa[pfr-2], fb[pfr-2], fc[pfr-2], fd[pfr-2], p1[pfr-2], p1[pfr-1], false);

    if (st_count)
    {
      double* amp=startamp[p];
      for (int c=0; c<st_count; c++)
      {
        double a1=amp[c], a2=(c+1<st_count)?amp[c+1]:1, da=(a2-a1)/st_offst;
        int lst=ixs[0]-dst+st_start+c*st_offst;
        double *lxrecm=&xrecm[lst];
        if (lst>0) for (int i=0; i<st_offst; i++) lxrecm[i]*=(a1+da*i);
        else for (int i=-lst; i<st_offst; i++) lxrecm[i]*=(a1+da*i);
      }
      for (int i=0; i<ixs[0]-dst+st_start; i++) xrecm[i]*=amp[0];
    }
    else
    {
      /*
      for (int i=0; i<=hwid; i++)
      {
        double tmp=0.5+0.5*cos(M_PI*i/hwid);
        xrecm[ixs[0]-dst-i]*=tmp;
        xrecm[ixs[pfr-2]-dst+offst+i]*=tmp;
      } */
    }

    for (int n=0; n<den-dst; n++) xrec[n]+=xrecm[n];
  }

  delete[] a1;
  return xrec;
}//SynthesisHSp

/*
  function SynthesisHSp: wrapper function.

  In: HS: a harmonic sinusoid.
  Out: [dst, den): time interval synthesized
       xrec[den-dst]: resynthesized harmonic sinusoid

  Returns pointer to xrec, which is created anew with malloc8() and must be freed by caller using free8().
*/
double* SynthesisHSp(THS* HS, int& dst, int& den)
{
  return SynthesisHSp(HS->M, HS->Fr, HS->Partials, dst, den, HS->startamp, HS->st_start, HS->st_offst, HS->st_count);
}//SynthesisHSp