/* cdf/inverse_normal.c
 *
 * Copyright (C) 2002 Przemyslaw Sliwa and Jason H. Stover.
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307, USA.
 */

/*
 * Computes the inverse normal cumulative distribution function
 * according to the algorithm shown in
 *
 *      Wichura, M.J. (1988).
 *      Algorithm AS 241: The Percentage Points of the Normal Distribution.
 *      Applied Statistics, 37, 477-484.
 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

#include "rat_eval.h"

static double
small (double q)
{
  const double a[8] = { 3.387132872796366608, 133.14166789178437745,
    1971.5909503065514427, 13731.693765509461125,
    45921.953931549871457, 67265.770927008700853,
    33430.575583588128105, 2509.0809287301226727
  };

  const double b[8] = { 1.0, 42.313330701600911252,
    687.1870074920579083, 5394.1960214247511077,
    21213.794301586595867, 39307.89580009271061,
    28729.085735721942674, 5226.495278852854561
  };

  double r = 0.180625 - q * q;

  double x = q * rat_eval (a, 8, b, 8, r);

  return x;
}

static double
intermediate (double r)
{
  const double a[] = { 1.42343711074968357734, 4.6303378461565452959,
    5.7694972214606914055, 3.64784832476320460504,
    1.27045825245236838258, 0.24178072517745061177,
    0.0227238449892691845833, 7.7454501427834140764e-4
  };

  const double b[] = { 1.0, 2.05319162663775882187,
    1.6763848301838038494, 0.68976733498510000455,
    0.14810397642748007459, 0.0151986665636164571966,
    5.475938084995344946e-4, 1.05075007164441684324e-9
  };

  double x = rat_eval (a, 8, b, 8, (r - 1.6));

  return x;
}

static double
tail (double r)
{
  const double a[] = { 6.6579046435011037772, 5.4637849111641143699,
    1.7848265399172913358, 0.29656057182850489123,
    0.026532189526576123093, 0.0012426609473880784386,
    2.71155556874348757815e-5, 2.01033439929228813265e-7
  };

  const double b[] = { 1.0, 0.59983220655588793769,
    0.13692988092273580531, 0.0148753612908506148525,
    7.868691311456132591e-4, 1.8463183175100546818e-5,
    1.4215117583164458887e-7, 2.04426310338993978564e-15
  };

  double x = rat_eval (a, 8, b, 8, (r - 5.0));

  return x;
}

double
gsl_cdf_ugaussian_Pinv (const double P)
{
  double r, x, pp;

  double dP = P - 0.5;

  if (P == 1.0)
    {
      return GSL_POSINF;
    }
  else if (P == 0.0)
    {
      return GSL_NEGINF;
    }

  if (fabs (dP) <= 0.425)
    {
      x = small (dP);

      return x;
    }

  pp = (P < 0.5) ? P : 1.0 - P;

  r = sqrt (-log (pp));

  if (r <= 5.0)
    {
      x = intermediate (r);
    }
  else
    {
      x = tail (r);
    }

  if (P < 0.5)
    {
      return -x;
    }
  else
    {
      return x;
    }

}

double
gsl_cdf_ugaussian_Qinv (const double Q)
{
  double r, x, pp;

  double dQ = Q - 0.5;

  if (Q == 1.0)
    {
      return GSL_NEGINF;
    }
  else if (Q == 0.0)
    {
      return GSL_POSINF;
    }

  if (fabs (dQ) <= 0.425)
    {
      x = small (dQ);

      return -x;
    }

  pp = (Q < 0.5) ? Q : 1.0 - Q;

  r = sqrt (-log (pp));

  if (r <= 5.0)
    {
      x = intermediate (r);
    }
  else
    {
      x = tail (r);
    }

  if (Q < 0.5)
    {
      return x;
    }
  else
    {
      return -x;
    }
}

double
gsl_cdf_gaussian_Pinv (const double P, const double sigma)
{
  return sigma * gsl_cdf_ugaussian_Pinv (P);
}

double
gsl_cdf_gaussian_Qinv (const double Q, const double sigma)
{
  return sigma * gsl_cdf_ugaussian_Qinv (Q);
}


/* statistics/gsl_statistics_ulong.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Jim Davies, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_STATISTICS_ULONG_H__
#define __GSL_STATISTICS_ULONG_H__

#include <stddef.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

GSL_EXPORT double gsl_stats_ulong_mean (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_variance (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_sd (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_variance_with_fixed_mean (const unsigned long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ulong_sd_with_fixed_mean (const unsigned long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ulong_absdev (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_skew (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_kurtosis (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_ulong_lag1_autocorrelation (const unsigned long data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_ulong_covariance (const unsigned long data1[], const size_t stride1,const unsigned long data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_ulong_variance_m (const unsigned long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ulong_sd_m (const unsigned long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ulong_absdev_m (const unsigned long data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_ulong_skew_m_sd (const unsigned long data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_ulong_kurtosis_m_sd (const unsigned long data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_ulong_lag1_autocorrelation_m (const unsigned long data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_ulong_covariance_m (const unsigned long data1[], const size_t stride1,const unsigned long data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

GSL_EXPORT double gsl_stats_ulong_pvariance (const unsigned long data1[], const size_t stride1, const size_t n1, const unsigned long data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_ulong_ttest (const unsigned long data1[], const size_t stride1, const size_t n1, const unsigned long data2[], const size_t stride2, const size_t n2);

GSL_EXPORT unsigned long gsl_stats_ulong_max (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT unsigned long gsl_stats_ulong_min (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_ulong_minmax (unsigned long * min, unsigned long * max, const unsigned long data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_ulong_max_index (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_ulong_min_index (const unsigned long data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_ulong_minmax_index (size_t * min_index, size_t * max_index, const unsigned long data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_ulong_median_from_sorted_data (const unsigned long sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_ulong_quantile_from_sorted_data (const unsigned long sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_ULONG_H__ */


//
// Created by L.Jonathan Feldstein
//

#ifndef CIMPLE_SYSTEM_H
#define CIMPLE_SYSTEM_H

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>
#include "cimple_polytope_library.h"

// EXAMPLE ALLOCATION FOR STRUCT
// Try to allocate structure.
// struct struct_name *return_**** = malloc(sizeof(struct));
// Try to allocate data, free structure if fail.
// return_****->**** = malloc(***);
// Free structure if fail, return NULL
// if (return_****->**** == NULL){
//    return NULL;
//}

/**
 * Functions that help perform several mathematical operations
 * on the way to calculating the input.
 */
typedef struct auxiliary_matrices{

    gsl_matrix * A_K;
    gsl_matrix * A_N;
    gsl_matrix * Ct;

    /*
    B_diag = B
    E_diag = E
    for i in range(N-1):
    B_diag = _block_diag2(B_diag, B)
    E_diag = _block_diag2(E_diag, E)
     */
    gsl_matrix * B_diag;
    gsl_matrix * E_diag;

    gsl_matrix * L_default;
    gsl_matrix * E_default;

    //LU = ([Uset->size1*N, n+N*m])

    gsl_matrix * LU;

    //MU = tile(Uset->G, N)

    gsl_vector * MU;

    //GU = ([Uset->size1*N, p*N])

    gsl_matrix * GU;

    //K_hat = tile(K, N)

    gsl_vector * K_hat;

}auxiliary_matrices;

/**
 * Cost function to be minimized:
 *
 * |Rx|_{ord} + |Qu|_{ord} + r'x + mid_weight * |xc - x(N)|_{ord}
 *
 *      R: state cost matrix for:
 *      x = [x(1)' x(2)' .. x(N)']'
 *      If empty, zero matrix is used.
 *      dim[N*n x N*n] (n = x.size)
 *
 *      r: cost vector for state trajectory:
 *      x = [x(1)' x(2)' .. x(N)']'
 *      r: size (N*xdim x 1)
 *
 *      Q: input cost matrix for control input::
 *      u = [u(0)' u(1)' .. u(N-1)']'
 *      If empty, identity matrix is used.
 *      dim[N*m x N*m] m = u.size
 *
 *      distance_error_weight: cost weight for |x(N)-xc|_{ord}
 *
 *      ord: norm used for cost function: ord in {1, 2, INFINITY}
*/
typedef struct cost_function{

    gsl_matrix *R;
    gsl_matrix *Q;
    gsl_vector *r;
    double distance_error_weight;

}cost_function;

/**
 * Abstraction of the system
 * contains all the polytopes
 *
 * number_of_region = total number of possible states that can be reached
 * regions: array of region, each containing one or multiple polytopes
 *
 * closed_loop: if 'true' only first input of next N calculated inputs is applied.
 *              All others are discarded.
 * conservative: if 'true' x(0)...x(N-1) are in starting polytope, x(N) is in final polytope
 *               if false x(1)...x(N-1) can be anywhere
 *
 * ord: norm that is used for minimizing cost function in {1, 2, INFINITY}
 *
 * time_horizon: number of next steps that are taken into account in calculation of the path
 */
typedef struct discrete_dynamics{

    int abstract_states_count;
    int number_of_original_regions;
    abstract_state **abstract_states_set;
    abstract_state **original_regions;
    int closed_loop;
    int conservative;
    int ord;
    size_t time_horizon;

}discrete_dynamics;

/**
 * Dynamics of the system
 *
 * x(t+1) = Ax(t) + Bu(t) + E
 *
 * A system dynamics
 * B input dynamics
 * E disturbance
 * U_set, W_set constraints on states and inputs
 * aux_matrices: auxiliary matrices to fasten calculation of next input
 */
typedef struct system_dynamics{

    gsl_matrix *A;
    gsl_matrix *B;
    gsl_matrix *E;
    gsl_vector *K;
    polytope *W_set;
    polytope *U_set;
    auxiliary_matrices *aux_matrices;

}system_dynamics;

/**
 * State plant is in at the beginning of the calculation
 *
 * current_abs_state starting region
 */
typedef struct current_state{

    int current_abs_state;
    gsl_vector *x;

} current_state;

/**
 * @brief "Constructor" Dynamically allocates the memory all auxiliary matrices need
 * @param n s_dyn.A.size2
 * @param p s_dyn.E.size2
 * @param m s_dyn.B.size2
 * @param u_set_size Uset.size1
 * @param N time horizon
 * @return
 */
struct auxiliary_matrices *aux_matrices_alloc(size_t n,
                                              size_t p,
                                              size_t m,
                                              size_t u_set_size,
                                              size_t N);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the auxiliary matrices
 * @param aux_matrices
 */
void aux_matrices_free(auxiliary_matrices *aux_matrices);

/**
 * @brief "Constructor" Dynamically allocates the memory the complete system dynamics need
 * @param n
 * @param m
 * @param p
 * @param w_set_size
 * @param u_set_size
 * @param N
 * @return
 */
struct system_dynamics *system_dynamics_alloc (size_t n,
                                               size_t m,
                                               size_t p,
                                               size_t w_set_size,
                                               size_t u_set_size,
                                               size_t N);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the system dynamics
 * @param system_dynamics
 */
void system_dynamics_free(system_dynamics * system_dynamics);

/**
 * @brief "Constructor" Dynamically allocates the memory for the state of the plant
 * @param n
 * @param abstract_state
 * @return
 */
struct current_state *state_alloc(size_t n, int abstract_state);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the state of the plant
 * @param state
 */
void state_free(current_state *state);

/**
 * @brief "Constructor" Dynamically allocates the memory for cost function matrices
 * @param n
 * @param m
 * @param N
 * @param distance_error_weight
 * @return
 */
struct cost_function *cost_function_alloc(size_t n,size_t m, size_t N, double distance_error_weight);

/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the cost function matrices
 * @param cost_function
 */
void cost_function_free(cost_function *cost_function);

/**
 * @brief "Constructor" Dynamically allocates the memory for the discrete abstraction of the system
 * @param polytopes_in_region
 * @param polytope_sizes
 * @param hull_sizes
 * @param orig_polytopes_in_region
 * @param orig_polytope_sizes
 * @param orig_hull_sizes
 * @param n
 * @param abstract_states_count
 * @param number_of_original_regions
 * @param closed_loop
 * @param conservative
 * @param ord
 * @param time_horizon
 * @return
 */
struct discrete_dynamics *discrete_dynamics_alloc(int *polytopes_in_region,
                                                  size_t *polytope_sizes,
                                                  size_t *hull_sizes,
                                                  int *orig_polytopes_in_region,
                                                  size_t *orig_polytope_sizes,
                                                  size_t *orig_hull_sizes,
                                                  int *transitions_in_sizes,
                                                  int *transitions_out_sizes,
                                                  size_t n,
                                                  int abstract_states_count,
                                                  int number_of_original_regions,
                                                  int closed_loop,
                                                  int conservative,
                                                  int ord,
                                                  size_t time_horizon);
