/**
 */
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_interp.h>
#include "aXe_utils.h"
#include "aXe_errors.h"
#include "fringe_conf.h"
#include "fringe_model.h"

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

/**
 * Function: compute_fringe_amplitude
 * The function computes the fringe image for a CCD setup stored
 * in a fringe configuration structure. This fringe configuration
 * structure completely describes the problem, and this function
 * executes the loops for every pixel over the wavelength
 * range spanned by the filter.
 *
 * Parameters
 * @param fconf - the fringe configuration structure
 *
 * Returns:
 * @return fringe_image - the image with the computed fringe amplitudes
 */
gsl_matrix  *
compute_fringe_amplitude(fringe_conf *fconf)
{
  gsl_matrix *fringe_image;

  gsl_vector **filter_vectors;

  int index=0;
  int ii=0;
  int jj=0;

  double lambda_mean;
  double pixel_ampl;
  //double phase_number;

  optical_property *optprops;

  // allocate the fringe image
  fringe_image = alloc_fringe_image(fconf->opt_layers);

  // allocate memory for the optical property structure
  optprops = alloc_optprops_list(fconf);

  // find the exact wavelength range,
  // and define the wavelength values and
  // normalized fiter throughput there
  filter_vectors = evaluate_wavelength_steps(fconf);
  //filter_vectors = get_PET_calibration_data();

  // compute the mean wavelength
  lambda_mean = gsl_vector_get(filter_vectors[0],filter_vectors[0]->size-1)/2.0
		 + gsl_vector_get(filter_vectors[0],0)/2.0;

  // initialize some values in the optical property list
  init_optprops_list(fconf, lambda_mean, optprops);

    for (ii=0; ii < (int)fringe_image->size1; ii++)
      //    for (ii=0; ii < 2; ii++)
    {
      fprintf(stderr, "Computing row No.: %i\n", ii);
            for (jj=0; jj < (int)fringe_image->size2; jj++)
	      //for (jj=0; jj < 2; jj++)
	{
	  // fill the optical thickness of the layers
	  // into the structure
	  fill_optprops_thickness(fconf->opt_layers, ii, jj, optprops);

	  pixel_ampl = 0.0;
	  for (index=0; index < (int)filter_vectors[0]->size; index++)
	    {
	      // fill all information in the optical
	      // property list
	      fill_optprops_all(fconf->opt_layers,
				gsl_vector_get(filter_vectors[0],index),
				optprops);

	      // compute and add the contribution at a wavelength
	      pixel_ampl += gsl_vector_get(filter_vectors[1],index)*
		fringe_contrib_single(optprops, fconf);
	    }

	  // finally set the pixel value
	  // in the output image
	  gsl_matrix_set(fringe_image, ii, jj,
			 fconf->fringe_amp * pixel_ampl + 1.0);
	}
    }

  // release the memory in the vectors
  gsl_vector_free(filter_vectors[0]);
  gsl_vector_free(filter_vectors[1]);
  free(filter_vectors);

  // free the optical property structure
  free_optprops_list(optprops);

  // return the fringe image
  return fringe_image;
}

/**
 * Function: alloc_fringe_image
 * The function browses through the the structure for the CCD layers
 * and extracts all information on image sizes stored there in
 * one or several thickness images. This information is checked
 * for consistency.
 * Finally, a matrix for a fringe image is allocated, initialized
 * and returned.
 *
 * Parameters:
 * @param opt_layers - the optical layers in the CCD
 *
 * Returns:
 * @return fringe_image - the allocated matrix for the fringe image
 */
gsl_matrix  *
alloc_fringe_image(const ccd_layers *opt_layers)
{
  int n1         = 0;
  int n2         = 0;
  int n1_new     = 0;
  int n2_new     = 0;
  int index      = 0;
  int is_defined = 0;

  gsl_matrix *fringe_image=NULL;
  // go through all CCD layers
  for (index=0; index < opt_layers->num_layers; index++)
    {
      // check whether the thickness information
      // is represented by an image
      if (opt_layers->opt_layer[index]->thickness2D != NULL)
	{

	  // mark that an image was found
	  is_defined=1;

	  // get the dimension of the image
	  n1_new = opt_layers->opt_layer[index]->thickness2D->size1;
	  n2_new = opt_layers->opt_layer[index]->thickness2D->size2;

	  // check the frst axix value against
	  // previous values, if possible
	  if (n1 && n1_new != n1)
	    // give an error if the new value is different
	    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
			 "Thickness image of layer %i "
			 "has %i pixels in first axis."
			 "This differs from the previous value %i\n",
			 index, n1_new, n1);
	  else
	    // store the new value
	    n1 = n1_new;

	  // check the frst axix value against
	  // previous values, if possible
	  if (n2 && n2_new != n2)
	    // give an error if the new value is different
	    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
			 "Thickness image of layer %i "
			 "has %i pixels in second axis."
			 "This differs from the previous value %i\n",
			 index, n2_new, n2);
	  else
	    // store the new value
	    n2 = n2_new;
	}
    }

#ifdef DEBUGFCONF
  fprintf(stdout, "Allocating an image with size: (%i, %i)\n", n1, n2);
#endif

  // check whether at least one image was
  // found as thickness information
  if (is_defined)
    {
      // allocate the image
      fringe_image =  gsl_matrix_alloc(n1, n2);

      // set all vallues to zero
      gsl_matrix_set_all(fringe_image, 0.0);
    }
  else
    {
      // no image found; report the error
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "The size of the fringe image is unknown\n"
		   "since none of the layers has an image "
		   "to specify its thickness!\n");
    }

  // return the image
  return fringe_image;
}

/**
 * Function: get_calibration_data()
 * This function is a data storage for filter throughput values
 * as computed by the orioginal program 'final.f' of J.Walsh.
 * The cubic spline routine used there turned out to give
 * (slightly) different results than the gsl-routine used
 * here. To be able making a in-depth comparison the throughuts
 * were computed for the filter 'ccd_ref.fits' and stored here.
 *
 * Parameters:
 *
 * Returns:
 * @return double_vector - interpolated standard filter throughputs
 */
gsl_vector **
get_calibration_data()
{
  gsl_vector *lambda_values;
  gsl_vector *through_values;

  gsl_vector **double_vector;

  // allocate space for the return vector
  double_vector  = (gsl_vector **)malloc(2*sizeof (gsl_vector *));

  // allocate the space for the vectors
  lambda_values  = gsl_vector_alloc(43);
  through_values = gsl_vector_alloc(43);

  // what follows is the wavelength steps as well as the normalized
  // filter throughputs which were computed in the original
  // program 'final.f' of J.Walsh.
  // The gsl-cubic spline routine yields slightly different values.
  // The values fixed here are good for a detailed cross-check
  // of the two programs.
  gsl_vector_set(through_values, 0, 0.0000000000000000);gsl_vector_set(lambda_values, 0,   0.9179);
  gsl_vector_set(through_values, 1, 0.0095066184313808);gsl_vector_set(lambda_values, 1,   0.9180);
  gsl_vector_set(through_values, 2, 0.0176006475945001);gsl_vector_set(lambda_values, 2,   0.9181);
  gsl_vector_set(through_values, 3, 0.0240259215044616);gsl_vector_set(lambda_values, 3,   0.9182);
  gsl_vector_set(through_values, 4, 0.0289185490230508);gsl_vector_set(lambda_values, 4,   0.9183);
  gsl_vector_set(through_values, 5, 0.0324146730103703);gsl_vector_set(lambda_values, 5,   0.9184);
  gsl_vector_set(through_values, 6, 0.0346504181940869);gsl_vector_set(lambda_values, 6,   0.9185);
  gsl_vector_set(through_values, 7, 0.0357619115684218);gsl_vector_set(lambda_values, 7,   0.9186);
  gsl_vector_set(through_values, 8, 0.0358852710613785);gsl_vector_set(lambda_values, 8,   0.9187);
  gsl_vector_set(through_values, 9, 0.0351566282002872);gsl_vector_set(lambda_values, 9,   0.9188);
  gsl_vector_set(through_values,10, 0.0337121099793692);gsl_vector_set(lambda_values,10,   0.9189);
  gsl_vector_set(through_values,11, 0.0316878433928458);gsl_vector_set(lambda_values,11,   0.9190);
  gsl_vector_set(through_values,12, 0.0292199509018293);gsl_vector_set(lambda_values,12,   0.9191);
  gsl_vector_set(through_values,13, 0.0264445572339866);gsl_vector_set(lambda_values,13,   0.9192);
  gsl_vector_set(through_values,14, 0.0234977916500934);gsl_vector_set(lambda_values,14,   0.9193);
  gsl_vector_set(through_values,15, 0.0205157811443709);gsl_vector_set(lambda_values,15,   0.9194);
  gsl_vector_set(through_values,16, 0.0176346504444861);gsl_vector_set(lambda_values,16,   0.9195);
  gsl_vector_set(through_values,17, 0.0149905288112146);gsl_vector_set(lambda_values,17,   0.9196);
  gsl_vector_set(through_values,18, 0.0127195375723916);gsl_vector_set(lambda_values,18,   0.9197);
  gsl_vector_set(through_values,19, 0.0109578003224067);gsl_vector_set(lambda_values,19,   0.9198);
  gsl_vector_set(through_values,20, 0.0098414542549762);gsl_vector_set(lambda_values,20,   0.9199);
  gsl_vector_set(through_values,21, 0.0095066184313808);gsl_vector_set(lambda_values,21,   0.9200);
  gsl_vector_set(through_values,22, 0.0100435357173845);gsl_vector_set(lambda_values,22,   0.9201);
  gsl_vector_set(through_values,23, 0.0113589226644173);gsl_vector_set(lambda_values,23,   0.9202);
  gsl_vector_set(through_values,24, 0.0133136264280558);gsl_vector_set(lambda_values,24,   0.9203);
  gsl_vector_set(through_values,25, 0.0157684646986687);gsl_vector_set(lambda_values,25,   0.9204);
  gsl_vector_set(through_values,26, 0.0185842800987238);gsl_vector_set(lambda_values,26,   0.9205);
  gsl_vector_set(through_values,27, 0.0216219095843027);gsl_vector_set(lambda_values,27,   0.9206);
  gsl_vector_set(through_values,28, 0.0247421844451007);gsl_vector_set(lambda_values,28,   0.9207);
  gsl_vector_set(through_values,29, 0.0278059450370313);gsl_vector_set(lambda_values,29,   0.9208);
  gsl_vector_set(through_values,30, 0.0306740203832353);gsl_vector_set(lambda_values,30,   0.9209);
  gsl_vector_set(through_values,31, 0.0332072463065172);gsl_vector_set(lambda_values,31,   0.9210);
  gsl_vector_set(through_values,32, 0.0352664608962358);gsl_vector_set(lambda_values,32,   0.9211);
  gsl_vector_set(through_values,33, 0.0367124999751954);gsl_vector_set(lambda_values,33,   0.9212);
  gsl_vector_set(through_values,34, 0.0374061970996460);gsl_vector_set(lambda_values,34,   0.9213);
  gsl_vector_set(through_values,35, 0.0372083835592830);gsl_vector_set(lambda_values,35,   0.9214);
  gsl_vector_set(through_values,36, 0.0359798997100196);gsl_vector_set(lambda_values,36,   0.9215);
  gsl_vector_set(through_values,37, 0.0335815745749970);gsl_vector_set(lambda_values,37,   0.9216);
  gsl_vector_set(through_values,38, 0.0298742553097917);gsl_vector_set(lambda_values,38,   0.9217);
  gsl_vector_set(through_values,39, 0.0247187709375447);gsl_vector_set(lambda_values,39,   0.9218);
  gsl_vector_set(through_values,40, 0.0179759414151792);gsl_vector_set(lambda_values,40,   0.9219);
  gsl_vector_set(through_values,41, 0.0095066184313808);gsl_vector_set(lambda_values,41,   0.9220);
  gsl_vector_set(through_values,42, 0.0000000000000000);gsl_vector_set(lambda_values,42,   0.9221);

  // build up the output array
  double_vector[0] = lambda_values;
  double_vector[1] = through_values;

  // return the two vectors
  return double_vector;
}

