annotate wavelet.cpp @ 1:6422640a802f

first upload

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

#include <math.h>
#include <mem.h>
#include "wavelet.h"
#include "matrix.h"

//---------------------------------------------------------------------------
/*
  function csqrt: real implementation of complex square root z=sqrt(x)

  In: xr and xi: real and imaginary parts of x
  Out:  zr and zi: real and imaginary parts of z=sqrt(x)

  No return value.
*/
void csqrt(double& zr, double& zi, double xr, double xi)
{
  if (xi==0)
    if (xr>=0) zr=sqrt(xr), zi=0;
    else zi=sqrt(-xr), zr=0;
  else
  {
    double xm=sqrt(xr*xr+xi*xi);
    double ri=sqrt((xm-xr)/2);
    zr=xi/2/ri;
    zi=ri;
  }
}//csqrt

/*
  function Daubechies: calculates the Daubechies filter of a given order p

  In: filter order p
  Out:  h[2p]: the 2p FIR coefficients

  No reutrn value. The calculated filters are minimum phase, which means the energy concentrates at the
  beginning. This is usually used for reconstruction. On the contrary, for wavelet analysis the filter
  is mirrored.
*/
void Daubechies(int p, double* h)
{
//initialize h(z)
  double r01=pow(2, -p-p+1.5);

  h[0]=1;
  for (int i=1; i<=p; i++)
  {
    h[i]=h[i-1]*(p+1-i)/i;
  }

  //construct polynomial p
  double *P=new double[p], *rp=new double[p], *ip=new double[p];

  P[p-1]=1;
  double r02=1;
  for (int i=p-1; i>0; i--)
  {
    double rate=(i+1-1.0)/(p-2.0+i+1);
    P[i-1]=P[i]*rate;
    r02/=rate;
  }
  Roots(p-1, P, rp, ip);
  for (int i=0; i<p-1; i++)
  {
  //current length of h is p+1+i, h[0:p+i]
    if (i<p-2 && rp[i]==rp[i+1] && ip[i]==-ip[i+1])
    {
      double ar=rp[i], ai=ip[i];
      double bkr=-2*ar+1, bki=-2*ai, ckr=4*(ar*ar-ai*ai-ar), cki=4*(2*ar*ai-ai), dlr, dli;
      csqrt(dlr, dli, ckr, cki);
      double akr=bkr+dlr, aki=bki+dli;
      if (akr*akr+aki*aki>1) akr=bkr-dlr, aki=bki-dli;
      double ak1=-2*akr, ak2=akr*akr+aki*aki;
      h[p+2+i]=ak2*h[p+i];
      h[p+1+i]=ak2*h[p-1+i]+ak1*h[p+i];
      for (int j=p+i; j>1; j--) h[j]=h[j]+ak1*h[j-1]+ak2*h[j-2];
      h[1]=h[1]+ak1*h[0];
      r02/=ak2;
      i++;
    }
    else //real root of P
    {
      double ak, bk=-(2*rp[i]-1), delk=4*rp[i]*(rp[i]-1);
      if (bk>0) ak=bk-sqrt(delk);
      else ak=bk+sqrt(delk);
      r02/=ak;
      h[p+1+i]=-ak*h[p+i];
      for (int j=p+i; j>0; j--) h[j]=h[j]-ak*h[j-1];
    }
  }
  delete[] P; delete[] rp; delete[] ip;
  r01=r01*sqrt(r02);
  for (int i=0; i<p*2; i++) h[i]*=r01;
}//Daubechies

/*
    Periodic wavelet decomposition and reconstruction routines

    The wavelet transform of an N-point sequence is arranged in "interleaved" format
    as another N-point sequance. Level 1 details are found at N/2 points 1, 3, 5, ...,
    N-1; level 2 details are found at N/4 points 2, 6, ..., N-2; level 3 details are
    found at N/8 points 4, 12, ..., N-4; etc.
*/

