/* Copyright (c) 1998-2000 Doshita Lab. Speech Group, Kyoto University */
/* Copyright (c) 2000-2002 Speech and Acoustics Processing Lab., NAIST */
/*   All rights reserved   */

/* wav2mfcc-pipe.c --- split Wav2MFCC to perform per-frame-basis,
   and also realtime CMN for 1st-pass pipe-lining */

/* $Id: wav2mfcc-pipe.c,v 1.4 2002/09/11 22:01:50 ri Exp $ */

/* most codes from "wav2mfcc.c" */
/************************************************************************/
/*    wav2mfcc.c   Convert Speech file to MFCC_E_D_(Z) file             */
/*----------------------------------------------------------------------*/
/*    Author    : Yuichiro Nakano                                       */
/*                                                                      */
/*    Copyright(C) Yuichiro Nakano 1996-1998                            */
/*----------------------------------------------------------------------*/
/************************************************************************/


#include <sent/stddefs.h>
#include <sent/mfcc.h>

static double *fbank;                    /* Filterbank */
static FBankInfo fb;

/* preparation */
void
WMP_init(Value para, float **bf, float *ssbuf, int ssbuflen)
{
  /* Get filterbank information */
  fb = InitFBank(para);
  
  if((fbank = (double *)mymalloc((para.fbank_num+1)*sizeof(double))) == NULL){
    j_error("WMP_init failed\n");
  }
  if((*bf = (float *)mymalloc(fb.fftN * sizeof(float))) == NULL){
    j_error("WMP_init failed\n");
  }

  if (ssbuf != NULL) {
    /* check ssbuf length */
    if (fb.fftN != ssbuflen) {
      j_error("Error: Wav2MFCC_E_D: noise spectrum length not match\n");
    }
  }
}

/* calc MFCC_E_Z for 1 frame */
void
WMP_calc(float *mfcc, float *bf, Value para, float *ssbuf)
{
  float energy;

  if (para.raw_e) {
    /* calculate log raw energy */
    energy = CalcLogRawE(bf, para.framesize);
    /* pre-emphasize */
    PreEmphasise(bf, para);
    /* hamming window */
    Hamming(bf, para.framesize);
  } else {
    /* pre-emphasize */
    PreEmphasise(bf, para);
    /* hamming window */
    Hamming(bf, para.framesize);
    /* calculate log energy */
    if (!para.c0) energy = CalcLogRawE(bf, para.framesize);
  }
  /* filterbank */
  MakeFBank(bf, fbank, fb, para, ssbuf);
  /* 0'th cepstral parameter */
  if(para.c0)energy = CalcC0(fbank, para);
  /* MFCC */
  MakeMFCC(fbank, mfcc, para);
  /* weight cepstrum */
  WeightCepstrum(mfcc, para);
  /* Normalise Log Energy is not implemented */
  if(para.enormal) {
    j_error("normalize log energy is not implemented in pipeline mode\n");
  }
  mfcc[para.mfcc_dim] = energy;
}

/* calc MFCC_E_D_Z from MFCC_E_Z for 1 frame */
void WMP_Delta(float **c, int t, int frame, Value para)
{
  int theta, n, dim, B = 0;
  float A1, A2, sum;

  for(theta = 1; theta <= para.delWin; theta++)
    B += theta * theta;

  dim = para.vec_num / 2;

  for(n = 1; n <= dim; n++){
    sum = 0;
    for(theta = 1; theta <= para.delWin; theta++){
      /* Replicate the first or last vector */
      /* at the beginning and end of speech */
      if(t - theta < 0) A1 = c[0][n - 1];
      else A1 = c[t - theta][n - 1];
      if(t + theta >= frame) A2 = c[frame - 1][n - 1];
      else A2 = c[t + theta][n - 1];
      sum += theta * (A2 - A1);
    }
    c[t][n + para.mfcc_dim] = sum / (2 * B);
  }
}


/* real-time CMN */
/* 01/01/19 store last 500frame samples */
#define CPMAX 500
static float *lastcsum = NULL;
static float **lastc = NULL;
static int dim;
static int cp = 0;
static int filled = 0;
static float *mfcc_ave = NULL;
static float *mfcc_ave_new = NULL;

/* calc mfcc_ave[] and clear lastcsum[] for next input */
void
CMN_realtime_update()
{
  int i;
  static float *p;

  if (mfcc_ave_new == NULL) {
    mfcc_ave_new = (float *)mymalloc(sizeof(float) * dim);
  }
  for(i=0;i<dim;i++) {
    mfcc_ave_new[i] = lastcsum[i] / (float)filled;
  }
  if (mfcc_ave == NULL) {
    mfcc_ave = (float *)mymalloc(sizeof(float) * dim);
  }
  /* swap current <-> new */
  p = mfcc_ave;
  mfcc_ave = mfcc_ave_new;
  mfcc_ave_new = p;
}