/**
 * Function: get_PET_calibration_data()
 */
gsl_vector **
get_PET_calibration_data()
{
  gsl_vector *lambda_values;
  gsl_vector *through_values;

  gsl_vector **double_vector;

  // allocate space for the return vector
  double_vector  = (gsl_vector **)malloc(2*sizeof (gsl_vector *));

  // allocate the space for the vectors
  lambda_values  = gsl_vector_alloc(21);
  through_values = gsl_vector_alloc(21);

  gsl_vector_set(lambda_values, 0,1.069460);  gsl_vector_set(through_values, 0, 0.000000);
  gsl_vector_set(lambda_values, 1,1.069653);  gsl_vector_set(through_values, 1, 0.012630);
  gsl_vector_set(lambda_values, 2,1.069846);  gsl_vector_set(through_values, 2, 0.025261);
  gsl_vector_set(lambda_values, 3,1.070039);  gsl_vector_set(through_values, 3, 0.037891);
  gsl_vector_set(lambda_values, 4,1.070232);  gsl_vector_set(through_values, 4,   0.050522);
  gsl_vector_set(lambda_values, 5,1.070424);  gsl_vector_set(through_values, 5,   0.063152);
  gsl_vector_set(lambda_values, 6,1.070617);  gsl_vector_set(through_values, 6,   0.069010);
  gsl_vector_set(lambda_values, 7,1.070810);  gsl_vector_set(through_values, 7,   0.069010);
  gsl_vector_set(lambda_values, 8,1.071003);  gsl_vector_set(through_values, 8,   0.069010);
  gsl_vector_set(lambda_values, 9,1.071196);  gsl_vector_set(through_values, 9,   0.069010);
  gsl_vector_set(lambda_values, 10,1.071389); gsl_vector_set(through_values, 10,   0.069010);
  gsl_vector_set(lambda_values, 11,1.071582); gsl_vector_set(through_values, 11,   0.069010);
  gsl_vector_set(lambda_values, 12,1.071775); gsl_vector_set(through_values, 12,   0.069010);
  gsl_vector_set(lambda_values, 13,1.071967); gsl_vector_set(through_values, 13,   0.069010);
  gsl_vector_set(lambda_values, 14,1.072160); gsl_vector_set(through_values, 14,   0.069010);
  gsl_vector_set(lambda_values, 15,1.072353); gsl_vector_set(through_values, 15,   0.063152);
  gsl_vector_set(lambda_values, 16,1.072546); gsl_vector_set(through_values, 16,   0.050522);
  gsl_vector_set(lambda_values, 17,1.072739); gsl_vector_set(through_values, 17,   0.037891);
  gsl_vector_set(lambda_values, 18,1.072932); gsl_vector_set(through_values, 18,   0.025261);
  gsl_vector_set(lambda_values, 19,1.073125); gsl_vector_set(through_values, 19,  0.012630);
  gsl_vector_set(lambda_values, 20,1.073318); gsl_vector_set(through_values, 20,   0.000000);

  //Wavelength: 10713.888672, fringe factor: 1.097945, (x,y): (18, 745)

  // build up the output array
  double_vector[0] = lambda_values;
  double_vector[1] = through_values;

  // return the two vectors
  return double_vector;
}

/**
 * Function: evaluate_wavelength_steps
 * The function defines and computes the wavelength steps and the
 * filter throughputs which are used to compute the fringe amplitude
 * for all pixels. The basis for the wavelength data are the
 * filter data and the general are where fringing is significant
 * and computed, which is defined in the fringe
 * configuration structure.
 * First all throughput values below a certain value are discarded
 * at the short and long wavelength edge. Then the final wavelength
 * range is determined using the fringing range. Then all wavelength
 * values and the filter throughputs are determined and returned
 * as two gsl-vectors.
 *
 * Parameters:
 * @param fconf - the fringe configuration structure
 *
 * Returns:
 * @return double_vector - a set of two gsl-vectors with wavlength
                           and throughput
 */
gsl_vector **
evaluate_wavelength_steps(fringe_conf *fconf)
{
  int index=0;
  int lower=0;
  int upper=0;

  int nsteps;

  double lambda_min;
  double lambda_max;
  double lambda_act;
  double through_act=0.0;
  double through_tot=0.0;

  gsl_vector *lambda_values;
  gsl_vector *through_values;

  gsl_vector **double_vector;

  // allocate space for the return vector
  double_vector  = (gsl_vector **)malloc(2*sizeof (gsl_vector *));

  // search through the filter data
  // to narrow the usable range from below
  lower=0;
  while (lower < fconf->filter_through->nvals
	 && fconf->filter_through->yvals[lower] < FILTER_THRESHOLD)
    lower++;
  lower = MAX(0,lower-1);

  // search through the filter data
  // to narrow the usable range from above
  upper=fconf->filter_through->nvals - 1;
  while (upper > -1
	 && fconf->filter_through->yvals[upper] < FILTER_THRESHOLD)
    upper--;
  upper = MIN(fconf->filter_through->nvals - 1, upper+1);

#ifdef DEBUGFCONF
  fprintf(stderr, "New lower range index: %i, new upper: %i, old number: %i\n",
	  lower, upper, fconf->filter_through->nvals);
#endif

  // replace the old filter interpolator
  // with a new one, if necessary
  if (lower != 0 || upper != (fconf->filter_through->nvals - 1))
    fconf->filter_through = redefine_filter_throughput(lower, upper, fconf);

  // compute the lower wavelength range by comparing
  // the filter throughput with the range important
  // for fringing; the latter is given in AA!!
  lambda_min = MAX(gsl_vector_get(fconf->fringe_range, 0),
		   fconf->filter_through->xmin);

  // compute the upper wavelength range by comparing
  // the filter throughput with the range important
  // for fringing; the latter is given in AA!!
  lambda_max = MIN(gsl_vector_get(fconf->fringe_range, 1),
		   fconf->filter_through->xmax);

  // find the number of steps to cover the wavelength range
  nsteps = (int)ceil((lambda_max-lambda_min)/(fconf->fringe_step)) + 1;

  // allocate the space for the vectors
  lambda_values  = gsl_vector_alloc(nsteps);
  through_values = gsl_vector_alloc(nsteps);

  // fill the vectors with the wavelength steps and the
  // filter throughput at those wavelengths.
  lambda_act = lambda_min;
  for (index=0; index < nsteps-1; index+=1)
    {
      gsl_vector_set(lambda_values, index, lambda_act);
      through_act = eval_interp(fconf->filter_through, lambda_act);
      through_tot += through_act;
      gsl_vector_set(through_values, index, through_act);
      lambda_act += fconf->fringe_step;
    }

  // add also the values at lambda_amax
  gsl_vector_set(lambda_values, nsteps-1, lambda_max);
  through_act = eval_interp(fconf->filter_through, lambda_max);
  through_tot += through_act;
  gsl_vector_set(through_values, nsteps-1, through_act);

  for (index=0; index < (int)through_values->size; index++)
    {
      // normalize the filter throughput values
      gsl_vector_set(through_values, index,
		     gsl_vector_get(through_values, index)/through_tot);

      // convert the wavelength from AA to micron
      gsl_vector_set(lambda_values, index,
		     gsl_vector_get(lambda_values, index)*1.0e-04);
    }
#ifdef DEBUGFCONF
  through_tot = 0.0;
  for (index=0; index < through_values->size; index++)
    {
      fprintf(stderr, "Wavelength: %f, Throughput: %f\n",
	      gsl_vector_get(lambda_values, index),
	      gsl_vector_get(through_values, index));
      through_tot += gsl_vector_get(through_values, index);
    }
  fprintf(stderr, "Total throughput: %f\n", through_tot);
#endif

  // build up the output array
  double_vector[0] = lambda_values;
  double_vector[1] = through_values;

  return double_vector;
}

/**
 * Function: redefine_filter_throughput
 * The function re-defines an existing interpolator using only the
 * existing data values in an index range given as parameters.
 *
 * Parameters:
 * @param lower - index with the lowest data point
 * @param upper - index with the highest data point
 *
 * Returns:
 * @return new_interp - the new interpolator
 */
interpolator *
redefine_filter_throughput(const int lower, const int upper,
			   fringe_conf *fconf)
{
  int new_nvals;
  int index;

  double *new_x;
  double *new_y;

  interpolator *new_interp;

   // allocate space for the return structure;
  // complain if this fails
  new_interp = (interpolator *)malloc (sizeof (interpolator));
  if (new_interp == NULL)
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Could not allocate memory for interpolator");

  // compute the new length of the data arrays
  new_nvals = upper-lower+1;

  // allocate space for the new data arrays
  new_x = (double *)malloc(new_nvals*sizeof(double));
  new_y = (double *)malloc(new_nvals*sizeof(double));

  // transfer the data
  for (index=lower; index < upper+1; index++)
    {
      new_x[index-lower] = fconf->filter_through->xvals[index];
      new_y[index-lower] = fconf->filter_through->yvals[index];
    }

  // free the old structure
  free_interp(fconf->filter_through);

  // build up the interpolator structure
  new_interp->xmin    = new_x[0];
  new_interp->xmax    = new_x[new_nvals-1];
  new_interp->nvals   = new_nvals;
  new_interp->xvals   = new_x;
  new_interp->yvals   = new_y;
  new_interp->acc     = gsl_interp_accel_alloc();
  new_interp->interp  = gsl_interp_alloc(FILTER_INTERP_TYPE,
					 new_interp->nvals);

  // initialize the cubic spline
  gsl_interp_init(new_interp->interp, new_interp->xvals, new_interp->yvals,
		  new_interp->nvals);

  // return the new structure
  return new_interp;
}

/**
 * Function: eval_linear_interp
 * The function computes and returns the interpolated value
 * at a given position for a linear interpolator.
 *
 * Parameters:
 * @param lin_int - the linear interpolator
 * @param xval    - the position to evaluate the linear interpolator
 *
 * Returns:
 * @return (value) - the interpolated data value
 */
double
eval_linear_interp(linear_interp *lin_int, const double xval)
{

  double factor=0.0;

  // check whether the x-value is within
  // the range spanned by the data;
  // complain if the x-value is outside
  if (xval < lin_int->xmin || xval > lin_int->xmax)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		 "independent linear interpolation value %f "
		 "is outside interval (%f, %f)\n", xval,
		 lin_int->xmin, lin_int->xmax);

  // check whether you have to search upwards or downwards
  if (xval >= gsl_vector_get(lin_int->xvals, lin_int->act_index))
    {

      // in case that you search upwards, go up
      // the independent values until you find the right interval
      lin_int->act_index += 1;
      while(xval > gsl_vector_get(lin_int->xvals, lin_int->act_index))
	lin_int->act_index++;
    }
  else
    {

      // in case that you search downwards, go down
      // the independent values  until you find the right interval
      //      while(wavelength < resp->spec[nact-1].lambda_mean)
      while(xval < gsl_vector_get(lin_int->xvals, lin_int->act_index-1))
	lin_int->act_index--;
    }

  // interpolate within the interval to calculate the
  // sensitivity
  factor = (xval - gsl_vector_get(lin_int->xvals, lin_int->act_index-1))/
    (gsl_vector_get(lin_int->xvals, lin_int->act_index) - gsl_vector_get(lin_int->xvals, lin_int->act_index-1));

  // compute and terutn the interpolated value
    return (gsl_vector_get(lin_int->yvals, lin_int->act_index-1)
    + factor * (gsl_vector_get(lin_int->yvals, lin_int->act_index)-gsl_vector_get(lin_int->yvals, lin_int->act_index-1)));
}

