#ifndef __DUMMY_H__
#define __DUMMY_H__
#include<stdio.h>
//#define NDEBUG
#include<stdint.h>
#include<omp.h>
#include<float.h>
#include<assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <time.h>
#include <math.h>
#include<string.h>
#include<dirent.h>
#include<sys/stat.h>
#include<sys/param.h>
#include<sys/types.h>
#include<signal.h>
//GSL library
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>
#include<gsl/gsl_matrix.h>
#include <utility>
#include <string>
#include <iostream>
#include<iterator>
#include<map>
#include<vector>
#include <cctype>
#include<algorithm>
#include<climits>
using namespace std ;

const uint16_t loop_max=500;
uint16_t loop=0;
const double  epsilon= 0.05; //error associated with user variable !
const double  epsilon_m=    0.0000001; //error associated with user variable !
const double epsilon_phi=   0.0000001; //error associated with user variable !
const double epsilon_theta= 0.0000001; //error associated with user variable !
const double lower_limit_theta= 0.0000000001;
const double lower_limit_m =    0.0000000001;
const double lower_limit_phi =    0.000000001;
clock_t  start;
clock_t  end;
gsl_rng *rand_obj ;

double maxima_phi_i_possible=0.00000000000000000000000000000000000001;
double log_Fx_maxima_phi_i_possible=-DBL_MAX;

double * diversity;
uint32_t *freq_single_den; //  freqency of theta_w in denominator

typedef struct {
  bool* word_caps; // keeps tag if word at this pos was
  bool* comma_present_before;
  bool* period_present_before;
  vector<string> word_list ;
  uint32_t *int_word_list ; // Its an array to words
  uint32_t  *topic_usage;
  int real_word_count ; //Number of words in document
  int true_label ;
  int sampled_label;
  uint8_t *topic_list ; //Topic associated with each word
  char* document_name ;
  bool* priority_flag ;
  bool  hasPriorityWord;
  int mygamma;
}document ;

#define topic_count 5   // 1 to 30 !
#define document_count 790
#define total_words_count 10000

document *d_ptr ;
//documet
//Number of topics defined in begining
int updated_doc=0 ;

//GIBBS  samples count
//
int gibbs_counter=0;
const int gibbs_sample_count =25 ;
double *M_gibbs  ;
double *THETA_gibbs  ;
double *PHI_gibbs ;
void init_gibbs_params();
void write_model_gibbs();
void do_gibbs_expectation();
double alpha_comma_Y_gibbs=0;
double alpha_comma_N_gibbs=0;
double alpha_period_Y_gibbs=0;
double alpha_period_N_gibbs=0;
double alpha_upper_Y_gibbs=0;
double alpha_upper_N_gibbs=0;

double alpha_scale=1;
double alpha_shape=1;

const double  log_ratio=log(1000);

double *M ;
double *THETA ;
double *PHI;

//File names
const char* M_file="M.bin";
const char* THETA_file="THETA.bin";
const char* PHI_file="PHI.bin";
const char* TOPIC_file="topics_list.bin";

//For keeping last maxima , this will  speed up  search
double *last_maxima_theta_iw;
double *last_maxima_m_ij;
double *last_maxima_phi ;

//Thread info
const  int m_thread_cnt=8;
const  int theta_thread_cnt=8;

//Priors
const double m_diag=500;
const double m_nondiag=1;
const double m_beta=1;

const double theta_k=1;
const double theta_beta=1;
/*
double phi_sigma_square=10000;
double phi_sigma_square=1;
double phi_mu=0;
*/
//FUNCTIONS
//INIT  functions
void init(); //wrapper   for  all the inits
void init_model_parameters() ; //Inits model params
void init_document_params(); //INIT  document structure
void init_variables();
void init_topic_assignment(document* current_doc);
void test_model();
void free_things();

//Topic update for each document
inline void  update_topic_usage(document* current_doc_ptr, const uint8_t  new_topic,const int old_topic  );
void topic_update(document* current_doc_ptr);
void update_TOPIC();
double get_factor12(const int word_curr,const int word_next,const int t_prime,const int t_last,const int t_next,document* doc,const uint32_t mypos);
//double get_factor12(const uint32_t word_curr,const int word_next,const uint8_t t_prime,const int t_last,const int t_next,document* doc,const uint32_t mypos);
inline double get_factor2_t_i(document* current_doc_ptr, double sum);
double get_current_sum(document* doc,const uint32_t N_d, int sz);
inline void scale_pdf(double* pdf ,const int size,double sum );
inline void make_cdf_from_pdf(double* pdf ,double* cdf , const int size );

//update m_i_j
void  update_M();
void  update_m_ij(const int row,const int col,double * sum_den,uint32_t* freq_den,uint32_t*  freq_num,uint32_t* ref);
void get_max_f_m_ij(const uint8_t i, const uint8_t j,double*log_F_max,double* maxima,double*sum_den,uint32_t*freq_den,uint32_t* freq_num,uint32_t* ref,double * maxima_possible,double * val_maxima);
double evaluate_m_ij_ll(double x0,const uint8_t i,const uint8_t j,double* sum_den,uint32_t* freq_den,uint32_t* freq_num,uint32_t* ref, double *maxima_m_ij_possible,double *log_Fx_maxima_m_ij_possible);
void m_compute_double_term_word_stat(const uint8_t i,double* sum_den ,uint32_t* freq_den,uint32_t* freq_num_i,uint32_t* w_topic_count_map);

//UPDATE THETA matrix
void update_THETA();
void  update_theta_iw(const uint8_t i,const uint32_t w, double* den_single_term_header,double * sum_den,uint32_t* freq_den,const uint32_t freq_single_num,uint32_t *  freq_num,const uint32_t freq_single_den_w,uint32_t * ref);
void get_max_f_theta_iw(const uint8_t i, const uint32_t w,double*log_F_max,double* maxima, double* den_single_term_header,double * sum_den,uint32_t* freq_den,const uint32_t freq_single_num,uint32_t *  freq_num,const uint32_t freq_single_den_w, uint32_t* ref, double * maxima_possible,double *value_possible);
double evaluate_theta_iw_ll(double x0,const uint8_t i,const uint32_t w,const double den_single_term_header,double * sum_den,uint32_t * freq_den,const uint32_t freq_single_num,uint32_t* freq_num,const  uint32_t freq_single_den_w,uint32_t* ref , double *maxima_theta_iw_possible,double *log_Fx_maxima_theta_iw_possible);
double get_den_theta_jw(const uint8_t j , const uint32_t w);
double compute_theta_single_term_denominator(const uint32_t  w );
void theta_compute_double_term_word_stat(const uint32_t w,double* sum_den,uint32_t* freq_den,uint32_t* freq_single_num_w,uint32_t* freq_double_num,uint32_t* ref );

//Common codes
double inline  get_number_between_two_numbers(double low, double high);
inline double rand_0_1();
void ctrl_c_handler(int single_num);
void  write_model();
double timetaken(clock_t start,clock_t  end , const char* mesaage);
double gamma_approx_sampler(double r,double log_F_max,double log_f_r,double maxima);
void   guess_lebel(document* current_doc_ptr);
double m_diagonistics(const int i,const int j,double* sum_den,uint32_t* freq_den,uint32_t* freq_num , uint32_t* ref ,double *maxima_possible,double*value_possible,const char*msg , double maxima);

double theta_diagonistics(const int i, const int w,const double den_single_term_header,double * sum_den,uint32_t* freq_den,const uint32_t freq_single_num,uint32_t *  freq_num,const  uint32_t freq_single_den_w,uint32_t* ref, double *possible_maxima,double * val_at_max,const char* msg,double maxima);
int linear_scan(double*cdf , const int size,double data);
double inline get_d_2D_m(double *ptr, const int row,const int col,const int row_length);
void inline set_d_2D_m(double *ptr, const int row,const int col,double value,const int row_length);
uint32_t  get_i_3D_m(const uint32_t *ptr ,int x,int y,int z,int y_size,int z_size);
void  set_i_3D_m(uint32_t *ptr,int x,int y,int z,int value,int x_size,int y_size);
uint32_t inline get_i_2D_m(uint32_t *ptr, int row,int col,int row_length);
void inline set_i_2D_m(uint32_t *ptr, int row,int col,int value,int row_length);
void load_model_params();
void load_topics();

//update PHI
double  evaluate_phi_i(const double x0,const uint8_t i );
void update_PHI();
void   get_max_f_phi_i(const uint8_t i, double* log_Fx,double* maxima ) ; // calculate maximum of function of log !

void print_topic_corpus();
void print_model_params();
void print_system_config();

void reset_topic_usage();

uint32_t  get_key_3D_m(int x,int y,int z,int x_size,int y_size);
void print_time();

//Makes word  of document
//void make_int_words(document* doc);

void make_int_words(document* doc,vector<string>& word_list );

//These things cam because of new data interpretation
//Now theyi are in strings so,I will use these data structures for
//string interpretation.
void make_word_id(string word);
std::map<string , int > word_id_map;
std::map<int , string > id_word_map;
string  get_word_from_id(int id);

int  get_id_from_word(string word);
//while we read document we will silently populate  this map for furtehr uses !
void  create_word_id_id_word_map(string word);

//helper  function for day to word
bool isWordStartsWithCaps(string word);
bool isCommaBeforeMe(string word) ;
bool isPeriodBeforeMe(string word) ;

//New variables  for  \alpha_c_Y and so on !
double alpha_comma_Y=0;
double alpha_comma_N=0;
double alpha_period_Y=0;
double alpha_period_N=0;
double alpha_upper_Y=0;
double alpha_upper_N=0;
// shape  and scale of \alpha

//make string lower case
string make_lowercase_string(string word);
bool doesWordStartsUpper(string data);

const string highlight_tag_start="<:t:>";
const string highlight_tag_end="<:/t:>";
const string comma=",";
const string period=".";

double get_punctuation_multipler_num(document* doc, int pos,int t_last,int t_next );
double get_punctuation_multipler_den(document* doc, int pos,int t_last, int t_next );

#define  total_configs_punctuation 8
double multiplier_values[total_configs_punctuation];
void fill_multiplier_values();

bool  didReturnDBL_MAX=false;

