/*
** CCM - connectivity consistency mapping
**
** G.Lohmann, July 2010
*/
#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/mu.h>
#include <viaio/option.h>
#include <viaio/os.h>
#include <viaio/VImage.h>

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

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

#define SQR(x) ((x) * (x))
#define ABS(x) ((x) > 0 ? (x) : -(x))

extern double VOCCC(gsl_matrix *,int,int);
extern double VICC(gsl_matrix *,int);
extern double VKendall_W(gsl_matrix *,int,int);
extern double SpearmanCorr(const float *data1,const float *data2, int n);

int metric = 1;
int verbose = 0;

VDictEntry TypeDict[] = {
  { "kendall", 0 },
  { "occc", 1 },
  { NULL }
};

double Correlation(const float *arr1,const float *arr2,int n)
{
  int i;
  double sx,sy,sxx,syy,sxy,rx;
  double tiny=1.0e-4;

  sxx = syy = sxy = sx = sy = 0;
  for (i=0; i<n; i++) {
    const double u = (double)arr1[i];
    const double v = (double)arr2[i];
    sx  += u;
    sy  += v;
    sxx += u*u;
    syy += v*v;
    sxy += u*v;
  }
  const double nx= n;
  const double u = nx*sxx - sx*sx;
  const double v = nx*syy - sy*sy;
  rx = 0;
  if (u*v > tiny)
    rx = (nx*sxy - sx*sy)/sqrt(u*v);
  return rx;
}

VImage VCCM(VImage **src, VImage mask, int nsubjects, int nslices,int first,int length,int type,int otype)
{
  VImage map=NULL,dest=NULL;
  int i,j,k,s,nvoxels,nt,len,last,b,r,c,bb,rr,cc,rad2,nrows,ncols;
  double u=0,v=0;
  gsl_matrix *data=NULL;
  gsl_matrix_float **mat=NULL;

  nrows = VImageNRows(mask);
  ncols = VImageNColumns(mask);

  /* count number of voxels, number of timesteps */
  nvoxels = nt = 0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	if (VGetPixel(mask,b,r,c) < 0.001) continue;
	if (VImageNBands(src[0][b]) > nt)
	  nt = VImageNBands(src[0][b]);
	nvoxels++;
      }
    }
  }

  /* get shortest length of time series across subjects */
  nt = 99999;
  for (s=0; s<nsubjects; s++) {
    for (b=0; b<nslices; b++) {
      for (r=0; r<nrows; r++) {
	for (c=0; c<ncols; c++) {
	  if (VGetPixel(mask,b,r,c) < 0.001) continue;
	  if (VImageNBands(src[s][b]) < nt)
	    nt = VImageNBands(src[s][b]);
	}
      }
    }
  }

  /* get time steps to include */
  if (length < 1) length = nt-2;
  last = first + length -1;
  if (last >= nt) last = nt-1;
  if (first < 0) first = 1;

  nt = last - first + 1;
  if (nt < 2) VError(" not enough timesteps, nt= %d",nt);

  fprintf(stderr," first= %d, last= %d, ntimesteps= %d\n",
          (int)first,(int)last,(int)nt);
  fprintf(stderr," nsubjects= %d,  nvoxels= %d\n",nsubjects,nvoxels);
  fprintf(stderr," type= %s\n",TypeDict[type].keyword);

  /*
  ** store voxel addresses
  */
  map = VCreateImage(1,3,nvoxels,VIntegerRepn);
  if (map == NULL) VError(" error allocating addr map");
  VFillImage(map,VAllBands,0);

  i = 0;
  for (b=0; b<nslices; b++) {
    for (r=0; r<nrows; r++) {
      for (c=0; c<ncols; c++) {
	if (VGetPixel(mask,b,r,c) < 0.001) continue;
	if (i >= nvoxels) VError(" nvox= %d",i);
	VPixel(map,0,0,i,VInteger) = b;
	VPixel(map,0,1,i,VInteger) = r;
	VPixel(map,0,2,i,VInteger) = c;
	i++;
      }
    }
  }

  /*
  ** avoid casting to float, copy data to matrix
  */
  mat = (gsl_matrix_float **) VCalloc(nsubjects,sizeof(gsl_matrix_float *));
  if (!mat) VError(" err allocating mat");

  for (s=0; s<nsubjects; s++) {
    mat[s] = gsl_matrix_float_calloc(nvoxels,nt);
    if (!mat[s]) VError(" err allocating mat[s]");

    for (i=0; i<nvoxels; i++) {
      b = VPixel(map,0,0,i,VInteger);
      r = VPixel(map,0,1,i,VInteger);
      c = VPixel(map,0,2,i,VInteger);

      float *ptr = gsl_matrix_float_ptr(mat[s],i,0);
      int k;
      for (k=first; k<=last; k++) {
	*ptr++ = (float) VGetPixel(src[s][b],k,r,c);
      }
    }
  }

  /*
  ** for each seed voxel assess intersubject consistency
  */
  dest = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(dest,VAllBands,0);
  VCopyImageAttrs (src[0][0], dest);

  /*
  ** get correlation maps
  */
  data = gsl_matrix_calloc(nsubjects,nvoxels);

  rad2 = 3*9;
  for (i=0; i<nvoxels; i++) {
    if (i%10 == 0) fprintf(stderr," i: %7d  of  %d\r",i,nvoxels);
    b = VPixel(map,0,0,i,VInteger);
    r = VPixel(map,0,1,i,VInteger);
    c = VPixel(map,0,2,i,VInteger);

    len = 0;
    for (s=0; s<nsubjects; s++) {
      const float *arr1 = gsl_matrix_float_ptr(mat[s],i,0);

      k = 0;
      for (j=0; j<nvoxels; j++) {
	if (i == j) continue;
	bb = VPixel(map,0,0,j,VInteger);
	rr = VPixel(map,0,1,j,VInteger);
	cc = VPixel(map,0,2,j,VInteger);
	/* exclude because of spatial smoothness: */
	if ((SQR(b-bb) + SQR(r-rr) + SQR(c-cc)) < rad2) continue;

	const float *arr2 = gsl_matrix_float_ptr(mat[s],j,0);
	u = Correlation(arr1,arr2,nt);

	gsl_matrix_set(data,s,k,(double)u);
	k++;
      }
      len = k;
    }
    if (len >= data->size2) VError(" len= %d %d",len,data->size2);

    if (type == 0)
      v = VKendall_W(data,len,otype);
    else if (type == 1)
      v = VOCCC(data,len,verbose);
    else
      VError(" illegal type");

    b = VPixel(map,0,0,i,VInteger);
    r = VPixel(map,0,1,i,VInteger);
    c = VPixel(map,0,2,i,VInteger);
    VPixel(dest,b,r,c,VFloat) = v;
  }
  fprintf(stderr,"\n");
  return dest;
}