/**
 * @brief "Destructor" Deallocates the dynamically allocated memory of the discrete dynamics
 * @param d_dyn
 */
void discrete_dynamics_free(discrete_dynamics *d_dyn);

/**
 *
 */
typedef struct control_computation_arguments{

    gsl_matrix *u;
    current_state * now;
    discrete_dynamics *d_dyn;
    system_dynamics *s_dyn;
    int target_abs_state;
    cost_function * f_cost;
    size_t current_time_horizon;
    polytope **polytope_list_backup;

}control_computation_arguments;

typedef struct total_safemode_computation_arguments{

    current_state *now;
    gsl_matrix *u;
    polytope *current;
    polytope *safe;
    system_dynamics *s_dyn;
    size_t time_horizon;
    cost_function *f_cost;
    polytope **polytope_list_backup;

}total_safemode_computation_arguments;

typedef struct next_safemode_computation_arguments{

    current_state *now;
    gsl_matrix *u;
    system_dynamics *s_dyn;
    size_t time_horizon;
    cost_function *f_cost;
    polytope **polytope_list_backup;

}next_safemode_computation_arguments;

/**
 * "Constructor" Dynamically allocates the space for the get_input thread
 */
struct control_computation_arguments *cc_arguments_alloc(current_state *now,
                                                         gsl_matrix* u,
                                                         system_dynamics *s_dyn,
                                                         discrete_dynamics *d_dyn,
                                                         cost_function *f_cost,
                                                         size_t current_time_horizon,
                                                         int target_abs_state,
                                                         polytope **polytope_list);

/**
 * "Constructor" Dynamically allocates the space for the arguments of the safemode computation thread
 */
struct total_safemode_computation_arguments *sm_arguments_alloc(current_state *now,
                                                                gsl_matrix * u,
                                                                polytope *current,
                                                                polytope *safe,
                                                                system_dynamics * s_dyn,
                                                                size_t time_horizon,
                                                                cost_function *f_cost,
                                                                polytope **polytope_list_safemode);

/**
 * "Constructor" Dynamically allocates the space for the arguments of the next step towards safemode computation thread
 */
struct next_safemode_computation_arguments *next_sm_arguments_alloc(current_state *now,
                                                                    gsl_matrix * u,
                                                                    system_dynamics * s_dyn,
                                                                    size_t time_horizon,
                                                                    cost_function *f_cost,
                                                                    polytope **polytope_list_safemode);
#endif //CIMPLE_SYSTEM_H


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
/*#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlin.h>*/
#include "ral_nlls.h"

/* define the usertype */
struct usertype {
  double y_data[67];
  double x_data[67];
};

void generate_data_example ( double *x_data, double *y_data, const int m); // prototype
int eval_F  ( const int n, const int m,  void *params,
      const double *X, double *f);
int eval_J  ( const int n, const int m,  void *params,
      const double *X, double *f);
int eval_HF ( const int n, const int m,  void *params,
	       const double *X, const double *f, double *hf);

/* A c driver for the ral_nlls program */
int main(void) {

  /* Problem data */
  const int n = 2;
  const int m = 67;

  /* Derived types */
  struct ral_nlls_options options;
  struct ral_nlls_inform status;
  struct usertype params;

  printf("===============\n");
  printf("RAL NLLS driver\n");
  printf("~  C version  ~\n");
  printf("===============\n");

  /* Generate the data... */
  generate_data_example(params.x_data,params.y_data,m);

  double x[n];
  x[0] = 1.0;
  x[1] = 2.0;

  ral_nlls_default_options(&options);

  options.print_level = 3;

  nlls_solve(n, m, x,
	   eval_F, eval_J, eval_HF, &params,
	     &options, &status, NULL, NULL, NULL, NULL );

  int i;
  printf("\nX = \n");
  for(i=0; i < n; i++) {
    printf("  %5.4f \n",x[i]);
  }

  return 0; /* success */
}

/* Do a function evaluation */
int eval_F(int n, int m, void *params,
	      const double *X, double *f){

  struct usertype *myparams = (struct usertype *) params;

  int i;

  for(i=0; i<m; i++) {
    f[i] = myparams->y_data[i] - exp( X[0] * myparams->x_data[i] + X[1] );
  }

  return 0;
}

/* Evaluate the Jacobian */
int eval_J( const int n, const int m, void *params,
	     const double *X, double *J){

  struct usertype *myparams = (struct usertype *) params;

  int i;

  for(i=0; i<m; i++) {
    J[i] = -myparams->x_data[i] * exp( X[0] * myparams->x_data[i] + X[1] );
    J[m + i] = - exp( X[0] * myparams->x_data[i] + X[1] );
  }

  return 0;
}

/* Evaluate the Hessian */
int eval_HF( const int n, const int m, void *params,
	     const double *X, const double *f,
	     double *hf){

  struct usertype *myparams = (struct usertype *) params;

  int i;

  for(i=0; i<n*n; i++) {
    hf[i] = 0.0;
  }

  return 0;
}

/* Generate some example data... */
void generate_data_example( double *x_data, double *y_data, const int m ) {

  int i;
  /* Note the 67 here needs to be hard-coded, and you can't initialize
     an array with a variable length in C */
  double tempx[67] =  { 0.0,
			0.075000000000000,
			0.150000000000000,
			0.225000000000000,
			0.300000000000000,
			0.375000000000000,
			0.450000000000000,
			0.525000000000000,
			0.600000000000000,
			0.675000000000000,
			0.750000000000000,
			0.825000000000000,
			0.900000000000000,
			0.975000000000000,
			1.050000000000000,
			1.125000000000000,
			1.200000000000000,
			1.275000000000000,
			1.350000000000000,
			1.425000000000000,
			1.500000000000000,
			1.575000000000000,
			1.650000000000000,
			1.725000000000000,
			1.800000000000000,
			1.875000000000000,
			1.950000000000000,
			2.025000000000000,
			2.100000000000000,
			2.175000000000000,
			2.250000000000000,
			2.325000000000000,
			2.400000000000000,
			2.475000000000000,
			2.550000000000000,
			2.625000000000000,
			2.700000000000000,
			2.775000000000000,
			2.850000000000000,
			2.925000000000000,
			3.000000000000000,
			3.075000000000000,
			3.150000000000000,
			3.225000000000001,
			3.300000000000000,
			3.375000000000000,
			3.450000000000000,
			3.525000000000000,
			3.600000000000001,
			3.675000000000000,
			3.750000000000000,
			3.825000000000000,
			3.900000000000000,
			3.975000000000000,
			4.050000000000001,
			4.125000000000000,
			4.200000000000000,
			4.275000000000000,
			4.350000000000001,
			4.425000000000000,
			4.500000000000000,
			4.575000000000000,
			4.650000000000000,
			4.725000000000001,
			4.800000000000000,
			4.875000000000000,
			4.950000000000000};

  double tempy[67] =  {0.907946872110432,
		       1.199579396036134,
		       1.060092431384317,
		       1.298370500472354,
		       0.952768858414788,
		       1.209665290655204,
		       1.256912538155493,
		       1.163922146095987,
		       1.004877938808100,
		       1.205944250961060,
		       0.952693297695969,
		       1.449662692280761,
		       1.402015259144406,
		       1.378094012325746,
		       1.560882147577552,
		       1.437185539058121,
		       1.559853079888265,
		       1.877814947316832,
		       1.818781749024682,
		       1.375546045112591,
		       1.233967904388409,
		       1.887793124397751,
		       1.610237096463521,
		       1.787032484792262,
		       1.850015127982676,
		       2.120553361509177,
		       1.942913663511919,
		       2.106517132599766,
		       2.271787117356578,
		       1.727554346001754,
		       2.002909500898113,
		       1.975837413903495,
		       2.337446525801909,
		       1.960190841677278,
		       2.447097025572309,
		       2.161663720225506,
		       2.748798529374621,
		       2.507814238594416,
		       2.423769408403069,
		       2.578119353028746,
		       2.460310096221557,
		       2.638362783992324,
		       2.765540456237868,
		       2.837165966564409,
		       3.179711963042789,
		       3.245315453091675,
		       3.289631922410174,
		       3.360995198615834,
		       3.470489725998371,
		       3.169513520153466,
		       3.363740517933189,
		       3.665288099084969,
		       3.620334359722351,
		       4.018911445550667,
		       3.512715166706162,
		       3.874661411575566,
		       4.197746303653517,
		       3.703511523106007,
		       4.076351488309604,
		       4.056340365649961,
		       4.297751562451419,
		       4.373076571153739,
		       4.577093065941748,
		       4.856619059058190,
		       4.927350280596274,
		       4.703122139742729,
		       4.870205182453842};

  for(i=0;i<m;i++){
    x_data[i] = tempx[i];
    y_data[i] = tempy[i];
  }

}


/*
 *  Copyright 2010, Weill Medical College of Cornell University
 *  All rights reserved.
 *
 *  This software is distributed WITHOUT ANY WARRANTY
 *  under license "license.txt" included with distribution and
 *  at http://neurodatabase.org/src/license.
 */

#include <gsl/gsl_vector.h>
#include<gsl/gsl_matrix.h>
#include<gsl/gsl_permutation.h>
#include<gsl/gsl_linalg.h>
#include<gsl/gsl_sf.h>
#include "../shared/toolkit_c.h"

#define BUB_MAXWORDS_LIMIT 100000

struct bub_options{
	double lambda_N;
	int use_weighting;
	double mesh_min;
	double mesh_max;
	double mesh_inc;
	int opt_use_weighting;
	double opt_mesh_min;
	double opt_mesh_max;
	double opt_mesh_inc;
	int bag_threshold;
};

int bag1(int N,double m,double *a,double lambda_0,struct bub_options *bubopts);
int bub1(int N,double m,double *a,double lambda_0,int K,int compat,struct bub_options *bubopts);
void make_binom(int N,int meshsize,double *p,double **B_jN);
void make_weight(double *p,int meshsize,double m,int use_weighting,double *f,double *c);
double compute_error(double *a,int N,double m,int compat,struct bub_options *bubopts);

int entropy_bub(struct hist1d *in,struct options_entropy *opts,struct estimate *entropy)
{
	double *a;
	int *h;
	double H;
	struct bub_options *bubopts;
	double eps,epsm;
	int N,i,j,max_bin;
	double m;
	int status;

	N = (*in).P;

	if(opts->possible_words_flag==0)
	{
		if(opts->bub_possible_words_strategy==0)
			m = (double)in->C;
		else if(opts->bub_possible_words_strategy==1)
			m = (double)in->N;
		else if(opts->bub_possible_words_strategy==2)
			m = MIN(max_possible_words(in,1),BUB_MAXWORDS_LIMIT);
		else
			m = (double)in->C;
	}
	else
		if(opts->possible_words>0)
			m = opts->possible_words;
		else
			switch((int)(opts->possible_words))
			{
				case 0: /* Inf */
					entropy->value = NAN; /* this value based on piece-meal derivation when m=INFINITY */
					return EXIT_SUCCESS;
				case -1: /* recommended */
					m = (double)in->C;
					break;
				case -2: /* unique */
					m = (double)in->C;
					break;
				case -3: /* total */
					m = (double)in->P;
					break;
				case -4: /* possible */
					m = max_possible_words(in,0);
					break;
				case -5: /* min_tot_pos */
					m = max_possible_words(in,1);
					break;
				case -6: /* min_lim_tot_pos */
					m = MIN(BUB_MAXWORDS_LIMIT,max_possible_words(in,1));
					break;
			}

	/* First find a vector */
	a = (double *)malloc((N+1)*sizeof(double));

	bubopts = (struct bub_options *)malloc(sizeof(struct bub_options));
	(*bubopts).bag_threshold = 20;
	if(N<(*bubopts).bag_threshold)
	{
		(*bubopts).lambda_N = 0;
		(*bubopts).mesh_min = 0;
		(*bubopts).mesh_max = 1;
		(*bubopts).mesh_inc = 1/(5*(double)N);
		(*bubopts).use_weighting = 0;
		status = bag1(N,m,a,(*opts).bub_lambda_0,bubopts);
	}
	else
	{
		eps = (1/(double)N)*1e-10;
		epsm = (1/m)*1e-10;
		(*bubopts).lambda_N = 1;
		(*bubopts).mesh_min = eps;
		(*bubopts).mesh_max = MIN(1,3/(double)N)-eps;
		(*bubopts).mesh_inc = MIN(1,3/(double)N)/100;
		(*bubopts).lambda_N = 1;
		(*bubopts).use_weighting = 0;
		(*bubopts).opt_mesh_min = epsm;
		(*bubopts).opt_mesh_max = MIN(1,3/m)-epsm;
		(*bubopts).opt_mesh_inc = MIN(1,3/m)/100;
		(*bubopts).opt_use_weighting = 0;
		status = bub1(N,m,a,(*opts).bub_lambda_0,(*opts).bub_K,(*opts).bub_compat,bubopts);
	}

	/* Get counts of counts */
	h = (int *)calloc(N+1,sizeof(int));
	for(i=0; i<(*in).C; i++)
		h[(int)(*in).wordcnt[i]]++;

	/* Then compute entropy */
	H = 0;
	for(j=0; j<=N; j++)
		H += a[j]*h[j];
	entropy->value = NAT2BIT(H);

	free(a);
	free(h);
	free(bubopts);

	return EXIT_SUCCESS; /*	TODO return status; reports errors with staverify.m - need to look into these errors and possibly pass them to message structure */
}

