#include "signal_processing.h"

// Vypocita a vrai cosinovu transformaci (II. typu) vektoru S.
matrix * sp_dct_II (matrix * s)
{

  unsigned int k, m, n, N, M;
  double sum;
  M = matrix_size_m(s);
  N = matrix_size_n(s);
  matrix * dct_s = matrix_alloc(M, N);

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

    sum = 0;

    for(m = 0; m<M; m++) {

      sum = sum+matrix_get(s, m, n) * 1/sqrt((double) M);

    }

    matrix_set(dct_s, 0, n, sum);

    for(k = 1; k<M; k++) {

      sum = 0;

      for(m = 0; m<M; m++) {

        sum = sum+matrix_get(s, m, n)*cos((PI/N)*k*(n+0.5))*sqrt(2/(double) M);

      }

      matrix_set(dct_s, k, n, sum);

    }

  }

  return dct_s;

}

// Vypocita a vrati Fourierovu transformaci matice S.
matrix * sp_dft (matrix * s)
{

  unsigned int k, m, n, N, M;
  double sum;
  M = matrix_size_m(s);
  N = matrix_size_n(s);
  matrix * dct_s = matrix_alloc(M, N);

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

    sum = 0;

    for(m = 0; m<M; m++) {

      sum = sum+matrix_get(s, m, n) * 1/sqrt((double) M);

    }

    matrix_set(dct_s, 0, n, sum);

    for(k = 1; k<M; k++) {

      sum = 0;

      for(m = 0; m<M; m++) {

        sum = sum+matrix_get(s, m, n)*cos((PI/N)*k*(n+0.5))*sqrt(2/(double) M);

      }

      matrix_set(dct_s, k, n, sum);

    }

  }

  return dct_s;

}

// Vypocita a vrati energii signalu sloupcove matice.
// E = SUM (S.^2)
matrix * sp_signal_log_energy (matrix * s)
{

  double energy;
  unsigned int m, n, N, M;
  matrix * E;
  M = matrix_size_m(s);
  N = matrix_size_n(s);
  E = matrix_alloc(1, N);

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

    energy = 0;

    for(m = 0; m<M; m++) {

      energy = energy+pow(matrix_get(s, m, n), 2);

    }

    matrix_set(E, 0, n, log(energy));

  }

  return E;

}

// Vrati matici kde v radcich jsou jednotlive segmenty vektoru VEC.
// Jednotlive segmenty jsou okna ze signalu VEC s velikosti WIN a posunem
// INC. Pokud je nastaven ADD_NULL nastave SP_ADD_NULL tak se koncove okno
// zarovna nulami.
matrix * sp_enframe(matrix * vec, unsigned int win, unsigned int inc, unsigned int add_null)
{
  matrix * M;
  unsigned int i, j, n, N;
  if(matrix_size_n(vec)!=1) {
    printf("error: %s: %i: Matice \"s\" ma %i sloupcu a musi mit 1.\n", (const char *) __FILE__, (int) __LINE__, matrix_size_n(vec));
    return NULL;
  }
  N = n = floor(((double) (matrix_size_m(vec)-win+inc))/((double) inc));

  if (add_null == SP_ADD_NULL) {
    N++;
  }

  M = matrix_alloc(win, N);
  // Naplneni matice MAT.
  for(j = 0; j<n; j++) {
    for(i = 0; i<win; i++) {
      matrix_set(M, i, j, matrix_get(vec, j*inc+i, 0));
    }
  }

  if (add_null == SP_ADD_NULL) {
    for (i = 0; i<matrix_size_m(vec)-n*inc; i++) {
      matrix_set(M, i, n, matrix_get(vec, n*inc+i, 0));
    }
    for (i = matrix_size_m(vec)-n*inc; i<win; i++) {
      matrix_set(M, i, n, 0);
    }
  }

  return M;
}

