libcm/cmProc.c

#include "cmPrefix.h"
#include "cmGlobal.h"
#include "cmRpt.h"
#include "cmErr.h"
#include "cmCtx.h"
#include "cmMem.h"
#include "cmMallocDebug.h"
#include "cmLinkedHeap.h"
#include "cmSymTbl.h"
#include "cmFloatTypes.h"
#include "cmComplexTypes.h"
#include "cmFileSys.h"
#include "cmProcObj.h"
#include "cmProcTemplate.h"
#include "cmAudioFile.h"
#include "cmMath.h"
#include "cmProc.h"
#include "cmVectOps.h"
#include "cmKeyboard.h"
#include "cmGnuPlot.h"

#include <time.h> // time()

//------------------------------------------------------------------------------------------------------------
void cmFloatPointExceptHandler( int signo, siginfo_t* info, void* context )
{
  char* cp = "<Type Unknown>";
  switch( info->si_code )
  {
    case FPE_INTDIV: cp = "integer divide by zero";               break;
    case FPE_INTOVF: cp = "integer overflow";                     break;
    case FPE_FLTDIV: cp = "divide by zero";                 break;
    case FPE_FLTUND: cp = "underflow";                      break;
    case FPE_FLTRES: cp = "inexact result";                 break;
    case FPE_FLTINV: cp = "invalid operation";              break;
    case FPE_FLTSUB: cp = "subscript range error"; break;
  }

  fprintf(stderr,"Floating point exception: Type: %s\n",cp);
  exit(1);
}

// set 'orgSaPtr' to NULL to discard the current signal action state
void cmSetupFloatPointExceptHandler( struct sigaction* orgSaPtr )
{
  struct sigaction sa;

  sa.sa_handler    = SIG_DFL;
  sa.sa_flags      = SA_SIGINFO;
  sa.sa_sigaction  = cmFloatPointExceptHandler;

  sigemptyset(&sa.sa_mask);

  // set all FP except flags excetp: FE_INEXACT 

#ifdef OS_OSX
  // we don't yet support FP exceptions on OSX
  // for an example of how to make this work with the linux interface as below
  // see: http://www-personal.umich.edu/~williams/archive/computation/fe-handling-example.c
  assert(0);
#else
  // int flags = FE_DIVBYZERO | FE_UNDERFLOW | FE_OVERFLOW | FE_INVALID;
  // feenableexcept(flags);

  assert(0);

#endif
  sigaction( SIGFPE, &sa, orgSaPtr ); 

}


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

cmAudioFileRd* cmAudioFileRdAlloc( cmCtx* c,  cmAudioFileRd* p, unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
{ 
  cmAudioFileRd* op = cmObjAlloc( cmAudioFileRd, c, p ); 

  if( fn != NULL )
    if( cmAudioFileRdOpen( op, procSmpCnt, fn, chIdx, begFrmIdx, endFrmIdx ) != cmOkRC )
      cmAudioFileRdFree(&op);

  return op;
}

cmRC_t               cmAudioFileRdFree(  cmAudioFileRd** pp )
{ 
  cmRC_t rc = cmOkRC;

  if( pp != NULL && *pp != NULL )
  {
    cmAudioFileRd* p = *pp;

    if((rc = cmAudioFileRdClose(p)) == cmOkRC )
    {
      cmMemPtrFree(&p->outV);
      cmMemPtrFree(&p->fn);
      cmObjFree(pp);
    }
  }

  return rc;
}

cmRC_t               cmAudioFileRdOpen(  cmAudioFileRd* p,  unsigned procSmpCnt, const cmChar_t* fn, unsigned chIdx, unsigned begFrmIdx, unsigned endFrmIdx )
{
  cmRC_t rc;
  cmRC_t afRC;

  if((rc = cmAudioFileRdClose(p)) != cmOkRC )
    return rc;

  p->h = cmAudioFileNewOpen( fn, &p->info, &afRC, p->obj.err.rpt ); 

  if( afRC != kOkAfRC )
    return cmCtxRtCondition( &p->obj, afRC, "Unable to open the audio file:'%s'", fn );

  p->chIdx          = chIdx;
  p->outN           = procSmpCnt;
  p->outV           = cmMemResizeZ( cmSample_t, p->outV, p->outN );  
  p->fn             = cmMemResizeZ( cmChar_t,   p->fn,   strlen(fn)+1 );
  strcpy(p->fn,fn);  
  //p->mfp            = cmCtxAllocDebugFile( p->obj.ctx,"audioFile");
  p->lastReadFrmCnt = 0;
  p->eofFl          = false;
  p->begFrmIdx      = begFrmIdx;
  p->endFrmIdx      = endFrmIdx;
  p->curFrmIdx      = p->begFrmIdx;

  if( p->begFrmIdx > 0 )
    rc = cmAudioFileRdSeek(p,p->begFrmIdx);

  return rc;
}

cmRC_t               cmAudioFileRdClose( cmAudioFileRd* p )
{
  cmRC_t rc = cmOkRC;
  cmRC_t afRC;

  if( p == NULL )
    return cmOkRC;

  //cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  if( cmAudioFileIsOpen(p->h) == false )
    return cmOkRC;

  if((afRC = cmAudioFileDelete(&p->h)) != cmOkRC )
    rc = cmCtxRtCondition( &p->obj, afRC, "An attempt to close the audio file'%s' failed.", p->fn );

  return rc;
}

cmRC_t               cmAudioFileRdRead(  cmAudioFileRd* p )
{
  cmRC_t rc = cmOkRC;
  cmRC_t afRC;

  if(p->eofFl || ((p->eofFl = cmAudioFileIsEOF(p->h)) == true) )
    return cmEofRC;

  unsigned n = p->endFrmIdx==cmInvalidIdx ? p->outN : cmMin( p->outN, p->endFrmIdx - p->curFrmIdx );

  if((afRC =  cmAudioFileReadSample( p->h, n, p->chIdx, 1, &p->outV, &p->lastReadFrmCnt )) != kOkAfRC )
    rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);

  p->curFrmIdx += p->lastReadFrmCnt;

  if( n < p->outN )
  {
    cmVOS_Zero(p->outV + p->lastReadFrmCnt, p->outN - p->lastReadFrmCnt);
    p->eofFl = true;
  }

  if( p->mfp != NULL )
    cmMtxFileSmpExec( p->mfp, p->outV, p->outN );

  return rc;    
}

cmRC_t               cmAudioFileRdSeek( cmAudioFileRd* p, unsigned frmIdx )
{
  cmRC_t rc = cmOkRC;
  cmRC_t afRC;

  if((afRC = cmAudioFileSeek( p->h, frmIdx )) != kOkAfRC )
    rc = cmCtxRtCondition( &p->obj, afRC, "Audio file read failed on:'%s'.", p->fn);

  return rc;
}

cmRC_t             cmAudioFileRdMinMaxMean( cmAudioFileRd* p, unsigned chIdx, cmSample_t* minPtr, cmSample_t* maxPtr, cmSample_t* meanPtr )
{
  cmRC_t rc = cmOkRC;
  cmRC_t afRC;

  if(( afRC = cmAudioFileMinMaxMean( p->h, chIdx, minPtr, maxPtr, meanPtr )) != kOkAfRC )
    rc = cmCtxRtCondition( &p->obj, afRC, "Audio file min, max, and mean calculation failed on '%s'", p->fn );

  return rc;
}



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

cmShiftBuf*       cmShiftBufAlloc( cmCtx* c, cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
{
  cmShiftBuf* op = cmObjAlloc( cmShiftBuf, c, p );

  if( procSmpCnt > 0 && wndSmpCnt > 0 && hopSmpCnt > 0 )
    if( cmShiftBufInit(op, procSmpCnt, wndSmpCnt, hopSmpCnt ) != cmOkRC)
      cmShiftBufFree(&op);

  return op;
}

cmRC_t            cmShiftBufFree(  cmShiftBuf** pp )
{
  cmRC_t rc = cmOkRC;

  if( pp != NULL && *pp != NULL )
  {
    if((rc = cmShiftBufFinal(*pp)) == cmOkRC )
    {
      cmMemPtrFree(&(*pp)->bufV);
      cmObjFree(pp);
    }
  }
  return rc;
}

cmRC_t            cmShiftBufInit(  cmShiftBuf* p, unsigned procSmpCnt, unsigned wndSmpCnt, unsigned hopSmpCnt )
{
  cmRC_t rc;

  if( hopSmpCnt > wndSmpCnt )
    return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The window sample count (%i) must be greater than or equal to the hop sample count (%i).", wndSmpCnt, hopSmpCnt );

  if((rc = cmShiftBufFinal(p)) != cmOkRC )
    return rc;

  // The worst case storage requirement is where there are wndSmpCnt-1 samples in outV[] and procSmpCnt new samples arrive.
  p->bufSmpCnt    = wndSmpCnt + procSmpCnt;
  p->bufV         = cmMemResizeZ( cmSample_t, p->outV, p->bufSmpCnt );
  p->outV         = p->bufV;
  p->outN         = wndSmpCnt;
  p->wndSmpCnt    = wndSmpCnt;
  p->procSmpCnt   = procSmpCnt;
  p->hopSmpCnt    = hopSmpCnt;
  p->inPtr        = p->outV;
  p->fl           = false;

  return cmOkRC;  
}

cmRC_t            cmShiftBufFinal( cmShiftBuf* p )
{
  return cmOkRC;
}


// This function should be called in a loop until it returns false.
// Note that 'sp' and 'sn' are ignored except p->fl == false.
bool           cmShiftBufExec(  cmShiftBuf* p, const cmSample_t* sp, unsigned sn   )
{
  assert( sn <= p->procSmpCnt );

  // The active samples are in outV[wndSmpCnt]
  // Stored samples are between outV + wndSmpCnt and inPtr.

  // if the previous call to this function returned true then the buffer must be
  // shifted by hopSmpCnt samples - AND sp[] is ignored.
  if( p->fl )
  {
    // shift the output buffer to the left to remove expired samples 
    p->outV += p->hopSmpCnt;

    // if there are not  wndSmpCnt samples left in the buffer 
    if( p->inPtr - p->outV < p->wndSmpCnt )
    {
      // then copy the remaining active samples (between outV and inPtr) 
      // to the base of the physicalbuffer
      unsigned n = p->inPtr - p->outV;
      memmove( p->bufV, p->outV, n * sizeof(cmSample_t));

      p->inPtr  = p->bufV + n; // update the input and output positions
      p->outV   = p->bufV;
    }
  }
  else
  {
    // if the previous call to this function returned false then sp[sn] should not be ignored
    assert( p->inPtr + sn <= p->outV + p->bufSmpCnt );
    // copy the incoming samples into the buffer
    cmVOS_Copy(p->inPtr,sn,sp);
    p->inPtr += sn;
  }

  // if there are at least wndSmpCnt available samples in outV[]
  p->fl = p->inPtr - p->outV >= p->wndSmpCnt;
  
  return p->fl;
}


void cmShiftBufTest( cmCtx* c )
{
  unsigned    smpCnt     = 48;
  unsigned    procSmpCnt =  5;

  unsigned    hopSmpCnt  =  6;
  unsigned    wndSmpCnt  =  7;
  unsigned    i;

  cmShiftBuf* b = cmShiftBufAlloc(c,NULL,procSmpCnt,wndSmpCnt,hopSmpCnt );

  cmSample_t x[ smpCnt ];
  cmVOS_Seq(x,smpCnt,1,1);

  //cmVOS_Print( rptFuncPtr, 1, smpCnt, x );
  
  for(i=0; i<smpCnt; i+=procSmpCnt)
  {
    while( cmShiftBufExec( b, x + i, procSmpCnt ) )
    {
      cmVOS_Print( c->obj.err.rpt, 1, wndSmpCnt, b->outV );
    }
  }


  cmShiftBufFree(&b);

}

/*
bool           cmShiftBufExec(  cmShiftBuf* p, const cmSample_t* sp, unsigned sn   )
{
  bool retFl = false;

  if( sn > p->procSmpCnt )
  {
     cmCtxRtAssertFailed( p->obj.ctx, cmArgAssertRC, "The input sample count (%i) must be less than or equal to the proc sample count (%i).", sn, p->procSmpCnt);
     return false;
  }

  assert( sn <= p->procSmpCnt );

  cmSample_t*  dbp = p->outV;
  cmSample_t*  dep = p->outV + (p->outN - sn);
  cmSample_t*  sbp = p->outV + sn;

  // shift the last bufCnt-shiftCnt samples over the first shiftCnt samples
  while( dbp < dep )
    *dbp++ = *sbp++;

  // copy in the new samples 
  dbp = dep;
  dep = dbp + sn;
  while( dbp < dep )
    *dbp++ = *sp++;

  // if any space remains at the end of the buffer then zero it
  dep = p->outV + p->outN;
  while( dbp < dep )
    *dbp++ = 0;


  if( p->firstPtr > p->outV )
    p->firstPtr = cmMax( p->outV, p->firstPtr - p->procSmpCnt);

  p->curHopSmpCnt += sn;

  if( p->curHopSmpCnt >= p->hopSmpCnt )
  {
    p->curHopSmpCnt -= p->hopSmpCnt;
    retFl         = true;
  }

  if( p->mfp != NULL )
    cmMtxFileSmpExec(p->mfp,p->outV,p->outN);

  return retFl;
}
*/

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

cmWndFunc*  cmWndFuncAlloc( cmCtx* c, cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kaiserSideLobeRejectDb )
{
  cmWndFunc* op = cmObjAlloc( cmWndFunc, c, p );

  if( wndId != kInvalidWndId )
    if( cmWndFuncInit(op,wndId,wndSmpCnt,kaiserSideLobeRejectDb ) != cmOkRC )
      cmWndFuncFree(&op);

  return op;
}

cmRC_t      cmWndFuncFree(  cmWndFunc** pp )
{
  cmRC_t rc = cmOkRC;
  if( pp != NULL && *pp != NULL )
  {

    cmWndFunc* p = *pp;
    if((rc = cmWndFuncFinal(p)) == cmOkRC )
    {
      cmMemPtrFree(&p->wndV);
      cmMemPtrFree(&p->outV);
      cmObjFree(pp);
    }
  }

  return rc;
}

cmRC_t      cmWndFuncInit(  cmWndFunc* p, unsigned wndId, unsigned wndSmpCnt, double kslRejectDb )
{
  cmRC_t rc;

  if( wndId == (p->wndId | p->flags) && wndSmpCnt == p->outN && kslRejectDb == p->kslRejectDb )
    return cmOkRC;

  if((rc = cmWndFuncFinal(p)) != cmOkRC )
    return rc;

  p->wndV        = cmMemResize( cmSample_t, p->wndV, wndSmpCnt );
  p->outV        = cmMemResize( cmSample_t, p->outV, wndSmpCnt );
  p->outN        = wndSmpCnt;
  p->wndId       = wndId;
  p->kslRejectDb = kslRejectDb;
  //p->mfp         = cmCtxAllocDebugFile(p->obj.ctx,"wndFunc");
  p->flags       = wndId & (~kWndIdMask);
 
  switch( wndId & kWndIdMask )
  {
    case kHannWndId:       cmVOS_Hann(       p->wndV, p->outN ); break;
    case kHannMatlabWndId: cmVOS_HannMatlab( p->wndV, p->outN ); break;
    case kHammingWndId:    cmVOS_Hamming(    p->wndV, p->outN ); break;
    case kTriangleWndId:   cmVOS_Triangle(   p->wndV, p->outN ); break;
    case kUnityWndId:      cmVOS_Fill(       p->wndV, p->outN, 1.0 ); break;
    case kKaiserWndId:    
      {
        double beta = cmVOS_KaiserBetaFromSidelobeReject(fabs(kslRejectDb));
        cmVOS_Kaiser( p->wndV,p->outN, beta); 
      }
      break;
    case kInvalidWndId:    break;

    default:
      { assert(0); }
  }

  cmSample_t den = 0;
  cmSample_t num = 1;
  if( cmIsFlag(p->flags,kNormBySumWndFl) )
  {
    den = cmVOS_Sum(p->wndV, p->outN);
    num = wndSmpCnt;
  }

  if( cmIsFlag(p->flags,kNormByLengthWndFl) )
    den += wndSmpCnt;
    
  if( den > 0 )
  {
    cmVOS_MultVS(p->wndV,p->outN,num);
    cmVOS_DivVS(p->wndV,p->outN,den);
  }

  return cmOkRC;
}

cmRC_t      cmWndFuncFinal( cmWndFunc* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}



cmRC_t      cmWndFuncExec(  cmWndFunc* p, const cmSample_t* sp, unsigned sn )
{
  if( sn > p->outN )
    return cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "The length of the input vector (%i)  is greater thean the length of the window function (%i).", sn, p->outN ); 

  if( p->wndId != kInvalidWndId )
    cmVOS_MultVVV( p->outV, sn, sp, p->wndV );

  if( p->mfp != NULL )
    cmMtxFileSmpExec(p->mfp,p->outV,p->outN);

  return cmOkRC;

}

void cmWndFuncTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH  )
{

    unsigned wndCnt               = 5;
    double kaiserSideLobeRejectDb = 30;

    cmCtx c;
    cmCtxAlloc(&c,rpt,lhH,stH);
    cmWndFunc*  p = cmWndFuncAlloc(&c,NULL,kHannWndId,wndCnt, 0 );
    cmVOS_Print( rpt, 1, wndCnt, p->wndV );
    
    cmWndFuncInit(p,kHammingWndId ,wndCnt, 0 );
    cmVOS_Print( rpt, 1, wndCnt, p->wndV );

    cmWndFuncInit(p,kTriangleWndId ,wndCnt, 0 );
    cmVOS_Print( rpt, 1, wndCnt, p->wndV );

    cmWndFuncInit(p,kKaiserWndId ,wndCnt, kaiserSideLobeRejectDb );
    cmVOS_Print( rpt, 1, wndCnt, p->wndV );

    cmSample_t wV[ wndCnt ];
    cmVOS_HannMatlab(wV,wndCnt);
    cmVOS_Print( rpt, 1, wndCnt, wV);

    cmWndFuncFree(&p);
}

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

cmSpecDelay* cmSpecDelayAlloc( cmCtx* c, cmSpecDelay* ap, unsigned maxDelayCnt, unsigned binCnt )
{
  cmSpecDelay* p = cmObjAlloc( cmSpecDelay, c, ap );


  if( maxDelayCnt > 0 && binCnt > 0 )
    if( cmSpecDelayInit(p,maxDelayCnt,binCnt) != cmOkRC  )
      cmSpecDelayFree(&p);

  return p;
}

cmRC_t       cmSpecDelayFree(  cmSpecDelay** pp )
{  
  cmRC_t rc = cmOkRC;

  if( pp != NULL && *pp != NULL  )
  {
    cmSpecDelay* p = *pp;
    if((rc=cmSpecDelayFinal(p)) == cmOkRC )
    {
      cmMemPtrFree(&p->bufPtr);
      cmObjFree(pp);
    }
  }
  return rc;
}


cmRC_t            cmSpecDelayInit( cmSpecDelay* p, unsigned maxDelayCnt, unsigned binCnt )
{
  cmRC_t rc;
  if((rc = cmSpecDelayFinal(p)) != cmOkRC )
    return rc;

  p->bufPtr      = cmMemResizeZ( cmSample_t, p->bufPtr, binCnt * maxDelayCnt );
  p->maxDelayCnt = maxDelayCnt;
  p->outN        = binCnt;
  p->inIdx       = 0;
  return cmOkRC;
}

cmRC_t            cmSpecDelayFinal(cmSpecDelay* p )
{ return cmOkRC; }

cmRC_t       cmSpecDelayExec(  cmSpecDelay* p, const cmSample_t* sp, unsigned sn )
{
  cmSample_t* dp = p->bufPtr + (p->inIdx * p->outN); 
  cmVOS_Copy( dp, cmMin(sn,p->outN), sp);
  p->inIdx = (p->inIdx+1) % p->maxDelayCnt;
  return cmOkRC;
}

const cmSample_t* cmSpecDelayOutPtr( cmSpecDelay* p, unsigned delayCnt )
{
  assert( delayCnt < p->maxDelayCnt );

  int i = p->inIdx - delayCnt;
  if( i < 0 )
    i = p->maxDelayCnt + i;

  return p->bufPtr + (i * p->outN);
}

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