int bag1(int N,double m,double *a,double lambda_0,struct bub_options *bubopts)
{
	int meshsize;
	int i,j;
	double *p,*Y,**X,**B_jN,*A_data,**D,*I0,*IN;
	double *f,c;
	gsl_vector *b,*x,*tau,*norm,*S,*work;
	gsl_matrix *A,*V;
	int *signum;
	int d_status, s_status, status = EXIT_SUCCESS;

	meshsize = floor(((*bubopts).mesh_max-(*bubopts).mesh_min)/(*bubopts).mesh_inc)+1;

	A = gsl_matrix_calloc(meshsize+(N+1)+2,N+1);
	A_data = (*A).data;
	X = VectorToMatrixDouble(&A_data[0],meshsize,N+1);
	D = VectorToMatrixDouble(&A_data[meshsize*(N+1)],N+1,N+1);
	I0 = &A_data[(meshsize+(N+1))*(N+1)];
	IN = &A_data[(meshsize+(N+1)+1)*(N+1)];

	b = gsl_vector_calloc(meshsize+(N+1)+2);
	Y = (*b).data;

	/* Make mesh */
	p = (double *)malloc(meshsize*sizeof(double));
	for(i=0; i<meshsize; i++)
		p[i] = (*bubopts).mesh_min + i*(*bubopts).mesh_inc;

	/* Make weighting */
	f = (double *)malloc(meshsize*sizeof(double));
	make_weight(p,meshsize,m,(*bubopts).use_weighting,f,&c);

	/* Make H */
	for(i=0; i<meshsize; i++)
		Y[i] = f[i]*XLOGX(p[i]);

	/* Make binomial coefficents */
	B_jN = MatrixDouble(meshsize,N+1);
	make_binom(N,meshsize,p,B_jN);

	for(i=0; i<meshsize; i++)
		for(j=0; j<N+1; j++)
			X[i][j] = f[i]*B_jN[i][j];

	/* Make D */
	for(j=0; j<N; j++)
	{
		D[j][j]=-1*sqrt((double)N)/c;
		D[j][j+1]=1*sqrt((double)N)/c;
	}

	/* Make I0 and IN */
	I0[0] = sqrt(((double)N)*lambda_0)/c;
	IN[N] = sqrt(((double)N)*(*bubopts).lambda_N)/c;

	/* Solve the least squares problem */
	V = gsl_matrix_calloc(N+1,N+1);
	S = gsl_vector_calloc(N+1);
	work = gsl_vector_calloc(N+1);
	d_status = gsl_linalg_SV_decomp(A,V,S,work);
	if(d_status)
		status = EXIT_FAILURE;
	x = gsl_vector_calloc(N+1);
	s_status = gsl_linalg_SV_solve(A,V,S,b,x);
	if(s_status)
		status = EXIT_FAILURE;

	/* Read out the answer from x */
	memcpy(a,(*x).data,(N+1)*sizeof(double));

	/* Free memory */
	gsl_matrix_free(V);
	gsl_vector_free(S);
	gsl_vector_free(work);
	gsl_vector_free(x);

	FreeMatrixDouble(B_jN);
	free(X);
	free(D);
	free(f);
	free(p);
	gsl_matrix_free(A);
	gsl_vector_free(b);

	return status;
}

int bub1(int N,double m,double *a,double lambda_0,int K,int compat,struct bub_options *bubopts)
{
	int meshsize;
	int i,j,k;
	double *p,*Y,**X,**B_jN,*A_data,*b_data,**D,*I0,*IN;
	double *f,c;
	gsl_vector *b,*x,*tau,*norm,*S,*work;
	gsl_matrix *A,*V;
	int d_status, s_status, status = EXIT_SUCCESS;
	double best_error,max_error,h_MM;
	double *a_MM,*a_temp;
	int k1;

	/* Compute the a coefficients using the Miller-Madow method */
	a_MM = (double *)calloc(N+1,sizeof(double));
	for(j=0; j<=N; j++)
		a_MM[j] = XLOGX((double)j/(double)N) + (1-((double)j/(double)N))/(2*(double)N);

	meshsize = floor(((*bubopts).mesh_max-(*bubopts).mesh_min)/(*bubopts).mesh_inc)+1;

	/* Make mesh */
	p = (double *)malloc(meshsize*sizeof(double));
	for(i=0; i<meshsize; i++)
		p[i] = (*bubopts).mesh_min + i*(*bubopts).mesh_inc;

	/* Make weighting */
	f = (double *)malloc(meshsize*sizeof(double));
	make_weight(p,meshsize,m,(*bubopts).use_weighting,f,&c);

	B_jN = MatrixDouble(meshsize,N+1);
	make_binom(N,meshsize,p,B_jN);

	best_error = HUGE_VAL;

	for(k=0; k<MIN(K,N); k++)
	{
		k1 = k+1;

		a_temp = (double *)calloc(N+1,sizeof(double));
		memcpy(a_temp,a_MM,(N+1)*sizeof(double));

		/*** MAKE A ***/
		A = gsl_matrix_calloc(meshsize+k1+2,k1);
		A_data = (*A).data;
		X = VectorToMatrixDouble(&A_data[0],meshsize,k1);
		D = VectorToMatrixDouble(&A_data[meshsize*k1],k1,k1);
		I0 = &A_data[(meshsize+k1)*k1];
		IN = &A_data[(meshsize+k1+1)*k1];

		/*** MAKE B ***/
		b = gsl_vector_calloc(meshsize+k1+2);
		b_data = (*b).data;
		Y = &b_data[0];
		if(compat)
			b_data[meshsize+k1+1] = (sqrt((double)N)/c)*a_MM[k];
		else
			b_data[meshsize+k1+1] = (sqrt((double)N)/c)*a_MM[k1];

		for(i=0; i<meshsize; i++)
		{
			h_MM = 0;
			for(j=k1; j<=N; j++)
				h_MM += B_jN[i][j]*a_MM[j];

			Y[i] = f[i]*(XLOGX(p[i]) - h_MM);
		}

		for(i=0; i<meshsize; i++)
			for(j=0; j<k1; j++)
				X[i][j] = f[i]*B_jN[i][j];

		/* Make D */
		for(j=0; j<k1-1; j++)
		{
			D[j][j]=-1*sqrt((double)N)/c;
			D[j][j+1]=1*sqrt((double)N)/c;
		}

		/* Make I0 and IN */
		I0[0] = sqrt(((double)N)*lambda_0)/c;
		IN[k1-1] = sqrt(((double)N)*(*bubopts).lambda_N)/c;

		/* Solve the least squares problem */
		V = gsl_matrix_calloc(k1,k1);
		S = gsl_vector_calloc(k1);
		work = gsl_vector_calloc(k1);
		d_status = gsl_linalg_SV_decomp(A,V,S,work);
		if(d_status)
			status = EXIT_FAILURE;
		x = gsl_vector_calloc(k1);
		s_status = gsl_linalg_SV_solve(A,V,S,b,x);
		if(s_status)
			status = EXIT_FAILURE;

		/* Read out the answer from x */
		memcpy(a_temp,(*x).data,k1*sizeof(double));

#ifdef DEBUG
		printf("k1=%d\n",k1);
		for(j=0; j<=k; j++)
			printf("%f ",a_temp[j]);
		printf("\n");
#endif

		/* Find the error */
		max_error = compute_error(a_temp,N,m,compat,bubopts);

		if(max_error<best_error)
		{
			best_error = max_error;
			memcpy(a,a_temp,(N+1)*sizeof(double));
		}

		free(a_temp);

		/* Free memory */
		gsl_matrix_free(V);
		gsl_vector_free(S);
		gsl_vector_free(work);
		gsl_vector_free(x);
		free(X);
		free(D);
		gsl_matrix_free(A);
		gsl_vector_free(b);
	}

	free(f);
	free(p);
	FreeMatrixDouble(B_jN);
	free(a_MM);
}

void make_binom(int N,int meshsize,double *p,double **B_jN)
{
	int i,j;
	double *NCj;

	NCj = (double *)calloc(N+1,sizeof(double));

	for(j=0; j<N+1; j++)
		NCj[j] = gsl_sf_lngamma(N+1)-gsl_sf_lngamma(j+1)-gsl_sf_lngamma(N-j+1);

	for(i=0; i<meshsize; i++)
		if(p[i]==0)
		{
			B_jN[i][0]=1;
			for(j=1; j<N+1; j++)
				B_jN[i][j]=0;
		}
		else if(p[i]==1)
		{
			for(j=0; j<N; j++)
				B_jN[i][j]=0;
			B_jN[i][N]=1;
		}
		else
		{
			for(j=0; j<N+1; j++)
				B_jN[i][j] = exp(NCj[j] + log(p[i])*j + log(1-p[i])*(N-j));
		}

	free(NCj);
}

void make_weight(double *p,int meshsize,double m,int use_weighting,double *f,double *c)
{
	int i;

	for(i=0; i<meshsize; i++)
		if(use_weighting && p[i]>1/m)
			f[i] = 1./p[i];
		else
			f[i] = m;

	if(use_weighting)
		*c = 2;
	else
		*c = 1;
}

double compute_error(double *a,int N,double m,int compat,struct bub_options *bubopts)
{
	int i,j;
	double max_bias,max_error,max_variance0,max_variance1;
	double *p,*f,**B_jN;
	double temp1,temp2,temp3,temp4,temp5;
	int meshsize;
	double c;

	meshsize = floor(((*bubopts).opt_mesh_max-(*bubopts).opt_mesh_min)/(*bubopts).opt_mesh_inc)+1;

	/* Make mesh */
	p = (double *)malloc(meshsize*sizeof(double));
	for(i=0; i<meshsize; i++)
		p[i] = (*bubopts).opt_mesh_min + i*(*bubopts).opt_mesh_inc;

	/* Make weighting */
	f = (double *)malloc(meshsize*sizeof(double));
	make_weight(p,meshsize,m,(*bubopts).opt_use_weighting,f,&c);

	/* Make B_jN */
	B_jN = MatrixDouble(meshsize,N+1);
	make_binom(N,meshsize,p,B_jN);

	/* Compute the worst-case bias */
	max_bias = 0;
	for(i=0; i<meshsize; i++)
	{
		temp1 = 0;
		for(j=0; j<N+1; j++)
			temp1+=B_jN[i][j]*a[j];
		temp2 = f[i]*fabs(temp1 - XLOGX(p[i]));
		if(temp2>max_bias)
			max_bias = temp2;
	}

	/* Steele loose bound on variance */
	max_variance1 = 0;
	for(i=0; i<meshsize; i++)
	{
		temp3 = 0;
		for(j=1; j<N+1; j++)
			temp3 += B_jN[i][j]*((double)j/(double)N)*pow(a[j]-a[j-1],2);
		temp4 = f[i]*temp3;
		if(temp4>max_variance1)
			max_variance1 = temp4;
	}
	if(compat)
		max_variance1 *= 4*N*c;
	else
		max_variance1 *= 2*N*c;

	/* McDiarmid bound on variance */
	max_variance0 = 0;
	for(j=1; j<N+1; j++)
	{
		temp5 = fabs(a[j]-a[j-1]);
		if(temp5>max_variance0)
			max_variance0=temp5;
	}
	max_variance0 = N*pow(max_variance0,2);

#ifdef DEBUG
	printf("maxbias=%f maxvariance0=%f maxvariance1=%f\n",max_bias,max_variance0,max_variance1);
#endif

	max_error = NAT2BIT(sqrt(pow(max_bias,2) + MIN(max_variance0,max_variance1)));
	free(p);
	free(f);
	FreeMatrixDouble(B_jN);

	return max_error;
}