/*
  function dwtpqmf: in this implementation h and g are the same as reconstruction qmf filters. In fact
  the actual filters used are their mirrors and filter origin are aligned to the ends of the real
  filters, which turn out to be the starts of h and g.

  The inverse transform is idwtp(), the same as inversing dwtp().

  In: in[Count]:  waveform data
      h[M], g[M]: quadratic mirror filter pair
      N:  maximal time resolution
        Count=kN, N=2^lN being the reciprocal of the most detailed frequency scale, i.e.
        N=1 for no transforming at all, N=2 for dividing into approx. and detail,
        N=4 for dividing into approx+detail(approx+detial), etc.
        Count*2/N=2k gives the smallest length to be convolved with h and g.
  Out: out[N], the wavelet transform, arranged in interleaved format.

  Returns maximal atom length (should equal N).
*/
int dwtpqmf(double* in, int Count, int N, double* h, double* g, int M, double* out)
{
  double* tmp=new double[Count];
  double *tmpa=tmp, *tmpla=in;
  int C=Count, L=0, n=1;

A:
  {
    //C: signal length at current layer
    //L: layer index, 0 being most detailed
    //n: atom length on current layer, equals 2^L.
    //C*n=(C<<L)=Count
    double* tmpd=&tmpa[C/2];
    for (int i=0; i<C; i+=2)
    {
      int i2=i/2;
      tmpa[i2]=tmpla[i]*h[0];
      tmpd[i2]=tmpla[i]*g[0];
      for (int j=1; j<M; j++)
      {
        if (i+j<C)
        {
          tmpa[i2]+=tmpla[i+j]*h[j];
          tmpd[i2]+=tmpla[i+j]*g[j];
        }
        else
        {
          tmpa[i2]+=tmpla[i+j-C]*h[j];
          tmpd[i2]+=tmpla[i+j-C]*g[j];
        }
      }
    }
    for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
    for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
    n*=2;
    if (n<N)
    {
      tmpla=tmpa;
      tmpa=tmpd;
      L++;
      C/=2;
      goto A;
    }
  }
  delete[] tmp;
  return n;
}//dwtpqmf

/*
  function dwtp: in this implementation h and g can be different from mirrored reconstruction filters,
  i.e. the biorthogonal transform. h[0] and g[0] are aligned at the ends of the filters, i.e. h[-M+1:0],
  g[-M+1:0].

  In: in[Count]:  waveform data
      h[-M+1:0], g[-M+1:0]: quadratic mirror filter pair
      N:  maximal time resolution
  Out: out[N], the wavelet transform, arranged in interleaved format.

  Returns maximal atom length (should equal N).
*/
int dwtp(double* in, int Count, int N, double* h, double* g, int M, double* out)
{
  double* tmp=new double[Count];
  double *tmpa=tmp, *tmpla=in;
  int C=Count, L=0, n=1;

A:
  {   
    double* tmpd=&tmpa[C/2];
    for (int i=0; i<C; i+=2)
    {
      int i2=i/2;
      tmpa[i2]=tmpla[i]*h[0];
      tmpd[i2]=tmpla[i]*g[0];
      for (int j=-1; j>-M; j--)
      {
        if (i-j<C)
        {
          tmpa[i2]+=tmpla[i-j]*h[j];
          tmpd[i2]+=tmpla[i-j]*g[j];
        }
        else
        {
          tmpa[i2]+=tmpla[i-j-C]*h[j];
          tmpd[i2]+=tmpla[i-j-C]*g[j];
        }
      }
    }
    for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
    for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
    n*=2;
    if (n<N)
    {
      tmpla=tmpa;
      tmpa=tmpd;
      L++;
      C/=2;
      goto A;
    }
  }
  delete[] tmp;
  return n;
}//dwtp

/*
  function dwtp: in this implementation h and g can be different size. h[0] and g[0] are aligned at the
  ends of the filters, i.e. h[-Mh+1:0], g[-Mg+1:0].

  In: in[Count]:  waveform data
      h[-Mh+1:0], g[-Mg+1:0]: quadratic mirror filter pair
      N:  maximal time resolution
  Out: out[N], the wavelet transform, arranged in interleaved format.

  Returns maximal atom length (should equal N).
*/
int dwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out)
{
  double* tmp=new double[Count];
  double *tmpa=tmp, *tmpla=in;
  int C=Count, L=0, n=1;

A:
  {   
    double* tmpd=&tmpa[C/2];
    for (int i=0; i<C; i+=2)
    {
      int i2=i/2;
      tmpa[i2]=tmpla[i]*h[0];
      tmpd[i2]=tmpla[i]*g[0];
      for (int j=-1; j>-Mh; j--)
      {
        if (i-j<C)
        {
          tmpa[i2]+=tmpla[i-j]*h[j];
        }
        else
        {
          tmpa[i2]+=tmpla[i-j-C]*h[j];
        }
      }
      for (int j=-1; j>-Mg; j--)
      {
        if (i-j<C)
        {
          tmpd[i2]+=tmpla[i-j]*g[j];
        }
        else
        {
          tmpd[i2]+=tmpla[i-j-C]*g[j];
        }
      }
    }
    for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
    for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
    n*=2;
    if (n<N)
    {
      tmpla=tmpa;
      tmpa=tmpd;
      L++;
      C/=2;
      goto A;
    }
  }
  delete[] tmp;
  return n;
}//dwtp