int main (int argc, char *argv[])
{
  static VArgVector in_files;
  static VString out_filename;
  static VString mask_filename;
  static VShort type   = 0;
  static VShort otype  = 0;
  static VShort first  = 2;
  static VShort length = 0;
  static VOptionDescRec options[] = {
    {"in", VStringRepn, 0, & in_files, VRequiredOpt, NULL,"Input files" },
    {"out", VStringRepn, 1, & out_filename, VRequiredOpt, NULL,"Output file" },
    {"mask",VStringRepn,1,(VPointer) &mask_filename,VRequiredOpt,NULL,"Mask file"},
    {"first",VShortRepn,1,(VPointer) &first,VOptionalOpt,NULL,"First timestep to use"},
    {"length",VShortRepn,1,(VPointer) &length,VOptionalOpt,NULL,
     "Length of time series to use, '0' to use full length"},
    {"type",VShortRepn,1,(VPointer) &type,VOptionalOpt,TypeDict,"Type of metric"},
    {"gauss",VShortRepn,1,(VPointer) &otype,VOptionalOpt,NULL,"Use gaussian version of kendall"},
  };
  VString in_filename;
  VAttrList list=NULL,geolist=NULL;
  int b,r,c,i=0;
  float u;
  char *prg = GetLipsiaName("vccm");

  /*
  ** parse command line
  */
  if (! VParseCommand (VNumber (options), options, & argc, argv)) {
    VReportUsage (argv[0], VNumber (options), options, NULL);
    exit (EXIT_FAILURE);
  }
  if (argc > 1) {
    VReportBadArgs (argc, argv);
    exit (EXIT_FAILURE);
  }

  /*
  ** read mask
  */
  VAttrList mask_list = VReadAttrList(mask_filename,0L,TRUE,FALSE);
  if (mask_list == NULL) VError(" error reading %s",mask_filename);
  VImage mask = VReadImage(mask_list);
  if (mask == NULL) VError(" no mask found");

  /*
  ** read input images
  */
  int nslices=0,nt=0,nrows=0,ncols=0;
  int xslices=0,xnt=0,xrows=0,xcols=0;
  int nsubjects = (int)in_files.number;
  fprintf(stderr," nsubjects= %d\n",nsubjects);
  if (nsubjects < 2) VError(" not enough input files (%d), CCM should be used on >= 2 data sets",nsubjects);
  VImage **src = (VImage **) VCalloc(nsubjects,sizeof(VImage *));

  for (i=0; i<nsubjects; i++) {
    in_filename = ((VString *) in_files.vector)[i];
    list    = VReadAttrList(in_filename,0L,TRUE,FALSE);
    xslices = VAttrListNumImages(list);
    if (i==0) nslices = xslices;
    else if (nslices != xslices) VError(" inconsistent dimensions in input files");
    src[i] = VAttrListGetImages(list,nslices);
    if (i==0) VImageDimensions(src[i],nslices,&nt,&nrows,&ncols);
    else {
      VImageDimensions(src[i],xslices,&xnt,&xrows,&xcols);
      if (nrows != xrows) VError(" inconsistent dimensions in input files");
      if (ncols != xcols) VError(" inconsistent dimensions in input files");
      if (nt != xnt) VError(" inconsistent dimensions in input files");
    }

    /* use geometry info from 1st file */
    if (geolist == NULL) geolist = VGetGeoInfo(list);
  }

  /*
  **  CCM
  */
  VImage dest = VCCM(src,mask,nsubjects,nslices,(int)first,(int)length,(int)type,(int)otype);

  /* invert to get discordant map */
  VImage disc = VCreateImageLike(dest);
  VFillImage(disc,VAllBands,0);
  for (b=0; b<VImageNBands(dest); b++) {
    for (r=0; r<VImageNRows(dest); r++) {
      for (c=0; c<VImageNColumns(dest); c++) {
        if (VGetPixel(mask,b,r,c) < 1) continue;
        u = VPixel(dest,b,r,c,VFloat);
        VPixel(disc,b,r,c,VFloat) = 1-u;
      }
    }
  }

  /*
  ** output
  */
  VAttrList out_list = VCreateAttrList();
  if (geolist != NULL) {
    double *D = VGetGeoDim(geolist,NULL);
    D[0] = 3;  /* 3D */
    D[4] = 1;  /* just one timestep */
    VSetGeoDim(geolist,D);
    VSetGeoInfo(geolist,out_list);
  }
  VAppendAttr (out_list,"concordant",NULL,VImageRepn,dest);
  VAppendAttr (out_list,"discordant",NULL,VImageRepn,disc);

  VHistory(VNumber(options),options,prg,&list,&out_list);
  FILE *fp = VOpenOutputFile (out_filename, TRUE);
  if (! VWriteFile (fp, out_list)) exit (1);
  fclose(fp);
  fprintf (stderr, "%s: done.\n", argv[0]);
  exit(0);
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_spline.h>
// #include "mex.h"

#ifndef verbose
#define verbose 0
#endif
/*
   $ ./fwhm > interp.dat
   $ graph -T ps < interp.dat > interp.ps
   */

struct dbg_data {
  double xm; // location of maxima
  double ym; // value at maxima
  double xleft;
  double xright;
};

FILE * logFile;

// my_f and my_f_params defines the interpolation function that the
// root-finding functions are run on

struct my_f_params {
  gsl_spline * spline;
  gsl_interp_accel * acc;
  double  offset; // shifts the function
};

double my_f(double , void * );
int findmin(double , double , gsl_spline *, gsl_interp_accel * , size_t , double * , double * );
static void createGaussian(double * , double * , size_t , double );
int fwhm(double * , double * , size_t , double * );

double my_f(double x, void * p)
{
  struct my_f_params * params = (struct my_f_params*) p;
#if verbose > 0
  printf("f(%f) + %f=  ", x, params->offset); fflush(stdout);
#endif

  double y =  -gsl_spline_eval(params->spline, x, params->acc) + params->offset;
#if verbose > 0
  printf("%f\n", y); fflush(stdout);
#endif

  return(y);
}

int findmin(double a, double b,
    gsl_spline *spline, gsl_interp_accel * acc,
    size_t N,
    double * xm, double * ym)
{

  // a, b: range of function
  int iter = 0, max_iter = 100;
  double m = 0; // expected location of minima

  int status;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;

  struct my_f_params f_params;
  f_params.spline = spline;
  f_params.acc = acc;
  f_params.offset = 0;

  gsl_function F;
  F.function = &my_f;
  F.params = (void *) &f_params;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);
  if(gsl_min_fminimizer_set (s, &F, m, a, b) == GSL_EINVAL)
  {
    // If no minima is enclosed ... i.e. the value at (a+b)/2 isn't lower than at a and b
    gsl_min_fminimizer_free(s);
    return 1;
  }

#if verbose > 0
  fflush(stdout);
  printf("Interval: [%f, %f]\n", a, b);
  double m_expected = -10;
  printf ("method: '%s'\n", gsl_min_fminimizer_name (s));
  printf ("%5s [%9s, %9s] %9s %10s %9s\n", "iter", "lower", "upper", "min", "err", "err(est)");
  printf ("%5d [%.7f, %.7f] %.7f %+.7f %.7f\n", iter, a, b, m, m - m_expected, b - a);
#endif

  do {
    iter++;
    status = gsl_min_fminimizer_iterate (s);

    a = gsl_min_fminimizer_x_lower (s);
    b = gsl_min_fminimizer_x_upper (s);

    status = gsl_min_test_interval (a, b, 0.001, 0.0);

#if verbose > 0
    float m = gsl_min_fminimizer_x_minimum (s);
    if (status == GSL_SUCCESS) {
      printf ("Converged:\n");
    } else { printf("No convergence %d\n", status); }

    printf ("%5d [%.7f, %.7f] " "%.7f %+.7f %.7f\n", iter, a, b, m, m - m_expected, b - a);
#endif

  } while (status == GSL_CONTINUE && iter < max_iter);

  xm[0] = (a+b)/2;
  ym[0] = my_f(xm[0], &f_params);

  gsl_min_fminimizer_free(s);

#if verbose > 0
  printf("Status: %d\n", status);
#endif

  return status;
}

int fwhm(double * x, double * y, size_t N, double * w)
{

  int useLog = 0;

  if(verbose)
  {
    useLog = 1;
  }

  if(useLog)
  {
    logFile = fopen("/tmp/fwhmlog", "w");
  }

#ifndef debug
gsl_set_error_handler_off();
#endif

  int N2 = (N-1)/2; // even number

  // Set up interpolation
  gsl_interp_accel *acc = gsl_interp_accel_alloc();  // Neccessary?
  gsl_spline *spline = gsl_spline_alloc (gsl_interp_cspline, N);
  gsl_spline_init(spline, x, y, N);

  // See that the correct value is given at x=0
#if verbose > 0
  printf("y[%f] = %f\n", x[N2], gsl_spline_eval(spline, x[N2], acc));
  fflush(stdout);
#endif

  // Find position of max, i.e., centre in [xmin, xmax]
  double xmin = -2.5;
  double xmax = 2.501;

  double xm = 10e99;
  double ym = 10e99;
#if verbose > 0
  printf("calling findmin\n"); fflush(stdout);
#endif
  if( findmin(xmin, xmax, spline, acc, N, &xm, &ym) )
  {
#if verbose > 0
    printf("Could not find the position of the maxima!\n");
#endif
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);

    if(useLog)
    {
      fclose(logFile);
    }
    return 1;
  }

  if(useLog)
  {
    fprintf(logFile, "xm: %f, ym: %f\n", xm, ym);
  }

#if verbose > 0
  printf("-> Minima: f(%f) = %f\n", xm, ym);
#endif

  // Determine background
  double bg = (y[0]+y[N-1])/2;
#if verbose > 0
  printf("Background level: %f\n", bg);
#endif

  if(useLog)
  {
    fprintf(logFile, "bg: %f\n", bg);
  }

  if(bg <= ym)
  {
    fprintf(logFile, "stopping, bg <= tm\n");
    if(useLog)
    {
      fclose(logFile);
    }
    gsl_spline_free (spline);
    gsl_interp_accel_free (acc);

    return 1;
  }

  // Find intersection .5*(max-bg) for each side
  // ROOT FINDING

  const gsl_root_fsolver_type * Tsolve = gsl_root_fsolver_bisection;
  gsl_root_fsolver * rsolve = gsl_root_fsolver_alloc(Tsolve);

  // Search for left and right intersections

  double intersections[2];
  intersections[0] = 0; intersections[1] = 0;

  for(int dire = 0; dire<2; dire++)
  {
    // Search in range [x_lo, x_hi]
    double x_lo;
    double x_hi;

    if(dire==0)
    {
      x_lo = -N2;
      x_hi = xm;
    }

    if(dire==1)
    {
      x_lo = xm;
      x_hi = N2;
    }

    struct my_f_params f_params;
    f_params.spline = spline;
    f_params.acc = acc;
    f_params.offset = (bg+(-ym-bg)/2);

    if(useLog)
    {
      fprintf(logFile, "x_lo: %f x_hi: %f\n", x_lo, x_hi);
      fprintf(logFile, "offset: %f, bg: %f, ym: %f\n", f_params.offset, bg, ym);
    }

    gsl_function F;
    F.function = &my_f;
    F.params = (void *) &f_params;

    /*
     * Function: int gsl_root_fsolver_set (gsl_root_fsolver * s, gsl_function * f, double x_lower, double x_upper)
     * This function initializes, or reinitializes, an existing solver s to use the function f and the initial
     * search interval [x_lower, x_upper]
     */

    int status =  gsl_root_fsolver_set(rsolve, &F, x_lo, x_hi);

    size_t iter = 0;
    size_t max_iter = 10;

#if verbose > 0
    double r_expected = (x_lo+x_hi)/2;
    printf ("using %s method\n", gsl_root_fsolver_name (rsolve));
    printf ("%5s [%9s, %9s] %9s %10s %9s\n", "iter", "lower", "upper", "root", "err", "err(est)");
#endif

    do {
      iter++;
      status = gsl_root_fsolver_iterate (rsolve);
      // update bounds
      x_lo = gsl_root_fsolver_x_lower (rsolve);
      x_hi = gsl_root_fsolver_x_upper (rsolve);
      // see if converged
      status = gsl_root_test_interval (x_lo, x_hi, 0, 0.001);
#if verbose > 0
      double r = gsl_root_fsolver_root (rsolve);
      if (status == GSL_SUCCESS)
        printf ("Converged:\n");
      printf ("%5lu [%.7f, %.7f] %.7f %+.7f %.7f\n", iter, x_lo, x_hi,
          r, r - r_expected,
          x_hi - x_lo);
#endif
    }
    while (status == GSL_CONTINUE && iter < max_iter);

#if verbose > 0
  printf("Found intersection at %f\n", (x_lo+x_hi)/2);
#endif
  intersections[dire] = (x_lo+x_hi)/2;

  if(useLog)
  {
    fprintf(logFile, "intersection %d: %f\n", dire, (x_lo+x_hi)/2);
  }
  }

  gsl_root_fsolver_free(rsolve);

  // Clean up
  gsl_spline_free (spline);
  gsl_interp_accel_free (acc);

  // fwhm: right-left positions
  w[0] = intersections[1] - intersections[0];

  if(useLog)
  {
    fclose(logFile);
  }

  return 0; // Ok
}