cmFilter* cmFilterAlloc( cmCtx* c, cmFilter* ap, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
{
  cmRC_t rc;
  cmFilter* p = cmObjAlloc(cmFilter,c,ap);

  if( (bn > 0 || an > 0) && procSmpCnt > 0 )
    if( (rc = cmFilterInit( p, b, bn, a, an, procSmpCnt, d)) != cmOkRC )
      cmFilterFree(&p);

  return p;
}

cmFilter* cmFilterAllocEllip( cmCtx* c, cmFilter* ap, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
{
  cmRC_t rc;
  cmFilter* p = cmObjAlloc(cmFilter,c,ap);

  if( srate > 0 && passHz > 0 && procSmpCnt > 0 )
    if( (rc = cmFilterInitEllip( p, srate, passHz, stopHz, passDb, stopDb, procSmpCnt, d)) != cmOkRC )
      cmFilterFree(&p);

  return p;
}

cmRC_t cmFilterFree( cmFilter** pp )
{
  cmRC_t rc = cmOkRC;

  if( pp != NULL && *pp != NULL )
  {
    cmFilter* p = *pp;
    if((rc = cmFilterFinal(p))  == cmOkRC )
    {
      cmMemPtrFree(&p->a);
      cmMemPtrFree(&p->b);
      cmMemPtrFree(&p->d);
      cmMemPtrFree(&p->outSmpV);
      cmObjFree(pp);
    }
  }
  return rc;
}

cmRC_t cmFilterInit( cmFilter* p, const cmReal_t* b, unsigned bn, const cmReal_t* a, unsigned an, unsigned procSmpCnt, const cmReal_t* d )
{
  assert( bn >= 1 );
  assert( an >= 1 && a[0] != 0 );

  cmRC_t rc;
  if((rc = cmFilterFinal(p)) != cmOkRC )
    return rc;

  int cn  = cmMax(an,bn) - 1;

  // The output vector may be used as either cmReal_t or cmSample_t.
  // Find the larger of the two possible types.
  if( sizeof(cmReal_t) > sizeof(cmSample_t) )
  {
    p->outRealV   = cmMemResizeZ( cmReal_t, p->outRealV, procSmpCnt );
    p->outSmpV    = (cmSample_t*)p->outRealV;
  }
  else
  {
    p->outSmpV   = cmMemResizeZ( cmSample_t, p->outSmpV, procSmpCnt );
    p->outRealV  = (cmReal_t*)p->outRealV;

  }  

  p->a        = cmMemResizeZ( cmReal_t, p->a, cn );
  p->b        = cmMemResizeZ( cmReal_t, p->b, cn ); 
  p->d        = cmMemResizeZ( cmReal_t, p->d, cn+1 );
  //p->outV   = cmMemResizeZ( cmSample_t, p->outV, procSmpCnt );
  p->outN     = procSmpCnt;
  p->an       = an;
  p->bn       = bn;
  p->cn       = cn;
  p->di       = 0;
  p->b0       = b[0] / a[0];
  
  int i;
  for(i=0; i<an-1; ++i)
    p->a[i] = a[i+1] / a[0];

  for(i=0; i<bn-1; ++i)
    p->b[i] = b[i+1] / a[0];

  if( d != NULL )
    cmVOR_Copy(p->d,cn,d);

  return cmOkRC;
}

// initialize an elliptic lowpass filter with the given characteristics
// ref: Parks & Burrus, Digital Filter Design, sec. 7.2.7 - 7.2.8
cmRC_t cmFilterInitEllip( cmFilter* p, cmReal_t srate, cmReal_t passHz, cmReal_t stopHz, cmReal_t passDb, cmReal_t stopDb, unsigned procSmpCnt, const cmReal_t* d )
{
  assert( srate > 0 );
  assert( passHz > 0 && stopHz > passHz && srate/2 > stopHz );

  cmReal_t  Wp, Ws, ep, v0,
            k,  kc, k1, k1c,
            K,  Kc, K1, K1c,
            sn, cn, dn,
            sm, cm, dm,
            zr, zi, pr, pi;

  unsigned  N,  L,  j;

  // prewarp Wp and Ws, calculate k
  Wp = 2 * srate * tan(M_PI * passHz / srate);
  Ws = 2 * srate * tan(M_PI * stopHz / srate);
  k  = Wp / Ws;

  // calculate ep and k1 from passDb and stopDb
  ep = sqrt(pow(10, passDb/10) - 1);
  k1 = ep / sqrt(pow(10, stopDb/10) - 1);

  // calculate complimentary moduli
  kc  = sqrt(1-k*k);
  k1c = sqrt(1-k1*k1);

  // calculate complete elliptic integrals
  K   = cmEllipK( kc );
  Kc  = cmEllipK( k );
  K1  = cmEllipK( k1c );
  K1c = cmEllipK( k1 );

  // calculate minimum integer filter order N
  N = ceil(K*K1c/Kc/K1);

  // recalculate k and kc from chosen N
  // Ws is minimized while other specs held constant
  k  = cmEllipDeg( K1c/K1/N );
  kc = sqrt(1-k*k);
  K  = cmEllipK( kc );
  Kc = cmEllipK( k );
  Ws = Wp / k;

  // initialize temporary coefficient arrays
  cmReal_t b[N+1], a[N+1];
  a[0] = b[0] = 1;
  memset(b+1, 0, N*sizeof(cmReal_t));
  memset(a+1, 0, N*sizeof(cmReal_t));

  // intermediate value needed for determining poles
  v0 = K/K1/N * cmEllipArcSc( 1/ep, k1 );
  cmEllipJ( v0, k, &sm, &cm, &dm );

  for( L=1-N%2; L<N; L+=2 )
  {
    // find the next pole and zero on s-plane
    cmEllipJ( K*L/N, kc, &sn, &cn, &dn );
    zr =  0;
    zi =  L ? Ws/sn : 1E25;
    pr = -Wp*sm*cm*cn*dn/(1-pow(dn*sm,2));
    pi =  Wp*dm*sn/(1-pow(dn*sm,2));

    // convert pole and zero to z-plane using bilinear transform
    cmBlt( 1, srate, &zr, &zi );
    cmBlt( 1, srate, &pr, &pi );

    if( L == 0 )
    {
      // first order section
      b[1] = -zr;
      a[1] = -pr;
    }
    else
    {
      // replace complex root and its conjugate with 2nd order section
      zi = zr*zr + zi*zi;
      zr *= -2;
      pi = pr*pr + pi*pi;
      pr *= -2;

      // combine with previous sections to obtain filter coefficients
      for( j = L+1; j >= 2; j-- )
      {
        b[j] = b[j] + zr*b[j-1] + zi*b[j-2];
        a[j] = a[j] + pr*a[j-1] + pi*a[j-2];
      }

      b[1] += zr;
      a[1] += pr;
    }
  }

  // scale b coefficients s.t. DC gain is 0 dB
  cmReal_t sumB = 0, sumA = 0;
  for( j = 0; j < N+1; j++ )
  {
    sumB += b[j];
    sumA += a[j];
  }
  sumA /= sumB;
  for( j = 0; j < N+1; j++ )
    b[j] *= sumA;

  return cmFilterInit( p, b, N+1, a, N+1, procSmpCnt, d );
}

cmRC_t cmFilterFinal( cmFilter* p )
{ return cmOkRC; }


cmRC_t cmFilterExecS(  cmFilter* p, const cmSample_t* x, unsigned xn, cmSample_t* yy, unsigned yn )
{
  
  cmSample_t* y;

  if( yy == NULL || yn==0 )
  {
    y  = p->outSmpV;
    yn = p->outN;
  }
  else
  {
    y = yy;
  }

  cmVOS_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );
  return cmOkRC;
  
  /*

  int i,j;
  cmSample_t  y0 = 0;
  cmSample_t* y;
  unsigned    n;

  if( yy == NULL || yn==0 )
  {
    n  = cmMin(p->outN,xn);
    y  = p->outSmpV;
    yn = p->outN;
  }
  else
  {
    n = cmMin(yn,xn);
    y = yy;
  }

  // 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)
  {
    //cmSample_t x0 = x[i];

    y[i] = (x[i] * p->b0) + p->d[0];

    y0 = y[i];

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


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

  return cmOkRC;
  */
}

cmRC_t cmFilterExecR(  cmFilter* p, const cmReal_t* x, unsigned xn, cmReal_t* yy, unsigned yn )
{

  cmReal_t* y;

  if( yy == NULL || yn==0 )
  {
    y  = p->outRealV;
    yn = p->outN;
  }
  else
  {
    //n = cmMin(yn,xn);
    y = yy;
  }

  cmVOR_Filter( y, yn, x, xn, p->b0, p->b, p->a, p->d, p->cn );

  return cmOkRC;
}

cmRC_t    cmFilterSignal( cmCtx* c, const cmReal_t b[], unsigned bn, const cmReal_t a[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
{
  int procSmpCnt = cmMin(1024,xn);
  cmFilter* p = cmFilterAlloc(c,NULL,b,bn,a,an,procSmpCnt,NULL);

  int i,n;

  for(i=0; i<xn && i<yn; i+=n)
  {
    n = cmMin(procSmpCnt,cmMin(yn-i,xn-i));
    cmFilterExecS(p,x+i,n,y+i,n);
  }

  if( i < yn )
    cmVOS_Fill(y+i,yn-i,0);

  cmFilterFree(&p);

  return cmOkRC;
}

cmRC_t    cmFilterFilterS(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmSample_t* x, unsigned xn, cmSample_t* y, unsigned yn )
{
  cmFilter* f  = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  cmVOS_FilterFilter( f, cmFilterExecS, bb,bn,aa,an,x,xn,y,yn);
  cmFilterFree(&f);
  return cmOkRC;
}

cmRC_t    cmFilterFilterR(cmCtx* c, const cmReal_t bb[], unsigned bn, const cmReal_t aa[], unsigned an, const cmReal_t* x, unsigned xn, cmReal_t* y, unsigned yn )
{
  cmFilter* f  = cmFilterAlloc(c,NULL,NULL,0,NULL,0,0,NULL);
  cmVOR_FilterFilter( f, cmFilterExecR, bb,bn,aa,an,x,xn,y,yn);
  cmFilterFree(&f);
  return cmOkRC;
}


void   cmFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
  cmCtx c;
  cmCtxAlloc(&c, rpt, lhH, stH );

  cmReal_t   b[] = { 0.16, 0.32, 0.16 };
  unsigned   bn  = sizeof(b)/sizeof(cmReal_t);

  cmReal_t   a[] = {1 , -.5949, .2348 };
  unsigned   an  = sizeof(a)/sizeof(cmReal_t);

  cmReal_t   x[] = { 1,0,0,0,1,0,0,0 };
  unsigned   xn  = sizeof(x)/sizeof(cmReal_t);

  cmReal_t d[] = { .5, -.25};

  //  0.1600     0.4152     0.3694     0.1223     0.1460     0.3781     0.3507     0.1198   
  // -0.0111    -0.0281 

  cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn,d);

  cmFilterExecR(p,x,xn,NULL,0);

  cmVOR_Print( rpt, 1, xn, p->outRealV );  

  cmVOR_Print( rpt, 1, p->cn, p->d );  

  cmFilterFree(&p);

  cmObjFreeStatic( cmCtxFree, cmCtx, c );

  /*
  cmReal_t   b[] = { 0.16, 0.32, 0.16 };
  unsigned   bn  = sizeof(b)/sizeof(cmReal_t);

  cmReal_t   a[] = { 1, -.5949, .2348};
  unsigned   an  = sizeof(a)/sizeof(cmReal_t);
  
  cmSample_t x[] = { 1,0,0,0,0,0,0,0,0,0 };
  unsigned   xn  = sizeof(x)/sizeof(cmSample_t);

  cmFilter* p = cmFilterAlloc(&c,NULL,b,bn,a,an,xn);

  cmFilterExec(&c,p,x,xn,NULL,0);
  cmVOS_Print( vReportFunc, 1, xn, p->outV );  

  cmVOR_Print( vReportFunc, 1, p->cn, p->d );  

  cmFilterExec(&c,p,x,xn,NULL,0);
  cmVOS_Print( vReportFunc, 1, xn, p->outV ); 

  cmFilterFree(&p);
  */

}

void   cmFilterFilterTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
  cmCtx c;
  cmCtxAlloc(&c, rpt, lhH, stH );

  cmReal_t   b[] = { 0.36, 0.32, 0.36 };
  unsigned   bn  = sizeof(b)/sizeof(cmReal_t);

  cmReal_t   a[] = {1 , -.5949, .2348 };
  unsigned   an  = sizeof(a)/sizeof(cmReal_t);

  cmReal_t   x[] = { 1,0,1,0,1,0,1,0,1,0,1,0,1,0,1,0 };
  unsigned   xn  = sizeof(x)/sizeof(cmReal_t);

  unsigned yn = xn;
  cmReal_t  y[yn];
  memset(y,0,sizeof(y));

  cmFilterFilterR(&c, b,bn,a,an,x,xn,y,yn );

  cmVOR_Print( rpt, 1, yn, y );  

  cmObjFreeStatic( cmCtxFree, cmCtx, c );

  

}

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

cmComplexDetect* cmComplexDetectAlloc(cmCtx* c, cmComplexDetect* p, unsigned binCnt )
{
  cmComplexDetect* op = cmObjAlloc( cmComplexDetect, c, p );
  
  cmSpecDelayAlloc(c,&op->phsDelay,0,0);
  cmSpecDelayAlloc(c,&op->magDelay,0,0);  

  if( binCnt > 0 )
    if( cmComplexDetectInit(op,binCnt) != cmOkRC && p == NULL )
      cmComplexDetectFree(&op);

  return op;
}

cmRC_t          cmComplexDetectFree( cmComplexDetect** pp )
{
  cmRC_t rc;
  if( pp != NULL && *pp != NULL )
  {
    cmComplexDetect* p = *pp;

    if((rc = cmComplexDetectFinal(p)) == cmOkRC )
    {
      cmSpecDelay* sdp;
      sdp = &p->phsDelay;
      cmSpecDelayFree(&sdp);
      sdp = &p->magDelay;
      cmSpecDelayFree(&sdp);

      cmObjFree(pp);
    }
  }

  return cmOkRC;
}

cmRC_t cmComplexDetectInit( cmComplexDetect* p, unsigned binCnt )
{
  cmRC_t rc;
  if((rc = cmComplexDetectFinal(p)) != cmOkRC )
    return rc;

  cmSpecDelayInit(&p->phsDelay,2,binCnt);
  cmSpecDelayInit(&p->magDelay,1,binCnt);
  p->binCnt      = binCnt;
  //p->mfp         = cmCtxAllocDebugFile(p->obj.ctx,"complexDetect");
  //p->cdfSpRegId  = cmStatsProcReg( p->obj.ctx->statsProcPtr, kCDF_FId, 1 );

  return cmOkRC;
}

cmRC_t           cmComplexDetectFinal( cmComplexDetect* p)
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t          cmComplexDetectExec( cmComplexDetect* p, const cmSample_t* magV, const cmSample_t* phsV, unsigned binCnt  )
{
  p->out = cmVOS_ComplexDetect( magV, cmSpecDelayOutPtr(&p->magDelay,0), phsV, cmSpecDelayOutPtr(&p->phsDelay,1), cmSpecDelayOutPtr(&p->phsDelay,0), binCnt);
  p->out /= 10000000;

  cmSpecDelayExec(&p->magDelay,magV,binCnt);
  cmSpecDelayExec(&p->phsDelay,phsV,binCnt);

  //if( p->mfp != NULL )
  //  cmMtxFileSmpExec(  p->mfp, &p->out, 1 );

  return cmOkRC;
}

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

cmSample_t _cmComplexOnsetMedian( const cmSample_t* sp, unsigned sn, void* userPtr )
{ return cmVOS_Median(sp,sn); }

cmComplexOnset* cmComplexOnsetAlloc( cmCtx* c, cmComplexOnset* p, unsigned procSmpCnt, double srate, unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
{
  cmComplexOnset* op = cmObjAlloc( cmComplexOnset, c, p );

  if( procSmpCnt > 0 && srate > 0 && medFiltWndSmpCnt > 0 )
    if( cmComplexOnsetInit( op, procSmpCnt, srate, medFiltWndSmpCnt, threshold, frameCnt ) != cmOkRC )
      cmComplexOnsetFree(&op);

  return op;
}

cmRC_t          cmComplexOnsetFree(  cmComplexOnset** pp)
{
  cmRC_t          rc = cmOkRC;
  cmComplexOnset* p  = *pp;  

  if( pp==NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmComplexOnsetFinal(*pp)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->df);
  cmMemPtrFree(&p->fdf);
  cmObjFree(pp);

  return cmOkRC;
}

cmRC_t cmComplexOnsetInit( cmComplexOnset* p, unsigned procSmpCnt, double srate,  unsigned medFiltWndSmpCnt, double threshold, unsigned frameCnt )
{

  cmRC_t rc;
  if(( rc = cmComplexOnsetFinal(p)) != cmOkRC )
    return rc;

  p->frmCnt    = frameCnt;
  p->dfi       = 0;
  p->df        = cmMemResizeZ( cmSample_t, p->df,  frameCnt );
  p->fdf       = cmMemResizeZ( cmSample_t, p->fdf, frameCnt );
  p->onrate    = 0;
  p->threshold = threshold;
  p->medSmpCnt = medFiltWndSmpCnt;
  //p->mfp       = cmCtxAllocDebugFile(p->obj.ctx,"complexOnset");
  return cmOkRC;
}

cmRC_t          cmComplexOnsetFinal(  cmComplexOnset* p)
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}


cmRC_t          cmComplexOnsetExec(  cmComplexOnset* p, cmSample_t cdf )
{
  p->df[p->dfi++] = cdf;
  return cmOkRC;
}

cmRC_t          cmComplexOnsetCalc(  cmComplexOnset* p )
{
  // df -= mean(df)
  cmVOS_SubVS(p->df,p->frmCnt,cmVOS_Mean(p->df,p->frmCnt));

  // low pass forward/backward filter df[] into fdf[]
  double d = 2 + sqrt(2);
  cmReal_t b[] = {1/d, 2/d, 1/d};
  unsigned bn  = sizeof(b)/sizeof(b[0]);
  cmReal_t a[] = {1, 0, 7-2*d};
  unsigned an  = sizeof(a)/sizeof(a[0]);
  cmFilterFilterS(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);

  // median filter to low-passed filtered fdf[] into df[]
  cmVOS_FnThresh(p->fdf,p->frmCnt,p->medSmpCnt,p->df,1,NULL);

  // subtract med filtered signal from the low passed signal.
  // fdf[] -= df[];
  cmVOS_SubVV(p->fdf,p->frmCnt,p->df);
  cmVOS_SubVS(p->fdf,p->frmCnt,p->threshold);
  cmSample_t *bp = p->df,
             *ep = bp + p->frmCnt - 1,
             *dp = p->fdf + 1;
  *bp++ = *ep = 0;
  for( ; bp<ep; bp++,dp++)
  {
    *bp = (*dp > *(dp-1) && *dp > *(dp+1) && *dp > 0) ? 1 : 0;
    p->onrate += *bp;
  }

  p->onrate /= p->frmCnt;

  /*
  if( p->mfp != NULL )
  {
    bp = p->df;
    ep = bp + p->frmCnt;
    while( bp < ep )
      cmMtxFileSmpExec(  p->mfp, bp++, 1 );
  }
  */

  return cmOkRC;
}

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