/*
  function dwtp: in this implementation h and g can be arbitrarily located: h from $sh to $eh, g from
  $sg to $eg.

  In: in[Count]:  waveform data
      h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair
      N:  maximal time resolution
  Out: out[N], the wavelet transform, arranged in interleaved format.

  Returns maximal atom length (should equal N).
*/
int dwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out)
{
  double* tmp=new double[Count];
  double *tmpa=tmp, *tmpla=in;
  int C=Count, L=0, n=1;

A:
  {
    double* tmpd=&tmpa[C/2];
    for (int i=0; i<C; i+=2)
    {
      int i2=i/2;
      tmpa[i2]=0;//tmpla[i]*h[0];
      tmpd[i2]=0;//tmpla[i]*g[0];
      for (int j=sh; j<=eh; j++)
      {
        int ind=i-j;
        if (ind>=C) tmpa[i2]+=tmpla[ind-C]*h[j];
        else if (ind<0) tmpa[i2]+=tmpla[ind+C]*h[j];
        else tmpa[i2]+=tmpla[ind]*h[j];
      }
      for (int j=sg; j<=eg; j++)
      {
        int ind=i-j;
        if (ind>=C) tmpd[i2]+=tmpla[i-j-C]*g[j];
        else if (ind<0) tmpd[i2]+=tmpla[i-j+C]*g[j];
        else tmpd[i2]+=tmpla[i-j]*g[j];
      }
    }
    for (int i=0; i*2+1<C; i++) out[(2*i+1)<<L]=tmpd[i];
    for (int i=0; i*2<C; i++) out[i<<(L+1)]=tmpa[i];
    n*=2;
    if (n<N)
    {
      tmpla=tmpa;
      tmpa=tmpd;
      L++;
      C/=2;
      goto A;
    }
  }
  delete[] tmp;
  return n;
}//dwtp

/*
  function idwtp: periodic wavelet reconstruction by qmf

  In: in[Count]: wavelet transform in interleaved format
      h[M], g[M]: quadratic mirror filter pair
      N:  maximal time resolution
  Out: out[N], waveform data (detail level 0).

  No return value.
*/
void idwtp(double* in, int Count, int N, double* h, double* g, int M, double* out)
{
  double* tmp=new double[Count];
  memcpy(out, in, sizeof(double)*Count);
  int n=N, C=Count/N, L=log2(N)-1;
  while (n>1)
  {
    memset(tmp, 0, sizeof(double)*C*2);
    //2k<<L being the approx, (2k+1)<<L being the detail
    for (int i=0; i<C; i++)
    {
      for (int j=0; j<M; j++)
      {
        if (i*2+j<C*2)
        {
          tmp[i*2+j]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j];
        }
        else
        {
          tmp[i*2+j-C*2]+=out[(2*i)<<L]*h[j]+out[(2*i+1)<<L]*g[j];
        }
      }
    }
    for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
    n/=2;
    C*=2;
    L--;
  }
  delete[] tmp;
}//idwtp