static void createGaussian(
    double * x,
    double * y,
    size_t N,
    double x0)
{
  /* Create a gaussian shaped test signal, y
   * over the domain x
   * x0 is the offset from center
   * */
  int N2 = (N-1)/2; // middle element

  for(int kk=0; kk<N; kk++)
  {
    double p = 2*((double) kk - N2)/N; // [-1,1];
    p = (p*3-x0);
    y[kk] =  exp(-p*p);
    x[kk] = kk-N2; // Set x to pixel position from centre
#if verbose>0
    printf("%f %f\n", x[kk], y[kk]);
#endif
  }
}

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

  int N = 11; // Size of test signal
  double * y = malloc(N*sizeof(double));
  double * x = malloc(N*sizeof(double));
  memset(x, 0, N*sizeof(double));
  memset(y, 0, N*sizeof(double));
  createGaussian(x,y, N, 0);

  double w = -1; // output

  if(1){
    for(double delta = -1; delta <=1; delta +=0.1)
    {

      createGaussian(x,y, N, delta);
      if(fwhm(x, y, N, &w))
      {
        printf("Error: Could not calculate fwhm!\n");
      }

      printf("offset: % .2f pixels, fwhm: %f pixels\n", delta, w);
    }
  }

  for(int kk = 0; kk<N; kk++)
  {
    y[kk] = 0;
  }

  if(1){
  printf("constant y\n");
  if(fwhm(x, y, N, &w))
  {
    printf("# Error!\n");
    free(y); free(x);
    return(1);
  }
  }

  free(y);
  free(x);
  return(0);
}


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

/*
 * This module handles the solution of the linear least squares
 * system:
 *
 * [     J~      ] p~ = - [ f ]
 * [ sqrt(mu)*D~ ]        [ 0 ]
 *
 * using a QR approach. Quantities are scaled according to:
 *
 * J~ = J S
 * D~ = D S
 * p~ = S^{-1} p
 *
 * where S is a diagonal matrix and S_jj = || J_j || and J_j is column
 * j of the Jacobian. This balancing transformation seems to be more
 * numerically stable for some Jacobians.
 */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>

#include "common.c"
#include "qrsolv.c"

typedef struct
{
  size_t p;
  gsl_matrix *QR;            /* QR factorization of J */
  gsl_vector *tau_Q;         /* Householder scalars for Q */
  gsl_matrix *T;             /* workspace matrix for qrsolv, p-by-p */
  gsl_permutation *perm;     /* permutation matrix */
  size_t rank;               /* rank of J */
  gsl_vector *residual;      /* residual of LS problem [ J; sqrt(mu) D ] p = - [ f; 0 ] */
  gsl_vector *qtf;           /* Q^T f */
  gsl_vector *workn;         /* workspace, length n */
  gsl_vector *workp;         /* workspace, length p */
  gsl_vector *work3p;        /* workspace, length 3*p */
  double mu;                 /* LM parameter */
} qr_state_t;

static int qr_init(const void * vtrust_state, void * vstate);
static int qr_presolve(const double mu, const void * vtrust_state, void * vstate);
static int qr_solve(const gsl_vector * f, gsl_vector *x,
                    const void * vtrust_state, void *vstate);
static int qr_rcond(double * rcond, void * vstate);

static void *
qr_alloc (const size_t n, const size_t p)
{
  qr_state_t *state;

  (void)n;

  state = calloc(1, sizeof(qr_state_t));
  if (state == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate qr state", GSL_ENOMEM);
    }

  state->QR = gsl_matrix_alloc(n, p);
  if (state->QR == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for QR", GSL_ENOMEM);
    }

  state->tau_Q = gsl_vector_alloc(p);
  if (state->tau_Q == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for tau_Q",
                      GSL_ENOMEM);
    }

  state->T = gsl_matrix_alloc(p, p);
  if (state->T == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for T", GSL_ENOMEM);
    }

  state->qtf = gsl_vector_alloc(n);
  if (state->qtf == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for qtf",
                      GSL_ENOMEM);
    }

  state->residual = gsl_vector_alloc(n);
  if (state->residual == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for residual",
                      GSL_ENOMEM);
    }

  state->perm = gsl_permutation_calloc(p);
  if (state->perm == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for perm",
                      GSL_ENOMEM);
    }

  state->workn = gsl_vector_alloc(n);
  if (state->workn == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workn",
                      GSL_ENOMEM);
    }

  state->workp = gsl_vector_alloc(p);
  if (state->workp == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for workp",
                      GSL_ENOMEM);
    }

  state->work3p = gsl_vector_alloc(3 * p);
  if (state->work3p == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for work3p",
                      GSL_ENOMEM);
    }

  state->p = p;
  state->mu = 0.0;
  state->rank = 0;

  return state;
}

static void
qr_free(void *vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;

  if (state->QR)
    gsl_matrix_free(state->QR);

  if (state->tau_Q)
    gsl_vector_free(state->tau_Q);

  if (state->T)
    gsl_matrix_free(state->T);

  if (state->qtf)
    gsl_vector_free(state->qtf);

  if (state->residual)
    gsl_vector_free(state->residual);

  if (state->perm)
    gsl_permutation_free(state->perm);

  if (state->workn)
    gsl_vector_free(state->workn);

  if (state->workp)
    gsl_vector_free(state->workp);

  if (state->work3p)
    gsl_vector_free(state->work3p);

  free(state);
}

/* compute J = Q R PT */
static int
qr_init(const void * vtrust_state, void * vstate)
{
  const gsl_multifit_nlinear_trust_state *trust_state =
    (const gsl_multifit_nlinear_trust_state *) vtrust_state;
  qr_state_t *state = (qr_state_t *) vstate;
  int signum;

  /* perform QR decomposition of J */
  gsl_matrix_memcpy(state->QR, trust_state->J);
  gsl_linalg_QRPT_decomp(state->QR, state->tau_Q, state->perm,
                         &signum, state->workp);

  return GSL_SUCCESS;
}

static int
qr_presolve(const double mu, const void * vtrust_state, void * vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;

  state->mu = mu;

  (void) vtrust_state;

  return GSL_SUCCESS;
}

static int
qr_solve(const gsl_vector * f, gsl_vector *x,
         const void * vtrust_state, void *vstate)
{
  qr_state_t *state = (qr_state_t *) vstate;
  int status;

  if (state->mu == 0.0)
    {
      /*
       * compute Gauss-Newton direction by solving
       * J x = f
       * with an attempt to identify rank deficiency in J
       */
      size_t rank = gsl_linalg_QRPT_rank(state->QR, -1.0);
      status = gsl_linalg_QRPT_lssolve2(state->QR, state->tau_Q, state->perm,
                                        f, rank, x, state->residual);
    }
  else
    {
      /*
       * solve:
       *
       * [     J      ] x = [ f ]
       * [ sqrt(mu) D ]     [ 0 ]
       *
       * using QRPT factorization of J
       */

      const gsl_multifit_nlinear_trust_state *trust_state =
        (const gsl_multifit_nlinear_trust_state *) vtrust_state;
      double sqrt_mu = sqrt(state->mu);

      /* compute qtf = Q^T f */
      gsl_vector_memcpy(state->qtf, f);
      gsl_linalg_QR_QTvec(state->QR, state->tau_Q, state->qtf);

      status = qrsolv(state->QR, state->perm, sqrt_mu, trust_state->diag,
                      state->qtf, state->T, x, state->workn);
    }

  /* reverse step to go downhill */
  gsl_vector_scale(x, -1.0);

  return status;
}

static int
qr_rcond(double * rcond, void * vstate)
{
  int status;
  qr_state_t *state = (qr_state_t *) vstate;

  status = gsl_linalg_QRPT_rcond(state->QR, rcond, state->work3p);

  return status;
}

static const gsl_multifit_nlinear_solver qr_type =
{
  "qr",
  qr_alloc,
  qr_init,
  qr_presolve,
  qr_solve,
  qr_rcond,
  qr_free
};

const gsl_multifit_nlinear_solver *gsl_multifit_nlinear_solver_qr = &qr_type;


#include <petscksp.h>
#include "AFS.h"
#include "AFS_ctmc_petsc.h"
#include "im_clam.h"
#include "sparseExp.h"
#include "cs.h"
#include "AFS_ctmc.h"
#include "nlopt.h"
#include "adkGSL.h"
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_blas.h>
//Functions for jointly computing CTMC transitions and embedded chain

//this function fills two petsc matrices: 1) embedded Discrete MC trans Mat, 2) CTMC trans mat
// also fills the rates vector