#include <jni.h>
#include <assert.h>
#include <lapacke.h>

JNIEXPORT jint Java_JAMAJni_SingularValueDecomposition_dgesvd (JNIEnv *env, jclass klass, jint matrix_layout, jchar jobu, jchar jobvt, jint m, jint n, jdoubleArray a, jint lda, jdoubleArray s, jdoubleArray u, jint ldu, jdoubleArray vt, jint ldvt, jdoubleArray superb){

    //superb contains the unconverged superdiagonal elements of an upper bidiagonal matrix B whose diagonal is in S (not necessarily sorted).

    double *aElems, *sElems, *uElems, *vtElems, *superbElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    sElems = (*env)-> GetDoubleArrayElements (env, s, NULL);
    uElems = (*env)-> GetDoubleArrayElements (env, u, NULL);
    vtElems = (*env)-> GetDoubleArrayElements (env, vt, NULL);
    superbElems = (*env)-> GetDoubleArrayElements (env, superb, NULL);

    assert(aElems && sElems && uElems && vtElems && superbElems);

    info = LAPACKE_dgesvd((int) matrix_layout, (char) jobu, (char) jobvt, (lapack_int) m, (lapack_int) n, aElems, (lapack_int) lda, sElems, uElems, (lapack_int) ldu, vtElems, (lapack_int) ldvt, superbElems);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, s, sElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, u, uElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, vt, vtElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, superb, superbElems, 0);

    return info;
}

JNIEXPORT jint Java_JAMAJni_SingularValueDecomposition_dgesdd
(JNIEnv *env, jclass klass, jint matrix_layout, jchar jobz, jint m, jint n,
 jdoubleArray a, jint lda, jdoubleArray s, jdoubleArray u, jint ldu,
 jdoubleArray vt, jint ldvt){

    double *aElems, *sElems, *uElems, *vtElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    sElems = (*env)-> GetDoubleArrayElements (env, s, NULL);
    uElems = (*env)-> GetDoubleArrayElements (env, u, NULL);
    vtElems = (*env)-> GetDoubleArrayElements (env, vt, NULL);

    assert(aElems && sElems && uElems && vtElems);

    info = LAPACKE_dgesdd((int) matrix_layout, (char) jobz, (lapack_int) m, (lapack_int) n, aElems, (lapack_int) lda, sElems, uElems, (lapack_int) ldu, vtElems, (lapack_int) ldvt);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, s, sElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, u, uElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, vt, vtElems, 0);

    return info;
}

JNIEXPORT jint Java_JAMAJni_SingularValueDecomposition_dgeev (JNIEnv *env, jclass klass, jint matrix_layout, jchar jobvl, jchar jobvr, jint n, jdoubleArray a, jint lda, jdoubleArray wr, jdoubleArray wi, jdoubleArray vl, jint ldvl, jdoubleArray vr, jint ldvr){

    double *aElems, *wrElems, *wiElems, *vlElems, *vrElems;
    int info;

    aElems = (*env)-> GetDoubleArrayElements (env, a, NULL);
    wrElems = (*env)-> GetDoubleArrayElements (env, wr, NULL);
    wiElems = (*env)-> GetDoubleArrayElements (env, wi, NULL);
    vlElems = (*env)-> GetDoubleArrayElements (env, vl, NULL);
    vrElems = (*env)-> GetDoubleArrayElements (env, vr, NULL);

    assert(aElems && wrElems && wiElems && vlElems && vrElems);

    info = LAPACKE_dgeev((int) matrix_layout, (char) jobvl, (char) jobvr, (lapack_int) n, aElems, (lapack_int) lda, wrElems, wiElems, vlElems, (lapack_int) ldvl, vrElems, (lapack_int) ldvr);

    (*env)-> ReleaseDoubleArrayElements (env, a, aElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, vl, vlElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, vr, vrElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, wr, wrElems, 0);
    (*env)-> ReleaseDoubleArrayElements (env, wi, wiElems, 0);

    return info;

}


#include "kepler.h"

#include <math.h>
#include <stdlib.h>
#include <stdio.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>

#define OVERPHASE(x)  ( 2.0 * M_PI * floor ( 0.5 + 0.5 * x * M_1_PI ) )

#define DEBUG 0

/****************************************************************************/

struct delta_params { double ecosmu, esinmu; };

static double delta_f ( double delta, void *params ) {

  struct delta_params * op = ( struct delta_params * ) params;
  double ecosmu = op->ecosmu;
  double esinmu = op->esinmu;

  return ecosmu * sin(delta) + esinmu * cos(delta) - delta;

}

static double delta_df ( double delta, void *params ) {

  struct delta_params * op = ( struct delta_params * ) params;
  double ecosmu = op->ecosmu;
  double esinmu = op->esinmu;

  return ecosmu * cos(delta) - esinmu * sin(delta) - 1.0;

}

static void delta_fdf ( double delta, void *params, double * f, double * df ) {
  struct delta_params * op = ( struct delta_params * ) params;
  double ecosmu = op->ecosmu;
  double esinmu = op->esinmu;
  double cosdelta = cos(delta);
  double sindelta = sin(delta);
  *f  = ecosmu * sindelta + esinmu * cosdelta - delta;
  *df = ecosmu * cosdelta - esinmu * sindelta - 1.0;
}

double kepler_psiofmu ( double mu, double ecc ) {

  int status;
  int iter = 0, max_iter = 100;
  double x0, x, x_expected;
  gsl_function_fdf FDF;
  static gsl_root_fdfsolver * solver;
  static int solverexists = 0;
  struct delta_params params = { ecc * cos(mu), ecc * sin(mu) };

  if ( ! solverexists ) { solverexists = 1;
    solver = gsl_root_fdfsolver_alloc ( gsl_root_fdfsolver_newton );
  }

  if ( DEBUG ) printf( "kepler_psiofmu ( mu = %lg, ecc = %lg )\n", mu, ecc );

  /* early exit make this more tollerant */
  if ( 0.0 == ecc ) return mu;
  if ( 0.0 == mu - OVERPHASE(mu) ) return 0.0;

  FDF.f = &delta_f;
  FDF.df = &delta_df;
  FDF.fdf = &delta_fdf;
  FDF.params = &params;

  x_expected = x = (&params)->esinmu;

  gsl_root_fdfsolver_set (solver, &FDF, x);

  if ( DEBUG ) printf ("using %s method\n", gsl_root_fdfsolver_name (solver));
  if ( DEBUG ) printf ("%-5s %10s %10s\n", "iter", "root", "err");

  do {
    iter++;
    status = gsl_root_fdfsolver_iterate (solver);
    x0 = x;
    x = gsl_root_fdfsolver_root (solver);
    status = gsl_root_test_delta (x, x0, 0, 1e-3);
    if ( DEBUG && status == GSL_SUCCESS ) printf ("Converged:\n");
    if ( DEBUG ) printf ("%5d %10.7f %+10.7f\n", iter, x, x - x_expected);
  } while (status == GSL_CONTINUE && iter < max_iter);

  if ( DEBUG ) printf( "kepler_psiofmu ( mu = %lg, ecc = %lg ) = %lg\n",
    mu, ecc, mu+x );

  return  mu + x;

}

/****************************************************************************/

double kepler_thetaofmu ( double mu, double ecc ) {

  double overmu = OVERPHASE(mu), psi, theta;

  psi = kepler_psiofmu ( mu, ecc );
  theta = 2.0 * atan( sqrt((1.0+ecc)/(1.0-ecc)) * tan(0.5*psi) );

  return  overmu + theta;

}

/****************************************************************************/

double kepler_rv ( double mu, double ecc, double omega ) {

  double psi, theta;

  psi = kepler_psiofmu ( mu, ecc );
  theta = 2.0 * atan( sqrt((1.0+ecc)/(1.0-ecc)) * tan(0.5*psi) );

  return  cos(theta + omega) + ecc*cos(omega);
}

/****************************************************************************/

double kepler_lite ( double mu, double ecc, double omega ) {

  double psi, theta;

  psi = kepler_psiofmu ( mu, ecc );
  theta = 2.0 * atan( sqrt((1.0+ecc)/(1.0-ecc)) * tan(0.5*psi) );

  return  (1.0 - ecc*ecc) * sin(theta + omega) / (1.0 + ecc*cos(theta));
}

/****************************************************************************/

double kepler_sep ( double mu, double ecc, double omega, double inc ) {

  double psi, theta, tmpd;

  psi = kepler_psiofmu ( mu, ecc );
  theta = 2.0 * atan( sqrt((1.0+ecc)/(1.0-ecc)) * tan(0.5*psi) );
  tmpd = sin(theta + omega) * sin(inc);
  tmpd = sqrt(1.0 - tmpd*tmpd);

  return  tmpd * (1.0 - ecc*ecc) / (1.0 + ecc * cos(theta));
}

/****************************************************************************/


/*
 * File:   BlshIndex.h
 * Author: chteflio
 *
 * Created on March 4, 2016, 3:19 PM
 */

#ifndef BLSHINDEX_H
#define	BLSHINDEX_H

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_result.h>
#include <gsl/gsl_errno.h>

namespace mips {

    class BlshIndex : public LshIndex {

        int computeMinMatches(int nTrials) {
            double p;
            double epsilon = 1 - R;
            int guess = nTrials / 2;
            int start = 0;
            int end = nTrials;

            //             std::cout<<"threshold: "<<threshold<<" epsilon: "<<epsilon<<std::endl;

            int retValue = -1;
            do {
                p = 1.0 - posteriorCdf(guess, nTrials, minMathesT);
                if (p > epsilon) {
                    if (start == end) {
                        retValue = start - 1;
                        break;
                    }
                    end = guess - 1;
                    if (end < start) {
                        retValue = guess - 1;
                        break;
                    }
                } else if (p < epsilon) {
                    if (start == end) {
                        retValue = start;
                        break;
                    }
                    start = guess + 1;
                    if (start > end) {
                        retValue = guess;
                        break;
                    }
                } else {
                    retValue = guess;
                    break;
                }
                guess = (start + end) / 2;
                if (end < 0 || start > nTrials) {
                    std::cout << "Problem in computeMinMatches start " << start << " end " << end << std::endl;
                    exit(1);
                }

            } while (1);

            if (retValue < 0)
                retValue = 0;
            else if (retValue > nTrials)
                retValue = nTrials;

            return retValue;
        }

        inline double posteriorCdf(double s, double n, double x) {
            if (x >= 1.0)
                return 1.0;
            if (x <= 0.5)
                return 0;
            double b1 = 1.0;
            double bHalf = boost::math::ibeta(s + 1, n - s + 1, 0.5);
            double bx = boost::math::ibeta(s + 1, n - s + 1, x);
            double den = b1 - bHalf;
            if (den < 1.0e-15)
                return exp(log(bx - bHalf) - log(den));
            else
                return (bx - bHalf) / den;

        }

    public:
        double minMathesT; // threshold for matching, r = c2r(simT)
        long long hashGroups = 0;
        int32_t *minMatches; // minimum number of hashes that should be observed to meet simT
        double R;
        double worst;

        BlshIndex() : LshIndex(), minMathesT(0), minMatches(nullptr) {
        }

        inline ~BlshIndex() {
            if (minMatches != nullptr)
                delete[] minMatches;
        }

        inline void allocateBayesLSHMemory(double worstCaseTheta) {
            if (minMatches == nullptr) {
                // set up cache space and pre-compute all minimum matches
                minMatches = da_i32malloc(hashGroups, NULL); //"allocateBayesLSHMemory: minMatches"

                //                                std::cout << "worstCaseTheta: " << worstCaseTheta << std::endl;
                minMathesT = (1.0 - acos(worstCaseTheta) * INVPI); // min matches threshold
                for (int i = 0; i < hashGroups; i++) {
                    minMatches[i] = computeMinMatches((i + 1) * 32);
                    //                                        std::cout<<minMatches[i]<<" ";
                }
                //                                std::cout <<  std::endl;

            }

        }