//alpha_comma_Y
void sample_alpha_comma_Y();
void  make_alpha_comma_Y_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_comma_Y_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_comma_Y_possible,double *log_Fx_maxima_alpha_comma_Y_possible);
void get_max_f_alpha_comma_Y(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_comma_Y_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

//alpha_comma_N
void sample_alpha_comma_N();
void  make_alpha_comma_N_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_comma_N_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_comma_N_possible,double *log_Fx_maxima_alpha_comma_N_possible);
void get_max_f_alpha_comma_N(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_comma_N_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

//alpha_upper_Y
void sample_alpha_upper_Y();
void  make_alpha_upper_Y_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_upper_Y_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_upper_Y_possible,double *log_Fx_maxima_alpha_upper_Y_possible);
void get_max_f_alpha_upper_Y(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_upper_Y_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

//alpha_upper_N
void sample_alpha_upper_N();
void  make_alpha_upper_N_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_upper_N_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_upper_N_possible,double *log_Fx_maxima_alpha_upper_N_possible);
void get_max_f_alpha_upper_N(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_upper_N_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

//alpha_upper_Y
void sample_alpha_period_Y();
void  make_alpha_period_Y_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_period_Y_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_period_Y_possible,double *log_Fx_maxima_alpha_period_Y_possible);
void get_max_f_alpha_period_Y(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_period_Y_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

//alpha_period_N
void sample_alpha_period_N();
void  make_alpha_period_N_statistics(int &num_count,double* sum_den,map<int,vector<int> >& ref);
double evaluate_alpha_period_N_log_posterior(double x0,const int &num_count, double* sum_den,const map<int , vector<int> > & ref,double *maxima_alpha_period_N_possible,double *log_Fx_maxima_alpha_period_N_possible);
void get_max_f_alpha_period_N(double* log_F_max,double *maxima ,int &num_count,double*sum_den,map<int,vector<int> >& ref,double * maxima_possible,double * val_maxima);
double alpha_period_N_diagonistics(int &num_count,double*sum_den,map<int,vector<int> >& ref,double * possible_maxima,double * val_at_max ,const char* msg,double maxima);

///home/rkumar2/final_test/data
///data1/rahul/projects/text_mining/branch/punctuation/chris_data
const char* data="/data1/rahul/projects/text_mining/branch/punctuation/chris_data/AB" ;
const char* dict_file="/data1/rahul/projects/text_mining/branch/punctuation/chris_data/word_list.txt" ;
#endif


/*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*
**                                                                                **
** This file forms part of the Underworld geophysics modelling application.         **
**                                                                                **
** For full license and copyright information, please refer to the LICENSE.md file  **
** located at the project root, or contact the authors.                             **
**                                                                                **
**~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*~*/
#ifndef _createK2_h
#define _createK2_h

#include <petsc.h>
#include <petscmat.h>
#include <petscvec.h>
#include <petscksp.h>

PetscErrorCode bsscr_buildK2(KSP ksp);
PetscErrorCode bsscr_DGMiGtD( Mat *_K2, Mat K, Mat G, Mat M);
PetscErrorCode bsscr_GMiGt( Mat *_K2, Mat K, Mat G, Mat M);
PetscErrorCode bsscr_GGt( Mat *_K2, Mat K, Mat G);
#endif


/**
 * Subroutines to calculate the
 * various contamination models
 *
 */

#include <math.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_interp.h>

#include "fitsio.h"
#include "inout_aper.h"
#include "aXe_grism.h"
#include "spce_PET.h"
#include "spc_wl_calib.h"
#include "aXe_errors.h"
#include "fringe_conf.h"
#include "spc_resp.h"
#include "spce_pathlength.h"
#include "aper_conf.h"
#include "trace_conf.h"
#include "specmodel_utils.h"
#include "model_utils.h"
#include "spc_fluxcube.h"

#define MAX(x,y) (((x)>(y))?(x):(y))
#define MIN(x,y) (((x)<(y))?(x):(y))
#define SQR(x) ((x)*(x))

/**
 * The function creates and loads a new flux_cube structure
 * The data is taken from the file specified in the input.
 *
 * @param  fcube_file - the file name of the fluxcube
 *
 * @return ret        - the new flux_cube structure
 */
flux_cube *
load_fluxcube(const char fcube_file[])
{
  flux_cube *fcube;

  //gsl_matrix *test;

  int nflux, n_ext=0;
  int i;

  // get the number of extensions
  n_ext = FITSextnum(fcube_file);
  if (n_ext <3)
    aXe_message(aXe_M_FATAL, __FILE__, __LINE__,
                "aXe_PETCONT: " "the fluxcube file %s has only %i extensions!\n",
                fcube_file, n_ext);

  // determine the number of flux images and allocate
  // the space for the fluxcube structure
  nflux = n_ext-2;
  fcube = alloc_fluxcube(nflux);

  // load XOFFS and YOFFS
  load_offsets(fcube_file, fcube);

  // load the segmentation image into the structure
  fcube->segmentation = load_segmentation(fcube_file);
  //  gsl_to_FITSimage (fcube->segmentation, "gogo.fits", 1, "MOO");

  // load the fluximages
  for (i=0; i < nflux; i++)
    {
      fcube->fluxims[i] = load_fluximage(fcube_file, i+3);
    }

  // fill the number of fluximages
  fcube->n_fimage = nflux;

  // order the fluximages and store the
  // vector with the ordered indices
  fcube->fimage_order = order_fluxims(fcube);

  return fcube;
}

/**
 * The function allocates space for a new
 * fluxcube structure.
 *
 * @param  nflux - the number of fluximages
 *
 * @return ret   - the fluxcube structure
 */
flux_cube *
alloc_fluxcube(const int nflux)
{
   flux_cube *fcube;

   // allocate space for the flux_cube structure
   fcube = (flux_cube *)malloc(sizeof(flux_cube));

   // allocate space for the array of fluximages
   fcube->fluxims      = (flux_image **)malloc(nflux * sizeof(flux_image *));

   // return the result
   return fcube;
}

/**
 * The function fills the keywords XOFFS and YOFFS
 * from the primary header of the fitscube file
 * into the according items of the fitscube structure.
 * In case that those keywords are not present,
 * they are set to 0.
 *
 * @param  fcube_file - the file name of the fluxcube
 * @param  fcube      - the fluxcube structure
 *
 * @return 1          - always true
 */
int
load_offsets(const char fcube_file[], flux_cube *fcube)
{
  char tmp[MAXCHAR];

  double dtmp=0.0;

  // avoid problems with the 'const' declaration
  strcpy(tmp, fcube_file);

  // read XOFFS from the primary header
  dtmp = get_float_from_keyword(tmp, 1, "XOFFS");

  // store the keyword value or 0 (if not present) in the fcube structure
  if (isnan(dtmp))
    fcube->xoffs = 0;
  else
    fcube->xoffs = (int)dtmp;

  // read XOFFS from the primary header
  dtmp = get_float_from_keyword(tmp, 1, "YOFFS");

  // store the keyword value or 0 (if not present) in the fcube structure
  if (isnan(dtmp))
    fcube->yoffs = 0;
  else
    fcube->yoffs = (int)dtmp;

  return 1;
}

/**
 * The function creates and loads the segmentation image
 * of a flux_cube file into a gsl_matrix_int structure.
 *
 * @param  fcube_file - the file name of the fluxcube
 *
 * @return ret        - pointer to the gsl_matrix_int structure
 */
gsl_matrix_int *
load_segmentation(const char fcube_file[])
{

  gsl_matrix_int *segmentation;
  gsl_matrix     *dvals;

  int i,j;

  double diff;

  // load the segmentation image into a gsl_matrix_(double)
  dvals = FITSimage_to_gsl(fcube_file, 2, 1);

  // add 0.5 to get proper rounding with (int)
  gsl_matrix_add_constant (dvals, 0.5);

  // allocate space for the integer matrix
  segmentation = gsl_matrix_int_alloc(dvals->size1,dvals->size2);

  // fill the integer matrix with values from the double matrix
  for (i=0; i < (int)dvals->size1; i++)
    {
      for (j=0; j < (int)dvals->size2; j++)
        {
          gsl_matrix_int_set(segmentation, i, j, (int)gsl_matrix_get(dvals, i, j));

          diff = gsl_matrix_get(dvals, i, j) - (double)gsl_matrix_int_get(segmentation, i, j);
          if (diff > 0.6 || diff < 0.4)
            fprintf(stdout, "diff %f; %f --> %i, (%i, %i)\n", diff, gsl_matrix_get(dvals, i, j), gsl_matrix_int_get(segmentation, i, j), i, j);
        }
    }

  // free the space for the double matrix
  gsl_matrix_free(dvals);

  return segmentation;
}

/**
 * The function creates and loads a fluximage structure
 * from an extention of a fluxcube file.
 *
 * @param  fcube_file - the file name of the fluxcube
 * @param  hdunum     - the extension number to load in a fluximage
 *
 * @return fimage     - pointer to the fluximage structure loaded
 */
flux_image *
load_fluximage(const char fcube_file[], int hdunum)
{
  flux_image *fimage;

  char tmp[MAXCHAR];

  double wavelength;

  strcpy(tmp, fcube_file);

  // allocate space for the fluximage structure
  fimage = (flux_image *)malloc(sizeof(flux_image));

  // load the array from the specified extension number
  fimage->flux = FITSimage_to_gsl(fcube_file, hdunum, 1);

  // get the keyword "WAVELENG' from the specified extension number
  wavelength = (double)get_float_from_keyword(tmp, hdunum, "WAVELENG");

  // report an error in case that the extension does not have that keyword.
  // Otherwise store its content in the fluximage structure
  if (isnan(wavelength))
    aXe_message(aXe_M_FATAL, __FILE__, __LINE__,
                "aXe_PETCONT: " "Missing keyword WAVELENGTH in fluxcube %s, extension %i!\n",
                fcube_file, hdunum);
  else
    fimage->wavelength = wavelength;

  return fimage;
}

/**
 * The function analyzes the fluximages in a
 * fluxcube structure and sorts their indices in
 * ascending wavelength. The vector with the
 * ordered indices is given back.
 *
 * @param  fcube - the pointer to the fluxcube structure
 *
 */
gsl_vector_int *
order_fluxims(flux_cube *fcube)
{
  gsl_vector     *fimage_wavelength;

  gsl_vector_int *fimage_order;

  int i, j, k;

  // allocate space for the order vector
  fimage_order = gsl_vector_int_alloc(fcube->n_fimage);

  // put a default order in the vector
  for (i=0; i<fcube->n_fimage; i++)
    gsl_vector_int_set(fimage_order, i, i);

  // order the wavelength
  if (fcube->n_fimage > 1)
    {

      // create a vector for the wavelengths
      fimage_wavelength = gsl_vector_alloc(fcube->n_fimage);

      // set the first entry for the wavelength
      gsl_vector_set(fimage_wavelength, 0, fcube->fluxims[0]->wavelength);

      // go over all fluximages
      for (i=1; i<fcube->n_fimage; i++)
        {

          // find the correct position of the fluximage
          j=0;
          while (j < i && gsl_vector_get(fimage_wavelength, j) <= fcube->fluxims[i]->wavelength)
            j++;

          // check whether the correct poition of the fluximage is at the end
          if (j == i)
            {

              // append the fluximage at the end
              gsl_vector_set(fimage_wavelength, j, fcube->fluxims[i]->wavelength);
              gsl_vector_int_set(fimage_order, j, i);
            }
          else
            {

              // move all fluximages one index up
              for (k=i; k > j; k--)
                {
                  gsl_vector_set(fimage_wavelength, k, gsl_vector_get(fimage_wavelength, k-1));
                  gsl_vector_int_set(fimage_order, k, gsl_vector_int_get(fimage_order, k-1));
                }

              // place the flux image at the correct position
              gsl_vector_set(fimage_wavelength, j, fcube->fluxims[i]->wavelength);
              gsl_vector_int_set(fimage_order, j, i);
            }
        }

      // release the space for the wavelength vector
      gsl_vector_free(fimage_wavelength);
    }

  // return the order vector
  return fimage_order;
}

/**
 * The function releases the space allocated
 * for a fluxcube structure.
 *
 * @param  fcube - the pointer to the fluxcube structure
 *
 */
void
free_fluxcube(flux_cube *fcube)
{
  int i;

  // free the segmentation matrix
  gsl_matrix_int_free(fcube->segmentation);

  // free the wavelength order vector
  gsl_vector_int_free(fcube->fimage_order);

  // go over the fluximages, and free them
  for (i=0; i < fcube->n_fimage; i++)
    free_fluximage(fcube->fluxims[i]);

  // free the fluximage vector
  free(fcube->fluxims);

  // free the fluximage order vector
  //  gsl_vector_int_free(fcube->fimage_order);

  // free the fluxcube
  free(fcube);

  // set the fluxcube to NULL
  fcube=NULL;
}

/**
 * The function releases the space allocated
 * for a fluximage structure.
 *
 * @param  fimage - the pointer to the fluximage structure
 *
 */
void
free_fluximage(flux_image *fimage)
{
  // free the flux matrix
  gsl_matrix_free(fimage->flux);

  // free the fluximage
  free(fimage);

  // set the fluximage to NULL
  fimage=NULL;
}

/**
 * The function transforms a pixel coordinate point in a
 * flt image into an pixel coordinate point in the associated
 * fluxcube image
 *
 * @param  fcube       - the fluxcube structure
 * @param  flt_point   - pixel coordinate point in the flt-image
 *
 * @return fcube_point - the pixel coordinate in the fluxcube
 */
d_point
flt_to_fcube_trans(const flux_cube *fcube, const d_point flt_point)
{
  d_point fcube_point;

  fcube_point.x = flt_point.x - (double)fcube->xoffs;
  fcube_point.y = flt_point.y - (double)fcube->yoffs;

  return fcube_point;
}

/**
 * The function transforms an image coordinate point in a
 * fluxcube into an image coo point in the associated
 * flt image
 *
 * @param  fcube       - the fluxcube structure
 * @param  fcube_point - image-coo in the fcube
 *
 * @return flt_point   - the image coo in the flt
 */
d_point
fcube_to_flt_trans(const flux_cube *fcube, const d_point fcube_point)
{
  d_point flt_point;

  flt_point.x = fcube_point.x + (double)fcube->xoffs;
  flt_point.y = fcube_point.y + (double)fcube->yoffs;

  return flt_point;
}

/**
 * The function converts the information stored in the segmenation
 * matrix of a fluxcube and the object list to a list of direct objects
 *
 * @param  fcube   - the fluxcube structure
 * @param  oblist  - the object list
 *
 * @return dirlist - the dirobject list created
 */
dirobject **
fluxcube_to_dirlist(const flux_cube *fcube, object  **oblist)
{
  dirobject **dirlist;
  dirobject  *actdir;

  //px_point fcube_point;
  px_point flt_point;

  d_point tmp1, tmp2;

  int iact=0;
  int nobjects=0;
  int ndir=0;
  int i, j;
  int objindex;

  //int itmp=0;

  // determine the number of objects in the object list
  nobjects = object_list_size(oblist);

  // allocate space for the dirobject list
  dirlist = (dirobject **) malloc((nobjects+1) * sizeof(dirobject *));
  if (dirlist == NULL)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
                 "fluxcube_to_dirlist:" " Could not allocate"
                 " memory for pointers to %i dirobject objects", nobjects+1);

  // initialize the number of direct object
  // and set the first item to NULL
  ndir=0;
  dirlist[ndir]=NULL;

  //  if (nobjects < 1)
  //    return dirlist;

  // go over each column in the segmentation image
  for (i=0; i < (int)fcube->segmentation->size1; i++)
    {
      // go over each row in the segmentation image
      for (j=0; j < (int)fcube->segmentation->size2; j++)
        {

          // get the pixel value in the segmentation image
          iact = gsl_matrix_int_get(fcube->segmentation, i, j);

          // do something if the pixel is part of an object
          if (iact > 0)
            {

              // transform the cooridnates from the fcube-system
              // into the flt system
              tmp1.x = (double)i;
              tmp1.y = (double)j;
              tmp2 = fcube_to_flt_trans(fcube, tmp1);
              flt_point.x = (int)tmp2.x;
              flt_point.y = (int)tmp2.y;

              // try to get a dirobject for that object (=pixel value)
              actdir = get_dirobject_from_list(dirlist, iact);

              // check whether a dirobject just exists
              if (actdir != NULL)
                {
                  // udate an existing object
                  update_dirobject(actdir, flt_point);
                }
              else
                {

                  // find the the object index for a (hypotetical) new dirobject
                  objindex = find_object_in_object_list(oblist, iact);
                  // check whether object exists in oblist
                  if (objindex >-1)
                    {
                      // create a new dirobject and enhance the counter
                      dirlist[ndir++] = dirobject_from_segpoint(flt_point, oblist[objindex]);

                      // set the actually last dirobject to NULL
                      // to get a properly closed list at all time
                      dirlist[ndir]=NULL;
                    }
                }
            }
        }
    }

  // return the direct object list
  return dirlist;
}

