libcm/vop/cmVectOpsTemplateCode.h

#ifdef cmVectOpsTemplateCode_h

void          VECT_OP_FUNC(VPrint)( cmRpt_t* rpt, const char* fmt, ... )
{
  va_list vl;
  va_start(vl,fmt);

  if( rpt != NULL )
    cmRptVPrintf(rpt,fmt,vl);
  else
    vprintf(fmt,vl);

  va_end(vl);
}

void          VECT_OP_FUNC(Printf)( cmRpt_t* rpt, unsigned rowCnt, unsigned colCnt, const VECT_OP_TYPE* sbp, unsigned fieldWidth, unsigned decPlCnt, const char* fmt, unsigned flags )
{
  unsigned cci;
  unsigned outColCnt = 10;

  if( fieldWidth < 0 )
    fieldWidth = 10;

  if( decPlCnt < 0 )
    decPlCnt = 4;

  if( outColCnt == -1 )
    outColCnt = colCnt;

  for(cci=0; cci<colCnt; cci+=outColCnt)
  {
    unsigned ci0 = cci;
    unsigned cn  = cci + outColCnt;
    unsigned ri;

    if(cn > colCnt)
      cn = colCnt;

    if( colCnt > outColCnt )
    {
      if( cmIsFlag(flags,cmPrintMatlabLabelsFl) )
        VECT_OP_FUNC(VPrint)(rpt,"Columns:%i to %i\n",ci0,cn-1);
      else
      if( cmIsFlag(flags,cmPrintShortLabelsFl) )
        VECT_OP_FUNC(VPrint)(rpt,"%3i: ",ci0);  
    }

    if( rowCnt > 1 )
      VECT_OP_FUNC(VPrint)(rpt,"\n");

    for(ri=0; ri<rowCnt; ++ri)
    {
      unsigned ci;

      for(ci=ci0; ci<cn; ++ci )
        VECT_OP_FUNC(VPrint)(rpt,fmt,fieldWidth,decPlCnt,sbp[ (ci*rowCnt) + ri ]);

      if( cn > 0 )
        VECT_OP_FUNC(VPrint)(rpt,"\n");
    }
  }
}