/* both accumulate to lastcsum[] and do CMN by the last mean vector */
void
CMN_realtime(float *mfcc, int cdim)
{
  int t,i;

  if (lastcsum == NULL) {	/* initialize */
    dim = cdim;
    lastcsum = (float *)mymalloc(sizeof(float) * dim);
    for(i=0;i<dim;i++) lastcsum[i] = 0.0;
    lastc = (float **)mymalloc(sizeof(float*) * CPMAX);
    for(t=0;t<CPMAX;t++) {
      lastc[t] = (float *)mymalloc(sizeof(float) * dim);
    }
    filled = 0;
    cp = 0;
  }
  if (filled < CPMAX) {
    filled++;
    for(i=0;i<dim;i++) {
      lastcsum[i] += mfcc[i];
      lastc[cp][i] = mfcc[i];
    }
  } else {
    for(i=0;i<dim;i++) {
      lastcsum[i] -= lastc[cp][i];
      lastcsum[i] += mfcc[i];
      lastc[cp][i] = mfcc[i];
    }
  }
  cp++;
  if (cp >= CPMAX) cp = 0;
  
  if (mfcc_ave != NULL) {
    for(i=0;i<dim;i++) {
      mfcc[i] -= mfcc_ave[i];
    }
  }
}

/* read binary with byte swap (assume file is BE) */
static boolean
myread(void *buf, size_t unitbyte, int unitnum, FILE *fp)
{
  if (myfread(buf, unitbyte, unitnum, fp) < unitnum) {
    return(FALSE);
  }
#ifndef WORDS_BIGENDIAN
  swap_bytes(buf, unitbyte, unitnum);
#endif
  return(TRUE);
}
/* write binary with byte swap (assume file is BE) */
static boolean
mywrite(void *buf, size_t unitbyte, int unitnum, int fd)
{
  int tmp;
#ifndef WORDS_BIGENDIAN
  swap_bytes(buf, unitbyte, unitnum);
#endif
  if (write(fd, buf, unitbyte * unitnum) < unitbyte * unitnum) {
    return(FALSE);
  }
#ifndef WORDS_BIGENDIAN
  swap_bytes(buf, unitbyte, unitnum);
#endif
  return(TRUE);
}

/* load CMN parameter from file */
boolean
CMN_load_from_file(char *filename, int dim)
{
  FILE *fp;
  int num;
  int veclen;
  if ((fp = fopen_readfile(filename)) == NULL) {
    j_printerr("Error: CMN_load_from_file: failed to open\n");
    return(FALSE);
  }
  /* read header */
  if (myread(&veclen, sizeof(int), 1, fp) == FALSE) {
    j_printerr("Error: CMN_load_from_file: failed to read header\n");
    fclose_readfile(fp);
    return(FALSE);
  }
  /* check length */
  if (veclen != dim) {
    j_printerr("Error: CMN_load_from_file: length mismatch\n");
    fclose_readfile(fp);
    return(FALSE);
  }
  /* read body */
  if (mfcc_ave == NULL) {
    mfcc_ave = (float *)mymalloc(sizeof(float) * dim);
  }
  if (myread(mfcc_ave, sizeof(float), dim, fp) == FALSE) {
    j_printerr("Error: CMN_load_from_file: failed to read\n");
    fclose_readfile(fp);
    return(FALSE);
  }
  if (fclose_readfile(fp) == -1) {
    j_printerr("Error: CMN_load_from_file: failed to close\n");
    return(FALSE);
  }

  return(TRUE);
}
/* save CMN parameter to file */
boolean
CMN_save_to_file(char *filename)
{
  int fd;

  if ((fd = creat(filename, 0644)) == -1) {
    j_printerr("Error: CMN_save_to_file: failed to open\n");
    return(FALSE);
  }
  /* write header */
  if (mywrite(&dim, sizeof(int), 1, fd) == FALSE) {
    j_printerr("Error: CMN_save_to_file: failed to write header\n");
    close(fd);
    return(FALSE);
  }
  /* write body */
  if (mfcc_ave == NULL) {
    j_printerr("Error: no data to write?? should not happen!\n");
  }
  if (mywrite(mfcc_ave, sizeof(float), dim, fd) == FALSE) {
    j_printerr("Error: CMN_save_to_file: failed to write header\n");
    close(fd);
    return(FALSE);
  }
  close(fd);
  
  return(TRUE);
}
  