        inline void initializeLists(const VectorMatrix& matrix, double worstCaseTheta, bool forProbeVectors, double recall,
                ta_size_type start = 0, ta_size_type end = 0) {

            omp_set_lock(&writelock);

            if (!initialized) {
                R = recall;
                end = (end == 0 ? matrix.rowNum : end);
                row_type nVectors = end - start;
                cosSketches = new CosineSketches(nVectors, LSH_CODE_LENGTH, LSH_SIGNATURES);
                hashGroups = LSH_CODE_LENGTH * LSH_SIGNATURES / 32;
                initializedSketches.resize(nVectors, 0);
                cosSketches->alloc();

                if (forProbeVectors) {
                    switch (LSH_CODE_LENGTH) {
                        case 8:
                            lshBins = new LshBinsDense();
                            break;
                        case 16:
                            lshBins = new LshBinsSparse<uint16_t>();
                            break;
                        case 24:
                        case 32:
                            lshBins = new LshBinsSparse<uint32_t>();
                            break;
                        default:
                            lshBins = new LshBinsSparse<uint64_t>();
                            break;
                    }

                    lshBins->init(cosSketches->bytesPerCode, cosSketches->numHashTables, cosSketches->nVectors);
                }

                if (worstCaseTheta < std::numeric_limits<double>::max()) {
                    if (worstCaseTheta > 0) {
                        worstCaseTheta /= matrix.getVectorLength(start);
                        worstCaseTheta = (worstCaseTheta > 1 ? 1 : worstCaseTheta); // it will break in the loop afterwards

                    } else {
                        worstCaseTheta /= matrix.getVectorLength(end - 1);
                        worstCaseTheta = (worstCaseTheta < -1 ? -1 : worstCaseTheta); // it will have to check everything
                    }

                    worst = worstCaseTheta;
                    allocateBayesLSHMemory(worstCaseTheta);
                }

                initialized = true;
            }
            omp_unset_lock(&writelock);
        }

    };

}

#endif	/* BLSHINDEX_H */


#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>

#include "su2_matrix.c"

#ifndef gauge_config
#include "config.h"
#define gauge_config 1

#define sq(x) ((x)*(x))
#define cb(x) ((x)*(x)*(x))
#endif

int acc, tot; // acceptance rate

#define FORWARD 1 //true
#define BACKWARD 0 //false

// Access pattern methods --------------------------------------------------

// index of link site=(t,x,y,z) in direction dir
int index(const int* site, int dir){
  return dir+site[0]*4+site[1]*N*4+site[2]*N*N*4+site[3]*N*N*N*4;
}

// shift site array by one to direction dir
void shift(int* site, int dir, int forward){
  if(forward == 1){
    site[dir] = (site[dir]+1)%N;
  }else{
    site[dir] = (site[dir]-1+N)%N;
  }
}

// end of access pattern methods -------------------------------------------

// Plaquette methods -------------------------------------------------------

// plaquette operator at site (t,x,y,z) in plane (mu,nu)
// normally mu>nu
double plaquette(const su2_matrix* links, int mu, int nu, int* site){
  su2_matrix A,B,C,D;
  A = links[index(site,mu)];
  shift(site,mu,FORWARD);
  B = links[index(site,nu)];
  shift(site,mu,BACKWARD);
  shift(site,nu,FORWARD);
  C = su2_inv(links[index(site,mu)]);
  shift(site,nu,BACKWARD); // restore site position
  D = su2_inv(links[index(site,nu)]);

  A = su2_mul(A,B);
  A = su2_mul(A,C);
  A = su2_mul(A,D);

  return su2_trace(A)/2.0;
}

double plaquette_mean(const su2_matrix* links, int mu, int nu){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            mean += plaquette(links,mu,nu,site);
        }
      }
    }
  }
  return mean / (N*N*N*N);
}

// end of plaquette methods ------------------------------------------------

// Metropolis update --------------------------------------------------------

void do_hb_update(su2_matrix* links, const gsl_rng* r, const su2_matrix* rands, int* site, int dir);
double action(su2_matrix* links, int* site, int dir);

// the big loop
void hb_update(su2_matrix* links, const gsl_rng* r, const su2_matrix* rands){
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
          for(int dir=0;dir<4;dir++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            do_hb_update(links,r,rands,site,dir);
          }
        }
      }
    }
  }
}

// update
void do_hb_update(su2_matrix* links, const gsl_rng* r, const su2_matrix* rands, int* site, int dir){
  if(verbose) {printf("Updating {%d, %d, %d, %d} direction %d",site[0],site[1],site[2],site[3],dir);getchar();}
  double dS,action_old;
  su2_matrix link_old;
  for(int times=0; times<Nk; times++){
    action_old = action(links,site,dir);
    link_old = links[index(site,dir)];
    links[index(site,dir)]=su2_mul(links[index(site,dir)],rands[gsl_rng_uniform_int(r, 50)]);

    dS = action(links,site,dir)-action_old;

    if(verbose) {printf("Old action = %f, ",action_old); su2_print(link_old); getchar();}
    if(verbose) {printf("New action = %f, ",action_old+dS); su2_print(links[index(site,dir)]); getchar();}
    acc++;tot++;
    if(dS>0){
      if(exp(-dS)<gsl_rng_uniform(r)){
        acc--;
        links[index(site,dir)]=link_old; // revert update
        if(verbose) {printf("Reverted"); getchar();}
      }
    }
  }

}

// calculating the action
// for each direction we need to find plaquette at 2 sites in each of the 3 planes
// mu > nu
double action(su2_matrix* links, int* site, int dir){
  int mu,nu;
  double result=0;
  for(int k=0; k<4; k++){
    if(k==dir) continue; // loop over planes
    if(k>dir){mu=k; nu=dir;} // mu=larger number
    if(k<dir){mu=dir; nu=k;}

    result += plaquette(links, mu,nu,site);
    shift(site,k,BACKWARD); // shift to the perpendicular direction backwards
    result += plaquette(links, mu,nu,site);
    shift(site,k,FORWARD);  // shift it back
  }

  return -beta*result;
}

// end of heat bath update -------------------------------------------------

// Wilson loop -------------------------------------------------------------

double wloop(int r, int t, int mu,const su2_matrix* links, int* site){
  su2_matrix A[(r+t)*2];

  // bottom side
  for(int i=0;i<t;i++){
    A[i]=links[index(site,0)];
    shift(site,0,FORWARD);
  }

  // right side
  for(int i=0;i<r;i++){
    A[t+i]=links[index(site,mu)];
    shift(site,mu,FORWARD);
  }

  // top side
  for(int i=0;i<t;i++){
    shift(site,0,BACKWARD);
    A[t+r+i]=su2_inv(links[index(site,0)]);
  }

  // left side
  for(int i=0;i<r;i++){
    shift(site,mu,BACKWARD);
    A[t+r+t+i]=su2_inv(links[index(site,mu)]);
  }

  // multiply
  for(int i=1;i<2*(r+t);i++){
    A[0]=su2_mul(A[0],A[i]);
  }

  return su2_trace(A[0])/2.0;
}

double wloop_mean(int wR, int wT, const su2_matrix* links){
  double mean = 0;
  int site[4];
  for(int z=0;z<N;z++){
    for(int y=0;y<N;y++){
      for(int x=0;x<N;x++){
        for(int t=0;t<N;t++){
            site[0]=t; site[1]=x; site[2]=y; site[3]=z;
            for(int mu=1;mu<4;mu++){
              mean += wloop(wR,wT,mu,links,site);
            }
        }
      }
    }
  }
  return mean/ (3*N*N*N*N); // 3 directions & N^3 sites
}

// end of Wilson loop ------------------------------------------------------


/** @file matrix3d.c
 *  @author T J Atherton
 *
 *  @brief Matrix math for 3d graphics
 */

#include "matrix3d.h"
#include <math.h>
#include <string.h>

/** Use Apple's Accelerate library for LAPACK and BLAS */
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#include <lapacke.h>
#define USE_LAPACKE
#endif

#define EPS 1e-16

/** @brief Normalizes a vector
 * @param[in] in - input vector
 * @param[out] out - output vector. */
void mat3d_vectornormalize(vec3 in, vec3 out) {
    float norm=cblas_snrm2(3, in, 1);
    if (norm>EPS) norm = 1.0/norm;
    if (out!=in) cblas_scopy(3, in, 1, out, 1);
    cblas_sscal(3, norm, out, 1);
}

/** @brief Stores the identity matrix in out.
 * @param[out] out - output matrix. */
void mat3d_identity4x4(mat4x4 out) {
    static float ident[] = { 1.0f, 0.0f, 0.0f, 0.0f,
                            0.0f, 1.0f, 0.0f, 0.0f,
                            0.0f, 0.0f, 1.0f, 0.0f,
                            0.0f, 0.0f, 0.0f, 1.0f };
    cblas_scopy(16, ident, 1, out, 1);
}

/** @brief Stores the identity matrix in out.
 * @param[out] out - output matrix. */
void mat3d_identity3x3(mat4x4 out) {
    static float ident[] = { 1.0f, 0.0f, 0.0f,
                             0.0f, 1.0f, 0.0f,
                             0.0f, 0.0f, 1.0f };
    cblas_scopy(9, ident, 1, out, 1);
}

/** @brief Multiply out = a*b
 * @param[in] a input matrix
 * @param[in] b input matrix
 * @param[out] out filled with a*b
 * @warning: out must be distinct from a and b */
void mat3d_mul4x4(mat4x4 a, mat4x4 b, mat4x4 out) {
    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 4, 4, 4, 1.0, a, 4, b, 4, 0.0, out, 4);
}

/** @brief Multiply: out = a*b
 * @param[in] a input matrix
 * @param[in] b input matrix
 * @param[out] out filled with a*b
 * @warning: out must be distinct from a and b */
void mat3d_mul3x3(mat3x3 a, mat3x3 b, mat3x3 out) {
    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, 3, 3, 3, 1.0, a, 3, b, 3, 0.0, out, 3);
}

/** @brief Add with scale: out = a + alpha*b
 * @param[in] a input matrix
 * @param[in] b input matrix
 * @param[out] out filled with a + alpha*b  */
void mat3d_addscale3x3(mat3x3 a, float alpha, mat3x3 b, mat3x3 out) {
    if (a!=out) cblas_scopy(9, a, 1, out, 1);
    cblas_saxpy(9, alpha, b, 1, out, 1);
}

/** @brief Copy: out = a
 * @param[in] a input matrix
 * @param[out] out filled with a*b */
void mat3d_copy4x4(mat4x4 a, mat4x4 out) {
    cblas_scopy(16, a, 1, out, 1);
}

/** @brief Matrix inversion
 * @param[in] a input matrix
 * @param[out] out filled with inverse(a)  */
void mat3d_invert4x4(mat4x4 a, mat4x4 out) {
    int m = 4, n = 4;
    int piv[4];
    int info;
    /* Copy a into out */
    memcpy(out, a, sizeof(float)*16);
    /* Compute LU decomposition, storing result in place */
#ifdef USE_LAPACKE
    info = LAPACKE_sgetrf(LAPACK_COL_MAJOR, m, n, out, m, piv);
#else
    sgetrf_(&m, &n, out, &m, piv, &info);
#endif

    if (!info) {
        /* Now compute inverse */
#ifdef USE_LAPACKE
        info=LAPACKE_sgetri(LAPACK_COL_MAJOR, n, out, n, piv);
#else
        float work[16];
        int lwork=16;
        sgetri_(&n, out, &n, piv, work, &lwork, &info);
#endif
    }
}

/** @brief Convert a 3x3 matrix to a 4x4 matrix
 * @param[in] in input matrix
 * @param[out] out filled with inverse(a)  */
void mat3d_lift(mat3x3 in, mat4x4 out) {
    mat4x4 new = { in[0], in[1], in[2], 0.0f, // Col major order!
                   in[3], in[4], in[5], 0.0f,
                   in[6], in[7], in[8], 0.0f,
                    0.0f,  0.0f,  0.0f, 1.0f };
    cblas_scopy(16, new, 1, out, 1);
}

/** @brief Print a 3x3 matrix */
void mat3d_print3x3(mat3x3 in) {
    for (unsigned int j=0; j<3; j++) { // row
        printf("[ ");
        for (unsigned int i=0; i<3; i++) { // column
            printf("%g ", in[i*3+j]);
        }
        printf("]\n");
    }
}