/**
 * Function: get_layer_thickness
 * The function determines the thickness of an individual layer
 * at a given pixel position.
 *
 * Parameters:
 * @param opt_layer - the optical layer
 * @param ii        - the first pixel index
 * @param jj        - the second pixel index
 *
 * Returns:
 * @return (value) - the thickness of the layer at the requested position
 */
double
get_layer_thickness(const ccd_layer *opt_layer, const int ii, const int jj)
{
  // check whether the thickness is 2D
  if (opt_layer->thickness2D)
    // return the 2D value
    return gsl_matrix_get(opt_layer->thickness2D, ii, jj);
  else
    // return the constant value
    return opt_layer->thickness;
}

/**
 * Function: get_complex_refindex
 * The function computes and returns the complex refraction index of
 * a given CCD layer at a given wavelength.
 *
 * Parameters:
 * @param opt_layer - the optical layer
 * @param lambda    - the wavelength
 *
 * Returns:
 * @return (value) - the complex refraction index
 */
gsl_complex
get_complex_refindex(const ccd_layer *opt_layer, const double lambda)
{
  double re;
  double im;

  // compute the real part
  re = eval_interp(opt_layer->re_refraction, lambda);

  // comute the imaginary part
  im = eval_interp(opt_layer->im_refraction, lambda);

  // return the complex number
  return gsl_complex_rect(re, im);
}

/**
 * Function: compute_reflection
 * The function computes and returns the reflection index (in %)
 * for two complex refraction indices  at a given wavelength.
 *
 * Parameters:
 * @param refract_l1 - refraction index of one layer
 * @param refract_l2 - refraction index of the second layer
 *
 * Returns:
 * @return (value) - the reflection index
 */
double
compute_reflection(const gsl_complex refract_l1,
		   const gsl_complex refract_l2)
{
  gsl_complex conj_l1;
  gsl_complex conj_l2;

  gsl_complex refl_compl;

  // get the comlex conjugated values of the input
  conj_l1 = gsl_complex_conjugate(refract_l1);
  conj_l2 = gsl_complex_conjugate(refract_l2);

  // compute the complex reflection
  refl_compl = gsl_complex_div(gsl_complex_sub(conj_l1, conj_l2),
			       gsl_complex_add(conj_l1, conj_l2));

  // return the magnitude of the complex reflection
  return gsl_complex_abs2(refl_compl);
}

/**
 * Function: compute_transmission
 * The function computes and returns the transmission index (in %)
 * for two complex refraction indices at a given wavelength.
 *
 * Parameters:
 * @param refract_l1 - refraction index of one layer
 * @param refract_l2 - refraction index of the second layer
 *
 * Returns:
 * @return (value) - the trnasmission index
 */
double
compute_transmission(const gsl_complex refract_l1,
		     const gsl_complex refract_l2)
{
  gsl_complex conj_l1;
  gsl_complex conj_l2;

  gsl_complex trans_compl;

  // get the comlex conjugated values of the input
  conj_l1 = gsl_complex_conjugate(refract_l1);
  conj_l2 = gsl_complex_conjugate(refract_l2);

  // compute the complex transmission
  trans_compl =
    gsl_complex_div(gsl_complex_sqrt(gsl_complex_mul(conj_l1, conj_l2)),
		    gsl_complex_add(conj_l1, conj_l2));

  // finish the computation of the complex transmission
  trans_compl = gsl_complex_mul_real (trans_compl, 2.0);

  // return the magnitude of the complex transmission
  return gsl_complex_abs2(trans_compl);
}

/**
 * Function: compute_attenuation
 * The function computes the attenuation/damping of a plane wave
 * with a given inverse wavelelength (phase number) traversing
 * TWO TIMES a layer of a given thickness and complex refraction
 * index.
 *
 * Parameters:
 * @param refract      - the complex refraction index
 * @param thickness    - the thickness of the layer
 * @param phase_number - the inverse wavelength
 *
 * Returns:
 * @return (value) - the attenuation
 */
double
compute_attenuation(const gsl_complex refract, const double thickness,
		    const double phase_number)
{
  // check if there is something to compute
  if (GSL_IMAG(refract))
    // compute and return the value
    return exp(-2.0*GSL_IMAG(refract)*thickness*phase_number);
  else
    // return the value for exp(0.0)
    return 1.0;
}

/**
 * Function: compute_pshift
 * The function computes the phase shift of a plane wave
 * with a given inverse wavelelength (phase number) traversing
 * TWO TIMES a layer of a given thickness and complex refraction
 * index.
 *
 * Parameters:
 * @param refract      - the complex refraction index
 * @param thickness    - the thickness of the layer
 * @param phase_number - the inverse wavelength
 *
 * Returns:
 * @return (value) - the complex phase shift
 */
gsl_complex
compute_pshift(const gsl_complex refract, const double thickness,
	       const double phase_number)
{
  // all in one line:
  return gsl_complex_exp(gsl_complex_rect(0.0,2.0*GSL_REAL(refract)*thickness*phase_number));
}

/**
 * Function: alloc_optprops_list
 * The function allocates space for a list of optical property
 * elements. the size of the list is derived from a fringe
 * configuration structure given in the input.
 *
 * Parameters:
 * @param fconf - the fringe configuration file
 *
 * Returns:
 * @return optprops - the list of optical properties
 */
optical_property *
alloc_optprops_list(const fringe_conf *fconf)
{
  optical_property *optprops;

  // allocate large enough memory
  optprops =
    (optical_property *)malloc(fconf->opt_layers->num_layers*sizeof(ap_pixel));

  // return the pointer
  return optprops;
}

/**
 * Function: print_optprops_list
 * The function prints the content of an optical property
 * list onto the screen.
 *
 * Parameters:
 * @param optprops - the optical property list
 *
 * Returns:
 * @return -
 */
void
print_optprops_list(const optical_property *optprops, const int num_entries)
{
  int index;

  for (index = 0; index < num_entries; index++)
    {
      fprintf(stdout, "#%i: %f %f %f %f %f %f \n",index,
	      optprops[index].trans_upper, optprops[index].reflect_upper,
	      GSL_REAL(optprops[index].double_phshift),
	      GSL_IMAG(optprops[index].double_phshift),
	      optprops[index].trans_lower, optprops[index].reflect_lower);
    }
}

/**
 * Function: free_optprops_list
 * The function releases the space allocated in an
 * optical property list.
 *
 * Parameters:
 * @param optprops - the optical property list
 *
 * Returns:
 * @return -
 */
void
free_optprops_list(optical_property *optprops)
{
  if (optprops != NULL)
    {
      // free the structure
      free(optprops);

      // set it to NULL
      optprops = NULL;
    }
}

/*
 * Function: init_optprops_list
 * The function initializes an optical property list. This means
 * it computes and stores values for the list elements. Here
 * only elements are fixed, which will not change during the
 * whole fringe computation and remain constant for all pixels
 * at all wavelengths.
 *
 * Parameters:
 * @params fconf       - the fringe configuration structure
 * @params lambda_mean - the mean wavelength of analyzed waveband
 * @params optprops    - the list of optical property elements
 *
 * Returns:
 * @return -
 */
void
init_optprops_list(const fringe_conf *fconf, const double lambda_mean,
		   optical_property *optprops)
{
  int num_layers;
  int index;

  double n1;
  double n2;

  // determine the number of layers
  num_layers = fconf->opt_layers->num_layers;

  if (num_layers > 0)
    {
      // set the transmission and reflection
      // vacuum -- first CCD layer
      // that's a bit heuristic, since
      // the true value using the true
      // refration index for vacuum would
      // give a different result
      optprops[0].trans_upper   = 1.0;
      optprops[0].reflect_upper = 0.0;

      // also that one is a bit heuristic
      // but assuming n(vacuum)=1.0 its also true
      optprops[0].sign_upper = 1.0;

      // set the sign of the lowest layer -- substrate
      // this is purely heuristic and works
      // only for the HRC/WFC fringing models!!
      optprops[num_layers-1].sign_lower = -1.0;
    }

  for (index=0; index < num_layers-1; index++)
    {
      // get the real part of the refraction index
      // of the upper layer
      n1 = GSL_REAL(get_complex_refindex(fconf->opt_layers->opt_layer[index],
					 lambda_mean));

      // get the real part of the refraction index
      // of the lower layer
      n2 = GSL_REAL(get_complex_refindex(fconf->opt_layers->opt_layer[index+1],
					 lambda_mean));

      // distribute the signs
      // for the upper and lower layer
      if (n1 < n2)
	{
	  optprops[index].sign_lower   =  1.0;
	  optprops[index+1].sign_upper = -1.0;
	}
      else
	{
	  optprops[index].sign_lower   = -1.0;
	  optprops[index+1].sign_upper =  1.0;
	}
    }
}

/*
 * Function: fill_optprops_thickness
 * The function fills the thickness of the optival layers
 * at a given pixel into a list of optical properties.
 *
 * Parameters:
 * @params opt_layers - the structure for the optical layers
 * @params ii         - the mean wavelength of analyzed waveband
 * @params jj         - the list of optical property elements
 * @params optprops   - the list of optical property elements
 *
 * Returns:
 * @return -
 */
void
fill_optprops_thickness(const ccd_layers *opt_layers, const int ii,
			const int jj, optical_property *optprops)
{
  int index;

  // go over all layers
  for (index=0; index < opt_layers->num_layers; index++)
    // get and store the thickness
    optprops[index].thickness =
      get_layer_thickness(opt_layers->opt_layer[index], ii, jj);
}

/*
 * Function: fill_optprops_all
 * This function computes and fills in all pixel AND wavelength
 * dependent values into a list of optical properties. The completely
 * filled list can then be used to determine the fringe contribution
 * at that wavelength according to a certain model which defines
 * the various light-paths.
 *
 * Parameters:
 * @params opt_layers - the structure for the optical layers
 * @params lambda     - the actual wavelength
 * @params optprops   - the list of optical property elements
 *
 * Returns:
 * @return -
 */
void
fill_optprops_all(const ccd_layers *opt_layers, const double lambda,
		  optical_property *optprops)
{
  int index;

  double phase_number;

  gsl_complex compl_upp;
  gsl_complex compl_low;

  int num_layers;

  // store the number of layers
  num_layers = opt_layers->num_layers;

  // compute the phase number
  phase_number = 2.0*M_PI / lambda;

  // get the complex refractive index for the first layer
  compl_upp =
    get_complex_refindex(opt_layers->opt_layer[0],lambda);

  for (index=0; index < num_layers-1; index++)
    {

      // get the refractive index for the next layer
      compl_low =
      	get_complex_refindex(opt_layers->opt_layer[index+1],lambda);

      // compute the reflection and transmission
      // to the lower layers
      optprops[index].reflect_lower =
	compute_reflection(compl_upp, compl_low);
      optprops[index].trans_lower   = 1.0 - optprops[index].reflect_lower;

      // compute the reflection and transmission
      // to the upper layers
      optprops[index+1].reflect_upper = optprops[index].reflect_lower;
      optprops[index+1].trans_upper   = 1.0 - optprops[index+1].reflect_upper;

      // compute the attenuation
      optprops[index].double_attenuation =
      	compute_attenuation(compl_upp,optprops[index].thickness,phase_number);

      // compute the phase shift
      optprops[index].double_phshift =
	compute_pshift(compl_upp, optprops[index].thickness, phase_number);

      // prepare the next iteration:
      // the now upper layer will then be the lower
      compl_upp = compl_low;
    }
  // compute the attenuation for the last layer
  optprops[num_layers-1].double_attenuation =
    compute_attenuation(compl_upp,optprops[num_layers-1].thickness,phase_number);

  // compute the phase shift for the last layer
  optprops[num_layers-1].double_phshift =
    compute_pshift(compl_upp, optprops[num_layers-1].thickness, phase_number);

  // compute the transmission of the last layer
  // towards the substrate
  optprops[num_layers-1].trans_lower =
    eval_interp(opt_layers->substrate, lambda);

  // compute the reflection of the last layer
  // towards the substrate
  optprops[num_layers-1].reflect_lower =
    1.0 - optprops[num_layers-1].trans_lower;

}