void          VECT_OP_FUNC(Print)( cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* sbp )
{ VECT_OP_FUNC(Printf)(rpt,rn,cn,sbp,cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*f ",cmPrintShortLabelsFl); }

void          VECT_OP_FUNC(PrintE)( cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* sbp )
{ VECT_OP_FUNC(Printf)(rpt,rn,cn,sbp,cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*e ",cmPrintShortLabelsFl); }

void          VECT_OP_FUNC(PrintLf)( const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp, unsigned fieldWidth, unsigned decPlCnt, const char* fmt )
{ 
  VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  VECT_OP_FUNC(Printf)( rpt, rn, cn, dbp, fieldWidth, decPlCnt,fmt,cmPrintShortLabelsFl );
}

void          VECT_OP_FUNC(PrintL)(  const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp )
{  
  VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  VECT_OP_FUNC(Printf)(  rpt, rn, cn, dbp, cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*f ",cmPrintShortLabelsFl );
}

void          VECT_OP_FUNC(PrintLE)( const char* label, cmRpt_t* rpt, unsigned rn, unsigned cn, const VECT_OP_TYPE* dbp )
{ 
  VECT_OP_FUNC(VPrint)( rpt, "%s\n", label );
  VECT_OP_FUNC(Printf)( rpt, rn, cn, dbp, cmDefaultFieldWidth,cmDefaultDecPlCnt,"%*.*e ",cmPrintShortLabelsFl );
}


VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbabilityVV)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
{
  VECT_OP_TYPE sum = VECT_OP_FUNC(Sum)(sbp,dn);  

  if( sum == 0 )
    sum = 1;

  return VECT_OP_FUNC(DivVVS)(dbp,dn,sbp,sum);
}

VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbability)(VECT_OP_TYPE* dbp, unsigned dn)
{ return VECT_OP_FUNC(NormalizeProbabilityVV)(dbp,dn,dbp); }

VECT_OP_TYPE* VECT_OP_FUNC(NormalizeProbabilityN)(VECT_OP_TYPE* dbp, unsigned dn, unsigned stride)
{
  VECT_OP_TYPE sum = VECT_OP_FUNC(SumN)(dbp,dn,stride);
  
  if( sum == 0 )
    return dbp;


  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + (dn*stride);
  for(;  dp < ep; dp+=stride )
    *dp /= sum;
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(StandardizeRows)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE* uV, VECT_OP_TYPE* sdV )
{
  bool uFl = false;
  bool sFl = false;
  unsigned i;

  if( uV == NULL )
  {
    uV = cmMemAllocZ(VECT_OP_TYPE,drn);
    uFl = true;
  }

  if( sdV == NULL )
  {
    sdV = cmMemAllocZ(VECT_OP_TYPE,drn);
    sFl = true;    
  }

  VECT_OP_FUNC(MeanM)(uV, dbp, drn, dcn, 1 );
  
  VECT_OP_FUNC(VarianceM)(sdV, dbp, drn, dcn, uV, 1 );

  for(i=0; i<dcn; ++i)
  {
    VECT_OP_FUNC(SubVV)(dbp + i * drn, drn,  uV );
    VECT_OP_FUNC(DivVV)(dbp + i * drn, drn, sdV );
  }

  if(uFl)
    cmMemFree(uV);

  if(sFl)
    cmMemFree(sdV);

  return dbp;
  
}

VECT_OP_TYPE* VECT_OP_FUNC(StandardizeCols)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE* uV, VECT_OP_TYPE* sdV )
{
  bool uFl = false;
  bool sFl = false;
  unsigned i;

  if( uV == NULL )
  {
    uV = cmMemAllocZ(VECT_OP_TYPE,dcn);
    uFl = true;
  }

  if( sdV == NULL )
  {
    sdV = cmMemAllocZ(VECT_OP_TYPE,dcn);
    sFl = true;    
  }

  VECT_OP_FUNC(MeanM)(uV, dbp, drn, dcn, 0 );
  
  VECT_OP_FUNC(VarianceM)(sdV, dbp, drn, dcn, uV, 0 );

  for(i=0; i<drn; ++i)
  {
    VECT_OP_FUNC(SubVVNN)(dbp + i, dcn,  drn,  uV, 1 );
    VECT_OP_FUNC(DivVVNN)(dbp + i, dcn,  drn, sdV, 1 );
  }

  if(uFl)
    cmMemFree(uV);

  if(sFl)
    cmMemFree(sdV);

  return dbp;
}


VECT_OP_TYPE* VECT_OP_FUNC(HalfWaveRectify)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dbp + dn;
  for(; dp < ep; ++dp,++sp )
    *dp = *sp < 0 ? 0 : *sp;

  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(CumSum)(VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
{
  VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE*  rp = dbp;
  VECT_OP_TYPE sum = 0;
  while( dbp < dep )
  {
    sum += *sbp++;
    *dbp++ = sum;

  }
  return rp;
}

VECT_OP_TYPE  VECT_OP_FUNC(Mean)( const VECT_OP_TYPE* bp, unsigned n )
{  return VECT_OP_FUNC(Sum)(bp,n)/n; }

VECT_OP_TYPE  VECT_OP_FUNC(MeanN)( const VECT_OP_TYPE* bp, unsigned n, unsigned stride )
{  return VECT_OP_FUNC(SumN)(bp,n,stride)/n; }

VECT_OP_TYPE*  VECT_OP_FUNC(MeanM)(    VECT_OP_TYPE* dp, const VECT_OP_TYPE* sp, unsigned srn, unsigned scn, unsigned dim )
{
  unsigned i;
  unsigned cn     = dim == 0 ? scn : srn;
  unsigned rn     = dim == 0 ? srn : scn;
  unsigned inc    = dim == 0 ? srn : 1;
  unsigned stride = dim == 0 ? 1   : srn;
  unsigned d0     = 0;

  for(i=0; i<cn; ++i, d0+=inc)
    dp[i] = VECT_OP_FUNC(MeanN)(sp + d0, rn, stride );

  return dp;
}

VECT_OP_TYPE*  VECT_OP_FUNC(Mean2)(   VECT_OP_TYPE* dp, const VECT_OP_TYPE* (*srcFuncPtr)(void* arg, unsigned idx ), unsigned D, unsigned N, void* argPtr )
{
  unsigned i,n;
  const VECT_OP_TYPE* sp;

  VECT_OP_FUNC(Zero)(dp,D);

  if( N > 1 )
  {
    n = 0;

    for(i=0; i<N; ++i)
      if((sp = srcFuncPtr(argPtr,i)) != NULL )
      {
        VECT_OP_FUNC(AddVV)(dp,D,sp);
        ++n;
      }

    VECT_OP_FUNC(DivVS)(dp,D,n);
  }

  return dp;
}

VECT_OP_TYPE  VECT_OP_FUNC(Variance)(  const VECT_OP_TYPE* sp, unsigned sn, const VECT_OP_TYPE* avgPtr )
{ return VECT_OP_FUNC(VarianceN)(sp,sn,1,avgPtr); }

VECT_OP_TYPE  VECT_OP_FUNC(VarianceN)(  const VECT_OP_TYPE* sp, unsigned sn, unsigned stride, const VECT_OP_TYPE* meanPtr )
{
  VECT_OP_TYPE mean = 0;

  if( sn <= 1 )
    return 0;

  if( meanPtr == NULL )
    mean = VECT_OP_FUNC(MeanN)( sp, sn, stride );
  else
    mean = *meanPtr;

  const VECT_OP_TYPE* ep = sp + (sn*stride);
  VECT_OP_TYPE sum = 0;
  for(; sp < ep; sp += stride )
    sum += (*sp-mean) * (*sp-mean);

  return sum / (sn-1);  
}

VECT_OP_TYPE*  VECT_OP_FUNC(VarianceM)(VECT_OP_TYPE* dp,  const VECT_OP_TYPE* sp, unsigned srn, unsigned scn, const VECT_OP_TYPE* avgPtr, unsigned dim )
{
  unsigned i;
  unsigned cn     = dim == 0 ? scn : srn;
  unsigned rn     = dim == 0 ? srn : scn;
  unsigned inc    = dim == 0 ? srn : 1;
  unsigned stride = dim == 0 ? 1   : srn;
  unsigned d0     = 0;

  for(i=0; i<cn; ++i, d0+=inc)
    dp[i] = VECT_OP_FUNC(VarianceN)(sp + d0, rn, stride, avgPtr==NULL ? NULL : avgPtr+i );

  return dp;  
}

unsigned  VECT_OP_FUNC(NormToMax)(    VECT_OP_TYPE* dp, unsigned dn )
{
  unsigned i = VECT_OP_FUNC(MaxIndex)(dp,dn,1);

  if( i != cmInvalidIdx )
  {  
    VECT_OP_TYPE v = dp[i];
    VECT_OP_FUNC(DivVS)(dp,dn,v);
  }

  return i;
}

VECT_OP_TYPE  VECT_OP_FUNC(AlphaNorm)(const VECT_OP_TYPE* sp, unsigned sn, VECT_OP_TYPE alpha )
{
  double sum = 0;
  const VECT_OP_TYPE* bp = sp;
  const VECT_OP_TYPE* ep = sp + sn;
  while( bp < ep )
    sum += pow(fabs(*bp++),alpha);

  return (VECT_OP_TYPE)pow(sum/sn,1.0/alpha);
}

void  VECT_OP_FUNC(GaussCovariance)(VECT_OP_TYPE* yM, unsigned D, const VECT_OP_TYPE* xM, unsigned xN, const VECT_OP_TYPE* uV, const unsigned* selIdxV, unsigned selKey )
{
  unsigned i,j,k,n = 0;
  VECT_OP_TYPE tV[ D ];
  
  VECT_OP_FUNC(Fill)(yM,D*D,0);

  // if the mean was not given - then calculate it
  if( uV == NULL )
  {
    VECT_OP_FUNC(Fill)(tV,D,0);

    // sum each row of xM[] into uM[]
    for(i=0; i<D; ++i)
    {
      n = 0;
      for(j=0; j<xN; ++j)
        if( selIdxV==NULL || selIdxV[j]==selKey )
        {
          tV[i] += xM[ (j*D) + i ];
          ++n;
        }
    }
    // form an average from the sum in tV[]
    VECT_OP_FUNC(DivVS)(tV,D,n);

    uV = tV;
  }

  for(i=0; i<D; ++i)
    for(j=i; j<D; ++j)
    {
      n = 0;

      for(k=0; k<xN; ++k)
        if( selIdxV==NULL || selIdxV[k]==selKey)
        {
          unsigned yi = (i*D)+j;
 
          yM[ yi ] += ((xM[ (k*D)+j ]-uV[j]) * (xM[ (k*D) + i ]-uV[i]));
          if( i != j )
            yM[ (j*D)+i ] = yM[ yi ];

          ++n;
        }
    }
  
  if( n>1 )
    VECT_OP_FUNC(DivVS)( yM, D*D, n-1 );

}

void  VECT_OP_FUNC(GaussCovariance2)(VECT_OP_TYPE* yM, unsigned D, const VECT_OP_TYPE* (*srcFunc)(void* userPtr, unsigned idx), unsigned xN, void* userPtr, const VECT_OP_TYPE* uV, const unsigned* selIdxV, unsigned selKey )
{
  unsigned i,j,k = 0,n;
  VECT_OP_TYPE tV[ D ];
  const VECT_OP_TYPE* sp;
  
  VECT_OP_FUNC(Fill)(yM,D*D,0);

  // if the mean was not given - then calculate it
  if( uV == NULL )
  {
    VECT_OP_FUNC(Fill)(tV,D,0);
    
    n = 0;

    // sum each row of xM[] into uM[]
    for(i=0; i<xN; ++i)
      if( (selIdxV==NULL || selIdxV[i]==selKey) && ((sp=srcFunc(userPtr,i))!=NULL) )
      {
        VECT_OP_FUNC(AddVV)(tV,D,sp);
        ++n;
      }

    // form an average from the sum in tV[]
    VECT_OP_FUNC(DivVS)(tV,D,n);

    uV = tV;
  }

  for(i=0; i<xN; ++i)
    if( selIdxV==NULL || selIdxV[i]==selKey )
    {
      // get a pointer to the ith data point
      const VECT_OP_TYPE* sV = srcFunc(userPtr,i);

      // note: this algorithm works because when a data point element (scalar) 
      // is multiplied by another data point element those two elements
      // are always part of the same data point (vector).  Two elements
      // from different data points are never multiplied.
      
      if( sV != NULL )
        for(j=0; j<D; ++j)
          for(k=j; k<D; ++k)
            yM[j + k*D] += (sV[j]-uV[j]) * (sV[k]-uV[k]);
    }

  if( n > 1 )
    VECT_OP_FUNC(DivVS)( yM, D*D, n-1 );

  // fill in the lower triangle
  for(j=0; j<D; ++j)
    for(k=j; k<D; ++k)
      yM[k + j*D] = yM[j + k*D];

}


bool          VECT_OP_FUNC(Equal)(    const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
{
  const VECT_OP_TYPE* ep = s0p + sn;
  while( s0p < ep )
    if( *s0p++ != *s1p++ )
      return false;
  return true;
}

bool          VECT_OP_FUNC(IsNormal)( const VECT_OP_TYPE* sp, unsigned sn )
{
  const VECT_OP_TYPE* ep = sp + sn;
  for(; sp<ep; ++sp)
    if( !isnormal(*sp) )
      return false;

  return true;
}

bool          VECT_OP_FUNC(IsNormalZ)(const VECT_OP_TYPE* sp, unsigned sn )
{
  const VECT_OP_TYPE* ep = sp + sn;
  for(; sp<ep; ++sp)
    if( (*sp != 0) && (!isnormal(*sp)) )
      return false;

  return true;
}

unsigned      VECT_OP_FUNC(FindNonNormal)( unsigned* dp, unsigned dn, const VECT_OP_TYPE* sbp )
{
  const VECT_OP_TYPE* sp = sbp;
  const VECT_OP_TYPE* ep = sp + dn;
  unsigned            n  = 0;

  for(; sp<ep; ++sp)
    if( !isnormal(*sp) )
      dp[n++] = sp - sbp;

  return n;

}

unsigned      VECT_OP_FUNC(FindNonNormalZ)( unsigned* dp, unsigned dn, const VECT_OP_TYPE* sbp )
{
  const VECT_OP_TYPE* sp = sbp;
  const VECT_OP_TYPE* ep = sp + dn;
  unsigned            n  = 0;

  for(; sp<ep; ++sp)
    if( (*sp!=0) && (!isnormal(*sp)) )
      dp[n++] = sp - sbp;

  return n;
}



unsigned      VECT_OP_FUNC(ZeroCrossCount)( const VECT_OP_TYPE* bp, unsigned bn, VECT_OP_TYPE* delaySmpPtr)
{
  unsigned n = delaySmpPtr != NULL ? ((*delaySmpPtr >= 0) != (*bp >= 0)) : 0 ;
  const VECT_OP_TYPE* ep = bp + bn;
  for(; bp<ep-1; ++bp)
    if( (*bp >= 0) != (*(bp+1) >= 0) )
      ++n;

  if( delaySmpPtr != NULL )
    *delaySmpPtr = *bp;

  return n;
}

VECT_OP_TYPE  VECT_OP_FUNC(RMS)( const VECT_OP_TYPE* bp, unsigned bn, unsigned wndSmpCnt )
{
  const VECT_OP_TYPE* ep = bp + bn;

  if( bn==0 )
    return 0;

  assert( bn <= wndSmpCnt );

  double sum = 0;
  for(; bp < ep; ++bp )
    sum += *bp * *bp;

  return (VECT_OP_TYPE)sqrt(sum/wndSmpCnt);
}

VECT_OP_TYPE* VECT_OP_FUNC(RmsV)( VECT_OP_TYPE* dp, unsigned dn, const VECT_OP_TYPE* sp, unsigned sn, unsigned wndSmpCnt, unsigned hopSmpCnt )
{
  const VECT_OP_TYPE* dep       = dp + dn;
  const VECT_OP_TYPE* sep       = sp + sn;
  VECT_OP_TYPE*       rp        = dp;

  for(; dp<dep && sp<sep; sp+=hopSmpCnt)
        *dp++ = VECT_OP_FUNC(RMS)( sp, cmMin(wndSmpCnt,sep-sp), wndSmpCnt );
  

  VECT_OP_FUNC(Zero)(dp,dep-dp);

  return rp;
}

VECT_OP_TYPE  VECT_OP_FUNC(EuclidNorm)( const VECT_OP_TYPE* sp, unsigned sn )
{ return (VECT_OP_TYPE)sqrt( VECT_OP_FUNC(MultSumVV)(sp,sp,sn)); }

/*
From:http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/doc/voicebox/distitpf.html
[nf1,p2]=size(pf1);
p1=p2-1;
nf2=size(pf2,1);
nx= min(nf1,nf2);
r = pf1(1:nx,:)./pf2(1:nx,:);
q = r-log(r);                            
s = sum( q(:,2:p1),2) + 0.5 * (q(:,1)+q(:,p2)) 
d= s/p1-1;

*/

VECT_OP_TYPE VECT_OP_FUNC(ItakuraDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
{
  VECT_OP_TYPE d = 0;

  VECT_OP_TYPE r[ sn ];
  VECT_OP_TYPE q[ sn ];

  // r = pf1(1:nx,:)./pf2(1:nx,:);
  VECT_OP_FUNC(DivVVV)(r,sn,s0p,s1p);

  //q=log(r);  
  VECT_OP_FUNC(LogV)(q,sn,r);

  //r = r - q = r - log(r)
  VECT_OP_FUNC(SubVV)(r,sn,q);

  //r = r - sn = r - log(r) - 1 
  VECT_OP_FUNC(SubVS)(r,sn,sn);

  // d  = sum(r);
  d = VECT_OP_FUNC(Sum)(r,sn);

  return (VECT_OP_TYPE)(d / sn);
  
  //d  = log( VECT_OP_FUNC(Sum)(r,sn) /sn );

  //d -= VECT_OP_FUNC(Sum)(q,sn)/sn;

  return d;
}

VECT_OP_TYPE  VECT_OP_FUNC(CosineDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
{
  VECT_OP_TYPE d0 = VECT_OP_FUNC(EuclidNorm)(s0p,sn);
  VECT_OP_TYPE d1 = VECT_OP_FUNC(EuclidNorm)(s1p,sn);

  if( d0 == 0 )
    d0 = cmReal_MIN;

  if( d1 == 0 )
    d1 = cmReal_MIN;
      
  return  (VECT_OP_TYPE)(VECT_OP_FUNC(MultSumVV)(s0p,s1p,sn) / (d0 * d1));  
}


VECT_OP_TYPE  VECT_OP_FUNC(EuclidDistance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
{
  double d = 0;

  const VECT_OP_TYPE* sep = s0p + sn;
  for(; s0p<sep; ++s0p,++s1p)
    d += (*s0p - *s1p) * (*s0p - *s1p);
  
  return (VECT_OP_TYPE)(sqrt(d));

}

VECT_OP_TYPE  VECT_OP_FUNC(L1Distance)( const VECT_OP_TYPE* s0p, const VECT_OP_TYPE* s1p, unsigned sn )
{
  double d = 0;

  const VECT_OP_TYPE* sep = s0p + sn;
  for(; s0p<sep; ++s0p,++s1p)
    d += (VECT_OP_TYPE)fabs(*s0p - *s1p);
  
  return d;
}

VECT_OP_TYPE  VECT_OP_FUNC(MahalanobisDistance)( const VECT_OP_TYPE* x, unsigned D, const VECT_OP_TYPE* u, const VECT_OP_TYPE* invCovM )
{
  VECT_OP_TYPE t[ D ];
  VECT_OP_TYPE d[ D ];

  // t[] = x[] - u[];
  VECT_OP_FUNC(SubVVV)(t,D,x,u);

  // d[1,D] = t[1,D] * covM[D,D]
  VECT_OP_FUNC(MultVVM)( d, D, t, D, invCovM );

  // d = sum(d[].*t[])
  VECT_OP_TYPE dist = VECT_OP_FUNC(MultSumVV)(d,t,D);  

  return (VECT_OP_TYPE)sqrt(dist);
}

VECT_OP_TYPE VECT_OP_FUNC(KL_Distance)( const VECT_OP_TYPE* up, const VECT_OP_TYPE* sp, unsigned sn )
{
  VECT_OP_TYPE v[ sn ];
  VECT_OP_FUNC(DivVVV)(v,sn,up,sp);  // v = up ./ sp
  VECT_OP_FUNC(LogV)(v,sn,v);        // v = log(v)
  VECT_OP_FUNC(MultVV)(v,sn,up);     // v *= up;
  return VECT_OP_FUNC(Sum)(v,sn);    // sum(v)
}

VECT_OP_TYPE VECT_OP_FUNC(KL_Distance2)( const VECT_OP_TYPE* up, const VECT_OP_TYPE* sp, unsigned sn )
{
  VECT_OP_TYPE v0[ sn ];
  VECT_OP_TYPE v1[ sn ];
  VECT_OP_FUNC(NormalizeProbabilityVV)(v0,sn,up);
  VECT_OP_FUNC(NormalizeProbabilityVV)(v1,sn,sp);
  return VECT_OP_FUNC(KL_Distance)(v0,v1,sn);
}

/// If dv[scn] is non NULL then return the Euclidean distance from sv[scn] to each column of sm[srn,scn].
/// The function returns the index of the closest data point (column) in sm[].
unsigned VECT_OP_FUNC(EuclidDistanceVM)( VECT_OP_TYPE* dv, const VECT_OP_TYPE* sv, const VECT_OP_TYPE* sm, unsigned srn, unsigned scn ) 
{

 unsigned             minIdx  = cmInvalidIdx;
 VECT_OP_TYPE         minDist = 0;
 unsigned             i       = 0;

  for(; i<scn; ++i )
  {
    VECT_OP_TYPE dist = VECT_OP_FUNC(EuclidDistance)(sv, sm + (i*srn), srn );

    if( dv != NULL )
     *dv++ = dist;

    if( dist < minDist || minIdx == cmInvalidIdx )
    {
      minIdx  = i;
      minDist = dist;
    }
  }

  return minIdx;
}

void VECT_OP_FUNC(DistVMM)( VECT_OP_TYPE* dM, VECT_OP_TYPE* mvV, unsigned* miV, unsigned rn, const VECT_OP_TYPE* s0M, unsigned s0cn, const VECT_OP_TYPE* s1M, unsigned s1cn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr )
{
  unsigned i,j,k;

  // for each col in s0M[];
  for(i=0,k=0; i<s0cn; ++i)
  {
    VECT_OP_TYPE min_val = VECT_OP_MAX;
    unsigned     min_idx = cmInvalidIdx;

    // for each col in s1M[]
    for(j=0; j<s1cn; ++j,++k)
    {
      // v = distance(s0M[:,i],s1M[:,j]
      VECT_OP_TYPE v = distFunc( userPtr, s1M + (j*rn), s0M + (i*rn), rn );

      if( dM != NULL )
        dM[k] = v;     // store distance

      // track closest col in s1M[]
      if( v < min_val || min_idx==cmInvalidIdx )
      {
        min_val = v;
        min_idx = j;
      }
    }

    if( mvV != NULL )
      mvV[i] = min_val;

    if( miV != NULL )
      miV[i] = min_idx;
  }
}

void VECT_OP_FUNC(SelectRandom) ( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn )
{
  bool freeFl = false;
  unsigned i;

  assert( selIdxN != 0 );

  // if no selIdxV[] was given then create one
  if( selIdxV == NULL )
  {
    selIdxV    = cmMemAlloc( unsigned, selIdxN );
    freeFl     = true;
  }

  // select  datapoints at random 
  cmVOU_UniqueRandom(selIdxV,selIdxN,scn);
  
  // copy the data points into the output matrix
  if( dM != NULL )
    for(i=0; i<selIdxN; ++i)
    {
      assert( selIdxV[i] < scn );

      VECT_OP_FUNC(Copy)( dM + (i*srn), srn, sM + selIdxV[i]*srn );
    }

  if( freeFl )
    cmMemPtrFree(&selIdxV);

}

void VECT_OP_FUNC(_SelectDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr, bool avgFl )
{
  unsigned i;
  unsigned dcn    = 0;
  bool     freeFl = false;

  assert( selIdxN > 0 );

  if( dM == NULL )
  {
    dM = cmMemAllocZ( VECT_OP_TYPE, srn*selIdxN );
    freeFl = true;
  }

  // allocate distM[scn,selIdxN] to hold the distances from each selected column to all columns in sM[]
  VECT_OP_TYPE* distM = cmMemAllocZ( VECT_OP_TYPE, scn*selIdxN );

  // sumV[] is a temp vector to hold the summed distances to from the selected columns to each column in sM[]
  VECT_OP_TYPE* sumV  = cmMemAllocZ( VECT_OP_TYPE, scn );

  // select a random point from sM[] and copy it to the first column of dM[]
  cmVOU_Random(&i,1,scn);
  VECT_OP_FUNC(Copy)(dM, srn, sM + (i*srn)); 
  
  if( selIdxV != NULL )
    selIdxV[0] = i;
  
  for(dcn=1; dcn<selIdxN; ++dcn)
  {
    // set distM[scn,dcn] with the dist from dM[dcn,srn] to each column in sM[]
    VECT_OP_FUNC(DistVMM)( distM, NULL, NULL, srn, dM, dcn, sM, scn, distFunc, userPtr );

    // sum the rows of distM[ scn, dcn ] into sumV[scn]
    VECT_OP_FUNC(SumMN)( distM, scn, dcn, sumV );

    if( avgFl )
      VECT_OP_FUNC(DivVS)( sumV, scn, dcn );
   
    // find the point in sM[] which has the greatest combined distance to all previously selected points.
    unsigned maxIdx = VECT_OP_FUNC(MaxIndex)(sumV, scn, 1 );

    // copy the point into dM[]
    VECT_OP_FUNC(Copy)(dM + (dcn*srn), srn, sM + (maxIdx*srn));   

    if( selIdxV != NULL )
      selIdxV[dcn] = maxIdx;
  }

  cmMemPtrFree(&distM);
  cmMemPtrFree(&sumV);

  if( freeFl )
    cmMemPtrFree(&dM);
}

void VECT_OP_FUNC(SelectMaxDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr )
{ VECT_OP_FUNC(_SelectDist)(dM,selIdxV,selIdxN,sM,srn,scn,distFunc,userPtr,false); }

void VECT_OP_FUNC(SelectMaxAvgDist)( VECT_OP_TYPE *dM, unsigned* selIdxV, unsigned selIdxN, const VECT_OP_TYPE* sM, unsigned srn, unsigned scn, VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ), void* userPtr ) 
{ VECT_OP_FUNC(_SelectDist)(dM,selIdxV,selIdxN,sM,srn,scn,distFunc,userPtr,true); }


#ifdef CM_VECTOP

VECT_OP_TYPE VECT_OP_FUNC(MultSumVV)( const VECT_OP_TYPE* s0p,  const VECT_OP_TYPE* s1p, unsigned sn )
{ return VECT_OP_BLAS_FUNC(dot)(sn, s0p, 1, s1p, 1); }

#else

VECT_OP_TYPE VECT_OP_FUNC(MultSumVV)( const VECT_OP_TYPE* s0p,  const VECT_OP_TYPE* s1p, unsigned sn )
{
  VECT_OP_TYPE sum = 0;
  const VECT_OP_TYPE* sep = s0p + sn;
  
  while(s0p<sep)
    sum += *s0p++ * *s1p++;

  return sum;
}
#endif

VECT_OP_TYPE VECT_OP_FUNC(MultSumVS)( const VECT_OP_TYPE* s0p, unsigned sn, VECT_OP_TYPE s1 )
{
  VECT_OP_TYPE sum = 0;
  const VECT_OP_TYPE* sep = s0p + sn;
  
  while(s0p<sep)
    sum += *s0p++ * s1;

  return sum;
}

#ifdef CM_VECTOP

VECT_OP_TYPE* VECT_OP_FUNC(MultVMV)( VECT_OP_TYPE* dbp, unsigned mrn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
{
  VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasNoTrans, mrn, mcn, 1.0, mp, mrn, vp, 1, 0.0, dbp, 1 );
  
  return dbp;
}

#else

VECT_OP_TYPE* VECT_OP_FUNC(MultVMV)( VECT_OP_TYPE* dbp, unsigned mrn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
{
  const VECT_OP_TYPE* dep = dbp + mrn;
  VECT_OP_TYPE*        dp  = dbp;
  const VECT_OP_TYPE*  vep = vp + mcn;

  // for each dest element
  for(; dbp < dep; ++dbp )
  {
    const VECT_OP_TYPE*  vbp = vp;
    const VECT_OP_TYPE*  mbp = mp++;

    *dbp = 0;

    // for each source vector row and src mtx col
    while( vbp < vep )
    {
      *dbp += *mbp * *vbp++;
       mbp += mrn;
    }
  } 

  return dp;
}
#endif


#ifdef CM_VECTOP

VECT_OP_TYPE* VECT_OP_FUNC(MultVVM)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* vp, unsigned vn, const VECT_OP_TYPE* mp )
{
  VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasTrans, vn, dn, 1.0, mp, vn, vp, 1, 0.0, dbp, 1 );
  return dbp;
}

#else

VECT_OP_TYPE* VECT_OP_FUNC(MultVVM)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* vp, unsigned vn, const VECT_OP_TYPE* mp )
{
  unsigned i; 
  for(i=0; i<dn; ++i)
    dbp[i] = VECT_OP_FUNC(MultSumVV)(vp,mp + (i*vn),vn);
  return dbp;
}
#endif


#ifdef CM_VECTOP

VECT_OP_TYPE* VECT_OP_FUNC(MultVMtV)( VECT_OP_TYPE* dbp, unsigned mcn, const VECT_OP_TYPE* mp, unsigned mrn, const VECT_OP_TYPE* vp )
{
  VECT_OP_BLAS_FUNC(gemv)( CblasColMajor, CblasTrans, mrn, mcn, 1.0, mp, mrn, vp, 1, 0.0, dbp, 1 );
  return dbp;
}

#else

VECT_OP_TYPE* VECT_OP_FUNC(MultVMtV)( VECT_OP_TYPE* dbp, unsigned mcn, const VECT_OP_TYPE* mp, unsigned mrn, const VECT_OP_TYPE* vp )
{
  const VECT_OP_TYPE*  dep = dbp + mcn;
  VECT_OP_TYPE*        dp  = dbp;
  const VECT_OP_TYPE*  vep = vp + mrn;

  // for each dest element
  for(; dbp < dep; ++dbp )
  {
    const VECT_OP_TYPE*  vbp = vp;

    *dbp = 0;

    // for each source vector row and src mtx col
    while( vbp < vep )
      *dbp += *mp++ * *vbp++;


  } 

  return dp;
}

#endif

VECT_OP_TYPE* VECT_OP_FUNC(MultDiagVMV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* mp, unsigned mcn, const VECT_OP_TYPE* vp )
{
  VECT_OP_TYPE* rp = dbp;

  const VECT_OP_TYPE* mep = mp + (dn*mcn);

  // for each dest element
  for(; mp < mep; mp += dn+1 )
    *dbp++ = *vp++ * *mp;

  return rp;  
}

/*
  Fortran Doc: http://www.netlib.org/blas/cgemm.f
  
  C Doc:  http://techpubs.sgi.com/library/tpl/cgi-bin/getdoc.cgi?cmd=getdoc&coll=0650&db=man&fname=3%20INTRO_CBLAS

  C = alpha * op(A) * op(B) + beta * C

  cblas_Xgemm(
  order,               enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102};
  transposeA,          enum CBLAS_TRANSPOSE { CblasNoTrans, CblasTrans, CBlasConjTrans }
  transposeB,
  M,                   row op(A) and rows C        (i.e. rows of A 'after' optional transpose)     
  N,                   col op(B) and cols C        (i.e. rows of B 'after' optional transpose)
  K,                   col op(A) and rows op(B)    
  alpha,               A scalar         
  A,                   pointer to source matrix A
  lda,                 number of rows in A as it is stored in memory (assuming col major order)
  B,                   pointer to source matrix B
  ldb,                 number of rows in B as it is stored in memory (assuming col major order)
  beta                 C scalar
  C,                   pointer to destination matrix C
  ldc                  number of rows in C as it is stored in memory  (assuming col major order)
  )  
  
 */

#ifdef CM_VECTOP
VECT_OP_TYPE* VECT_OP_FUNC(MultMMM1)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE alpha, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n, VECT_OP_TYPE beta, unsigned flags )
{
  bool t0fl = cmIsFlag(flags,kTransposeM0Fl);
  bool t1fl = cmIsFlag(flags,kTransposeM1Fl);

  VECT_OP_BLAS_FUNC(gemm)( 
    CblasColMajor, 
    t0fl ? CblasTrans : CblasNoTrans, 
    t1fl ? CblasTrans : CblasNoTrans, 
    drn, dcn, n, 
    alpha, 
    m0,  t0fl ? n   : drn, 
    m1,  t1fl ? dcn : n, 
    beta, 
    dbp, drn );

  return dbp;

}
#else

// Not implemented.

#endif

#ifdef CM_VECTOP
VECT_OP_TYPE* VECT_OP_FUNC(MultMMM2)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, VECT_OP_TYPE alpha, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n, VECT_OP_TYPE beta, unsigned flags, unsigned dprn, unsigned m0prn, unsigned m1prn )
{

  VECT_OP_BLAS_FUNC(gemm)( 
    CblasColMajor, 
    cmIsFlag(flags,kTransposeM0Fl) ? CblasTrans : CblasNoTrans, 
    cmIsFlag(flags,kTransposeM1Fl) ? CblasTrans : CblasNoTrans, 
    drn, dcn, n, 
    alpha, 
    m0,  m0prn, 
    m1,  m1prn, 
    beta, 
    dbp, dprn );

  return dbp;
}
#else

// Not implemented.

#endif


#ifdef CM_VECTOP

VECT_OP_TYPE* VECT_OP_FUNC(MultMMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned n )
{
  VECT_OP_BLAS_FUNC(gemm)( 
    CblasColMajor, 
    CblasNoTrans, CblasNoTrans, 
    drn, dcn, n, 
    1.0, m0,  drn, 
         m1,  n, 
    0.0, dbp, drn );
  return dbp;
}

#else

VECT_OP_TYPE* VECT_OP_FUNC(MultMMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
{
  unsigned i;

  for(i=0; i<dcn; ++i)
    VECT_OP_FUNC(MultVMV)(dbp+(i*drn),drn,m0,m0cn_m1rn,m1+(i*m0cn_m1rn));

  return dbp;
}

#endif

#ifdef CM_VECTOP
VECT_OP_TYPE* VECT_OP_FUNC(MultMMMt)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
{
  VECT_OP_BLAS_FUNC(gemm)( CblasColMajor, CblasNoTrans, CblasTrans, 
    drn, dcn, m0cn_m1rn, 
    1.0, m0, drn, 
         m1, dcn, 
    0.0, dbp, drn );

  return dbp;
}
#else
VECT_OP_TYPE* VECT_OP_FUNC(MultMMMt)(VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* m0, const VECT_OP_TYPE* m1, unsigned m0cn_m1rn )
{
  unsigned i,j,k;
  VECT_OP_FUNC(Zero)(dbp,drn*dcn);

  for(i=0; i<dcn; ++i)
    for(j=0; j<drn; ++j)
      for(k=0; k<m0cn_m1rn; ++k)
        dbp[ i*drn + j ] += m0[ k*drn + j ] * m1[ k*dcn + i ];

  return dbp;
}
#endif

VECT_OP_TYPE* VECT_OP_FUNC(PowVS)(  VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE expo )
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + dn;
  for(; dp < ep; ++dp )
    *dp = (VECT_OP_TYPE)pow(*dp,expo);

  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(PowVVS)(  VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE expo )
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + dn;
  for(; dp < ep; ++dp,++sp )
    *dp = (VECT_OP_TYPE)pow(*sp,expo);

  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(LogV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp )
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + dn;
  for(; dp <ep; ++dp,++sbp)
    *dp = (VECT_OP_TYPE)log(*sbp);
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(AmplToDbVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE minDb )
{
  VECT_OP_TYPE  minVal = pow(10.0,minDb/20.0);
  VECT_OP_TYPE* dp     = dbp;
  VECT_OP_TYPE* ep     = dp + dn;

  for(; dp<ep; ++dp,++sbp)
    *dp = *sbp<minVal ? minDb : 20.0 * log10(*sbp);
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(DbToAmplVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + dn;
  for(; dp<ep; ++dp,++sbp)
    *dp = pow(10.0,*sbp/20.0);
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(PowToDbVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE minDb )
{
  VECT_OP_TYPE  minVal = pow(10.0,minDb/10.0);
  VECT_OP_TYPE* dp     = dbp;
  VECT_OP_TYPE* ep     = dp + dn;

  for(; dp<ep; ++dp,++sbp)
    *dp = *sbp<minVal ? minDb : 10.0 * log10(*sbp);
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(DbToPowVV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp)
{
  VECT_OP_TYPE* dp = dbp;
  VECT_OP_TYPE* ep = dp + dn;
  for(; dp<ep; ++dp,++sbp)
    *dp = pow(10.0,*sbp/10.0);
  return dbp;
}


VECT_OP_TYPE* VECT_OP_FUNC(RandSymPosDef)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE* t )
{
  unsigned i,j;

  bool fl = t == NULL;
  
  if( fl )
    t = cmMemAlloc( VECT_OP_TYPE , dn*dn );

  do
  {
    // intialize t[] as a square symetric matrix with random values
    for(i=0; i<dn; ++i)
      for(j=i; j<dn; ++j)
      {
        VECT_OP_TYPE v = (VECT_OP_TYPE)rand()/RAND_MAX;

        t[ (i*dn) + j ] = v;

        if( i != j )
          t[ (j*dn) + i ] = v;
      }


    // square t[] to force the eigenvalues to be positive
    VECT_OP_FUNC(MultMMM)(dbp,dn,dn,t,t,dn);

    VECT_OP_FUNC(Copy)(t,dn*dn,dbp);

    // test that func is positive definite
  }while( VECT_OP_FUNC(Chol)(t,dn)==NULL );


  if( fl )
    cmMemFree(t);

  return dbp;  
}


// Calculate the determinant of a matrix previously factored by
// the lapack function dgetrf_()
VECT_OP_TYPE VECT_OP_FUNC(LUDet)( const VECT_OP_TYPE* lu, const int_lap_t* ipiv, int rn )
{
  VECT_OP_TYPE  det1 = 1;
  int           det2 = 0;
  int           i;

  for(i=0; i<rn; ++i)
  {
    if( ipiv != NULL && ipiv[i] != (i+1) )
      det1 = -det1;

    det1 = lu[ (i*rn) + i ] * det1;

    if( det1 == 0 )
      break;

    while( fabs(det1) <= 1 )
    {
      det1 *= 10;
      det2 -= 1;
    }
    //continue;

    while( fabs(det1) >= 10 )
    {
      det1 /= 10;
      det2 += 1;
    }      
  }

  // Here's where underflow or overflow might happen.
  // Enable floating point exception handling to trap.
  det1 *=  pow(10.0,det2);

  return det1;
}

// take the inverse of a matrix factored via lapack dgetrf_()
VECT_OP_TYPE* VECT_OP_FUNC(LUInverse)(VECT_OP_TYPE* dp, int_lap_t* ipiv, int drn )
{

  int_lap_t ispec = 1;
  int_lap_t rn    = drn;
  int_lap_t n1    = drn;
  int_lap_t n2    = drn;
  int_lap_t n3    = drn;
  int_lap_t n4    = drn;


  char funcNameStr[] = {"DGETRI"};

  // Calculate the NB factor for LWORK - 
  // The two args are length of string args 'funcNameStr' and ' '.
  // It is not clear how many 'n' args are requred so all are passed set to 'drn'
  int nb = ilaenv_(&ispec, funcNameStr, " ", &n1,&n2,&n3,&n4, strlen(funcNameStr), 1 );

  VECT_OP_TYPE w[drn * nb];    // allocate working memory
  int_lap_t    info;

  // calculate inv(A) base on LU factorization 
  VECT_OP_LAP_FUNC(getri_)(&rn,dp,&rn,ipiv,w,&rn,&info);

  assert(info==0);

  return info ==0 ? dp : NULL;
  
}

VECT_OP_TYPE  VECT_OP_FUNC(DetM)( const VECT_OP_TYPE* sp, unsigned srn )
{
  int_lap_t arn = srn;
  VECT_OP_TYPE A[ arn * arn ];
  int_lap_t ipiv[ arn ];
  int_lap_t info;

  VECT_OP_FUNC(Copy)(A,arn*arn,sp);

  // PLU factor
  VECT_OP_LAP_FUNC(getrf_)(&arn,&arn,A,&arn,ipiv,&info);

  if( info == 0 )
    return VECT_OP_FUNC(LUDet)(A,ipiv,arn);

  return 0;
}

VECT_OP_TYPE  VECT_OP_FUNC(DetDiagM)( const VECT_OP_TYPE* sp, unsigned srn )
{ return VECT_OP_FUNC(LUDet)(sp,NULL,srn); }


VECT_OP_TYPE  VECT_OP_FUNC(LogDetM)( const VECT_OP_TYPE* sp, unsigned srn )
{
  cmReal_t            det    = 0;
  unsigned            ne2    = srn * srn;

  VECT_OP_TYPE        U[ne2];
  const VECT_OP_TYPE* up     = U;
  const VECT_OP_TYPE* ep     = up  + ne2;

  VECT_OP_FUNC(Copy)(U,ne2,sp);
  VECT_OP_FUNC(Chol)(U,srn);

  for(; up<ep; up += (srn+1) )
    det += log(*up);

  return 2*det;
}

VECT_OP_TYPE  VECT_OP_FUNC(LogDetDiagM)( const VECT_OP_TYPE* sp, unsigned srn )
{ return log(VECT_OP_FUNC(DetDiagM)(sp,srn)); }

VECT_OP_TYPE*  VECT_OP_FUNC(InvM)( VECT_OP_TYPE* dp, unsigned drn )
{
  int_lap_t rn = drn;
  int_lap_t ipiv[ rn ];
  int_lap_t info;

  // PLU factor
  VECT_OP_LAP_FUNC(getrf_)(&rn,&rn,dp,&rn,ipiv,&info);

  if( info == 0 )
    return VECT_OP_FUNC(LUInverse)(dp,ipiv,rn );

  return NULL;
}

VECT_OP_TYPE* VECT_OP_FUNC(InvDiagM)(    VECT_OP_TYPE* dp, unsigned drn )
{
  const VECT_OP_TYPE* dep = dp + (drn*drn);
  VECT_OP_TYPE*       rp  = dp;

  for(; dp < dep; dp += drn+1 )
  {
    *dp = 1.0 / *dp;

    // if any element on the diagonal is zero then the 
    // determinant is zero and the matrix is not invertable
    if( *dp == 0 )
      break;
  }

  return dp < dep ? NULL : rp;
}


VECT_OP_TYPE* VECT_OP_FUNC(SolveLS)( VECT_OP_TYPE* A, unsigned an, VECT_OP_TYPE* B, unsigned bcn )
{
  int_lap_t aN   = an;
  int_lap_t bcN  = bcn;
  int_lap_t ipiv[ an ];
  int_lap_t info = 0;

  VECT_OP_LAP_FUNC(gesv_)(&aN,&bcN,(VECT_OP_TYPE*)A,&aN,ipiv,B,&aN,&info); 
  
  return info == 0 ? B : NULL;
}

VECT_OP_TYPE* VECT_OP_FUNC(Chol)(VECT_OP_TYPE* A, unsigned an )
{
  char      uplo = 'U'; 

  int_lap_t N    = an;
  int_lap_t lda  = an;
  int_lap_t info = 0;
  
  VECT_OP_LAP_FUNC(potrf_(&uplo,&N,(VECT_OP_TYPE*)A,&lda,&info));

  return info == 0 ? A : NULL;
}

VECT_OP_TYPE* VECT_OP_FUNC(CholZ)(VECT_OP_TYPE* A, unsigned an )
{
  unsigned i,j;
  VECT_OP_FUNC(Chol)(A,an);

  // zero the lower triangle of A
  for(i=0; i<an; ++i)
    for(j=i+1; j<an; ++j)
      A[ (i*an) + j ] = 0;

  return A;
}

VECT_OP_TYPE VECT_OP_FUNC(FracAvg)( double bi, double ei, const VECT_OP_TYPE* sbp, unsigned sn )
{
  unsigned bii   = cmMax(0,cmMin(sn-1,(unsigned)ceil(bi)));
  unsigned eii   = cmMax(0,cmMin(sn,(unsigned)floor(ei)+1));

  double   begW  = bii - bi;
  double   endW  = eii - floor(ei);
  
  double   cnt   = eii - bii;

  double   sum   = (double)VECT_OP_FUNC(Sum)(sbp+bii,eii-bii);

  if( begW>0 && bii > 0 )
  {
    cnt += begW;
    sum += begW * sbp[ bii-1 ];
  }

  if( endW>0 && eii+1 < sn )
  {
    cnt += endW;
    sum += endW * sbp[ eii+1 ];

  }

  return (VECT_OP_TYPE)(sum / cnt);
  
  
}

VECT_OP_TYPE* VECT_OP_FUNC(DownSampleAvg)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
{
  const VECT_OP_TYPE* dep  = dbp + dn;
  VECT_OP_TYPE*       rp   = dbp;
  unsigned            i    = 0;
  double              fact = (double)sn / dn;

  assert( sn >= dn );

  for(i=0; dbp < dep; ++i )
    *dbp++ = VECT_OP_FUNC(FracAvg)( fact*i, fact*(i+1), sbp, sn );

  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(UpSampleInterp)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  const VECT_OP_TYPE* sep = sbp + sn;
  VECT_OP_TYPE* rp   = dbp;
  double        fact = (double)sn / dn;
  double        phs  = 0;

  assert( sn <= dn );
		
  while( dbp<dep )
  {
    if( sbp < sep )
      *dbp++ =  (VECT_OP_TYPE)((*sbp) + (phs * ((*(sbp+1)) - (*sbp))));
    else
      *dbp++ = (*(sep-1));
			
    phs += fact;
			
    while( phs > 1.0 )
    {
      phs -= 1.0;
      sbp++;
    }
  }		

  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(FitToSize)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn )
{
  if( dn == sn )
    return VECT_OP_FUNC(Copy)(dbp,dn,sbp);

  if( dn < sn )
    return VECT_OP_FUNC(DownSampleAvg)(dbp,dn,sbp,sn);
  
  return VECT_OP_FUNC(UpSampleInterp)(dbp,dn,sbp,sn);
}

VECT_OP_TYPE* VECT_OP_FUNC(LinearMap)(VECT_OP_TYPE* dV, unsigned dn, VECT_OP_TYPE* sV, unsigned sn )
{
  if( dn == sn )
  {
    memcpy(dV,sV,dn*sizeof(VECT_OP_TYPE));
    return dV;
  }
  
  unsigned     i,j,k;

  // if stretching
  if( dn > sn )
  {
    VECT_OP_TYPE f_n  = (VECT_OP_TYPE)dn/sn;
    VECT_OP_TYPE f_nn = f_n;
    unsigned     i_n  = floor(f_n);

    k = 0;
    i = 0;
  
    // for each set of ceiling(dn/sn) dst values
    while(1)
    {
      // repeat floor(dn/sn) src val into dst
      for(j=0; j<i_n; ++j,++i)
        dV[i] = sV[k];

      if( k + 1 == sn )
        break;

      // interpolate between the cur and nxt source value
      VECT_OP_TYPE w = f_nn - floor(f_nn);
      dV[i] = sV[k] + w * (sV[k+1]-sV[k]);
      ++i;
      ++k;

      i_n   = floor(f_n - (1.0-w));
      f_nn += f_n;
    }
  } 
  else // if shrinking
  {
    VECT_OP_TYPE f_n  = (VECT_OP_TYPE)sn/dn;
    VECT_OP_TYPE f_nn = f_n;
    unsigned     i_n  = floor(f_n);

    k = 0;
    i = 0;

    VECT_OP_TYPE acc = 0;
  
    // for each seq of ceil(sn/dn) src values
    while(1)
    {
      // accum first floor(sn/dn) src values
      for(j=0; j<i_n; ++j,++i)
        acc += sV[i];

      if( k == dn-1 )
      {
        dV[k] = acc/f_n;
        break;
      }

      // interpolate frac of last src value
      VECT_OP_TYPE w = f_nn - floor(f_nn);

      // form avg
      dV[k] = (acc + (w*sV[i]))/f_n;


      // reload acc with inverse frac of src value
      acc = (1.0-w) * sV[i];

      ++i;
      ++k;

      i_n   = floor(f_n-(1.0-w));
      f_nn += f_n;

    }
  }

  return dV;
}


VECT_OP_TYPE* VECT_OP_FUNC(Random)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE minVal, VECT_OP_TYPE maxVal )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* dp =dbp;
  double fact = (maxVal - minVal)/RAND_MAX;

  while( dbp < dep )
    *dbp++ = fact *  rand() + minVal;

  return dp;
}

unsigned*      VECT_OP_FUNC(WeightedRandInt)( unsigned *dbp, unsigned dn, const VECT_OP_TYPE* wp, unsigned wn )
{
  unsigned i,j;
  VECT_OP_TYPE a[ wn ];

  // form  bin boundaries by taking a cum. sum of the weight values.
  VECT_OP_FUNC(CumSum)(a,wn,wp);

  for(j=0; j<dn; ++j)
  {
    // gen a random number from a uniform distribution betwen 0 and the max value from the cumsum.
    VECT_OP_TYPE rv = (VECT_OP_TYPE)rand() * a[wn-1] / RAND_MAX;

    // find the bin the rv falls into 
    for(i=0; i<wn-1; ++i)
      if( rv <= a[i] )
      {
        dbp[j] = i;
        break;
      }

    if(i==wn-1)
      dbp[j]= wn-1;  
  }

  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGauss)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE mean, VECT_OP_TYPE var )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* rp = dbp;

  // The code below implements the Box-Muller uniform to 
  // Gaussian distribution transformation.  In rectangular 
  // coordinates this transform is defined as:
  //  y1 = sqrt( - 2.0 * log(x1) ) * cos( 2.0*M_PI*x2 )
  //  y2 = sqrt( - 2.0 * log(x1) ) * sin( 2.0*M_PI*x2 )
  //

  while( dbp < dep )
    *dbp++ = sqrt( -2.0 * log((VECT_OP_TYPE)rand()/RAND_MAX)) * cos(2.0*M_PI*((VECT_OP_TYPE)rand()/RAND_MAX)) * var + mean;

  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussV)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* varV )
{
  VECT_OP_TYPE* rp = dbp;
  const VECT_OP_TYPE* dep = dbp + dn;
  while( dbp < dep )
    VECT_OP_FUNC(RandomGauss)( dbp++, 1, *meanV++, *varV++ );
  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussM)( VECT_OP_TYPE* dbp, unsigned rn, unsigned cn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* varV )
{
  unsigned i;
  for(i=0; i<cn; ++i)
    VECT_OP_FUNC(RandomGaussV)( dbp+(i*rn), rn, meanV, varV );
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussDiagM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* covarM )
{
  unsigned i,j;
  for(i=0; i<dcn; ++i)
    for(j=0; j<drn; ++j)
      VECT_OP_FUNC(RandomGauss)(dbp + (i*drn)+j, 1, meanV[j], covarM[ (j*drn) + j]);
  return dbp;  
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussNonDiagM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* covarM, VECT_OP_TYPE* t )
{

  bool     fl = t == NULL;
  if( fl  )
    t = cmMemAlloc(VECT_OP_TYPE,  drn * drn );

  VECT_OP_FUNC(Copy)(t,drn*drn,covarM);

  if( VECT_OP_FUNC(CholZ)(t,drn) == NULL )
  {
    // Cholesky decomposition failed - should try eigen analysis next
    // From octave mvnrnd.m
    // [E,Lambda]=eig(Sigma);
	  // if (min(diag(Lambda))<0),error('Sigma must be positive semi-definite.'),end
	  // U = sqrt(Lambda)*E';

    assert(0);
  }
  /*
  unsigned i,j;
  for(i=0; i<drn; ++i)
  {
    for(j=0; j<drn; ++j)
      printf("%f ",t[ (j*drn) + i]);
    printf("\n");
  }     
  */

  VECT_OP_FUNC(RandomGaussNonDiagM2)(dbp,drn,dcn,meanV,t);

  if(fl)
    cmMemFree(t);

  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussNonDiagM2)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* uM )
{  
  unsigned i;

  for(i=0; i<dcn; ++i)
  {
    VECT_OP_TYPE r[ drn ];
    VECT_OP_FUNC(RandomGauss)(r,drn,0,1);             // r = randn(drn,1);
    VECT_OP_FUNC(MultVVM)( dbp+(i*drn),drn,r,drn,uM); // dbp[:i] = r * uM;
    VECT_OP_FUNC(AddVV)(   dbp+(i*drn),drn,meanV);    // dbp[:,i] += meanV;
  }

  return dbp;
}