/**
 * The function creates a a dirobject on the basis
 * of an object and the pixel coos of a point
 * which is part of that object.
 *
 * @param  flt_point - the fluxcube structure
 * @param  actobject - the object list
 *
 * @return dirobject - the dirobject created
 */
dirobject *
dirobject_from_segpoint(const px_point flt_point, const object  *actobject)
{

  dirobject  *actdir;

  // allocate space for the dirobject
  actdir = (dirobject *) malloc (sizeof (dirobject));
  if (actdir == NULL)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
                 "fill_dirobject:" " Could not allocate"
                 " memory for a dirobject object");

  // transfer refpoint and ID
  actdir->ID = actobject->ID;
  actdir->refpoint.x = actobject->beams[0].refpoint.x;
  actdir->refpoint.y = actobject->beams[0].refpoint.y;

  // derive and store min/max in x/y for the corners
  actdir->ix_min = flt_point.x;
  actdir->ix_max = flt_point.x;
  actdir->iy_min = flt_point.y;
  actdir->iy_max = flt_point.y;

  // derive the distortion scales along the major an minor axis
  actdir->drzscale.x = 1.0;
  actdir->drzscale.y = 1.0;

  // set the variable
  actdir->bb_sed = 0;

  // make it a dummy
  actdir->SED = NULL;

  // return the new dirobject
  return actdir;
}

/**
 * The function updates a dirobject with a point
 * belonging to the same object. The new point
 * may extend the object size.
 *
 * @param actdir    - the direct object
 * @param flt_point - the new coordinate point
 *
 */
void
update_dirobject(dirobject *actdir, const px_point flt_point)
{
  // check whether the new points extends the
  // old min/max values in x or y.
  // correct the values if necessary.
  actdir->ix_min = MIN(actdir->ix_min, flt_point.x);
  actdir->ix_max = MAX(actdir->ix_max, flt_point.x);
  actdir->iy_min = MIN(actdir->iy_min, flt_point.y);
  actdir->iy_max = MAX(actdir->iy_max, flt_point.y);
}

/**
 * The function computes and stores at the according position
 * the xy offsets of a direct image. This is only important
 * in the fluxcube emission model, since the image coordinates
 * in fluxcubes are 'filter' coordinates and do not take into
 * account the offsets introduce by the grism.
 * For each object and beam those offsets are evaluated
 * and stored in the direct object structure.
 *
 * @param dirlist   - the direct object
 * @param CONF_file - the new coordinate point
 *
 */
void fill_xy_offsets(dirobject **dirlist, char CONF_file[])
{
  aperture_conf   *conf;

  gsl_vector *x_coeffs;
  gsl_vector *y_coeffs;

  d_point m_point;

  //double xoffs, yoffs;

  int beamID=0;
  int i;

  // load the configuration file
  conf = get_aperture_descriptor (CONF_file);

  // go over each beam defined in the configuration
  // file
  for (beamID=0; beamID < conf->nbeams; beamID++)
    {

      // determine the coeeficients for the offsets
      // in x and in y
      x_coeffs = get_beam_trace_xoff (CONF_file, beamID);
      y_coeffs = get_beam_trace_yoff (CONF_file, beamID);

      // go over each direct object
      i=0;
      while (dirlist[i] !=NULL)
        {

          // get the mean direct object position
          m_point = get_dirobject_meanpos(dirlist[i]);

          // evaluate the coefficients for the 2D variable offsets
          // at the mean position
          dirlist[i]->xy_off[beamID].x = eval_trace_off_at_pos (x_coeffs, m_point, beamID);
          dirlist[i]->xy_off[beamID].y = eval_trace_off_at_pos (y_coeffs, m_point, beamID);

          // iterate the counter
          i++;
        }

      // free the vectors for the coefficients
      gsl_vector_free(x_coeffs);
      gsl_vector_free(y_coeffs);
    }
        // release memory
        free_aperture_conf(conf);
}

/**
 * The function fills the flux information from a point in
 * a fluxcube structure into the flux part of a direct object.
 *
 * @param fcube  - the fluxcube structure
 * @param point  - the position to take the flux from
 * @param actdir - the direct object to update
 *
 */
void fill_fluxvalues(const flux_cube *fcube, const px_point point,
                     dirobject *actdir, const int inter_type)
{

  energy_distrib *sed;
  double *sed_wavs;
  double *sed_flux;

  int i;
  int iact;

  sed = (energy_distrib*) malloc(sizeof(energy_distrib));
  sed_wavs = (double*) malloc(fcube->n_fimage*sizeof(double));
  sed_flux = (double*) malloc(fcube->n_fimage*sizeof(double));

  if (actdir->SED)
    free_enerdist (actdir->SED);

  // go over each fluximage in the fluxcube structure
  for (i=0; i < fcube->n_fimage; i++)
    {

      // determine the index of the next fluximage
      iact = gsl_vector_int_get(fcube->fimage_order, i);

      // transfer the wavelength and flux from the fluxcube
      sed_wavs[i] = fcube->fluxims[iact]->wavelength;
      sed_flux[i] = gsl_matrix_get(fcube->fluxims[iact]->flux, point.x, point.y);
    }

  sed->npoints    = fcube->n_fimage;
  sed->wavelength = sed_wavs ;
  sed->flux       = sed_flux;

  if (fcube->n_fimage > 1)
    {
      if (inter_type == 2)
        sed->interp     = gsl_interp_alloc (gsl_interp_polynomial, (size_t)fcube->n_fimage);
      else if (inter_type == 3)
        sed->interp     = gsl_interp_alloc (gsl_interp_cspline, (size_t)fcube->n_fimage);
      else
        sed->interp     = gsl_interp_alloc (gsl_interp_linear, (size_t)fcube->n_fimage);

      sed->accel      = gsl_interp_accel_alloc ();
      gsl_interp_init (sed->interp, sed->wavelength, sed->flux, (size_t)sed->npoints);
    }
  else
    {
      sed->interp = NULL;
      sed->accel= NULL;
    }

  // transfer the SED;
  // announce the SED
  actdir->SED    = sed;
  actdir->bb_sed = 1;
}

/**
 * The function prints the main properties of a
 * fluxcube structure onto the screen.
 * This is mostly for debuggin/development
 * purposes, and not for production
 *
 * @param fcube      - the fluxcube structure
 *
 */
void
print_fluxcube(const flux_cube *fcube)
{
  int i;

  fprintf(stdout, "XOFFS,YOFFS: %i,%i\n", fcube->xoffs, fcube->yoffs);
  fprintf(stdout, "Number of flux-extension: %i\n", fcube->n_fimage);

  for (i=0; i<fcube->n_fimage; i++)
    fprintf(stdout, "Wavelength for flux extension %i: %f\n", i,
            fcube->fluxims[gsl_vector_int_get(fcube->fimage_order,i)]->wavelength);
}