/** @brief Print a 3x3 matrix */
void mat3d_print4x4(mat4x4 in) {
    for (unsigned int j=0; j<4; j++) { // row
        printf("[ ");
        for (unsigned int i=0; i<4; i++) { // column
            printf("%g ", in[i*4+j]);
        }
        printf("]\n");
    }
}

/** @brief Translate by a vector
 * @param[in] in input matrix
 * @param[in] vec translation vector
 * @param[out] out on output, contains T*in where T is the translation matrix computed from vec */
void mat3d_translate(mat4x4 in, vec3 vec, mat4x4 out) {
    mat4x4 tr = { 1.0f, 0.0f, 0.0f, 0.0f, // Col major order!
                  0.0f, 1.0f, 0.0f, 0.0f,
                  0.0f, 0.0f, 1.0f, 0.0f,
                  vec[0], vec[1], vec[2], 1.0f };
    mat4x4 in2;
    if (in==out) mat3d_copy4x4(in, in2); /* Use a copy if in and out are the same matrix */
    if (in) mat3d_mul4x4(tr, (in==out ? in2 : in), out);
    else mat3d_copy4x4(tr, out);
}

/** @brief Scale by a factor
 * @param[in] in input matrix
 * @param[in] scale scale factor
 * @param[out] out on output, contains T*in where T is the translation matrix computed from vec */
void mat3d_scale(mat4x4 in, float scale, mat4x4 out) {
    mat4x4 tr = { scale, 0.0f, 0.0f, 0.0f, // Col major order!
                  0.0f, scale, 0.0f, 0.0f,
                  0.0f, 0.0f, scale, 0.0f,
                  0.0f, 0.0f,  0.0f, 1.0f };
    mat4x4 in2;
    if (in==out) mat3d_copy4x4(in, in2); /* Use a copy if in and out are the same matrix */
    if (in) mat3d_mul4x4(tr, (in==out ? in2 : in), out);
    else mat3d_copy4x4(tr, out);
}

/** @brief Rotate by angle around an axis
 * @param[in] in input matrix
 * @param[in] axis rotation axis
 * @param[in] angle rotation angle
 * @param[out] out on output, contains R*in where R is the translation matrix computed from vec */
void mat3d_rotate(mat4x4 in, vec3 axis, float angle, mat4x4 out) {
    vec3 u;
    mat3x3 rot;
    mat4x4 rot4;
    mat4x4 in2;
    if (in==out) mat3d_copy4x4(in, in2); /* Use a copy if in and out are the same matrix */

    /* Construct rotation matrix from Rodrigues formula */
    mat3d_vectornormalize(axis, u);
    mat3x3 w = { 0.0f,  u[2], -u[1],  // Col major order
                -u[2],  0.0f,  u[0],
                 u[1], -u[0],  0.0f };
    mat3x3 w2;

    /* Rodrigues formula: R = I + sin(a) W + 2 sin(a/2)^2 W^2 */
    mat3d_identity3x3(rot);
    mat3d_addscale3x3(rot, sin(angle), w, rot);
    mat3d_mul3x3(w, w, w2);
    float phi = sin(angle/2);
    mat3d_addscale3x3(rot, 2*phi*phi, w2, rot);

    /* Convert to 4x4 matrix */
    mat3d_lift(rot, rot4);

    /* Multiply the input matrix by this */
    if (in) mat3d_mul4x4(rot4, (in==out ? in2 : in), out);
    else mat3d_copy4x4(rot4, out);
}

/** @brief Orthographic projection matrix
 * @param[in] in input matrix
 * @param[in] left     } Bounds of the viewing area
 * @param[in] right   }
 * @param[in] bottom }
 * @param[in] top        }
 * @param[in] near      }
 * @param[in] far        }
 * @param[out] out on output, contains R*in where R is the translation matrix computed from vec */
void mat3d_ortho(mat4x4 in, mat4x4 out, float left, float right, float bottom, float top, float near, float far) {
    mat4x4 pr = { 2.0f/(right-left), 0.0f, 0.0f, 0.0f, // Col major order!
                  0.0f, 2.0f/(top-bottom), 0.0f, 0.0f,
                  0.0f, 0.0f, -2.0f/(far-near), 0.0f,
                  0.0f, 0.0f, 0.0f, 1.0f };
    mat4x4 in2;
    if (in==out) mat3d_copy4x4(in, in2); /* Use a copy if in and out are the same matrix */

    /* Multiply the input matrix by this */
    if (in) mat3d_mul4x4(pr, (in==out ? in2 : in), out);
    else mat3d_copy4x4(pr, out);
}

/** @brief Perspective projection matrix
 * @param[in] in input matrix
 * @param[in] left     } Bounds of the viewing area
 * @param[in] right   }
 * @param[in] bottom }
 * @param[in] top        }
 * @param[in] near      }
 * @param[in] far        }
 * @param[out] out on output, contains R*in where R is the translation matrix computed from vec */
void mat3d_frustum(mat4x4 in, mat4x4 out, float left, float right, float bottom, float top, float near, float far) {
    mat4x4 pr = { 2*near/(right-left), 0.0f, 0.0f, 0.0f, // Col major order!
                  0.0f, 2*near/(top-bottom), 0.0f, 0.0f,
                  (right+left)/(right-left), (top+bottom)/(top-bottom), -(far+near)/(far-near), -1.0f,
                  0.0f, 0.0f, -2*far*near/(far-near), 0.0f };
    mat4x4 in2;
    if (in==out) mat3d_copy4x4(in, in2); /* Use a copy if in and out are the same matrix */

    /* Multiply the input matrix by this */
    if (in) mat3d_mul4x4(pr, (in==out ? in2 : in), out);
    else mat3d_copy4x4(pr, out);
}


/*
 * test_shaw.c
 *
 * Test L-curve (Tikhonov) regression routines using Shaw
 * problem. See example 1.10 of
 *
 * [1] R.C. Aster, B. Borchers and C. H. Thurber,
 *     Parameter Estimation and Inverse Problems (2nd ed), 2012.
 */

#include <gsl/gsl_sf_trig.h>

/* alternate (and inefficient) method of computing G(lambda) */
static double
shaw_gcv_G(const double lambda, const gsl_matrix * X, const gsl_vector * y,
           gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;
  gsl_matrix * XTX = gsl_matrix_alloc(p, p);
  gsl_matrix * XI = gsl_matrix_alloc(p, n);
  gsl_matrix * XXI = gsl_matrix_alloc(n, n);
  gsl_vector * c = gsl_vector_alloc(p);
  gsl_vector_view d;
  double rnorm, snorm;
  double term1, term2, G;
  size_t i;

  /* compute regularized solution with this lambda */
  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute X^T X */
  gsl_blas_dsyrk(CblasLower, CblasTrans, 1.0, X, 0.0, XTX);

  /* add lambda*I */
  d = gsl_matrix_diagonal(XTX);
  gsl_vector_add_constant(&d.vector, lambda * lambda);

  /* invert (X^T X + lambda*I) */
  gsl_linalg_cholesky_decomp1(XTX);
  gsl_linalg_cholesky_invert(XTX);
  gsl_matrix_transpose_tricpy('L', 0, XTX, XTX);

  /* XI = (X^T X + lambda*I)^{-1} X^T */
  gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, XTX, X, 0.0, XI);

  /* XXI = X (X^T X + lambda*I)^{-1} X^T */
  gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, X, XI, 0.0, XXI);

  /* compute: term1 = Tr(I - X XI) */
  term1 = 0.0;
  for (i = 0; i < n; ++i)
    {
      double *Ai = gsl_matrix_ptr(XXI, i, i);
      term1 += 1.0 - (*Ai);
    }

  gsl_matrix_free(XTX);
  gsl_matrix_free(XI);
  gsl_matrix_free(XXI);
  gsl_vector_free(c);

  term2 = rnorm / term1;

  return term2 * term2;;
}

/* construct design matrix and rhs vector for Shaw problem */
static int
shaw_system(gsl_matrix * X, gsl_vector * y)
{
  int s = GSL_SUCCESS;
  const size_t n = X->size1;
  const size_t p = X->size2;
  const double dtheta = M_PI / (double) p;
  size_t i, j;
  gsl_vector *m = gsl_vector_alloc(p);

  /* build the design matrix */
  for (i = 0; i < n; ++i)
    {
      double si = (i + 0.5) * M_PI / n - M_PI / 2.0;
      double csi = cos(si);
      double sni = sin(si);

      for (j = 0; j < p; ++j)
        {
          double thetaj = (j + 0.5) * M_PI / p - M_PI / 2.0;
          double term1 = csi + cos(thetaj);
          double term2 = gsl_sf_sinc(sni + sin(thetaj));
          double Xij = term1 * term1 * term2 * term2 * dtheta;

          gsl_matrix_set(X, i, j, Xij);
        }
    }

  /* construct coefficient vector */
  {
    const double a1 = 2.0;
    const double a2 = 1.0;
    const double c1 = 6.0;
    const double c2 = 2.0;
    const double t1 = 0.8;
    const double t2 = -0.5;

    for (j = 0; j < p; ++j)
      {
        double tj = -M_PI / 2.0 + (j + 0.5) * dtheta;
        double mj = a1 * exp(-c1 * (tj - t1) * (tj - t1)) +
                    a2 * exp(-c2 * (tj - t2) * (tj - t2));
        gsl_vector_set(m, j, mj);
      }
  }

  /* construct rhs vector */
  gsl_blas_dgemv(CblasNoTrans, 1.0, X, m, 0.0, y);

  gsl_vector_free(m);

  return s;
}

static int
test_shaw_system_l(gsl_rng *rng_p, const size_t n, const size_t p,
                   const double lambda_expected,
                   gsl_vector *rhs)
{
  const size_t npoints = 1000; /* number of points on L-curve */
  const double tol1 = 1.0e-12;
  const double tol2 = 1.0e-9;
  const double tol3 = 1.0e-5;
  gsl_vector * reg_param = gsl_vector_alloc(npoints);
  gsl_vector * rho = gsl_vector_alloc(npoints);
  gsl_vector * eta = gsl_vector_alloc(npoints);

  gsl_matrix * X = gsl_matrix_alloc(n, p);
  gsl_matrix * cov = gsl_matrix_alloc(p, p);
  gsl_vector * c = gsl_vector_alloc(p);
  gsl_vector * ytmp = gsl_vector_alloc(n);
  gsl_vector * y;
  gsl_vector * r = gsl_vector_alloc(n);
  gsl_multifit_linear_workspace * work =
    gsl_multifit_linear_alloc (n, p);

  size_t reg_idx, i;
  double lambda, rnorm, snorm;

  /* build design matrix */
  shaw_system(X, ytmp);

  if (rhs)
    y = rhs;
  else
    {
      y = ytmp;

      /* add random noise to exact rhs vector */
      test_random_vector_noise(rng_p, y);
    }

  /* SVD decomposition */
  gsl_multifit_linear_svd(X, work);

  /* calculate L-curve */
  gsl_multifit_linear_lcurve(y, reg_param, rho, eta, work);

  /* test rho and eta vectors */
  for (i = 0; i < npoints; ++i)
    {
      double rhoi = gsl_vector_get(rho, i);
      double etai = gsl_vector_get(eta, i);
      double lami = gsl_vector_get(reg_param, i);

      /* solve regularized system and check for consistent rho/eta values */
      gsl_multifit_linear_solve(lami, X, y, c, &rnorm, &snorm, work);
      gsl_test_rel(rhoi, rnorm, tol3, "shaw rho n=%zu p=%zu lambda=%e",
                   n, p, lami);
      gsl_test_rel(etai, snorm, tol1, "shaw eta n=%zu p=%zu lambda=%e",
                   n, p, lami);
    }

  /* calculate corner of L-curve */
  gsl_multifit_linear_lcorner(rho, eta, &reg_idx);

  lambda = gsl_vector_get(reg_param, reg_idx);

  /* test against known lambda value if given */
  if (lambda_expected > 0.0)
    {
      gsl_test_rel(lambda, lambda_expected, tol1,
                   "shaw: n=%zu p=%zu L-curve corner lambda",
                   n, p);
    }

  /* compute regularized solution with optimal lambda */
  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute residual norm ||y - X c|| */
  gsl_vector_memcpy(r, y);
  gsl_blas_dgemv(CblasNoTrans, 1.0, X, c, -1.0, r);

  /* test rnorm value */
  gsl_test_rel(rnorm, gsl_blas_dnrm2(r), tol2,
               "shaw: n=%zu p=%zu rnorm", n, p);

  /* test snorm value */
  gsl_test_rel(snorm, gsl_blas_dnrm2(c), tol2,
               "shaw: n=%zu p=%zu snorm", n, p);

  gsl_matrix_free(X);
  gsl_matrix_free(cov);
  gsl_vector_free(reg_param);
  gsl_vector_free(rho);
  gsl_vector_free(eta);
  gsl_vector_free(r);
  gsl_vector_free(c);
  gsl_vector_free(ytmp);
  gsl_multifit_linear_free(work);

  return 0;
} /* test_shaw_system_l() */