// Aplikuje na matici X digitalni filtr s koeficienty A, B (A,B - vektory).
matrix * sp_filter (matrix * x, matrix * A, matrix * B)
{

  unsigned int mx, nx, na, nb, i, j, k;
  mx = matrix_size_m(x);
  nx = matrix_size_n(x);
  na = matrix_size_n(A);
  nb = matrix_size_n(B);
  double sum;

  matrix * filt_x = matrix_alloc(mx, nx);

  // Pro vsechny radky matice X.
  for(i = 0; i<mx; i++) {

    // Pro kazdy prvek v radku vektoru X.
    for(j = 0; j<nx; j++) {

      sum = 0;

      // Vypocitani hodnoty FILT_X[i][j] pomoci koeficientu B1, B2, ..., BN.
      for(k = 0; (k<nb)&(k<j+1); k++) {

        sum = sum+matrix_get(B, 0, k)*matrix_get(x, i, j-k);

      }

      // Vypocitani hodnoty FILT_X[i][j] pomoci koeficientu A1, A2, ..., AN.
      for(k = 1; (k<na)&(k<j+1); k++) {

        sum = sum-matrix_get(A, 0, k)*matrix_get(filt_x, i, j-k);

      }

      matrix_set(filt_x, i, j, sum);

    }

  }

  return filt_x;

}

matrix *sp_diff(matrix *s, double dt)
{
  unsigned int i, j, m, n;
  double fa, fb;
  matrix *ds = matrix_alloc(matrix_size_m(s), matrix_size_n(s));
  m = matrix_size_m(ds);
  n = matrix_size_n(ds);

  // Vypocet dopredne derivace
  for (i = 0; i<m; i++) {

    for (j = 0; j<n-1; j++) {
      fa = matrix_get(s, i, j);
      fb = matrix_get(s, i, j+1);
      matrix_set(ds, i, j, (fb-fa)/dt);
    }

  }

  // Konec derivace je aproximovan primkou
  for (i = 0; i<m; i++) {
    matrix_set(ds, i, n-1, matrix_get(ds, i, n-2));
  }

  return ds;
}

matrix *sp_deltas(matrix *x, unsigned int M)
{
  int i, j, m;
  double delta, sum, fb, fa;

  sum = 0;
  for (m = 1; m <= M; m++) {
    sum += pow(m, 2);
  }

  // Alokace matice pro vypocet delta priznaku.
  matrix *deltas_x = matrix_alloc(matrix_size_m(x), matrix_size_n(x));
  // Vypocek delta priznaku
  for (i = M; i < matrix_size_n(x)-M; i++) {
    for (j = 0; j < matrix_size_m(x); j++) {
      delta = 0;
      for (m = 1; m <= M; m++) {
        fb = matrix_get(x, j, i+m);
        fa = matrix_get(x, j, i-m);
        delta += m * (fb - fa);
      }
      matrix_set(deltas_x, j, i, delta/sum );
    }
  }

  // Aproximace deta priznaku v prvnich M sloupcich
  for (i = 0; i < M; i++) {
    for (j = 0; j < matrix_size_m(x); j++) {
      delta = 0;
      for (m = 1; m <= M; m++) {
        fb = matrix_get(x, j, i+m);
        if (i-m <= 0) {
          fa = matrix_get(x, j, 0);
        } else {
          fa = matrix_get(x, j, i-m);
        }
        delta += m * (fb - fa);
      }
      matrix_set(deltas_x, j, i, delta/sum );
    }
  }

  // Aproximace deta priznaku v poslednich M sloupcich
  for (i = matrix_size_n(x)-M; i < matrix_size_n(x); i++) {
    for (j = 0; j < matrix_size_m(x); j++) {
      delta = 0;
      for (m = 1; m <= M; m++) {
        if ( m+i >= matrix_size_n(x)-1 ) {
          fb = matrix_get(x, j, matrix_size_n(x)-1);
        } else {
          fb = matrix_get(x, j, i+m);
        }
        fa = matrix_get(x, j, i-m);
        delta += m * (fb - fa);
      }
      matrix_set(deltas_x, j, i, delta/sum );
    }
  }
  
  return deltas_x;
}