VECT_OP_TYPE* VECT_OP_FUNC(RandomGaussMM)( VECT_OP_TYPE* dbp, unsigned drn, unsigned dcn, const VECT_OP_TYPE* meanM, const VECT_OP_TYPE* varM, unsigned K )
{
  unsigned k;
  unsigned D = drn;
  unsigned N = dcn/K;
  for(k=0; k<K; ++k)
    VECT_OP_FUNC(RandomGaussM)( dbp + (k*N*D), drn, N, meanM + (k*D), varM + (k*D) );
  return dbp;
}

VECT_OP_TYPE* VECT_OP_FUNC(CircleCoords)( VECT_OP_TYPE* dbp, unsigned dn, VECT_OP_TYPE x, VECT_OP_TYPE y, VECT_OP_TYPE varX, VECT_OP_TYPE varY  )
{
  unsigned i;
  for(i=0; i<dn; ++i)
  {
    double a = 2.0*M_PI*i/(dn-1);

    dbp[ i ]    = (VECT_OP_TYPE)(varX * cos(a) + x);
    dbp[ i+dn ] = (VECT_OP_TYPE)(varY * sin(a) + y);
  }

  return dbp;
}


unsigned      VECT_OP_FUNC(SynthSine)(      VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
{
    const VECT_OP_TYPE* dep = dbp + dn;
  double rps = 2.0*M_PI*hz/srate;

  while( dbp < dep )
    *dbp++ = (VECT_OP_TYPE)sin( rps * phase++ );

  return phase;
}

unsigned      VECT_OP_FUNC(SynthCosine)(    VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  double rps = 2.0*M_PI*hz/srate;

  while( dbp < dep )
    *dbp++ = (VECT_OP_TYPE)cos( rps * phase++ );

  return phase;
}

unsigned      VECT_OP_FUNC(SynthSquare)(    VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
{
  const VECT_OP_TYPE* dep = dbp + dn;

  if( otCnt > 0 )
  {
    unsigned i;

    // initialize the buffer with the fundamental
    VECT_OP_FUNC(SynthSine)( dbp, dn, phase, srate, hz );

    otCnt *= 2;

    // sum in each additional harmonic
    for(i=3; i<otCnt; i+=2)
    {

      VECT_OP_TYPE* dp  = dbp;      
      double        rps = 2.0 * M_PI * i  * hz / srate;
      unsigned      phs = phase;
      double        g   = 1.0/i;

      while( dp < dep )
        *dp++ += (VECT_OP_TYPE)(g * sin( rps * phs++ ));

    }
  }
  return phase + (dep - dbp);
}

unsigned      VECT_OP_FUNC(SynthTriangle)(  VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  if( otCnt > 0 )
  {
    unsigned i;

    // initialize the buffer with the fundamental
    VECT_OP_FUNC(SynthCosine)( dbp, dn, phase, srate, hz );

    otCnt *= 2;

    // sum in each additional harmonic
    for(i=3; i<otCnt; i+=2)
    {

      VECT_OP_TYPE* dp  = dbp;      
      double        rps = 2.0 * M_PI * i  * hz / srate;
      unsigned      phs = phase;
      double        g   = 1.0/(i*i);
      while( dp < dep )
        *dp++ += (VECT_OP_TYPE)(g * cos( rps * phs++ ));
    }
  }
  return phase + (dep - dbp);
}

unsigned      VECT_OP_FUNC(SynthSawtooth)(  VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  if( otCnt > 0 )
  {
    unsigned i;

    // initialize the buffer with the fundamental
    VECT_OP_FUNC(SynthSine)( dbp, dn, phase, srate, hz );

    // sum in each additional harmonic
    for(i=2; i<otCnt; ++i)
    {

      VECT_OP_TYPE* dp  = dbp;      
      double        rps = 2.0 * M_PI * i  * hz / srate;
      unsigned      phs = phase;
      double        g   = 1.0/i;
      
      while( dp < dep )
        *dp++ += (VECT_OP_TYPE)(g * sin( rps * phs++ ));
    }

    VECT_OP_FUNC(MultVS)(dbp,dn,2.0/M_PI);
  }
  return phase + (dep - dbp);
}

unsigned      VECT_OP_FUNC(SynthPulseCos)(  VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz, unsigned otCnt )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  if( otCnt > 0 )
  {
    unsigned i;


    // initialize the buffer with the fundamental
    VECT_OP_FUNC(SynthCosine)( dbp, dn, phase, srate, hz );

    // sum in each additional harmonic
    for(i=1; i<otCnt; ++i)
    {

      VECT_OP_TYPE* dp  = dbp;      
      double        rps = 2.0 * M_PI * i  * hz / srate;
      unsigned      phs = phase;
      
      while( dp < dep )
        *dp++ += (VECT_OP_TYPE)cos( rps * phs++ );
    }

    VECT_OP_FUNC(MultVS)(dbp,dn,1.0/otCnt);
  }
  return phase + (dep - dbp);
}

unsigned      VECT_OP_FUNC(SynthImpulse)(   VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  double pi2 = 2.0*M_PI;
  double rps = pi2*hz/srate;
  
  double v0,v1 = fmod( rps * phase, pi2);

  if( dbp == dep )
    return phase;

  // the phase is set to zero when the first output should be a 1
  if( phase == 0 )
  {
    *dbp++ = 1;
    ++phase;
  }

  
  while( dbp < dep )
  {
    // the phase vector will always be increasing
    // the modulus of the phase vector will wrap with frequency 'hz'
    v0 = fmod(  rps * phase++, pi2 );

    // notice when wrapping occurs
    *dbp++ = (VECT_OP_TYPE)(v0 < v1);

    v1 = v0;
  }

  // check if the next output should be a 1 
  // (this eliminates the problem of not having access to v1 on the next call to this function
  if( fmod(  rps * phase, pi2 ) < v1 )
    phase = 0;
  
  
  return phase;
}

VECT_OP_TYPE VECT_OP_FUNC(SynthPinkNoise)( VECT_OP_TYPE* dbp, unsigned n, VECT_OP_TYPE delaySmp )
{
  const VECT_OP_TYPE* dep = dbp + n;
  VECT_OP_TYPE tmp[ n ];
  VECT_OP_FUNC(Random)(tmp,n,-1.0,1.0);
  VECT_OP_TYPE* sp = tmp;
  VECT_OP_TYPE  reg = delaySmp;

  for(; dbp < dep; ++sp)
  {
    *dbp++ = (*sp + reg)/2.0;
    reg = *sp;
  }
  return *sp;
}

VECT_OP_TYPE* VECT_OP_FUNC(LinearToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE mult )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* rp = dbp;
  while( dbp < dep )
    *dbp++ = (VECT_OP_TYPE)(mult * log10( VECT_OP_EPSILON +  *sp++ ));
  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(dBToLinear)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp, VECT_OP_TYPE mult )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* rp = dbp;
  while( dbp < dep )
    *dbp++ =  (VECT_OP_TYPE)pow(10.0, *sp++ / mult );
  return rp;
}