void fillPetscTransMats(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2, Mat *DTMC, Mat *CTMC, gsl_vector *rates){
	int i, j,k, newnz, abcount=0;
	int N = S->nstates,abFlag;
	double  c0r[N],c1r[N],m0r[N],m1r[N],totR[N],rowSums[N], tmp;;

	gsl_vector_set_zero(rates);
	//set rates vector
	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1)  / theta1 ;
		if(theta2 == 0)
			c1r[i] = 0.0;
		else
			c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1)  / theta2;

		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];

		rowSums[i] = 0.0;
		if(S->states[i]->nalleles > 1  && c0r[i] >= 0 && c1r[i] >= 0 && totR[i] != 0.0)
			gsl_vector_set(rates,i,1.0/totR[i]);
		else
			gsl_vector_set(rates,i,0);
	}

	newnz=0;
	for(k=0;k<*nzCount;k++){
		i = dim1[k];
		j= dim2[k];
//		printf("i: %d, j:%d, movetype: %d\n",i,j,moveType[k]);
		if(S->states[i]->nalleles ==1 || S->states[j]->nalleles ==1 )abFlag=1;
	//	if(S->states[i]->nalleles ==1 )abFlag=1;
		else abFlag = 0;
		switch(moveType[k]){
			case 0: //coal pop0
			if(totR[i]==0)tmp=0;
			else tmp = c0r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)MatSetValue(*DTMC,i,j,tmp,INSERT_VALUES);
			break;
			case 1: //coal pop1
			if(totR[i]==0)tmp=0;
			else tmp = c1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)MatSetValue(*DTMC,i,j,tmp,INSERT_VALUES);
			break;
			case 2: //mig pop0
			if(totR[i]==0)tmp=0;
			else tmp = m0r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)MatSetValue(*DTMC,i,j,tmp,INSERT_VALUES);
			break;
			case 3: //mig pop1
			if(totR[i]==0)tmp=0;
			else tmp = m1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)MatSetValue(*DTMC,i,j,tmp,INSERT_VALUES);
			break;
			case -1:
			abcount+=1;
			break;
		}
	}
	newnz=*nzCount ;
	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
			MatSetValue(*CTMC,k,k,0.0-rowSums[k],INSERT_VALUES);
			newnz++;
		}
		MatSetValue(*DTMC,k,k,0.0,INSERT_VALUES);
	}
	*nzCount = newnz;
	/* Assemble the matrix */
  	MatAssemblyBegin(*CTMC,MAT_FINAL_ASSEMBLY);
  	MatAssemblyEnd(*CTMC,MAT_FINAL_ASSEMBLY);
  	MatAssemblyBegin(*DTMC,MAT_FINAL_ASSEMBLY);
  	MatAssemblyEnd(*DTMC,MAT_FINAL_ASSEMBLY);
}

//fillPetscCsparseTransMats--this function fills one petsc matrix with CTMC transmat and returns pointer to CSparse matrix for DTMC trans mat
// also fills the rates vector
//note that the DTMC matrix needs to be cleared before entry to this function and compressed afterward.

struct cs_di_sparse * fillPetscCsparseTransMats(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2, Mat *CTMC, gsl_vector *rates){
	int i, j,k, newnz, abcount=0;
	int N = S->nstates,abFlag;
	double  c0r[N],c1r[N],m0r[N],m1r[N],totR[N],rowSums[N], tmp;;
	struct cs_di_sparse *triplet, *tmat;

	//allocate new cs_sparse obj
	triplet = cs_spalloc(N, N, *nzCount + N , 1, 1); //alloc sparse mat with extra space for nonzero identity mats

	gsl_vector_set_zero(rates);
	//set rates vector
	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1)  / theta1 ;
		if(theta2 == 0)
			c1r[i] = 0.0;
		else
			c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1)  / theta2;
		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];

		rowSums[i] = 0.0;
		if(S->states[i]->nalleles > 1  && c0r[i] >= 0 && c1r[i] >= 0 && totR[i] != 0.0)
			gsl_vector_set(rates,i,1.0/totR[i]);
		else
			gsl_vector_set(rates,i,0);
	}

	newnz=0;
	for(k=0;k<*nzCount;k++){
		i = dim1[k];
		j= dim2[k];
//		printf("i: %d, j:%d, movetype: %d\n",i,j,moveType[k]);
		if(S->states[i]->nalleles ==1 || S->states[j]->nalleles ==1 )abFlag=1;
	//	if(S->states[i]->nalleles ==1 )abFlag=1;
		else abFlag = 0;
		switch(moveType[k]){
			case 0: //coal pop0
			if(totR[i]==0)tmp=0;
			else tmp = c0r[i] / totR[i] * topol[k];
		//	printf("here--- i:%d j:%d tmp*totR[i]:%f",i,j,tmp*totR[i]);
			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 1: //coal pop1
			if(totR[i]==0)tmp=0;
			else tmp = c1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 2: //mig pop0
			if(totR[i]==0)tmp=0;
			else tmp = m0r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 3: //mig pop1
			if(totR[i]==0)tmp=0;
			else tmp = m1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case -1:
			abcount+=1;
			break;
		}
	}
	newnz=*nzCount ;
	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
			MatSetValue(*CTMC,k,k,0.0-rowSums[k],INSERT_VALUES);
			newnz++;
		}
	}
	*nzCount = newnz;
	/* Assemble the matrix */
  	MatAssemblyBegin(*CTMC,MAT_FINAL_ASSEMBLY);
  	MatAssemblyEnd(*CTMC,MAT_FINAL_ASSEMBLY);
	tmat = cs_compress(triplet);
	cs_spfree(triplet);
	return(tmat);
}

//fillPetscCsparseTransMats_prealloc--this function fills one petsc matrix with CTMC transmat and returns pointer to CSparse matrix for DTMC trans mat
// also fills the rates vector
struct cs_di_sparse * fillPetscCsparseTransMats_prealloc(afsStateSpace *S, double *topol, int *moveType, int *nzCount,
	int *dim1, int *dim2, double theta1, double theta2, double mig1, double mig2,
	Mat *CTMC, gsl_vector *rates, struct cs_di_sparse *triplet){

	int i, j,k, newnz, abcount=0;
	int N = S->nstates,abFlag;
	double  *c0r,*c1r,*m0r,*m1r,*totR,*rowSums, tmp;
	struct cs_di_sparse *tmat;

	c0r = malloc(sizeof(double)*N);
	c1r = malloc(sizeof(double)*N);
	m0r = malloc(sizeof(double)*N);
	m1r = malloc(sizeof(double)*N);
	totR = malloc(sizeof(double)*N);
	rowSums = malloc(sizeof(double)*N);

	gsl_vector_set_zero(rates);
	//set rates vector
	for(i=0;i<N;i++){
		c0r[i] = S->states[i]->aCounts[0] * (S->states[i]->aCounts[0]-1)  / theta1 ;
		if(theta2 == 0)
			c1r[i] = 0.0;
		else
			c1r[i] = S->states[i]->aCounts[1] * (S->states[i]->aCounts[1]-1)  / theta2;
		m0r[i] = S->states[i]->aCounts[0] * mig1 ;
		m1r[i] = S->states[i]->aCounts[1] * mig2 ;
		totR[i] = c0r[i] + c1r[i] + m0r[i] + m1r[i];

		rowSums[i] = 0.0;
		if(S->states[i]->nalleles > 1  && c0r[i] >= 0 && c1r[i] >= 0 && totR[i] != 0.0)
			gsl_vector_set(rates,i,1.0/totR[i]);
		else
			gsl_vector_set(rates,i,0);
	}

	newnz=0;
	for(k=0;k<*nzCount;k++){
		i = dim1[k];
		j= dim2[k];
//		printf("i: %d, j:%d, movetype: %d\n",i,j,moveType[k]);
		if(S->states[i]->nalleles ==1 || S->states[j]->nalleles ==1 )abFlag=1;
	//	if(S->states[i]->nalleles ==1 )abFlag=1;
		else abFlag = 0;
		switch(moveType[k]){
			case 0: //coal pop0
			if(totR[i]==0)tmp=0;
			else tmp = c0r[i] / totR[i] * topol[k];
		//	printf("here--- i:%d j:%d tmp*totR[i]:%f",i,j,tmp*totR[i]);
			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 1: //coal pop1
			if(totR[i]==0)tmp=0;
			else tmp = c1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 2: //mig pop0
			if(totR[i]==0)tmp=0;
			else tmp = m0r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case 3: //mig pop1
			if(totR[i]==0)tmp=0;
			else tmp = m1r[i] / totR[i] * topol[k];

			MatSetValue(*CTMC,i,j,tmp*totR[i],INSERT_VALUES);
			rowSums[i] += tmp* totR[i];
			if(abFlag==0)cs_entry(triplet,i,j,tmp);
			break;
			case -1:
			abcount+=1;
			break;
		}
	}
	newnz=*nzCount ;
	//now add diagonal elements; topol pre-set to -1
	for(k=0;k<N;k++){
		if((rowSums[k]) != 0.0){
			MatSetValue(*CTMC,k,k,0.0-rowSums[k],INSERT_VALUES);
			newnz++;
		}
	}
	*nzCount = newnz;
	/* Assemble the matrix */
  	MatAssemblyBegin(*CTMC,MAT_FINAL_ASSEMBLY);
  	MatAssemblyEnd(*CTMC,MAT_FINAL_ASSEMBLY);
	tmat = cs_compress(triplet);

	free(c0r);
	free(c1r);
	free(m0r);
	free(m1r);
	free(totR);
	free(rowSums);
	return(tmat);
}

