#include <cmath>
#include <cassert>
#include <iostream>
#include <limits> // for std::numeric_limits<>::epsilon()

#include <gsl/gsl_rng.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

#define SHOW(x, ...) std::cout << #x << " :: " << (x) << std::endl

double const USE_MATLAB_TOL = -1;

size_t const np = 250;     // number of training patterns
size_t const ni = 4;       // number of inputs
size_t const no = 2;       // number of outputs
size_t const nr = 10;      // number of rules

gsl_rng * grng;

// product of vector elements
double ap_gsl_prod(const gsl_vector *v){
  double prod = 1.0;
  for(size_t i = 0; i < v->size; ++i) prod *= gsl_vector_get(v, i);
  return prod;
}


// sum of vector elements
double ap_gsl_sum(const gsl_vector *v){
  double sum = 0.0;
  for(size_t i = 0; i < v->size; ++i) sum += gsl_vector_get(v, i);
  return sum;
}


gsl_matrix* ap_gsl_clone(gsl_matrix const * m){
  gsl_matrix * clone = gsl_matrix_alloc(m->size1, m->size2);
  gsl_matrix_memcpy(clone, m);
  return clone;
}


// fuzzy rule base membership values
gsl_matrix * memberships(gsl_matrix const * x, gsl_matrix const * c,
                         gsl_matrix const * s){
  gsl_matrix * mu = gsl_matrix_alloc(np*nr, ni);
  double xik = 0.0, cjk = 0.0, sjk = 0.0, z = 0.0;
  for(size_t i = 0; i < np; ++i){
   for(size_t j = 0; j < nr; ++j){
     for(size_t k = 0; k < ni; ++k){
        xik = gsl_matrix_get(x, i, k);
        cjk = gsl_matrix_get(c, j, k);
        sjk = gsl_matrix_get(s, j, k);
        z = (xik-cjk)/sjk; 
        gsl_matrix_set(mu, i*nr+j, k, exp(-0.5*z*z));
      }
    }
  }
  return mu;
}


void truth_helper(gsl_matrix *tau, const gsl_matrix *mu){
  // multiply along row
  for(size_t p = 0; p < np; ++p){
    for(size_t r = 0; r < nr; ++r){
      gsl_vector_const_view mu_row_view =
        gsl_matrix_const_row(mu, p*nr+r);
      double value = ap_gsl_prod(&mu_row_view.vector);
      gsl_matrix_set(tau, p, r, value);
    }
  }
}


gsl_matrix * truth(gsl_matrix const * x, gsl_matrix const * centers,
                   gsl_matrix const * sigmas){
  gsl_matrix * tau = gsl_matrix_alloc(np, nr);
  gsl_matrix * mu = memberships(x, centers, sigmas);
  truth_helper(tau, mu);
  gsl_matrix_free(mu);
  return tau;
}


void weights_helper(gsl_matrix *w, const gsl_matrix *tau){
  // divide each row by the sum of row elements
  gsl_matrix_memcpy(w, tau);
  for(size_t r = 0; r < w->size1; ++r){
    gsl_vector_view v = gsl_matrix_row(w, r);
    double scale = 1.0/ap_gsl_sum(&v.vector);
    gsl_vector_scale(&v.vector, scale);
  }
}


gsl_matrix * weights(gsl_matrix const * x, gsl_matrix const * centers,
                     gsl_matrix const * sigmas){
  gsl_matrix * tau = truth(x, centers, sigmas);
  gsl_matrix * w = gsl_matrix_alloc(tau->size1, tau->size2);
  weights_helper(w, tau);
  gsl_matrix_free(tau);
  return w;
}

void design_matrix_helper(gsl_matrix * d, const gsl_matrix * x,
                          const gsl_matrix * w){
  gsl_vector *z = gsl_vector_alloc(ni+1);
  for(size_t i = 0; i < x->size1; ++i){
    for(size_t j = 0; j < nr; ++j){
      gsl_vector_set(z, 0, 1.0);
      gsl_vector_view z_sub_vec_view = gsl_vector_subvector(z, 1, ni);
      gsl_vector_const_view xrow_view = gsl_matrix_const_row(x, i);
      gsl_vector_memcpy(&z_sub_vec_view.vector, &xrow_view.vector);
      gsl_vector_scale(z, gsl_matrix_get(w,i,j));
      gsl_vector_view d_sub_row_view = gsl_matrix_subrow(d, i, j*(ni+1), ni+1);
      gsl_vector_memcpy(&d_sub_row_view.vector, z);
    }
  }
  gsl_vector_free(z);
}