// Vrati 1 pokud je X >= 0 a 0 pokud je X < 0.
int sp_signum (double x)
{

  if(x>=0) {

    return 1;

  } else {

    return -1;

  }

}

// Vrati pocet pruchodu nulou.
matrix * sp_zero_crossing (matrix * mat)
{

  int i, j, M, N;
  M = matrix_size_m(mat);
  N = matrix_size_n(mat);
  matrix * zc = matrix_alloc(0, N);

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

    matrix_set(zc, 0, i, 0);

  }

  for(i = i; i<M; i++) {

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

      matrix_set(zc, i, j, abs(sp_signum(matrix_get(mat, i, j))-sp_signum(matrix_get(mat, i-1, j))));

    }

  }

  return zc;

}

unsigned int sp_frame_first_index (unsigned int si, unsigned int win, unsigned int inc)
{
  unsigned int I, NFI, FI, NI, IRETURN;
  // Zjisti na kolikatem ramci se vyskytuje SI.
  I = floor(((double)si)/((double)inc));
  FI = si-I*inc;
  NFI = win-FI;
  NI = floor(((double)NFI)/((double)inc));

  // Pokud by vychazel zaporny index znamena to ze SI se nachazi v nultem ramci.
  if ((((int)I)-((int)NI))<0) {
    IRETURN = 0;
  } else {
     IRETURN = I-NI;
  }

  return IRETURN;
}


unsigned int sp_frame_last_index (unsigned int si, unsigned int win, unsigned int inc)
{
  return floor(((double)si)/((double)inc));
}


unsigned int sp_frame_middle_index (double si, unsigned int win, unsigned int inc)
{
  //unsigned int ;
  double i, j, S;

  S = si-((double) win)/2;

  i = S/((double) inc);

  if (i<=0) {
    return 0;
  }

  j = i-((unsigned int) i);
  
  if (j>0.5) {
    return ((unsigned int) i)+1;
  } else {
    return ((unsigned int) i);
  }

}

/*
 * Do BEG a END ulozi indexy zacatku a konce slova daneho ramce v useku signalu SN.
 * WIN - delka okna
 * INC - posun okna
 */
void sp_frame_location(unsigned int sa, unsigned int sb, unsigned int *beg, unsigned int *end, unsigned int win, unsigned int inc)
{
  unsigned int a, b, c;
  a = sp_frame_last_index(sa, win, inc);
  b = sp_frame_first_index(sb, win, inc);
  // Pokud je
  if (sa >= sb) {
    c = (unsigned int) round((((double) sa) + ((double) sb))/2);
    a = b = sp_frame_middle_index(c, win, inc);
  }
  *beg = a;
  *end = b;  
}


sp_signal_centre(matrix *signal)
{
  double mean = matrix_mean(signal);
  unsigned int i, j;

  // Nyni se vsechny prvky signalu usttredni.
  for (i = 0; i<matrix_size_m(signal); i++) {
    for (j = 0; j<matrix_size_n(signal); j++) {
      matrix_set(signal, i, j, matrix_get(signal, i, j)-mean); // Odecteni stredni slozky
    }
  }
}


double sp_freq2mel (double f_hz)
{

  return (2959*log10(1+f_hz/700));

}

// Prepocita melovou frekvenci F_MEL na klasickou frekvenci F_HZ.


double sp_mel2freq(double f_mel)
{

  return (700*exp(log(10)*f_mel/2959)-700);

}