//calcLogAFS_IM -- returns the expects log AFS from an IM model
void calcLogAFS_IM(void * p){
	struct clam_lik_params * params = (struct clam_lik_params *) p;
 	PetscInt i,j, N = params->stateSpace->nstates;
  	cs *spMat, *mt, *tmpMat ;
  	double timeV, sum, thetaA, theta2, mig1, mig2;
  	PetscInt Na;
  	gsl_vector *tmpStates;
 	css *S ;
	csn *NN ;
	PetscInt n,Ntmp ;
	double *xx;
 	PetscInt        iStart,iEnd, *idx;
	const PetscInt *idx2;
	PetscScalar    *tmpArray;
	const PetscScalar *tmpArrayC;
	PetscErrorCode ierr;
	MFNConvergedReason reason;

 	//initialize some vectors
 	gsl_vector_set_zero(params->rates);
	MatZeroEntries(params->C);
	MatZeroEntries(params->C2);
 	tmpStates = gsl_vector_alloc(N);
	Ntmp=N;
	Na=params->Na;
 	//For straight MLE the paramVector takes the form [N2,NA,m1,m2,t]
 	theta2 = gsl_vector_get(params->paramVector,0);
 	thetaA = gsl_vector_get(params->paramVector,1);
 	mig1 = gsl_vector_get(params->paramVector,2);
 	mig2 = gsl_vector_get(params->paramVector,3);
 	timeV = gsl_vector_get(params->paramVector,4);
// 	printf("params-> %f %f %f %f %f\n",theta2,thetaA,mig1,mig2,timeV);
// 	printf("nnz %d nnzA%d \n",params->nnz,params->nnzA);

	//fill transMat
 //	tmpMat = fillPetscCsparseTransMats_prealloc(params->stateSpace, params->top, params->move, &params->nnz,
 //		params->dim1, params->dim2, 1, theta2, mig1, mig2,  &params->C, params->rates,params->triplet);

	tmpMat = fillPetscCsparseTransMats(params->stateSpace, params->top, params->move, &params->nnz,
	 		params->dim1, params->dim2, 1, theta2, mig1, mig2,  &params->C, params->rates);
	//using CSparse
	// S = (I-P)^-1
	//add negative ident
//	ident = cs_spalloc(N,N,N,1,1);
//	for(i=0;i<N;i++) cs_entry(ident,i,i,1);
//	eye = cs_compress(ident);
	spMat = cs_add(params->eye,tmpMat,1.0,-1.0);
	//cs_print_adk(spMat);
	mt = cs_transpose(spMat,1);
	//cs_print_adk(mt);

	n = mt->n ;
	S = cs_sqr (0, mt, 0) ;              /* ordering and symbolic analysis */
	NN = cs_lu (mt, S, 1e-12) ;                 /* numeric LU factorization */
	xx = cs_malloc (n, sizeof (double)) ;    /* get workspace */

	MatGetOwnershipRange(params->denseMat1,&iStart,&iEnd);
	PetscMalloc1(N,&idx);
	for(j=0;j<N;j++) idx[j]=j;

	////Compute Entire Inverse Mat
	for(j=iStart;j<iEnd;j++){
	//create unit array for solve
		for(i=0; i<N; i++)params->b[i] = 0.0;
		params->b[j]=1.0;
		//factor outside loop leads to ~40x speedup
		cs_ipvec (NN->pinv, params->b, xx, n) ;       /* x = b(p) */
		cs_lsolve (NN->L, xx) ;               /* x = L\x */
		cs_usolve (NN->U, xx) ;               /* x = U\x */
		cs_ipvec (S->q, xx, params->b, n) ;          /* b(q) = x */
	//	if(!rank)printf("row %d\n",j);
		MatSetValues(params->denseMat1,1,&j,N,idx,params->b,INSERT_VALUES);
	}
	MatAssemblyBegin(params->denseMat1,MAT_FINAL_ASSEMBLY);
	MatAssemblyEnd(params->denseMat1,MAT_FINAL_ASSEMBLY);
	//temporary clean up
	free(xx);
	cs_spfree(spMat);
	cs_spfree(mt);
//	cs_spfree(ident);
//	cs_spfree(eye);
	cs_spfree(tmpMat);
	cs_nfree(NN);
	cs_sfree(S);
	PetscFree(idx);

//	MatView(params->denseMat1,PETSC_VIEWER_STDOUT_WORLD);

	//broadcast the invMat[0,] as vector to each processor
	if(params->rank==0){
		MatGetRow(params->denseMat1,0,&N,&idx2,&tmpArrayC);
		for (j = 0; j < N; j++) params->b[j] = tmpArrayC[j];
		MatRestoreRow(params->denseMat1,0,&Ntmp,&idx2,&tmpArrayC);	//have to use Ntmp here as MatRestoreRow does something funky
	}
	MPI_Bcast(params->b,N,MPI_DOUBLE,0,PETSC_COMM_WORLD);

	//Get Island Time 0-INF Unnormal
	gsl_matrix_set_zero(params->expAFS);
	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS);
	// printf("////////////////////Island Time 0 - INF unnormalized\n");
	// for(i=0;i< (params->n1+1) ;i++){
	// 	for(j=0;j< (params->n2+1);j++){
	// 		printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
	// 	}
	// 	printf("\n");
	// }

	//Matrix Exponentiation to get state vector at time t
	//SLEPC Stuff ahead!
	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	Create the solver and set various options
	- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

	/*
	Set operator matrix, the function to compute, and other options
	*/
	//transpose
	MatTranspose(params->C,MAT_REUSE_MATRIX,&params->C);
	ierr = MFNSetOperator(params->mfn,params->C);CHKERRV(ierr);
	/*
	Set solver parameters at runtime
	*/

	ierr = MFNSetScaleFactor(params->mfn,timeV); CHKERRV(ierr);
	ierr = MFNSetFromOptions(params->mfn);CHKERRV(ierr);

	/* set v = e_1 */
	ierr = MatGetVecs(params->C,PETSC_NULL,&params->y);CHKERRV(ierr);
	ierr = MatGetVecs(params->C,&params->v, PETSC_NULL);CHKERRV(ierr);
	ierr = VecSetValue(params->v,0,1.0,INSERT_VALUES);CHKERRV(ierr);

	ierr = VecAssemblyBegin(params->v);CHKERRV(ierr);
	ierr = VecAssemblyEnd(params->v);CHKERRV(ierr);
	ierr = MFNSolve(params->mfn,params->v,params->y);CHKERRV(ierr);
	ierr = MFNGetConvergedReason(params->mfn,&reason);CHKERRV(ierr);
	if (reason!=MFN_CONVERGED_TOL) ierr = 1; CHKERRV(ierr);

	//retranspose for later use
	MatTranspose(params->C,MAT_REUSE_MATRIX,&params->C);

	//state vector at time t stored in y
	//scatter y and store in expoArray
	VecScatterCreateToAll(params->y,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->y,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->y,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < N; j++){
		params->expoArray[j] = tmpArray[j];
		//printf("%.5f\n",expoArray[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);
	VecDuplicate(params->y,&params->x);

	//VecView(y, PETSC_VIEWER_STDOUT_WORLD);
	MatMultTranspose(params->denseMat1,params->y,params->x);
	VecScatterCreateToAll(params->x,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->x,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->x,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < N; j++){
		params->b[j] = tmpArray[j];
		//printf("rank %d expoArray[%d]:%.5f\n",rank,j,expoArray[j]);
		//printf("rank %d b[%d]:%.5f\n",rank,j,b[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);

	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS2);
	//now subtract older AFS (t_t - t_INF) from total AFS (t_0 - t_INF)
	/////////////////
	///////

	gsl_matrix_sub(params->expAFS,params->expAFS2);

	//////////////////////
	//Final phase, collapse popns, reset rates
	//now collapse populations
	//state vector at time t stored in st[] for largerStateSpace; use reverseMap
	for(i=0;i<N;i++)params->st[i]=0.0;
	for(i=0;i<N;i++){params->st[params->map[i]]+=params->expoArray[i];
	//	printf("expoArray[%d]:%f\n",i,expoArray[i]);
	}
	for(i=0; i<params->reducedStateSpace->nstates; i++){
		VecSetValue(params->ancStateVec,i,params->st[params->reverseMap[i]],INSERT_VALUES );
	}

	//Re-Fill trans mats for Ancestral Params

	params->nnzA = coalMarkovChainTopologyMatrix_sparse(params->reducedStateSpace,params->topA,params->moveA, params->dim1A, params->dim2A);

	tmpMat=fillPetscCsparseTransMats(params->reducedStateSpace, params->topA, params->moveA, &params->nnzA,
		params->dim1A, params->dim2A, thetaA, 0, 0, 0, &params->C2, params->rates);
	//////////////////////
	//
	// S = (I-P)^-1
	//
	/////////
	//////
	//add negative ident
//	ident = cs_spalloc(Na,Na,Na,1,1);
//	for(i=0;i<Na;i++) cs_entry(ident,i,i,1);
//	eye = cs_compress(ident);
	spMat = cs_add(params->eyeAnc,tmpMat,1.0,-1.0);

	//cs_print_adk(spMat);
	mt = cs_transpose(spMat,1);
	//VecView(y,PETSC_VIEWER_STDOUT_WORLD);
	n = mt->n ;
	S = cs_sqr (0, mt, 0) ;              /* ordering and symbolic analysis */
	NN = cs_lu (mt, S, 1e-12) ;                 /* numeric LU factorization */
	xx = cs_malloc (n, sizeof (double)) ;    /* get workspace */
	MatGetOwnershipRange(params->denseMat2,&iStart,&iEnd);
//	printf("here\n");
	PetscFree(idx);
	PetscMalloc1(Na,&idx);
	for(j=0;j<Na;j++) idx[j]=j;
	////Compute Entire Inverse Mat
	for(j=iStart;j<iEnd;j++){
	//create unit array for solve
		for(i=0; i<Na; i++)params->b[i] = 0.0;
		params->b[j]=1.0;
		//factor outside loop leads to ~40x speedup
		cs_ipvec (NN->pinv, params->b, xx, n) ;       /* x = b(p) */
		cs_lsolve (NN->L, xx) ;               /* x = L\x */
		cs_usolve (NN->U, xx) ;               /* x = U\x */
		cs_ipvec (S->q, xx, params->b, n) ;          /* b(q) = x */
	//	if(!rank)printf("row %d\n",j);
		MatSetValues(params->denseMat2,1,&j,Na,idx,params->b,INSERT_VALUES);
	}
	MatAssemblyBegin(params->denseMat2,MAT_FINAL_ASSEMBLY);
	MatAssemblyEnd(params->denseMat2,MAT_FINAL_ASSEMBLY);
	free(xx);

	MatMultTranspose(params->denseMat2,params->ancStateVec,params->ancResVec);
	VecScatterCreateToAll(params->ancResVec,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->ancResVec,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->ancResVec,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);

	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < Na; j++){
		params->expoArray[j] = tmpArray[j];
		//printf("%.5f\n",expoArray[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);

	//get contribution starting at st
	gsl_matrix_set_zero(params->expAFS2);
	fillExpectedAFS_unnorm(params->reducedStateSpace, params->expoArray,params->rates,params->expAFS2);

	// printf("////////////////////--Ancestral bit\n");
	// for(i=0;i< (n1+1) ;i++){
	// 	for(j=0;j< (n2+1);j++){
	// 		printf("%.5f ",gsl_matrix_get(expAFS2,i,j));
	// 	}
	// 	printf("\n");
	// }

	//if(params->rank==0)printf("////////////////////normalized IM:\n");

	gsl_matrix_add(params->expAFS,params->expAFS2);
	sum = matrixSumDouble(params->expAFS);
	gsl_matrix_scale(params->expAFS, 1.0 / sum);
	params->meanTreeLength = sum;
	// for(i=0;i< (params->n1+1) ;i++){
	// 		for(j=0;j< (params->n2+1);j++){
	// 			if(params->rank==0)printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
	// 		}
	// 		if(params->rank==0)printf("\n");
	// 	}
	//

	//clean up
	PetscFree(idx);
	VecDestroy(&params->y);
	VecDestroy(&params->x);
	VecDestroy(&params->v);
	PetscFree(tmpArray);
	cs_spfree(spMat);
	cs_spfree(mt);
//	cs_spfree(ident);
//	cs_spfree(eye);
	cs_spfree(tmpMat);
	gsl_vector_free(tmpStates);
	cs_nfree(NN);
	cs_sfree(S);

}