/*
 * Function: fill_contrib_single
 * The function computes the contribution to the fringe amplitude
 * on the basis of an appropriately filled list of optical properties.
 * In the properties the ingredients such as phase shifts and
 * attenuation and transmissions and reflections are defined.
 * This function just combines the ingredients using a model
 * which includes all single reflected beam combinations
 * as contributors to the fringing.
 *
 * Parameters:
 * @params optprops   - the list of optical property elements
 * @params fconf      - the fringe configuration structure
 *
 * Returns:
 * @return (value)    - the fringe contribution
 */
double
fringe_contrib_single(const optical_property *optprops,
		      const fringe_conf *fconf)
{
  int index=2;

  gsl_complex amp_tmp;
  gsl_complex amp_tot;

  // compute the initial phase at the top of the
  // first layer
  amp_tot =
    gsl_complex_mul_real(gsl_complex_exp(gsl_complex_rect(0.0,fconf->fringe_phase*M_PI/180.0)), 1.0);

  // transfer the intitial value
  // to the running variable
  amp_tmp = amp_tot;

  // pile up the amplitude,
  // also putting in the transmission
  amp_tmp =
    gsl_complex_mul(amp_tmp,
		    gsl_complex_mul_real(optprops[0].double_phshift,
					 1.0*1.0*optprops[0].double_attenuation));

  // compute the intermediate result
  // for the first layer
  amp_tot =
    gsl_complex_add(amp_tot,
		    gsl_complex_mul_real(amp_tmp,
					 optprops[0].sign_lower*optprops[0].reflect_lower));

  // go over layer 2 to the end
  for (index=1; index < fconf->opt_layers->num_layers; index++)
    {
      // pile up the amplitude
      // to that layer
      amp_tmp =
	gsl_complex_mul(amp_tmp,
			gsl_complex_mul_real(optprops[index].double_phshift,
					   optprops[index-1].trans_lower*optprops[index].trans_upper*optprops[index].double_attenuation));

      // compute the intermediate result
      // for the that layer
      amp_tot =
	gsl_complex_add(amp_tot,
			gsl_complex_mul_real(amp_tmp,
					     optprops[index].sign_lower*optprops[index].reflect_lower));
      //      if (index==3)
      //	fprintf(stdout,"ccc:%f, %f\n", GSL_REAL(amp_tmp),GSL_IMAG(amp_tmp) );

    }

  // return the final value
  return (GSL_REAL(amp_tot) - 1.0);
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_interp.h>

typedef double (*eval_func)(const gsl_interp*,const double *x,const double *y,
                            double,gsl_interp_accel*);

static const double EPSILON = 0.00000001;
static const double EPS = 0.0001;
static const int PTS = 801;

static const double x_values[] = { 0, 1, 2, 3, 4, 5 };
static const double y_values[] = { 5, 0, 5, 2, 0, 5 };
static double x_range;

static const int N = sizeof(x_values)/sizeof(double);

static double* interpolate(const gsl_interp_type *type, const size_t order);
static void    check_discont(gsl_interp *interp, const eval_func func,
                             const char *dsym, const double x,
                             gsl_interp_accel *accel);

static inline double
scale_x(const int i)
{
  return x_values[0] + (double)i/(PTS-1)*x_range;
}

int
main(int argc, char *argv[])
{
  int i;
  double *cspline, *cspline_d, *cspline_dd;
  double *akima, *akima_d, *akima_dd;

  x_range = x_values[N-1] - x_values[0];

  akima = interpolate(gsl_interp_akima_periodic, 0);
  akima_d = interpolate(gsl_interp_akima_periodic, 1);
  akima_dd = interpolate(gsl_interp_akima_periodic, 2);
  cspline = interpolate(gsl_interp_cspline_periodic, 0);
  cspline_d = interpolate(gsl_interp_cspline_periodic, 1);
  cspline_dd = interpolate(gsl_interp_cspline_periodic, 2);

  puts("# x   akima  cspline  akima'  cspline'  akima''  cspline''");
  for (i = -PTS/2; i < PTS/2; i++)
    printf("%0.5f %0.5f %0.5f %0.5f %0.5f %0.5f %0.5f\n",
           scale_x(i),
           akima[(i + PTS)%PTS],
           cspline[(i + PTS)%PTS],
           akima_d[(i + PTS)%PTS],
           cspline_d[(i + PTS)%PTS],
           akima_dd[(i + PTS)%PTS],
           cspline_dd[(i + PTS)%PTS]);

  free(akima);
  free(akima_d);
  free(akima_dd);
  free(cspline);
  free(cspline_d);
  free(cspline_dd);
  return 0;
}

static double*
interpolate(const gsl_interp_type *type, const size_t order)
{
  static const eval_func DERIV_ORDERS[] = {
    gsl_interp_eval,
    gsl_interp_eval_deriv,
    gsl_interp_eval_deriv2
  };
  static const char *DERIV_SYMS[] = {
    "",
    "'",
    "''"
  };

  gsl_interp *interp = gsl_interp_alloc(type, N);
  gsl_interp_accel *accel = gsl_interp_accel_alloc();
  double *plotpts = (double*)malloc(PTS*sizeof(double));
  int i;

  assert(order < sizeof(DERIV_ORDERS));
  gsl_interp_init(interp, x_values, y_values, N);
  for (i = 0; i < PTS; i++)
    plotpts[i] = DERIV_ORDERS[order](interp, x_values, y_values,
                                     scale_x(i), accel);

  for (i = 0; i < N-1; i++)
    check_discont(interp, DERIV_ORDERS[order], DERIV_SYMS[order],
                  x_values[i], accel);

  gsl_interp_free(interp);
  gsl_interp_accel_free(accel);

  return plotpts;
}

static void
check_discont(gsl_interp *interp, const eval_func func, const char *dsym,
              const double x, gsl_interp_accel *accel)
{
  double xm, xp;
  double ym, yp;

  xm = fmod(x - EPSILON + x_range, x_range);
  xp = fmod(x + EPSILON, x_range);
  ym = func(interp, x_values, y_values, xm, accel);
  yp = func(interp, x_values, y_values, xp, accel);
  if (fabs(yp - ym) > EPS) {
    fprintf(stderr, "discontinuity in %s%s at %f:\n"
                    "\t left value: %f\n"
                    "\tright value: %f\n"
                    "\t difference: %f\n\n",
            gsl_interp_name(interp), dsym, x, ym, yp, yp-ym);
  }
}


#include <stdio.h>

//#ifndef DARWIN
//#include <malloc.h>
//#endif

#include <stddef.h>
#include <Python.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <string.h>
#include <math.h>

/*
 *  A simple helper function to display the contents of a 2-D gsl matrix
 */
int print_matrix(FILE *f, const gsl_matrix *m)
{
  int status, n = 0;
  size_t i, j;

  for (i = 0; i < m->size1; i++) {
    for (j = 0; j < m->size2; j++) {
      if ((status = fprintf(f,"%g ", gsl_matrix_get(m, i, j))) < 0)
        return -1;
      n += status;
    }

    if ((status = fprintf(f, "\n")) < 0)
      return -1;
    n += status;
  }
  return n;
}

/*
 *  A simple helper function to display the contennts of a 1-D gsl vector
 */
int print_vector(FILE *f, const gsl_vector *m)
{
  int status, n = 0;
  size_t i;

  for (i = 0; i < m->size; i++) {
    if ((status = fprintf(f, "%g ", gsl_vector_get(m, i))) < 0)
      return -1;
    n += status;
  }
  return n;
}

void print_mat(double* mat, int dim1, int dim2)
{
  int i, j;
  printf("\n");
  for (i=0; i<dim1; i++) {
    for (j=0; j<dim2; j++) {
      printf("%6.2f,", mat[i*dim1 + j]);
    }
    printf("\n");
  }
}
void print_vec(double* vec, int dim1)
{
  int i;
  printf("\n");
  for (i=0; i<dim1; i++) {
    printf("%6.2f,", vec[i]);
  }
  printf("\n");
}

/* Function: get_lnoverlaps
 * ------------------------
 *   Calculates the log overlap (convolution) with a set of 6D Gaussians with
 *   a single 6D Gaussian, each in turn.
 *
 *   In Chronostar, this is used to see how well the kinematic properties
 *   of stars overlap with a proposed Gaussian distribution.
 *
 * Paramaters
 *  name      type            description
 * ----------
 *  gr_cov        (6*6 npArray)   group's covariance matrix
 *  gr_mn         (1*6 npArray)   group's central estimate (mean)
 *  st_covs       (n*6*6 npArray) array of each star's cov matrix
 *  st_mns:       (n*6 npArray)   array of each star's central estimate
 *  lnols_output: (n npArray)     used to store and return calculated overlaps
 *  nstars:       (int)           number of stars, used for array dimensions
 *
 * Returns
 * -------
 *  (nstars) array of calculated log overlaps of every star with component
 *      thanks to Swig magic the result is stored in `lnols_output` which is
 *      returned as output to the python call as a numpy array
 *
 * Notes
 * -----
 *   For each star calculates:
 *     log(1/sqrt( (2*PI)^6*det(C) ) * exp( -0.5*(b-a)^T*(C)^-1*(b-a) )
 *   where
 *     C = st_cov + gr_cov
 *   and
 *     a = gr_mn, b = st_mn
 *   Expanding and simplifying this becomes:
 *     -0.5[ 6*ln(2*PI) + ln(|C|) + (b-a)^T*(C^-1)*(b-a) ]
 *
 *   Stark improvement on previous implementations. Doesn't require input as
 *   inverse covariance matrices. Never performs a matrix inversion.
 */
void get_lnoverlaps(
  double* gr_cov, int gr_dim1, int gr_dim2,
  double* gr_mn, int gr_mn_dim,
  double* st_covs, int st_dim1, int st_dim2, int st_dim3,
  double* st_mns, int st_mn_dim1, int st_mn_dim2,
  double* lnols_output, int n
  )
{
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double d_temp, result, ln_det_BpA;
  gsl_permutation *p1;

  gsl_matrix *BpA      = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //will hold (B+A)
  gsl_vector *bma      = gsl_vector_alloc(MAT_DIM);          //will hold b - a
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);

  p1 = gsl_permutation_alloc(BpA->size1);

  // Go through each star, calculating and storing overlap
  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRIX
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        //performing st_cov+gr_cov as part of the initialisation
        gsl_matrix_set(
          BpA,i,j,
          st_covs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j] +
          gr_cov[i*MAT_DIM+j]
        );

    // INITIALISE CENTRAL ESTIMATES
    // performing st_mn - gr_mn as part of the initialisation
    for (i=0; i<MAT_DIM; i++) {
      gsl_vector_set(
        bma, i,
        st_mns[star_count*MAT_DIM + i] - gr_mn[i]
      );
    }

    // CALCULATE OVERLAPS
    // Performed in 4 stages
    // Calc and sum up the inner terms:
    // 1) 6 ln(2pi)
    // 2) ln(|C|)
    // 3) (b-a)^T(C^-1)(b-a)
    // Then apply -0.5 coefficient

    // 1) Calc 6 ln(2pi)
    result = 6*log(2*M_PI);

    // 2) Get log determiant of C
    gsl_linalg_LU_decomp(BpA, p1, &signum);
    ln_det_BpA = log(fabs(gsl_linalg_LU_det(BpA, signum)));
    result += ln_det_BpA;

    // 3) Calc (b-a)^T(C^-1)(b-a)
    gsl_vector_set_zero(v_temp);
    gsl_linalg_LU_solve(BpA, p1, bma, v_temp); /* v_temp holds (B+A)^-1 (b-a) *
                                                * utilises `p1` as calculated *
                                                * above                       */
    gsl_blas_ddot(v_temp, bma, &d_temp); //d_temp holds (b-a)^T (B+A)-1 (b-a)
    result += d_temp;

    // 4) Apply coefficient
    result *= -0.5;

    // STORE RESULT 'lnols_output'
    lnols_output[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(BpA);
  gsl_vector_free(bma);
  gsl_vector_free(v_temp);
  gsl_permutation_free(p1);
}