gsl_matrix * design_matrix(gsl_matrix const * x, gsl_matrix const * centers,
                           gsl_matrix const * sigmas){
  gsl_matrix * w = weights(x, centers, sigmas);
  gsl_matrix * d = gsl_matrix_alloc(np, (ni+1)*nr);
  design_matrix_helper(d, x, w);
  gsl_matrix_free(w);
  return d;
}
 

gsl_matrix * svd_solve_jacobi(gsl_matrix const * a, gsl_matrix const * b,
                              double tol = 0.0){
  // solve for X in AX=B by SVD decomposition method. 

  size_t const m = a->size1;
  size_t const n = a->size2;
  size_t const p = b->size2;

  gsl_matrix * x = gsl_matrix_alloc(n, p);

  gsl_matrix * U = gsl_matrix_alloc(m, n);
  gsl_matrix * V = gsl_matrix_alloc(n, n);
  gsl_vector * s = gsl_vector_alloc(n);

  gsl_matrix_memcpy(U, a);
  
  assert(GSL_SUCCESS == gsl_linalg_SV_decomp_jacobi(U, V, s));

  // std::cout << "jacobi singular values" << std::endl;
  // gsl_vector_fprintf(stdout, s, "%+22.18e");

  if(tol < 0){
    // default tolerance used in MATLAB pinv()
    tol = std::max(m,n) * gsl_vector_max(s) *
      std::numeric_limits<double>::epsilon();
    //SHOW(tol);
  }

  for(size_t i = 0; i < n; ++i){
    if(tol > gsl_vector_get(s, i)){
      gsl_vector_set(s, i, 0.0);
    }
  }

  for(size_t i = 0; i < p; ++i){
    gsl_vector_view xcol = gsl_matrix_column(x,i);
    gsl_vector_const_view bcol = gsl_matrix_const_column(b, i); 
    gsl_linalg_SV_solve(U, V, s, &bcol.vector, &xcol.vector);
  }
  
  gsl_matrix_free(U);
  gsl_matrix_free(V);
  gsl_vector_free(s);

  return x;
}



gsl_matrix * svd_solve(gsl_matrix const * a, gsl_matrix const * b,
                       double tol = 0.0){
  // solve for X in AX=B by SVD decomposition method. 

  size_t const m = a->size1;
  size_t const n = a->size2;
  size_t const p = b->size2;

  gsl_matrix * x = gsl_matrix_alloc(n, p);

  gsl_matrix * U = gsl_matrix_alloc(m, n);
  gsl_matrix * V = gsl_matrix_alloc(n, n);
  gsl_vector * s = gsl_vector_alloc(n);
  gsl_vector * w = gsl_vector_alloc(n);

  gsl_matrix_memcpy(U, a);
  
  assert(GSL_SUCCESS == gsl_linalg_SV_decomp(U, V, s, w));

  // std::cout << "singular values" << std::endl;
  // gsl_vector_fprintf(stdout, s, "%+22.18e");

  if(tol < 0){
    // default tolerance used in MATLAB pinv()
    tol = std::max(m,n) * gsl_vector_max(s) *
      std::numeric_limits<double>::epsilon();
  }

  for(size_t i = 0; i < n; ++i){
    if(tol > gsl_vector_get(s, i)){
      gsl_vector_set(s, i, 0.0);
    }
  }

  for(size_t i = 0; i < p; ++i){
    gsl_vector_view xcol = gsl_matrix_column(x,i);
    gsl_vector_const_view bcol = gsl_matrix_const_column(b, i); 
    gsl_linalg_SV_solve(U, V, s, &bcol.vector, &xcol.vector);
  }
  
  gsl_matrix_free(U);
  gsl_matrix_free(V);
  gsl_vector_free(s);
  gsl_vector_free(w);

  return x;
}


double one_norm(gsl_matrix const * m){
  double sum = 0.0;
  for(size_t i = 0; i < m->size1; ++i){ 
    for(size_t j = 0; j < m->size2; ++j){
      sum += fabs(gsl_matrix_get(m, i, j));
    }
  }
  return sum;
}


double reconstruction_error(gsl_matrix const * cons, gsl_matrix const * d,
                         gsl_matrix const * y){
  // form product of d and estimated cons and subtract from y
  gsl_matrix * yy = gsl_matrix_alloc(y->size1, y->size2);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, d, cons, 0.0, yy);
  gsl_matrix_sub(yy, y);
  double ans = one_norm(yy);
  gsl_matrix_free(yy);
  return ans;
}