// Vypocita a vrati ukazatel na vektor MFCC koeficientu o poctu NB.
// Kazdy flitr ma NF hodnot. Trojuhelnikove filtry zacinaji
// na frekvenci FHZ_MIN a koncina FHZ_MAX.
//
// Vypocet MFCC :
// 1. DFT signalu
// 2. Vynasobeni vahovymi filtry
// 3. eneregie jednotlivych koeficientu
// 4. logaritmus jednotlivych koeficientu
// 5. DCT II. typu z jednotlivych koeficientu
//
// pozn.: Funkce neaplikuje na signal zadny filtr ani ho nijak neupravuje.
//	  Veskere upravy signalu se musi udelat pred zavolanim funkce.
matrix * sp_mfcc(matrix * s, unsigned int nb, double mel_min, double mel_max, double F)
{

  if (matrix_size_m(s) % 2!=0) {
    printf("error: %s: %i: Pocet prvku na ramec neni delitelny dvema beze zbytku !.\n", __FILE__, __LINE__);
    exit(2);
  }

  unsigned int i, j, l, m, n;
  m = matrix_size_m(s);
  n = matrix_size_n(s);
  matrix * mfcc_coef = matrix_alloc(nb, n);
  struct complex_t *cs = malloc(sizeof(struct complex_t)*m);
  struct complex_t *dft_cs;

  for (i = 0; i<n; i++) {
    for (j = 0; j<m; j++) {
      cs[j].re = matrix_get(s, j, i)*0.54-0.46*cos(2*PI*j/(m-1));
      cs[j].im = 0;
    }
    // Rychla fourierova transformace
    dft_cs = FFT_simple(cs, m);

    double A, fa, fb, fc, ma, mb, mc, dm, dfn, w, coef;
    unsigned int ia, ib, ic;
    dm = (mel_max-mel_min)/nb;
    // urceni kolik dilku je 1 Hz. Dvojka ve jmenovateli je tam z toho duvodu,
    // protoze spektrum je symetricke.
    dfn = ((double) n)/(2*F);

    // Vsechny banky filtru vynasobim signalem z FFT.
    for (j = 1; j<=nb; j++) {
      // Melovske frekvence daneho trojuhelniku.
      ma = mel_min+dm*(j-1);
      mb = mel_min+dm * j;
      mc = mel_min+dm*(j+1);
      // Frekvence (Hz) trojuhelnikovych filtru.
      fa = sp_mel2freq(ma);
      fb = sp_mel2freq(mb);
      fc = sp_mel2freq(mc);
      // Zjisteni I-tych indexu podle fa,fb,fc a F.
      ia = round(fa*dfn);
      ib = round(fb*dfn);
      ic = round(fc*dfn);
      // Vysledny koeficient pro dany filtr.
      coef = 0;

      for (l = ia; l<ib; l++) {
        A = sqrt(pow(dft_cs[l].re, 2)+pow(dft_cs[l].im, 2));
        w = l*dfn/(fb-fa)-fa/(fb-fa);
        coef = coef+w * A;
      }

      for (l = ib; l<ic+1; l++) {
        A = sqrt(pow(dft_cs[l].re, 2)+pow(dft_cs[l].im, 2));
        w = l*dfn/(fc-fb)-fc/(fc-fb);
        coef = coef+w * A;
      }

      // Do mfcc_coef ulozim ulozim log ( coef ^ 2 ).
      matrix_set(mfcc_coef, j-1, i, log10(pow(coef, 2)));
    }

    free(dft_cs);
  }

  free(cs);
  matrix * mfcc_return = sp_dct_II(mfcc_coef);
  matrix_free(mfcc_coef);
  return mfcc_return;
}


void sp_stat_norm_aply(matrix *mat, matrix *avarge, matrix *stdev)
{
  // Kontrola jestli maji vsechny matice stejny pocet radku
  if ((matrix_size_m(mat)!=matrix_size_m(avarge))|(matrix_size_m(mat)!=matrix_size_m(stdev))) {
    fprintf(stderr, "%s: %i: Vstupni matice (%i radku) nemuze byt transformovana, protoze matice prumeru (%i) a matice smerodatnych odchylek (%i) nemaji stejny pocet radku.\n", __FILE__, __LINE__, matrix_size_m(mat), matrix_size_m(avarge), matrix_size_m(stdev));
    exit(1);
  }

  unsigned int i, j;
  double av, std;

  for (i = 0; i<matrix_size_m(mat); i++) {
    av = matrix_get(avarge, i, 0); // Prumerna hodnota I-teho priznaku
    std = matrix_get(stdev, i, 0); // Smerodatna odchylka I-teho priznaku

    // Cepstral Mean Normalization
    for (j = 0; j<matrix_size_n(mat); j++) {
      matrix_set(mat, i, j, (matrix_get(mat, i, j)-av)/std);
    }

  }

}