/* NOTE:
 * Everything below this line is left simply for correctness comparisons
 */
double get_overlap(double* gr_icov, int gr_dim1, int gr_dim2,
                   double* gr_mn, int gr_mn_dim, double gr_icov_det,
                   double* st_icov, int st_dim1, int st_dim2,
                   double* st_mn, int st_mn_dim, double st_icov_det)
{
  int MAT_DIM = gr_dim1;
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

//for (l=0; l<1; l++){ //DELETE

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);
      gsl_matrix_set (B, i, j, st_icov[i*MAT_DIM + j]);
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    gsl_vector_set (a, i, gr_mn[i]);
    gsl_vector_set (b, i, st_mn[i]);
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
  gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));
//} //DELETE THIS

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

double get_overlap2(PyObject *gr_icov, PyObject *gr_mn, double gr_icov_det,
                    PyObject *st_icov, PyObject *st_mn, double st_icov_det)
{
  int MAT_DIM = 6;
  int i, j, signum;
  double ApB_det, d_temp, result;
  PyObject *o1, *o2;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  //Inserting values into matricies and vectors
  for (i=0; i<MAT_DIM; i++)
  {
    for (j=0; j<MAT_DIM; j++)
    {
      o1 = PyList_GetItem(gr_icov, i*MAT_DIM + j);
      gsl_matrix_set (A, i, j, PyFloat_AS_DOUBLE(o1));
      o2 = PyList_GetItem(st_icov, i*MAT_DIM + j);
      gsl_matrix_set (B, i, j, PyFloat_AS_DOUBLE(o2));
    }
  }

  for (i=0; i<MAT_DIM; i++)
  {
    o1 = PyList_GetItem(gr_mn, i);
    gsl_vector_set (a, i, PyFloat_AS_DOUBLE(o1));
    o2 = PyList_GetItem(st_mn, i);
    gsl_vector_set (b, i, PyFloat_AS_DOUBLE(o2));
  }

  // Adding A and B together and storing in ApB
  gsl_matrix_set_zero(ApB);
  gsl_matrix_add(ApB, A);
  gsl_matrix_add(ApB, B);

  // Storing the result A*a + B*b in AapBb
  gsl_vector_set_zero(AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, A, a,
                 1.0, AapBb);
  gsl_blas_dgemv(CblasNoTrans,
                 1.0, B, b,
                 1.0, AapBb);

  // Getting determinant of ApB
  gsl_linalg_LU_decomp(ApB, p, &signum);
  //ApB_det = gsl_linalg_LU_det(ApB, signum);
  ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

  // Solve for c
  gsl_linalg_LU_solve(ApB, p, AapBb, c);

  // Compute the overlap formula
  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(a, v_temp);       //v_temp holds a
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
  gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, A, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (a-c)

  result = 0.0;
  gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

  result += d_temp;

  gsl_vector_set_zero(v_temp);
  gsl_blas_dcopy(b, v_temp);       //v_temp holds b
  gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
  gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

  // CAN'T HAVE v_temp and v_temp2 be the same vector.
  // Results in 0's being stored in v_temp2.
  gsl_blas_dgemv(CblasNoTrans, 1.0, B, v_temp, 0.0, v_temp2);
  //v_temp2 holds A (b-c)

  gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
  result += d_temp;

  result = -0.5 * result;
  result = exp(result);

  result *= sqrt((gr_icov_det * st_icov_det/ApB_det)
                / pow(2*M_PI, MAT_DIM));

  // Freeing memory
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);

  return result;
}

/* Main function which performs fastest so far:
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 */
void get_overlaps(double* gr_icov, int gr_dim1, int gr_dim2,
                  double* gr_mn, int gr_mn_dim,
                  double gr_icov_det,
                  double* st_icovs, int st_dim1, int st_dim2, int st_dim3,
                  double* st_mns, int st_mn_dim1, int st_mn_dim2,
                  double* st_icov_dets, int st_icov_dets_dim,
                  double* rangevec, int n)
{
  // ALLOCATE MEMORY
  int star_count = 0;
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double ApB_det, d_temp, result;
  gsl_permutation *p;

  gsl_matrix *A        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *B        = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_matrix *ApB      = gsl_matrix_alloc(MAT_DIM, MAT_DIM);
  gsl_vector *a        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *b        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *AapBb    = gsl_vector_alloc(MAT_DIM);
  gsl_vector *c        = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);
  gsl_vector *v_temp2  = gsl_vector_alloc(MAT_DIM);
  gsl_vector *amc      = gsl_vector_alloc(MAT_DIM); //will hold a - c
  gsl_vector *bmc      = gsl_vector_alloc(MAT_DIM); //will hold b - c

  p = gsl_permutation_alloc(A->size1);

  // INITIALISE GROUP MATRICES
  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++)
      gsl_matrix_set (A, i, j, gr_icov[i*MAT_DIM + j]);

  for (i=0; i<MAT_DIM; i++)
    gsl_vector_set (a, i, gr_mn[i]);

  for (star_count=0; star_count<n; star_count++) {
    // INITIALISE STAR MATRICES
    for (i=0; i<MAT_DIM; i++)
      for (j=0; j<MAT_DIM; j++)
        gsl_matrix_set(B,i,j, st_icovs[star_count*MAT_DIM*MAT_DIM+i*MAT_DIM+j]);

    for (i=0; i<MAT_DIM; i++)
      gsl_vector_set (b, i, st_mns[star_count*MAT_DIM + i]);

    // FIND OVERLAP
    // Adding A and B together and storing in ApB
    gsl_matrix_set_zero(ApB);
    gsl_matrix_add(ApB, A);
    gsl_matrix_add(ApB, B);

    // Storing the result A*a + B*b in AapBb
    gsl_vector_set_zero(AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, A, a, 1.0, AapBb);
    gsl_blas_dsymv(CblasUpper, 1.0, B, b, 1.0, AapBb);

    // Getting determinant of ApB
    gsl_linalg_LU_decomp(ApB, p, &signum);
    //ApB_det = gsl_linalg_LU_det(ApB, signum);
    ApB_det = fabs(gsl_linalg_LU_det(ApB, signum)); //temp doctoring determinant

    // Solve for c
    gsl_linalg_LU_solve(ApB, p, AapBb, c);

    // Compute the overlap formula
    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(a, v_temp);       //v_temp holds a
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds a - c
    gsl_blas_dcopy(v_temp, amc);     //amc holds a - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, A, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (a-c)

    result = 0.0;
    gsl_blas_ddot(v_temp2, amc, &d_temp); //d_temp holds (a-c)^T A (a-c)

    result += d_temp;

    gsl_vector_set_zero(v_temp);
    gsl_blas_dcopy(b, v_temp);       //v_temp holds b
    gsl_blas_daxpy(-1.0, c, v_temp); //v_temp holds b - c
    gsl_blas_dcopy(v_temp, bmc);     //bmc holds b - c

    // CAN'T HAVE v_temp and v_temp2 be the same vector.
    // Results in 0's being stored in v_temp2.
    gsl_blas_dsymv(CblasUpper, 1.0, B, v_temp, 0.0, v_temp2);
    //v_temp2 holds A (b-c)

    gsl_blas_ddot(v_temp2, bmc, &d_temp); //d_temp holds (b-c)^T B (b-c)
    result += d_temp;

    result = -0.5 * result;
    result = exp(result);

    result *= sqrt((gr_icov_det * st_icov_dets[star_count]/ApB_det)
                  / pow(2*M_PI, MAT_DIM));

    // STORE IN 'rangevec'
    rangevec[star_count] = result;
  }

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gsl_matrix_free(ApB);
  gsl_vector_free(a);
  gsl_vector_free(b);
  gsl_vector_free(AapBb);
  gsl_vector_free(c);
  gsl_vector_free(v_temp);
  gsl_vector_free(v_temp2);
  gsl_vector_free(amc);
  gsl_vector_free(bmc);

  gsl_permutation_free(p);
}

/* New main function, speed not yet tested
 * mostly here for debugging reasons
 * --parameters--
 *  group_icov     (6*6 npyArray) the group's inverse covariance matrix
 *  group_mn       (1*6 npyArray) which is the group's mean kinematic info
 *  group_icov_det (flt)          the determinent of the group_icov
 *  Bs             (nstars*6*6)   an array of each star's icov matrix
 *  bs:            (nstars*6)     an array of each star's mean kinematic info
 *  B_dets:        (nstars)       an array of the determinent of each icov
 *  nstars:        (int)          number of stars, used to determine the size
 *                          of npyArray which will return calculated overlaps)
 *
 * returns: (nstars) array of calculated overlaps of every star with 1 group
 *
 * todo: instead of calling internal function actually use cblas functions
 *          this will save time on the reallocation and deallocation
 *
 *      look up how to find inverse
 */
double new_get_lnoverlap(
  double* gr_cov, int gr_dim1, int gr_dim2,
  double* gr_mn, int gr_mn_dim,
  double* st_cov, int st_dim1, int st_dim2,
  double* st_mn, int st_mn_dim
  )
{
  //printf("Inside new_get_lnoverlaps function\n");
  printf("-----------------------------------------------------------\n");
  printf("new_get_lnoverlap(): In c implemntation of Tim's derivation\n");
  printf("-----------------------------------------------------------\n");
  //printf("Inputs are:\n");
  //printf("  A\n");
  //print_mat(gr_cov, gr_dim1, gr_dim2);
  //printf("  a\n");
  //print_vec(gr_mn, gr_mn_dim);
  //printf("  B\n");
  //print_mat(st_cov, st_dim1, st_dim2);
  //printf("  b\n");
  //print_vec(st_mn, st_mn_dim);

  // ALLOCATE MEMORY
  int MAT_DIM = gr_dim1; //Typically set to 6
  int i, j, signum;
  double d_temp, result, ln_det_BpA;
  FILE* fout = stdout;
  gsl_permutation *p1;

  gsl_matrix *BpA      = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)
  //gsl_matrix *BpAi     = gsl_matrix_alloc(MAT_DIM, MAT_DIM); //(B+A)^-1
  gsl_vector *bma      = gsl_vector_alloc(MAT_DIM); //will hold b - a
  gsl_vector *v_temp   = gsl_vector_alloc(MAT_DIM);

  p1 = gsl_permutation_alloc(BpA->size1);

  printf("Memory allocated\n");
  // INITIALISE STAR MATRICES
  for (i=0; i<MAT_DIM; i++)
    for (j=0; j<MAT_DIM; j++) {
      //perform B+A as part of the initialisation
      gsl_matrix_set(
        BpA,i,j,
        st_cov[i*MAT_DIM+j] +
        gr_cov[i*MAT_DIM+j]
      );
      printf("Printing BpA\n");
      print_matrix(fout, BpA);
    }

  for (i=0; i<MAT_DIM; i++) {
    gsl_vector_set(
      bma, i,
      st_mn[i] -
      gr_mn[i]
    );
  }
  printf("Printing bma\n");
  print_vec(bma->data, 6);
  printf("Matrices initialised\n\n");

  result = 6*log(2*M_PI);
  printf("Added 6log(2pi):\n%6.2f\n", result);

  // Get inverse of BpA, this line is wrong, fix when have internet
  gsl_linalg_LU_decomp(BpA, p1, &signum);
  ln_det_BpA = log(fabs(gsl_linalg_LU_det(BpA, signum)));
  printf("Log of det(ApB): %6.2f\n", ln_det_BpA);
  result += ln_det_BpA;

  printf("result so far:\n%6.2f\n",result);
  //
  gsl_vector_set_zero(v_temp);
  gsl_linalg_LU_solve(BpA, p1, bma, v_temp); //v_temp holds (B+A)^-1 (b-a)
  gsl_blas_ddot(v_temp, bma, &d_temp); //d_temp holds (b-a)^T (B+A)-1 (b-a)
  printf("Printing bma_BpAi_bma\n");
  printf("%6.2f\n\n", d_temp);

  result += d_temp;
  printf("result after bma_BpAi_bma:\n%6.2f\n",result);

  result *= -0.5;
  printf("Everything calculated\n");
  printf("Final result:\n%6.8f\n", result);
  //

  // DEALLOCATE THE MEMORY
  gsl_matrix_free(BpA);
  //gsl_matrix_free(BpAi);
  gsl_vector_free(bma);
  gsl_vector_free(v_temp);

  gsl_permutation_free(p1);

  return result;
  //printf("At end of new_get_lnoverlaps function\n");
}