static int
test_shaw_system_gcv(gsl_rng *rng_p, const size_t n, const size_t p,
                     const double lambda_expected,
                     gsl_vector *rhs)
{
  const size_t npoints = 200; /* number of points on L-curve */
  const double tol1 = 1.0e-12;
  const double tol2 = 1.4e-10;
  const double tol3 = 1.0e-5;
  gsl_vector * reg_param = gsl_vector_alloc(npoints);
  gsl_vector * G = gsl_vector_alloc(npoints);

  gsl_matrix * X = gsl_matrix_alloc(n, p);
  gsl_matrix * cov = gsl_matrix_alloc(p, p);
  gsl_vector * c = gsl_vector_alloc(p);
  gsl_vector * ytmp = gsl_vector_alloc(n);
  gsl_vector * y;
  gsl_vector * r = gsl_vector_alloc(n);
  gsl_multifit_linear_workspace * work =
    gsl_multifit_linear_alloc (n, p);

  size_t reg_idx, i;
  double lambda, rnorm, snorm, G_lambda;

  /* build design matrix */
  shaw_system(X, ytmp);

  if (rhs)
    y = rhs;
  else
    {
      y = ytmp;

      /* add random noise to exact rhs vector */
      test_random_vector_noise(rng_p, y);
    }

  /* SVD decomposition */
  gsl_multifit_linear_svd(X, work);

  /* calculate GCV curve */
  gsl_multifit_linear_gcv(y, reg_param, G, &lambda, &G_lambda, work);

  /* test G vector */
  for (i = 0; i < npoints; ++i)
    {
      double lami = gsl_vector_get(reg_param, i);

      if (lami > 1.0e-5)
        {
          /* test unreliable for small lambda */
          double Gi = gsl_vector_get(G, i);
          double Gi_expected = shaw_gcv_G(lami, X, y, work);

          gsl_test_rel(Gi, Gi_expected, tol3, "shaw[%zu,%zu] gcv G i=%zu lambda=%e",
                       n, p, i, lami);
        }
    }

  /* test against known lambda value if given */
  if (lambda_expected > 0.0)
    {
      gsl_test_rel(lambda, lambda_expected, tol2,
                   "shaw gcv: n=%zu p=%zu lambda",
                   n, p);
    }

  /* compute regularized solution with optimal lambda */
  gsl_multifit_linear_solve(lambda, X, y, c, &rnorm, &snorm, work);

  /* compute residual norm ||y - X c|| */
  gsl_vector_memcpy(r, y);
  gsl_blas_dgemv(CblasNoTrans, 1.0, X, c, -1.0, r);

  /* test rnorm value */
  gsl_test_rel(rnorm, gsl_blas_dnrm2(r), tol2,
               "shaw gcv: n=%zu p=%zu rnorm", n, p);

  /* test snorm value */
  gsl_test_rel(snorm, gsl_blas_dnrm2(c), tol2,
               "shaw gcv: n=%zu p=%zu snorm", n, p);

  gsl_matrix_free(X);
  gsl_matrix_free(cov);
  gsl_vector_free(reg_param);
  gsl_vector_free(G);
  gsl_vector_free(r);
  gsl_vector_free(c);
  gsl_vector_free(ytmp);
  gsl_multifit_linear_free(work);

  return 0;
} /* test_shaw_system_gcv() */

void
test_shaw(void)
{
  gsl_rng * r = gsl_rng_alloc(gsl_rng_default);

  {
    double shaw20_y[] = {
      8.7547455124379323e-04, 5.4996835885761936e-04,
      1.7527999407005367e-06, 1.9552372913117047e-03,
      1.4411645433785081e-02, 5.2800013336393704e-02,
      1.3609152023257112e-01, 2.7203484587635818e-01,
      4.3752225136193390e-01, 5.7547667319875240e-01,
      6.2445052213539942e-01, 5.6252658286441348e-01,
      4.2322239923561566e-01, 2.6768469219560631e-01,
      1.4337901162734543e-01, 6.5614569346074361e-02,
      2.6013851831752945e-02, 9.2336933089481269e-03,
      3.2269066658993694e-03, 1.3999201459261811e-03 };
    gsl_vector_view rhs = gsl_vector_view_array(shaw20_y, 20);

    /* lambda and rhs values from [1] */
    test_shaw_system_l(r, 20, 20, 5.793190958069266e-06, &rhs.vector);

    test_shaw_system_gcv(r, 20, 20, 1.24921780949051038e-05, &rhs.vector);
  }

  {
    size_t n, p;

    for (n = 10; n <= 50; n += 2)
      {
        for (p = n - 6; p <= n; p += 2)
          test_shaw_system_l(r, n, p, -1.0, NULL);
      }
  }

  gsl_rng_free(r);
} /* test_shaw() */


/*
Performing approximate Bayesian computation sequential Monte Carlo (Toni et al.
2009) for linear regression.

Synthetic data is generated by `ground_truth_and_analysis.ipynb`.

This script writes particles.csv, where each row corresponds to a particle and
each column corresponds to a round of SMC.

Defining #DEBUG_MODE will silence all writing to stdout. One may then add
printf statements in the code, and perhaps write the output to file as:
`./run.sh > output.txt`

Parameter ordering convention:
0 - gradient
1 - intercept
2 - standard deviation

Author: Juvid Aryaman
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_fit.h>

#define N_DATA 30
#define N_PARAMETERS 3

#define N_PARTICLES 2000
#define N_ROUNDS_SMC 25
#define QUANTILE_ACCEPT_DISTANCE 0.8

#define RND gsl_rng_uniform(r)
#define SEED 1
#define DISTANCE_THRESHOLD_INIT_GRADIENT 2
#define DISTANCE_THRESHOLD_INIT_INTERCEPT 50
#define DISTANCE_THRESHOLD_INIT_SIGMA 2

#define X_DATA_FILENAME "x.csv"
#define Y_DATA_FILENAME "y.csv"

//#define DEBUG_MODE

#include "smc.h"
#include "lin_reg.h"

int main(int argc, char *argv[]) {

/* set up GSL RNG */
gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
/* end of GSL setup */

gsl_rng_set(r, SEED);

/////////////////////////
/*Read data*/
/////////////////////////

FILE *data_pointer_x, *data_pointer_y;

data_pointer_x = fopen(X_DATA_FILENAME, "r");
data_pointer_y = fopen(Y_DATA_FILENAME, "r");

double data_x[N_DATA];
double data_y[N_DATA];
int i, j, read_error_status_x, read_error_status_y;
for (i=0; i < N_DATA; i++){
	read_error_status_x = fscanf(data_pointer_x, "%lf\n", &data_x[i]);
	read_error_status_y = fscanf(data_pointer_y, "%lf\n", &data_y[i]);
}
if (read_error_status_x != 1){printf("Error reading X data\n"); return 0;}
if (read_error_status_y != 1){printf("Error reading Y data\n"); return 0;}

/////////////////////////
/*Initialise variables*/
/////////////////////////

/*Fit a linear model to the data, which will be used as summary statistics of
the data*/
double gradient_fit_data, intercept_fit_data, sigma_fit_data, cov00, cov01, cov11, sumsq;
int gsl_fit_return_value;
gsl_fit_return_value = gsl_fit_linear(data_x, 1, data_y, 1, N_DATA,
																			&intercept_fit_data, &gradient_fit_data,
																			&cov00, &cov01, &cov11, &sumsq);
if (gsl_fit_return_value != 0) {printf("Fit failed.\n"); return -1;}
sigma_fit_data = sqrt(sumsq/(N_DATA-2));

#ifndef DEBUG_MODE
	printf("gradient ML = %.8f\n", gradient_fit_data);
	printf("intercept ML = %.8f\n", intercept_fit_data);
	printf("sigma ML = %.8f\n", sigma_fit_data);
#endif

/*Make a (N_PARAMETERS X N_ROUNDS_SMC X N_PARTICLES) array to store all
particles at all rounds of SMC*/
double*** theta_particle;
theta_particle = (double***) malloc(N_PARAMETERS * sizeof(double**));
for (i = 0; i < N_PARAMETERS; i++){
	theta_particle[i] = (double**) malloc(N_ROUNDS_SMC * sizeof(double*));
}
for (i = 0; i < N_PARAMETERS; i++){
	for (j = 0; j < N_ROUNDS_SMC; j++){
		theta_particle[i][j] = (double*) malloc(N_PARTICLES * sizeof(double));
	}
}

double** distance;
distance = (double**) malloc(N_PARAMETERS * sizeof(double*));
for (i = 0; i < N_PARAMETERS; i++) {
	distance[i] = (double*) malloc(N_PARTICLES * sizeof(double));
}
double **distance_threshold_all = malloc(N_PARAMETERS * sizeof(double*));
for (i = 0; i < N_PARAMETERS; i++) {
	distance_threshold_all[i] = (double*) malloc(N_ROUNDS_SMC * sizeof(double));
}

double distance_threshold[] = {DISTANCE_THRESHOLD_INIT_GRADIENT,
															 DISTANCE_THRESHOLD_INIT_INTERCEPT,
														   DISTANCE_THRESHOLD_INIT_SIGMA};

double *simulated_data = (double*) malloc(N_DATA * sizeof(double));
double *weight = malloc(N_PARTICLES * sizeof(double));

double weight_normalizer = 0.0;

int time_smc=0; // an index of each round of SMC
int param_index_chosen, prior_violated;
int particle_index;
/////////////////////////
/*Perform ABC SMC*/
/////////////////////////

/*For every round of SMC*/
for (time_smc = 0; time_smc < N_ROUNDS_SMC; time_smc++) {
	#ifndef DEBUG_MODE
		printf("Round %d of SMC\n", time_smc);
	#endif
	for (i = 0; i < N_PARAMETERS; i++) {
		distance_threshold_all[i][time_smc] = distance_threshold[i];
	}

	/*Draw or perturb a particle and compute distance*/
	for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
		// printf("%d\n", particle_index);
		for (i = 0; i < N_PARAMETERS; i++) {
			// reset distance of particle along each dimension
			distance[i][particle_index] = distance_threshold[i] + 1.0;
		}
		while ((distance[0][particle_index] > distance_threshold[0])||
					 (distance[1][particle_index] > distance_threshold[1])||
				   (distance[2][particle_index] > distance_threshold[2])) {
			if (time_smc == 0) {
				// Sample from the prior
				sample_prior(r, theta_particle, particle_index);
			}
			else{
				/*Sample from the old weights*/
				param_index_chosen = weighted_choice(r, weight);
				if ((param_index_chosen < 0)||(param_index_chosen >= N_PARTICLES)) {
					printf("Error in param_index_chosen\n");
					printf("time_smc = %d\n", time_smc);
					return -1;
				}

				perturb_particle(r, theta_particle, time_smc, param_index_chosen,
												 particle_index);

				// Check if prior support is 0
				prior_violated = check_prior_violated(theta_particle, time_smc,
																							particle_index);
				if(prior_violated == 1) continue;
				}

			simulate_dataset(r, theta_particle, data_x, simulated_data, time_smc,
											 particle_index);

			// Compute distance between data and simulation
			distance_metric_sum_stats(simulated_data,
															 data_x,
															 gradient_fit_data,
															 intercept_fit_data,
															 sigma_fit_data,
														   distance, particle_index);
			//distance[particle_index] = distance_metric_sum_res(simulated_data, data_y);
			// distance[particle_index] = distance_metric_sum_abs_res(simulated_data, data_y);

		}
	}

	#ifndef DEBUG_MODE
		printf("Particles sampled.\n");
	#endif

	/*Compute weights*/
	if (time_smc==0){ for (i = 0; i < N_PARTICLES; i++) weight[i] = 1.0;}
	else{
		weight_normalizer = 0.0;
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			weight_normalizer +=  weight[particle_index]*kernel_pdf();
		}
		// print_double_array(weight, N_PARTICLES);
		// printf("\n" );
		// printf("weight_normalizer=%f\n", weight_normalizer);
		// printf("kernel_pdf()=%f\n", kernel_pdf());
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			weight[particle_index] = prior_pdf(theta_particle, time_smc,
																				 particle_index)/weight_normalizer;
		}

	}

	/*Normalise weights*/
	weight_normalizer = 0.0;
	for (i = 0; i < N_PARTICLES; i++)	weight_normalizer += weight[i];
	for (i = 0; i < N_PARTICLES; i++)	weight[i] = weight[i]/weight_normalizer;

	/* Resample weights*/
	distance_threshold[0] = update_distance_threshold(distance[0]);
	distance_threshold[1] = update_distance_threshold(distance[1]);
	distance_threshold[2] = update_distance_threshold(distance[2]);

}