/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "xform.h"
#include "imgfeatures.h"
#include "utils.h"

//#include "cxcore.h"
//#include <highgui.h>
// Universal include for all versions of OpenCV
#include <mrpt/otherlibs/do_opencv_includes.h>

#include <gsl/gsl_sf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include <time.h>

/************************* Local Function Prototypes *************************/

static __inline struct feature* get_match( struct feature*, int );
int get_matched_features( struct feature*, int, int, struct feature*** );
int calc_min_inliers( int, int, double, double );
struct feature** draw_ransac_sample( struct feature**, int, int, gsl_rng* );
void extract_corresp_pts( struct feature**, int, int, CvPoint2D64f**,
						 CvPoint2D64f** );
int find_consensus( struct feature**, int, int, CvMat*, ransac_err_fn,
				   double, struct feature*** );
static __inline void release_mem( CvPoint2D64f*, CvPoint2D64f*,
struct feature** );

/********************** Functions prototyped in xform.h **********************/

/*
Calculates a best-fit image transform from image feature correspondences
using RANSAC.

For more information refer to:

Fischler, M. A. and Bolles, R. C.  Random sample consensus: a paradigm for
model fitting with applications to image analysis and automated cartography.
Communications of the ACM, 24, 6 (1981), pp. 381--395.

@param features an array of features; only features with a non-NULL match
	of type mtype are used in homography computation
@param n number of features in feat
@param mtype determines which of each feature's match fields to use
	for model computation; should be one of FEATURE_FWD_MATCH,
	FEATURE_BCK_MATCH, or FEATURE_MDL_MATCH; if this is FEATURE_MDL_MATCH,
	correspondences are assumed to be between a feature's img_pt field
	and its match's mdl_pt field, otherwise correspondences are assumed to
	be between the the feature's img_pt field and its match's img_pt field
@param xform_fn pointer to the function used to compute the desired
	transformation from feature correspondences
@param m minimum number of correspondences necessary to instantiate the
	model computed by xform_fn
@param p_badxform desired probability that the final transformation
	returned by RANSAC is corrupted by outliers (i.e. the probability that
	no samples of all inliers were drawn)
@param err_fn pointer to the function used to compute a measure of error
	between putative correspondences and a computed model
@param err_tol correspondences within this distance of a computed model are
	considered as inliers
@param inliers if not NULL, output as an array of pointers to the final
	set of inliers
@param n_in if not NULL and \a inliers is not NULL, output as the final
	number of inliers

@return Returns a transformation matrix computed using RANSAC or NULL
	on error or if an acceptable transform could not be computed.
*/
CvMat* ransac_xform( struct feature* features, int n, int mtype,
					ransac_xform_fn xform_fn, int m, double p_badxform,
					ransac_err_fn err_fn, double err_tol,
struct feature*** inliers, int* n_in )
{
	struct feature** matched, ** sample, ** consensus, ** consensus_max = NULL;
	struct ransac_data* rdata;
	CvPoint2D64f* pts, * mpts;
	CvMat* M = NULL;
	gsl_rng* rng;
	double p, in_frac = RANSAC_INLIER_FRAC_EST;
	int i, nm, in, in_min, in_max = 0, k = 0;

	nm = get_matched_features( features, n, mtype, &matched );
	if( nm < m )
	{
		fprintf( stderr, "Warning: not enough matches to compute xform, %s" \
			" line %d\n", __FILE__, __LINE__ );
		goto end;
	}

	/* initialize random number generator */
	rng = gsl_rng_alloc( gsl_rng_mt19937 );
	gsl_rng_set( rng, time(NULL) );

	in_min = calc_min_inliers( nm, m, RANSAC_PROB_BAD_SUPP, p_badxform );
	p = pow( 1.0 - pow( in_frac, m ), k );
	i = 0;
	while( p > p_badxform )
	{
		sample = draw_ransac_sample( matched, nm, m, rng );
		extract_corresp_pts( sample, m, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, m );
		if( ! M )
			goto iteration_end;
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		if( in > in_max )
		{
			if( consensus_max )
				free( consensus_max );
			consensus_max = consensus;
			in_max = in;
			in_frac = (double)in_max / nm;
		}
		else
			free( consensus );
		cvReleaseMat( &M );

iteration_end:
		release_mem( pts, mpts, sample );
		p = pow( 1.0 - pow( in_frac, m ), ++k );
	}

	/* calculate final transform based on best consensus set */
	if( in_max >= in_min )
	{
		extract_corresp_pts( consensus_max, in_max, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in_max );
		in = find_consensus( matched, nm, mtype, M, err_fn, err_tol, &consensus);
		cvReleaseMat( &M );
		release_mem( pts, mpts, consensus_max );
		extract_corresp_pts( consensus, in, mtype, &pts, &mpts );
		M = xform_fn( pts, mpts, in );
		if( inliers )
		{
			*inliers = consensus;
			consensus = NULL;
		}
		if( n_in )
			*n_in = in;
		release_mem( pts, mpts, consensus );
	}
	else if( consensus_max )
	{
		if( inliers )
			*inliers = NULL;
		if( n_in )
			*n_in = 0;
		free( consensus_max );
	}

	gsl_rng_free( rng );
end:
	for( i = 0; i < nm; i++ )
	{
		rdata = feat_ransac_data( matched[i] );
		matched[i]->feature_data = rdata->orig_feat_data;
		free( rdata );
	}
	free( matched );
	return M;
}

/*
Calculates a least-squares planar homography from point correspondeces.

@param pts array of points
@param mpts array of corresponding points; each pts[i], i=0..n-1, corresponds
	to mpts[i]
@param n number of points in both pts and mpts; must be at least 4

@return Returns the 3 x 3 least-squares planar homography matrix that
	transforms points in pts to their corresponding points in mpts or NULL if
	fewer than 4 correspondences were provided
*/
CvMat* lsq_homog( CvPoint2D64f* pts, CvPoint2D64f* mpts, int n )
{
	CvMat* H, * A, * B, X;
	double x[9];
	int i;

	if( n < 4 )
	{
		fprintf( stderr, "Warning: too few points in lsq_homog(), %s line %d\n",
			__FILE__, __LINE__ );
		return NULL;
	}

	/* set up matrices so we can unstack homography into X; AX = B */
	A = cvCreateMat( 2*n, 8, CV_64FC1 );
	B = cvCreateMat( 2*n, 1, CV_64FC1 );
	X = cvMat( 8, 1, CV_64FC1, x );
	H = cvCreateMat(3, 3, CV_64FC1);
	cvZero( A );
	for( i = 0; i < n; i++ )
	{
		cvmSet( A, i, 0, pts[i].x );
		cvmSet( A, i+n, 3, pts[i].x );
		cvmSet( A, i, 1, pts[i].y );
		cvmSet( A, i+n, 4, pts[i].y );
		cvmSet( A, i, 2, 1.0 );
		cvmSet( A, i+n, 5, 1.0 );
		cvmSet( A, i, 6, -pts[i].x * mpts[i].x );
		cvmSet( A, i, 7, -pts[i].y * mpts[i].x );
		cvmSet( A, i+n, 6, -pts[i].x * mpts[i].y );
		cvmSet( A, i+n, 7, -pts[i].y * mpts[i].y );
		cvmSet( B, i, 0, mpts[i].x );
		cvmSet( B, i+n, 0, mpts[i].y );
	}
	cvSolve( A, B, &X, CV_SVD );
	x[8] = 1.0;
	X = cvMat( 3, 3, CV_64FC1, x );
	cvConvert( &X, H );

	cvReleaseMat( &A );
	cvReleaseMat( &B );
	return H;
}

/*
Calculates the transfer error between a point and its correspondence for
a given homography, i.e. for a point x, it's correspondence x', and
homography H, computes d(x', Hx)^2.

@param pt a point
@param mpt pt's correspondence
@param H a homography matrix

@return Returns the transfer error between pt and mpt given H
*/
double homog_xfer_err( CvPoint2D64f pt, CvPoint2D64f mpt, CvMat* H )
{
	CvPoint2D64f xpt = persp_xform_pt( pt, H );

	return sqrt( dist_sq_2D( xpt, mpt ) );
}

/*
Performs a perspective transformation on a single point.  That is, for a
point (x, y) and a 3 x 3 matrix T this function returns the point
(u, v), where

[x' y' w']^T = T * [x y 1]^T,

and

(u, v) = (x'/w', y'/w').

Note that affine transforms are a subset of perspective transforms.

@param pt a 2D point
@param T a perspective transformation matrix

@return Returns the point (u, v) as above.
*/
CvPoint2D64f persp_xform_pt( CvPoint2D64f pt, CvMat* T )
{
	CvMat XY, UV;
	double xy[3] = { pt.x, pt.y, 1.0 }, uv[3] = { 0 };
	CvPoint2D64f rslt;

	cvInitMatHeader( &XY, 3, 1, CV_64FC1, xy, CV_AUTOSTEP );
	cvInitMatHeader( &UV, 3, 1, CV_64FC1, uv, CV_AUTOSTEP );
	cvMatMul( T, &XY, &UV );
	rslt = cvPoint2D64f( uv[0] / uv[2], uv[1] / uv[2] );

	return rslt;
}

/************************ Local funciton definitions *************************/

/*
Returns a feature's match according to a specified match type

@param feat feature
@param mtype match type, one of FEATURE_FWD_MATCH, FEATURE_BCK_MATCH, or
FEATURE_MDL_MATCH

@return Returns feat's match corresponding to mtype or NULL for bad mtype
*/
static __inline struct feature* get_match( struct feature* feat, int mtype )
{
	if( mtype == FEATURE_MDL_MATCH )
		return feat->mdl_match;
	if( mtype == FEATURE_BCK_MATCH )
		return feat->bck_match;
	if( mtype == FEATURE_FWD_MATCH )
		return feat->fwd_match;
	return NULL;
}

/*
Finds all features with a match of a specified type and stores pointers
to them in an array.  Additionally initializes each matched feature's
feature_data field with a ransac_data structure.

@param features array of features
@param n number of features in features
@param mtype match type, one of FEATURE_{FWD,BCK,MDL}_MATCH
@param matched output as an array of pointers to features with a match of
the specified type

@return Returns the number of features output in matched.
*/
int get_matched_features( struct feature* features, int n, int mtype,
struct feature*** matched )
{
	struct feature** _matched;
	struct ransac_data* rdata;
	int i, m = 0;

	_matched = calloc( n, sizeof( struct feature* ) );
	for( i = 0; i < n; i++ )
		if( get_match( features + i, mtype ) )
		{
			rdata = malloc( sizeof( struct ransac_data ) );
			memset( rdata, 0, sizeof( struct ransac_data ) );
			rdata->orig_feat_data = features[i].feature_data;
			_matched[m] = features + i;
			_matched[m]->feature_data = rdata;
			m++;
		}
		*matched = _matched;
		return m;
}