/* interpolation/interp_poly.c
 *
 * Copyright (C) 2001 DAN, HO-JIN
 *
 * 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.
 */

#include <config.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_interp.h>

typedef struct
{
  double *d;
  double *coeff;
  double *work;
}
polynomial_state_t;

static void *
polynomial_alloc (size_t size)
{
  polynomial_state_t *state =
    (polynomial_state_t *) malloc (sizeof (polynomial_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for polynomial state",
                      GSL_ENOMEM);
    }

  state->d = (double *) malloc (sizeof (double) * size);

  if (state->d == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->coeff = (double *) malloc (sizeof (double) * size);

  if (state->coeff == 0)
    {
      free (state->d);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  state->work = (double *) malloc (sizeof (double) * size);

  if (state->work == 0)
    {
      free (state->coeff);
      free (state->d);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for d", GSL_ENOMEM);
    }

  return state;
}

static int
polynomial_init (void *vstate,
                 const double xa[], const double ya[], size_t size)
{
  polynomial_state_t *state = (polynomial_state_t *) vstate;

  int status = gsl_poly_dd_init (state->d, xa, ya, size);

  return status;
}

static int
polynomial_eval (const void *vstate,
                 const double xa[], const double ya[], size_t size, double x,
                 gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  *y = gsl_poly_dd_eval (state->d, xa, size, x);

  return GSL_SUCCESS;
}

static int
polynomial_deriv (const void *vstate,
                  const double xa[], const double ya[], size_t size, double x,
                  gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  gsl_poly_dd_taylor (state->coeff, x, state->d, xa, size, state->work);

  *y = state->coeff[1];

  return GSL_SUCCESS;
}

static int
polynomial_deriv2 (const void *vstate,
                   const double xa[], const double ya[], size_t size,
                   double x, gsl_interp_accel * acc, double *y)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;

  gsl_poly_dd_taylor (state->coeff, x, state->d, xa, size, state->work);

  *y = 2.0 * state->coeff[2];

  return GSL_SUCCESS;
}

static int
polynomial_integ (const void *vstate, const double xa[], const double ya[],
                  size_t size, gsl_interp_accel * acc, double a, double b,
                  double *result)
{
  const polynomial_state_t *state = (const polynomial_state_t *) vstate;
  size_t i;
  double sum;

  gsl_poly_dd_taylor (state->coeff, 0.0, state->d, xa, size, state->work);

  sum = state->coeff[0] * (b - a);

  for (i = 1; i < size; i++)
    {
      sum += state->coeff[i] * (pow (b, i + 1) - pow (a, i + 1)) / (i + 1.0);
    }

  *result = sum;

  return GSL_SUCCESS;
}

static void
polynomial_free (void *vstate)
{
  polynomial_state_t *state = (polynomial_state_t *) vstate;
  free (state->d);
  free (state->coeff);
  free (state->work);
  free (state);
}

static const gsl_interp_type polynomial_type = {
  "polynomial",
  3,
  &polynomial_alloc,
  &polynomial_init,
  &polynomial_eval,
  &polynomial_deriv,
  &polynomial_deriv2,
  &polynomial_integ,
  &polynomial_free,
};

const gsl_interp_type *gsl_interp_polynomial = &polynomial_type;


/*
 * kjg_fpca.c
 *
 *  Created on: Apr 28, 2014
 *      Author: Kevin
 */

#include <stdio.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_blas.h>

#include "kjg_fpca.h"
#include "kjg_gsl.h"

#include "admutils.h"
#include "gval.h"

size_t KJG_FPCA_ROWS = 256;

void
kjg_fpca (size_t K, size_t L, size_t I, double *eval, double *evec)
{
  if (K >= L)
    exit (1);
  if (I == 0)
    exit (1);

  size_t m = get_ncols ();
  size_t n = get_nrows ();

  // PART A - compute Q such that X ~ Q * (Q^T) * X
  gsl_matrix *G1 = gsl_matrix_alloc (n, L);
  gsl_matrix *G2 = gsl_matrix_alloc (n, L);
  gsl_matrix *Q = gsl_matrix_alloc (m, (I + 1) * L);
  gsl_matrix *Gswap;

  gsl_rng *r = kjg_gsl_rng_init ();
  kjg_gsl_ran_ugaussian_matrix (r, G1);
  gsl_rng_free (r);

  size_t i;
  for (i = 0; i < I; i++)
    {
      gsl_matrix_view Qi = gsl_matrix_submatrix (Q, 0, i * L, m, L);

      // do the multiplication
      kjg_fpca_XTXA (G1, &Qi.matrix, G2);

      // scale to prevent G2 from blowing up
      gsl_matrix_scale (G2, 1.0 / m);

      Gswap = G2;
      G2 = G1;
      G1 = Gswap;
    }

  gsl_matrix_view Qi = gsl_matrix_submatrix (Q, 0, I * L, m, L);
  kjg_fpca_XA (G1, &Qi.matrix);

  {
    gsl_matrix *V = gsl_matrix_alloc (Q->size2, Q->size2);
    gsl_vector *S = gsl_vector_alloc (Q->size2);

    kjg_gsl_SVD (Q, V, S);

    gsl_matrix_free (V);
    gsl_vector_free (S);
  }

  // kjg_gsl_matrix_QR(Q); // QR decomposition is less accurate than SVD

  gsl_matrix_free (G1);
  gsl_matrix_free (G2);

  // PART B - compute B matrix, take SVD and return
  gsl_matrix *B = gsl_matrix_alloc (n, (I + 1) * L);
  kjg_fpca_XTB (Q, B);

  gsl_matrix *Utilda = gsl_matrix_alloc ((I + 1) * L, (I + 1) * L);
  gsl_vector *Stilda = gsl_vector_alloc ((I + 1) * L);

  kjg_gsl_SVD (B, Utilda, Stilda);

  gsl_matrix_view Vk = gsl_matrix_submatrix (B, 0, 0, n, K);
  gsl_matrix_view evec_view = gsl_matrix_view_array (evec, n, K);
  gsl_matrix_memcpy (&evec_view.matrix, &Vk.matrix);

  gsl_vector_view Sk = gsl_vector_subvector (Stilda, 0, K);
  gsl_vector_view eval_view = gsl_vector_view_array (eval, K);
  gsl_vector_mul (&Sk.vector, &Sk.vector);
  gsl_vector_scale (&Sk.vector, 1.0 / m);
  gsl_vector_memcpy (&eval_view.vector, &Sk.vector);

  gsl_matrix_free (Q);
  gsl_matrix_free (B);
  gsl_matrix_free (Utilda);
  gsl_vector_free (Stilda);
}

void
kjg_fpca_XTXA (const gsl_matrix * A1, gsl_matrix * B, gsl_matrix * A2)
{
  size_t m = get_ncols ();
  size_t n = get_nrows ();

  size_t i, r;			// row index
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);	// normalized

  gsl_matrix_view Bi, Xi;

  gsl_matrix_set_zero (A2);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xi = gsl_matrix_view_array (Y, r, n);
      Bi = gsl_matrix_submatrix (B, i, 0, r, B->size2);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1, &Xi.matrix, A1, 0,
		      &Bi.matrix);
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1, &Xi.matrix, &Bi.matrix,
		      1, A2);
    }

  free (Y);
}

void
kjg_fpca_XA (const gsl_matrix * A, gsl_matrix * B)
{
  size_t n = get_nrows ();
  size_t m = get_ncols ();

  size_t i, r;
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);

  gsl_matrix_view Hmat, Xmat;

  gsl_matrix_set_zero (B);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xmat = gsl_matrix_view_array (Y, r, n);
      Hmat = gsl_matrix_submatrix (B, i, 0, r, B->size2);
      gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1, &Xmat.matrix, A, 0,
		      &Hmat.matrix);
    }

  free (Y);
}

void
kjg_fpca_XTB (const gsl_matrix * B, gsl_matrix * A)
{
  size_t n = get_nrows ();
  size_t m = get_ncols ();

  size_t i, r;
  double *Y = malloc (sizeof (double) * n * KJG_FPCA_ROWS);
  gsl_matrix_view Xmat;

  gsl_matrix_set_zero (A);

  for (i = 0; i < m; i += KJG_FPCA_ROWS)
    {
      r = kjg_geno_get_normalized_rows (i, KJG_FPCA_ROWS, Y);
      Xmat = gsl_matrix_view_array (Y, r, n);
      gsl_matrix_const_view Hmat = gsl_matrix_const_submatrix (B, i, 0, r,
							       B->size2);
      gsl_blas_dgemm (CblasTrans, CblasNoTrans, 1, &Xmat.matrix, &Hmat.matrix,
		      1, A);
    }

  free (Y);
}


//im_clam.c --
// A. Kern 3/20/15
//
// Composite Likelihood estimation of canonical IM models via AFS

//going to use NLopt for optimization

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <nlopt.h>
#include "AFS.h"
#include "adkGSL.h"
#include "cs.h"
#include "time.h"
#include <unistd.h>
#include "adkCSparse.h"
#include "AFS_ctmc.h"
#include <slepcmfn.h>
#include "AFS_ctmc_petsc.h"
#include "im_clam.h"

void import2DSFSData(const char *fileName, gsl_matrix *obsData);

PetscInt n1, n2;

afsStateSpace *stateSpace, *reducedStateSpace;

clam_lik_params *currentParams, *nextParams;

gsl_matrix *transMat;
double *res;

double lowerBounds[5] = {0.01,0.01,0.0,0.0,0.001};
double upperBounds[5] = {10.0,10.0,20.0,20.0,10.0};
PetscBool vbse = PETSC_FALSE;

static char help[] = "im_clam\n\
	Example: mpiexec -n <np> ./im_clam -s <stateSpace file> -m <mats file> -d <data file> \n\n\toptions:\n\
	\t-exp expected value mode (requires -x flag too)\n\
	\t-GIM uncertainty estimation mode (requires -x flag too)\n\
	\t-mo multiple optimizations from different start points\n\
	\t-global multi-level optimization (MLSL algo.)\n\
	\t-x <theta_2, theta_A, mig12, mig21, t_div> parameter starting values\n\
	\t-obs (prints out observed AFS as well as that expected from MLE params)\n\
	\t-u mutation rate per base pair per generation (only used to unscale parameters; default 1e-8)\n\
	\t-g generation time (gens/year; default 20)\n\
	\t-seqLen sequence length scanned for polymorphisms (used to unscale parameter)\n\
	\t-put upper bound for optimization of thetas\n\
	\t-pum upper bound for optimization of migration rates\n\
	\t-pudt upper bound for optimization of divergence time\n\
	\t-r randomSeed\n\
	\t-v verbose output\n";