/*
  function idwtp: in which h and g can have different length.

  In: in[Count]: wavelet transform in interleaved format
      h[Mh], g[Mg]: quadratic mirror filter pair
      N:  maximal time resolution
  Out: out[N], waveform data (detail level 0).

  No return value.
*/
void idwtp(double* in, int Count, int N, double* h, int Mh, double* g, int Mg, double* out)
{
  double* tmp=new double[Count];
  memcpy(out, in, sizeof(double)*Count);
  int n=N, C=Count/N, L=log2(N)-1;
  while (n>1)
  {
    memset(tmp, 0, sizeof(double)*C*2);
    //2k<<L being the approx, (2k+1)<<L being the detail
    for (int i=0; i<C; i++)
    {
      for (int j=0; j<Mh; j++)
      {
        int ind=i*2+j+(Mg-Mh)/2;
        if (ind>=C*2)
        {
          tmp[ind-C*2]+=out[(2*i)<<L]*h[j];
        }
        else if (ind<0)
        {
          tmp[ind+C*2]+=out[(2*i)<<L]*h[j];
        }
        else
        {
          tmp[ind]+=out[(2*i)<<L]*h[j];
        }
      }
    }
    for (int i=0; i<C; i++)
    {
      for (int j=0; j<Mg; j++)
      {
        int ind=i*2+j+(Mh-Mg)/2;
        if (ind>=C*2)
        {
          tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j];
        }
        else if (ind<0)
        {
          tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j];
        }
        else
        {
          tmp[ind]+=out[(2*i+1)<<L]*g[j];
        }
      }
    }
    for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
    n/=2;
    C*=2;
    L--;
  }
  delete[] tmp;
}//idwtp

/*
  function idwtp: in which h and g can be arbitrarily located: h from $sh to $eh, g from $sg to $eg

  In: in[Count]: wavelet transform in interleaved format
      h[sh:eh-1], g[sg:eg-1]: quadratic mirror filter pair
      N: maximal time resolution
  Out: out[N], waveform data (detail level 0).

  No return value.
*/
void idwtp(double* in, int Count, int N, double* h, int sh, int eh, double* g, int sg, int eg, double* out)
{
  double* tmp=new double[Count];
  memcpy(out, in, sizeof(double)*Count);
  int n=N, C=Count/N, L=log2(N)-1;
  while (n>1)
  {
    memset(tmp, 0, sizeof(double)*C*2);
    //2k<<L being the approx, (2k+1)<<L being the detail
    for (int i=0; i<C; i++)
    {
      for (int j=sh; j<=eh; j++)
      {
        int ind=i*2+j;
        if (ind>=C*2) tmp[ind-C*2]+=out[(2*i)<<L]*h[j];
        else if (ind<0) tmp[ind+C*2]+=out[(2*i)<<L]*h[j];
        else tmp[ind]+=out[(2*i)<<L]*h[j];
      }
    }
    for (int i=0; i<C; i++)
    {
      for (int j=sg; j<=eg; j++)
      {
        int ind=i*2+j;
        if (ind>=C*2) tmp[ind-C*2]+=out[(2*i+1)<<L]*g[j];
        else if (ind<0) tmp[ind+C*2]+=out[(2*i+1)<<L]*g[j];
        else tmp[ind]+=out[(2*i+1)<<L]*g[j];
      }
    }
    for (int i=0; i<C*2; i++) out[i<<L]=tmp[i];
    n/=2;
    C*=2;
    L--;
  }
  delete[] tmp;
}//idwtp

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

/*
  Spline biorthogonal wavelet routines.

  Further reading: "Calculation of biorthogonal spline wavelets.pdf"
*/

//function Cmb: combination number C(n, k) (n>=k>=0)
int Cmb(int n, int k)
{
  if (k>n/2) k=n-k;
  int c=1;
  for (int i=1; i<=k; i++) c=c*(n+1-i)/i;
  return c;
}