matrix *sp_lpc_autocor(matrix *S, unsigned int P)
{
  unsigned int n, j, k, p;
  double sum;
  matrix *autocor = matrix_alloc(P, matrix_size_n(S));

  for (j = 0; j < matrix_size_n(S); j++) {

    for (k = 0; k<P; k++) {
      sum = 0;

      for (n = 0; n<matrix_size_m(S)-k; n++) {

        sum += matrix_get(S, n, j)*matrix_get(S, n+k, j);

      }

      matrix_set(autocor, k, j, sum);
    }

  }
  
  return autocor;
}

matrix **sp_lpc_linsys(matrix *autocor)
{
  int i, j, N;
  N = matrix_size_m(autocor);
  matrix *A = matrix_alloc(matrix_size_m(autocor), matrix_size_m(autocor));
  matrix *B = matrix_alloc(matrix_size_m(autocor), 1);
  matrix **out = malloc(sizeof(matrix *)*2);
  out[0] = A;
  out[1] = B;

  for (i = 0; i<matrix_size_m(A); i++) {

    for (j = 0; j<i; j++) {
      matrix_set(A, i, j, matrix_get(autocor, i-j, 0));
    }

    for (j = i; j<matrix_size_n(A); j++) {
      matrix_set(A, i, j, matrix_get(autocor, j-i, 0));
    }

    matrix_set(B, i, 0, matrix_get(autocor, i, 0));
  }

  return out;
}

matrix *sp_lpc(matrix *s, unsigned int P)
{
  matrix *autocor = sp_lpc_autocor(s, P+1);
  matrix *R_LD = matrix_levinson_durbin(autocor, P+1);
  matrix *a = matrix_alloc(P, 1);
  unsigned int i;
  for (i = 0; i<matrix_size_m(a); i++) {
    matrix_set(a, i, 0, matrix_get(R_LD, i+1, P));
  }
  matrix_free(autocor);
  matrix_free(R_LD);
  return a;
}

matrix *sp_lpc_cepstrum(matrix *s, unsigned int P, unsigned int N)
{
  if (P>N) {
    fprintf(stderr, "%s: %i: ERROR: P (%i) musi byt vetsi nez N (%i).\n", __FILE__, __LINE__, P, N);
    exit(1);
  }

  matrix *lpc, *LPCC, *E, *column;
  unsigned int j, n, k;
  double sum;
  LPCC = matrix_alloc(N, matrix_size_n(s));
  E = sp_signal_log_energy(s);

  for (j = 0; j<matrix_size_n(s); j++) {

    column = matrix_submatrix(s, 0, matrix_size_m(s)-1, j, j);
    lpc = sp_lpc(column, P);
    matrix_free(column);
    matrix_set(LPCC, 0, j, matrix_get(E, 0, j));

    for (n = 1; n<=P-1; n++) {

      sum = 0;
      for (k = 1; k <= n-1; k++) {
        sum += k*matrix_get(LPCC, k, 0)*matrix_get(lpc, n-k, 0);
      }
      sum = sum/n;

      matrix_set(LPCC, n, j, -matrix_get(lpc, n, 0)-sum);

    }

    for (n = P; n<N; n++) {

      sum = 0;
      for (k = 1; k <= n-1; k++) {
        sum += k*matrix_get(LPCC, k, 0)*matrix_get(lpc, n-k, 0);
      }
      sum = sum/n;
      
    }

    matrix_free(lpc);

  }
  matrix_free(E);
  return LPCC;
}