int main(int argc, char **argv){
	PetscInt i,j, N,runMode;
	clock_t time1, time2;
	PetscInt nnz;
	PetscInt seed;
	char  filename[PETSC_MAX_PATH_LEN], filename2[PETSC_MAX_PATH_LEN], filename3[PETSC_MAX_PATH_LEN] ;
	double lik = 0.0;
	double mle[5] = {0.1,2,1,1,5};
 	const gsl_rng_type * T;
  	gsl_rng * r;
	PetscErrorCode ierr;
	PetscMPIInt    rank,size;
	PetscBool      flg,obsFlag;
//	Vec tmpVec;
	PetscInt dim=5;
	double snpNumber, pi_est, p, N0;
	double u=1e-8;
	double genPerYear=20;
	double put = 10.0;
	double pum = 20.0;
	double pudt=10.0;
	double propSnp;
	PetscInt seqLen;
	gsl_matrix *fi,*gi;
	FILE *infile;
	PetscInt testInt=0;
	PetscScalar one = 1.0;
	cs *ident;

	///////////////////
	/////	PETSC / Slepc library version of this code
	SlepcInitialize(&argc,&argv,(char*)0,help);
	ierr = MPI_Comm_rank(PETSC_COMM_WORLD, &rank);CHKERRQ(ierr);
	ierr = MPI_Comm_size(PETSC_COMM_WORLD, &size);CHKERRQ(ierr);
	if(rank==0){

		printf("\n.___   _____             .__\n|   | /     \\       ____ |  | _____    _____\n|   |/  \\ /  \\    _/ ___\\|  | \\__  \\  /     \\\n|   /    Y    \\   \\  \\___|  |__/ __ \\|  Y Y  \\\n|___\\____|__  /____\\___  >____(____  /__|_|  /\n            \\/_____/   \\/          \\/      \\/\n\n\n");

		printf("im_clam -- Isolation with Migration Composite Likelihood Analysis using Markov chains\n");
		//printf("A.D. Kern 2015\n///////////////////\n");
		printf("\n\n");
	}
	if(argc<2){
		printf("%s",help);
		exit(666);
	}
	time1=clock();

	ierr = PetscOptionsGetString(NULL,"-s",filename,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetString(NULL,"-m",filename2,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetString(NULL,"-d",filename3,PETSC_MAX_PATH_LEN,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetInt(NULL,"-r",&seed,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-u",&u,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-g",&genPerYear,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetInt(NULL,"-seqLen",&seqLen,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-put",&put,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-pum",&pum,&flg);CHKERRQ(ierr);
	ierr = PetscOptionsGetReal(NULL,"-pudt",&pudt,&flg);CHKERRQ(ierr);
	//set upper bounds
	upperBounds[0]=put;upperBounds[1]=put;upperBounds[2]=pum;upperBounds[3]=pum;upperBounds[4]=pudt;

	if(!flg) seed=time(NULL);

	//printf("rank:%d %d\n",rank,seed);
	MPI_Bcast(&seed,1,MPI_INT,0,PETSC_COMM_WORLD);
	//printf("rank:%d %d\n",rank,seed);

	runMode = 1;
	obsFlag=PETSC_FALSE;

	//setup RNG for starting point for optimization
	gsl_rng_env_setup();
	T = gsl_rng_default;
	r = gsl_rng_alloc (T);
	gsl_rng_set(r,seed);

	for(i=0;i<dim;i++){
		mle[i] = gsl_ran_flat(r,lowerBounds[i], upperBounds[i]);
	}

	PetscOptionsHasName(NULL,"-v",&flg); if(flg) vbse=PETSC_TRUE;
	PetscOptionsHasName(NULL,"-obs",&flg); if(flg) obsFlag=PETSC_TRUE;

	PetscOptionsHasName(NULL,"-exp",&flg); if(flg) runMode=2;
	ierr = PetscOptionsGetRealArray(NULL,"-x",mle,&dim,&flg);CHKERRQ(ierr);

	//double check we are set for exp mode
	if(runMode == 2 && !flg){
		printf("for exp runmode need to set parameter values using -x flag\n");
		exit(1);
	}

	PetscOptionsHasName(NULL,"-mo",&flg); if(flg) runMode=3;
	PetscOptionsHasName(NULL,"-global",&flg); if(flg) runMode=4;
	PetscOptionsHasName(NULL,"-GIM",&flg); if(flg) runMode=5;

	//import state space
	stateSpace = afsStateSpaceImportFromFile(filename);
	N = stateSpace->nstates;

	//quick peak at the mats file to set nnz
	//open file
	infile = fopen(filename2, "r");
	if (infile == NULL){
		fprintf(stderr,"Error opening mats file! ARRRRR!!!!\n");
		exit(1);
	}
	testInt=fscanf(infile,"nnz: %d", &nnz);
	fclose(infile);
	//got nnz, time for allocs

	//setup currentParams struct; alloc all arrays
	currentParams = malloc(sizeof(clam_lik_params));
	currentParams->rng = r;
	currentParams->n1 = n1 = stateSpace->states[0]->popMats[0]->size1 - 1;
	currentParams->n2 = n2 = stateSpace->states[0]->popMats[1]->size2 - 1;
	currentParams->stateSpace = stateSpace;
	currentParams->rank = rank;
//	currentParams->snpNumber = snpNumber;
	currentParams->expAFS = gsl_matrix_alloc(n1+1,n2+1);
	currentParams->expAFS2 = gsl_matrix_alloc(n1+1,n2+1);
	currentParams->map = malloc(sizeof(PetscInt)*N);
	currentParams->reverseMap = malloc(sizeof(PetscInt)*N);
	currentParams->rates = gsl_vector_alloc(stateSpace->nstates);
	currentParams->stateVec = gsl_vector_alloc(stateSpace->nstates);
	currentParams->resVec = gsl_vector_alloc(stateSpace->nstates);
	gsl_vector_set_zero(currentParams->resVec);
//	currentParams->paramVector = gsl_vector_alloc(5);
//	currentParams->paramCIUpper = gsl_vector_alloc(5);
//	currentParams->paramCILower = gsl_vector_alloc(5);
//	currentParams->mlParams = gsl_vector_alloc(5);

	currentParams->top = malloc(sizeof(double) * nnz);
	currentParams->topA = malloc(sizeof(double) * nnz);

	currentParams->move = malloc(sizeof(PetscInt) * nnz);
	currentParams->moveA = malloc(sizeof(PetscInt) * nnz);
//	printf("allocated moveType...\n");
	for(i=0;i< (nnz);i++){
		currentParams->move[i]=0;
		currentParams->top[i]=0;
		currentParams->moveA[i]=0;
		currentParams->topA[i]=0;
	}
	currentParams->dim1 = malloc(sizeof(PetscInt) * nnz);
	currentParams->dim2 = malloc(sizeof(PetscInt) * nnz);
	currentParams->dim1A = malloc(sizeof(PetscInt) * N * 10);
	currentParams->dim2A = malloc(sizeof(PetscInt) * N * 10);
	currentParams->b = malloc(N*sizeof(double));
	currentParams->expoArray = malloc(N*sizeof(double));
	currentParams->st = malloc(N*sizeof(double));
	currentParams->paramVector = gsl_vector_alloc(5);
	gsl_vector_set_all(currentParams->paramVector,1.0);
	currentParams->fEvals=0;

	currentParams->triplet = cs_spalloc(N, N, nnz + N , 1, 1); //alloc sparse mat with extra space for nonzero identity mats
	ident = cs_spalloc(N,N,N,1,1);
	for(i=0;i<N;i++) cs_entry(ident,i,i,1);
	currentParams->eye = cs_compress(ident);
	cs_spfree(ident);

	//set up some petsc matrices//////////////////////////////////////////////////////////////
	//
	//

	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->C);CHKERRQ(ierr);
//	MatSetType(C,MATMPIAIJ);
	ierr = MatSetSizes(currentParams->C, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->C);CHKERRQ(ierr);
//	MatSetOption(currentParams->C, MAT_NEW_NONZERO_ALLOCATION_ERR, PETSC_FALSE);
	ierr = MatSetUp(currentParams->C);CHKERRQ(ierr);

	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, N, N, NULL, &currentParams->denseMat1);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->denseMat1);CHKERRQ(ierr);
	MatAssemblyBegin(currentParams->denseMat1,MAT_FINAL_ASSEMBLY);
	MatAssemblyEnd(currentParams->denseMat1,MAT_FINAL_ASSEMBLY);

	///////// Set up MFN ////////////////////////////////////////////////////////////////////////////////////////
	ierr = MFNCreate(PETSC_COMM_WORLD,&currentParams->mfn);CHKERRQ(ierr);
	ierr = MFNSetFunction(currentParams->mfn,SLEPC_FUNCTION_EXP);CHKERRQ(ierr);
	ierr = MFNSetTolerances(currentParams->mfn,1e-07,PETSC_DEFAULT);CHKERRQ(ierr);

	//mapping stateSpace to reducedSpace
	currentParams->reducedStateSpace = afsStateSpaceNew();
	afsStateSpaceMapAndReducePopn(currentParams->stateSpace, currentParams->map, currentParams->reducedStateSpace, currentParams->reverseMap);
	currentParams->Na=currentParams->reducedStateSpace->nstates;

	VecCreate(PETSC_COMM_WORLD,&currentParams->ancStateVec);
	VecSetSizes(currentParams->ancStateVec,PETSC_DECIDE,currentParams->Na);
	VecSetFromOptions(currentParams->ancStateVec);
	VecDuplicate(currentParams->ancStateVec,&currentParams->ancResVec);

	//set up more matrices
	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->C2);CHKERRQ(ierr);
	MatSetType(currentParams->C2,MATMPIAIJ);
	ierr = MatSetSizes(currentParams->C2, PETSC_DECIDE, PETSC_DECIDE,currentParams->Na,currentParams->Na);CHKERRQ(ierr);
	ierr = MatSetFromOptions(currentParams->C2);CHKERRQ(ierr);
	ierr = MatSetUp(currentParams->C2);CHKERRQ(ierr);

	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, currentParams->Na, currentParams->Na, NULL, &currentParams->denseMat2);CHKERRQ(ierr);

	ident = cs_spalloc(currentParams->Na,currentParams->Na,currentParams->Na,1,1);
	for(i=0;i<currentParams->Na;i++) cs_entry(ident,i,i,1);
	currentParams->eyeAnc = cs_compress(ident);
	cs_spfree(ident);

	//DTMC with PETSC  /////////////
//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->D);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->D, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->D);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->D);CHKERRQ(ierr);

//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->D_copy);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->D_copy, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->D_copy);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->D_copy);CHKERRQ(ierr);

	//sparse identity mat
//	ierr = MatCreate(PETSC_COMM_WORLD,&currentParams->ident);CHKERRQ(ierr);
//	ierr = MatSetSizes(currentParams->ident, PETSC_DECIDE, PETSC_DECIDE,N,N);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->ident);CHKERRQ(ierr);
//	ierr = MatSetUp(currentParams->ident);CHKERRQ(ierr);

  //	MatAssemblyBegin(currentParams->ident,MAT_FINAL_ASSEMBLY);
  //	MatAssemblyEnd(currentParams->ident,MAT_FINAL_ASSEMBLY);
//	MatShift(currentParams->ident,one);

	//dense identity mat
//	ierr = MatCreateDense(PETSC_COMM_WORLD,PETSC_DECIDE, PETSC_DECIDE, N, N, NULL, &currentParams->denseIdent);CHKERRQ(ierr);
//	ierr = MatSetFromOptions(currentParams->denseIdent);CHKERRQ(ierr);

//	MatAssemblyBegin(currentParams->denseIdent,MAT_FINAL_ASSEMBLY);
  //	MatAssemblyEnd(currentParams->denseIdent,MAT_FINAL_ASSEMBLY);
//	MatShift(currentParams->denseIdent,one);