/*
  function splinewl: computes spline biorthogonal wavelet filters. This version of splinewl calcualtes
  the positive-time half of h and hm coefficients only.

  p1 and p2 must have the same parity. If p1 is even, p1 coefficients will be returned in h1; if p1 is
  odd, p1-1 coefficients will be returned in h1.

  Actual length of h is p1+1, of hm is p1+2p2-1. only a half of each is kept.
  p even: h[0:p1/2] <- [p1/2:p1], hm[0:p1/2+p2-1] <- [p1/2+p2-1:p1+2p2-2]
  p odd: h[0:(p1-1)/2] <- [(p1+1)/2:p1], hm[0:(p1-3)/2+p2] <- [(p1-1)/2+p2:p1+2p2-2]
  i.e. h[0:hp1] <- [p1-hp1:p1], hm[0:hp1+p2-1] <- [p1-hp1-1+p2:p1+2p2-2]

  In: p1, p2: specify vanishing moments of h and hm
  Out: h[] and hm[] as specified above.

  No return value.
*/
void splinewl(int p1, int p2, double* h, double* hm)
{
  int hp1=p1/2, hp2=p2/2;
  int q=(p1+p2)/2;
  h[hp1]=sqrt(2.0)*pow(2, -p1);
//  h1[hp1]=1;
  for (int i=1, j=hp1-1; i<=hp1; i++, j--)
  {
    h[j]=h[j+1]*(p1+1-i)/i;
  }

  double *_hh1=new double[p2+1], *_hh2=new double[2*q];
  double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q];

  hh1[hp2]=sqrt(2.0)*pow(2, -p2);
  for (int i=1, j=hp2-1; i<=hp2; i++, j--)
  {
    hh1[j]=hh1[j+1]*(p2+1-i)/i;
  }
  if (p2%2) //p2 is odd
  {
    for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i];
  }
  else //p2 even
  {
    for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i];
  }

  memset(_hh2, 0, sizeof(double)*2*q);
  for (int n=1-q; n<=q-1; n++)
  {
    int k=abs(n);
    int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n);  //CC=1.0*C(q-1+k, k)*C(2*k, k-n)
    for (; k<=q-1; k++)
    {
      hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k);
      CC1=CC1*(q+k)/(k+1);
      CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n));
    }
    hh2[n]*=pow(-1, n);
  }

  //hh1[hp2-p2:hp2], hh2[1-q:q-1]
  //h2=conv(hh1, hh2), but the positive half only
  memset(hm, 0, sizeof(double)*(hp1+p2));
  for (int i=hp2-p2; i<=hp2; i++)
    for (int j=1-q; j<=q-1; j++)
  {
    if (i+j>=0 && i+j<hp1+p2)
      hm[i+j]+=hh1[i]*hh2[j];
  }
  
  delete[] _hh1;
  delete[] _hh2;
}//splinewl