//calcLogAFS_IM_allPETSC -- returns the expects log AFS from an IM model; only uses PETSC
void calcLogAFS_IM_allPETSC(void * p){
	struct clam_lik_params * params = (struct clam_lik_params *) p;
	PetscInt i,j, N = params->stateSpace->nstates;
	cs *spMat, *mt, *ident, *eye, *tmpMat ;
	double timeV, sum, thetaA, theta2, mig1, mig2;
	PetscInt Na;
	gsl_vector *tmpStates;
	css *S ;
	csn *NN ;
	PetscInt n,Ntmp ;
	double *xx;
	PetscInt        iStart,iEnd, *idx;
	const PetscInt *idx2;
	PetscScalar    *tmpArray;
	PetscScalar negOne = -1.0;
//	PetscScalar one = 1.0;

	const PetscScalar *tmpArrayC;
	PetscErrorCode ierr;
	IS perm,iperm;
	MatFactorInfo  info;
	MFNConvergedReason reason;

	//initialize some vectors
	gsl_vector_set_zero(params->rates);
	MatZeroEntries(params->C);
	MatZeroEntries(params->C2);
	MatZeroEntries(params->D);
	tmpStates = gsl_vector_alloc(N);
	Ntmp=N;
	Na=params->Na;

	//For straight MLE the paramVector takes the form [N2,NA,m1,m2,t]
	theta2 = gsl_vector_get(params->paramVector,0);
	thetaA = gsl_vector_get(params->paramVector,1);
	mig1 = gsl_vector_get(params->paramVector,2);
	mig2 = gsl_vector_get(params->paramVector,3);
	timeV = gsl_vector_get(params->paramVector,4);
// 	printf("params-> %f %f %f %f %f\n",theta2,thetaA,mig1,mig2,timeV);
// 	printf("nnz %d nnzA%d \n",params->nnz,params->nnzA);

	//fill transMat
	fillPetscTransMats(params->stateSpace, params->top, params->move, &params->nnz,
		params->dim1, params->dim2, 1, theta2, mig1, mig2, &params->D, &params->C, params->rates);

	//using CSparse
	// S = (I-P)^-1
	//subtract DTMC mat from identity

	MatZeroEntries(params->D_copy);
	MatCopy(params->ident,params->D_copy,DIFFERENT_NONZERO_PATTERN);
	MatAXPY(params->D_copy,negOne,params->D,DIFFERENT_NONZERO_PATTERN);
//	MatTranspose(params->D_copy,MAT_REUSE_MATRIX,&params->D);

	ierr = MatGetOrdering(params->D_copy,  MATORDERINGNATURAL,  &perm,  &iperm);

	ierr = MatFactorInfoInitialize(&info);
	ierr = MatGetFactor(params->D_copy,MATSOLVERSUPERLU_DIST,MAT_FACTOR_LU,&params->F);
	PetscInt icntl_7 = 5;
	//ierr = MatMumpsSetIcntl(params->F,7,icntl_7);
	info.fill = 5.0;
	ierr = MatLUFactorSymbolic(params->F,params->D_copy,perm,iperm,&info);
	ierr = MatLUFactorNumeric(params->F,params->D_copy,&info);

	//ierr = MatLUFactor(params->D_copy, perm, iperm, &info);

	////Compute Entire Inverse Mat
	ierr = MatMatSolve(params->F,params->denseIdent,params->denseMat1);

	//MatView(params->denseMat1,PETSC_VIEWER_STDOUT_SELF);

//	MatGetOwnershipRange(params->denseMat1,&iStart,&iEnd);
//	PetscMalloc1(N,&idx);
//	for(j=0;j<N;j++) idx[j]=j;

// 	for(j=0;j<N;j++){
// 	//create unit array for solve
// 		VecZeroEntries(params->bInv);
// 		VecZeroEntries(params->xInv);
// 		VecSetValue(params->bInv,j,one,INSERT_VALUES);
// 		VecAssemblyBegin(params->bInv);
// 		VecAssemblyEnd(params->bInv);
//
// 		KSPSolve(params->ksp,params->bInv,params->xInv);
// 		VecGetValues(params->x,N,idx,hold);
// 		MatSetValues(params->denseMat1,1,&j,N,idx,hold,INSERT_VALUES);
// 	}
// 	MatAssemblyBegin(params->denseMat1,MAT_FINAL_ASSEMBLY);
// 	MatAssemblyEnd(params->denseMat1,MAT_FINAL_ASSEMBLY);
// 	//temporary clean up
// //	free(xx);
// //	cs_spfree(spMat);
// //	cs_spfree(mt);
// //	cs_spfree(ident);
// //	cs_spfree(eye);
// //	cs_spfree(tmpMat);
// //	cs_nfree(NN);
// //	cs_sfree(S);
// 	PetscFree(idx);
// 	KSPDestroy(&params->ksp);
//	MatView(params->denseMat1,PETSC_VIEWER_STDOUT_WORLD);

	MatDestroy(&params->F);
	//broadcast the invMat[0,] as vector to each processor
	if(params->rank==0){
		MatGetRow(params->denseMat1,0,&N,&idx2,&tmpArrayC);
		for (j = 0; j < N; j++) params->b[j] = tmpArrayC[j];
		MatRestoreRow(params->denseMat1,0,&Ntmp,&idx2,&tmpArrayC);	//have to use Ntmp here as MatRestoreRow does something funky
	}
	MPI_Bcast(params->b,N,MPI_DOUBLE,0,PETSC_COMM_WORLD);

	//Get Island Time 0-INF Unnormal
	gsl_matrix_set_zero(params->expAFS);
	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS);
	// printf("////////////////////Island Time 0 - INF unnormalized\n");
	// for(i=0;i< (params->n1+1) ;i++){
	// 	for(j=0;j< (params->n2+1);j++){
	// 		printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
	// 	}
	// 	printf("\n");
	// }

	//Matrix Exponentiation to get state vector at time t
	//SLEPC Stuff ahead!
	/* - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
	Create the solver and set various options
	- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - */

	/*
	Set operator matrix, the function to compute, and other options
	*/
	//transpose
	MatTranspose(params->C,MAT_REUSE_MATRIX,&params->C);
	ierr = MFNSetOperator(params->mfn,params->C);CHKERRV(ierr);
	/*
	Set solver parameters at runtime
	*/

	ierr = MFNSetScaleFactor(params->mfn,timeV); CHKERRV(ierr);
	ierr = MFNSetFromOptions(params->mfn);CHKERRV(ierr);

	/* set v = e_1 */
	ierr = MatGetVecs(params->C,PETSC_NULL,&params->y);CHKERRV(ierr);
	ierr = MatGetVecs(params->C,&params->v, PETSC_NULL);CHKERRV(ierr);
	ierr = VecSetValue(params->v,0,1.0,INSERT_VALUES);CHKERRV(ierr);

	ierr = VecAssemblyBegin(params->v);CHKERRV(ierr);
	ierr = VecAssemblyEnd(params->v);CHKERRV(ierr);
	ierr = MFNSolve(params->mfn,params->v,params->y);CHKERRV(ierr);
	ierr = MFNGetConvergedReason(params->mfn,&reason);CHKERRV(ierr);
	if (reason!=MFN_CONVERGED_TOL) ierr = 1; CHKERRV(ierr);

	//retranspose for later use
	MatTranspose(params->C,MAT_REUSE_MATRIX,&params->C);

	//state vector at time t stored in y
	//scatter y and store in expoArray
	VecScatterCreateToAll(params->y,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->y,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->y,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < N; j++){
		params->expoArray[j] = tmpArray[j];
		//printf("%.5f\n",expoArray[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);
	VecDuplicate(params->y,&params->x);

	//VecView(y, PETSC_VIEWER_STDOUT_WORLD);
	MatMultTranspose(params->denseMat1,params->y,params->x);
	VecScatterCreateToAll(params->x,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->x,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->x,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < N; j++){
		params->b[j] = tmpArray[j];
		//printf("rank %d expoArray[%d]:%.5f\n",rank,j,expoArray[j]);
		//printf("rank %d b[%d]:%.5f\n",rank,j,b[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);

	fillExpectedAFS_unnorm(params->stateSpace, params->b,params->rates,params->expAFS2);
	//now subtract older AFS (t_t - t_INF) from total AFS (t_0 - t_INF)
	/////////////////
	///////

	gsl_matrix_sub(params->expAFS,params->expAFS2);

	//////////////////////
	//Final phase, collapse popns, reset rates
	//now collapse populations
	//state vector at time t stored in st[] for largerStateSpace; use reverseMap
	for(i=0;i<N;i++)params->st[i]=0.0;
	for(i=0;i<N;i++){params->st[params->map[i]]+=params->expoArray[i];
	//	printf("expoArray[%d]:%f\n",i,expoArray[i]);
	}
	for(i=0; i<params->reducedStateSpace->nstates; i++){
		VecSetValue(params->ancStateVec,i,params->st[params->reverseMap[i]],INSERT_VALUES );
	}

	//Re-Fill trans mats for Ancestral Params

	params->nnzA = coalMarkovChainTopologyMatrix_sparse(params->reducedStateSpace,params->topA,params->moveA, params->dim1A, params->dim2A);

	tmpMat=fillPetscCsparseTransMats(params->reducedStateSpace, params->topA, params->moveA, &params->nnzA,
		params->dim1A, params->dim2A, thetaA, 0, 0, 0, &params->C2, params->rates);
	//////////////////////
	//
	// S = (I-P)^-1
	//
	/////////
	//////
	//add negative ident
	ident = cs_spalloc(Na,Na,Na,1,1);
	for(i=0;i<Na;i++) cs_entry(ident,i,i,1);
	eye = cs_compress(ident);
	spMat = cs_add(eye,tmpMat,1.0,-1.0);

	//cs_print_adk(spMat);
	mt = cs_transpose(spMat,1);
	//VecView(y,PETSC_VIEWER_STDOUT_WORLD);
	n = mt->n ;
	S = cs_sqr (0, mt, 0) ;              /* ordering and symbolic analysis */
	NN = cs_lu (mt, S, 1e-12) ;                 /* numeric LU factorization */
	xx = cs_malloc (n, sizeof (double)) ;    /* get workspace */
	MatGetOwnershipRange(params->denseMat2,&iStart,&iEnd);
//	printf("here\n");
//	PetscFree(idx);
	PetscMalloc1(Na,&idx);
	for(j=0;j<Na;j++) idx[j]=j;
	////Compute Entire Inverse Mat
	for(j=iStart;j<iEnd;j++){
	//create unit array for solve
		for(i=0; i<Na; i++)params->b[i] = 0.0;
		params->b[j]=1.0;
		//factor outside loop leads to ~40x speedup
		cs_ipvec (NN->pinv, params->b, xx, n) ;       /* x = b(p) */
		cs_lsolve (NN->L, xx) ;               /* x = L\x */
		cs_usolve (NN->U, xx) ;               /* x = U\x */
		cs_ipvec (S->q, xx, params->b, n) ;          /* b(q) = x */
	//	if(!rank)printf("row %d\n",j);
		MatSetValues(params->denseMat2,1,&j,Na,idx,params->b,INSERT_VALUES);
	}
	MatAssemblyBegin(params->denseMat2,MAT_FINAL_ASSEMBLY);
	MatAssemblyEnd(params->denseMat2,MAT_FINAL_ASSEMBLY);
	free(xx);

	MatMultTranspose(params->denseMat2,params->ancStateVec,params->ancResVec);
	VecScatterCreateToAll(params->ancResVec,&params->ctx,&params->v_seq);
	VecScatterBegin(params->ctx,params->ancResVec,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);
	VecScatterEnd(params->ctx,params->ancResVec,params->v_seq,INSERT_VALUES,SCATTER_FORWARD);

	VecGetArray(params->v_seq,&tmpArray);
	for (j = 0; j < Na; j++){
		params->expoArray[j] = tmpArray[j];
		//printf("%.5f\n",expoArray[j]);
	}

	VecRestoreArray(params->v_seq,&tmpArray);
	VecScatterDestroy(&params->ctx);
	VecDestroy(&params->v_seq);

	//get contribution starting at st
	gsl_matrix_set_zero(params->expAFS2);
	fillExpectedAFS_unnorm(params->reducedStateSpace, params->expoArray,params->rates,params->expAFS2);

	// printf("////////////////////--Ancestral bit\n");
	// for(i=0;i< (n1+1) ;i++){
	// 	for(j=0;j< (n2+1);j++){
	// 		printf("%.5f ",gsl_matrix_get(expAFS2,i,j));
	// 	}
	// 	printf("\n");
	// }

	//if(params->rank==0)printf("////////////////////normalized IM:\n");

	gsl_matrix_add(params->expAFS,params->expAFS2);
	sum = matrixSumDouble(params->expAFS);
	gsl_matrix_scale(params->expAFS, 1.0 / sum);
	// for(i=0;i< (params->n1+1) ;i++){
	// 		for(j=0;j< (params->n2+1);j++){
	// 			if(params->rank==0)printf("%.5f ",gsl_matrix_get(params->expAFS,i,j));
	// 		}
	// 		if(params->rank==0)printf("\n");
	// 	}
	//

	//clean up
	PetscFree(idx);
	VecDestroy(&params->y);
	VecDestroy(&params->x);
	VecDestroy(&params->v);
	ierr = ISDestroy(&perm);
  	ierr = ISDestroy(&iperm);
	PetscFree(tmpArray);
	cs_spfree(spMat);
	cs_spfree(mt);
	cs_spfree(ident);
	cs_spfree(eye);
	cs_spfree(tmpMat);
	gsl_vector_free(tmpStates);
	cs_nfree(NN);
	cs_sfree(S);

}
double calcLikNLOpt(unsigned n, const double *point, double *gradients, void *p){
	int i,j;
	struct clam_lik_params * params = (struct clam_lik_params *) p;
	double localNNZ = params->nnz;
	double output = 666.0;
	double lik=0.0;
	double x[5];

	for(i = 0;i<5;i++) x[i] = point[i];
	MPI_Bcast(x,5,MPI_DOUBLE,0,PETSC_COMM_WORLD);

	for(i = 0;i<5;i++) gsl_vector_set(params->paramVector, i, x[i]);

	//fill in the expAFS table
	calcLogAFS_IM(p);
	params->nnz = localNNZ;
	//compute lik
	for(i=0;i<params->obsData->size1;i++){
		for(j=0;j<params->obsData->size2;j++){
			if (gsl_matrix_get(params->expAFS,i,j) > 0.0) //corners of AFS are zero prob
				lik += gsl_matrix_get(params->obsData,i,j) * (log(gsl_matrix_get(params->expAFS,i,j)));
		}
	}
	output = -1.0* lik;
	MPI_Bcast(&output,1,MPI_DOUBLE,0,PETSC_COMM_WORLD);
	if(params->rank == 0 && vbse){
		for(i = 0;i<5;i++) printf("x[%d]: %lf\t",i,x[i]);
		printf("lik: %lf\n",lik);
	}
	params->fEvals += 1;
//	gsl_matrix_prettyPrint(params->expAFS);
	return(output);
}

double calcLikNLOpt_gradients(unsigned n, const double *point, double *gradients, void *p){
	int i,j;
	//struct clam_lik_params * params = (struct clam_lik_params *) p;
	//double localNNZ = params->nnz;
	double output = 666.0;
	double lik=0.0;
	double newF;
	double x[5];
	double temp[5];
	double epsilon=1e-5;

	for(i = 0;i<5;i++) x[i] = point[i];
	MPI_Bcast(x,5,MPI_DOUBLE,0,PETSC_COMM_WORLD);

	lik = calcLikNLOpt(n, point, NULL, p);
	if (gradients != NULL){
		for(i=0; i<n ;i++){
			for( j=0; j< n; j++)
				temp[j] = x[j];

			temp[i] +=  epsilon;
			newF = calcLikNLOpt(n,temp,NULL,p);
			gradients[i] = (newF -  lik)/epsilon;
		}
	}

	output = lik;
	MPI_Bcast(&output,1,MPI_DOUBLE,0,PETSC_COMM_WORLD);
//	gsl_matrix_prettyPrint(params->expAFS);
	return(output);
}

//maximizeLik-- maximizes the logLikFunction function using constraints via
// nlOpt library

void maximizeLikNLOpt(double *lik, void *p, double *mle){
	//struct clam_lik_params * params = (struct clam_lik_params *) p;
	int np = 5;
	int err;
	//double x[5] = {1.0,1.0,1.0,1.0,1.5};
	double minimum=666.6;
	nlopt_opt opt;

	opt = nlopt_create(NLOPT_LD_LBFGS, np);
	//define constraints

	nlopt_set_lower_bounds(opt, lowerBounds);
	nlopt_set_upper_bounds(opt, upperBounds);
	nlopt_set_min_objective(opt, calcLikNLOpt_gradients, (void *) p);

	nlopt_set_xtol_rel(opt, 1e-6);
	//set the seed for deterministic sequence in COBYLA alg
	//nlopt_srand(1232333);
	err = nlopt_optimize(opt, mle, &minimum);
	if (err < 0) {
	    fprintf(stderr,"nlopt failed with code %d!\n",err);
	}
//	MPI_Barrier(MPI_COMM_WORLD);
	*lik=minimum;
	nlopt_destroy(opt);

}

//maximizeLikNLOpt_twoStage-- begins with coarse global, then goes local

void maximizeLikNLOpt_twoStage(double *lik, void *p, double *mle){
	//struct clam_lik_params * params = (struct clam_lik_params *) p;
	int np = 5;
	int err;
	//double x[5] = {1.0,1.0,1.0,1.0,1.5};
	double minimum=666.6;
	nlopt_opt opt;

	opt = nlopt_create(NLOPT_GN_DIRECT_L_RAND, np);

	nlopt_set_lower_bounds(opt, lowerBounds);
	nlopt_set_upper_bounds(opt, upperBounds);
	nlopt_set_min_objective(opt, calcLikNLOpt_gradients, (void *) p);

	//nlopt_set_xtol_rel(opt, 0.25);
	//set the seed for deterministic sequence in COBYLA alg
	//nlopt_srand(1232333);
	err = nlopt_optimize(opt, mle, &minimum);
	if (err < 0) {
	    fprintf(stderr,"nlopt failed with code %d!\n",err);
	}
//	MPI_Barrier(MPI_COMM_WORLD);
	*lik=minimum;
	nlopt_destroy(opt);

}

//maximizeLikNLOpt_MLSL-- multi-level restarts

void maximizeLikNLOpt_MLSL(double *lik, void *p, double *mle){
	int np = 5;
	int err;
	//double x[5] = {1.0,1.0,1.0,1.0,1.5};
	double minimum=666.6;
	nlopt_opt opt, l_opt;

	opt = nlopt_create(NLOPT_G_MLSL, np);

	nlopt_set_lower_bounds(opt, lowerBounds);
	nlopt_set_upper_bounds(opt, upperBounds);
	nlopt_set_min_objective(opt, calcLikNLOpt_gradients, (void *) p);

	l_opt = nlopt_create(NLOPT_LD_LBFGS, np);
	nlopt_set_lower_bounds(l_opt, lowerBounds);
	nlopt_set_upper_bounds(l_opt, upperBounds);
	nlopt_set_min_objective(l_opt, calcLikNLOpt_gradients, (void *) p);
	//define constraints

	nlopt_set_local_optimizer(opt, l_opt);

	//nlopt_set_xtol_rel(opt, 0.25);
	//set the seed for deterministic sequence in COBYLA alg
	//nlopt_srand(1232333);
	err = nlopt_optimize(opt, mle, &minimum);
	if (err < 0) {
	    fprintf(stderr,"nlopt failed with code %d!\n",err);
	}
//	MPI_Barrier(MPI_COMM_WORLD);
	*lik=minimum;
	nlopt_destroy(l_opt);
	nlopt_destroy(opt);

}

///////////////// Hessian stuff ///////////////////

double hessianMatrix_element(double lik, double *mle, int i, int j, double hi, double hj, void *p){
	//struct clam_lik_params * params = (struct clam_lik_params *) p;
	double Pi0, Pj0,output,fpp,fpm,fmp,fmm;

	Pi0 = mle[i]; Pj0 = mle[j];

	//printf("mle: %f\n",lik);
	if(i==j){
		mle[i] = Pi0 + hi;
		fpp = calcLikNLOpt(5,mle,NULL,p);
		mle[i] = Pj0 - hi;
		fmm = calcLikNLOpt(5,mle,NULL,p);
		output = ((fpp - lik) + (fmm - lik))/(hi*hi);
	}
	else{
		// f(xi + hi, xj + h)
		mle[i] = Pi0 + hi;
		mle[j] = Pj0 + hj;
		fpp = calcLikNLOpt(5,mle,NULL,p);

	        // f(xi + hi, xj - hj)
		mle[i] = Pi0 + hi;
		mle[j] = Pj0 - hj;
		fpm = calcLikNLOpt(5,mle,NULL,p);

	        // f(xi - hi, xj + hj)
		mle[i] = Pi0 - hi;
		mle[j] = Pj0 + hj;
		fmp = calcLikNLOpt(5,mle,NULL,p);

	        // f(xi - hi, xj - hj)
		mle[i] = Pi0 - hi;
		mle[j] = Pj0 - hj;
		fmm = calcLikNLOpt(5,mle,NULL,p);

		output = (fpp - fpm - fmp + fmm)/(4 * hi * hj);
	}
	mle[i]=Pi0; mle[j]=Pj0;
	return output;
}

gsl_matrix *hessian(double *mle, double lik, void *p){
	int i, j;
	double eps;
	gsl_matrix *H;

	H = gsl_matrix_alloc(5,5);
	eps = 0.01;
	for(i=0;i<5;i++){
		for (j=i;j<5;j++){
			gsl_matrix_set(H,i,j,hessianMatrix_element(lik, mle, i, j, eps, eps, p));
			gsl_matrix_set(H,j,i,gsl_matrix_get(H,i,j));
		}
	}
	return(H);
}

gsl_matrix *getFisherInfoMatrix(double *mle, double lik, void *p){
	gsl_matrix *H;
	gsl_matrix *fi;
	gsl_permutation *perm = gsl_permutation_alloc(5);
	int s;

	H = hessian(mle,lik,p);
	//gsl_matrix_prettyPrint(H);printf("\n");
	fi = gsl_matrix_alloc(5,5);
	gsl_matrix_scale(H,-1.0);
	gsl_linalg_LU_decomp (H, perm, &s);
	gsl_linalg_LU_invert (H, perm, fi);
	//gsl_matrix_prettyPrint(fi);printf("\n");
	return(fi);
}

gsl_matrix *getGodambeInfoMatrix(double *mle, double lik, void *p){
	gsl_matrix *H;
	gsl_matrix *J,*Jtemp, *Jinv, *origData, *boot;
	gsl_vector *cU, *grad_temp;
	struct clam_lik_params * params = (struct clam_lik_params *) p;
	gsl_permutation *perm = gsl_permutation_alloc(5);
	int s,i, nBoot;

	origData = params->obsData;
	nBoot = 100;

	H = hessian(mle,lik,p);
	gsl_matrix_scale(H,-1.0);
	//gsl_matrix_prettyPrint(H);printf("\n");
	J = gsl_matrix_alloc(5,5);
	Jinv = gsl_matrix_alloc(5,5);
	Jtemp = gsl_matrix_alloc(5,5);
	boot = gsl_matrix_alloc(origData->size1,origData->size2);

	gsl_matrix_set_zero(J);
	gsl_matrix_set_zero(Jinv);
	gsl_matrix_set_zero(Jtemp);
	cU = gsl_vector_alloc(5);
	gsl_vector_set_zero(cU);

	//do bootstraps, estimate J
	for(i=0;i<nBoot;i++){
		gsl_matrix_bootstrap(origData, boot, params->rng);
		params->obsData = boot;
		grad_temp = getGradient(mle,p);
		gsl_vector_outer_product(grad_temp,grad_temp,Jtemp);
		gsl_matrix_add(J,Jtemp);
		gsl_vector_add(cU,grad_temp);
		//gsl_matrix_prettyPrint(Jtemp);
		gsl_vector_free(grad_temp);
	}
	gsl_vector_scale(cU,1.0/(float)nBoot);
	gsl_matrix_scale(J,1.0/(float)nBoot);
	//printf("cU:\n");
	//gsl_vector_fprintf(stdout,cU,"%f");
	//printf("J:\n");
	//gsl_matrix_prettyPrint(J);printf("\n");
	//G = H*J^-1*H
	gsl_linalg_LU_decomp (J, perm, &s);
	gsl_linalg_LU_invert (J, perm, Jinv);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, H, Jinv, 0.0, Jtemp);
	gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, Jtemp, H, 0.0, J);
	//printf("godambe:\n");
	//gsl_matrix_prettyPrint(J);printf("\n");
	//need to invert this to get the Godambe information matrix
	gsl_linalg_LU_decomp (J, perm, &s);
	gsl_linalg_LU_invert (J, perm, Jinv);
	return(Jinv);
}