/*
Calculates the minimum number of inliers as a function of the number of
putative correspondences.  Based on equation (7) in

Chum, O. and Matas, J.  Matching with PROSAC -- Progressive Sample Consensus.
In Conference on Computer Vision and Pattern Recognition (CVPR),
(2005), pp. 220--226.

@param n number of putative correspondences
@param m min number of correspondences to compute the model in question
@param p_badsupp prob. that a bad model is supported by a correspondence
@param p_badxform desired prob. that the final transformation returned is bad

@return Returns the minimum number of inliers required to guarantee, based
	on p_badsupp, that the probability that the final transformation returned
	by RANSAC is less than p_badxform
*/
int calc_min_inliers( int n, int m, double p_badsupp, double p_badxform )
{
	double pi, sum;
	int i, j;

	for( j = m+1; j <= n; j++ )
	{
		sum = 0;
		for( i = j; i <= n; i++ )
		{
			pi = ( i - m ) * log( p_badsupp ) + ( n - i + m ) * log( 1.0 - p_badsupp ) +
				gsl_sf_lnchoose( n - m, i - m );
			sum += exp( pi );
		}
		if( sum < p_badxform )
			break;
	}
	return j;
}

/*
Draws a RANSAC sample from a set of features.

@param features array of pointers to features from which to sample
@param n number of features in features
@param m size of the sample
@param rng random number generator used to sample

@return Returns an array of pointers to the sampled features; the sampled
	field of each sampled feature's ransac_data is set to 1
*/
struct feature** draw_ransac_sample( struct feature** features, int n,
									int m, gsl_rng* rng )
{
	struct feature** sample, * feat;
	struct ransac_data* rdata;
	int i, x;

	for( i = 0; i < n; i++ )
	{
		rdata = feat_ransac_data( features[i] );
		rdata->sampled = 0;
	}

	sample = calloc( m, sizeof( struct feature* ) );
	for( i = 0; i < m; i++ )
	{
		do
		{
			x = gsl_rng_uniform_int( rng, n );
			feat = features[x];
			rdata = feat_ransac_data( feat );
		}
		while( rdata->sampled );
		sample[i] = feat;
		rdata->sampled = 1;
	}

	return sample;
}

/*
Extrancs raw point correspondence locations from a set of features

@param features array of features from which to extract points and match
	points; each of these is assumed to have a match of type mtype
@param n number of features
@param mtype match type; if FEATURE_MDL_MATCH correspondences are assumed
	to be between each feature's img_pt field and it's match's mdl_pt field,
	otherwise, correspondences are assumed to be between img_pt and img_pt
@param pts output as an array of raw point locations from features
@param mpts output as an array of raw point locations from features' matches
*/
void extract_corresp_pts( struct feature** features, int n, int mtype,
						 CvPoint2D64f** pts, CvPoint2D64f** mpts )
{
	struct feature* match;
	CvPoint2D64f* _pts, * _mpts;
	int i;

	_pts = calloc( n, sizeof( CvPoint2D64f ) );
	_mpts = calloc( n, sizeof( CvPoint2D64f ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->mdl_pt;
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			_pts[i] = features[i]->img_pt;
			_mpts[i] = match->img_pt;
		}

		*pts = _pts;
		*mpts = _mpts;
}

/*
For a given model and error function, finds a consensus from a set of
feature correspondences.

@param features set of pointers to features; every feature is assumed to
	have a match of type mtype
@param n number of features in features
@param mtype determines the match field of each feature against which to
	measure error; if this is FEATURE_MDL_MATCH, correspondences are assumed
	to be between the feature's img_pt field and the match's mdl_pt field;
	otherwise matches are assumed to be between img_pt and img_pt
@param M model for which a consensus set is being found
@param err_fn error function used to measure distance from M
@param err_tol correspondences within this distance of M are added to the
	consensus set
@param consensus output as an array of pointers to features in the
	consensus set

@return Returns the number of points in the consensus set
*/
int find_consensus( struct feature** features, int n, int mtype,
				   CvMat* M, ransac_err_fn err_fn, double err_tol,
				   struct feature*** consensus )
{
	struct feature** _consensus;
	struct feature* match;
	CvPoint2D64f pt, mpt;
	double err;
	int i, in = 0;

	_consensus = calloc( n, sizeof( struct feature* ) );

	if( mtype == FEATURE_MDL_MATCH )
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->mdl_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}

	else
		for( i = 0; i < n; i++ )
		{
			match = get_match( features[i], mtype );
			if( ! match )
				fatal_error( "feature does not have match of type %d, %s line %d",
							mtype, __FILE__, __LINE__ );
			pt = features[i]->img_pt;
			mpt = match->img_pt;
			err = err_fn( pt, mpt, M );
			if( err <= err_tol )
				_consensus[in++] = features[i];
		}
	*consensus = _consensus;
	return in;
}

/*
Releases memory and reduces code size above

@param pts1 an array of points
@param pts2 an array of points
@param features an array of pointers to features; can be NULL
*/
static __inline void release_mem( CvPoint2D64f* pts1, CvPoint2D64f* pts2,
struct feature** features )
{
	free( pts1 );
	free( pts2 );
	if( features )
		free( features );
}


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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <gsl/gsl_integration.h>

#include "ccl.h"

//
// Macros for replacing relative paths
#define EXPAND_STR(s) STRING(s)
#define STRING(s) #s

const ccl_configuration default_config = {
  ccl_boltzmann_class, ccl_halofit, ccl_nobaryons,
  ccl_tinker10, ccl_duffy2008, ccl_emu_strict};

//Precision parameters
/**
 * Default relative precision if not otherwise specified
 */
#define GSL_EPSREL 1E-4

/**
 * Default number of iterations for integration and root-finding if not otherwise
 * specified
 */
#define GSL_N_ITERATION 1000

/**
 * Default number of Gauss-Kronrod points in QAG integration if not otherwise
 * specified
 */
#define GSL_INTEGRATION_GAUSS_KRONROD_POINTS GSL_INTEG_GAUSS41

/**
 * Relative precision in sigma_R calculations
 */
#define GSL_EPSREL_SIGMAR 1E-5

/**
 * Relative precision in k_NL calculations
 */
#define GSL_EPSREL_KNL 1E-5

/**
 * Relative precision in distance calculations
 */
#define GSL_EPSREL_DIST 1E-6

/**
 * Relative precision in growth calculations
 */
#define GSL_EPSREL_GROWTH 1E-6

/**
 * Relative precision in dNdz calculations
 */
#define GSL_EPSREL_DNDZ 1E-6

const ccl_gsl_params default_gsl_params = {
  GSL_N_ITERATION,                     // N_ITERATION
  GSL_INTEGRATION_GAUSS_KRONROD_POINTS,// INTEGRATION_GAUSS_KRONROD_POINTS
  GSL_EPSREL,                          // INTEGRATION_EPSREL
  GSL_INTEGRATION_GAUSS_KRONROD_POINTS,// INTEGRATION_LIMBER_GAUSS_KRONROD_POINTS
  GSL_EPSREL,                          // INTEGRATION_LIMBER_EPSREL
  GSL_EPSREL_DIST,                     // INTEGRATION_DISTANCE_EPSREL
  GSL_EPSREL_SIGMAR,                   // INTEGRATION_SIGMAR_EPSREL
  GSL_EPSREL_KNL,                      // INTEGRATION_KNL_EPSREL
  GSL_EPSREL,                          // ROOT_EPSREL
  GSL_N_ITERATION,                     // ROOT_N_ITERATION
  GSL_EPSREL_GROWTH,                   // ODE_GROWTH_EPSREL
  1E-6,                                // EPS_SCALEFAC_GROWTH
  1E7,                                 // HM_MMIN
  1E17,                                // HM_MMAX
  0.0,                                 // HM_EPSABS
  1E-4,                                // HM_EPSREL
  1000,                                // HM_LIMIT
  GSL_INTEG_GAUSS41                    // HM_INT_METHOD
  };

#undef GSL_EPSREL
#undef GSL_N_ITERATION
#undef GSL_INTEGRATION_GAUSS_KRONROD_POINTS
#undef GSL_EPSREL_SIGMAR
#undef GSL_EPSREL_KNL
#undef GSL_EPSREL_DIST
#undef GSL_EPSREL_GROWTH
#undef GSL_EPSREL_DNDZ

const ccl_spline_params default_spline_params = {

  // scale factor spline params
  250,  // A_SPLINE_NA
  0.1,  // A_SPLINE_MIN
  0.01,  // A_SPLINE_MINLOG_PK
  0.1,  // A_SPLINE_MIN_PK,
  0.01,  // A_SPLINE_MINLOG_SM,
  0.1,  // A_SPLINE_MIN_SM,
  1.0,  // A_SPLINE_MAX,
  0.0001,  // A_SPLINE_MINLOG,
  250,  // A_SPLINE_NLOG,

  // mass splines
  0.025,  // LOGM_SPLINE_DELTA
  50,  // LOGM_SPLINE_NM
  6,  // LOGM_SPLINE_MIN
  17,  // LOGM_SPLINE_MAX

  // PS a and k spline
  13,  // A_SPLINE_NA_SM
  6,  // A_SPLINE_NLOG_SM
  40,  // A_SPLINE_NA_PK
  11,  // A_SPLINE_NLOG_PK

  // k-splines and integrals
  50,  // K_MAX_SPLINE
  1E3,  // K_MAX
  5E-5,  // K_MIN
  0.025,  // DLOGK_INTEGRATION
  167,  // N_K
  100000,  // N_K_3DCOR

  // correlation function parameters
  0.01,  // ELL_MIN_CORR
  60000,  // ELL_MAX_CORR
  5000,  // N_ELL_CORR

  //Spline types
  NULL,
  NULL,
  NULL,
  NULL,
  NULL,
  NULL,
  NULL
};

ccl_physical_constants ccl_constants = {
  /**
   * Lightspeed / H0 in units of Mpc/h (from CODATA 2014)
   */
  2997.92458,

  /**
   * Newton's gravitational constant in units of m^3/Kg/s^2
   */
  //6.6738e-11,  /(from PDG 2013) in m^3/Kg/s^2
  //6.67428e-11, // CLASS VALUE
  6.67408e-11, // from CODATA 2014

  /**
   * Solar mass in units of kg (from GSL)
   */
  //GSL_CONST_MKSA_SOLAR_MASS,
  //1.9885e30, //(from PDG 2015) in Kg
  1.9884754153381438E+30, //from IAU 2015

  /**
   * Mpc to meters (from PDG 2016 and using M_PI)
   */
  3.085677581491367399198952281E+22,

  /**
   * pc to meters (from PDG 2016 and using M_PI)
   */
  3.085677581491367399198952281E+16,

  /**
   * Rho critical in units of M_sun/h / (Mpc/h)^3
   */
  ((3*100*100)/(8*M_PI*6.67408e-11)) * (1000*1000*3.085677581491367399198952281E+22/1.9884754153381438E+30),

  /**
   * Boltzmann constant in units of J/K
  */
  //GSL_CONST_MKSA_BOLTZMANN,
  1.38064852E-23, //from CODATA 2014

  /**
   * Stefan-Boltzmann constant in units of kg/s^3 / K^4
   */
  //GSL_CONST_MKSA_STEFAN_BOLTZMANN_CONSTANT,
  5.670367E-8, //from CODATA 2014

  /**
   * Planck's constant in units kg m^2 / s
   */
  //GSL_CONST_MKSA_PLANCKS_CONSTANT_H,
  6.626070040E-34, //from CODATA 2014

  /**
   * The speed of light in m/s
   */
  //GSL_CONST_MKSA_SPEED_OF_LIGHT,
  299792458.0, //from CODATA 2014

  /**
   * Electron volt to Joules convestion
   */
  //GSL_CONST_MKSA_ELECTRON_VOLT,
  1.6021766208e-19,  //from CODATA 2014

  /**
   * Temperature of the CMB in K
   */
  2.725,
  //2.7255, // CLASS value

  /**
   * T_ncdm, as taken from CLASS, explanatory.ini
   */
  0.71611,

  /**
   * neutrino mass splitting differences
   * See Lesgourgues and Pastor, 2012 for these values.
   * Adv. High Energy Phys. 2012 (2012) 608515,
   * arXiv:1212.6154, page 13
  */
  7.62E-5,
  2.55E-3,
  -2.43E-3
};