/*
  function splinewl: calculates the analysis and reconstruction filter pairs of spline biorthogonal
  wavelet (h, g) and (hm, gm). h has the size p1+1, hm has the size p1+2p2-1.

  If p1+1 is odd, then all four filters are symmetric; if not, then h and hm are symmetric, while g and
  gm are anti-symmetric.

  The concatenation of filters h with hm (or g with gm) introduces a time shift of p1+p2-1, which is the
  return value multiplied by -1.

  If normmode==1, the results are normalized so that ||h||^2=||g||^2=1;
  if normmode==2, the results are normalized so that ||hm||^2=||gm||^2=1,
  if normmode==3, the results are normalized so that ||h||^2==||g||^2=||hm||^2=||gm||^2.

  If a *points* buffer is specified, the function returns the starting and ending
    positions (inclusive) of h, hm, g, and gm, in the order of (hs, he, hms, hme,
    gs, ge, gms, gme), as ps[0]~ps[7].

  In: p1 and p2, specify vanishing moments of h and hm, respectively.
      normmode: mode for normalization
  Out: h[p1+1], g[p1+1], hm[p1+2p2-1], gm[p1+2p2-1], points[8] (optional)

  Returns -p1-p2+1.
*/
int splinewl(int p1, int p2, double* h, double* hm, double* g, double* gm, int normmode, int* points)
{
  int lf=p1+1, lb=p1+2*p2-1;
  int hlf=lf/2, hlb=lb/2;

  double *h1=&h[hlf], *h2=&hm[hlb];
  int hp1=p1/2, hp2=p2/2;
  int q=(p1+p2)/2;
  h1[hp1]=sqrt(2.0)*pow(2, -p1);
//  h1[hp1]=2*pow(2, -p1);
  for (int i=1, j=hp1-1; i<=hp1; i++, j--) h1[j]=h1[j+1]*(p1+1-i)/i;

  double *_hh1=new double[p2+1+2*q];
  double *_hh2=&_hh1[p2+1];
  double *hh1=&_hh1[p2-hp2], *hh2=&_hh2[q];
  hh1[hp2]=sqrt(2.0)*pow(2, -p2);
//  hh1[hp2]=pow(2, -p2);
  for (int i=1, j=hp2-1; i<=hp2; i++, j--) hh1[j]=hh1[j+1]*(p2+1-i)/i;
  if (p2%2) for (int i=0; i<=hp2; i++) hh1[-i-1]=hh1[i];
  else for (int i=1; i<=hp2; i++) hh1[-i]=hh1[i];
  memset(_hh2, 0, sizeof(double)*2*q);
  for (int n=1-q; n<=q-1; n++)
  {
    int k=abs(n);
    int CC1=Cmb(q-1+k, k), CC2=Cmb(2*k, k-n);  //CC=1.0*C(q-1+k, k)*C(2*k, k-n)
    for (int k=abs(n); k<=q-1; k++)
    {
      hh2[n]=hh2[n]+1.0*CC1*CC2*pow(0.25, k);
      CC1=CC1*(q+k)/(k+1);
      CC2=CC2*(2*k+1)*(2*k+2)/((k+1-n)*(k+1+n));
    }
    hh2[n]*=pow(-1, n);
  }
  //hh1[hp2-p2:hp2], hh2[1-q:q-1]
  //h2=conv(hh1, hh2), but the positive half only
  memset(h2, 0, sizeof(double)*(hp1+p2));
  for (int i=hp2-p2; i<=hp2; i++) for (int j=1-q; j<=q-1; j++)
    if (i+j>=0 && i+j<hp1+p2) h2[i+j]+=hh1[i]*hh2[j];
  delete[] _hh1;

  int hs, he, hms, hme, gs, ge, gms, gme;
  if (lf%2)
  {
    hs=-hlf, he=hlf, hms=-hlb, hme=hlb;
    gs=-hlb+1, ge=hlb+1, gms=-hlf-1, gme=hlf-1;
  }
  else
  {
    hs=-hlf, he=hlf-1, hms=-hlb+1, hme=hlb;
    gs=-hlb, ge=hlb-1, gms=-hlf+1, gme=hlf;
  }

  if (lf%2)
  {
    for (int i=1; i<=hlf; i++) h1[-i]=h1[i];
    for (int i=1; i<=hlb; i++) h2[-i]=h2[i];
    double* _g=&g[hlb-1], *_gm=&gm[hlf+1];
    for (int i=gs; i<=ge; i++) _g[i]=(i%2)?h2[1-i]:-h2[1-i];
    for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?h1[-1-i]:-h1[-1-i];
  }
  else
  {
    for (int i=0; i<hlf; i++) h1[-i-1]=h1[i];
    for (int i=0; i<hlb; i++) h2[-i-1]=h2[i];
    h2=&h2[-1];
    double *_g=&g[hlb], *_gm=&gm[hlf-1];
    for (int i=gs; i<=ge; i++) _g[i]=(i%2)?-h2[-i]:h2[-i];
    for (int i=gms; i<=gme; i++) _gm[i]=(i%2)?-h1[-i]:h1[-i];
  }

  if (normmode)
  {
    double sumh=0; for (int i=0; i<=he-hs; i++) sumh+=h[i]*h[i];
    double sumhm=0; for (int i=0; i<=hme-hms; i++) sumhm+=hm[i]*hm[i];
    if (normmode==1)
    {
      double rr=sqrt(sumh);
      for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
      rr=1.0/rr;
      for (int i=0; i<=he-hs; i++) h[i]*=rr;
      rr=sqrt(sumhm);
      for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
      rr=1.0/rr;
      for (int i=0; i<=ge-gs; i++) g[i]*=rr;
    }
    else if (normmode==2)
    {
      double rr=sqrt(sumh);
      for (int i=0; i<=ge-gs; i++) g[i]*=rr;
      rr=1.0/rr;
      for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
      rr=sqrt(sumhm);
      for (int i=0; i<=he-hs; i++) h[i]*=rr;
      rr=1.0/rr;
      for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
    }
    else if (normmode==3)
    {
      double rr=pow(sumh/sumhm, 0.25);
      for (int i=0; i<=hme-hms; i++) hm[i]*=rr;
      for (int i=0; i<=ge-gs; i++) g[i]*=rr;
      rr=1.0/rr;
      for (int i=0; i<=he-hs; i++) h[i]*=rr;
      for (int i=0; i<=gme-gms; i++) gm[i]*=rr;
    }
  }

  if (points)
  {
    points[0]=hs, points[1]=he, points[2]=hms, points[3]=hme;
    points[4]=gs, points[5]=ge, points[6]=gms, points[7]=gme;
  }
  return -p1-p2+1;
}//splinewl