VECT_OP_TYPE* VECT_OP_FUNC(AmplitudeToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
{ return VECT_OP_FUNC(LinearToDb)(dbp,dn,sp,20.0); }

VECT_OP_TYPE* VECT_OP_FUNC(PowerToDb)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
{ return VECT_OP_FUNC(LinearToDb)(dbp,dn,sp,10.0); }

VECT_OP_TYPE* VECT_OP_FUNC(dBToAmplitude)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
{ return VECT_OP_FUNC(dBToLinear)( dbp,dn,sp,20); }

VECT_OP_TYPE* VECT_OP_FUNC(dBToPower)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sp )
{ return VECT_OP_FUNC(dBToLinear)( dbp,dn,sp,10); }


unsigned      VECT_OP_FUNC(SynthPhasor)(VECT_OP_TYPE* dbp, unsigned dn, unsigned phase, double srate, double hz )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  while( dbp < dep )
    *dbp++ = (VECT_OP_TYPE)fmod( (hz *  phase++)/srate, 1.0 );

  return phase;
}
	

VECT_OP_TYPE VECT_OP_FUNC(KaiserBetaFromSidelobeReject)(  double sidelobeRejectDb )
	{ 
		double beta;
		
		if( sidelobeRejectDb < 13.26 )
			sidelobeRejectDb = 13.26;
		else
		if( sidelobeRejectDb > 120.0)
			sidelobeRejectDb = 120.0; 

		if( sidelobeRejectDb < 60.0 )
			beta = (0.76609 * pow(sidelobeRejectDb - 13.26,0.4))  + (0.09834*(sidelobeRejectDb-13.26));
		else
			beta = 0.12438 * (sidelobeRejectDb + 6.3);
			
		return (VECT_OP_TYPE)beta;
	}
	

