#include "somp.h"


matrix *somp_max_base(matrix *S)
{
  unsigned int i, j, k, idmax;
  matrix *base, *norm_base, *ck, *R;
  R = matrix_clone(S);
  ck = matrix_alloc(1, matrix_size_n(R));
  double product, sum_ck, maximum, sum;

  // Normalizace sloupcu
  for (i = 0; i<matrix_size_n(R); i++) {
    sum = 0;

    for (j = 0; j<matrix_size_m(R); j++) {
      sum += pow(matrix_get(R, j, i), 2);
    }

    sum = sqrt(sum);

    if (sum > 0) {

      for (j = 0; j<matrix_size_m(R); j++) {
        matrix_set(R, j, i, matrix_get(R, j, i)/sum);
      }

    }

  }

  // Kazdy sloupes s resudialni matice bude vyzkousen jako baze.
  for (k = 0; k<matrix_size_n(R); k++) {

    // Vytvoreni K-te baze
    base = matrix_submatrix(R, 0, matrix_size_m(R)-1, k, k);
    
    // Soucet vsech Ck pro vsechny vyjma K-teho sloupce residualni matice .
    sum_ck = 0;

    for (j = 0; j<k; j++) {
      product = 0;
      for (i = 0; i<matrix_size_m(R); i++) {
        product += matrix_get(R, i, j)*matrix_get(base, i, 0);
      }
      sum_ck += somp_abs(product);
    }

    for (j = k+1; j<matrix_size_n(R); j++) {
      product = 0;
      for (i = 0; i<matrix_size_m(R); i++) {
        product += matrix_get(R, i, j)*matrix_get(base, i, 0);
      }
      sum_ck += somp_abs(product);
    }

    // Nastaveni sumy Ck.
    matrix_set(ck, 0, k, sum_ck);
    matrix_free(base);

  }

  // Nalezeni baze, ktera je zastoupena nejvice (nejvetsi ck).
  maximum = matrix_get(ck, 0, 0);
  idmax = 0;
  for (k = 1; k<matrix_size_n(ck); k++) {
    if (matrix_get(ck, 0, k)>maximum) {
      idmax = k;
      maximum = matrix_get(ck, 0, k);
    }
  }

  
  // Vytvoreni normovane baze.
  norm_base = matrix_submatrix(R, 0, matrix_size_m(R)-1, idmax, idmax);
  matrix_free(R);
  // Vraceni normovane baze
  return norm_base;

}


matrix *somp_base_projection(matrix *S, matrix *basis)
{
  #ifdef SOMP_DEBUG
  if (matrix_size_m(S)!=matrix_size_m(basis)) {
    printf("%s:%i: error: Baze (= %i x %i) ma jiny pocet radku nez matice S (= %i x %i).\n", __FILE__, __LINE__, matrix_size_m(S), matrix_size_n(S), matrix_size_m(basis), matrix_size_n(basis));
    exit(1);
  }
  #endif
  unsigned int i, j, k;
  double product;
  matrix *projection = matrix_alloc(matrix_size_n(basis), matrix_size_n(S));

  // Pro vsechny sloupce se vypocitaji vsechny bazove souradnice.
  for (i = 0; i<matrix_size_n(S); i++) {

    // Vypocitani souradnic v jednotlivych bazich.
    for (j = 0; j<matrix_size_n(basis); j++) {

      // Vektorovy soucin.
      product = 0;
      for (k = 0; k<matrix_size_m(S); k++) {
        product += matrix_get(S, k, i)*matrix_get(basis, k, j);
      }
      // Ulozeni souradnice.
      matrix_set(projection, j, i, product);
    }

  }

  return projection;

}


matrix *somp_ck_projection(matrix *ck, matrix *basis)
{
  #ifdef SOMP_DEBUG
  if (matrix_size_m(ck)!=matrix_size_n(basis)) {
    printf("%s:%i: error: Baze (= %i x %i) ma jiny pocet sloupcu nez matice bazovych souradnic Ck (= %i x %i).\n", __FILE__, __LINE__, matrix_size_m(ck), matrix_size_n(ck), matrix_size_m(basis), matrix_size_n(basis));
    exit(1);
  }
  #endif
  unsigned int i, j, k;
  matrix *projection = matrix_alloc(matrix_size_m(basis), matrix_size_n(ck));
  matrix_zeros(projection);
  
  // Vypocitani aproximace podle jednotlivych bazovych souradnic Ck.
  for (i = 0; i<matrix_size_n(projection); i++) {
    
    // Do aproximace se zahrnou vsechny bazove souradnice Ck.
    for (j = 0; j<matrix_size_m(ck); j++) {

      // Vektorovy soucin.
      for (k = 0; k<matrix_size_m(basis); k++) {
        matrix_set(projection, k, i, matrix_get(projection, k, i)+matrix_get(basis, k, j)*matrix_get(ck, j, i));
      }

    }

  }

  return projection;

}


matrix **somp_algorithm (matrix *S, unsigned int T)
{
  #ifdef SOMP_DEBUG
  if (matrix_not_null(S)) {
    printf("%s:%i: error: Nelze vytvorit baze z nulove matice.\n", __FILE__, __LINE__);
    exit(1);
  }
  #endif

  unsigned int t;

  matrix *R0 = matrix_clone(S);
  matrix *Rt, *RtSwap, *AT, *ATSwap, *At, *Pt, *basis, *base;
  list *basis_list = list_alloc();
  matrix **OUT;

  Rt = R0;
  AT = matrix_alloc(matrix_size_m(S), matrix_size_n(S));
  matrix_zeros(AT);

  for (t = 0; t<T-1; t++) {
    // Nalezeni nejlepsi baze.
    base = somp_max_base(Rt);
    list_add_last(basis_list, element_alloc_matrix(base));  // Pridani baze do seznamu
    // Projekce baze.
    Pt = somp_base_projection(Rt, base);
    // Aproximace pomoci bazove funkce base.
    At = somp_ck_projection(Pt, base);
    // Pricteni At do vystupni aproximace.
    ATSwap = matrix_adition(AT, At);
    matrix_free(AT);
    AT = ATSwap;
    // Odecteni baze z residualni matice Rt.
    RtSwap = matrix_substraction(Rt, At);
    // Uvolneni pomocnych promenych z pameti.
    matrix_free(Rt);
    matrix_free(At);
    matrix_free(Pt);
    // Swap resudialni matice pro pro dalsi cyklus.
    Rt = RtSwap;
  }
  
  // Posledni cyklus.
  base = somp_max_base(Rt);
  list_add_last(basis_list, element_alloc_matrix(base));  // Pridani baze do seznamu
  Pt = somp_base_projection(Rt, base);
  At = somp_ck_projection(Pt, base);
  matrix_free(Pt);
  ATSwap = matrix_adition(AT, At);
  matrix_free(AT);
  AT = ATSwap;
  RtSwap = matrix_substraction(Rt, At);
  matrix_free(Rt);
  Rt = RtSwap;
  
  // Ulozeni matice bazi
  basis = matrix_cat_list(basis_list, MATRIX_CAT_HORIZONTALLY);
  
  // Ulozeni vystupni struktury {basis, At, RT}
  OUT = malloc(sizeof(matrix *) * 3);
  OUT[0] = basis;
  OUT[1] = AT;
  OUT[2] = Rt;
  
  return OUT;
}


double somp_abs (double x)
{
  if (x < 0) {
    return -x;
  }
  return x;
}