//---------------------------------------------------------------------------
/*
  function wavpacqmf: calculate pseudo local cosine transforms using wavelet packet

  In: data[Count], Count=fr*WID, waveform data
      WID: largest scale, equals 2^ORDER
      wid: smallest scale, euqals 2^order
      h[M], g[M]: quadratic mirror filter pair, fr>2*M
  Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid]

  No return value.
*/
void wavpacqmf(double*** spec, double* data, int Count, int WID, int wid, int M, double* h, double* g)
{
  int fr=Count/WID, ORDER=log2(WID), order=log2(wid);
  double* _data1=new double[Count*2];
  double *data1=_data1, *data2=&_data1[Count];
  //the qmf always filters data1 and puts the results to data2
  memcpy(data1, data, sizeof(double)*Count);
  int l=0, C=fr*WID, FR=1;
  double *ldata, *ldataa, *ldatad;
  while (l<ORDER)
  {
    //qmf
    for (int f=0; f<FR; f++)
    {
      ldata=&data1[f*C];
      if (f%2==0)
        ldataa=&data2[f*C], ldatad=&data2[f*C+C/2];
      else
        ldatad=&data2[f*C], ldataa=&data2[f*C+C/2];

      memset(&data2[f*C], 0, sizeof(double)*C);
      for (int i=0; i<C; i+=2)
      {
        int i2=i/2;
        ldataa[i2]=ldata[i]*h[0];
        ldatad[i2]=ldata[i]*g[0];
        for (int j=1; j<M; j++)
        {
          if (i+j<C)
          {
            ldataa[i2]+=ldata[i+j]*h[j];
            ldatad[i2]+=ldata[i+j]*g[j];
          }
          else
          {
            ldataa[i2]+=ldata[i+j-C]*h[j];
            ldatad[i2]+=ldata[i+j-C]*g[j];
          }
        }
      }
    }
    double *tmp=data2; data2=data1; data1=tmp;
    l++;
    C=(C>>1);
    FR=(FR<<1);
    if (l>=order)
    {
      for (int f=0; f<FR; f++)
        for(int i=0; i<C; i++)
          spec[ORDER-l][i][f]=data1[f*C+i];
    }
  }

  delete[] _data1;
}//wavpacqmf

/*
  function iwavpacqmf: inverse transform of wavpacqmf

  In: spec[Fr][Wid], Fr>M*2
      h[M], g[M], quadratic mirror filter pair
  Out: data[Fr*Wid]

  No return value.
*/
void iwavpacqmf(double* data, double** spec, int Fr, int Wid, int M, double* h, double* g)
{
  int Count=Fr*Wid, Order=log2(Wid);
  double* _data1=new double[Count];
  double *data1, *data2, *ldata, *ldataa, *ldatad;
  if (Order%2) data1=_data1, data2=data;
  else data1=data, data2=_data1;
  //data pass to buffer
  for (int i=0, t=0; i<Wid; i++)
    for (int j=0; j<Fr; j++)
      data1[t++]=spec[j][i];

  int l=Order;
  int C=Fr;
  int K=Wid/2;
  while (l>0)
  {
    memset(data2, 0, sizeof(double)*Count);
    for (int k=0; k<K; k++)
    {
      if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C];
      else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C];
      ldata=&data2[2*k*C];
      //qmf
      for (int i=0; i<C; i++)
      {
        for (int j=0; j<M; j++)
        {
          if (i*2+j<C*2)
          {
            ldata[i*2+j]+=ldataa[i]*h[j]+ldatad[i]*g[j];
          }
          else
          {
            ldata[i*2+j-C*2]+=ldataa[i]*h[j]+ldatad[i]*g[j];
          }
        }
      }
    }

    double *tmp=data2; data2=data1; data1=tmp;
    l--;
    C=(C<<1);
    K=(K>>1);
  }
  delete[] _data1;
}//iwavpacqmf