/* ------- ROUTINE: ccl_cosmology_create ------
INPUTS: ccl_parameters params
        ccl_configuration config
TASK: creates the ccl_cosmology struct and passes some values to it
DEFINITIONS:
chi: comoving distance [Mpc]
growth: growth function (density)
fgrowth: logarithmic derivative of the growth (density) (dlnD/da?)
E: E(a)=H(a)/H0
growth0: growth at z=0, defined to be 1
sigma: ?
p_lnl: nonlinear matter power spectrum at z=0?
computed_distances, computed_growth,
computed_power, computed_sigma: store status of the computations
*/
ccl_cosmology * ccl_cosmology_create(ccl_parameters params, ccl_configuration config)
{
  ccl_cosmology * cosmo = malloc(sizeof(ccl_cosmology));
  cosmo->params = params;
  cosmo->config = config;
  cosmo->gsl_params = default_gsl_params;
  cosmo->spline_params = default_spline_params;
  cosmo->spline_params.A_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.K_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.M_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.D_SPLINE_TYPE = gsl_interp_akima;
  cosmo->spline_params.PNL_SPLINE_TYPE = gsl_interp2d_bicubic;
  cosmo->spline_params.PLIN_SPLINE_TYPE = gsl_interp2d_bicubic;
  cosmo->spline_params.CORR_SPLINE_TYPE = gsl_interp_akima;

  cosmo->data.chi = NULL;
  cosmo->data.growth = NULL;
  cosmo->data.fgrowth = NULL;
  cosmo->data.E = NULL;
  cosmo->data.growth0 = 1.;
  cosmo->data.achi = NULL;

  cosmo->data.logsigma = NULL;

  cosmo->data.rsd_splines[0] = NULL;
  cosmo->data.rsd_splines[1] = NULL;
  cosmo->data.rsd_splines[2] = NULL;

  cosmo->computed_distances = false;
  cosmo->computed_growth = false;
  cosmo->computed_sigma = false;
  cosmo->status = 0;
  ccl_cosmology_set_status_message(cosmo, "");

  if(cosmo->spline_params.A_SPLINE_MAX !=1.) {
    cosmo->status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(cosmo,
                                     "ccl_core.c: ccl_cosmology_create(): "
                                     "A_SPLINE_MAX needs to be 1.\n");
  }

  return cosmo;
}

/* ------ ROUTINE: ccl_parameters_fill_initial -------
INPUT: ccl_parameters: params
TASK: fill parameters not set by ccl_parameters_create with some initial values
DEFINITIONS:
Omega_g = (Omega_g*h^2)/h^2 is the radiation parameter; "g" is for photons, as in CLASS
T_CMB: CMB temperature in Kelvin
Omega_l: Lambda
A_s: amplitude of the primordial PS, enforced here to initially set to NaN
sigma8: variance in 8 Mpc/h spheres for normalization of matter PS, enforced here to initially set to NaN
z_star: recombination redshift
 */
void ccl_parameters_fill_initial(ccl_parameters * params, int *status)
{
  // Fixed radiation parameters
  // Omega_g * h**2 is known from T_CMB
  params->T_CMB =  ccl_constants.T_CMB;
  // kg / m^3
  double rho_g = 4. * ccl_constants.STBOLTZ / pow(ccl_constants.CLIGHT, 3) * pow(params->T_CMB, 4);
  // kg / m^3
  double rho_crit =
    ccl_constants.RHO_CRITICAL *
    ccl_constants.SOLAR_MASS/pow(ccl_constants.MPC_TO_METER, 3) *
    pow(params->h, 2);
  params->Omega_g = rho_g/rho_crit;

  // Get the N_nu_rel from Neff and N_nu_mass
  params->N_nu_rel = params->Neff - params->N_nu_mass * pow(ccl_constants.TNCDM, 4) / pow(4./11.,4./3.);

  // Temperature of the relativistic neutrinos in K
  double T_nu= (params->T_CMB) * pow(4./11.,1./3.);
  // in kg / m^3
  double rho_nu_rel =
    params->N_nu_rel* 7.0/8.0 * 4. *
    ccl_constants.STBOLTZ / pow(ccl_constants.CLIGHT, 3) *
    pow(T_nu, 4);
  params-> Omega_nu_rel = rho_nu_rel/rho_crit;

  // If non-relativistic neutrinos are present, calculate the phase_space integral.
  if((params->N_nu_mass)>0) {
    params->Omega_nu_mass = ccl_Omeganuh2(
      1.0, params->N_nu_mass, params->m_nu, params->T_CMB, status) / ((params->h)*(params->h));
  }
  else{
    params->Omega_nu_mass = 0.;
  }

  params->Omega_m = params->Omega_b + params-> Omega_c + params->Omega_nu_mass;
  params->Omega_l = 1.0 - params->Omega_m - params->Omega_g - params->Omega_nu_rel - params->Omega_k;
  // Initially undetermined parameters - set to nan to trigger
  // problems if they are mistakenly used.
  if (isfinite(params->A_s)) {params->sigma8 = NAN;}
  if (isfinite(params->sigma8)) {params->A_s = NAN;}
  params->z_star = NAN;

  if(fabs(params->Omega_k)<1E-6)
    params->k_sign=0;
  else if(params->Omega_k>0)
    params->k_sign=-1;
  else
    params->k_sign=1;
  params->sqrtk=sqrt(fabs(params->Omega_k))*params->h/ccl_constants.CLIGHT_HMPC;
}

/* ------ ROUTINE: ccl_parameters_create -------
INPUT: numbers for the basic cosmological parameters needed by CCL
TASK: fill params with some initial values provided by the user
DEFINITIONS:
Omega_c: cold dark matter
Omega_b: baryons
Omega_m: matter
Omega_k: curvature
little omega_x means Omega_x*h^2
Neff : Effective number of neutrino speces
mnu : Pointer to either sum of neutrino masses or list of three masses.
mnu_type : how the neutrino mass(es) should be treated
w0: Dark energy eq of state parameter
wa: Dark energy eq of state parameter, time variation
H0: Hubble's constant in km/s/Mpc.
h: Hubble's constant divided by (100 km/s/Mpc).
A_s: amplitude of the primordial PS
n_s: index of the primordial PS

 */
ccl_parameters ccl_parameters_create(double Omega_c, double Omega_b, double Omega_k,
				     double Neff, double* mnu, int n_mnu,
				     double w0, double wa, double h, double norm_pk,
				     double n_s, double bcm_log10Mc, double bcm_etab,
				     double bcm_ks, double mu_0, double sigma_0,
				     int nz_mgrowth, double *zarr_mgrowth,
				     double *dfarr_mgrowth, int *status)
{
  #ifndef USE_GSL_ERROR
    gsl_set_error_handler_off();
  #endif

  ccl_parameters params;
  // Initialize params
  params.m_nu = NULL;
  params.z_mgrowth=NULL;
  params.df_mgrowth=NULL;
  params.sigma8 = NAN;
  params.A_s = NAN;
  params.Omega_c = Omega_c;
  params.Omega_b = Omega_b;
  params.Omega_k = Omega_k;
  params.Neff = Neff;
  params.m_nu = malloc(n_mnu*sizeof(double));
  params.sum_nu_masses = 0.;
  for(int i = 0; i<n_mnu; i=i+1){
     params.m_nu[i] = mnu[i];
     params.sum_nu_masses = params.sum_nu_masses + mnu[i];
  }

  if(params.sum_nu_masses<1e-15){
    params.N_nu_mass = 0;
  }else{
    params.N_nu_mass = n_mnu;
   }

  // Dark Energy
  params.w0 = w0;
  params.wa = wa;

  // Hubble parameters
  params.h = h;
  params.H0 = h*100;

  // Primordial power spectra
  if(norm_pk<1E-5)
    params.A_s=norm_pk;
  else
    params.sigma8=norm_pk;
  params.n_s = n_s;

  //Baryonic params
  if(bcm_log10Mc<0)
    params.bcm_log10Mc=log10(1.2e14);
  else
    params.bcm_log10Mc=bcm_log10Mc;
  if(bcm_etab<0)
    params.bcm_etab=0.5;
  else
    params.bcm_etab=bcm_etab;
  if(bcm_ks<0)
    params.bcm_ks=55.0;
  else
    params.bcm_ks=bcm_ks;

  // Params of the mu / Sigma parameterisation of MG
  params.mu_0 = mu_0;
  params.sigma_0 = sigma_0;

  // Set remaining standard and easily derived parameters
  ccl_parameters_fill_initial(&params, status);

  //Trigger modified growth function if nz>0
  if(nz_mgrowth>0) {
    params.has_mgrowth=true;
    params.nz_mgrowth=nz_mgrowth;
    params.z_mgrowth=malloc(params.nz_mgrowth*sizeof(double));
    params.df_mgrowth=malloc(params.nz_mgrowth*sizeof(double));
    memcpy(params.z_mgrowth,zarr_mgrowth,params.nz_mgrowth*sizeof(double));
    memcpy(params.df_mgrowth,dfarr_mgrowth,params.nz_mgrowth*sizeof(double));
  }
  else {
    params.has_mgrowth=false;
    params.nz_mgrowth=0;
    params.z_mgrowth=NULL;
    params.df_mgrowth=NULL;
  }

  return params;
}

/**
 * Write a cosmology parameters object to a file in yaml format.
 * @param cosmo Cosmological parameters
 * @param f FILE* pointer opened for reading
 * @return void
 */
void ccl_parameters_write_yaml(ccl_parameters * params, const char * filename, int *status)
{
  FILE * f = fopen(filename, "w");

  if (!f){
    *status = CCL_ERROR_FILE_WRITE;
    return;
  }

#define WRITE_DOUBLE(name) fprintf(f, #name ": %le\n",params->name)
#define WRITE_INT(name) fprintf(f, #name ": %d\n",params->name)

  // Densities: CDM, baryons, total matter, curvature
  WRITE_DOUBLE(Omega_c);
  WRITE_DOUBLE(Omega_b);
  WRITE_DOUBLE(Omega_m);
  WRITE_DOUBLE(Omega_k);
  WRITE_INT(k_sign);

  // Dark Energy
  WRITE_DOUBLE(w0);
  WRITE_DOUBLE(wa);

  // Hubble parameters
  WRITE_DOUBLE(H0);
  WRITE_DOUBLE(h);

  // Neutrino properties
  WRITE_DOUBLE(Neff);
  WRITE_INT(N_nu_mass);
  WRITE_DOUBLE(N_nu_rel);

  if (params->N_nu_mass>0){
    fprintf(f, "m_nu: [");
    for (int i=0; i<params->N_nu_mass; i++){
      fprintf(f, "%le, ", params->m_nu[i]);
    }
    fprintf(f, "]\n");
  }

  WRITE_DOUBLE(sum_nu_masses);
  WRITE_DOUBLE(Omega_nu_mass);
  WRITE_DOUBLE(Omega_nu_rel);

  // Primordial power spectra
  WRITE_DOUBLE(A_s);
  WRITE_DOUBLE(n_s);

  // Radiation parameters
  WRITE_DOUBLE(Omega_g);
  WRITE_DOUBLE(T_CMB);

  // BCM baryonic model parameters
  WRITE_DOUBLE(bcm_log10Mc);
  WRITE_DOUBLE(bcm_etab);
  WRITE_DOUBLE(bcm_ks);

  // Modified gravity parameters
  WRITE_DOUBLE(mu_0);
  WRITE_DOUBLE(sigma_0);

  // Derived parameters
  WRITE_DOUBLE(sigma8);
  WRITE_DOUBLE(Omega_l);
  WRITE_DOUBLE(z_star);

  WRITE_INT(has_mgrowth);
  WRITE_INT(nz_mgrowth);

  if (params->has_mgrowth){
    fprintf(f, "z_mgrowth: [");
    for (int i=0; i<params->nz_mgrowth; i++){
      fprintf(f, "%le, ", params->z_mgrowth[i]);
    }
    fprintf(f, "]\n");

    fprintf(f, "df_mgrowth: [");
    for (int i=0; i<params->nz_mgrowth; i++){
      fprintf(f, "%le, ", params->df_mgrowth[i]);
    }
    fprintf(f, "]\n");
  }

#undef WRITE_DOUBLE
#undef WRITE_INT

  fclose(f);
}