gsl_vector *getGradient(double *mle, void *p){
	int i, j;
	double eps, epsVals[5], paramTemp[5], fp, fm;
	gsl_vector *gradient;

	gradient = gsl_vector_alloc(5);
	eps = 0.01;
	for(i=0;i<5;i++){
		if( mle[i] != 0){
			epsVals[i] = mle[i] * eps;
			//printf("eps[%d]=%f\n",i,epsVals[i]);
		}
		else{
			epsVals[i] = eps;
		}
		paramTemp[i] = mle[i];
	}
	for(i=0;i<5;i++){
		for(j=0;j<5;j++) paramTemp[j] = mle[j];
		if( mle[i] != 0){
			paramTemp[i] = mle[i] + epsVals[i];
			fp = calcLikNLOpt(5,paramTemp,NULL,p);
			paramTemp[i] = mle[i] - epsVals[i];
			fm = calcLikNLOpt(5,paramTemp,NULL,p);
			gsl_vector_set(gradient,i,(fp-fm)/(2*epsVals[i]));
		}
		else{
			paramTemp[i] = mle[i] + epsVals[i];
			fp = calcLikNLOpt(5,paramTemp,NULL,p);
			paramTemp[i] = mle[i];
			fm = calcLikNLOpt(5,paramTemp,NULL,p);
			gsl_vector_set(gradient,i,(fp-fm)/epsVals[i]);
		}
	}
	return(gradient);
}