cmMfcc* cmMfccAlloc( cmCtx* c, cmMfcc* ap, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
{
  cmMfcc* p = cmObjAlloc( cmMfcc, c, ap );

  if( melBandCnt > 0 &&  binCnt > 0 && dctCoeffCnt > 0 )
    if( cmMfccInit( p, srate, melBandCnt, dctCoeffCnt, binCnt ) != cmOkRC )
      cmMfccFree(&p);
  return p;
}

cmRC_t  cmMfccFree(  cmMfcc** pp )
{
  cmRC_t rc = cmOkRC;

  if( pp==NULL ||  *pp == NULL )
    return cmOkRC;

  cmMfcc* p = *pp;

  if( (rc = cmMfccFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->melM);
  cmMemPtrFree(&p->dctM);
  cmMemPtrFree(&p->outV);
  cmObjFree(pp);
  
  return rc;
}

cmRC_t  cmMfccInit(  cmMfcc* p, double srate, unsigned melBandCnt, unsigned dctCoeffCnt, unsigned binCnt )
{
  cmRC_t rc;
  if((rc = cmMfccFinal(p)) != cmOkRC )
    return rc;

  p->melM = cmMemResize( cmReal_t, p->melM, melBandCnt  * binCnt );
  p->dctM = cmMemResize( cmReal_t, p->dctM, dctCoeffCnt * melBandCnt );
  p->outV = cmMemResize( cmReal_t, p->outV, dctCoeffCnt );

  // each row of the matrix melp contains a mask
  cmVOR_MelMask(  p->melM, melBandCnt,  binCnt, srate, kShiftMelFl );

  // each row contains melBandCnt elements
  cmVOR_DctMatrix(p->dctM, dctCoeffCnt, melBandCnt ); 

  p->melBandCnt  = melBandCnt;
  p->dctCoeffCnt = dctCoeffCnt;
  p->binCnt      = binCnt;
  p->outN        = dctCoeffCnt;
  //p->mfp         = cmCtxAllocDebugFile(p->obj.ctx,"mfcc");

  //if( p->obj.ctx->statsProcPtr != NULL )
  //  p->mfccSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kMFCC_FId, p->outN );

  return cmOkRC;
}

cmRC_t  cmMfccFinal( cmMfcc* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t  cmMfccExecPower(  cmMfcc* p, const cmReal_t* magPowV, unsigned binCnt )
{
  assert( binCnt == p->binCnt );

  cmReal_t t[ p->melBandCnt ];

  // apply the mel filter mask to the power spectrum
  cmVOR_MultVMV( t, p->melBandCnt, p->melM, binCnt, magPowV );

  // convert mel bands to dB
  cmVOR_PowerToDb( t, p->melBandCnt, t );
 
  // decorellate the bands with a DCT
  cmVOR_MultVMV( p->outV, p->dctCoeffCnt, p->dctM, p->melBandCnt, t );


  /*
  cmPlotSelectSubPlot(0,0);
  cmPlotClear();
  //cmPlotLineS( "power",  NULL, magPowV, NULL, 35, NULL, kSolidPlotLineId  );    
  cmPlotLineS( "mel",   NULL, t0, NULL, p->melBandCnt, NULL, kSolidPlotLineId  );    

  cmPlotSelectSubPlot(1,0);
  cmPlotClear();
  //cmPlotLineS( "meldb",  NULL, t1, NULL, p->melBandCnt, NULL, kSolidPlotLineId  );    
  cmPlotLineS( "mfcc",  NULL, p->outV+1, NULL, p->dctCoeffCnt-1, NULL, kSolidPlotLineId  );    
  */

  if( p->mfp != NULL )
    cmMtxFileRealExec(p->mfp,p->outV, p->outN);

  

  return cmOkRC;
}

cmRC_t  cmMfccExecAmplitude(  cmMfcc* p, const cmReal_t* magAmpV, unsigned binCnt )
{
  cmReal_t t[ binCnt ];

  cmVOR_MultVVV( t,binCnt, magAmpV, magAmpV );

  cmMfccExecPower(p,t,binCnt);

  if( p->mfp != NULL )
    cmMtxFileRealExec(p->mfp,p->outV, p->outN);

  return cmOkRC;
}

//------------------------------------------------------------------------------------------------------------
enum { cmSonesEqlConBinCnt = kEqualLoudBandCnt, cmSonesEqlConCnt=13 };

cmSones* cmSonesAlloc( cmCtx* c, cmSones*  ap, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
{
  cmSones* p = cmObjAlloc( cmSones, c, ap );

  if( srate > 0 && barkBandCnt > 0 && binCnt > 0 )
    if( cmSonesInit(p,srate,barkBandCnt,binCnt,flags) != cmOkRC )
      cmSonesFree(&p);
  return p; 
}

cmRC_t   cmSonesFree(  cmSones** pp )
{
  cmRC_t   rc = cmOkRC;
  cmSones* p  = *pp;

  if( pp==NULL || *pp==NULL)
    return cmOkRC;

  if((rc = cmSonesFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->ttmV);
  cmMemPtrFree(&p->sfM);
  cmMemPtrFree(&p->barkIdxV);
  cmMemPtrFree(&p->barkCntV);
  cmMemPtrFree(&p->outV);
  cmObjFree(pp);

  return rc;
}

cmRC_t   cmSonesInit(  cmSones*  p, double srate, unsigned barkBandCnt, unsigned binCnt, unsigned flags )
{
  p->ttmV     = cmMemResize( cmReal_t, p->ttmV,     binCnt);
  p->sfM      = cmMemResize( cmReal_t, p->sfM,      binCnt*barkBandCnt);
  p->barkIdxV = cmMemResize( unsigned, p->barkIdxV, barkBandCnt);
  p->barkCntV = cmMemResize( unsigned, p->barkCntV, barkBandCnt);
  p->outV     = cmMemResize( cmReal_t, p->outV,     barkBandCnt);

  // calc outer ear filter
  cmVOR_TerhardtThresholdMask( p->ttmV, binCnt, srate, kNoTtmFlags );

  // calc shroeder spreading function
  cmVOR_ShroederSpreadingFunc(p->sfM, barkBandCnt, srate);

  // calc the bin to bark maps
  p->barkBandCnt     = cmVOR_BarkMap( p->barkIdxV, p->barkCntV, barkBandCnt, binCnt, srate );  

  //unsigned i = 0;
  //for(; i<barkBandCnt; ++i)
  //  printf("%i %i %i\n", i+1, barkIdxV[i]+1, barkCntV[i]);

  bool     elFl      = cmIsFlag(p->flags, kUseEqlLoudSonesFl);

  p->binCnt          = binCnt;
  p->outN            = elFl ? cmSonesEqlConCnt : p->barkBandCnt;
  p->overallLoudness = 0;
  p->flags           = flags;
  //p->mfp             = cmCtxAllocDebugFile(p->obj.ctx,"sones");
  //p->sonesSpRegId    = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSones_FId, p->outN );
  //p->loudSpRegId     = cmStatsProcReg( p->obj.ctx->statsProcPtr, kLoud_FId,  1 );
  return cmOkRC;  
}

cmRC_t   cmSonesFinal( cmSones*  p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}



cmRC_t   cmSonesExec(  cmSones*  p, const cmReal_t* magPowV, unsigned binCnt )
{
  assert( binCnt == p->binCnt );

  // Equal-loudness and phon map from: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion, 

  // equal-loudness contours
  double eqlcon[cmSonesEqlConCnt][cmSonesEqlConBinCnt] = 
    { 
      {12,7,4,1,0,0,0,-0.5,-2,-3,-7,-8,-8.5,-8.5,-8.5},
      {20,17,14,12,10,9.5,9,8.5,7.5,6.5,4,3,2.5,2,2.5},
      {29,26,23,21,20,19.5,19.5,19,18,17,15,14,13.5,13,13.5},
      {36,34,32,30,29,28.5,28.5,28.5,28,27.5,26,25,24.5,24,24.5},
      {45,43,41,40,40,40,40,40,40,39.5,38,37,36.5,36,36.5},
      {53,51,50,49,48.5,48.5,49,49,49,49,48,47,46.5,45.5,46},
      {62,60,59,58,58,58.5,59,59,59,59,58,57.5,57,56,56},
      {70,69,68,67.5,67.5,68,68,68,68,68,67,66,65.5,64.5,64.5},
      {79,79,79,79,79,79,79,79,78,77.5,76,75,74.5,73,73},
      {89,89,89,89.5,90,90,90,89.5,89,88.5,87,86,85.5,84,83.5},
      {100,100,100,100,100,99.5,99,99,98.5,98,96,95,94.5,93.5,93},
      {112,112,112,112,111,110.5,109.5,109,108.5,108,106,105,104.5,103,102.5},
      {122,122,121,121,120.5,120,119,118,117,116.5,114.5,113.5,113,111, 110.5}
    };

  // loudness levels (phone scales) 
  double phons[cmSonesEqlConCnt]=  {0.0,10.0,20.0,30.0,40.0,50.0,60.0,70.0,80.0,90.0,100.0,110.0,120.0};

  unsigned i,j;

  cmReal_t t0[ binCnt ];
  cmReal_t t1[ p->barkBandCnt ];
  cmReal_t t2[ p->barkBandCnt ];

  unsigned* idxV = p->barkIdxV; 
  unsigned* cntV = p->barkCntV; 
  cmReal_t* sfM  = p->sfM;

  // apply the outer ear filter
  cmVOR_MultVVV( t0, binCnt, magPowV, p->ttmV);


  // apply the bark frequency warping
  for(i=0; i<p->barkBandCnt; ++i)
  {
    if( cntV[i] == 0 )
      t1[i] = 0;
    else
    {
      t1[i] = t0[ idxV[i] ];
      
      for(j=1; j<cntV[i]; ++j)
        t1[i] += t0[ idxV[i] + j ];
    }
  }


  // apply the spreading filters
  cmVOR_MultVMV( t2, p->barkBandCnt, sfM, p->barkBandCnt, t1 );

  bool     elFl      = cmIsFlag(p->flags, kUseEqlLoudSonesFl);
  unsigned bandCnt   = elFl ? cmMin(p->barkBandCnt,cmSonesEqlConBinCnt) : p->barkBandCnt;

  //p->outN            = elFl ? cmSonesEqlConCnt                          : p->barkBandCnt;
  p->overallLoudness = 0;

  for( i = 0; i <bandCnt; i++ )
  {
    // if using the equal-loudness contours begin with the third bark band
    // and end with the 18th bark band
    unsigned k = elFl ? i+3 : i;

    if( k < p->barkBandCnt )
    {
      double v = t2[k];

      // convert to db
      v = 10*log10( v<1 ? 1 : v );

      if( elFl )
      {
        // db to phons 
        // see: Y. Wonho, 1999, EMBSD: an objective speech quality measure based on audible distortion, 

        j = 0;

        // find the equal loudness curve  for this frequency and db level
        while( v >= eqlcon[j][i] )
          ++j;

        if( j == cmSonesEqlConCnt )
        {
          cmCtxRtAssertFailed( &p->obj, cmArgAssertRC, "Bark band %i is out of range during equal-loudness mapping.",j );
          continue;
        }

        // convert db to phons
        if( j == 0 )
          v = phons[0];
        else 
        {
          double t1  = ( v - eqlcon[j-1][i] ) / ( eqlcon[j][i] - eqlcon[j-1][i] );
          v = phons[j-1] + t1 * (phons[j] - phons[j-1]);
        }
      }

      // convert to sones
      // bladon and lindblom, 1981, JASA, modelling the judment of vowel quality differences

      if( v >= 40 )
        p->outV[i] = pow(2,(v-40)/10);
      else
        p->outV[i] = pow(v/40,2.642);
      
      p->overallLoudness += p->outV[i];

    }

  }

  if( p->mfp != NULL )
    cmMtxFileRealExec( p->mfp, p->outV, p->outN );

  return cmOkRC;
}

void cmSonesTest()
{
  cmKbRecd   kb;
  double     srate   = 44100;
  unsigned   bandCnt =    23;
  unsigned   binCnt  =   513;
  cmSample_t tv[ binCnt ];
  cmSample_t sm[ bandCnt * bandCnt ];
  cmSample_t t[  bandCnt * bandCnt ];
  unsigned   binIdxV[ bandCnt ];
  unsigned   cntV[ bandCnt ];
  unsigned i;

  cmPlotSetup("Sones",1,1);

  cmVOS_TerhardtThresholdMask(tv,binCnt,srate, kModifiedTtmFl );
  
  cmVOS_ShroederSpreadingFunc(sm, bandCnt, srate);

  cmVOS_Transpose( t, sm, bandCnt, bandCnt );
 
  bandCnt = cmVOS_BarkMap(binIdxV,cntV, bandCnt, binCnt, srate );

  for(i=0; i<bandCnt; ++i)
    printf("%i %i %i\n", i, binIdxV[i], cntV[i] );

  for(i=0; i<bandCnt; ++i )
  {
    cmPlotLineS( NULL,  NULL,  t+(i*bandCnt), NULL, bandCnt, NULL, kSolidPlotLineId  );    
  }
 
  //cmPlotLineS( NULL,  NULL, tv, NULL, binCnt, NULL, kSolidPlotLineId  );    

  cmPlotDraw();
  cmKeyPress(&kb);    

}


//------------------------------------------------------------------------------------------------------------
  cmAudioOffsetScale* cmAudioOffsetScaleAlloc( cmCtx* c, cmAudioOffsetScale* ap, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
{
  cmAudioOffsetScale* p = cmObjAlloc( cmAudioOffsetScale, c, ap );


  if( procSmpCnt > 0 && srate > 0 )
    if( cmAudioOffsetScaleInit( p, procSmpCnt, srate, offset, rmsWndSecs, dBref, flags ) != cmOkRC )
      cmAudioOffsetScaleFree(&p);
  return p;
}

cmRC_t        cmAudioOffsetScaleFree(  cmAudioOffsetScale** pp )
{
  cmRC_t              rc = cmOkRC;
  cmAudioOffsetScale* p  = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;


  if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->cBufPtr);
  cmMemPtrFree(&p->cCntPtr);
  cmMemPtrFree(&p->outV);

  cmObjFree(pp);

  return rc;
}

cmRC_t        cmAudioOffsetScaleInit(  cmAudioOffsetScale* p, unsigned procSmpCnt, double srate, cmSample_t offset, double rmsWndSecs, double dBref, unsigned flags )
{
  assert( procSmpCnt > 0 && srate > 0);

  cmRC_t rc;
  if((rc = cmAudioOffsetScaleFinal(p)) != cmOkRC )
    return rc;

  p->cBufCnt = 0;

  if( cmIsFlag(flags, kRmsAudioScaleFl) )
  {
    if( rmsWndSecs > 0 )
    {
      unsigned rmsSmpCnt = srate * rmsWndSecs;
      p->cBufCnt =  (unsigned)ceil( rmsSmpCnt / procSmpCnt );

      if( p->cBufCnt > 0 )
      {
        p->cBufPtr = cmMemResizeZ( cmReal_t, p->cBufPtr, p->cBufCnt );
        p->cCntPtr = cmMemResizeZ( unsigned, p->cCntPtr, p->cBufCnt );
      }
    }
    else
    {
      p->cBufCnt = 0;
      p->cBufPtr = NULL;
      p->cCntPtr = NULL;
    }
  }

  p->cBufIdx    = 0;
  p->cBufCurCnt = 0;
  p->cBufSum    = 0;
  p->cCntSum    = 0;
  p->outV       = cmMemResize( cmSample_t, p->outV, procSmpCnt );
  p->outN       = procSmpCnt;
  p->offset     = offset;
  p->dBref      = dBref;
  p->flags      = flags;
  //p->mfp        = cmCtxAllocDebugFile(p->obj.ctx,"audioOffsetScale");
  return cmOkRC;

}

cmRC_t        cmAudioOffsetScaleFinal( cmAudioOffsetScale* p )
{
  //if( p != NULL)
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t        cmAudioOffsetScaleExec(  cmAudioOffsetScale* p, const cmSample_t* sp, unsigned sn )
{

  double            Pref    = 20.0 / 1000000; // 20 micro Pascals
  cmSample_t*       dbp     = p->outV;
  const cmSample_t* dep     = dbp + sn;
  double            scale   = 0;


  // if no scaling was requested then add offset only
  if( cmIsFlag(p->flags, kNoAudioScaleFl) )
  {
    while( dbp < dep )
      *dbp++ = *sp++ + p->offset;
    goto doneLabel;
  }

  // if fixed scaling
  if( cmIsFlag(p->flags, kFixedAudioScaleFl) )
  {
    if( scale == 0 )
      scale = pow(10,p->dBref/20);

    while( dbp < dep )
      *dbp++ = (*sp++ + p->offset) * scale;

  }
  else // if RMS scaling
  {
    double sum = 0;
    double rms = 0;


    while( dbp < dep )
    {
      double v = (*sp++ + p->offset) / Pref;

      sum += v*v;

      *dbp++ = v;
    }

    // if there is no RMS buffer calc RMS on procSmpCnt samles
    if( p->cBufCnt == 0 )
      rms = sqrt( sum / sn );
    else
    {
      p->cBufSum   -= p->cBufPtr[ p->cBufIdx ];
      p->cBufSum   += sum;
      p->cCntSum   -= p->cCntPtr[ p->cBufIdx ];
      p->cCntSum   += sn;
      p->cBufIdx    = (p->cBufIdx+1) % p->cBufCnt;
      p->cBufCurCnt = cmMin( p->cBufCurCnt+1, p->cBufCnt );
 
      assert( p->cCntSum > 0 );

      rms           = sqrt( p->cBufSum / p->cCntSum );
    }

    double sigSPL = 20*log10(rms);

    scale   = pow(10,(p->dBref - sigSPL)/20); 

    dbp = p->outV;
    while( dbp < dep )
      *dbp++ *= scale;

  }

 doneLabel:
  dbp = p->outV + sn;
  dep = p->outV + p->outN;
  while( dbp < dep )
    *dbp++ = 0;

  if( p->mfp != NULL )
    cmMtxFileSmpExec(p->mfp,p->outV,p->outN);

  return cmOkRC;

}

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

cmSpecMeas* cmSpecMeasAlloc( cmCtx* c, cmSpecMeas* ap, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
{
  cmSpecMeas* p = cmObjAlloc( cmSpecMeas, c, ap );

  if( srate > 0 && binCnt > 0 )
    if( cmSpecMeasInit( p, srate, binCnt, wndFrmCnt, flags ) != cmOkRC )
      cmSpecMeasFree(&p);
  return p;
}

cmRC_t      cmSpecMeasFree(  cmSpecMeas** pp )
{
  cmRC_t      rc = cmOkRC;
  cmSpecMeas* p  = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmSpecMeasFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->rmsV);
  cmMemPtrFree(&p->hfcV);
  cmMemPtrFree(&p->scnV);
  cmObjFree(pp);
  return rc;

}

cmRC_t      cmSpecMeasInit(  cmSpecMeas* p, double srate, unsigned binCnt, unsigned wndFrmCnt, unsigned flags )
{
  cmRC_t rc;
  if((rc = cmSpecMeasFinal(p)) != cmOkRC )
    return rc;

  if( cmIsFlag(flags, kUseWndSpecMeasFl) )
  {
    p->rmsV = cmMemResizeZ( cmReal_t, p->rmsV, wndFrmCnt );
    p->hfcV = cmMemResizeZ( cmReal_t, p->hfcV, wndFrmCnt );
    p->scnV = cmMemResizeZ( cmReal_t, p->scnV, wndFrmCnt );
  }

  p->rmsSum     = 0;
  p->hfcSum     = 0;
  p->scnSum     = 0;

  p->binCnt     = binCnt;
  p->flags      = flags;
  p->wndFrmCnt  = wndFrmCnt;
  p->frameCnt   = 0;
  p->frameIdx   = 0;
  p->binHz      = srate / ((binCnt-1) * 2);
  //p->mfp        = cmCtxAllocDebugFile(p->obj.ctx,"specMeas");

  //p->rmsSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kRMS_FId, 1);
  //p->hfcSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kHFC_FId, 1);
  //p->scSpRegId  = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSC_FId,  1);
  //p->ssSpRegId  = cmStatsProcReg( p->obj.ctx->statsProcPtr, kSS_FId,  1);

  return cmOkRC;
}

cmRC_t      cmSpecMeasFinal( cmSpecMeas* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t      cmSpecMeasExec(  cmSpecMeas* p, const cmReal_t* magPowV, unsigned binCnt )
{
  assert( binCnt == p->binCnt );

  unsigned        i      = 0;
  const cmReal_t* sbp    = magPowV;
  const cmReal_t* sep    = sbp + binCnt;
  cmReal_t        rmsSum = 0;
  cmReal_t        hfcSum = 0;
  cmReal_t        scnSum = 0;
  cmReal_t        ssSum  = 0;

  for(i=0; sbp < sep; ++i, ++sbp )
  {
    rmsSum += *sbp;
    hfcSum += *sbp * i;
    scnSum += *sbp * i * p->binHz;
  }

  p->frameCnt++;

  if( cmIsFlag(p->flags, kUseWndSpecMeasFl) )
  {
    p->frameCnt = cmMin( p->frameCnt, p->wndFrmCnt );
    
    cmReal_t* rmsV = p->rmsV + p->frameIdx;
    cmReal_t* hfcV = p->hfcV + p->frameIdx; 
    cmReal_t* scnV = p->scnV + p->frameIdx; 

    p->rmsSum -= *rmsV;
    p->hfcSum -= *hfcV;
    p->scnSum -= *scnV;

    *rmsV = rmsSum;
    *hfcV = hfcSum;
    *scnV = scnSum;

    p->frameIdx = (p->frameIdx+1) % p->frameCnt;    

  }

  p->rmsSum += rmsSum;
  p->hfcSum += hfcSum;
  p->scnSum += scnSum;


  p->rms    = sqrt(p->rmsSum / (p->binCnt * p->frameCnt) );
  p->hfc    = p->hfcSum / ( p->binCnt * p->frameCnt );
  p->sc     = p->scnSum / cmMax( cmReal_EPSILON, p->rmsSum );

    
  sbp = magPowV;
  for(i=0; sbp < sep; ++i )
  {
    cmReal_t t = (i*p->binHz) - p->sc;
    ssSum += *sbp++ * (t*t);
  }

  p->ss = sqrt(ssSum / cmMax( cmReal_EPSILON, p->rmsSum ));
  
  if( p->mfp != NULL )
  {
    cmReal_t a[4] = { p->rms, p->hfc, p->sc, p->ss };
    cmMtxFileRealExec(  p->mfp, a, 4 );
  }

  return cmOkRC;
}

//------------------------------------------------------------------------------------------------------------
cmSigMeas* cmSigMeasAlloc( cmCtx* c, cmSigMeas* ap, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
{
  cmSigMeas* p = cmObjAlloc( cmSigMeas, c, ap );

  p->sbp = cmShiftBufAlloc(c,&p->shiftBuf,0,0,0);

  if( srate > 0 && procSmpCnt > 0 && measSmpCnt > 0 )
    if( cmSigMeasInit( p, srate, procSmpCnt, measSmpCnt ) != cmOkRC )
      cmSigMeasFree(&p);
  return p;
}

cmRC_t     cmSigMeasFree(  cmSigMeas** pp )
{
  cmRC_t rc = cmOkRC;
  cmSigMeas* p = *pp;

  if( pp==NULL || *pp==NULL)
    return cmOkRC;

  if((rc = cmSigMeasFinal(p)) != cmOkRC )
    return rc;

  cmShiftBufFree(&p->sbp);
  
  cmObjFree(pp);
  return rc;
}

cmRC_t     cmSigMeasInit(  cmSigMeas* p, double srate, unsigned procSmpCnt, unsigned measSmpCnt )
{
  cmRC_t rc;

  if((rc = cmSigMeasFinal(p)) != cmOkRC )
    return rc;

  if( procSmpCnt != measSmpCnt )
    cmShiftBufInit( p->sbp, procSmpCnt, measSmpCnt, procSmpCnt );

  p->zcrDelay   = 0;
  p->srate      = srate;
  p->measSmpCnt = measSmpCnt;
  p->procSmpCnt = procSmpCnt;
  //p->zcrSpRegId = cmStatsProcReg( p->obj.ctx->statsProcPtr, kZCR_FId, 1 );
  //p->mfp        = cmCtxAllocDebugFile(p->obj.ctx,"sigMeas");

  return cmOkRC;
}

cmRC_t     cmSigMeasFinal( cmSigMeas* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t     cmSigMeasExec(  cmSigMeas* p, const cmSample_t* sp, unsigned sn )
{  
  if( p->procSmpCnt != p->measSmpCnt )
  {
    cmShiftBufExec( p->sbp, sp, sn );
    sp = p->sbp->outV;
    sn = p->sbp->wndSmpCnt;
    assert( p->sbp->wndSmpCnt == p->measSmpCnt );
  }

  unsigned zcn  =    cmVOS_ZeroCrossCount( sp, sn, NULL );
  p->zcr = (cmReal_t)zcn * p->srate / p->measSmpCnt;

  if( p->mfp != NULL )
    cmMtxFileRealExec(  p->mfp, &p->zcr, 1 );

  return cmOkRC;
}


//------------------------------------------------------------------------------------------------------------
cmSRC* cmSRCAlloc( cmCtx* c, cmSRC* ap, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
{
  cmSRC* p = cmObjAlloc( cmSRC, c,ap );

  cmFilterAlloc(c,&p->filt,NULL,0,NULL,0,0,NULL);
 
  if( srate > 0 && procSmpCnt > 0 )
    if( cmSRCInit( p, srate, procSmpCnt, upFact, dnFact ) != cmOkRC )
      cmSRCFree(&p);

  return p;
}

cmRC_t cmSRCFree(  cmSRC** pp )
{
  cmRC_t rc;

  if( pp != NULL && *pp != NULL )
  {
    cmSRC* p = *pp;

    if((rc = cmSRCFinal( p )) == cmOkRC )
    {
      cmFilter* fp = &p->filt;
      cmFilterFree(&fp);
      cmMemPtrFree(&p->outV);
      cmObjFree(pp);
    }
  }
  return cmOkRC;
}

cmRC_t cmSRCInit(  cmSRC* p, double srate, unsigned procSmpCnt, unsigned upFact, unsigned dnFact )
{
  cmRC_t rc;

  if((rc = cmSRCFinal(p)) != cmOkRC )
    return rc;

  double hiRate = upFact * srate;
  double loRate = hiRate / dnFact;
  double minRate= cmMin( loRate, srate ); 
  double fcHz   = minRate/2;
  double dHz    = (fcHz * .1); // transition band is 5% of min sample rate 
  double passHz = fcHz-dHz;
  double stopHz = fcHz;
  double passDb = 0.001;
  double stopDb = 20;
 
  cmFilterInitEllip( &p->filt, hiRate, passHz, stopHz,  passDb, stopDb, procSmpCnt, NULL );

  //printf("CoeffCnt:%i dHz:%f passHz:%f stopHz:%f passDb:%f stopDb:%f\n", p->fir.coeffCnt, dHz, passHz, stopHz, passDb, stopDb );

  p->outN   = (unsigned)ceil(procSmpCnt * upFact / dnFact); 
  p->outV   = cmMemResize( cmSample_t, p->outV, p->outN );  
  p->upi    = 0;
  p->dni    = 0;
  p->upFact = upFact;
  p->dnFact = dnFact;
  //p->mfp    = cmCtxAllocDebugFile(p->obj.ctx,"src");
  return cmOkRC;
}

cmRC_t cmSRCFinal( cmSRC* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t cmSRCExec(  cmSRC* p, const cmSample_t* sp, unsigned sn )
{
  const cmSample_t* sep = sp + sn;
  cmSample_t*       op  = p->outV;
  const cmSample_t* oep = op + p->outN;
  unsigned          iN  = sn * p->upFact;
  unsigned          i,j; 

  // run the filter at the upsampled rate ...
  for(i=0; i<iN; ++i) 
  {
    assert( p->upi!=0 || sp<sep ); 

    cmSample_t x0 = p->upi==0 ? *sp++ : 0,
               y0 = x0 * p->filt.b0 + p->filt.d[0];

    // ... but output at the down sampled rate
    if( p->dni==0 )
    {
      assert( op < oep );
      *op++ = y0;
    }

    // advance the filter delay line
    for(j=0; j<p->filt.cn; ++j)
      p->filt.d[j] = p->filt.b[j]*x0 - p->filt.a[j]*y0 + p->filt.d[j+1];

    // update the input/output clocks
    p->upi = (p->upi + 1) % p->upFact;
    p->dni = (p->dni + 1) % p->dnFact;
    
  }

  p->outN = op - p->outV;

  if( p->mfp != NULL )
    cmMtxFileSmpExec(p->mfp,p->outV,p->outN );

  return cmOkRC;
}


//------------------------------------------------------------------------------------------------------------
cmConstQ* cmConstQAlloc( cmCtx* c, cmConstQ* ap, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
{
  cmConstQ* p = cmObjAlloc( cmConstQ, c, ap );

  if( srate >0  )
    if( cmConstQInit(p,srate,minMidiPitch,maxMidiPitch,binsPerOctave,thresh) != cmOkRC )
      cmConstQFree(&p);
  return p;
}

cmRC_t    cmConstQFree(  cmConstQ** pp )
{
  cmRC_t    rc;
  cmConstQ* p = *pp;

  if( pp==NULL || *pp==NULL)
    return cmOkRC;

  if((rc = cmConstQFinal(p)) != cmOkRC )
    return rc;
  
  cmMemPtrFree(&p->fiV);
  cmMemPtrFree(&p->foV);
  cmMemPtrFree(&p->skM);
  cmMemPtrFree(&p->outV);
  cmMemPtrFree(&p->magV);
  cmMemPtrFree(&p->skBegV);
  cmMemPtrFree(&p->skEndV);
  cmObjFree(pp);

  return cmOkRC;
}



cmRC_t    cmConstQInit(  cmConstQ* p, double srate, unsigned minMidiPitch, unsigned maxMidiPitch, unsigned binsPerOctave, double thresh )
{
  cmRC_t rc;
  if((rc = cmConstQFinal(p)) != cmOkRC )
    return rc;

  cmReal_t        minHz = cmMidiToHz(minMidiPitch);
  cmReal_t        maxHz = cmMidiToHz(maxMidiPitch);
  cmReal_t        Q     = 1.0/(pow(2,(double)1.0/binsPerOctave)-1);
  unsigned        K     = (unsigned)ceil( binsPerOctave * log2(maxHz/minHz) );
  unsigned        fftN  = cmNextPowerOfTwo( (unsigned)ceil(Q*srate/minHz) );
  unsigned        k     = 0;
  p->fiV                = cmMemResize(cmComplexR_t, p->fiV, fftN);
  p->foV                = cmMemResize(cmComplexR_t, p->foV, fftN);
  
  cmFftPlanR_t     plan = cmFft1dPlanAllocR(fftN, p->fiV, p->foV, FFTW_FORWARD, FFTW_ESTIMATE );

  p->wndSmpCnt    = fftN;
  p->constQBinCnt = K;
  p->binsPerOctave= binsPerOctave;
  p->skM          = cmMemResizeZ( cmComplexR_t, p->skM,     p->wndSmpCnt * p->constQBinCnt);
  p->outV         = cmMemResizeZ( cmComplexR_t, p->outV,    p->constQBinCnt);
  p->magV         = cmMemResizeZ( cmReal_t,     p->magV,    p->constQBinCnt);
  p->skBegV       = cmMemResizeZ( unsigned,     p->skBegV,  p->constQBinCnt);
  p->skEndV       = cmMemResizeZ( unsigned,     p->skEndV,  p->constQBinCnt);


  //p->mfp          = cmCtxAllocDebugFile( p->obj.ctx, "constQ");

  //printf("hz:%f %f bpo:%i sr:%f thresh:%f Q:%f K%i (cols)  fftN:%i (rows)\n", minHz,maxHz,binsPerOctave,srate,thresh,Q,K,fftN);

  double* hamm = NULL;

  // note that the bands are created in reverse order 
  for(k=0; k<K; ++k)
  {
    unsigned iN    = ceil( Q * srate / (minHz * pow(2,(double)k/binsPerOctave)));
    unsigned start = fftN/2;
    //double   hamm[ iN ];
    hamm = cmMemResizeZ(double,hamm,iN);

    memset( p->fiV, 0, fftN * sizeof(cmComplexR_t));
    memset( p->foV, 0, fftN * sizeof(cmComplexR_t));

    cmVOD_Hamming( hamm, iN );

    cmVOD_DivVS(  hamm, iN, iN );

    if( cmIsEvenU(iN) )
      start -=  iN/2;
    else
      start -= (iN+1)/2;

    //printf("k:%i iN:%i start:%i %i\n",k,iN,start,start+iN-1);

    unsigned i = start;
    for(; i<=start+iN-1; ++i)
    {
      double arg = 2.0*M_PI*Q*(i-start)/iN;
      double mag = hamm[i-start];

      p->fiV[i-1]   = (mag * cos(arg)) + (mag * I * sin(arg));       
    }
    
    cmFftExecuteR(plan);

    // since the bands are created in reverse order they are also stored in reverse order
    // (i.e column k-1 is stored first and column 0 is stored last)
    i=0;
    unsigned minIdx = -1;
    unsigned maxIdx = 0;
    for(; i<fftN; ++i)
    {
      bool fl = cabs(p->foV[i]) <= thresh;

      p->skM[ (k*p->wndSmpCnt) + i ] =  fl ? 0 : p->foV[i]/fftN;

      if( fl==false && minIdx == -1 )
        minIdx = i;

      if( fl==false && i>maxIdx )
        maxIdx = i;      
    }

    p->skBegV[k] = minIdx;
    p->skEndV[k] = maxIdx;

  }

  cmMemPtrFree(&hamm);

  cmFftPlanFreeR(plan);

 
  return cmOkRC;
}

cmRC_t    cmConstQFinal( cmConstQ* p )
{
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t    cmConstQExec(  cmConstQ* p, const cmComplexR_t* ftV, unsigned srcBinCnt )
{
  //acVORC_MultVVM( p->outV, p->constQBinCnt,ftV,p->wndSmpCnt, p->skM );
  
  cmReal_t*            mbp = p->magV;
  cmComplexR_t*        dbp = p->outV;
  const cmComplexR_t*  dep = p->outV + p->constQBinCnt;
  
  unsigned i = 0;

  for(; dbp < dep; ++dbp,++i,++mbp )
  {
    const cmComplexR_t*    sbp = ftV + p->skBegV[i];
    const cmComplexR_t*    kp  = p->skM + (i*p->wndSmpCnt) + p->skBegV[i];
    const cmComplexR_t*    ep  = kp + (p->skEndV[i] - p->skBegV[i]) + 1;

    *dbp = 0;
    while( kp < ep )
      *dbp +=   *sbp++ * *kp++; 

    *mbp = cmCabsR(*dbp);
  }
 

  if( p->mfp != NULL )
    cmMtxFileComplexExec(  p->mfp, p->outV, p->constQBinCnt, 1 );

  return cmOkRC;
 
}

void cmConstQTest( cmConstQ* p )
{
  cmKbRecd kb;

  unsigned i,j;
  cmSample_t* t = cmMemAlloc( cmSample_t, p->wndSmpCnt );

  for(i=0; i<p->constQBinCnt; ++i)
  {
    for(j=0; j<p->wndSmpCnt; ++j)
      t[j] = cabs( p->skM[ (i*p->wndSmpCnt) + j ]);


    //cmPlotClear();
    cmPlotLineS( NULL,  NULL, t, NULL, 500, NULL, kSolidPlotLineId  );    
  }

  cmPlotDraw();
  cmKeyPress(&kb);

  cmMemPtrFree(&t);

}



//------------------------------------------------------------------------------------------------------------
cmHpcp* cmTunedHpcpAlloc( cmCtx* c, cmHpcp* ap, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
{
  cmHpcp* p = cmObjAlloc( cmHpcp, c, ap );

  if( binsPerOctave > 0 && constQBinCnt >> 0 )
    if( cmTunedHpcpInit( p, binsPerOctave, constQBinCnt, cqMinMidiPitch, frameCnt, medFiltOrder ) != cmOkRC)
      cmTunedHpcpFree(&p);
  return p;
}

cmRC_t         cmTunedHpcpFree(  cmHpcp** pp )
{
  cmRC_t rc = cmOkRC;
  cmHpcp* p = *pp;

  if(pp==NULL || *pp==NULL)
    return cmOkRC;

  if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
    return rc;
  
  cmMemPtrFree(&p->hpcpM);
  cmMemPtrFree(&p->fhpcpM);
  cmMemPtrFree(&p->histV);
  cmMemPtrFree(&p->outM);
  cmMemPtrFree(&p->meanV);
  cmMemPtrFree(&p->varV);

  cmObjFree(pp);

  return cmOkRC;
}

cmRC_t         cmTunedHpcpInit(  cmHpcp* p, unsigned binsPerOctave, unsigned constQBinCnt, unsigned cqMinMidiPitch, unsigned frameCnt, unsigned medFiltOrder )
{
  assert( binsPerOctave % kStPerOctave == 0 );
  assert( cmIsOddU( binsPerOctave / kStPerOctave ) );

  cmRC_t rc;
  if((rc = cmTunedHpcpFinal(p)) != cmOkRC )
    return rc;

  p->histN  = binsPerOctave/kStPerOctave;
  p->hpcpM  = cmMemResizeZ( cmReal_t, p->hpcpM,  frameCnt*binsPerOctave );
  p->fhpcpM = cmMemResizeZ( cmReal_t, p->fhpcpM, binsPerOctave*frameCnt );
  p->histV  = cmMemResizeZ( unsigned, p->histV,  p->histN );
  p->outM   = cmMemResizeZ( cmReal_t, p->outM,   kStPerOctave * frameCnt );
  p->meanV  = cmMemResizeZ( cmReal_t, p->meanV,  kStPerOctave );
  p->varV   = cmMemResizeZ( cmReal_t, p->varV,   kStPerOctave );

  p->constQBinCnt  = constQBinCnt;
  p->binsPerOctave = binsPerOctave;
  p->frameCnt      = frameCnt;
  p->frameIdx      = 0;
  p->cqMinMidiPitch= cqMinMidiPitch;
  p->medFiltOrder  = medFiltOrder;
  //p->mf0p          = cmCtxAllocDebugFile(p->obj.ctx,"hpcp");
  //p->mf1p          = cmCtxAllocDebugFile(p->obj.ctx,"fhpcp");
  //p->mf2p          = cmCtxAllocDebugFile(p->obj.ctx,"chroma");
  return cmOkRC;
}

cmRC_t         cmTunedHpcpFinal( cmHpcp* p )
{
  /*
  if( p != NULL )
  {
    cmCtxFreeDebugFile(p->obj.ctx,&p->mf0p);
    cmCtxFreeDebugFile(p->obj.ctx,&p->mf1p);
    cmCtxFreeDebugFile(p->obj.ctx,&p->mf2p);
  }
  */
  return cmOkRC;
}

cmRC_t         cmTunedHpcpExec(  cmHpcp* p, const cmComplexR_t* cqp, unsigned cqn )
{
  assert( cqn == p->constQBinCnt );

  // if there is no space to store the output then do nothing
  if( p->frameIdx >= p->frameCnt )
    return cmOkRC;

  unsigned        octCnt = (unsigned)floor(p->constQBinCnt / p->binsPerOctave);
  unsigned        i;
  cmReal_t        hpcpV[ p->binsPerOctave + 2 ];
  unsigned        idxV[ p->binsPerOctave ];
  unsigned        binsPerSt = p->binsPerOctave / kStPerOctave;
  

  // Notice that the first and last elements of p->hpcp are reserved for
  // use in producing the appeareance of circularity for the peak picking
  // algorithm.  The cmtual hpcp[] data begins on the index 1 (not 0) and 
  // ends on p->binsPerOctave (not p->binsPerOctave-1).
    
  // sum the constQBinCnt constant Q bins into binsPerOctave bins to form the HPCP
  for(i=0; i<p->binsPerOctave; ++i)
  {
    cmReal_t           sum = 0;
    const cmComplexR_t* sbp = cqp + i;
    const cmComplexR_t* sep = cqp + (octCnt * p->binsPerOctave);

    for(; sbp < sep; sbp += p->binsPerOctave)
      sum += cmCabsR(*sbp);

    hpcpV[i+1] = sum;
  }

  // shift the lowest ST center bin to (binsPerSt+1)/2 such that an equal number of
  // flat and sharp bins are above an below it
  int rotateCnt = ((binsPerSt+1)/2) - 1;
  
  // shift pitch class C to the lowest semitone boundary 
  rotateCnt -= ( 48-(int)p->cqMinMidiPitch) * binsPerSt;

  // perform the shift
  cmVOR_Rotate(hpcpV+1, p->binsPerOctave, rotateCnt);

  // duplicate the first and last bin to produce circularity in the hpcp
  hpcpV[0]                    = hpcpV[ p->binsPerOctave ];
  hpcpV[ p->binsPerOctave+1 ] = hpcpV[1];

  // locate the indexes of the positive peaks in the hpcp
  unsigned pkN       = cmVOR_PeakIndexes( idxV, p->binsPerOctave, hpcpV, p->binsPerOctave, 0 );

  // Convert the peak indexes to values in the range 0 to binsPerSet-1
  // If stPerBin == 3 :  0=flat 1=in tune 2=sharp
  cmVOU_Mod( idxV, pkN, binsPerSt );
  
  // Form a histogram to keep count of the number of flat,in-tune and sharp peaks
  cmVOU_Hist( p->histV, binsPerSt, idxV, pkN );

  // store the hpcpV[] to the row p->hpcpM[p->frameIdx,:]
  cmVOR_CopyN( p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt, hpcpV+1, 1 );
    
  // write the hpcp debug file
  if( p->mf0p != NULL )
    cmMtxFileRealExecN(  p->mf0p, p->hpcpM + p->frameIdx, p->binsPerOctave, p->frameCnt );

  p->frameIdx++; 
  
  return cmOkRC;

}

cmRC_t         cmTunedHpcpTuneAndFilter( cmHpcp* p)
{
  // note: p->frameIdx now holds the cmtual count of frames in p->hpcpA[].
  //       p->frameCnt holds the allocated count of frames in p->hpcpA[].

  unsigned i,j;

  // filter each column of hpcpA[] into each row of fhpcpA[]
  for(i=0; i<p->binsPerOctave; ++i)
  {
    cmVOR_MedianFilt( p->hpcpM + (i * p->frameCnt), p->frameIdx, p->medFiltOrder, p->fhpcpM + i, p->binsPerOctave );  

    // write the fhpcp[i,:] to the debug file
    if( p->mf1p != NULL )
      cmMtxFileRealExecN(  p->mf1p, p->fhpcpM + i, p->frameIdx, p->binsPerOctave );

  }

  unsigned binsPerSt = p->histN;

  assert( (binsPerSt > 0) && (cmIsOddU(binsPerSt)) );

  
  unsigned maxIdx    = cmVOU_MaxIndex(p->histV,binsPerSt,1);
  int      tuneShift = -(maxIdx - ((binsPerSt+1)/2));
  cmReal_t gaussWndV[ binsPerSt ];

  // generate a gaussian window
  cmVOR_GaussWin( gaussWndV, binsPerSt, 2.5 );

  // Rotate the window to apply tuning via the weighted sum operation below
  // (the result will be equivalent to rotating p->fhpcpM[] prior to reducing the )
  cmVOR_Rotate(gaussWndV, binsPerSt, tuneShift);

  // zero the meanV[] before summing into it
  cmVOR_Fill(p->meanV,kStPerOctave,0);

  // for each frame
  for(i=0; i<p->frameIdx; ++i)
  {
    // for each semitone
    for(j=0; j<kStPerOctave; ++j)
    {
      // reduce each semitone to a single value by forming a weighted sum of all the assoc'd bins
      cmReal_t sum = cmVOR_MultSumVV( gaussWndV, p->fhpcpM + (i*p->binsPerOctave) + (j*binsPerSt), binsPerSt );

      // store time-series output to the ith column
      p->outM[ (i*kStPerOctave) + j ] = sum;

      // calc the sum of all chroma values in bin j. 
      p->meanV[ j ] += sum;
    }      

    // write the chroma debug file
    if( p->mf2p != NULL )
      cmMtxFileRealExec( p->mf2p, p->outM + (i*kStPerOctave), kStPerOctave );

  }

  // form the chroma mean from the sum calc'd above
  cmVOR_DivVS( p->meanV, kStPerOctave, p->frameIdx );

  // variance
  for(j=0; j<kStPerOctave; ++j)
    p->varV[j] = cmVOR_VarianceN( p->outM + j, p->frameIdx, kStPerOctave, p->meanV + j );


  return cmOkRC;    
}


//------------------------------------------------------------------------------------------------------------
/*
cmStatsProc* cmStatsProcAlloc( cmCtx* c, cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
{
  cmStatsProc* op = cmObjAlloc(cmStatsProc,c,p);

  if( wndEleCnt > 0 )
    if( cmStatsProcInit(op, wndEleCnt, flags ) != cmOkRC )
      cmStatsProcFree(&op);
  
  if( op != NULL )
    cmCtxSetStatsProc(c,op);

  return op;
}


cmRC_t       cmStatsProcFree(  cmStatsProc** pp )
{
  cmRC_t       rc  = cmOkRC;
  cmStatsProc* p   = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmStatsProcFinal(p) ) != cmOkRC )
    return rc;

  cmCtxSetStatsProc(p->obj.ctx,NULL); // complement to cmSetStatsProc() in cmStatsProcAlloc()

  cmMemPtrFree(&p->regArrayV);
  cmMemPtrFree(&p->m);
  cmMemPtrFree(&p->sumV);
  cmMemPtrFree(&p->meanV);
  cmMemPtrFree(&p->varV);
  cmObjFree(pp);

  return cmOkRC;    
}

cmRC_t       cmStatsProcInit(  cmStatsProc* p, unsigned wndEleCnt, unsigned flags )
{
  cmRC_t rc;

  if((rc = cmStatsProcFinal(p)) != cmOkRC )
    return rc;

  p->wndEleCnt = wndEleCnt;
  p->dimCnt    = 0;
  p->curIdx    = 0;
  p->curCnt    = 1;
  p->flags     = flags;
  p->execCnt   = 0;
  p->regArrayN = 0;
  //p->mfp       = cmCtxAllocDebugFile(p->obj.ctx,"statsProc");
  return rc;
}

cmRC_t cmStatsProcFinal( cmStatsProc* p )
{
  if( p != NULL )
    cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

unsigned     cmStatsProcReg(    cmStatsProc* p, unsigned featId, unsigned featEleCnt )
{
  cmStatsProcRecd r;

  r.featId = featId;
  r.idx    = p->dimCnt;
  r.cnt    = featEleCnt;

  p->dimCnt += featEleCnt;

  //unsigned i;
  // printf("B:\n");
  //for(i=0; i<p->regArrayN; ++i)
  //  printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
  //printf("\nA:\n");
  
  p->regArrayV = cmMemResizePZ( cmStatsProcRecd, p->regArrayV, p->regArrayN+1 );
  p->regArrayV[ p->regArrayN ] = r;
  p->regArrayN++;


  //for(i=0; i<p->regArrayN; ++i)
  //  printf("fid:%i idx:%i cnt:%i : fid:%i cnt:%i\n", p->regArrayV[i].featId,p->regArrayV[i].idx,p->regArrayV[i].cnt,featId,featEleCnt);
  //printf("\n");

  return  p->regArrayN-1; // return the index of the new reg recd
}

const cmStatsProcRecd* cmStatsProcRecdPtr( cmStatsProc* p, unsigned regId )
{
  assert( regId < p->regArrayN );
  return  p->regArrayV + regId;
}

cmRC_t       cmStatsProcExecD(  cmStatsProc* p, unsigned regId, const double v[],    unsigned vCnt )
{
  cmRC_t rc = cmOkRC;

  // on the first pass allcoate the storage buffer (m) and vectors (sumV,meanV and varV)
  if( p->execCnt == 0 )
  {
    p->sumV  = cmMemResizeZ( double, p->sumV,  p->dimCnt);
    p->meanV = cmMemResizeZ( double, p->meanV, p->dimCnt );
    p->varV  = cmMemResizeZ( double, p->varV,  p->dimCnt );
    p->m     = cmMemResizeZ( double, p->m,     p->dimCnt * p->wndEleCnt );
  }

  // if the storage matrix is full 
  if( p->curIdx == p->wndEleCnt )
    return rc;

  // get the pointer to this data source reg recd
  assert( regId < p->regArrayN);
  cmStatsProcRecd* r = p->regArrayV + regId;

  // the dimensionality of the incoming data must be <= the registered dimensionality
  assert( r->cnt <= vCnt );

  unsigned dimIdx  = r->idx;
  bool    updateFl = cmIsFlag(p->flags,kUpdateOnExecStatProcFl);
  double* sbp      = p->sumV + dimIdx;                     // sum base ptr

  // mbp point to a segment (mbp[vCnt]) in column p->curIdx
  double* mbp = p->m + (p->curIdx * p->dimCnt) + dimIdx;   // mem col base ptr

  const double* mep = p->m + p->dimCnt * p->wndEleCnt;

  // decr the current col segment from the sum
  if( updateFl )
    cmVOD_SubVV( sbp, vCnt, mbp );

  assert( p->m <= mbp && mbp < mep && p->m <= mbp+vCnt && mbp+vCnt <= mep );

  // copy in the incoming values to mem col segment
  cmVOD_Copy(  mbp, vCnt, v );

  if( updateFl )
  {
    // incr the sum from the incoming value 
    cmVOD_AddVV( sbp, vCnt, mbp );

    // use the new sum to compute new mean values
    cmVOD_DivVVS( p->meanV + dimIdx, vCnt, sbp, p->curCnt );


    // update the variance - cmross each row 
    unsigned di;

    for(di=dimIdx; di<dimIdx+vCnt; ++di )
      p->varV[di] = cmVOD_VarianceN( p->m + dimIdx, p->curCnt, p->dimCnt, p->meanV + dimIdx );
  }

  ++p->execCnt;

  return cmOkRC;
}


cmRC_t       cmStatsProcExecF(  cmStatsProc* p, unsigned regId, const float  v[],   unsigned vCnt )
{
  double dv[ vCnt ];
  cmVOD_CopyF(dv,vCnt,v);
  cmStatsProcExecD(p,regId,dv,vCnt);
  return cmOkRC;
}

cmRC_t       cmStatsProcCalc(cmStatsProc* p )
{
  unsigned colCnt = cmMin(p->curCnt,p->wndEleCnt);
  unsigned i      = 0;

  cmVOD_Fill(p->sumV,p->dimCnt,0);
 
  // sum the ith col of p->m[] into p->sumV[i]
  for(; i<colCnt; ++i)
  {
    cmVOD_AddVV( p->sumV, p->dimCnt, p->m + (i * p->dimCnt) );
    if( p->mfp != NULL )
      cmMtxFileDoubleExec(  p->mfp, p->sumV, p->dimCnt, 1 );
  }

  // calc the mean of each row
  cmVOD_DivVVS( p->meanV, p->dimCnt, p->sumV, colCnt );

  // calc the variance cmross each row
  for(i=0; i<p->dimCnt; ++i)
    p->varV[i] = cmVOD_VarianceN(p->m + i, colCnt, p->dimCnt, p->meanV + i );

  return cmOkRC;
  
}

cmRC_t       cmStatsProcAdvance(    cmStatsProc* p )
{
  ++p->curIdx;
  if( p->curIdx > p->wndEleCnt )
    p->curIdx = 0;

  p->curCnt = cmMin(p->curCnt+1,p->wndEleCnt);

  return cmOkRC;
}


void         cmStatsProcTest( cmVReportFuncPtr_t vReportFunc )
{
  enum
  {
    wndEleCnt = 7,
    dDimCnt   = 3,
    fDimCnt   = 2,
    dimCnt    = dDimCnt + fDimCnt,

    kTypeId0 = 0,
    kTypeId1 = 1
  };

  unsigned flags     = 0;
  unsigned i;

  double dd[ dDimCnt * wndEleCnt ] =
    { 
      0, 1, 2, 
      3, 4, 5, 
      6, 7, 8, 
      9,  10, 11, 
      12, 13, 14, 
      15, 16, 17, 
      18, 19, 20 
    };

  float fd[ 14 ] = 
    {
      0, 1,
      2, 3,
      4, 5,
      6, 7,
      8, 9,
      10, 11,
      12, 13
    };

  cmCtx c;
  cmCtxInit(&c, vReportFunc, vReportFunc, NULL );
  cmStatsProc* p = cmStatsProcAlloc( &c, NULL, wndEleCnt, flags );

  unsigned regId0 = cmStatsProcReg( p, kTypeId0, dDimCnt );
  unsigned regId1 = cmStatsProcReg( p, kTypeId1, fDimCnt );
  

  for(i=0; i<wndEleCnt; ++i)
  {
    cmStatsProcExecD( p, regId0, dd + (i*dDimCnt), dDimCnt );
    cmStatsProcExecF( p, regId1, fd + (i*fDimCnt), fDimCnt );
    cmStatsProcAdvance(p);
  }

  cmStatsProcCalc( p);
  cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->meanV );
  cmVOD_PrintE( vReportFunc, 1, p->dimCnt, p->varV );

  cmStatsProcFree(&p);

}
*/

//------------------------------------------------------------------------------------------------------------
cmBeatHist* cmBeatHistAlloc( cmCtx* c, cmBeatHist* ap, unsigned frmCnt )
{
  cmBeatHist* p = cmObjAlloc(cmBeatHist,c,ap);
  
  p->fft  = cmFftAllocRR(c,NULL,NULL,0,kToPolarFftFl);
  p->ifft = cmIFftAllocRR(c,NULL,0);

  if( frmCnt > 0 )
    if( cmBeatHistInit(p,frmCnt) != cmOkRC )
      cmBeatHistFree(&p);

  return p;
}

cmRC_t      cmBeatHistFree(  cmBeatHist** pp )
{
  cmRC_t rc = cmOkRC;
  cmBeatHist* p = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmBeatHistFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->m);
  cmMemPtrFree(&p->H);
  cmMemPtrFree(&p->df);
  cmMemPtrFree(&p->fdf);
  cmMemPtrFree(&p->histV);
  cmFftFreeRR(&p->fft);
  cmIFftFreeRR(&p->ifft);

  cmObjFree(&p);
  return rc;

}

void _cmBeatHistInitH( cmReal_t* H, unsigned hrn, unsigned hcn, unsigned ri, unsigned c0, unsigned c1 )
{
  unsigned ci;
  for(ci=c0; ci<=c1; ++ci)
    H[ (ci*hrn) + ri ] = 1;
}

cmRC_t      cmBeatHistInit(  cmBeatHist* p, unsigned frmCnt )
{
  cmRC_t rc;
  unsigned i,j,k;
  enum { kLagFact = 4, kHistBinCnt=15, kHColCnt=128 };

  if((rc = cmBeatHistFinal(p)) != cmOkRC )
    return rc;  

  p->frmCnt    = frmCnt;
  p->maxLagCnt = (unsigned)floor(p->frmCnt / kLagFact);
  p->histBinCnt= kHistBinCnt;
  p->hColCnt   = kHColCnt;
  p->dfi       = 0;
  
  unsigned cfbMemN = p->frmCnt     * p->maxLagCnt;
  unsigned hMemN   = p->histBinCnt * kHColCnt;

  //cmArrayResizeVZ(p->obj.ctx,&p->cfbMem, cmReal_t, &p->m, cfbMemN, &p->H, hMemN, NULL);
  p->m = cmMemResizeZ( cmReal_t, p->m, cfbMemN );
  p->H = cmMemResizeZ( cmReal_t, p->H, hMemN );

  //p->df    = cmArrayResizeZ(c,&p->dfMem, 2*p->frmCnt + kHistBinCnt, cmReal_t);
  //p->fdf   = p->df  + p->frmCnt;
  //p->histV = p->fdf + p->frmCnt;
  //cmArrayResizeVZ(p->obj.ctx, &p->dfMem, cmReal_t, &p->df, p->frmCnt, &p->fdf, p->frmCnt, &p->histV, kHistBinCnt, NULL );
  p->df    = cmMemResizeZ( cmReal_t, p->df,    p->frmCnt );
  p->fdf   = cmMemResizeZ( cmReal_t, p->fdf,   p->frmCnt );
  p->histV = cmMemResizeZ( cmReal_t, p->histV, kHistBinCnt );
  
  cmFftInitRR( p->fft,NULL,cmNextPowerOfTwo(2*frmCnt),kToPolarFftFl);
  cmIFftInitRR(p->ifft,p->fft->binCnt);

  // initialize H
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 0, 103, 127 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 1, 86,  102 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 2, 73,   85 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 3, 64,   72 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 4, 57,   63 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 5, 51,   56 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 6, 46,   50 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 7, 43,   45 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 8, 39,   42 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 9, 36,   38 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 10,32,   35 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 11,28,   31 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 12,25,   27 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 13,21,   24 );
  _cmBeatHistInitH( p->H,  p->histBinCnt, p->hColCnt, 14,11,   20 );  

  // for each column
  for(i=0; i<p->maxLagCnt; ++i)
  {
    
    // for each lag group
    for(j=0; j<kLagFact; ++j)
    {
      for(k=0; k<=2*j; ++k)
      {
        unsigned idx = (i*p->frmCnt) + (i*j) + i + k;
        if( idx < cfbMemN )
          p->m[ idx ] = 1.0/(2*j+1);
      }
    }
  }

  double g[ p->maxLagCnt ];
  double g_max = 0;
  double b     = 43;

  b = b*b;
  for(i=0; i<p->maxLagCnt; ++i)
  {
    double n = i+1;
    g[i] = n/b * exp(-(n*n) / (2*b));
    if( g[i] > g_max )
      g_max = g[i];
  }

  // normalize g[]
  cmVOD_DivVS( g, p->maxLagCnt, g_max );

  // for each column of p->m[]
  for(i=0; i<p->maxLagCnt; ++i)
  {
    double gg = g[i];

    k = i*p->frmCnt;

    for(j=0; j<p->frmCnt; ++j)
      p->m[  k + j ] *= gg;
  }

  //p->mfp = cmCtxAllocDebugFile(p->obj.ctx,"beatHist");
  return cmOkRC;
}

cmRC_t      cmBeatHistFinal( cmBeatHist* p )
{  
  //if( p != NULL )
  //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  return cmOkRC;
}

cmRC_t      cmBeatHistExec(  cmBeatHist* p, cmSample_t df  )
{
  if( p->dfi < p->frmCnt )
    p->df[p->dfi++] = df;
  return cmOkRC;
}

cmRC_t cmBeatHistCalc( cmBeatHist* p )
{
  unsigned i;

  // df -= mean(df)
  cmVOR_SubVS(p->df,p->frmCnt,cmVOR_Mean(p->df,p->frmCnt));

  //cmPlotLineR( "dfm",  NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId  );  

  // take alpha norm of df
  double alpha = 9;
  cmVOR_DivVS(p->df,p->frmCnt, cmVOR_AlphaNorm(p->df,p->frmCnt,alpha));


  //cmPlotLineS( "dfd",  NULL, p->df, NULL, p->frmCnt, NULL, kSolidPlotLineId  );  

  // low pass forward/backward filter df[] into fdf[]
  cmReal_t b[] = {0.1600, 0.3200, 0.1600};
  unsigned bn  = sizeof(b)/sizeof(b[0]);
  cmReal_t a[] = {1.0000, -0.5949, 0.2348};
  unsigned an  = sizeof(a)/sizeof(a[0]);
  cmFilterFilterR(p->obj.ctx,b,bn,a,an,p->df,p->frmCnt,p->fdf,p->frmCnt);

  //cmPlotLineS( "fdf",  NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId  );  

  // median filter to low-passed filtered fdf[] into df[]
  cmVOR_FnThresh(p->fdf,p->frmCnt,16,p->df,1,NULL);

  // subtract med filtered signal from the low pa1ssed signal.
  // fdf[] -= df[];
  cmVOR_SubVV(p->fdf,p->frmCnt,p->df);

  // half-wave rectify fdf[] = set all negative values in fdf[] to zero.
  cmVOR_HalfWaveRectify(p->fdf,p->frmCnt,p->fdf);

  //cmPlotLineS( "meddf",  NULL, p->fdf, NULL, p->frmCnt, NULL, kSolidPlotLineId  );  

  // take FT of fdf[]
  cmFftExecRR(p->fft,p->fdf,p->frmCnt);

  // square FT magn.
  cmVOR_PowVS(p->fft->magV,p->fft->binCnt,2);

  //cmPlotLineS( "mag",  NULL, p->fft->magV, NULL, p->fft->binCnt, NULL, kSolidPlotLineId  );  

  // take the IFFT of the squared magnitude vector.
  cmVOR_Fill(p->fft->phsV,p->fft->binCnt,0);
  cmIFftExecRectRR(p->ifft,p->fft->magV,p->fft->phsV);

  // Matlab automatically provides this scaling as part of the IFFT.
  cmVOR_DivVS(p->ifft->outV,p->ifft->outN,p->ifft->outN);
  
  // remove bias for short periods from CMF
  for(i=0; i<p->frmCnt; ++i)
    p->ifft->outV[i] /= (p->frmCnt - i);

  //cmPlotLineS( "cm",  NULL, p->ifft->outV, NULL, p->frmCnt, NULL, kSolidPlotLineId  );  
  
  // apply comb filter to the CMF and store result in df[maxLagCnt]
  cmVOR_MultVMtV(p->df,p->maxLagCnt,p->m,p->frmCnt,p->ifft->outV);

  //acPlotLineS( "cfb",  NULL, p->df, NULL, p->maxLagCnt, NULL, kSolidPlotLineId  );  

  //acVOR_Print(p->obj.err.rpt,1,p->maxLagCnt,p->df);

  assert( p->maxLagCnt == p->hColCnt );
  cmVOR_MultVMV(p->histV,p->histBinCnt,p->H,p->hColCnt,p->df);

  cmReal_t bins[] = { 25, 17, 13, 9, 7, 6, 5, 3, 4, 3, 4, 4, 3, 4, 10};
  
  cmVOR_DivVV( p->histV, p->histBinCnt, bins );

  //cmPlotLineS( "cfb",  NULL, p->histV, NULL, p->histBinCnt, NULL, kSolidPlotLineId  );  

  if( p->mfp != NULL )
    cmMtxFileRealExec(  p->mfp, p->histV, p->histBinCnt );

  return cmOkRC;  
}

//------------------------------------------------------------------------------------------------------------
cmGmm_t* cmGmmAlloc( cmCtx* c, cmGmm_t* ap, unsigned K, unsigned D, const cmReal_t* gM, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
{
  cmGmm_t* p = cmObjAlloc( cmGmm_t, c, ap );

  if( K > 0 && D > 0 )
    if( cmGmmInit(p,K,D,gM,uM,sMM,uflags) != cmOkRC )
      cmGmmFree(&p);

  return p;
}

cmRC_t          cmGmmFree(  cmGmm_t** pp )
{
  cmRC_t   rc = cmOkRC;
  cmGmm_t* p  = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmGmmFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->gV);
  cmMemPtrFree(&p->uM);
  cmMemPtrFree(&p->sMM);
  cmMemPtrFree(&p->isMM);
  cmMemPtrFree(&p->uMM);
  cmMemPtrFree(&p->logDetV);
  cmMemPtrFree(&p->t);
  cmObjFree(pp);
  return rc;
}

cmRC_t _cmGmmUpdateCovar( cmGmm_t* p, const cmReal_t* sMM )
{
  unsigned i;

  if( sMM == NULL )
    return cmOkRC;

  unsigned De2  = p->D*p->D;
  unsigned KDe2 = p->K*De2;
  
  cmVOR_Copy(p->sMM, KDe2, sMM);
  cmVOR_Copy(p->isMM,KDe2, sMM);
  cmVOR_Copy(p->uMM, KDe2, sMM);

  //if( cmIsFlag(p->uflags,cmGmmCovarNoProcFl) )
  //  return cmOkRC;

  // for each component
  for(i=0; i<p->K; ++i)
  {
    cmReal_t*       is = p->isMM + i*De2;
    cmReal_t*       u  = p->uMM  + i*De2;
    cmReal_t*       r;

    // if the covariance matrix is diagnal
    if( cmIsFlag(p->uflags,cmGmmDiagFl))
    {
      p->logDetV[i] = cmVOR_LogDetDiagM(is,p->D); // calc the det. of diag. covar. mtx
      r             = cmVOR_InvDiagM(is,p->D);    // calc the inverse in place
    }
    else
    {
      p->logDetV[i] = cmVOR_LogDetM(is,p->D);     // calc the det. of covar mtx
      r             = cmVOR_InvM(is,p->D);        // calc the inverse in place      
    }
   
    if( fabs(p->logDetV[i]) < 1e-20 )
    {
      cmCtxPrint(p->obj.ctx,"%i\n",i);
      cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);
    }
   

    if( cmVOR_CholZ(u,p->D) == NULL )
    {
      return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (Cholesky factorization failed.)  was encountered in _cmGmmUpdateCovar().");
    }

    if( p->logDetV[i] == 0 )
    {
      cmGmmPrint(p,true);
      return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (det==0)  was encountered in _cmGmmUpdateCovar().");
    }

    if( r == NULL )
    {
      //cmCtxPrint(c,"%i\n",i);
      //cmVOR_PrintLE("DANGER SM:\n",p->obj.err.rpt,p->D,p->D,p->sMM);

      return cmCtxRtCondition(&p->obj, cmSingularMtxRC, "A singular covariance matrix (inversion failed) was encountered in _cmGmmUpdateCovar().");
    }
  
  }
  return cmOkRC;
}

cmRC_t          cmGmmInit(  cmGmm_t* p, unsigned K, unsigned D, const cmReal_t* gV, const cmReal_t* uM, const cmReal_t* sMM, unsigned uflags )
{
  cmRC_t rc;
  if((rc = cmGmmFinal(p)) != cmOkRC )
    return rc;

  //          gM[K]    uM[DK]  sMM[DDK] isMM[DDK]+ uMM[DDK] logDetV[K] t[DD]  fact[K]
  /* 
  unsigned n =  K    + (D*K) + (D*D*K) + (D*D*K) + (D*D*K)     + K    + (D*D) + K; 
  p->gV      = cmArrayResizeZ(c,&p->memA, n, cmReal_t );
  p->uM      = p->gV      +  K;
  p->sMM     = p->uM      + (D*K);
  p->isMM    = p->sMM     + (D*D*K);
  p->uMM     = p->isMM    + (D*D*K);
  p->logDetV = p->uMM     + (D*D*K);
  p->t       = p->logDetV + K;
  */

  //cmArrayResizeVZ(c, &p->memA, cmReal_t,  &p->gV,K, &p->uM,D*K, &p->sMM,D*D*K,  
  //&p->isMM,D*D*K,  &p->uMM,D*D*K,  &p->logDetV,K, &p->t,D*D, NULL );

  p->gV      = cmMemResizeZ( cmReal_t, p->gV,      K );
  p->uM      = cmMemResizeZ( cmReal_t, p->uM,      D*K);
  p->sMM     = cmMemResizeZ( cmReal_t, p->sMM,     D*D*K);
  p->isMM    = cmMemResizeZ( cmReal_t, p->isMM,    D*D*K);
  p->uMM     = cmMemResizeZ( cmReal_t, p->uMM,     D*D*K);
  p->logDetV = cmMemResizeZ( cmReal_t, p->logDetV, K); 
  p->t       = cmMemResizeZ( cmReal_t, p->t,       D*D );

  p->K       = K;
  p->D       = D;  
  p->uflags  = uflags;
  
  if( gV != NULL )
    cmVOR_Copy(p->gV,K,gV);

  if( uM != NULL )
    cmVOR_Copy(p->uM,D*K,uM);

  return _cmGmmUpdateCovar(p,sMM );

}


cmRC_t          cmGmmFinal( cmGmm_t* p )
{ return cmOkRC; }


typedef struct
{
  cmGmm_t*          p;
  const cmReal_t* xM;
  unsigned        colCnt;
} _cmGmmRdFuncData_t;

const cmReal_t* _cmGmmReadFunc( void* userPtr, unsigned colIdx )
{   
  assert(colIdx < ((const _cmGmmRdFuncData_t*)userPtr)->colCnt); 
  return ((const _cmGmmRdFuncData_t*)userPtr)->xM + (colIdx * ((const _cmGmmRdFuncData_t*)userPtr)->p->D); 
}


// xM[D,xN]
// yV[xN]
// yM[xN,K]
cmRC_t          cmGmmEval(  cmGmm_t* p,  const cmReal_t* xM, unsigned xN, cmReal_t* yV, cmReal_t* yM )
{
  _cmGmmRdFuncData_t r;
  r.colCnt = xN;
  r.p      = p;
  r.xM     = xM;

  return cmGmmEval2(p,_cmGmmReadFunc,&r,xN,yV,yM);
}


cmRC_t   cmGmmEval2(  cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, cmReal_t* yV, cmReal_t* yM)
{
  cmReal_t  tV[xN];
  unsigned k;

  //cmVOR_PrintL("cV: ",p->obj.err.rpt, 1, p->K, p->gV);
  //cmVOR_PrintL("uM:\n",p->obj.err.rpt, p->D, p->K, p->uM );
  
  //
  cmVOR_Fill(yV,xN,0);

  // for each component PDF
  for(k=0; k<p->K; k++)
  {
    const cmReal_t* meanV  = p->uM   + (k*p->D);
    const cmReal_t* isM    = p->isMM + (k*p->D*p->D);
    cmReal_t*       pV;
    
    if( yM == NULL )
      pV = tV;
    else
      pV = yM + (k*xN);


    // evaluate the kth component PDF with xM[1:T]

    //cmVOR_MultVarGaussPDF2( pV, xM,                    meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );
    cmVOR_MultVarGaussPDF3( pV, readFunc, userFuncPtr, meanV, isM, p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );


    // apply the kth component weight
    cmVOR_MultVS( pV, xN, p->gV[k] );

    // sum the result into the output vector
    cmVOR_AddVV( yV, xN, pV );

  }
  return cmOkRC;   
}



// Evaluate each component for a single data point
// xV[D] - observed data point 
// yV[K] - output contains the evaluation for each component
cmRC_t          cmGmmEval3(  cmGmm_t* p,  const cmReal_t* xV,  cmReal_t* yV )
{
  unsigned k;

  for(k=0; k<p->K; ++k)
  {
    const cmReal_t* meanV  = p->uM   + (k*p->D);
    const cmReal_t* isM    = p->isMM + (k*p->D*p->D);

    // evaluate the kth component PDF with xM[1:T]
    cmVOR_MultVarGaussPDF2( yV + k, xV, meanV, isM, p->logDetV[k], p->D, 1, cmIsFlag(p->uflags,cmGmmDiagFl) );

    // apply the kth component weight
    yV[k] *= p->gV[k];

  }

  return cmOkRC;
}

cmReal_t _cmGmmKmeansDistFunc( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
{ return  cmVOR_EuclidDistance(v0,v1,vn); }

cmRC_t cmGmmRandomize2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* funcUserPtr, unsigned xN, const cmReal_t* uM, const bool* roFlV )
{
  unsigned k;
  unsigned iV[ p->K ];

  if( uM == NULL )
    roFlV = NULL;

  // randomize the mixture coefficients
  cmVOR_Random( p->gV, p->K, 0.0, 1.0 );
  cmVOR_NormalizeProbability(p->gV,p->K);

  // fill iV with random integers between 0 and xN-1
  cmVOU_Random( iV, p->K, xN-1 );

  
  // for each component
  for(k=0; k<p->K; ++k)
  {
    cmReal_t r[ p->D ];
    
    // if this component's mean is not read-only
    if( roFlV==NULL || roFlV[k]==false )
    {
      const cmReal_t* xV = NULL;

      if( uM == NULL )
        xV = readFunc( funcUserPtr, iV[k] ); // read a random frame index
      else
        xV = uM + (k*p->D);                  // select a user supplied mean vector

      assert( xV != NULL );

      // set the random feature vector as this components mean value
      cmVOR_Copy(p->uM+(k*p->D),p->D,xV); 
    }

    cmReal_t* sM = p->sMM+(k*p->D*p->D);

    // create a random covariance mtx
    if( cmIsFlag(p->uflags,cmGmmDiagFl) )
    {
      // create a random diag. covar mtx
      cmVOR_Random(r,p->D,0.0,1.0);    
      cmVOR_Diag(sM,p->D,r);
    }
    else
    {
      // create a random symetric positive definite matrix
      cmVOR_RandSymPosDef(sM,p->D,p->t);
    }       
  }

  unsigned* classIdxV = cmMemAllocZ(unsigned, xN );

  // if some components have read-only mean's 
  if( uM != NULL && roFlV != NULL )
  {
    assert( xN >= p->K );

    for(k=0; k<p->K; ++k)
      classIdxV[k] = roFlV[k];
  }

  // use kmeans clustering to move the means closer to their center values
  if( cmIsFlag( p->uflags, cmGmmSkipKmeansFl) == false )
    cmVOR_Kmeans2( classIdxV, p->uM, p->K, readFunc, p->D, xN, funcUserPtr, _cmGmmKmeansDistFunc, NULL, -1, 0 ); 

  cmMemPtrFree(&classIdxV);

  return _cmGmmUpdateCovar(p,p->sMM);

}

cmRC_t cmGmmRandomize( cmGmm_t* p, const cmReal_t* xM, unsigned xN )
{
  _cmGmmRdFuncData_t r;
  r.colCnt = xN;
  r.p      = p;
  r.xM     = xM;

  return cmGmmRandomize2(p,_cmGmmReadFunc,&r,xN,NULL,NULL);
}


// xM[D,xN]
cmRC_t cmGmmTrain( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
{
  unsigned i,k;
  cmRC_t rc;

  if((rc = cmGmmRandomize(p,xM,xN)) != cmOkRC )
    return rc;

  //cmGmmPrint(c,p);

  // wM[xN,K]
  cmReal_t wM[ xN * p->K ]; // wM[N,K] soft assignment mtx
  unsigned wV[ xN ];        // wV[N]   hard assignment vector
  unsigned stopCnt = 0;
  unsigned curStopCnt = 0;

  if( iterCntPtr != NULL )
  {
    stopCnt     = *iterCntPtr;
    *iterCntPtr = 0;
  }
  else
  {
    // BUG BUG BUG 
    // stopCnt is used uninitialized when iterCntPtr == NULL
    assert( 0 );
  }

  while(1)
  {

    //cmVOR_NormalizeProbability(p->gV,p->K);

    cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );

    cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
    cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1,    p->K, p->gV );

    //cmGmmPrint(c,p);

    for(k=0; k<p->K; ++k)
    {
      cmReal_t* wp = wM + (k*xN);
   
      // calc the prob that each data point in xM[] was generated by the kth gaussian
      // and store as a column vector in wM[:,k]
      cmVOR_MultVarGaussPDF2( wp, xM, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );

        
      // scale the prob by the gain coeff for gaussian k
      cmVOR_MultVS( wp, xN, p->gV[k]);

    }



    //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);

    bool doneFl = true;
    for(i=0; i<xN; ++i)
    {        
      
      // form a probability for the ith data point weights
      cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
 
      // select the cluster to which the ith data point is most likely to belong
      unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);

      // if the ith data point changed clusters
      if( mi != wV[i] )
      {
        doneFl = false;
        wV[i]  = mi;
      }
    }

    curStopCnt = doneFl ? curStopCnt+1 : 0;
    

    // if no data points changed owners then the clustering is complete
    if( curStopCnt == stopCnt )
    {
      //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
      break;
    }

    // for each cluster
    for(k=0; k<p->K; ++k)
    {
      cmReal_t* uV  = p->uM  + (k*p->D);            // meanV[k]
      cmReal_t* sM  = p->sMM + (k*p->D*p->D);       // covarM[k]

      cmReal_t  sw  = cmVOR_Sum(wM + (k*xN), xN ); 


      // update the kth weight
      p->gV[k] = sw / xN;


      // form a sum of all data points weighted by their soft assignment to cluster k      
      cmReal_t sumV[p->D];      
      cmVOR_MultVMV( sumV, p->D, xM, xN, wM + k*xN );

      // update the mean[k]
      cmVOR_DivVVS(uV, p->D, sumV, sw );
         

      // update the covar[k]
      // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)')); 
      cmVOR_Fill(sM,p->D*p->D,0);
      for(i=0; i<xN; ++i)
      {
        cmReal_t duV[ p->D ];
        
        cmVOR_SubVVV(  duV,  p->D, xM + (i*p->D), uV );      // duV[]  = xM[:,i] - uV[];
        cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 );      // t[,]   = duV[] * duV[]' 
        cmVOR_MultVS(  p->t, p->D*p->D, wM[ k * xN + i ]);   // t[,]  *= wM[i,k]
        cmVOR_AddVV(   sM, p->D*p->D, p->t );                // sM[,] += t[,];
      }          

      cmVOR_DivVS( sM, p->D*p->D, sw );  // sM[,] ./ sw;
    }  

    // update the inverted covar mtx and covar det.
    if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
      return rc;

    if( iterCntPtr != NULL )
      *iterCntPtr += 1;
  }
  return cmOkRC;
}

// xM[D,xN]
cmRC_t cmGmmTrain2( cmGmm_t* p, cmGmmReadFunc_t readFunc, void* userFuncPtr, unsigned xN, unsigned* iterCntPtr, const cmReal_t* uM, const bool* roFlV, int maxIterCnt )
{
  unsigned i,j,k;
  cmRC_t rc;

  // if uM[] is not set then ignore roFlV[]
  if( uM == NULL )
    roFlV=NULL;

  if((rc = cmGmmRandomize2(p,readFunc,userFuncPtr,xN,uM,roFlV)) != cmOkRC )
    return rc;

  //cmGmmPrint(c,p);

  // wM[xN,K] soft assignment mtx
  cmReal_t* wM = cmMemAlloc(cmReal_t, xN * p->K ); 

  // wV[N]   hard assignment vector
  unsigned* wV = cmMemAlloc(unsigned, xN );
        
  unsigned stopCnt    = 0;
  unsigned curStopCnt = 0;

  if( iterCntPtr != NULL )
  {
    stopCnt     = *iterCntPtr;
    *iterCntPtr = 0;
  }
  else
  {
    // BUG BUG BUG 
    // stopCnt is used uninitialized when iterCntPtr == NULL
    assert( 0 );
  }

  while(1)
  {

    //cmCtxPrint(p->obj.ctx,"iter:%i --------------------------------------------\n",*iterCntPtr );
    //cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->K, p->uM );
    cmVOR_PrintL("gV:\n", p->obj.err.rpt, 1,    p->K, p->gV );
    //cmGmmPrint(c,p);

    for(k=0; k<p->K; ++k)
    {
      cmReal_t* wp = wM + (k*xN);
   
      // calc the prob that each data point in xM[] was generated by the kth gaussian
      // and store as a column vector in wM[:,k]
      cmVOR_MultVarGaussPDF3( wp, readFunc, userFuncPtr, p->uM + (k*p->D), p->isMM + (k*p->D*p->D), p->logDetV[k], p->D, xN, cmIsFlag(p->uflags,cmGmmDiagFl) );

        
      // scale the prob by the gain coeff for gaussian k
      cmVOR_MultVS( wp, xN, p->gV[k]);

    }

    //cmVOR_PrintL("wM:\n",p->obj.err.rpt,xN,p->K,wM);

    bool doneFl = true;
    unsigned changeCnt = 0;
    for(i=0; i<xN; ++i)
    {        
      
      // form a probability for the ith data point weights
      cmVOR_NormalizeProbabilityN( wM + i, p->K, xN);
 
      // select the cluster to which the ith data point is most likely to belong
      unsigned mi = cmVOR_MaxIndex(wM + i, p->K, xN);

      // if the ith data point changed clusters
      if( mi != wV[i] )
      {
        ++changeCnt;
        doneFl = false;
        wV[i]  = mi;
      }
    }

    curStopCnt = doneFl ? curStopCnt+1 : 0;
    
    printf("%i stopCnt:%i changeCnt:%i\n",*iterCntPtr,curStopCnt,changeCnt);

    // if no data points changed owners then the clustering is complete
    if( curStopCnt == stopCnt )
    {
      //cmVOU_PrintL("wV: ",p->obj.err.rpt,xN,1,wV);
      break;
    }

    // if a maxIterCnt was given and the cur iter cnt exceeds the max iter cnt then stop
    if( maxIterCnt>=1 && *iterCntPtr >= maxIterCnt )
      break;

    // form a sum of all data points weighted by their soft assignment to cluster k      
    // NOTE: cmGmmTrain() performs this step more efficiently because it use
    //       an LAPACK matrix multiply.  
    cmReal_t sumM[ p->D * p->K ];

    cmVOR_Zero(sumM,p->D*p->K);

    for(i=0; i<xN; ++i)
    {
      const cmReal_t* xV = readFunc( userFuncPtr, i );
      
      assert( xV != NULL );

      for(k=0; k<p->K; ++k)
      {
        cmReal_t weight = wM[ i + (k*xN)];

        for(j=0; j<p->D; ++j)
          sumM[ j + (k*p->D) ] += xV[j] * weight;
      }
    }



    // for each cluster that is not marked as read-only
    for(k=0; k<p->K; ++k)
    {
      cmReal_t* uV  = p->uM  + (k*p->D);            // meanV[k]
      cmReal_t* sM  = p->sMM + (k*p->D*p->D);       // covarM[k]

      cmReal_t  sw  = cmVOR_Sum(wM + (k*xN), xN ); 


      // update the kth weight
      p->gV[k] = sw / xN;


      // if this component's mean is not read-only
      if( (roFlV==NULL || roFlV[k]==false) && sw != 0)
      {
        // get vector of all data points weighted by their soft assignment to cluster k      
        cmReal_t* sumV = sumM + (k*p->D);  // sumV[p->D];      

        // update the mean[k]
        cmVOR_DivVVS(uV, p->D, sumV, sw );
      }  

      // update the covar[k]
      // sM += ( W(i,k) .* ((X(:,i) - uV) * (X(:,i) - uV)')); 
      cmVOR_Fill(sM,p->D*p->D,0);
      for(i=0; i<xN; ++i)
      {
        cmReal_t duV[ p->D ];

        const cmReal_t* xV = readFunc( userFuncPtr, i );
        
        assert( xV != NULL );

        
        cmVOR_SubVVV(  duV,  p->D, xV, uV );                 // duV[]  = xM[:,i] - uV[];
        cmVOR_MultMMM( p->t, p->D, p->D, duV, duV, 1 );      // t[,]   = duV[] * duV[]' 
        cmVOR_MultVS(  p->t, p->D*p->D, wM[ k * xN + i ]);   // t[,]  *= wM[i,k]
        cmVOR_AddVV(   sM, p->D*p->D, p->t );                // sM[,] += t[,];
      }          

      if( sw != 0 )        
        cmVOR_DivVS( sM, p->D*p->D, sw );  // sM[,] ./ sw;
    }  

    // update the inverted covar mtx and covar det.
    if((rc = _cmGmmUpdateCovar(p,p->sMM )) != cmOkRC )
      goto errLabel;

    if( iterCntPtr != NULL )
      *iterCntPtr += 1;
  }

  cmMemPtrFree(&wM);
  cmMemPtrFree(&wV);

 errLabel:

  return cmOkRC;
}

cmRC_t cmGmmTrain3( cmGmm_t* p, const cmReal_t* xM, unsigned xN, unsigned* iterCntPtr )
{
  _cmGmmRdFuncData_t r;
  r.colCnt = xN;
  r.p      = p;
  r.xM     = xM;
  
  return cmGmmTrain2(p,_cmGmmReadFunc,&r,xN,iterCntPtr,NULL,NULL,-1);

}


cmRC_t cmGmmGenerate( cmGmm_t* p, cmReal_t* yM, unsigned yN )
{
  unsigned i=0;

  unsigned kV[yN];  

  // use weighted random selection to choose the component for each output value
  cmVOR_WeightedRandInt(kV,yN, p->gV, p->K );

  // cmVOU_Print(p->obj.err.rpt,1,yN,kV);

  for(i=0; i<yN; ++i)
  {
    const cmReal_t* uV  = p->uM  + (kV[i] * p->D);
    unsigned        idx = kV[i] * p->D * p->D;

    //cmVOR_PrintL("sM\n",p->obj.err.rpt,p->D,p->D,sM);

    if( cmIsFlag(p->uflags,cmGmmDiagFl) )
    {
      const cmReal_t* sM = p->sMM + idx;
      cmVOR_RandomGaussDiagM( yM + (i*p->D), p->D, 1, uV, sM ); 
    }
    else
    {
      const cmReal_t* uM = p->uMM + idx;
      cmVOR_RandomGaussNonDiagM2(yM + (i*p->D), p->D, 1, uV, uM ); 
    }

  }
  return cmOkRC;
}

void     cmGmmPrint( cmGmm_t* p, bool fl )
{
  unsigned k;
  //cmCtx* c = p->obj.ctx;

  cmVOR_PrintL("gV: ",  p->obj.err.rpt, 1,    p->K, p->gV );
  cmVOR_PrintL("mM:\n", p->obj.err.rpt, p->D, p->K, p->uM );

  for(k=0; k<p->K; ++k)
    cmVOR_PrintL("sM:\n", p->obj.err.rpt, p->D, p->D, p->sMM + (k*p->D*p->D));

  if( fl )
  {
    for(k=0; k<p->K; ++k)
      cmVOR_PrintL("isM:\n", p->obj.err.rpt, p->D, p->D, p->isMM + (k*p->D*p->D));

    for(k=0; k<p->K; ++k)
      cmVOR_PrintL("uM:\n", p->obj.err.rpt, p->D, p->D, p->uMM + (k*p->D*p->D));

    cmVOR_PrintL("logDetV:\n", p->obj.err.rpt, 1, p->K, p->logDetV);

  }

}

void cmGmmTest(  cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
  cmCtx* c = cmCtxAlloc(NULL,rpt,lhH,stH);

  unsigned K        = 2;
  unsigned D        = 2;
  cmReal_t gV[ 2 ]  = { .5, .5 };
  cmReal_t uM[ 4 ]  = { .3, .3, .8, .8 };
  cmReal_t sMM[ 8 ] = { .1,  0,  0, .1, .1,  0,  0, .1 };
  unsigned flags    = cmGmmDiagFl ;

  unsigned M = 100;
  cmReal_t xM[ D*M ];
  cmReal_t yV[ M ];
  unsigned i,j;

  cmPlotSetup("MultDimGauss Test",1,1);

  cmGmm_t* p = cmGmmAlloc(c, NULL, K, D, gV, uM, sMM, flags );

  if(0)
  {
    cmGmmPrint(p,true);

    for(i=0; i<10; i++)
      for(j=0; j<20; j+=2)
      {
        xM[(i*20)+j]     = .1 * i;;
        xM[(i*20)+j + 1] = .1 * (j/2);;
      } 

    // octave equivalent
    // x0= .1 * ones(1,10);
    // x = [ 0*x0 1*x0 2*x0 3*x0 4*x0 5*x0 6*x0 7*x0 8*x0 9*x0];
    // x = [x; repmat([0:.1:1],1,10)];
    // y = mvnpdf(x',[.3 .3],[.1 0; 0 .1]); plot(y);

    cmGmmEval(p,xM,M,yV,NULL);

  
    //cmVOR_PrintL( "xM\n", rpt, D,   M, xM );
    cmVOR_PrintL( "yV\n",   rpt, 10, 10, yV );

    //printf("y:%f\n",yV[0]);


    cmPlotLineD( NULL, NULL, yV, NULL, M, NULL, kSolidPlotLineId );


  }

  if(0)
  {
    cmReal_t yM[  D*M ];
    cmReal_t yMt[ M*D ];
    cmReal_t uMt[ p->K*D];
    unsigned iterCnt = 10;

    //srand( time(NULL) );

    cmGmmGenerate( p, yM, M );

    p->uflags = 0; // turn off diagonal condition

    if( cmGmmTrain3( p, yM, M, &iterCnt ) != cmOkRC )
      return;

    cmCtxPrint(c,"iterCnt:%i\n",iterCnt);

    cmGmmPrint( p, true );

    cmVOR_Transpose(yMt, yM, D, M );

    //cmVOR_PrintL("yMt\n",vReportFunc,M,D,yMt);

    cmPlotLineD(NULL, yMt, yMt+M, NULL, M, "blue", kAsteriskPlotPtId );
    
    cmVOR_Transpose( uMt, p->uM, D, p->K);

    cmVOR_PrintL("uMt:\n", p->obj.err.rpt, p->K, p->D, uMt );

    cmPlotLineD(NULL, uMt, uMt+p->K, NULL, p->D, "red", kXPlotPtId );

  }

  if(1)
  {
    cmGmmFree(&p);

    
    cmReal_t cV0[]  = { .7, .3 };
    cmReal_t uM0[]  = { .2, .1, .1, .2 };
    cmReal_t sMM0[] = { .01, 0, 0, .01,   .01, 0, 0, .01 };
    unsigned flags  = 0;

    K = 2;
    D = 2;

    cmGmm_t* p = cmGmmAlloc(c,NULL, K, D, cV0, uM0, sMM0, flags );
    
    xM[0] = 0.117228;
    xM[1] = 0.110079; 

    cmGmmEval(p,xM,1,yV,NULL);

    cmCtxPrint(c,"y: %f\n",yV[0]);
    
  }

  cmPlotDraw();
  cmGmmFree(&p);
  cmCtxFree(&c);

}

//------------------------------------------------------------------------------------------------------------
cmChmm_t* cmChmmAlloc( cmCtx* c, cmChmm_t* ap, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
{
  cmChmm_t* p = cmObjAlloc(cmChmm_t,c,ap);

  if( stateN >0 && dimN > 0 )
    if( cmChmmInit(p,stateN,mixN,dimN,iV,aM) != cmOkRC )
      cmChmmFree(&p);

  return p;  
}

cmRC_t    cmChmmFree(  cmChmm_t** pp )
{
  cmRC_t    rc = cmOkRC;
  cmChmm_t* p  = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmChmmFinal(p)) != cmOkRC )
    return rc;

  cmMemPtrFree(&p->iV);
  cmMemPtrFree(&p->aM);
  cmMemPtrFree(&p->bV);
  cmMemPtrFree(&p->bM);
  cmObjFree(pp);
  return cmOkRC;

}

cmRC_t    cmChmmInit(  cmChmm_t* p, unsigned stateN, unsigned mixN, unsigned dimN, const cmReal_t* iV, const cmReal_t* aM )
{
  cmRC_t rc;
  unsigned i;

  if((rc = cmChmmFinal(p)) != cmOkRC )
    return rc;

  //             iV[]         aM             
  /*
  unsigned n = stateN + (stateN*stateN);
  p->iV      = cmArrayResizeZ(c, &p->memA, n, cmReal_t );
  p->aM      = p->iV + stateN;
  */

  //cmArrayResizeVZ(c,&p->memA, cmReal_t, &p->iV,stateN, &p->aM, stateN*stateN, NULL );

  p->iV = cmMemResizeZ( cmReal_t, p->iV, stateN );
  p->aM = cmMemResizeZ( cmReal_t, p->aM, stateN * stateN );
  p->bV = cmMemResizeZ( cmGmm_t*, p->bV, stateN );
  p->N  = stateN;
  p->K  = mixN;
  p->D  = dimN;

  if( iV != NULL )
    cmVOR_Copy(p->iV,p->N,iV);

  if( aM != NULL )
    cmVOR_Copy(p->aM,p->N*p->N,aM);

  for(i=0; i<p->N; ++i)
    p->bV[i] = cmGmmAlloc( p->obj.ctx, NULL, p->K, p->D, NULL, NULL, NULL, 0 ); 

  //p->mfp = cmCtxAllocDebugFile( p->obj.ctx,"chmm");

  return cmOkRC;
}

cmRC_t    cmChmmFinal( cmChmm_t* p )
{ 
  if( p != NULL )
  {
    unsigned i;

    for(i=0; i<p->N; ++i)
      cmGmmFree( &p->bV[i] );

    cmMemPtrFree(&p->bM);

    //if( p->mfp != NULL )
    //  cmCtxFreeDebugFile(p->obj.ctx,&p->mfp);

  }
  return cmOkRC; 
}

cmRC_t      cmChmmRandomize( cmChmm_t* p, const cmReal_t* oM, unsigned T )
{
  cmRC_t rc;
  unsigned i;
  unsigned N = p->N;

  // randomize the initial state probabilities
  cmVOR_Random( p->iV, N, 0.0, 1.0 ); 
  cmVOR_NormalizeProbability( p->iV, N ); 

  // randomize the state transition matrix
  cmVOR_Random( p->aM, N*N, 0.0, 1.0 );

  for(i=0; i<N; ++i)
  {
    cmVOR_NormalizeProbabilityN( p->aM + i, N, N );  // rows of aM must sum to 1.0

    if((rc = cmGmmRandomize( p->bV[i], oM, T )) != cmOkRC)  // randomize the GMM assoc'd with state i
      return rc;
  }

  cmMemPtrFree(&p->bM); // force bM[] to be recalculated

  return cmOkRC;
}

cmReal_t  _cmChmmKmeansDist( void* userPtr, const cmReal_t* v0, const cmReal_t* v1, unsigned vn )
{ return cmVOR_EuclidDistance(v0,v1,vn); }

cmRC_t    cmChmmSegKMeans( cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t threshProb, unsigned maxIterCnt, unsigned iterCnt )
{
  cmCtx* c  = p->obj.ctx;
  cmRC_t rc = cmOkRC;
  unsigned i,j,k,t;

  unsigned N = p->N;
  unsigned K = p->K;
  unsigned D = p->D;

  //unsigned qV[T];
  //cmReal_t alphaM[N*T];
  //unsigned clusterIdxV[T];
  //cmReal_t centroidM[D*N];

  /*
  unsigned sz = 2*ALIGN_B(T,unsigned) +
                ALIGN_B(N*T,cmReal_t) +
                ALIGN_B(D*N,cmReal_t);
  cmArray mem;
  cmArrayAlloc(c, &mem);

  unsigned* qV          = (unsigned*) cmArrayResize(c, &mem, sz, char);
  cmReal_t* alphaM      = (cmReal_t*) (qV          + ALIGN_T(T,  unsigned));
  unsigned* clusterIdxV = (unsigned*) (alphaM      + ALIGN_T(N*T,cmReal_t));
  cmReal_t* centroidM   = (cmReal_t*) (clusterIdxV + ALIGN_T(T,  unsigned));
  */

  unsigned* qV          = cmMemAlloc( unsigned, T );
  cmReal_t* alphaM      = cmMemAlloc( cmReal_t, N*T);
  unsigned* clusterIdxV = cmMemAlloc( unsigned, T );
  cmReal_t* centroidM   = cmMemAlloc( cmReal_t, D*N);


  cmReal_t logPr = 0;
  bool reportFl = true;

  cmChmmRandomize(p,oM,T);
  
  // cluster the observations into N groups
  cmVOR_Kmeans( qV, centroidM, N, oM, D, T, NULL, 0, false, _cmChmmKmeansDist, NULL ); 

  for(i=0; i<maxIterCnt; ++i)
  {
    unsigned jnV[N];

    if( reportFl )
      cmCtxPrint(c,"SegKM: ----------------------------------------------------%i\n",i);

    // get the count of data points in each state
    cmVOU_Fill(jnV,N,0);
    for(t=0; t<T; ++t)
      ++jnV[ qV[t] ]; 

    // for each state
    for(j=0; j<N; ++j)
    {
      cmGmm_t* g = p->bV[j];
      
      // cluster all datapoints which were assigned to state j
      cmVOR_Kmeans( clusterIdxV, g->uM, K, oM, D, T, qV, j, false, _cmChmmKmeansDist, NULL ); 

      // for each cluster
      for(k=0; k<K; ++k)
      {
        unsigned kN = 0;

        // kN is count of data points assigned to cluster k
        for(t=0; t<T; ++t)
          if( clusterIdxV[t] == k )
            ++kN;

        g->gV[k] = (cmReal_t)kN/jnV[j];

        
        // the covar of the kth component is the sample covar of cluster k
        cmVOR_GaussCovariance(g->sMM + (k*D*D), D, oM, T, g->uM + (k*D), clusterIdxV, k );

      }      

      if((rc = _cmGmmUpdateCovar(g, g->sMM )) != cmOkRC )
        goto errLabel;
    }

    if( i== 0 )
    {
      // count transitions from i to j
      for(t=0; t<T-1; ++t)
        p->aM[  (qV[t+1]*N) + qV[t] ] += 1.0;

      for(j=0; j<N; ++j)
      {
        // normalize state transitions by dividing by times in each state
        for(k=0; k<N; k++)
          p->aM[ (k*N) + j ] /= jnV[j];

        cmVOR_NormalizeProbabilityN(p->aM + j, N, N);

        cmGmmEval( p->bV[j], oM, 1, p->iV + j, NULL );
      }
    }


    if((rc = cmChmmTrain(p, oM, T, iterCnt,0,0 )) != cmOkRC )
      goto errLabel;

    // calculate the prob. that the new model generated the data
    cmReal_t logPr0 = cmChmmForward(p,oM,T,alphaM,NULL);
    cmReal_t dLogPr = logPr0 - logPr;
  
    if( reportFl )
      cmCtxPrint(c,"pr:%f d:%f\n",logPr0,dLogPr);

    if( (dLogPr > 0) && (dLogPr < threshProb) )
      break;

    logPr = logPr0;



    // fill qV[] with the state at each time t
    cmChmmDecode(p,oM,T,qV);

  }

 errLabel:

  cmMemPtrFree(&qV);
  cmMemPtrFree(&alphaM);
  cmMemPtrFree(&clusterIdxV);
  cmMemPtrFree(&centroidM);

  return rc;

}


cmRC_t    cmChmmSetGmm(   cmChmm_t* p, unsigned i, const cmReal_t* wV, const cmReal_t* uM, const cmReal_t* sMM, unsigned flags )
{
  assert( i < p->N);
  cmMemPtrFree(&p->bM); // force bM[] to be recalculated
  return cmGmmInit(p->bV[i],p->K,p->D,wV,uM,sMM,flags);
}


// Return the probability of the observation for each state.
// oV[D] - multi-dim. observation data point
// pV[N] - probability of this observation for each state
void cmChmmObsProb( const cmChmm_t* p, const cmReal_t* oV,  cmReal_t* prV )
{
  unsigned i;
  for(i=0; i<p->N; ++i)
    cmGmmEval( p->bV[i], oV, 1, prV + i, NULL ); 
}

// oM[D,T]     - observation matrix
// alphaM[N,T] - prob of being in each state and observtin oM(:,t)
// bM[N,T]     - (optional) state-observation probability matrix
// logPrV[T]   - (optional) record the log prob of the data given the model at each time step 
// Returns sum(logPrV[T])
cmReal_t cmChmmForward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* alphaM, cmReal_t* logPrV )
{
  unsigned t;

  cmReal_t logPr = 0;

  // calc the prob of starting in each state
  if( p->bM == NULL )
    cmChmmObsProb( p, oM, alphaM );
  else
    cmVOR_Copy( alphaM, p->N*T, p->bM );
  

  cmVOR_MultVV( alphaM, p->N, p->iV );

  cmReal_t s = cmVOR_Sum(alphaM,p->N);
  
  cmVOR_DivVS(alphaM,p->N,s);
  //cmVOR_PrintL("alpha:\n",p->obj.err.rpt,p->N,1,alphaM);

  for(t=1; t<T; ++t)
  {
    cmReal_t  tmp[p->N];

    cmReal_t* alphaV = alphaM + t*p->N;

    // calc the prob of the observation for each state
    if( p->bM == NULL )
      cmChmmObsProb(p,oM + (t*p->D), alphaV );

    // calc. the prob. of transitioning to each state at time t, from each state at t-1
    cmVOR_MultVVM(tmp,p->N, alphaM + ((t-1)*p->N), p->N, p->aM ); 

    // calc the joint prob of transitioning from each state to each state and observing O(t).
    cmVOR_MultVV(alphaV, p->N, tmp );
    
    // scale the probabilities to prevent underflow
    s = cmVOR_Sum(alphaV,p->N);
    cmVOR_DivVS(alphaV,p->N,s);

    // track the log prob. of the model having generated the data up to time t.
    cmReal_t pr = log(s);

    if( logPrV != NULL )
      logPrV[t] = pr;

    logPr += pr;
  }

  return logPr;
  
}

// oM[D,T]
// betaM[N,T]
void cmChmmBackward( const cmChmm_t* p, const cmReal_t* oM, unsigned T, cmReal_t* betaM )
{
  cmVOR_Fill(betaM,p->N*T,1.0);

  assert(T >= 2 );

  int t = (int)T - 2;
  for(; t>=0; --t)
  {
    cmReal_t tmp[p->N];

    if( p->bM == NULL )
      cmChmmObsProb(p,oM+((t+1)*p->D), tmp );
    else
      cmVOR_Copy(tmp,p->N,p->bM + ((t+1)*p->N));

    cmVOR_MultVV(tmp,p->N,betaM + ((t+1)*p->N));

    cmVOR_MultVMV(betaM+(t*p->N),p->N, p->aM, p->N, tmp );

    cmVOR_NormalizeProbability(betaM+(t*p->N),p->N );

  }
    
}

cmReal_t  cmChmmCompare( const cmChmm_t* p0, const cmChmm_t* p1, unsigned T )
{
  assert(p0->D == p1->D);
  assert(p0->N == p1->N);

  cmReal_t oM[p0->D*T];
  cmReal_t alphaM[p0->N*T];
  
  cmChmmGenerate(p0,oM,T,NULL);
  cmReal_t logPr00 = cmChmmForward(p0,oM,T,alphaM,NULL);
  cmReal_t logPr01 = cmChmmForward(p1,oM,T,alphaM,NULL);

  cmChmmGenerate(p1,oM,T,NULL);
  cmReal_t logPr10 = cmChmmForward(p0,oM,T,alphaM,NULL);
  cmReal_t logPr11 = cmChmmForward(p1,oM,T,alphaM,NULL);

  cmReal_t d0 = (logPr01-logPr00)/T;
  cmReal_t d1 = (logPr10-logPr11)/T;

  return (d0+d1)/2;

}

cmRC_t    cmChmmGenerate( const cmChmm_t* p, cmReal_t* oM,  unsigned T, unsigned* sV )
{
  unsigned i,si;

  // use weighted random selection to choose an intitial state
  cmVOR_WeightedRandInt(&si, 1, p->iV, p->N );

  for(i=0; i<T; ++i)
  {
    if( sV != NULL )
      sV[i] = si;

    // generate a random value using the GMM assoc'd with the current state
    cmGmmGenerate( p->bV[si], oM + (i*p->D), 1 );

    // choose the next state using the transition weights from the current state
    cmVOR_WeightedRandInt(&si, 1, p->aM + (si*p->N), p->N );
  }  
 
  return cmOkRC;
}


cmRC_t    cmChmmDecode(   cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned* yV )
{
  int i,j,t;
  unsigned N = p->N;

  //unsigned psiM[N*T];
  //cmReal_t delta[N];
  /*
  unsigned sz = ALIGN_B(N*T,unsigned) + ALIGN_B(N,cmReal_t);

  cmArray  mem;
  cmArrayAlloc(c, &mem);

  unsigned* psiM  = (unsigned*) cmArrayResize(c, &mem, sz, char);
  cmReal_t* delta = (cmReal_t*) (psiM + ALIGN_T(N*T,unsigned));
  */

  unsigned* psiM = cmMemAlloc( unsigned, N*T );
  cmReal_t* delta= cmMemAlloc( cmReal_t, N );

  // get the prob of starting in each state
  if( p->bM == NULL )
    cmChmmObsProb( p, oM, delta );
  else
    cmVOR_Copy( delta, N, p->bM );

  cmVOR_MultVV( delta, p->N, p->iV );
  cmVOR_NormalizeProbability(delta, p->N);
  
  for(t=1; t<T; ++t)
  {
    cmReal_t         mV[N];

    const cmReal_t*  ap = p->aM;

    // calc. the most likely new state given the most likely prev state
    // and the transition matrix
    for(i=0; i<N; ++i)
    {
      const cmReal_t*  dp     = delta;
      unsigned         psiIdx = t*N + i;

      mV[i]          = *ap++ * *dp++;
      psiM[ psiIdx ] = 0;

      // find max value of: delta .* A(:,i)
      for(j=1; j<N; ++j )
      {        
        cmReal_t v = *ap++ * *dp++;

        if( v > mV[i] )
        {
          mV[i]          = v;
          psiM[ psiIdx ] = j;
        }
      }
    }

    // mV[] now holds the prob. of the max likelihood state at time t 
    // for each possible state at t-1

    // psiM[:,t] holds the index of the max likelihood state 

    // condition the most likely new state on the observations
    if( p->bM == NULL )
      cmChmmObsProb(p,oM + (t*p->D), delta);  
    else
      cmVOR_Copy(delta, N, p->bM + (t*p->N) );

    cmVOR_MultVV(delta, N, mV );              // condition it on the max. like current states
    cmVOR_NormalizeProbability( delta, N );   // normalize the prob.
  }

  // unwind psiM[] to form the max. likelihood state  sequence
  yV[T-1] = cmVOR_MaxIndex(delta,N,1);

  for(t=T-2; t>=0; --t)
    yV[t] = psiM[ ((t+1)*N) + yV[t+1] ];


  cmMemPtrFree(&psiM);
  cmMemPtrFree(&delta);

  return cmOkRC;
}


cmRC_t    cmChmmTrain( cmChmm_t* p, const cmReal_t* oM, unsigned T, unsigned iterCnt, cmReal_t thresh, unsigned flags )
{
  cmRC_t   rc = cmOkRC;
  unsigned i,j,k,t,d;
  unsigned iter;
  unsigned N          = p->N;
  unsigned K          = p->K;
  unsigned D          = p->D;
  unsigned De2        = D * D;
  bool     mixFl      = !cmIsFlag(flags,kNoTrainMixCoeffChmmFl);
  bool     meanFl     = !cmIsFlag(flags,kNoTrainMeanChmmFl);
  bool     covarFl    = !cmIsFlag(flags,kNoTrainCovarChmmFl);
  bool     bFl        = mixFl | meanFl | covarFl;
  bool     calcBFl    = true;
  bool     progFl     = false;
  bool     timeProgFl = false;
  cmReal_t progInc    = 0.1;
  cmReal_t progFrac   = 0;
  cmReal_t logProb    = 0;

  //cmReal_t alphaM[N*T]; // alpha[N,T]
  //cmReal_t betaM[N*T];  // betaM[N,T]
  //cmReal_t logPrV[T];
  //cmReal_t EpsM[N*N];
  //cmReal_t BK[N*K*T]; 
  //cmReal_t gamma_jk[N*K];
  //cmReal_t uM[K*D*N];
  //cmReal_t sMM[K*De2*N];
  /*
  unsigned sz = ALIGN_T(N*T,    cmReal_t) +
                ALIGN_T(N*T,    cmReal_t) +
                ALIGN_T(T,      cmReal_t) +
                ALIGN_T(N*N,    cmReal_t) +
                ALIGN_T(N*K*T,  cmReal_t) +
                ALIGN_T(N*K,    cmReal_t) +
                ALIGN_T(K*D*N,  cmReal_t) +
                ALIGN_T(K*De2*N,cmReal_t);
  cmArray mem;
  cmArrayAlloc(c, &mem);

  cmReal_t* alphaM   = cmArrayResize(c, &mem, sz, cmReal_t); // alpha[N,T]
  cmReal_t* betaM    = alphaM   + ALIGN_T(N*T,  cmReal_t);   // betaM[N,T]
  cmReal_t* logPrV   = betaM    + ALIGN_T(N*T,  cmReal_t);
  cmReal_t* EpsM     = logPrV   + ALIGN_T(T,    cmReal_t);
  cmReal_t* BK       = EpsM     + ALIGN_T(N*N,  cmReal_t);
  cmReal_t* gamma_jk = BK       + ALIGN_T(N*K*T,cmReal_t);
  cmReal_t* uM       = gamma_jk + ALIGN_T(N*K,  cmReal_t);
  cmReal_t* sMM      = uM       + ALIGN_T(K*D*N,cmReal_t);
  */

  cmReal_t* alphaM   = cmMemAlloc( cmReal_t, N*T ); 
  cmReal_t* betaM    = cmMemAlloc( cmReal_t, N*T ); 
  cmReal_t* logPrV   = cmMemAlloc( cmReal_t, T ); 
  cmReal_t* EpsM     = cmMemAlloc( cmReal_t, N*N ); 
  cmReal_t* BK       = cmMemAlloc( cmReal_t, N*K*T ); 
  cmReal_t* gamma_jk = cmMemAlloc( cmReal_t, N*K ); 
  cmReal_t* uM       = cmMemAlloc( cmReal_t, K*D*N ); 
  cmReal_t* sMM      = cmMemAlloc( cmReal_t, K*De2*N ); 

  if( thresh <=0 )
    thresh = 0.0001;

  //cmArrayResizeZ(c,&p->memC,N*T,cmReal_t);
  p->bM = cmMemResizeZ( cmReal_t, p->bM, N*T); 

  for(iter=0; iter<iterCnt; ++iter)
  {
    // zero the mean and covar summation arrays
    cmVOR_Fill(uM,  K*D  *N,0);
    cmVOR_Fill(sMM, K*De2*N,0);
    cmVOR_Fill(EpsM,N*N,    0);
    cmVOR_Fill(gamma_jk,N*K,0);

    // 
    // B[i,t]     The prob that state i generated oM(:,t)                                           
    // BK[i,k,t]  The prob that state i component k generated oM(:,t)
    // Note: B[i,t] = sum(BK(i,k,:))
    //
    if( calcBFl || bFl )
    {
      calcBFl = false;
      for(t=0; t<T; ++t)
      {
        // prob. that state i generated objservation O[t]
        for(i=0; i<N; ++i )
          cmGmmEval(  p->bV[i], oM + (t*D), 1, p->bM + (t*N) + i, BK + (t*N*K) + (i*K) );
      }
    }

    // alpha[N,T] is prob. of transitioning forward to each state given the observed data
    cmReal_t logProb0 =  cmChmmForward( p, oM, T, alphaM, logPrV );

    // check for convergence
    cmReal_t dLogProb =  labs(logProb0-logProb) / ((labs(logProb0)+labs(logProb)+cmReal_EPSILON)/2);
    if( dLogProb < thresh )
      break;

    logProb = logProb0;

    // betaM[N,T] is prob of transitioning backward from each state given the observed data
    cmChmmBackward(p, oM, T, betaM );


    if(progFl)
      cmCtxPrint(p->obj.ctx,"%i (%f) ",iter+1, dLogProb );

    if(timeProgFl)
      progFrac = progInc;


    // for each time step
    for(t=0; t<T-1; ++t)
    {
      // oV[D] is the observation at step t
      const cmReal_t* oV = oM + (t*D); 


      //
      // Update EpsM[N,N] (6.37)
      // (prob. of being in state i at time t and transitioning
      //  to state j at time t+1)
      //
      cmReal_t E[N*N];

      // for each possible state transition
      for(i=0; i<N; ++i)
        for(j=0; j<N; ++j)
        {

          E[ i + (j*N) ] 
            =   exp(log(alphaM[ (t*N) + i ])
              + log(p->aM[  i + (j*N) ])
              + log(p->bM[ ((t+1)*N) + j ])
              + log(betaM[ ((t+1)*N) + j ]));           
        }


      cmVOR_NormalizeProbability( E, N*N );
      cmVOR_AddVV( EpsM, N*N, E );


      // If t==0 then update the initial state prob's
      if( t == 0 )
      {
        for(i=0; i<N; ++i)
          p->iV[i] = cmVOR_SumN(EpsM+i, N, N);

        assert( cmVOR_IsNormal(p->iV,N) );    
      }

      if( bFl )
      {
        // 
        // Calculate gamma_jk[]
        // 
        cmReal_t gtjk[N*K];  // gamma_jk[N,K] at time t
        cmReal_t abV[N];     //

      
        // (alphaM[j,t] * betaM[j:t]) / (sum(alphaM[:,t] * betaM[:,t]))
        cmVOR_MultVVV(abV,N,alphaM + t*N, betaM+t*N);
        cmReal_t abSum = cmVOR_Sum(abV,N);

        if( abSum<=0 )
          assert(abSum>0);

        cmVOR_DivVS(abV,N,abSum);

     
        for(j=0; j<N; ++j)
        {
          cmReal_t bkSum = cmVOR_Sum(BK + (t*N*K) + (j*K), K );
        
          for(k=0; k<K; ++k)
            gtjk[ (k*N)+j ]  = abV[j] * (BK[ (t*N*K) + (j*K) + k ] / bkSum);       
        }


        // sum gtjk[N,K] into gamma_jk (integrate gamma over time)
        cmVOR_AddVV( gamma_jk, N*K, gtjk );


        // update the mean and covar numerators
        for(j=0; j<N; ++j)
        {
          cmReal_t* uV = uM  + (j*D*K);
          cmReal_t* sV = sMM + (j*De2*K);

          for(k=0; k<K; ++k,uV+=D,sV+=De2)
          {
            cmReal_t c = gtjk[ (k*N)+j ];

            if( covarFl )
            {
              cmReal_t dV[D];
              cmReal_t dM[D*D];

              // covar numerator b[j].sM[k]
              cmVOR_SubVVV(dV, D, oV, p->bV[j]->uM + (k*D));
              cmVOR_MultMMM( dM, D, D, dV, dV, 1 );
              cmVOR_MultVS(  dM, De2, c );
              cmVOR_AddVV(   sV, De2, dM ); 
            }

            if( meanFl )
            {
              // mean numerator b[j].uM[k]
              for(d=0; d<D; ++d)
                uV[d] += c * oV[ d ];
            }
          }
        }
      }

      if( timeProgFl && (t >= floor(T*progFrac)) )
      {
        cmCtxPrint(p->obj.ctx,"%i  ", (unsigned)round(progFrac*100) );
        progFrac+=progInc;
      }

    } // end time loop


    for(i=0; i<N; ++i)
    {

      // update the state transition matrix
      cmReal_t den = cmVOR_SumN(EpsM + i, N, N );

      assert(den != 0 );

      for(j=0; j<N; ++j)
        p->aM[ i + (j*N) ] = EpsM[ i + (j*N) ] / den;

      if( bFl )
      {
        // update the mean, covariance and mix coefficient
        cmGmm_t*        g   = p->bV[i];
        const cmReal_t* uV  = uM  + (i*D*K);
        const cmReal_t* sMV = sMM + (i*De2*K);

        for(k=0; k<K; ++k,uV+=D,sMV+=De2)
        {
          cmReal_t gjk = gamma_jk[ (k*N) + i ];

          if( meanFl )
            cmVOR_DivVVS(g->uM  + (k*D),   D,   uV,  gjk );
          
          if( covarFl )
            cmVOR_DivVVS(g->sMM + (k*De2), De2, sMV, gjk );

          if( mixFl )
            g->gV[k] =  gjk / cmVOR_SumN( gamma_jk + i, K, N );
        }

        if((rc = _cmGmmUpdateCovar(g,g->sMM)) != cmOkRC )
          goto errLabel;
      }
    }        

    assert( cmVOR_IsNormalZ(p->aM,N*N) );

    if( timeProgFl )
      cmCtxPrint(p->obj.ctx,"\n");

  } // end iter loop

  if( progFl)
    cmCtxPrint(p->obj.ctx,"\n");

  if( p->mfp != NULL )
  {
    // first line is iV[N]
    cmMtxFileRealExec(p->mfp,p->iV,p->N);
    
    // next N lines are aM[N,N]
    for(i=0; i<p->N; ++i)
      cmMtxFileRealExecN(p->mfp,p->aM + i,p->N,p->N);

    // next T lines are bM[T,N]
    if( p->bM != NULL )
      for(i=0; i<T; ++i)
        cmMtxFileRealExec(p->mfp, p->bM + (i*p->N),p->N);
  }

 errLabel:
  cmMemPtrFree(&alphaM);
  cmMemPtrFree(&betaM);
  cmMemPtrFree(&logPrV);
  cmMemPtrFree(&EpsM);
  cmMemPtrFree(&BK);
  cmMemPtrFree(&gamma_jk);
  cmMemPtrFree(&uM);
  cmMemPtrFree(&sMM);

  return rc;
}


void      cmChmmPrint(    cmChmm_t* p )
{
  unsigned i;
  cmCtxPrint(p->obj.ctx,"======================================== \n");
  cmVOR_PrintL("iV: ",  p->obj.err.rpt, 1,    p->N, p->iV);
  cmVOR_PrintL("aM:\n", p->obj.err.rpt, p->N, p->N, p->aM);

  for(i=0; i<p->N; ++i)
  {
    cmCtxPrint(p->obj.ctx,"bV[%i] ----------------- %i \n",i,i);
    cmGmmPrint(p->bV[i],false);
  }
}


void     cmChmmTestForward( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH  )
{
  cmReal_t oM[] = {
    0.117228,   0.110079,
    0.154646,   0.210436,
    0.947468,   0.558136,
    0.202023,   0.138123,
    0.929933,   0.456102,
    0.897566,   0.685078,
    0.945177,   0.663145,
    0.272399,   0.055107,
    0.863386,   0.621546,
    0.217545,   0.274709,
    0.838777,   0.650038,
    0.134966,   0.159472,
    0.053990,   0.264051,
    0.884269,   0.550019,
    0.764787,   0.554484,
    0.114771,   0.077518,
    0.835121,   0.606137,
    0.070733,   0.120015,
    0.819814,   0.588482,
    0.105511,   0.197699,
    0.824778,   0.533047,
    0.945223,   0.511411,
    0.126971,   0.050083,
    0.869497,   0.567737,
    0.144866,   0.197363,
    0.985726,   0.590402,
    0.181094,   0.192827,
    0.162179,   0.155297,
    1.034691,   0.513413,
    0.220708,   0.036158,
    0.750061,   0.671224,
    0.246971,   0.093246,
    0.997567,   0.680491,
    0.916887,   0.530981,
    0.022328,   0.121969,
    0.794031,   0.618081,
    0.845066,   0.625512,
    0.174731,   0.094773,
    0.968665,   0.652435,
    0.932484,   0.388081,
    0.202732,   0.148710,
    0.911307,   0.637139,
    0.211127,   0.201362,
    0.138152,   0.057290,
    0.819132,   0.579888,
    0.135625,   0.176140,
    0.146017,   0.157853,
    0.950319,   0.624150,
    0.285064,   0.038825,
    0.716844,   0.575189,
    0.907433,   0.504946,
    0.219772,   0.129993,
    0.076507,   0.193079,
    0.808906,   0.548409,
    0.880892,   0.523950,
    0.758099,   0.636729,
    1.014017,   0.557120,
    0.277888,   0.181492,
    0.877588,   0.508634,
    0.251266,   0.225890,
    0.990904,   0.482949,
    0.999899,   0.534579,
    0.904179,   0.707349,
    0.952879,   0.617955,
    0.172068,   0.151984,
    1.026262,   0.662600,
    0.812003,   0.430856,
    0.173393,   0.017885,
    0.099370,   0.146661,
    0.785785,   0.564333,
    0.698222,   0.449299,
    0.276539,   0.225314,
    0.799271,   0.618159,
    0.098813,   0.090839,
    0.883666,   0.554150,
    0.274934,   0.185403,
    0.200419,   0.109972,
    0.925076,   0.608610,
    0.864486,   0.348689,
    0.176733,   0.136235,
    0.967278,   0.656875,
    0.986994,   0.659877,
    1.015618,   0.596549,
    0.689903,   0.528107,
    0.978238,   0.630989,
    0.269847,   0.144358,
    0.092303,   0.139894,
    0.168185,   0.095327,
    0.897767,   0.584203,
    0.068316,   0.018452,
    0.953395,   0.530545,
    0.266405,   0.173987,
    0.233845,   0.205276,
    0.900060,   0.477108,
    0.052909,   0.053077,
    0.885850,   0.496546,
    0.268494,   0.104785,
    1.041405,   0.655079,
    1.055915,   0.697988,
    0.181569,   0.146840
  };

  unsigned i;
  cmReal_t iV[] = { .5 , .5};
  cmReal_t A[]  = { .3, .6, .7, .4 };

  cmReal_t cV0[] = { .7, .3 };
  cmReal_t uM0[] = { .2, .1, .1, .2 };
  cmReal_t sMM0[]= { .01, 0, 0, .01,  .01, 0, 0, .01 };

  cmReal_t cV1[] = { .2, .8 };
  cmReal_t uM1[] = { .8, .9, .9, .5 };
  cmReal_t sMM1[]= { .01, 0, 0, .01,  .01, 0, 0, .01 };
  
  
  unsigned T = 100;
  unsigned N = 2;
  unsigned K = 2;
  unsigned D = 2;

  cmReal_t alphaM[N*T];
  cmReal_t betaM[N*T];
  cmReal_t logPrV[T];
  unsigned qV[T];
  unsigned sV[T];
  cmReal_t oMt[T*D];

  // scale covariance
  cmVOR_MultVS(sMM0,D*D*K,1);
  cmVOR_MultVS(sMM1,D*D*K,1);


  cmCtx c;
  cmCtxAlloc(&c,rpt,lhH,stH);

  cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);

  cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);

  cmChmmPrint(p);

  cmChmmGenerate(p, oM, T, sV );

  cmChmmForward( p, oM, T, alphaM,logPrV );
  
  //cmVOR_PrintL("logPrV:\n",rpt,1,T,logPrV);

  cmCtxPrint(&c,"log prob:%f\n", cmVOR_Sum(logPrV,T));

  cmChmmBackward( p, oM, T, betaM );

  //cmVOR_PrintL("beta:\n",rpt,N,T,betaM);

  cmChmmDecode(p,oM,T,qV);

  cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  cmVOU_PrintL("qV:\n",rpt,1,T,qV);

  unsigned d=0;
  for(i=0; i<T; ++i)
    d += sV[i] != qV[i];

  cmCtxPrint(&c,"Diff:%i\n",d);


  cmPlotSetup("Chmm Forward Test",1,1);
  cmVOR_Transpose(oMt,oM,D,T); 
  cmPlotLineD(NULL, oMt, oMt+T, NULL, T, "blue", kXPlotPtId );
  cmPlotDraw();

  cmChmmFree(&p);

}

void     cmChmmTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
  
  time_t t =  time(NULL); //0x4b9e82aa; //time(NULL);
  srand( t );
  printf("TIME: 0x%x\n",(unsigned)t);
  
  //cmChmmTestForward(vReportFunc);
  //return;

  unsigned i;
  cmReal_t iV[]   = { 1.0/3.0, 1.0/3.0, 1.0/3.0 };
  cmReal_t A[]    = { .1, .4, .7, .4, .2, .2 };

  cmReal_t cV0[]  = { .7, .3 };
  cmReal_t uM0[]  = { .2, .1, .1, .2 };
  cmReal_t sMM0[] = { .01, 0, 0, .01,   .01, 0, 0, .01 };

  cmReal_t cV1[]  = { .2, .8 };
  cmReal_t uM1[]  = { .8, .9, .9, .8 };
  cmReal_t sMM1[] = { .01, 0, 0, .01,   .01, 0, 0, .01 };

  cmReal_t cV2[]  = { .5, .5 };
  cmReal_t uM2[]  = { .5, .5, .5, .5 };
  cmReal_t sMM2[] = { .01, 0, 0, .01,   .01, 0, 0, .01 };


  cmReal_t kmThreshProb = 0.001;
  unsigned kmMaxIterCnt = 10;
  unsigned iterCnt      = 20;


  unsigned N       = sizeof(iV)  / sizeof(iV[0]);
  unsigned K       = sizeof(cV0) / sizeof(cV0[0]);
  unsigned D       = sizeof(uM0) / sizeof(uM0[0]) / K;
  unsigned T       = 100;

  cmReal_t alphaM[N*T];
  cmReal_t oM[D*T];
  unsigned sV[T];
  unsigned qV[T];
  
  cmCtx c;
  cmCtxAlloc(&c,rpt,lhH,stH);

  cmCtxPrint(&c,"N:%i K:%i D:%i\n",N,K,D);

  cmChmm_t* p = cmChmmAlloc(&c,NULL,N,K,D,iV,A);

  cmChmmSetGmm(p,0,cV0,uM0,sMM0,0);
  cmChmmSetGmm(p,1,cV1,uM1,sMM1,0);
  cmChmmSetGmm(p,2,cV2,uM2,sMM2,0);

  // generate data using the parameters above
  cmChmmGenerate(p,oM,T,sV);  

  cmVOU_PrintL("sV: ",rpt,1,T,sV);

  cmChmmRandomize(p,oM,T);

  if(cmChmmSegKMeans(p,oM,T,kmThreshProb,kmMaxIterCnt,iterCnt) != cmOkRC )
    goto errLabel;


  if( cmChmmTrain(p,oM,T,iterCnt,0,0) != cmOkRC )
    goto errLabel;

  //cmChmmPrint(p);

  cmChmmDecode(p,oM,T,qV);

  cmReal_t pr = cmChmmForward(p,oM,T,alphaM,NULL);

  cmCtxPrint(&c,"pr:%f\n",pr);
  cmVOU_PrintL("sV:\n",rpt,1,T,sV);
  cmVOU_PrintL("qV:\n",rpt,1,T,qV);

  unsigned d=0;
  for(i=0; i<T; ++i)
    d += sV[i] != qV[i];

  cmCtxPrint(&c,"Diff:%i\n",d);

 errLabel:


  cmChmmFree(&p);
 
}

//------------------------------------------------------------------------------------------------------------
cmChord* cmChordAlloc( cmCtx* c, cmChord* ap, const cmReal_t* chromaM, unsigned T )
{
  unsigned i,j;
  unsigned S = 6;
  unsigned N = 24;
  unsigned D = 12;
  cmChord* p = cmObjAlloc(cmChord,c,ap);

  p->h = cmChmmAlloc( p->obj.ctx, NULL, 0, 0, 0, NULL, NULL );

  if( chromaM != NULL && T > 0 )
    if( cmChordInit(p,chromaM,T) != cmOkRC )
      cmChordFree(&p);

  p->N = N;
  p->D = D;
  p->S = kTonalSpaceDimCnt;

  /*
  // iv[N] aM[N*N] uM[D*N] sMM[D*D*N] phiM[D*S] tsxxxV[S]
  unsigned n   = ALIGN_T(N,    cmReal_t) +
                 ALIGN_T(N*N,  cmReal_t) +
                 ALIGN_T(D*N,  cmReal_t) +
                 ALIGN_T(D*D*N,cmReal_t) +
                 ALIGN_T(D*S,  cmReal_t) +
               2*ALIGN_T(S,    cmReal_t);

  p->iV        = cmArrayResizeZ(c, &p->memA, n, cmReal_t); 
  p->aM        = p->iV      + ALIGN_T(N,    cmReal_t);
  p->uM        = p->aM      + ALIGN_T(N*N,  cmReal_t);
  p->sMM       = p->uM      + ALIGN_T(D*N,  cmReal_t);
  p->phiM      = p->sMM     + ALIGN_T(D*D*N,cmReal_t);
  p->tsMeanV   = p->phiM    + ALIGN_T(D*S,  cmReal_t);
  p->tsVarV    = p->tsMeanV + ALIGN_T(S,    cmReal_t);
  */

  p->iV        = cmMemAllocZ( cmReal_t, N );
  p->aM        = cmMemAllocZ( cmReal_t, N*N);
  p->uM        = cmMemAllocZ( cmReal_t, D*N);
  p->sMM       = cmMemAllocZ( cmReal_t, D*D*N );
  p->phiM      = cmMemAllocZ( cmReal_t, D*S);
  p->tsMeanV   = cmMemAllocZ( cmReal_t, S );
  p->tsVarV    = cmMemAllocZ( cmReal_t, S );


  // initialize iV[N] (HMM initial state probabilities)
  cmVOR_Fill(p->iV,N,1.0/N);


  // initialize aM[N,N] (HMM transition matrix)
  cmReal_t epsilon    = 0.01;
  cmReal_t CMaj2any[] = { 12, 2, 8, 6, 4, 10, 0, 10, 4, 6, 8, 2, 5, 5, 9, 1, 11, 3, 7, 7, 3, 11, 1, 9 };
  
  for(i=0; i<N; ++i)
  {
    cmVOR_Copy( p->aM+(i*N), N, CMaj2any );
    cmVOR_Rotate( CMaj2any, N, 1 );
  }

  cmVOR_AddVS(p->aM, N*N, epsilon);
  cmVOR_DivVS(p->aM, N*N, ( (N/2)*(N/2) ) + (N*epsilon) );

  //cmVOR_PrintL("A:\n",p->obj.err.rpt,N,N,A);


  // initialize sMM[D*D,N] (HMM covariance matrices)
  cmReal_t diagMV[] = { 1, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
  cmReal_t diagmV[] = { 1, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 1.0, 0.2, 0.2, 0.2, 0.2 };
  cmReal_t Maj[D*D];
  cmReal_t Min[D*D];
  cmVOR_DiagZ(Maj,D,diagMV);
  Maj[ (4*D) + 0 ] = 0.6;  Maj[ (0*D) + 4 ] = 0.6;
  Maj[ (7*D) + 0 ] = 0.8;  Maj[ (0*D) + 7 ] = 0.8;
  Maj[ (7*D) + 4 ] = 0.8;  Maj[ (4*D) + 7 ] = 0.8;

  
  cmVOR_DiagZ(Min,D,diagmV);
  Min[ (3*D) + 0 ] = 0.6;  Min[ (0*D) + 3 ] = 0.6;
  Min[ (7*D) + 0 ] = 0.8;  Min[ (0*D) + 7 ] = 0.8;
  Min[ (7*D) + 3 ] = 0.8;  Min[ (3*D) + 7 ] = 0.8;

  cmReal_t* sM = p->sMM;
  for(i=0; i<N/2; ++i,sM+=D*D)
    cmVOR_RotateM( sM, D, D, Maj, i, i );

  for(i=0; i<N/2; ++i,sM+=D*D)
    cmVOR_RotateM( sM, D, D, Min, i, i );
  

  /*
  cmVOR_PrintL("Maj:\n",p->obj.err.rpt,D,D,Maj);
  cmVOR_PrintL("Min:\n",p->obj.err.rpt,D,D,Min);

  for(i=0; i<N; ++i)
  {
    cmCtxPrint(c,"%i----\n",i);
    cmVOR_PrintL("sM:\n",p->obj.err.rpt,D,D,sMM + (i*D*D));
  }
  */

  // initialize uM[D,N]  (HMM GMM mean vectors)
  cmVOR_Fill(p->uM,D*N,0);
  for(i=0; i<D; ++i)
  {
    unsigned dom  = (i+7) % D;
    unsigned medM = (i+4) % D;
    unsigned medm = (i+3) % D;

    p->uM[ (i     * D) + i    ] = 1;
    p->uM[ (i     * D) + medM ] = 1;
    p->uM[ (i     * D) + dom  ] = 1;

    p->uM[ ((i+D) * D) + i    ] = 1;
    p->uM[ ((i+D) * D) + medm ] = 1;
    p->uM[ ((i+D) * D) + dom  ] = 1;
  }

  cmVOR_AddVS(p->uM,D*N,0.01);
  
  for(i=0; i<N; ++i)
    cmVOR_NormalizeProbability( p->uM + (i*D), D);


  // initialize phiM[D,S]
  cmReal_t phi[D*S];
  for(i=0,j=0; i<D; ++i,++j) 
    phi[j] = sin( M_PI*7.0*i/6.0 );

  for(i=0; i<D; ++i,++j) 
    phi[j] = cos( M_PI*7.0*i/6.0 );

  for(i=0; i<D; ++i,++j) 
    phi[j] = sin( M_PI*3.0*i/2.0 );

  for(i=0; i<D; ++i,++j) 
    phi[j] = cos( M_PI*3.0*i/2.0 );

  for(i=0; i<D; ++i,++j) 
    phi[j] = 0.5 * sin( M_PI*2.0*i/3.0 );

  for(i=0; i<D; ++i,++j) 
    phi[j] = 0.5 * cos( M_PI*2.0*i/3.0 );
  
  cmVOR_Transpose(p->phiM,phi,D,S);
  

  return p;

}

cmRC_t     cmChordFree( cmChord** pp )
{
  cmRC_t   rc = cmOkRC;
  cmChord* p  = *pp;

  if( pp == NULL || *pp == NULL )
    return cmOkRC;

  if((rc = cmChordFinal(p)) != cmOkRC )
    return rc;

  cmChmmFree( &p->h );

  cmMemPtrFree(&p->iV);
  cmMemPtrFree(&p->aM);
  cmMemPtrFree(&p->uM);
  cmMemPtrFree(&p->sMM);
  cmMemPtrFree(&p->phiM);
  cmMemPtrFree(&p->tsMeanV);
  cmMemPtrFree(&p->tsVarV);

  cmMemPtrFree(&p->chromaM);
  cmMemPtrFree(&p->tsM);
  cmMemPtrFree(&p->cdtsV);


  cmObjFree(pp);
  return rc;
}

cmRC_t     cmChordInit( cmChord* p, const cmReal_t* chromaM, unsigned T )
{
  cmRC_t   rc     = cmOkRC;
  unsigned i;
  unsigned N     = p->N;    // count of states
  unsigned K     = 1;       // count of components per mixture
  unsigned D     = p->D;    // dimensionality of the observation vector
  unsigned S     = p->S;    //
  cmReal_t alpha = 6.63261; // alpha norm coeff

  if((rc = cmChordFinal(p)) != cmOkRC )
    return rc;

  // Create the hidden markov model
  cmChmmInit( p->h, N, K, D, p->iV, p->aM);

  // load the GMM's for each markov state
  cmReal_t mixCoeff = 1.0;
  bool     diagFl   = false;
  for(i=0; i<N; ++i)
    if((rc = cmChmmSetGmm(p->h, i, &mixCoeff, p->uM + (i*D), p->sMM+(i*D*D), diagFl )) != cmOkRC )
      return rc;


  // Allocate memory
  //           chromaM[D,T]               tsM[S,T]                   cdtsV[T]
  /*
  unsigned n = ALIGN_T(D*T,cmReal_t) + ALIGN_T(S*T,cmReal_t) + ALIGN_T(T,cmReal_t);
  p->chromaM = cmArrayResizeZ(c, &p->memB, n, cmReal_t);
  p->tsM     = p->chromaM + ALIGN_T(D*T,cmReal_t);
  p->cdtsV   = p->tsM     + ALIGN_T(S*T,cmReal_t);
  p->T       = T;
  */

  p->chromaM = cmMemResizeZ( cmReal_t, p->chromaM, p->D*T );
  p->tsM     = cmMemResizeZ( cmReal_t, p->tsM,     p->S*T );
  p->cdtsV   = cmMemResizeZ( cmReal_t, p->cdtsV,   p->D*T );
  p->T       = T;

  // Allocate local memory
  //  qV[], triadIntV[]        triadSeqV[]       tsNormsV[]
  /*
  n = 2*ALIGN_B(T,unsigned) + ALIGN_B(T,int) + ALIGN_B(T,cmReal_t);
  cmArray mem;
  cmArrayAlloc(c, &mem);

  unsigned* qV        = (unsigned*) cmArrayResize(c, &mem, n, char);
  unsigned* triadSeqV = (unsigned*) (qV        + ALIGN_T(T,unsigned));
  int*      triadIntV = (int*)      (triadSeqV + ALIGN_T(T,unsigned));
  cmReal_t* tsNormsV  = (cmReal_t*) (triadIntV + ALIGN_T(T,int));
  */

  //unsigned qV[T];
  //unsigned triadSeqV[T];
  //int triadIntV[T];
  //cmReal_t tsNormsV[T];


  unsigned* qV        = cmMemAlloc( unsigned, T );
  unsigned* triadSeqV = cmMemAlloc( unsigned, T );
  int*      triadIntV = cmMemAlloc( int,      T );
  cmReal_t* tsNormsV  = cmMemAlloc( cmReal_t, T );


  // Take the alpha norm of chroma and store the result in p->chromaM[]
  for(i=0; i<T; ++i)
    p->chromaM[i] = cmVOR_AlphaNorm( chromaM + (i*D), D, alpha);      

  cmVOR_DivVVS(p->chromaM,D*T,chromaM, cmVOR_AlphaNorm(p->chromaM,T,alpha));


  // Train the HMM iniital state prob. p->h->iV[] and transition matrix p->h->aM[]
  unsigned flags   = kNoTrainMixCoeffChmmFl | kNoTrainMeanChmmFl | kNoTrainCovarChmmFl;
  unsigned iterCnt = 40;
  if( chromaM != NULL && T > 0 )
    if((rc = cmChmmTrain(p->h, p->chromaM, p->T, iterCnt, 0, flags )) != cmOkRC )
      goto errLabel;

  // Find the most likely chords using a Viterbi decoding of the chroma.
  cmChmmDecode(p->h,p->chromaM,T,qV);


  // Reorder the chord sequence cmcording to circle of fifths
  unsigned map[] = {0, 14, 4, 18, 8, 22, 12, 2, 16, 6, 20, 10, 17, 7, 21, 11, 1, 15, 5, 19, 9, 23, 13, 3 };

  for(i=0; i<T; ++i)
    qV[i] = map[ qV[i] ];

  //cmVOU_PrintL("qV:\n",p->obj.err.rpt,1,T,qV);

  // Smooth the chord sequence with a median filter.
  cmVOU_MedianFilt(qV,T,3,triadSeqV,1);

  //cmVOU_PrintL("triadSeqV:\n",p->obj.err.rpt,1,T,triadSeqV);

  // Calculate the chord change distance on the circle of fifths.
  int d = 0;
  for(i=0; i<T; ++i)
  {
    int v = abs(d);

    assert(v<N);

    v = (v<=11) ? v : -(12-(v-12));

    if( i > 0 )
      triadIntV[i-1] = (d < 0 ? -1 : 1) * v;

    if( i + 1 < T)
      d = triadSeqV[i+1] - triadSeqV[i];    
  }

  // Project chroma into a 6D tonal space.
  cmVOR_MultMMM( p->tsM,S,T,p->phiM,p->chromaM,D);

  // Find the norm of p->tsM[6,T]
  cmVOR_Fill(tsNormsV,T,0);
  for(i=0; i<T; ++i)
    tsNormsV[i] = cmVOR_MultSumVV( p->tsM + (i*S), p->tsM + (i*S), S );
  
  cmVOR_PowVS(tsNormsV,T,0.5);

  // Take the cosine distance.
  p->cdtsV[0] = 1;
  for(i=1; i<T; ++i)
    p->cdtsV[i] = cmVOR_MultSumVV( p->tsM + ((i-1)*S), p->tsM + (i*S), S );

  for(i=0; i<T-1; ++i)
    p->cdtsV[i+1] /= tsNormsV[i] * tsNormsV[i+1];
  
  //cmVOR_PrintL("tsNormsV:\n",p->obj.err.rpt,1,T,tsNormsV);
  //cmVOR_PrintL("CDTS:\n",    p->obj.err.rpt,1,T,p->cdtsV);


  p->triadSeqMode = cmVOU_Mode(triadSeqV,T);
  p->triadSeqVar  = cmVOU_Variance(triadSeqV,T,NULL);
  p->triadIntMean = cmVOI_Mean(triadIntV,T);
  p->triadIntVar  = cmVOI_Variance(triadIntV,T,&p->triadIntMean);


  cmVOR_MeanM(     p->tsMeanV, p->tsM, S, T, 1 );
  cmVOR_VarianceM( p->tsVarV,  p->tsM, S, T, p->tsMeanV, 1);

  p->cdtsMean = cmVOR_Mean(     p->cdtsV, T );
  p->cdtsVar  = cmVOR_Variance( p->cdtsV, T, &p->cdtsMean );

  /*
  cmReal_t tsMt[T*S];
  cmKbRecd kb;
  cmPlotInitialize(NULL);
  cmPlotSetup("Chords",1,1);
  cmCtxPrint(c,"%s\n","press any key");
  //cmPlotLineD( NULL, NULL, p->cdtsV, NULL, T, NULL, kSolidPlotLineId );
  cmVOR_Transpose(tsMt,p->tsM,S,T);
  cmPlotLineMD(NULL, tsMt, NULL, T, S, kSolidPlotLineId);
  cmPlotDraw();
  cmKeyPress(&kb);
  cmPlotFinalize();
  */

 errLabel:
  cmMemPtrFree(&qV);
  cmMemPtrFree(&triadSeqV);
  cmMemPtrFree(&triadIntV);
  cmMemPtrFree(&tsNormsV);

  return rc;
}

cmRC_t     cmChordFinal(    cmChord* p )
{ return cmOkRC; }

void       cmChordTest( cmRpt_t* rpt, cmLHeapH_t lhH, cmSymTblH_t stH )
{
  cmCtx c;
  cmCtxAlloc(&c,rpt,lhH,stH);

  cmChord* p = cmChordAlloc(&c,NULL,NULL,0);

  cmChordFree(&p);
}