#ifndef DEBUG_MODE
	printf("Writing particles to file\n");
#endif
	write_particles_to_csv(theta_particle);

	char *dist_filename = "distances.txt";
	write_2d_double_array_to_csv(distance_threshold_all, N_PARAMETERS, N_ROUNDS_SMC, dist_filename);
#ifndef DEBUG_MODE
	printf("Done!\n");
#endif

return 0; //return from main
} //close main


/* -----------------------------------------------------------------------------
  JAM_AXI_VEL_LOSINT

    Calculates integrand for line-of-sight integral required for first moment
    calculation.

    INPUTS
      zp     : line-of-sight coordinate z' (integration variable)
      params : function parameters passed as a structure

    NOTES
      * Based on janis1_jeans_mge_los_integrand IDL code by Michele Cappellari.

  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
----------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"

double jam_axi_vel_losint(double zp, void *params) {

    struct params_losint *lp;
    struct params_mgeint mp;
    double xp, yp, si, ci, r, z, r2, z2, nu, intg, result, error, nu_i;
    double sign_kappa, sum;
    int i;

    // get parameters
    lp = params;
    xp = lp->xp;
    yp = lp->yp;

    // if the integrationFlag is already set, do not proceed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // intrinsic R and z
    si = sin(lp->incl);
    ci = cos(lp->incl);
    r = sqrt(pow(zp*si - yp*ci, 2) + pow(xp, 2));                   // eqn 25
    z = sqrt(pow(zp*ci + yp*si, 2));

    // do some prep for the integrand to avoid repeat calculations
    r2 = r * r;
    z2 = z * z;

    // parameters for integrand function
    mp.r2 = r2;
    mp.z2 = z2;
    mp.pot = lp->pot;
    mp.s2p = lp->s2p;
    mp.e2p = lp->e2p;

    // perform integration
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);
    gsl_set_error_handler_off();
    gsl_function F;
    F.function = &jam_axi_vel_mgeint;

    sum = 0.;
    for (i=0; i<lp->lum->ntotal; i++) {
        if (lp->kappa[i]==0.) sign_kappa = 0.;
        else sign_kappa = lp->kappa[i]/fabs(lp->kappa[i]);
        nu_i = lp->lum->area[i] * exp(-0.5/lp->s2l[i]*(r2+z2/lp->q2l[i]));

        mp.bani = lp->bani[i];
        mp.s2l = lp->s2l[i];
        mp.q2l = lp->q2l[i];
        mp.s2q2l = lp->s2q2l[i];
        F.params = ∓
        *lp->integrationFlag += gsl_integration_qag(&F, 0., 1., 0., 1e-5,
            1000, 6, w, &result, &error);
        sum += sign_kappa * pow(lp->kappa[i], 2) * nu_i * fabs(result);
    }

    gsl_integration_workspace_free(w);

    // check if the integration failed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // mge volume density
    nu = mge_dens(lp->lum, r, z);

    // keep track of kappa signs - see note 8 p77 of Cappellari 2008
    intg = nu*sum/fabs(nu*sum)*sqrt(fabs(nu*sum));

    intg *= pow(zp, lp->zpow);

    return intg;

}


/* roots/brent.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Reid Priedhorsky, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/* brent.c -- brent root finding algorithm */

#include <config.h>

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double a, b, c, d, e;
    double fa, fb, fc;
  }
brent_state_t;

static int brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int
brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper) ;

  SAFE_FUNC_CALL (f, x_lower, &f_lower);
  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->a = x_lower;
  state->fa = f_lower;

  state->b = x_upper;
  state->fb = f_upper;

  state->c = x_upper;
  state->fc = f_upper;

  state->d = x_upper - x_lower ;
  state->e = x_upper - x_lower ;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))
    {
      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);
    }

  return GSL_SUCCESS;

}

static int
brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double tol, m;

  int ac_equal = 0;

  double a = state->a, b = state->b, c = state->c;
  double fa = state->fa, fb = state->fb, fc = state->fc;
  double d = state->d, e = state->e;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      ac_equal = 1;
      c = a;
      fc = fa;
      d = b - a;
      e = b - a;
    }

  if (fabs (fc) < fabs (fb))
    {
      ac_equal = 1;
      a = b;
      b = c;
      c = a;
      fa = fb;
      fb = fc;
      fc = fa;
    }

  tol = 0.5 * GSL_DBL_EPSILON * fabs (b);
  m = 0.5 * (c - b);

  if (fb == 0)
    {
      *root = b;
      *x_lower = b;
      *x_upper = b;

      return GSL_SUCCESS;
    }

  if (fabs (m) <= tol)
    {
      *root = b;

      if (b < c)
        {
          *x_lower = b;
          *x_upper = c;
        }
      else
        {
          *x_lower = c;
          *x_upper = b;
        }

      return GSL_SUCCESS;
    }

  if (fabs (e) < tol || fabs (fa) <= fabs (fb))
    {
      d = m;            /* use bisection */
      e = m;
    }
  else
    {
      double p, q, r;   /* use inverse cubic interpolation */
      double s = fb / fa;

      if (ac_equal)
        {
          p = 2 * m * s;
          q = 1 - s;
        }
      else
        {
          q = fa / fc;
          r = fb / fc;
          p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));
          q = (q - 1) * (r - 1) * (s - 1);
        }

      if (p > 0)
        {
          q = -q;
        }
      else
        {
          p = -p;
        }

      if (2 * p < GSL_MIN (3 * m * q - fabs (tol * q), fabs (e * q)))
        {
          e = d;
          d = p / q;
        }
      else
        {
          /* interpolation failed, fall back to bisection */

          d = m;
          e = m;
        }
    }

  a = b;
  fa = fb;

  if (fabs (d) > tol)
    {
      b += d;
    }
  else
    {
      b += (m > 0 ? +tol : -tol);
    }

  SAFE_FUNC_CALL (f, b, &fb);

  state->a = a ;
  state->b = b ;
  state->c = c ;
  state->d = d ;
  state->e = e ;
  state->fa = fa ;
  state->fb = fb ;
  state->fc = fc ;

  /* Update the best estimate of the root and bounds on each
     iteration */

  *root = b;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      c = a;
    }

  if (b < c)
    {
      *x_lower = b;
      *x_upper = c;
    }
  else
    {
      *x_lower = c;
      *x_upper = b;
    }

  return GSL_SUCCESS ;
}

static const gsl_root_fsolver_type brent_type =
{"brent",                               /* name */
 sizeof (brent_state_t),
 &brent_init,
 &brent_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_brent = &brent_type;


#include <ceed.h>
#include <petsc.h>
#include "../problems/mooney-rivlin.h"

// Build libCEED context object
PetscErrorCode PhysicsContext_MR(MPI_Comm comm, Ceed ceed, Units *units,
                                 CeedQFunctionContext *ctx) {
  PetscErrorCode ierr;
  Physics_MR phys;

  PetscFunctionBegin;

  ierr = PetscMalloc1(1, units); CHKERRQ(ierr);
  ierr = PetscMalloc1(1, &phys); CHKERRQ(ierr);
  ierr = ProcessPhysics_MR(comm, phys, *units); CHKERRQ(ierr);
  CeedQFunctionContextCreate(ceed, ctx);
  CeedQFunctionContextSetData(*ctx, CEED_MEM_HOST, CEED_COPY_VALUES,
                              sizeof(*phys), phys);
  ierr = PetscFree(phys); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// Build libCEED smoother context object
PetscErrorCode PhysicsSmootherContext_MR(MPI_Comm comm, Ceed ceed,
    CeedQFunctionContext ctx, CeedQFunctionContext *ctx_smoother) {
  PetscErrorCode ierr;
  PetscScalar nu_smoother = 0;
  PetscBool nu_flag = PETSC_FALSE;
  Physics_MR phys, phys_smoother;

  PetscFunctionBegin;

  ierr = PetscOptionsBegin(comm, NULL,
                           "Mooney Rivlin physical parameters for smoother", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-nu_smoother", "Poisson's ratio for smoother",
                            NULL, nu_smoother, &nu_smoother, &nu_flag);
  CHKERRQ(ierr);
  if (nu_smoother < 0 ||
      nu_smoother >= 0.5) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                    "Mooney-Rivlin model requires Poisson ratio -nu option in [0, .5)");

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  if (nu_flag) {
    // Copy context
    CeedQFunctionContextGetData(ctx, CEED_MEM_HOST, &phys);
    ierr = PetscMalloc1(1, &phys_smoother); CHKERRQ(ierr);
    ierr = PetscMemcpy(phys_smoother, phys, sizeof(*phys)); CHKERRQ(ierr);
    CeedQFunctionContextRestoreData(ctx, &phys);
    // Create smoother context
    CeedQFunctionContextCreate(ceed, ctx_smoother);
    phys_smoother->lambda = 2 * (phys_smoother->mu_1 + phys_smoother->mu_2) *
                            nu_smoother / (1 - 2*nu_smoother);
    CeedQFunctionContextSetData(*ctx_smoother, CEED_MEM_HOST, CEED_COPY_VALUES,
                                sizeof(*phys_smoother), phys_smoother);
    ierr = PetscFree(phys_smoother); CHKERRQ(ierr);
  } else {
    *ctx_smoother = NULL;
  }

  PetscFunctionReturn(0);
}

// Process physics options - Mooney-Rivlin
PetscErrorCode ProcessPhysics_MR(MPI_Comm comm, Physics_MR phys, Units units) {
  PetscErrorCode ierr;
  PetscReal nu = -1;
  phys->mu_1 = -1;
  phys->mu_2 = -1;
  phys->lambda = -1;
  units->meter     = 1;        // 1 meter in scaled length units
  units->second    = 1;        // 1 second in scaled time units
  units->kilogram  = 1;        // 1 kilogram in scaled mass units

  PetscFunctionBeginUser;

  ierr = PetscOptionsBegin(comm, NULL, "Mooney Rivlin physical parameters", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-mu_1", "Material Property mu_1", NULL,
                            phys->mu_1, &phys->mu_1, NULL); CHKERRQ(ierr);
  if (phys->mu_1 < 0) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                "Mooney-Rivlin model requires non-negative -mu_1 option (Pa)");

  ierr = PetscOptionsScalar("-mu_2", "Material Property mu_2", NULL,
                            phys->mu_2, &phys->mu_2, NULL); CHKERRQ(ierr);
  if (phys->mu_2 < 0) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                "Mooney-Rivlin model requires non-negative -mu_2 option (Pa)");

  ierr = PetscOptionsScalar("-nu", "Poisson ratio", NULL,
                            nu, &nu, NULL); CHKERRQ(ierr);
  if (nu < 0 || nu >= 0.5) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                     "Mooney-Rivlin model requires Poisson ratio -nu option in [0, .5)");
  phys->lambda = 2 * (phys->mu_1 + phys->mu_2) * nu / (1 - 2*nu);

  ierr = PetscOptionsScalar("-units_meter", "1 meter in scaled length units",
                            NULL, units->meter, &units->meter, NULL);
  CHKERRQ(ierr);
  units->meter = fabs(units->meter);

  ierr = PetscOptionsScalar("-units_second", "1 second in scaled time units",
                            NULL, units->second, &units->second, NULL);
  CHKERRQ(ierr);
  units->second = fabs(units->second);

  ierr = PetscOptionsScalar("-units_kilogram", "1 kilogram in scaled mass units",
                            NULL, units->kilogram, &units->kilogram, NULL);
  CHKERRQ(ierr);
  units->kilogram = fabs(units->kilogram);

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  // Define derived units
  units->Pascal = units->kilogram / (units->meter * PetscSqr(units->second));

  // Scale material parameters based on units of Pa
  phys->mu_1 *= units->Pascal;
  phys->mu_2 *= units->Pascal;
  phys->lambda *= units->Pascal;

  PetscFunctionReturn(0);
};