VECT_OP_TYPE VECT_OP_FUNC(KaiserFreqResolutionFactor)( double sidelobeRejectDb )
{	return (6.0 * (sidelobeRejectDb + 12.0))/155.0;	}


VECT_OP_TYPE* VECT_OP_FUNC(Kaiser)( VECT_OP_TYPE* dbp, unsigned n, double beta )
{
  bool	   zeroFl 	= false;
  int		   M		    = 0;
  double	 den		  = cmBessel0(beta);	// wnd func denominator
  int		   cnt		  = n;
  int      i;

  assert( n >= 3 );

  // force ele cnt to be odd 
  if( cmIsEvenU(cnt)  )
  {
    cnt--;
    zeroFl = true;
  }

  // at this point cnt is odd and >= 3
		
  // calc half the window length
  M = (int)((cnt - 1.0)/2.0);
		
  double Msqrd = M*M;
		
  for(i=0; i<cnt; i++)
  {		
    double v0 = (double)(i - M);

    double num = cmBessel0(beta * sqrt(1.0 - ((v0*v0)/Msqrd)));
			
    dbp[i] = (VECT_OP_TYPE)(num/den);
  }								 


  if( zeroFl )
    dbp[cnt] = 0.0;  // zero the extra element in the output array

  return dbp;
}
	
VECT_OP_TYPE*	VECT_OP_FUNC(Gaussian)( VECT_OP_TYPE* dbp, unsigned dn, double mean, double variance )
{
  
  int      M		    =  dn-1;
  double   sqrt2pi	= sqrt(2.0*M_PI);
  unsigned i;

  for(i=0; i<dn; i++)
  {
    double arg = ((((double)i/M) - 0.5) * M);

    arg = pow( (double)(arg-mean), 2.0);

    arg = exp( -arg / (2.0*variance));

    dbp[i] = (VECT_OP_TYPE)(arg / (sqrt(variance) * sqrt2pi));
  }
		
  return dbp;
}