/**
 * Write a cosmology parameters object to a file in yaml format.
 * @param cosmo Cosmological parameters
 * @param f FILE* pointer opened for reading
 * @return void
 */
ccl_parameters ccl_parameters_read_yaml(const char * filename, int *status) {
  FILE * f = fopen(filename, "r");

  if (!f) {
    *status = CCL_ERROR_FILE_READ;
    ccl_parameters bad_params;
    ccl_raise_warning(CCL_ERROR_FILE_READ,
                      "ccl_core.c: ccl_parameters_read_yaml(): "
                      "Failed to read parameters from file.");

    return bad_params;
  }

#define READ_DOUBLE(name) double name; *status |= (0==fscanf(f, #name ": %le\n",&name));
#define READ_INT(name) int name; *status |= (0==fscanf(f, #name ": %d\n",&name))

  // Densities: CDM, baryons, total matter, curvature
  READ_DOUBLE(Omega_c);
  READ_DOUBLE(Omega_b);
  READ_DOUBLE(Omega_m);
  READ_DOUBLE(Omega_k);
  READ_INT(k_sign);

  // Dark Energy
  READ_DOUBLE(w0);
  READ_DOUBLE(wa);

  // Hubble parameters
  READ_DOUBLE(H0);
  READ_DOUBLE(h);

  // Neutrino properties
  READ_DOUBLE(Neff);
  READ_INT(N_nu_mass);
  READ_DOUBLE(N_nu_rel);

  double mnu[3] = {0.0, 0.0, 0.0};
  if (N_nu_mass>0){
    *status |= (0==fscanf(f, "m_nu: ["));
    for (int i=0; i<N_nu_mass; i++){
      *status |= (0==fscanf(f, "%le, ", mnu+i));
    }
    *status |= (0==fscanf(f, "]\n"));
  }

  READ_DOUBLE(sum_nu_masses);
  READ_DOUBLE(Omega_nu_mass);
  READ_DOUBLE(Omega_nu_rel);

  // Primordial power spectra
  READ_DOUBLE(A_s);
  READ_DOUBLE(n_s);

  // Radiation parameters
  READ_DOUBLE(Omega_g);
  READ_DOUBLE(T_CMB);

  // BCM baryonic model parameters
  READ_DOUBLE(bcm_log10Mc);
  READ_DOUBLE(bcm_etab);
  READ_DOUBLE(bcm_ks);

  // Modified gravity parameters
  READ_DOUBLE(mu_0);
  READ_DOUBLE(sigma_0);

  // Derived parameters
  READ_DOUBLE(sigma8);
  READ_DOUBLE(Omega_l);
  READ_DOUBLE(z_star);

  READ_INT(has_mgrowth);
  READ_INT(nz_mgrowth);

  double *z_mgrowth;
  double *df_mgrowth;

  if (has_mgrowth){
    z_mgrowth = malloc(nz_mgrowth*sizeof(double));
    df_mgrowth = malloc(nz_mgrowth*sizeof(double));
    *status |= (0==fscanf(f, "z_mgrowth: ["));
    for (int i=0; i<nz_mgrowth; i++){
      *status |= (0==fscanf(f, "%le, ", z_mgrowth+i));
    }
    *status |= (0==fscanf(f, "]\n"));

    *status |= (0==fscanf(f, "df_mgrowth: ["));
    for (int i=0; i<nz_mgrowth; i++){
      *status |= (0==fscanf(f, "%le, ", df_mgrowth+i));
    }
    *status |= (0==fscanf(f, "]\n"));
  }
  else{
    z_mgrowth = NULL;
    df_mgrowth = NULL;
  }

#undef READ_DOUBLE
#undef READ_INT

  fclose(f);

  if (*status) {
    ccl_raise_warning(
      *status,
      "ccl_core.c: ccl_parameters_read_yaml():"
      "Structure of YAML file incorrect: %s",
      filename);
  }

  double norm_pk;

  if (isnan(A_s)){
    norm_pk = sigma8;
  }
  else{
   norm_pk = A_s;
  }

  ccl_parameters params = ccl_parameters_create(
    Omega_c, Omega_b, Omega_k,
    Neff, mnu, N_nu_mass,
    w0, wa, h, norm_pk,
    n_s, bcm_log10Mc, bcm_etab,
    bcm_ks, mu_0, sigma_0, nz_mgrowth, z_mgrowth,
    df_mgrowth, status);

  if(z_mgrowth) free(z_mgrowth);
  if (df_mgrowth) free(df_mgrowth);

  return params;
}

/* ------- ROUTINE: ccl_data_free --------
INPUT: ccl_data
TASK: free the input data
*/
void ccl_data_free(ccl_data * data) {
  //We cannot assume that all of these have been allocated
  //TODO: it would actually make more sense to do this within ccl_cosmology_free,
  //where we could make use of the flags "computed_distances" etc. to figure out
  //what to free up
  gsl_spline_free(data->chi);
  gsl_spline_free(data->growth);
  gsl_spline_free(data->fgrowth);
  gsl_spline_free(data->E);
  gsl_spline_free(data->achi);
  gsl_spline2d_free(data->logsigma);
  ccl_f1d_t_free(data->rsd_splines[0]);
  ccl_f1d_t_free(data->rsd_splines[1]);
  ccl_f1d_t_free(data->rsd_splines[2]);
}

/* ------- ROUTINE: ccl_cosmology_set_status_message --------
INPUT: ccl_cosmology struct, status_string
TASK: set the status message safely.
*/
void ccl_cosmology_set_status_message(ccl_cosmology * cosmo, const char * message, ...) {
  const int trunc = 480; /* must be < 500 - 4 */

  va_list va;
  va_start(va, message);

  #pragma omp critical
  {
    vsnprintf(cosmo->status_message, trunc, message, va);

    /* if truncation happens, message[trunc - 1] is not NULL, ... will show up. */
    strcpy(&cosmo->status_message[trunc], "...");
  }

  va_end(va);
}

/* ------- ROUTINE: ccl_parameters_free --------
INPUT: ccl_parameters struct
TASK: free allocated quantities in the parameters struct
*/
void ccl_parameters_free(ccl_parameters * params) {
  if (params->m_nu != NULL){
    free(params->m_nu);
    params->m_nu = NULL;
  }
  if (params->z_mgrowth != NULL){
    free(params->z_mgrowth);
    params->z_mgrowth = NULL;
  }
  if (params->df_mgrowth != NULL){
    free(params->df_mgrowth);
    params->df_mgrowth = NULL;
  }
}

/* ------- ROUTINE: ccl_cosmology_free --------
INPUT: ccl_cosmology struct
TASK: free the input data and the cosmology struct
*/
void ccl_cosmology_free(ccl_cosmology * cosmo) {
  if (cosmo != NULL)
    ccl_data_free(&cosmo->data);
  free(cosmo);
}

int ccl_get_pk_spline_na(ccl_cosmology *cosmo) {
  return cosmo->spline_params.A_SPLINE_NA_PK + cosmo->spline_params.A_SPLINE_NLOG_PK - 1;
}

void ccl_get_pk_spline_a_array(ccl_cosmology *cosmo,int ndout,double* doutput,int *status) {
  double *d = NULL;
  if (ndout != ccl_get_pk_spline_na(cosmo))
    *status = CCL_ERROR_INCONSISTENT;
  if (*status == 0) {
    d = ccl_linlog_spacing(cosmo->spline_params.A_SPLINE_MINLOG_PK,
      cosmo->spline_params.A_SPLINE_MIN_PK,
      cosmo->spline_params.A_SPLINE_MAX,
      cosmo->spline_params.A_SPLINE_NLOG_PK,
      cosmo->spline_params.A_SPLINE_NA_PK);
    if (d == NULL)
      *status = CCL_ERROR_MEMORY;
  }
  if(*status==0)
    memcpy(doutput, d, ndout*sizeof(double));

  free(d);
}

int ccl_get_pk_spline_nk(ccl_cosmology *cosmo) {
  double ndecades = log10(cosmo->spline_params.K_MAX) - log10(cosmo->spline_params.K_MIN);
  return (int)ceil(ndecades*cosmo->spline_params.N_K);
}

void ccl_get_pk_spline_lk_array(ccl_cosmology *cosmo,int ndout,double* doutput,int *status) {
  double *d = NULL;
  if (ndout != ccl_get_pk_spline_nk(cosmo))
    *status = CCL_ERROR_INCONSISTENT;
  if (*status == 0) {
    d = ccl_log_spacing(cosmo->spline_params.K_MIN, cosmo->spline_params.K_MAX, ndout);
    if (d == NULL)
      *status = CCL_ERROR_MEMORY;
  }
  if (*status == 0) {
    for(int ii=0; ii < ndout; ii++)
      doutput[ii] = log(d[ii]);
  }
  free(d);
}


/*
 * Barcode
 * Copyright E.G.P. Bos and F.S. Kitaura
 *
 * Distributed under the terms of the MIT License.
 * The full license is in the file LICENSE, distributed with this software.
 */

#pragma once

// EGP: cleared out most includes, just left the two here:
#include <gsl/gsl_rng.h>  // For the gsl_rng type // EGP: added this one
#include "define_opt.h"  // For the ULONG and real_prec types
// #include "curses_funcs.h"
// EGP: toch maar weer terug gezet, gingen dingen mis
/* #include <math.h> */
/* #include <fstream> */
/* #include <iomanip> */
// #include <iostream>
/* #include <string>          */
/* #include <string.h> */
/* #include <cassert> */
/* #include <cfloat> */
/* #include <stdlib.h> */
/* #include <stdio.h> */
/* #include <fftw3.h> */
/* #include <omp.h> */
/* #include <sstream> */
/* #include <netinet/in.h> */

/* #include <gsl/gsl_errno.h> */
/* #include <gsl/gsl_spline.h> */
/* #include <gsl/gsl_randist.h> */
/* #include <gsl/gsl_integration.h> */
/* #include <gsl/gsl_sf.h> */