int main(){
  grng = gsl_rng_alloc(gsl_rng_default);
  gsl_rng_set(grng, 123UL); // <-- FIXED SEED

  // randomly generate x between [-10,+10]
  gsl_matrix * x = gsl_matrix_alloc(np, ni);
  for(size_t i = 0; i < np; ++i){ 
    for(size_t j = 0; j < ni; ++j){
      gsl_matrix_set(x, i, j, -10.0 + 20.0*gsl_rng_uniform(grng));
    }
  }

  // gaussian membership function centers
  gsl_matrix * centers = gsl_matrix_alloc(nr, ni);
  // each column entry is equi-spaced from -10 to +10
  for(std::size_t i = 0; i < nr; ++i){ 
    for(std::size_t j = 0; j < ni; ++j){
      gsl_matrix_set(centers, i, j, -10.0 + i*20.0/(nr-1));
    }
  }

  // gaussian membership function sigmas
  gsl_matrix * sigmas = gsl_matrix_alloc(nr, ni);
  gsl_matrix_set_all(sigmas, 10.0);

  // design matrix 
  gsl_matrix * d = design_matrix(x, centers, sigmas);
  SHOW(d->size1);
  SHOW(d->size2);
  FILE * dfile = fopen("d.txt", "w");
  gsl_matrix_fprintf(dfile, d, "%+22.18e");
  fclose(dfile);


  // fuzzy consequent parameters 
  gsl_matrix * cons = gsl_matrix_alloc((ni+1)*nr, no);
  for(size_t i = 0; i < cons->size1; ++i){ 
    for(size_t j = 0; j < cons->size2; ++j){
      gsl_matrix_set(cons, i, j, gsl_rng_uniform(grng));
    }
  }
  SHOW(one_norm(cons)); // base norm value
  SHOW(cons->size1);
  SHOW(cons->size2);
  FILE * consfile = fopen("cons.txt", "w");
  gsl_matrix_fprintf(consfile, cons, "%+22.18e");
  fclose(consfile);
  
  std::cout << std::endl;

  // form the product y = d*cons
  gsl_matrix * y = gsl_matrix_alloc(np, no);
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, d, cons, 0.0, y);
  SHOW(y->size1);
  SHOW(y->size2);
  FILE * yfile = fopen("y.txt", "w");
  gsl_matrix_fprintf(yfile, y, "%+22.18e");
  fclose(yfile);
  

  // estimate cons using pseudo-inverse 
  gsl_matrix * cons_hat = svd_solve(d, y);
  SHOW(one_norm(cons_hat)); // reasonable but greater than base value
  SHOW(reconstruction_error(cons_hat, d, y));

  std::cout << std::endl;

  // estimate cons using pseudo-inverse using jacobi method
  gsl_matrix * cons_hat_jacobi = svd_solve_jacobi(d, y);
  SHOW(one_norm(cons_hat_jacobi)); // huge ???
  SHOW(reconstruction_error(cons_hat_jacobi, d, y)); 

  std::cout << "\nlet's use tolerance to discard small singular values" << std::endl;
  std::cout << std::endl;
  
  gsl_matrix_free(cons_hat);
  cons_hat = svd_solve(d, y, USE_MATLAB_TOL);
  SHOW(one_norm(cons_hat)); // smaller than base value GOOD
  SHOW(reconstruction_error(cons_hat, d, y));

  std::cout << std::endl;

  gsl_matrix_free(cons_hat_jacobi);
  cons_hat_jacobi = svd_solve_jacobi(d, y, USE_MATLAB_TOL);
  SHOW(one_norm(cons_hat_jacobi)); // still huge ???
  SHOW(reconstruction_error(cons_hat_jacobi, d, y));

  std::cout << "increase tolerance value for the jacobi method" << std::endl;

  gsl_matrix_free(cons_hat_jacobi);
  cons_hat_jacobi = svd_solve_jacobi(d, y, 1e-6);
  SHOW(one_norm(cons_hat_jacobi)); // still huge ???
  SHOW(reconstruction_error(cons_hat_jacobi, d, y));

  std::cout << std::endl;

  gsl_matrix_free(cons_hat_jacobi);
  cons_hat_jacobi = svd_solve_jacobi(d, y, 1e-3);
  SHOW(one_norm(cons_hat_jacobi)); // still huge ???
  SHOW(reconstruction_error(cons_hat_jacobi, d, y)); 

                                                              
  gsl_rng_free(grng);
  gsl_matrix_free(d);
  gsl_matrix_free(x);
  gsl_matrix_free(y);
  gsl_matrix_free(centers);
  gsl_matrix_free(sigmas);
  gsl_matrix_free(cons);
  gsl_matrix_free(cons_hat);
  gsl_matrix_free(cons_hat_jacobi);
}