VECT_OP_TYPE* VECT_OP_FUNC(Hamming)( VECT_OP_TYPE* dbp, unsigned dn )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* dp   = dbp;
  double        fact = 2.0 * M_PI / (dn-1);
  unsigned      i;

  for(i=0;  dbp < dep; ++i )
    *dbp++  = (VECT_OP_TYPE)(.54 - (.46 * cos(fact*i)));

  return dp;
}

VECT_OP_TYPE* VECT_OP_FUNC(Hann)( VECT_OP_TYPE* dbp, unsigned dn )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* dp   = dbp;
  double        fact = 2.0 * M_PI / (dn-1);
  unsigned      i;

  for(i=0;  dbp < dep; ++i )
    *dbp++  = (VECT_OP_TYPE)(.5 - (.5 * cos(fact*i)));

  return dp;
}

VECT_OP_TYPE* VECT_OP_FUNC(HannMatlab)( VECT_OP_TYPE* dbp, unsigned dn )
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* dp   = dbp;
  double        fact = 2.0 * M_PI / (dn+1);
  unsigned      i;

  for(i=0;  dbp < dep; ++i )
    *dbp++  = (VECT_OP_TYPE)(0.5*(1.0-cos(fact*(i+1))));

  return dp;
}



VECT_OP_TYPE* VECT_OP_FUNC(Triangle)( VECT_OP_TYPE* dbp, unsigned dn )
{
  unsigned     n    = dn/2;
  VECT_OP_TYPE incr = 1.0/n;

  VECT_OP_FUNC(Seq)(dbp,n,0,incr);

  VECT_OP_FUNC(Seq)(dbp+n,dn-n,1,-incr);

  return dbp;
}

VECT_OP_TYPE*	VECT_OP_FUNC(GaussWin)( VECT_OP_TYPE* dbp, unsigned dn, double arg )	
{
  const VECT_OP_TYPE* dep = dbp + dn;
  VECT_OP_TYPE* rp = dbp;
  int           N  = (dep - dbp) - 1;
  int           n  = -N/2;
  
  if( N == 0 )
    *dbp = 1.0;
  else
  {
    while( dbp < dep )
    {
      double a = (arg * n++) / (N/2);

      *dbp++ = (VECT_OP_TYPE)exp( -(a*a)/2 );
    }
  }
  return rp;
}


VECT_OP_TYPE* VECT_OP_FUNC(Filter)( 
  VECT_OP_TYPE*       y, 
  unsigned            yn, 
  const VECT_OP_TYPE* x, 
  unsigned            xn, 
  cmReal_t            b0, 
  const cmReal_t*     b, 
  const cmReal_t*     a,  
  cmReal_t*           d, 
  unsigned            dn )
{
  int           i,j;
  VECT_OP_TYPE  y0 = 0;
  unsigned      n = cmMin( yn, xn );

  // This is a direct form II algorithm based on the MATLAB implmentation
  // http://www.mathworks.com/access/helpdesk/help/techdoc/ref/filter.html#f83-1015962

  for(i=0; i<n; ++i)
  {
    y[i] = (x[i] * b0) + d[0];

    y0 = y[i];

    for(j=0; j<dn; ++j)
      d[j] = (b[j] * x[i]) - (a[j] * y0) + d[j+1]; 
   
  }


  // if fewer input samples than output samples - zero the end of the output buffer
  if( yn > xn )
    VECT_OP_FUNC(Fill)(y+i,yn-i,0);

  return cmOkRC;
  
}


VECT_OP_TYPE*    VECT_OP_FUNC(FilterFilter)(struct cmFilter_str* f, cmRC_t (*func)( struct cmFilter_str* f,  const VECT_OP_TYPE* x, unsigned xn, VECT_OP_TYPE* y, unsigned yn ), const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const VECT_OP_TYPE* x, unsigned xn, VECT_OP_TYPE* y, unsigned yn )
{
  int             i,j;
  int             nfilt = cmMax(bn,an);
  int             nfact = 3*(nfilt-1);
  const cmReal_t* a     = aa;
  const cmReal_t* b     = bb;
  cmReal_t*       m     = NULL;
  cmReal_t*       p;
  unsigned        zn    = (nfilt-1)*(nfilt-1);
  unsigned        mn    = 2*zn; // space  for mtx z0 and z1

  mn += nfilt;   // space for zero padded coeff vector

  mn += 2*nfact; // space for begin/end sequences

  if( nfact >= xn )
  {
    return cmOkRC;
  }

  m = cmMemAllocZ( cmReal_t, mn );
  p = m;

  cmReal_t*   z0 = p;
  p  += zn;

  cmReal_t*   z1 = p;
  p  += zn;

  cmReal_t* s0 = p;  
  p  += nfact;

  cmReal_t* s1 = p;
  p  += nfact;

  // zero pad the shorter coeff vect
  if( bn < nfilt )
  {
    cmVOR_Copy(p,bn,bb);
    b = p;
    p += nfilt;
  }
  else
  if( an < nfilt )
  {
    cmVOR_Copy(p,an,aa);
    a = p;
    p += nfilt;
  }


  // z0=eye(nfilt-1)
  cmVOR_Identity(z0,nfilt-1,nfilt-1);

  // z1=[eye(nfilt-1,nfilt-2); zeros(1,nfilt-1)];
  cmVOR_Identity(z1,nfilt-1,nfilt-2);

  // z0(:,1) -= a(:)
  for(i=0; i<nfilt-1; ++i)
    z0[i] -= -a[i+1];

  // z0(:,2:end) -= z1; 
  for(i=1; i<nfilt-1; ++i)
    for(j=0; j<nfilt-1; ++j)
      z0[ (i*(nfilt-1)) + j ] -= z1[ ((i-1)*(nfilt-1)) + j ];

  // z1 = b - (a * b[0])
  for(i=1; i<nfilt; ++i)
    z1[i-1] = b[i] - (a[i] * b[0]);

  // z1 = z0\z1
  cmVOR_SolveLS(z0,nfilt-1,z1,1);

  // if yn<xn then truncate x.
  xn = cmMin(xn,yn);
  yn = xn;

  // fill in the beginning sequence
  for(i=0; i<nfact; ++i)
    s0[i] = 2*x[0] - x[ nfact-i ];

  // fill in the ending sequence
  for(i=0; i<nfact; ++i)
    s1[i] = 2*x[xn-1] - x[ xn-2-i ];


  cmVOR_MultVVS( z0, nfact, z1, s0[0]);

  unsigned  pn = cmMin(1024,xn);
  //acFilter* f  = cmFilterAlloc(c,NULL,b,bn,a,an,pn,z0);

  cmFilterInit(f,b,bn,a,an,pn,z0);

  const VECT_OP_TYPE* xx = x;

  for(j=0; j<2; ++j)
  {
    unsigned n = pn;

    // filter begining sequence
    cmFilterExecR(f,s0,nfact,s0,nfact); 

    // filter middle sequence
    for(i=0; i<xn; i+=n)
    {
      n = cmMin(pn,xn-i);
      func(f,xx+i,n,y+i,n);
    }

    // filter ending sequence
    cmFilterExecR(f,s1,nfact,s1,nfact);

   
    // flip all the sequences
    cmVOR_Flip(s0,nfact);
    cmVOR_Flip(s1,nfact);
    VECT_OP_FUNC(Flip)(y,yn);    

    if( j==0)
    {

      // swap the begin and end sequences
      cmReal_t* t = s0;
      s0 = s1;
      s1 = t;

      xx = y;

      cmVOR_MultVVS( z0, nfact, z1, s0[0]);

      cmFilterInit(f,b,bn,a,an,pn,z0);

    }
    
  }

  //cmFilterFree(&f);
  cmMemPtrFree(&m);

  return y;
}



VECT_OP_TYPE* VECT_OP_FUNC(LP_Sinc)(VECT_OP_TYPE* dp, unsigned dn, double srate, double fcHz, unsigned flags )
{
  VECT_OP_TYPE* rp = dp;

  int    dM       = dn % 2;                  // dM is used to handle odd length windows
  int    M        = (dn - dM)/2;
  int    Mi       = -M;
  double signFact = cmIsFlag(flags, kHighPass_LPSincFl) ? -0.5 : 0.5;
  double phsFact  = 2.0 * M_PI * fcHz / srate;
  double sum      = 0;


  M += dM; 

  //printf("M=%i Mi=%i sign:%f phs:%f\n",M,Mi,signFact,phsFact);


  for(; Mi<M; ++Mi,++dp)
  {
    double phs = phsFact * Mi;
    *dp = Mi == 0 ? 0.5 : signFact * sin(phs)/phs;
    sum += *dp;
  }

  if( cmIsFlag(flags,kNormalize_LPSincFl) )
    VECT_OP_FUNC(DivVS)(rp,dn,sum);

  return rp;
}

VECT_OP_TYPE  VECT_OP_FUNC(ComplexDetect)(const VECT_OP_TYPE* mag0V, const VECT_OP_TYPE* mag1V, const VECT_OP_TYPE* phs0V, const VECT_OP_TYPE* phs1V, const VECT_OP_TYPE* phs2V, unsigned binCnt )
{
  double              sum  = 0;
  const VECT_OP_TYPE*  ep  = mag0V + binCnt;

  unsigned i = 0;

  for(;  mag0V < ep; ++i )
  {
    // calc phase deviation from expected
    double dev_rads    = *phs0V++ - (2 * *phs1V++) + *phs2V++;   

    // map deviation into range: -pi to pi
    //double dev_rads1    = mod(dev_rads0 + M_PI, -2*M_PI ) + M_PI;            

    while( dev_rads > M_PI)
      dev_rads -= 2*M_PI;

    while( dev_rads < -M_PI)
      dev_rads += 2*M_PI;    

    // convert into rect coord's
    double m1r =  *mag1V++;
    double m0r =  *mag0V   * cos(dev_rads);
    double m0i =  *mag0V++ * sin(dev_rads);

    // calc the combined amplitude and phase deviation 
    // sum += hypot( m1 - (m0 * e^(-1*dev_rads)));

    sum += hypot( m1r-m0r, -m0i );

  }

  return (VECT_OP_TYPE)sum; 
}

VECT_OP_TYPE* VECT_OP_FUNC(MelMask)( VECT_OP_TYPE* maskMtx, unsigned filterCnt, unsigned binCnt, double srate, unsigned flags )
{
  unsigned fi,bi;

  double mxh = srate/2.0;                        // nyquist
  double dh  = mxh/(binCnt-1) ;                  // binHz
  double mxm = 1127.0 * log( 1.0 + mxh/700.0);   // max mel value in Hz
  double dm  = mxm / (filterCnt+1);              // avg mel band hz
  double sum = 0;

  for(fi=0; fi<filterCnt; ++fi)
  {
    double   m     =  (fi+1) * dm;

    // calc min/center/max frequencies for this band
    double   minHz =  700.0 * (exp((m-dm)/1127.01048)-1.0);
    double   ctrHz =  700.0 * (exp( m    /1127.01048)-1.0);
    double   maxHz =  700.0 * (exp((m+dm)/1127.01048)-1.0);


    // shift the band min/ctr/max to the nearest bin ctr frequency
    if( cmIsFlag(flags,kShiftMelFl) )
    {
      unsigned i;

      i = (unsigned)floor(minHz/dh);
      minHz =  minHz - (dh*i) <  dh*(i+1) - minHz ? dh*i : dh*(i+1);

      i = (unsigned)floor(ctrHz/dh);
      ctrHz =  ctrHz - (dh*i) <  dh*(i+1) - ctrHz ? dh*i : dh*(i+1);

      i = (unsigned)floor(maxHz/dh);
      maxHz =  maxHz - (dh*i) <  dh*(i+1) - maxHz ? dh*i : dh*(i+1);

    }

    // calc the height of the triangle - such that all bands have equal area
    double   a     =  2.0/(maxHz - minHz);

    for(bi=0; bi<binCnt; ++bi)
    {
      double   h  = bi*dh;
      unsigned mi = bi*filterCnt + fi;

 
      if( h < minHz || h >  maxHz )
        maskMtx[mi] = 0;
      else
      {
        if( h <= ctrHz )
          maskMtx[mi] = a * (h - minHz)/(ctrHz-minHz);   
        else
          maskMtx[mi] = a * (maxHz - h)/(maxHz-ctrHz);

        sum += maskMtx[mi];
      } 

    }
  }

  if( cmIsFlag(flags,kNormalizeMelFl) )
    VECT_OP_FUNC(DivVS)( maskMtx, (filterCnt*binCnt), sum );
    

  return maskMtx;
}