//	VecCreate(PETSC_COMM_WORLD,&currentParams->xInv);
//	VecSetSizes(currentParams->xInv,PETSC_DECIDE,N);
//	VecSetFromOptions(currentParams->xInv);
//	VecDuplicate(currentParams->xInv,&currentParams->bInv);

	///////////////////////////////////////

	//////////////////////////////

	// setup done! ///////////////
	////////////////////////////////////

	//import mats
	mcMatsImportFromFile(filename2, &nnz, currentParams->top, currentParams->move, currentParams->dim1, currentParams->dim2);
	currentParams->nnz = nnz;

	//init snpNumber to avoid compiler warning
	snpNumber=0;
	if(runMode != 2){
		//alloc data matrix and import data; estimate pi and N1
		currentParams->obsData = gsl_matrix_alloc(n1+1,n2+1);
		import2DSFSData(filename3, currentParams->obsData);
		snpNumber = gsl_matrix_sum(currentParams->obsData);
		propSnp = (float) snpNumber / (float) seqLen;
	//	pi_est = 0.0;
	//	for(i=1;i<n1;i++){
	//		for(j=0;j<n2+1;j++){
	//			p =  (float) i / n1;
	//			pi_est += (2 * p * (1.0 - p)) * gsl_matrix_get(currentParams->obsData,i,j);
	//		}
	//	}
	//	pi_est *= n1 / (n1 - 1) / snpNumber;
	//	N0=pi_est / u / 4;
	}

	////////////////////////////
	time2=clock();
	if(rank==0)printf("setup time:%f secs\n\n",(double) (time2-time1)/CLOCKS_PER_SEC);

	time1=clock();
	//expected AFS
	//printf("runMode = %d\n",runMode);
	switch(runMode){

		case 1:
		if(rank==0){
			printf("\nParameter estimation run mode\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		maximizeLikNLOpt(&lik, currentParams, mle);
		fi = getGodambeInfoMatrix(mle, lik, currentParams);
		//get N0 estimate
		N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;
		if(rank == 0){
			printf("for scaling:\nu: %e gen: %lf N0:%lf meanTreeLength:%lf seqLen:%d\n",u,genPerYear,N0,currentParams->meanTreeLength,seqLen);
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");

			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float)mle[i]*N0);
			for(i=2;i<4;i++)printf("%f\t",(float)mle[i]/N0/4);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))*N0);
			for(i=2;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))/N0/4);
			printf("%f\t",(float)sqrt(gsl_matrix_get(fi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");

			printf("\n\nlikelihood: %lf\n",-lik);
			currentParams->nnz = nnz;
			printf("\nExpected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
		}

		break;
		case 2:
		if(rank == 0)printf("expected value run mode\n");
		for(i=0;i<5;i++){
			gsl_vector_set(currentParams->paramVector,i,mle[i]);
		}
		calcLogAFS_IM(currentParams);
		currentParams->nnz = nnz;
		if(rank == 0){
			printf("Expected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
			printf("parameter values used:\n");
			for(i=0;i<dim;i++)printf("%f\t",gsl_vector_get(currentParams->paramVector,i));
			printf("\n\n");
		}
		time2=clock();
		//lik = calcLikNLOpt(5,mle,NULL,currentParams);
		//

		//printf("Likelihood: %g\n",lik);
		//if(rank==0)printf("w/ CSPARSE time:%f secs\n Liklihood Func. Evals: %d\n",(double) (time2-time1)/CLOCKS_PER_SEC,currentParams->fEvals);
		time1=clock();
		break;
		case 3:
		if(rank==0){
			printf("\nParameter estimation run mode with multiple optimizations\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		for(j = 0; j < 3; j++){

			for(i=0;i<5;i++)mle[i] = gsl_ran_flat(r,lowerBounds[i], upperBounds[i]);
			printf("optimization %d initial parameter guess:\n",j);
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			maximizeLikNLOpt(&lik, currentParams, mle);
			if(rank == 0){
				printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
				printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
				for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
				printf("\n\nlikelihood: %lf\n",-lik);
				currentParams->nnz = nnz;
				printf("\nExpected AFS:\n");
				gsl_matrix_prettyPrint(currentParams->expAFS);
			}
		}

		break;
		case 4:
		if(rank==0){
			printf("\nParameter estimation run mode\n\n");
			printf("now optimizing....\n\n");
			printf("initial parameter guess:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n\n");
		}
		maximizeLikNLOpt_MLSL(&lik, currentParams, mle);
		fi = getGodambeInfoMatrix(mle, lik, currentParams);
		if(rank == 0){
			N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");

			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float)mle[i]*N0);
			for(i=2;i<4;i++)printf("%f\t",(float)mle[i]/N0/4);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<2;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))*N0);
			for(i=2;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(fi,i,i))/N0/4);
			printf("%f\t",(float)sqrt(gsl_matrix_get(fi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");

			printf("\n\nlikelihood: %lf\n",-lik);
			currentParams->nnz = nnz;
			printf("\nExpected AFS:\n");
			gsl_matrix_prettyPrint(currentParams->expAFS);
		}

		break;
		case 5:
		for(i=0;i<5;i++){
			gsl_vector_set(currentParams->paramVector,i,mle[i]);
		}
		if(rank==0){
			printf("\nUncertainty estimation (via Godambe Information) run mode\n\n");
			printf("MLE parameters:\n");
			for(i=0;i<5;i++)printf("%f ",mle[i]);
			printf("\n");
		}
		lik = calcLikNLOpt(5,mle,NULL,currentParams);
		gi = getGodambeInfoMatrix(mle, lik, currentParams);
		N0 = propSnp / currentParams->meanTreeLength / 4.0 / u;

		// if(rank==0){
		// 	printf("FIM:\n");
		// 	gsl_matrix_prettyPrint(fi);
		// 	printf("\nGIM:\n");
		// 	gsl_matrix_prettyPrint(gi);
		// 	printf("\n");
		// }
		if(rank==0){
			printf("\nlikelihood: %lf\n",-lik);
			printf("Composite Likelihood estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float)mle[i]);
			printf("\n\nComposite Likelihood estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<4;i++)printf("%f\t",(float)mle[i]*N0);
			printf("%f\t",(float)mle[i] * N0 * 4.0/ (float)genPerYear);
			printf("\n\nUncertainty estimates of IM params (scaled by 1/theta_pop1):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<5;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(gi,i,i)));
			printf("\n\nUncertainty estimates of IM params (unscaled):\n");
			printf("theta_pop2\ttheta_anc\tmig_1->2\tmig_2->1\tt_div\n");
			for(i=0;i<4;i++)printf("%f\t",(float) sqrt(gsl_matrix_get(gi,i,i))*N0);
			printf("%f\t",(float)sqrt(gsl_matrix_get(gi,i,i)) * N0 * 4.0/ (float)genPerYear);
			printf("\n");
		}
		break;
	}

	if(obsFlag && rank==0){
		gsl_matrix_scale(currentParams->obsData,1.0/snpNumber);
		printf("%f SNPs in dataset\n",snpNumber);
		printf("Observed AFS:\n");
		gsl_matrix_prettyPrint(currentParams->obsData);
		gsl_matrix_scale(currentParams->obsData,snpNumber);
	}

	MatDestroy(&currentParams->denseMat1);
	MatDestroy(&currentParams->denseMat2);
	MatDestroy(&currentParams->C);
	MatDestroy(&currentParams->C2);
	MatDestroy(&currentParams->D);
	MatDestroy(&currentParams->D_copy);
	MatDestroy(&currentParams->denseIdent);
	MatDestroy(&currentParams->ident);
	MFNDestroy(&currentParams->mfn);
	VecDestroy(&currentParams->ancStateVec);
	VecDestroy(&currentParams->ancResVec);
	VecDestroy(&currentParams->xInv);
	VecDestroy(&currentParams->bInv);
	gsl_rng_free (r);

	afsStateSpaceFree(currentParams->reducedStateSpace);
	time2=clock();
	if(rank==0)printf("total run time:%f secs\n Liklihood Func. Evals: %d\n",(double) (time2-time1)/CLOCKS_PER_SEC,currentParams->fEvals);
	ierr = PetscFinalize();

	return(0);
}

//import2DSFSData -- imports a matrix from fileName and stuffs it in prealloc'd obsData
void import2DSFSData(const char *fileName, gsl_matrix *obsData){
	FILE *infile;

	//open file
	infile = fopen(fileName, "r");
	if (infile == NULL){
		fprintf(stderr,"Error opening data file! ARRRRR!!!!\n");
		exit(1);
	}
	gsl_matrix_fscanf(infile,obsData);
	fclose(infile);
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */

/*
 *
 *
 *       Filename:  red_gen.c
 *
 *    Description:  Redfield-type Generator
 *
 *        Version:  1.0
 *        Created:  25/04/2014 22:28:19
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_math.h>
#include <math.h>
#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  red_mat
 *  Description:  Creating the Redfield generator matrix in Bloch form, taking as
 *  		  arguments all the integrals, the physical parameters
 *  		  and the pointer to the matrix
 *
 */
int red_mat ( gsl_matrix* red_mx , void* params )
{
	double integrals[12] ;
	if ( integration(params,integrals) != 0 )
		return -1;

	/* Setting the integrals */
	double rcc, icc, rss, iss, rsc, isc, rcs, ics, rc0, ic0, rs0, is0 ;
	rcc = integrals[0] ;
	icc = integrals[1] ;
	rss = integrals[2] ;
	iss = integrals[3] ;
	rsc = integrals[4] ;
	isc = integrals[5] ;
	rcs = integrals[6] ;
	ics = integrals[7] ;
	rc0 = integrals[8] ;
	ic0 = integrals[9] ;
	rs0 = integrals[10] ;
	is0 = integrals[11] ;

	/* Copying the parameters */
	struct f_params* pars = (struct f_params*) params ;
	double o_c, b, O, o_1 ;
	assign_p ( pars, &o_c, &b, &O, &o_1 ) ;
	double D = sqrt(POW_2(o_1)-POW_2(O)) ;

	/* Ensuring that the matrix is zeroed */
	gsl_matrix_set_zero ( red_mx ) ;

	/* Building the matrix */
	gsl_matrix_set ( red_mx, 1, 0, (D/o_1)*(-(O/o_1)*ic0+iss+(O/o_1)*icc) ) ;
	gsl_matrix_set ( red_mx, 1, 1, POW_2(D/o_1)*rc0+(O/o_1)*rss+POW_2(O/o_1)*rcc ) ;
	gsl_matrix_set ( red_mx, 1, 2, o_1/4+(O/o_1)*rsc-POW_2(O/o_1)*rcs ) ;
	gsl_matrix_set ( red_mx, 1, 3, (D/o_1)*(rsc-(O/o_1)*rcs) ) ;
	gsl_matrix_set ( red_mx, 2, 0, (D/o_1)*(is0+isc-(O/o_1)*ics) ) ;
	gsl_matrix_set ( red_mx, 2, 1, -(o_1/4)-(O/o_1)*rsc+rcs ) ;
	gsl_matrix_set ( red_mx, 2, 2, POW_2(D/o_1)*rc0+rcc+(O/o_1)*rss ) ;
	gsl_matrix_set ( red_mx, 2, 3, -(D/o_1)*((O/o_1)*rcc+rss) ) ;
	gsl_matrix_set ( red_mx, 3, 0, (1+POW_2(O/o_1))*ics-2*(O/o_1)*isc ) ;
	gsl_matrix_set ( red_mx, 3, 1, -(D/o_1)*rs0 ) ;
	gsl_matrix_set ( red_mx, 3, 2, -(O*D/(o_1*o_1))*rc0 ) ;
	gsl_matrix_set ( red_mx, 3, 3, (1+POW_2(O/o_1))*rcc+2*(O/o_1)*rss ) ;

	/* Multiplies times -4 to obtain the Bloch matrix */
	gsl_matrix_scale ( red_mx, -4 ) ;

	return 0;
}		/* -----  end of function red_mat  ----- */