#include "dtw.h"


double dtw (matrix * A, matrix * B)
{

  double D[matrix_size_n(A)][matrix_size_n(B)];
  double G[matrix_size_n(A)][matrix_size_n(B)];

  // Overeni jestli maji matice stejny pocet radku
  if(matrix_size_m(A)!=matrix_size_m(B)) {
    printf("%s:%i: Matice A a B musi mit stejny pocet radku", __FILE__, __LINE__);
    return 0;
  }

  int m, n_a, n_b;
  // Zjisteni rozmeru matic
  m = matrix_size_m(A);
  n_a = matrix_size_n(A);
  n_b = matrix_size_n(B);
  // Vytvoreni matice vzajmenych vzdalenosti ramcu (sloupcu) D

  double distance;
  long unsigned int i, j, k;
  // Vypocitani matice D pomoci euklidovy vzdalenosti 2 sloupcu matic (ramcu)
  for(i = 0; i<n_a; i++) {
    for(j = 0; j<n_b; j++) {
      distance = 0;
      for(k = 0; k<m; k++) {
        distance += pow(matrix_get(A, k, i)-matrix_get(B, k, j), 2);
      }
      distance = sqrt(distance);
      D[i][j] = distance;
      //matrix_set(D, i, j, distance);
    }
  }

  // Inicializace pocatecni hodnoty matice G[0][0] = 0
  G[0][0] = D[0][0];
  // Vypocet prvniho sloupce matice G
  for(i = 1; i<n_a; i++) {
    G[i][0] = D[i][0]+G[i-1][0];
  }
  // Vypocet prvniho radku matice G
  for(i = 1; i<n_b; i++) {
    G[0][i] = D[0][i]+G[0][i-1];
  }

  // Vzdalenost I-teho sloupce matice A k J-tem sloupci matice B z matice D.
  // Hodnoty matic G podle 3 ruznych cest
  double dist_ij, value_up, value_left, value_up_left;
  // Vypocet zbylych hodnot matice G bude probihat po radcich
  for(i = 1; i<n_a; i++) {
    for(j = 1; j<n_b; j++) {
      dist_ij = D[i][j];
      // Vypocet kumulovanyc vzdalenosti podle 3 moznych cest
      value_up = G[i-1][j]+dist_ij;
      value_left = G[i][j-1]+dist_ij;
      value_up_left = G[i-1][j-1]+dist_ij*2;
      if((value_up<=value_left)&(value_up<=value_up_left)) {
        G[i][j] = value_up;
      } else {
        if((value_left<=value_up)&(value_left<=value_up_left)) {
          G[i][j] = value_left;
        } else {
          G[i][j] = value_up_left;
        }
      }
    }
  }
  double L1 = G[n_a-1][n_b-1];
  //printf("d = %f\n", L1);
  return L1/((double)(n_a+n_b));
  //return L1;
}


matrix *dtw2 (matrix * A, matrix * B)
{
  double D[DTW_FAST_N][DTW_FAST_M];

  // Overeni jestli maji matice stejny pocet radku
  if(matrix_size_m(A)!=matrix_size_m(B)) {
    printf("%s:%i: Matice A a B musi mit stejny pocet radku (%i , %i)\n", __FILE__, __LINE__, matrix_size_m(A), matrix_size_m(B));
    return 0;
  }

  int m, n_a, n_b;
  // Zjisteni rozmeru matic
  m = matrix_size_m(A);
  n_a = matrix_size_n(A);
  n_b = matrix_size_n(B);

  // Vypocet matice vzajemnych vzdalenosti
  double distance;
  long unsigned int i, j, k;
  // Vypocitani matice D pomoci euklidovy vzdalenosti 2 sloupcu matic (ramcu)
  for(i = 0; i<n_a; i++) {
    for(j = 0; j<n_b; j++) {
      distance = 0;
      for(k = 0; k<m; k++) {
        distance += pow(matrix_get(A, k, i)-matrix_get(B, k, j), 2);
      }
      distance = sqrt(distance);
      D[i][j] = distance;
      //matrix_set(D, i, j, distance);
    }
  }

  // Alokace matice kumulovanych vzdalenosti G
  matrix * G = matrix_alloc(n_a, n_b);
  // Inicializace pocatecni hodnoty matice G[0][0] = 0
  matrix_set(G, 0, 0, D[0][0]);
  // Vypocet prvniho sloupce matice G
  for(i = 1; i<n_a; i++) {
    matrix_set(G, i, 0, D[i][0]+matrix_get(G, i-1, 0));
  }
  // Vypocet prvniho radku matice G
  for(i = 1; i<n_b; i++) {
    matrix_set(G, 0, i, D[0][i]+matrix_get(G, 0, i-1));
  }

  // Vzdalenost I-teho sloupce matice A k J-tem sloupci matice B z matice D.
  // Hodnoty matic G podle 3 ruznych cest
  double dist_ij, value_up, value_left, value_up_left;
  // Vypocet zbylych hodnot matice G bude probihat po radcich
  for(i = 1; i<n_a; i++) {
    for(j = 1; j<n_b; j++) {
      dist_ij = D[i][j];
      // Vypocet kumulovanyc vzdalenosti podle 3 moznych cest
      value_up = matrix_get(G, i-1, j)+dist_ij*1;
      value_left = matrix_get(G, i, j-1)+dist_ij*1;
      value_up_left = matrix_get(G, i-1, j-1)+dist_ij*2;
      if((value_up<=value_left)&(value_up<=value_up_left)) {
        matrix_set(G, i, j, value_up);
      } else {
        if((value_left<=value_up)&(value_left<=value_up_left)) {
          matrix_set(G, i, j, value_left);
        } else {
          matrix_set(G, i, j, value_up_left);
        }
      }
    }
  }

  for (i = 0; i<n_a; i++) {
    for (j = 0; j<n_b; j++) {
      matrix_set(G, i, j, matrix_get(G, i, j)/((double)(i+j+1)));
    }
  }
  
  //puts("G");
  //matrix_print(G);
  //puts("S");
  //matrix_print(S);
  //printf("d = %f\n", L1);
  return G;
}


matrix *dtw_way (matrix *G)
{
  unsigned int i, j;
  double up_left, up, left, D;
  i = matrix_size_m(G)-1;
  j = matrix_size_n(G)-1;
  list *K = list_alloc();
  D = matrix_get(G, i, j)+1;

  while ((i!=0)|(j!=0)) {

    if ((i>0)&(j>0)) {
      up_left = matrix_get(G, i-1, j-1);
    } else {
      up_left = D;
    }

    if (i>0) {
      up = matrix_get(G, i-1, j);
    } else {
      up = D;
    }

    if (j>0) {
      left = matrix_get(G, i, j-1);
    } else {
      left = D;
    }
    
    if((up<=left)&(up<=up_left)) {
      dtw_add_way(K, --i, j);
    } else {
      if((left<=up)&(left<=up_left)) {
        dtw_add_way(K, i, --j);
      } else {
        dtw_add_way(K, --i, --j);
      }
    }

  }

  matrix *out = matrix_cat_list(K, MATRIX_CAT_VERTICALLY);
  list_free(K);
  return out;
}

void dtw_add_way (list *L, double k, double l)
{
  matrix *out = matrix_alloc(1, 2);
  matrix_set(out, 0, 0, k);
  matrix_set(out, 0, 1, l);
  list_add_first(L, element_alloc_matrix(out));
}