/* #include "fftw_array.h" */

/* #include "define_opt.h" */

#include <vector>
#include "HMC_models.h"
#include "struct_main.h"
#include <sstream>

using namespace std;

/* --- structures --- */

struct HAMIL_NUMERICAL {
  unsigned N1;  /* Number of cells per axis */
  unsigned N2;  /* Number of cells per axis */
  unsigned N3;  /* Number of cells per axis */
  ULONG N;
  ULONG Nhalf;

  int mk;

  real_prec L1;
  real_prec L2;
  real_prec L3;

  real_prec vol;

  real_prec d1;
  real_prec d2;
  real_prec d3;

  real_prec min1;
  real_prec min2;
  real_prec min3;

  // Redshift space distortions
  real_prec xobs;
  real_prec yobs;
  real_prec zobs;
  bool planepar;
  bool periodic;

  ULONG N_bin;

  ULONG iGibbs;
  ULONG rejections;
  ULONG total_steps_lim;

  // EGP: some extra parameters for statistics on performance:
  bool accepted;
  real_prec epsilon;
  ULONG Neps;
  real_prec dH, dK, dE, dprior, dlikeli, psi_prior, psi_likeli, psi_prior_i,
  psi_prior_f, psi_likeli_i, psi_likeli_f, H_kin_i, H_kin_f;

  int method;			/* which method */
  int filter;			/* which filter method */
  ULONG itmax;			/* maximal number of iterations */
  int i_reset;			/* number to reset */
  int INV_SUCCESS;		/* indicate inversion success */

  real_prec epsi;  /* precision */

  real_prec kth;

  // EGP: pseudo-timestep parameters
  real_prec N_eps_fac;
  real_prec eps_fac;
  real_prec eps_fac_target;
  real_prec eps_fac_power;
  ULONG s_eps_total;

  // mass type
  bool mass_fs;
  bool mass_rs;
  int mass_type;
  ULONG massnum_init;
  ULONG massnum_burn;

  // EGP: testing:
  real_prec mass_factor;

  /// TESTING ///
  real_prec grad_psi_prior_factor;
  real_prec grad_psi_likeli_factor;
  bool grad_psi_prior_conjugate;
  bool grad_psi_likeli_conjugate;
  bool grad_psi_prior_times_i;
  bool grad_psi_likeli_times_i;
  bool div_dH_by_N;
  int calc_h;
  real_prec deltaQ_factor;

  bool correct_delta;

  int particle_kernel;
  real_prec particle_kernel_h;

  // FFTW
  real_prec *in_r2c;
  real_prec *out_c2r;
  complex_prec *in_c2r;
  complex_prec *out_r2c;
  struct plan_pkg *R2Cplan;
  struct plan_pkg *C2Rplan;
};

struct HAMIL_DATA {
  struct HAMIL_NUMERICAL *numerical;  // numerical parameters

  // set arrays
  real_prec *mass_f;
  real_prec *mass_r;
  // real_prec *massi;
  real_prec *gradpsi;
  real_prec *x;
  // real_prec *growth;

  real_prec *posx;
  real_prec *posy;
  real_prec *posz;

  real_prec *signal_PS;
  real_prec *window;
  real_prec *func;
  real_prec *noise;
  real_prec *nobs;
  real_prec *deltaX;
  real_prec *corrf;

  // function pointers
  void (*partial_f_delta_x_log_like)(struct HAMIL_DATA*, real_prec*,
                                     real_prec*);
  real_prec (*log_like)(struct HAMIL_DATA*, real_prec*);
  void (*grad_f_delta_x_comp)(struct HAMIL_DATA *, real_prec *, real_prec *, unsigned int);

  real_prec (*log_prior)(struct HAMIL_DATA*, real_prec*);
  void (*grad_log_prior)(struct HAMIL_DATA*, real_prec*, real_prec*);

  // model flags
  int sfmodel;
  bool rsd_model;

  // set scalars
  real_prec Nmean_Gal;
  real_prec rho_c;
  real_prec code_norm;
  real_prec mu;

  real_prec sigma_fac;
  real_prec sigma_min;
  real_prec delta_min;

  real_prec biasP;
  real_prec biasE;

  real_prec z;
  real_prec ascale;

  real_prec D1;
  real_prec D2;

  real_prec OM;
  real_prec OL;
  real_prec h;

  // external inputs
  real_prec *M;

  // curses
  // struct CURSES_STRUCT *curses;

  // stuff for SPH_kernel_3D_cells
  const int N_cells;
  const std::vector<int> kernel_cells_i, kernel_cells_j, kernel_cells_k;

  // default constructor; initialize everything yourself!
  HAMIL_DATA() : N_cells(0), kernel_cells_i(0, 0), kernel_cells_j(0, 0),
  kernel_cells_k(0, 0) {
    numerical = new HAMIL_NUMERICAL;
  }

  // secondary constructor; used by call_hamil
  HAMIL_DATA(struct DATA *data, real_prec *A, real_prec *B_f, real_prec *B_r,
             real_prec *C, real_prec *D, real_prec *E, vector<int> cells_i,
             std::vector<int> cells_j, std::vector<int> cells_k,
             int N_cells_i) :
  N_cells(N_cells_i),
  kernel_cells_i(cells_i.begin(), cells_i.end()),
  kernel_cells_j(cells_j.begin(), cells_j.end()),
  kernel_cells_k(cells_k.begin(), cells_k.end()) {
    numerical = new HAMIL_NUMERICAL;

    numerical->N1 = data->numerical->N1;
    numerical->N2 = data->numerical->N2;
    numerical->N3 = data->numerical->N3;
    numerical->N = data->numerical->N;
    numerical->Nhalf = data->numerical->Nhalf;
    numerical->L1 = data->numerical->L1;
    numerical->L2 = data->numerical->L2;
    numerical->L3 = data->numerical->L3;
    numerical->vol = data->numerical->vol;
    numerical->d1 = data->numerical->d1;
    numerical->d2 = data->numerical->d2;
    numerical->d3 = data->numerical->d3;
    numerical->min1 = data->numerical->xllc;
    numerical->min2 = data->numerical->yllc;
    numerical->min3 = data->numerical->zllc;
    // Redshift space distortions
    numerical->xobs = data->numerical->xobs;
    numerical->yobs = data->numerical->yobs;
    numerical->zobs = data->numerical->zobs;
    numerical->planepar = data->numerical->planepar;
    numerical->periodic = data->numerical->periodic;

    numerical->N_bin = data->numerical->N_bin;
    numerical->mk = data->numerical->mk;

    numerical->kth = data->numerical->slength;

    numerical->iGibbs = data->numerical->iGibbs;

    numerical->rejections = 0; // EGP: added for rejection rate
    numerical->total_steps_lim = data->numerical->total_steps_lim;

    numerical->itmax = 2000;
    numerical->INV_SUCCESS = 0;/* indicate inversion success initialize with 0*/

    numerical->N_eps_fac = data->numerical->N_eps_fac;
    numerical->eps_fac = data->numerical->eps_fac;
    numerical->eps_fac_target = data->numerical->eps_fac_target;
    numerical->eps_fac_power = data->numerical->eps_fac_power;
    numerical->s_eps_total = data->numerical->s_eps_total;

    numerical->mass_type = data->numerical->mass_type;
    switch (numerical->mass_type) {
      case 0:  // 0: R: all ones (essentially: no mass term)
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      case 1:  // 1: FS: inverse power spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 2:  // 2: FS+FS: inverse power spectrum + likelihood force spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 3:  // 3: FS+FS: inverse power spectrum + mean likelihood force (Wang+13)
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 4:  // 4: FS: power spectrum
      numerical->mass_rs = false;
      numerical->mass_fs = true;
      break;
      case 5:  // 5: FS+R: inverse power spectrum + 1st order likelihood force expansion (Jasche+13)
      numerical->mass_rs = true;
      numerical->mass_fs = true;
      break;
      case 6:  // 1st order likelihood force expansion (Jasche+13) (R)
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      case 60:  // type 0 until burn-in, type 6 afterwards
      numerical->mass_rs = true;
      numerical->mass_fs = false;
      break;
      default:
      stringstream message;
      message << "mass_type " << numerical->mass_type << " is not a valid value!";
      throw runtime_error(message.str());
    }

    numerical->massnum_init = data->numerical->massnum_init;
    numerical->massnum_burn = data->numerical->massnum_burn;

    gradpsi = A;
    mass_f = B_f;
    mass_r = B_r;
    posx = C;
    posy = D;
    posz = E;

    z = data->cosmology->z;
    ascale = data->cosmology->ascale;

    D1 = data->cosmology->D1;
    D2 = data->cosmology->D2;

    OM = data->cosmology->omega_m;
    OL = data->cosmology->omega_q;
    h = data->cosmology->h;

    biasP = data->observational->biasP;
    biasE = data->observational->biasE;

    x = data->observational->signal;
    nobs = data->observational->nobs;
    deltaX = data->observational->signalX;
    noise = data->observational->noise_sf;
    corrf = data->observational->corrf;

    Nmean_Gal = data->observational->Nmean_Gal;
    rho_c = data->observational->rho_c;
    code_norm = data->numerical->code_norm;
    mu = data->observational->mu;

    sigma_fac = data->observational->sigma_fac;
    sigma_min = data->observational->sigma_min;
    delta_min = data->observational->delta_min;

    signal_PS = data->observational->Power;

    window = data->observational->window;

    // function pointers
    log_like = data->observational->log_like;
    partial_f_delta_x_log_like = data->observational->partial_f_delta_x_log_like;
    grad_f_delta_x_comp = data->observational->grad_f_delta_x_comp;
    log_prior = data->observational->log_prior;
    grad_log_prior = data->observational->grad_log_prior;

    // model flags
    sfmodel = data->numerical->sfmodel;
    rsd_model = data->numerical->rsd_model;

    // Testing:
    numerical->mass_factor = data->numerical->mass_factor;
    numerical->grad_psi_prior_factor = data->numerical->grad_psi_prior_factor;
    numerical->grad_psi_likeli_factor = data->numerical->grad_psi_likeli_factor;
    numerical->grad_psi_prior_conjugate = data->numerical->grad_psi_prior_conjugate;
    numerical->grad_psi_likeli_conjugate = data->numerical->grad_psi_likeli_conjugate;
    numerical->grad_psi_prior_times_i = data->numerical->grad_psi_prior_times_i;
    numerical->grad_psi_likeli_times_i = data->numerical->grad_psi_likeli_times_i;
    numerical->div_dH_by_N = data->numerical->div_dH_by_N;
    numerical->calc_h = data->numerical->calc_h;
    numerical->deltaQ_factor = data->numerical->deltaQ_factor;

    numerical->particle_kernel = data->numerical->particle_kernel;
    numerical->particle_kernel_h = data->numerical->particle_kernel_h;

    // FFTW
    numerical->in_r2c = data->numerical->in_r2c;
    numerical->out_c2r = data->numerical->out_c2r;
    numerical->in_c2r = data->numerical->in_c2r;
    numerical->out_r2c = data->numerical->out_r2c;
    numerical->R2Cplan = data->numerical->R2Cplan;
    numerical->C2Rplan = data->numerical->C2Rplan;

    // curses
    // curses = data->curses;

    numerical->correct_delta = data->numerical->correct_delta;
  }

  ~HAMIL_DATA() {
    delete numerical;
  }
};