unsigned VECT_OP_FUNC(BarkMap)(unsigned* binIdxV, unsigned* cntV, unsigned bandCnt, unsigned binCnt, double srate )
{
  if( bandCnt == 0 )
    return 0;

  //zwicker & fastl: psychoacoustics 1999, page 159
  double bandUprHz[] = { 100, 200, 300, 400, 510, 630, 770, 920, 1080, 1270, 1480, 1720, 2000, 2320, 2700, 3150, 3700, 4400, 5300, 6400, 7700, 9500, 12000, 15500 };

  unsigned hn = sizeof(bandUprHz)/sizeof(double);
  
  unsigned i, bi = 0;

  bandCnt    = cmMin(hn,bandCnt);

  binIdxV[0] = 0;
  cntV[0]    = 1;

  for(i=1; bi < bandCnt  && i<binCnt; ++i)
  {
    double hz =  srate * i / (2 * (binCnt-1));

    if( hz <=  bandUprHz[bi] )
      cntV[bi]++;
    else
    {
      //printf("%i %i %i %f\n",bi,binIdxV[bi],cntV[bi],bandUprHz[bi]);

      ++bi;
      if( bi < bandCnt )
      {
        binIdxV[bi] = i;
        cntV[bi]    = 1;
      }
    }    


  }
  
  return bi;
}

VECT_OP_TYPE* VECT_OP_FUNC(TriangleMask)(VECT_OP_TYPE* maskMtx, unsigned bandCnt, unsigned binCnt, const VECT_OP_TYPE* ctrHzV, VECT_OP_TYPE binHz, VECT_OP_TYPE stSpread, const VECT_OP_TYPE* lfV, const VECT_OP_TYPE* hfV )
{
  unsigned     i,j;
  VECT_OP_TYPE v0[ bandCnt ];
  VECT_OP_TYPE v1[ bandCnt ];

  // if no lower/upper band limits were give use a fixed semitone band width
  if( lfV==NULL || hfV==NULL)
  {

    for(i=0; i<bandCnt; ++i)
    {
      v0[i] = ctrHzV[i] * pow(2.0,-stSpread/12.0);
      v1[i] = ctrHzV[i] * pow(2.0, stSpread/12.0);
    }    

    lfV = v0;
    hfV = v1;

  }

  VECT_OP_FUNC(Zero)(maskMtx,bandCnt*binCnt);

  // for each band 
  for(i=0; i<bandCnt; ++i)
  {
    // calc bin index of first possible bin in this band
    // j = (unsigned)floor(lfV[i] / binHz); 
    
    double binHz_j = 0;

    // for each bin whose ctr frq is  <= the band upper limit
    for(j=0; j<binCnt; ++j)
    {
      double v;

      // if bin[j] is inside the lower leg of the triangle
      if( lfV[i] <= binHz_j && binHz_j <= ctrHzV[i] )
        v =  (binHz_j - lfV[i]) / cmMax(VECT_OP_MIN, ctrHzV[i] - lfV[i] );
      else

      // if bin[j] is inside the upper leg of the triangle
      if( ctrHzV[i] < binHz_j && binHz_j <= hfV[i] )
        v =  (hfV[i] - binHz_j) / cmMax(VECT_OP_MIN, hfV[i] - ctrHzV[i] );
      else
        v = 0;

      maskMtx[ (j*bandCnt)+i ]  = v;

      binHz_j = binHz * (j+1);
      
    }
  }

  return maskMtx;
}

VECT_OP_TYPE* VECT_OP_FUNC(BarkMask)(VECT_OP_TYPE* maskMtx, unsigned bandCnt, unsigned binCnt, double binHz )
{
  //                -1   0  1   2   3   4   5   6   7    8   9    10   11   12   13   14   15   16   17   18   19   20   21   22    23    (23+1)
  VECT_OP_TYPE b[]= {0, 50,150,250,350,450,570,700,840,1000,1170,1370,1600,1850,2150,2500,2900,3400,4000,4800,5800,7000,8500,10500,13500, 15500 };

  bandCnt = cmMin(bandCnt,kDefaultBarkBandCnt);

  VECT_OP_FUNC(TriangleMask)(maskMtx, bandCnt, binCnt, b+1, binHz, 0, b+0, b+2 );
  
  return maskMtx;
}

VECT_OP_TYPE* VECT_OP_FUNC(TerhardtThresholdMask)(VECT_OP_TYPE* maskV, unsigned binCnt, double srate, unsigned flags )
{
  unsigned i;

  double c0 = cmIsFlag(flags,kModifiedTtmFl) ? 0.6 : 1.0;
  double c1 = cmIsFlag(flags,kModifiedTtmFl) ? 0.5 : 6.5;

  maskV[0]=0;

  for(i=0; i<binCnt; ++i)
  {
    double hz =  srate * i / (2 * (binCnt-1));
    maskV[i] = pow(pow(10,(c0 * -3.64* pow(hz/1000,-0.8)  + c1 * exp(-0.6 * pow(hz/1000 - 3.3,2)) - 0.001* pow(hz/1000,4))/20),2);
  }

  return maskV;
}


VECT_OP_TYPE* VECT_OP_FUNC(ShroederSpreadingFunc)(VECT_OP_TYPE* m, unsigned bandCnt, double srate)
{
  int fi,bi;

  for(fi=0; fi<bandCnt; ++fi)
    for(bi=0; bi<bandCnt; ++bi )
      m[ fi + (bi*bandCnt) ] = pow(10,(15.81 + 7.5 * ((fi-bi)+0.474)-17.5*pow(1+pow((fi-bi)+0.474,2),0.5))/10);

  return m;
}


VECT_OP_TYPE* VECT_OP_FUNC(DctMatrix)( VECT_OP_TYPE* dp, unsigned coeffCnt, unsigned filtCnt )
{
  VECT_OP_TYPE* dbp = dp;

  double c0 = 1.0/sqrt(filtCnt/2); // row 1-coeffCnt factor
  double c1 = c0 * sqrt(2)/2;      // row 0 factor

  unsigned i,j;

  // for each column
  for(i=0; i<filtCnt; ++i)
    // for each row
    for(j=0; j<coeffCnt; ++j)
      *dp++ = (j==0 ? c1 : c0) * cos( (0.5 + i) * M_PI * j / filtCnt);

  return dbp;
}

unsigned VECT_OP_FUNC(PeakIndexes)( unsigned* dbp, unsigned dn, const VECT_OP_TYPE* sbp, unsigned sn, VECT_OP_TYPE threshold )
{
  unsigned pkCnt = 0;
  const unsigned*     dep = dbp + dn;
  const VECT_OP_TYPE* sep = sbp + sn;
  const VECT_OP_TYPE* s2p = sbp;
  const VECT_OP_TYPE* s0p = s2p++;
  const VECT_OP_TYPE* s1p = s2p++;


  while( dbp < dep && s2p < sep )
  {
    if( (*s0p < *s1p) && (*s1p > *s2p) && (*s1p >= threshold) )
    {
      *dbp++ = s1p - sbp;
      s0p    = s2p++;
      s1p    = s2p++;
      ++pkCnt;
    }
    else
    {
      s0p = s1p;
      s1p = s2p++;     
    }
  }
  
  return pkCnt;
}

unsigned VECT_OP_FUNC(BinIndex)( const VECT_OP_TYPE* sbp, unsigned sn, VECT_OP_TYPE v )
{
  const VECT_OP_TYPE* sep = sbp + sn;
  const VECT_OP_TYPE* bp = sbp;
  sep--;
  for(; sbp < sep; ++sbp  )
    if( *sbp <= v && v < *(sbp+1) )
      return sbp - bp;
      
  return cmInvalidIdx;
}



unsigned VECT_OP_FUNC(Kmeans)( 
  unsigned*           classIdxV,         // classIdxV[scn] - data point class assignments
  VECT_OP_TYPE*       centroidM,         // centroidM[srn,K] - cluster centroids
  unsigned            K,                 // count of clusters
  const VECT_OP_TYPE* sM,                // sM[srn,scn] source data matrix
  unsigned            srn,               // dimensionality of each data point
  unsigned            scn,               // count of data points
  const unsigned*     selIdxV,           // data subset selection id vector (optional)
  unsigned            selKey,            // data subset selection key       (optional)
  bool                initFromCentroidFl,// true if the starting centroids are in centroidM[]
  VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ),
  void*               userDistPtr
) 
{
  unsigned D       = srn;   // data dimensionality
  unsigned N       = scn;   // count of data points to cluster
  unsigned iterCnt = 0;
  unsigned ki;
  unsigned i       = 0;
  unsigned selN    = N;

  // if a data point selection vector was given
  if( selIdxV != NULL )
  {
    selN = 0;

    for(i=0; i<N; ++i)
    {
      selN        += selIdxV[i]==selKey;
      classIdxV[i] = K;
    }
  }


  assert(K<=selN);

  // if the numer of datapoints and the number of clusters is the same
  // make the datapoints the centroids and return
  if( K == selN )
  {
    ki = 0;
    for(i=0; i<N; ++i)
      if( selIdxV==NULL || selIdxV[i]==selKey )
      {
        VECT_OP_FUNC(Copy)(centroidM+(ki*D),D,sM+(i*D));
        classIdxV[ki] = ki;
        ++ki;
      }
    return 0;
  }

    
  // if centroidM[] has not been initialized with the starting centroid vectors.
  if( initFromCentroidFl == false )
  {
    unsigned* kiV     = cmMemAlloc( unsigned, N );

    // select K unique datapoints at random as the initial centroids
    cmVOU_RandomSeq(kiV,N);
  
    for(i=0,ki=0; i<N && ki<K; ++i)
    {
      if( selIdxV==NULL || selIdxV[ kiV[i] ]==selKey )
      {
        VECT_OP_FUNC(Copy)( centroidM + (ki*D), D, sM + (kiV[i]*D) );
        ++ki;
      }  
    }
  
    cmMemPtrFree(&kiV);
  }
    
  unsigned* nV = cmMemAllocZ( unsigned,K);

  while(1)
  {
    unsigned changeCnt = 0;

    cmVOU_Zero(nV,K);

    // for each data point - assign data point to a cluster
    for(i=0; i<N; ++i)
      if( selIdxV==NULL || selIdxV[i] == selKey )
      {
        // set ki with the index of the centroid closest to sM[:,i]
        VECT_OP_FUNC(DistVMM)( NULL, NULL, &ki, D, sM + (i*srn), 1, centroidM, K, distFunc, userDistPtr );

        assert(ki<K);

        nV[ki]++;

        changeCnt += ( ki != classIdxV[i] );
        classIdxV[i] = ki;        
      }


    // if no data points change classes then the centroids have converged
    if( changeCnt == 0 )
      break;

    ++iterCnt;

    // zero the centroid matrix
    VECT_OP_FUNC(Fill)(centroidM, D*K, 0 );

    // update the centroids
    for(ki=0; ki<K; ++ki)
    {
      unsigned n = 0;

      // sum the all datapoints belonging to class ki
      for(i=0; i<N; ++i)
        if( classIdxV[i] == ki )
        {
          VECT_OP_FUNC(AddVV)(centroidM + (ki*D), D, sM + (i*srn) );
          ++n;
        }

      // convert the sum to a mean to form the centroid
      if( n > 0 )
        VECT_OP_FUNC(DivVS)(centroidM + (ki*D), D, n );      


    } 
  } 

  cmVOU_PrintL("class cnt:",NULL,1,K,nV);
  cmMemPtrFree(&nV);
  return iterCnt;
}

unsigned VECT_OP_FUNC(Kmeans2)( 
  unsigned*           classIdxV,         // classIdxV[scn] - data point class assignments
  VECT_OP_TYPE*       centroidM,         // centroidM[srn,K] - cluster centroids
  unsigned            K,                 // count of clusters
  const VECT_OP_TYPE* (*srcFunc)(void* userPtr, unsigned frmIdx ),
  unsigned            srn,               // dimensionality of each data point
  unsigned            scn,               // count of data points
  void*               userSrcPtr,        // callback data for srcFunc
  VECT_OP_TYPE (*distFunc)( void* userPtr, const VECT_OP_TYPE* s0V, const VECT_OP_TYPE* s1V, unsigned sn ),
  void*               distUserPtr,
  int                 maxIterCnt,
  int                 deltaStopCnt
) 
{
  unsigned D       = srn;   // data dimensionality
  unsigned N       = scn;   // count of data points to cluster
  unsigned iterCnt = 0;
  unsigned ki;
  unsigned i       = 0;
  const VECT_OP_TYPE* sp;

  assert(K<N);

  deltaStopCnt = cmMax(0,deltaStopCnt);
       
  // nV[K] - class assignment vector
  unsigned* nV  = cmMemAllocZ( unsigned,2*K);  

  // roV[K] - read-only flag centroid 
  // centroids flagged as read-only will not be updated by the clustering routine
  unsigned* roV = nV + K;                      

  // copy the read-only flags into roV[K]
  for(i=0; i<K; ++i)
    roV[i] = classIdxV[i];

  while(1)
  {
    unsigned changeCnt = 0;

    cmVOU_Zero(nV,K);

    // for each data point - assign data point to a cluster
    for(i=0; i<N; ++i)
      if((sp = srcFunc(userSrcPtr,i)) != NULL)
      {
        // set ki with the index of the centroid closest to sM[:,i]
        VECT_OP_FUNC(DistVMM)( NULL, NULL, &ki, D, sp, 1, centroidM, K, distFunc, distUserPtr );

        assert(ki<K);

        // track the number of data points assigned to each centroid
        nV[ki]++;  

        // track the number of data points  which change classes
        changeCnt += ( ki != classIdxV[i] );

        // update the class that this data point belongs to
        classIdxV[i] = ki;        
      }


    // if the count of data points which changed classes is less than deltaStopCnt 
    // then the centroids have converged
    if( changeCnt <= deltaStopCnt )
      break;

    if( maxIterCnt!=-1 && iterCnt>=maxIterCnt )
      break;

    // track the number of interations required to converge
    ++iterCnt;

    fprintf(stderr,"%i:%i (", iterCnt,changeCnt );
    for(i=0; i<K; ++i)
      fprintf(stderr,"%i ",nV[i]);
    fprintf(stderr,") ");
    fflush(stderr);

    // update the centroids
    for(ki=0; ki<K; ++ki)
      if( roV[ki]==0 )
      {
        unsigned n = 0;

        VECT_OP_FUNC(Zero)(centroidM + (ki*D), D );

        // sum the all datapoints belonging to class ki
        for(i=0; i<N; ++i)
          if( classIdxV[i] == ki && ((sp=srcFunc(userSrcPtr,i))!=NULL))
          {
            VECT_OP_FUNC(AddVV)(centroidM + (ki*D), D, sp );
            ++n;
          }

        // convert the sum to a mean to form the centroid
        if( n > 0 )
          VECT_OP_FUNC(DivVS)(centroidM + (ki*D), D, n );      

      } 
  } 

  cmMemPtrFree(&nV);
  return iterCnt;
}