/*
  function wavpac: calculate pseudo local cosine transforms using wavelet packet,

  In: data[Count], Count=fr*WID, waveform data
      WID: largest scale, equals 2^ORDER
      wid: smallest scale, euqals 2^order
      h[hs:he-1], g[gs:ge-1]: filter pair
  Out: spec[0][fr][WID], spec[1][2*fr][WID/2], ..., spec[ORDER-order-1][FR][wid]

  No return value.
*/
void wavpac(double*** spec, double* data, int Count, int WID, int wid, double* h, int hs, int he, double* g, int gs, int ge)
{
  int fr=Count/WID, ORDER=log2(WID), order=log2(wid);
  double* _data1=new double[Count*2];
  double *data1=_data1, *data2=&_data1[Count];
  //the qmf always filters data1 and puts the results to data2
  memcpy(data1, data, sizeof(double)*Count);
  int l=0, C=fr*WID, FR=1;
  double *ldata, *ldataa, *ldatad;
  while (l<ORDER)
  {
    //qmf
    for (int f=0; f<FR; f++)
    {
      ldata=&data1[f*C];
      if (f%2==0)
        ldataa=&data2[f*C], ldatad=&data2[f*C+C/2];
      else
        ldatad=&data2[f*C], ldataa=&data2[f*C+C/2];

      memset(&data2[f*C], 0, sizeof(double)*C);
      for (int i=0; i<C; i+=2)
      {
        int i2=i/2;
        ldataa[i2]=0;//ldata[i]*h[0];
        ldatad[i2]=0;//ldata[i]*g[0];
        for (int j=hs; j<=he; j++)
        {
          int ind=i-j;
          if (ind>=C)
          {
            ldataa[i2]+=ldata[ind-C]*h[j];
          }
          else if (ind<0)
          {
            ldataa[i2]+=ldata[ind+C]*h[j];
          }
          else
          {
            ldataa[i2]+=ldata[ind]*h[j];
          }
        }
        for (int j=gs; j<=ge; j++)
        {
          int ind=i-j;
          if (ind>=C)
          {
            ldatad[i2]+=ldata[ind-C]*g[j];
          }
          else if (ind<0)
          {
            ldatad[i2]+=ldata[ind+C]*g[j];
          }
          else
          {
            ldatad[i2]+=ldata[ind]*g[j];
          }
        }
      }
    }
    double *tmp=data2; data2=data1; data1=tmp;
    l++;
    C=(C>>1);
    FR=(FR<<1);
    if (l>=order)
    {
      for (int f=0; f<FR; f++)
        for(int i=0; i<C; i++)
          spec[ORDER-l][i][f]=data1[f*C+i];
    }
  }

  delete[] _data1;
}//wavpac

/*
  function iwavpac: inverse transform of wavpac

  In: spec[Fr][Wid]
      h[hs:he-1], g[gs:ge-1], reconstruction filter pair
  Out: data[Fr*Wid]

  No return value.
*/
void iwavpac(double* data, double** spec, int Fr, int Wid, double* h, int hs, int he, double* g, int gs, int ge)
{
  int Count=Fr*Wid, Order=log2(Wid);
  double* _data1=new double[Count];
  double *data1, *data2, *ldata, *ldataa, *ldatad;
  if (Order%2) data1=_data1, data2=data;
  else data1=data, data2=_data1;
  //data pass to buffer
  for (int i=0, t=0; i<Wid; i++)
    for (int j=0; j<Fr; j++)
      data1[t++]=spec[j][i];

  int l=Order;
  int C=Fr;
  int K=Wid/2;
  while (l>0)
  {
    memset(data2, 0, sizeof(double)*Count);
    for (int k=0; k<K; k++)
    {
      if (k%2==0) ldataa=&data1[2*k*C], ldatad=&data1[(2*k+1)*C];
      else ldatad=&data1[2*k*C], ldataa=&data1[(2*k+1)*C];
      ldata=&data2[2*k*C];
      //qmf
      for (int i=0; i<C; i++)
      {
        for (int j=hs; j<=he; j++)
        {
          int ind=i*2+j;
          if (ind>=C*2)
          {
            ldata[ind-C*2]+=ldataa[i]*h[j];
          }
          else if (ind<0)
          {
            ldata[ind+C*2]+=ldataa[i]*h[j];
          }
          else
          {
            ldata[ind]+=ldataa[i]*h[j];
          }
        }
        for (int j=gs; j<=ge; j++)
        {
          int ind=i*2+j;
          if (ind>=C*2)
          {
            ldata[ind-C*2]+=ldatad[i]*g[j];
          }
          else if (ind<0)
          {
            ldata[ind+C*2]+=ldatad[i]*g[j];
          }
          else
          {
            ldata[ind]+=ldatad[i]*g[j];
          }
        }
      }
    }

    double *tmp=data2; data2=data1; data1=tmp;
    l--;
    C=(C<<1);
    K=(K>>1);
  }
  delete[] _data1;
}//iwavpac