#include <ls_system.h>
#include <lighting_models.h>
#include <nl_system.h>
#include <sve_minn.h>
#include <stdlib.h>
#include <rmxn.h>
#include <rmxn_extra.h>

double non_linear_compute_S_star(
    nl_model_t* nl,
    double** X,
    double* F,
    int n,
    double* parameters,
    void* l_data_original,
    int nSteps
){
  void* l_data = nl->unpack_parameters(parameters,l_data_original);
  ls_model_t* lm = create_ls_lighting_model(nl->type);
  lm->write_param(stderr,l_data_original);
  double* weights = (double*)malloc(sizeof(double)*n);
  int i;
  for(i = 0; i < n; i++){
    weights[i] = 1;
  }
  lm->weights = weights;
  fitModelToFunction(X,F, n,lm,l_data,nSteps);
    
  /*Now we have to compute the square error*/
  double sum = 0;
  double sumw = 10e-5;
  for(i = 0; i < n; i++){
    double f = lm->evaluate(X[i],l_data);
    sum+= (F[i] - f)*(F[i] - f)*weights[i];
    sumw+= weights[i];
  }
  
  double q_error = sum/sumw;
  //free(weights);
  lm->release_data(l_data);
  if(lm->weights != NULL) free(lm->weights);
  if(lm->wpos != NULL) free(lm->wpos);
  return q_error;
  
}

void * non_linear_approximate(
  nl_model_t* nl,
  void * l_data_original,
  double* deviations,
  double** X,
  double* F,
  int n,
  int LSSteps,
  int NLSteps
  ){
  
  void* l_data = nl->copy_data(l_data_original);
  int numIters = 0;
  double epsilon = 10e-10;  
  double diff;
  
  do{
      void* previous_ldata = nl->copy_data(l_data);
      int n_params = nl->get_num_packed_params(l_data);

      /*Symplex operations */
      //The array {simplex_array} is interpreted as a matrix with {n+1} rows and {n} columns
      fprintf(stderr,"-----------------------------------------------\n");
      fprintf(stderr,"LSSteps: %d NLSteps: %d\n",LSSteps,NLSteps);
      nl->write_param(stderr,l_data);
      fprintf(stderr,"-----------------------------------------------\n");
      int n_simplex_rows = n_params+1;
      int n_simplex_cols = n_params;
      int n_simplex_elems = n_simplex_cols*n_simplex_rows;
      double* simplex_array_initial = (double*) malloc(sizeof(double)*n_simplex_elems);
      rmxn_regular_simplex(n_params, simplex_array_initial);
      double simplex_radius = rmxn_regular_simplex_radius(n_params);
      int i;
      for(i = 0; i < n_simplex_elems; i++){
	simplex_array_initial[i] = simplex_array_initial[i]/simplex_radius;
      }
      double* simplex_array = (double*) malloc(sizeof(double)*n_simplex_elems);
      rmxn_spin_rows(n_simplex_rows,n_simplex_cols, simplex_array_initial, simplex_array);
      free(simplex_array_initial);
      
      double* fi = (double*)malloc(sizeof(double)*n_simplex_rows);
      double** xi = (double**)malloc(sizeof(double*)*n_simplex_rows);
      double* parameters = nl->pack_parameters(l_data);
      for(i = 0; i < n_simplex_rows; i++){
	int j;
	xi[i] = (double*)malloc(sizeof(double)*n_params);
	for(j = 0; j < n_simplex_cols; j++){
	  int ind = i*n_simplex_rows + j;
	  double zij = simplex_array[ind];
	  if(deviations[j] == 0) zij = 0;
	  xi[i][j] = parameters[j] + zij*deviations[j]; 
	}
	fprintf(stderr,"++++++++++++++++++++++++++++++++++++++++++\n");
	for(j = 0; j < n_simplex_cols; j++){
	  fprintf(stderr,"%9.6lf ",xi[i][j]);
	}
	fprintf(stderr,"\n");
	fi[i] = non_linear_compute_S_star(nl,X,F,n,xi[i],l_data_original,LSSteps);
	fprintf(stderr,"++++++++++++++++++++++++++++++++++++++++++\n");
      }
      /*Get the baricenter*/
      double baricenter[n_simplex_rows];
      sve_minn_step(n_params, fi,baricenter);
      double optimal_params[n_params];
      for(i = 0 ; i < n_params ; i++){
	optimal_params[i] = 0;
	int j;
	for(j = 0; j < n_simplex_rows; j++){
	  optimal_params[i] += baricenter[j]*xi[j][i];
	}
      }
      
      void* new_l_data = nl->unpack_parameters(optimal_params,l_data_original);
      nl->release_data(l_data);
      l_data = new_l_data;
      diff = nl->compare(previous_ldata,l_data);
      numIters++;
            
      /*Cleaning*/
      nl->release_data(previous_ldata);
      free(fi);
      free(parameters);
      free(simplex_array);
      for(i = 0; i < n_simplex_rows; i++){
	free(xi[i]);
      }
      free(xi);
      
  }while( (numIters < NLSteps) && (diff > epsilon) );
    
  return l_data;
  
}