VECT_OP_TYPE* VECT_OP_FUNC(GaussPDF)( VECT_OP_TYPE* dbp, unsigned dn, const VECT_OP_TYPE* sbp, VECT_OP_TYPE mean, VECT_OP_TYPE stdDev )
{
  VECT_OP_TYPE*       rp    = dbp;
  const VECT_OP_TYPE* dep   = dbp + dn;
  VECT_OP_TYPE        var   = stdDev * stdDev;
  VECT_OP_TYPE        fact0 = 1.0/sqrt(2*M_PI*var);
  VECT_OP_TYPE        fact1 = 2.0 * var;

  for(; dbp < dep; ++sbp )
    *dbp++ = fact0 * exp( -((*sbp-mean)*(*sbp-mean))/ fact1 );

  return rp;
}

/// Evaluate a multivariate normal distribution defined by meanV[D] and covarM[D,D]
/// at the data points held in the columns of xM[D,N]. Return the evaluation
/// results in the vector yV[N]. 
bool VECT_OP_FUNC(MultVarGaussPDF)( VECT_OP_TYPE* yV, const VECT_OP_TYPE* xM, const VECT_OP_TYPE* meanV,  const VECT_OP_TYPE* covarM, unsigned D, unsigned N, bool diagFl )
{
  VECT_OP_TYPE det0;

  // calc the determinant of the covariance matrix
  if( diagFl )
    // kpl 1/16/11 det0  = VECT_OP_FUNC(LogDetDiagM)(covarM,D);
    det0  = VECT_OP_FUNC(DetDiagM)(covarM,D);
  else
    // kpl 1/16/11 det0 = VECT_OP_FUNC(LogDetM)(covarM,D);
    det0 = VECT_OP_FUNC(DetM)(covarM,D);

  assert(det0 != 0 );

  if( det0 == 0 )
    return false;

  // calc the inverse of the covariance matrix
  VECT_OP_TYPE icM[D*D];
  VECT_OP_FUNC(Copy)(icM,D*D,covarM);
  
  VECT_OP_TYPE* r;
  if( diagFl )
    r = VECT_OP_FUNC(InvDiagM)(icM,D);
  else
    r = VECT_OP_FUNC(InvM)(icM,D);

  if( r == NULL )
      return false;
  
  VECT_OP_FUNC(MultVarGaussPDF2)( yV, xM, meanV, icM, det0, D, N, diagFl );

  return true;
}

VECT_OP_TYPE* VECT_OP_FUNC(MultVarGaussPDF2)( VECT_OP_TYPE* yV, const VECT_OP_TYPE* xM, const VECT_OP_TYPE* meanV, const VECT_OP_TYPE* icM, VECT_OP_TYPE logDet, unsigned D, unsigned N, bool diagFl )
{
  unsigned i;

  double fact = (-(cmReal_t)D/2) * log(2.0*M_PI) - 0.5*logDet;

  for(i=0; i<N; ++i)
  {
    VECT_OP_TYPE dx[D];
    VECT_OP_TYPE t[D];

    // dx[] difference between mean and ith data point
    VECT_OP_FUNC(SubVVV)(dx,D, xM + (i*D), meanV);

    // t[] = dx[] * inv(covarM);
    if( diagFl )
      VECT_OP_FUNC(MultDiagVMV)(t,D,icM,D,dx);
    else
      VECT_OP_FUNC(MultVMV)(t,D,icM,D,dx);

    // dist = sum(dx[] * t[])
    cmReal_t dist = VECT_OP_FUNC(MultSumVV)(t,dx,D);

    yV[i] = exp( fact - (0.5*dist) );

  }

  return yV;
}


VECT_OP_TYPE* VECT_OP_FUNC(MultVarGaussPDF3)( 
  VECT_OP_TYPE*       yV, 
  const VECT_OP_TYPE* (*srcFunc)(void* funcDataPtr, unsigned frmIdx ),
  void*               funcDataPtr,
  const VECT_OP_TYPE* meanV, 
  const VECT_OP_TYPE* icM, 
  VECT_OP_TYPE        logDet, 
  unsigned            D, 
  unsigned            N, 
  bool                diagFl )
{
  unsigned i;

  double fact = (-(cmReal_t)D/2) * log(2.0*M_PI) - 0.5*logDet;

  for(i=0; i<N; ++i)
  {
    VECT_OP_TYPE dx[D];
    VECT_OP_TYPE t[D];

    const VECT_OP_TYPE* xV = srcFunc( funcDataPtr, i );

    if( xV == NULL )
      yV[i] = 0;
    else
    {
      // dx[] difference between mean and ith data point
      VECT_OP_FUNC(SubVVV)(dx, D, xV, meanV);

      // t[] = dx[] * inv(covarM);
      if( diagFl )
        VECT_OP_FUNC(MultDiagVMV)(t,D,icM,D,dx);
      else
        VECT_OP_FUNC(MultVMV)(t,D,icM,D,dx);

      // dist = sum(dx[] * t[])
      cmReal_t dist = VECT_OP_FUNC(MultSumVV)(t,dx,D);

      yV[i] = exp( fact - (0.5*dist) );
    }
  }

  return yV;
}


/// stateV[timeN]
/// a[stateN,stateN], 
/// b[stateN,timeN]
/// phi[stateN].
void VECT_OP_FUNC(DiscreteViterbi)(unsigned* stateV, unsigned tN, unsigned sN, const VECT_OP_TYPE* phi, const VECT_OP_TYPE* a, const VECT_OP_TYPE* b )
{
  unsigned*     psiM   = cmMemAlloc( unsigned,     sN*tN );  // psi[sN,tN]
  VECT_OP_TYPE* dV     = cmMemAlloc( VECT_OP_TYPE, 2*sN );
  VECT_OP_TYPE* d0V    = dV;
  VECT_OP_TYPE* d1V    = dV + sN;

  int     t,i,j;

  // calc the prob of starting in each state given the observations
  VECT_OP_FUNC(MultVVV)( d0V, sN, phi, b );
  VECT_OP_FUNC(NormalizeProbability)( d0V, sN ); // scale to prevent underflow

  // for each time step
  for(t=1; t<tN; ++t)
  {
    // for each possible next state 
    for(j=0; j<sN; ++j)
    {
      VECT_OP_TYPE mv = 0;
      unsigned     mi = 0;

      // The following loop could be replaced with these vector op's:
      // VECT_OP_TYPE tV[ sN ];
      // VECT_OP_TYPE(MultVVV)(tV,sN,d0V,a + (j*sN));
      // mi = VECT_OP_TYPE(MaxIndex)(tV,sN);
      // mv = tV[mi];

      // for each possible prev state 
      for(i=0; i<sN; ++i)
      {
        // calc prob of having ended in state i and transitioning to state j
        VECT_OP_TYPE v =  d0V[i] * a[ i + (j*sN) ];

        // track the most likely transition ending in state j
        if( v > mv )
        {
          mv = v;
          mi = i;
        }
      }  

      // scale the prob of the most likely state by the prob of the obs given that state
      d1V[j] = mv * b[ (t*sN) + j ];

      // store the most likely previous state given that the current state is j
      // (this is the key to understanding the backtracking step below)
      psiM[ (t*sN) + j ] = mi;
    }

    VECT_OP_FUNC(NormalizeProbability)( d1V, sN ); // scale to prevent underflow

    // swap d0V and d1V
    VECT_OP_TYPE* tmp = d0V;
    d0V = d1V;
    d1V = tmp;
  }

  // store the most likely ending state
  stateV[tN-1] = VECT_OP_FUNC(MaxIndex)( d0V, sN, 1 );

  // given the most likely next step select the most likely previous step
  for(t=tN-2; t>=0; --t)
    stateV[t] = psiM[ ((t+1)*sN) + stateV[t+1] ];


  cmMemPtrFree( &psiM   );
  cmMemPtrFree( &dV );
}

bool VECT_OP_FUNC(ClipLine2)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax, VECT_OP_TYPE* t0, VECT_OP_TYPE* t1 )
{

  VECT_OP_TYPE dx = x1 - x0;
  VECT_OP_TYPE dy = y1 - y0;

  VECT_OP_TYPE p=0,q=0,r=0;

  *t0 = 0.0;
  *t1 = 1.0;

  unsigned i;
  for(i=0; i<4; ++i)
  {
    switch(i)
    {
      case 0: p=-dx; q=-(xMin - x0); break; // left
      case 1: p= dx; q= (xMax - x0); break; // right
      case 2: p=-dy; q=-(yMin - y0); break; // bottom 
      case 3: p= dy; q= (yMax - y0); break; // top        
    }

    // if parallel to edge i
    if( p == 0 )
    {
      // if entirely outside of window
      if( q < 0 )
        return false;

      continue;
    }

    r = p/q;

    // if travelling right/up
    if( p < 0 )
    {
      // travelling away from x1,y1 
      if( r > *t1 )
        return false;

      // update distance on line to point of intersection 
      if( r > *t0 )
        *t0 = r;
    }
    else // if travelling left/down
    {
      // travelling away from x1,y1
      if( r < *t0 )
        return false;

      // update distance on line to point of intersection 
      if( r < *t1 )
        *t1 = r;
    }
   
  }


  return true;

}


/// (Uses the Laing-Barsky clipping algorithm)
/// From: http://www.skytopia.com/project/articles/compsci/clipping.html
bool VECT_OP_FUNC(ClipLine)( VECT_OP_TYPE* x0, VECT_OP_TYPE* y0, VECT_OP_TYPE* x1, VECT_OP_TYPE* y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax )
{
  VECT_OP_TYPE t0;
  VECT_OP_TYPE t1;

  if( VECT_OP_FUNC(ClipLine2)(*x0,*y0,*x1,*y1,xMin,yMin,xMax,yMax,&t0,&t1) )
  {
    VECT_OP_TYPE dx = *x1 - *x0;
    VECT_OP_TYPE dy = *y1 - *y0;

    *x0 = *x0 + t0*dx;
    *x1 = *x0 + t1*dx;

    *y0 = *y0 + t0*dy;
    *y1 = *y0 + t1*dy;

    return true;
  }

  return false;

}

bool VECT_OP_FUNC(IsLineInRect)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE xMin, VECT_OP_TYPE yMin, VECT_OP_TYPE xMax, VECT_OP_TYPE yMax )
{
  VECT_OP_TYPE t0;
  VECT_OP_TYPE t1;

  return VECT_OP_FUNC(ClipLine2)(x0,y0,x1,y1,xMin,yMin,xMax,yMax,&t0,&t1);

}

VECT_OP_TYPE VECT_OP_FUNC(PtToLineDistance)( VECT_OP_TYPE x0, VECT_OP_TYPE y0, VECT_OP_TYPE x1, VECT_OP_TYPE y1, VECT_OP_TYPE px, VECT_OP_TYPE py)
{
  // from:http://en.wikipedia.org/wiki/Distance_from_a_point_to_a_line
  double normalLength = sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0));

  if( normalLength <= 0 )
    return 0;

  return (VECT_OP_TYPE)fabs((px - x0) * (y1 - y0) - (py - y0) * (x1 - x0)) / normalLength;
}                              

void VECT_OP_FUNC(Lsq1)(const VECT_OP_TYPE* x, const VECT_OP_TYPE* y, unsigned n, VECT_OP_TYPE* b0, VECT_OP_TYPE* b1 )
{
  VECT_OP_TYPE sx = 0;
  VECT_OP_TYPE sy = 0;
  VECT_OP_TYPE sx_2 = 0;
  VECT_OP_TYPE sxy  = 0;
  unsigned i;  

  if( x == NULL )
  {
    for(i=0; i<n; ++i)
    {
      VECT_OP_TYPE xx = i;
      sx   += xx;
      sx_2 += xx * xx;
      sxy  += xx * y[i];
      sy   += y[i];
    }
  }
  else
  {
    for(i=0; i<n; ++i)
    {
      sx   += x[i];
      sx_2 += x[i] * x[i];
      sxy  += x[i] * y[i];
      sy   += y[i];
    }
  }

  *b1 = (sxy * n - sx * sy) / (sx_2 * n - sx*sx);
  *b0 = (sy - (*b1) * sx) / n;      

}




#endif