// Copyright (c) 2017, Lawrence Livermore National Security, LLC. Produced at
// the Lawrence Livermore National Laboratory. LLNL-CODE-734707. All Rights
// reserved. See files LICENSE and NOTICE for details.
//
// This file is part of CEED, a collection of benchmarks, miniapps, software
// libraries and APIs for efficient high-order finite element and spectral
// element discretizations for exascale applications. For more information and
// source code availability see http://github.com/ceed.
//
// The CEED research is supported by the Exascale Computing Project 17-SC-20-SC,
// a collaborative effort of two U.S. Department of Energy organizations (Office
// of Science and the National Nuclear Security Administration) responsible for
// the planning and preparation of a capable exascale ecosystem, including
// software, applications, hardware, advanced system engineering and early
// testbed platforms, in support of the nation's exascale computing imperative.

#ifndef setup_h
#define setup_h

#include <stdbool.h>
#include <string.h>

#include <petsc.h>
#include <petscdmplex.h>
#include <petscksp.h>
#include <petscfe.h>

#include <ceed.h>

#if PETSC_VERSION_LT(3,14,0)
#  define DMAddBoundary(a,b,c,d,e,f,g,h,i,j,k,l) DMAddBoundary(a,b,c,d,e,f,g,h,j,k,l)
#endif

#ifndef PHYSICS_STRUCT
#define PHYSICS_STRUCT
typedef struct Physics_private *Physics;
struct Physics_private {
  CeedScalar   nu;      // Poisson's ratio
  CeedScalar   E;       // Young's Modulus
};
#endif

// -----------------------------------------------------------------------------
// Command Line Options
// -----------------------------------------------------------------------------
// Problem options
typedef enum {
  ELAS_LIN = 0, ELAS_HYPER_SS = 1, ELAS_HYPER_FS = 2
} problemType;
static const char *const problemTypes[] = {"linElas",
                                           "hyperSS",
                                           "hyperFS",
                                           "problemType","ELAS_",0
                                          };
static const char *const problemTypesForDisp[] = {"Linear elasticity",
                                                  "Hyper elasticity small strain",
                                                  "Hyper elasticity finite strain"
                                                 };

// Forcing function options
typedef enum {
  FORCE_NONE = 0, FORCE_CONST = 1, FORCE_MMS = 2
} forcingType;
static const char *const forcingTypes[] = {"none",
                                           "constant",
                                           "mms",
                                           "forcingType","FORCE_",0
                                          };
static const char *const forcingTypesForDisp[] = {"None",
                                                  "Constant",
                                                  "Manufactured solution"
                                                 };

// Multigrid options
typedef enum {
  MULTIGRID_LOGARITHMIC = 0, MULTIGRID_UNIFORM = 1, MULTIGRID_NONE = 2
} multigridType;
static const char *const multigridTypes [] = {"logarithmic",
                                              "uniform",
                                              "none",
                                              "multigridType","MULTIGRID",0
                                             };
static const char *const multigridTypesForDisp[] = {"P-multigrid, logarithmic coarsening",
                                                    "P-multigrind, uniform coarsening",
                                                    "No multigrid"
                                                   };

typedef PetscErrorCode BCFunc(PetscInt, PetscReal, const PetscReal *, PetscInt,
                              PetscScalar *, void *);
// Note: These variables should be updated if additional boundary conditions
//         are added to boundary.c.
BCFunc BCMMS, BCZero, BCClamp;

// MemType Options
static const char *const memTypes[] = {"host","device","memType",
                                       "CEED_MEM_",0
                                      };

// -----------------------------------------------------------------------------
// Structs
// -----------------------------------------------------------------------------
// Units
typedef struct Units_private *Units;
struct Units_private {
  // Fundamental units
  PetscScalar meter;
  PetscScalar kilogram;
  PetscScalar second;
  // Derived unit
  PetscScalar Pascal;
};

// Application context from user command line options
typedef struct AppCtx_private *AppCtx;
struct AppCtx_private {
  char          ceedResource[PETSC_MAX_PATH_LEN];     // libCEED backend
  char          meshFile[PETSC_MAX_PATH_LEN];         // exodusII mesh file
  PetscBool     testMode;
  PetscBool     viewSoln;
  PetscBool     viewFinalSoln;
  problemType   problemChoice;
  forcingType   forcingChoice;
  multigridType multigridChoice;
  PetscScalar   nuSmoother;
  PetscInt      degree;
  PetscInt      qextra;
  PetscInt      numLevels;
  PetscInt      *levelDegrees;
  PetscInt      numIncrements;                        // Number of steps
  PetscInt      bcClampCount;
  PetscInt      bcClampFaces[16];
  PetscScalar   bcClampMax[16][7];
  PetscInt      bcTractionCount;
  PetscInt      bcTractionFaces[16];
  PetscScalar   bcTractionVector[16][3];
  PetscScalar   forcingVector[3];
  PetscBool     petscHaveCuda, setMemTypeRequest;
  CeedMemType   memTypeRequested;
};

// Problem specific data
// *INDENT-OFF*
typedef struct {
  CeedInt           qdatasize;
  CeedQFunctionUser setupgeo, apply, jacob, energy, diagnostic;
  const char        *setupgeofname, *applyfname, *jacobfname, *energyfname,
                    *diagnosticfname;
  CeedQuadMode      qmode;
} problemData;
// *INDENT-ON*

// Data specific to each problem option
extern problemData problemOptions[3];

// Forcing function data
typedef struct {
  CeedQFunctionUser setupforcing;
  const char        *setupforcingfname;
} forcingData;

extern forcingData forcingOptions[3];

// Data for PETSc Matshell
typedef struct UserMult_private *UserMult;
struct UserMult_private {
  MPI_Comm        comm;
  DM              dm;
  Vec             Xloc, Yloc, NBCs;
  CeedVector      Xceed, Yceed;
  CeedOperator    op;
  CeedQFunction   qf;
  Ceed            ceed;
  PetscScalar     loadIncrement;
  CeedQFunctionContext ctxPhys, ctxPhysSmoother;
  CeedMemType     memType;
  int (*VecGetArray)(Vec, PetscScalar **);
  int (*VecGetArrayRead)(Vec, const PetscScalar **);
  int (*VecRestoreArray)(Vec, PetscScalar **);
  int (*VecRestoreArrayRead)(Vec, const PetscScalar **);
};

// Data for Jacobian setup routine
typedef struct FormJacobCtx_private *FormJacobCtx;
struct FormJacobCtx_private {
  UserMult     *jacobCtx;
  PetscInt     numLevels;
  SNES         snesCoarse;
  Mat          *jacobMat, jacobMatCoarse;
  Vec          Ucoarse;
};

// Data for PETSc Prolongation/Restriction Matshell
typedef struct UserMultProlongRestr_private *UserMultProlongRestr;
struct UserMultProlongRestr_private {
  MPI_Comm     comm;
  DM           dmC, dmF;
  Vec          locVecC, locVecF;
  CeedVector   ceedVecC, ceedVecF;
  CeedOperator opProlong, opRestrict;
  Ceed         ceed;
  CeedMemType   memType;
  int (*VecGetArray)(Vec, PetscScalar **);
  int (*VecGetArrayRead)(Vec, const PetscScalar **);
  int (*VecRestoreArray)(Vec, PetscScalar **);
  int (*VecRestoreArrayRead)(Vec, const PetscScalar **);
};

// libCEED data struct for level
typedef struct CeedData_private *CeedData;
struct CeedData_private {
  Ceed                ceed;
  CeedBasis           basisx, basisu, basisCtoF, basisEnergy, basisDiagnostic;
  CeedElemRestriction Erestrictx, Erestrictu, Erestrictqdi,
                      ErestrictGradui, ErestrictEnergy, ErestrictDiagnostic,
                      ErestrictqdDiagnostici;
  CeedQFunction       qfApply, qfJacob, qfEnergy, qfDiagnostic;
  CeedOperator        opApply, opJacob, opRestrict, opProlong, opEnergy,
                      opDiagnostic;
  CeedVector          qdata, qdataDiagnostic, gradu, xceed, yceed, truesoln;
};

// -----------------------------------------------------------------------------
// Process command line options
// -----------------------------------------------------------------------------
// Process general command line options
PetscErrorCode ProcessCommandLineOptions(MPI_Comm comm, AppCtx appCtx);

// Process physics options
PetscErrorCode ProcessPhysics(MPI_Comm comm, Physics phys, Units units);

// -----------------------------------------------------------------------------
// Setup DM
// -----------------------------------------------------------------------------
PetscErrorCode CreateBCLabel(DM dm, const char name[]);

// Create FE by degree
PetscErrorCode PetscFECreateByDegree(DM dm, PetscInt dim, PetscInt Nc,
                                     PetscBool isSimplex, const char prefix[],
                                     PetscInt order, PetscFE *fem);

// Read mesh and distribute DM in parallel
PetscErrorCode CreateDistributedDM(MPI_Comm comm, AppCtx appCtx, DM *dm);

// Setup DM with FE space of appropriate degree
PetscErrorCode SetupDMByDegree(DM dm, AppCtx appCtx, PetscInt order,
                               PetscBool boundary, PetscInt ncompu);

// -----------------------------------------------------------------------------
// libCEED Functions
// -----------------------------------------------------------------------------
// Destroy libCEED objects
PetscErrorCode CeedDataDestroy(CeedInt level, CeedData data);

// Utility function - essential BC dofs are encoded in closure indices as -(i+1)
PetscInt Involute(PetscInt i);

// Utility function to create local CEED restriction from DMPlex
PetscErrorCode CreateRestrictionFromPlex(Ceed ceed, DM dm, CeedInt P,
    CeedInt height, DMLabel domainLabel, CeedInt value,
    CeedElemRestriction *Erestrict);

// Utility function to get Ceed Restriction for each domain
PetscErrorCode GetRestrictionForDomain(Ceed ceed, DM dm, CeedInt height,
                                       DMLabel domainLabel, PetscInt value, CeedInt P, CeedInt Q, CeedInt qdatasize,
                                       CeedElemRestriction *restrictq, CeedElemRestriction *restrictx,
                                       CeedElemRestriction *restrictqdi);

// Set up libCEED for a given degree
PetscErrorCode SetupLibceedFineLevel(DM dm, DM dmEnergy, DM dmDiagnostic,
                                     Ceed ceed, AppCtx appCtx,
                                     CeedQFunctionContext physCtx,
                                     CeedData *data, PetscInt fineLevel,
                                     PetscInt ncompu, PetscInt Ugsz,
                                     PetscInt Ulocsz, CeedVector forceCeed,
                                     CeedVector neumannCeed);

// Set up libCEED multigrid level for a given degree
PetscErrorCode SetupLibceedLevel(DM dm, Ceed ceed, AppCtx appCtx,
                                 CeedData *data, PetscInt level,
                                 PetscInt ncompu, PetscInt Ugsz,
                                 PetscInt Ulocsz, CeedVector fineMult);

// Setup context data for Jacobian evaluation
PetscErrorCode SetupJacobianCtx(MPI_Comm comm, AppCtx appCtx, DM dm, Vec V,
                                Vec Vloc, CeedData ceedData, Ceed ceed,
                                CeedQFunctionContext ctxPhys,
                                CeedQFunctionContext ctxPhysSmoother,
                                UserMult jacobianCtx);

// Setup context data for prolongation and restriction operators
PetscErrorCode SetupProlongRestrictCtx(MPI_Comm comm, AppCtx appCtx, DM dmC,
                                       DM dmF, Vec VF, Vec VlocC, Vec VlocF,
                                       CeedData ceedDataC, CeedData ceedDataF,
                                       Ceed ceed,
                                       UserMultProlongRestr prolongRestrCtx);

// -----------------------------------------------------------------------------
// Jacobian setup
// -----------------------------------------------------------------------------
PetscErrorCode FormJacobian(SNES snes, Vec U, Mat J, Mat Jpre, void *ctx);

// -----------------------------------------------------------------------------
// Solution output
// -----------------------------------------------------------------------------
PetscErrorCode ViewSolution(MPI_Comm comm, Vec U, PetscInt increment,
                            PetscScalar loadIncrement);

PetscErrorCode ViewDiagnosticQuantities(MPI_Comm comm, DM dmU,
                                        UserMult user, Vec U,
                                        CeedElemRestriction ErestrictDiagnostic);

// -----------------------------------------------------------------------------
// libCEED Operators for MatShell
// -----------------------------------------------------------------------------
// This function uses libCEED to compute the local action of an operator
PetscErrorCode ApplyLocalCeedOp(Vec X, Vec Y, UserMult user);

// This function uses libCEED to compute the non-linear residual
PetscErrorCode FormResidual_Ceed(SNES snes, Vec X, Vec Y, void *ctx);

// This function uses libCEED to apply the Jacobian for assembly via a SNES
PetscErrorCode ApplyJacobianCoarse_Ceed(SNES snes, Vec X, Vec Y, void *ctx);

// This function uses libCEED to compute the action of the Jacobian
PetscErrorCode ApplyJacobian_Ceed(Mat A, Vec X, Vec Y);

// This function uses libCEED to compute the action of the prolongation operator
PetscErrorCode Prolong_Ceed(Mat A, Vec X, Vec Y);

// This function uses libCEED to compute the action of the restriction operator
PetscErrorCode Restrict_Ceed(Mat A, Vec X, Vec Y);

// This function returns the computed diagonal of the operator
PetscErrorCode GetDiag_Ceed(Mat A, Vec D);

// This function calculates the strain energy in the final solution
PetscErrorCode ComputeStrainEnergy(DM dmEnergy, UserMult user,
                                   CeedOperator opEnergy, Vec X,
                                   PetscReal *energy);

// -----------------------------------------------------------------------------
// Boundary Functions
// -----------------------------------------------------------------------------
// Note: If additional boundary conditions are added, an update is needed in
//         elasticity.h for the boundaryOptions variable.

// BCMMS - boundary function
// Values on all points of the mesh is set based on given solution below
// for u[0], u[1], u[2]
PetscErrorCode BCMMS(PetscInt dim, PetscReal loadIncrement,
                     const PetscReal coords[], PetscInt ncompu,
                     PetscScalar *u, void *ctx);

// BCClamp - fix boundary values with affine transformation at fraction of load
//   increment
PetscErrorCode BCClamp(PetscInt dim, PetscReal loadIncrement,
                       const PetscReal coords[], PetscInt ncompu,
                       PetscScalar *u, void *ctx);

#endif //setup_h


/**
 */
#include <time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_sort_double.h>
#include "inout_aper.h"
#include "aXe_grism.h"
#include "spce_sect.h"
#include "spc_back.h"
#include "spce_PET.h"
#include "spc_wl_calib.h"
#include "aXe_errors.h"
#include "fringe_conf.h"
#include "spc_resp.h"
#include "spce_pathlength.h"
#include "aper_conf.h"
#include "specmodel_utils.h"
#include "model_utils.h"
#include "spc_fluxcube.h"
#include "fringe_conf.h"
#include "dirimage_model.h"

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

int
compute_dirimage_model(char dirim_file[], char conf_file[], char tpass_file[],
		       char specmod_file[], char objmod_file[], char aper_file[],
		       const double model_scale, const double tel_area, const double lambda_psf,
		       observation *obs, char map_file[])
{
  object          **oblist;
  dirobject       **dirlist;
  spectral_models *spec_mod;
  object_models   *obj_mod;

  interpolator    *tpass;

  gsl_matrix *dirimage_matrix=NULL;

  px_point npixels;

  // load the object list
  fprintf (stdout, "aXe_DIRIMAGE: Loading object aperture list...");
  oblist = file_to_object_list_seq (aper_file, obs);
  fprintf (stdout,"%d objects loaded.\n",object_list_size(oblist));

  // check whether highres models
  // are given
  if (strlen(specmod_file) > 0)
    // load the spectral models
    spec_mod = load_spectral_models(specmod_file);
  else
    // or set the struct to NULL
    spec_mod = NULL;

  // check whether direct emission models
  // are given
  if (strlen(objmod_file) > 0)
    obj_mod = load_object_models(objmod_file);
  else
    obj_mod = NULL;

  // get the sensitivity curve of the total passband
  tpass =  get_filter_sensitivity(tpass_file, tel_area);

  // get the image dimensions
  npixels = get_npixel(obs);

  // create the list of direct objects to be simulated
  // the interpolation type is fixed to linear
  dirlist = oblist_to_dirlist2(dirim_file, conf_file, npixels, oblist,
			       spec_mod, obj_mod, model_scale, 1);

  // determine the XOFF and YOFF values
  // for the various beams
  fill_xy_offsets(dirlist, conf_file);

  // create and fill the pixel matrix for the direct images
  dirimage_matrix = make_dirimage(oblist, dirlist, npixels, lambda_psf, tpass);

  // store the contamination image
  gsl_to_FITSimage (dirimage_matrix, map_file, 1, NULL);

  // check if memory must be released;
  // do it if necessary
  if (spec_mod != NULL)
    free_spectral_models(spec_mod);
  if (obj_mod != NULL)
    free_object_models(obj_mod);
  if (oblist !=NULL)
    free_oblist (oblist);

  // always release these
  // objects
  free_dirlist(dirlist);
  gsl_matrix_free(dirimage_matrix);
  free_interp(tpass);

  // return always '1'
  return 1;
}

gsl_matrix *
make_dirimage(object **oblist, dirobject **dirlist, const px_point npixels,
	      const double lambda_psf, interpolator *tpass)
{
  dirobject       *actdir;
  beam             actbeam;

  gsl_matrix      *dirimage_matrix;

  double cps        = 0.0;
  double sval       = 0.0;
  double value      = 0.0;

  int ii=0;
  int nx=0;
  int ny=0;

  d_point dpixel;

   // allocate memory for the image matrix
  dirimage_matrix = gsl_matrix_alloc(npixels.x, npixels.y);
  gsl_matrix_set_all(dirimage_matrix,0.0);

  // go over each beam model
  ii = 0;
  while (oblist[ii] != NULL)
    {

      // get the direct object for the actual model spectrum
      actdir = get_dirobject_from_list(dirlist, oblist[ii]->ID);

      // get the beam for the actual model spectrum
      actbeam = oblist[ii]->beams[0];

      // report onto the screen
      fprintf(stdout, "aXe_DIRIMAGE: modelling object %i ...", oblist[ii]->ID);

      // get the integrated direct image intensity in cps
      cps = get_cps_for_dirobject(tpass, actdir);

      // correct the direct image positions for
      // the offset values introduced by building
      // the direct image objects around the reference
      // position, which is shifted from the true
      // direct image position by XOFF and YOFF as
      // given in the aXe configuration file
      actdir->ix_min -= (int)floor(actdir->xy_off[actbeam.ID].x + 0.5);
      actdir->ix_max -= (int)floor(actdir->xy_off[actbeam.ID].x + 0.5);
      actdir->iy_min -= (int)floor(actdir->xy_off[actbeam.ID].y + 0.5);
      actdir->iy_max -= (int)floor(actdir->xy_off[actbeam.ID].y + 0.5);
      actbeam.refpoint.x -= actdir->xy_off[actbeam.ID].x;
      actbeam.refpoint.y -= actdir->xy_off[actbeam.ID].y;

      // go over each pixel in the direct object area
      for (nx=actdir->ix_min; nx<=actdir->ix_max; nx++)
	{
	  // make sure to be inside the image
	  if (nx < 0 || nx >=  (int)dirimage_matrix->size1)
	    // skip column if not
	    continue;
	  for (ny=actdir->iy_min; ny<=actdir->iy_max; ny++)
	    {
	      // make sure to be inside the image
	      if (ny < 0 || ny >=  (int)dirimage_matrix->size2)
		// skip element if not
		continue;

	      // fill the dpixel structure
	      dpixel.x = (double)nx;
	      dpixel.y = (double)ny;

	      // check whether there is
	      // an image template
	      if (actdir->dirim)
		// get the pixel intensity from the image template
		sval = get_diremission_value(actdir->dirim, dpixel.x - actbeam.refpoint.x, dpixel.y - actbeam.refpoint.y);
	      else
		// get the Gaussian pixel intensity;
		//
		// do a subsampling over the pixel
		// to get a more appropriate value for the
		// emission val
		sval = get_sub_emodel_value(dpixel, actbeam, actdir->drzscale);

	      // compute and set the new pixel values
	      value = gsl_matrix_get(dirimage_matrix, nx, ny) + sval*cps;
	      gsl_matrix_set(dirimage_matrix, nx, ny, value);
	    }
	}

      // release the space for the various structures
      fprintf(stdout, " Done\n");
      ii++;
    }

  // return the image matrix
  return dirimage_matrix;
}

double
get_cps_for_dirobject(interpolator *tpass, dirobject *actdir)
{
  interpolator *combine;

  double cps=0.0;

  // create aa new multiplicator
  // interpolator
  combine = combine_tpass_SED(tpass, actdir);

  // integrate over the interpolator
  cps = integrate_interpolator(combine);

  // release memory
  free_interp(combine);

  // return the cps value
  return cps;
}

double
integrate_interpolator(interpolator *combine)
{
  double integral=0.0;

  int index;

  for (index=1; index < combine->nvals-1; index++)
    // add the increment
    integral +=  combine->yvals[index] * (combine->xvals[index+1] - combine->xvals[index-1]);

  // dont forget the start and end piece
  integral +=  combine->yvals[0]                * (combine->xvals[1]                - combine->xvals[0]);
  integral +=  combine->yvals[combine->nvals-1] * (combine->xvals[combine->nvals-1] - combine->xvals[combine->nvals-2]);

  // then return the half
  return integral/2.0;
}

interpolator *
combine_tpass_SED(interpolator *tpass, dirobject *actdir)
{
  interpolator *combine;

  int index=0;
  int n_additional=0;
  int act_index;

  gsl_vector     *indep_data;

  // go over all SED data points
  for (index = 0; index < actdir->SED->npoints; index++)
    {
      // transform to Angstrom
      actdir->SED->wavelength[index] *= 10.0;

      // check whether the independent data point
      // falls in the area of the sensitivity
      if (actdir->SED->wavelength[index] < tpass->xmax
	  && actdir->SED->wavelength[index] >  tpass->xmin)
	// enhance the number
	// of additional data points
	n_additional += 1;
    }

  // allocate memory for the new independent value list
  indep_data   = gsl_vector_alloc(tpass->nvals + n_additional);

  // go over all sensitivity data points and fill in
  // the independent values
  for (index = 0; index < tpass->nvals; index++)
    gsl_vector_set(indep_data, index, tpass->xvals[index]);

  // set the counter to the next free index
  act_index = tpass->nvals;

  // go over all SED data points
  for (index = 0; index < actdir->SED->npoints; index++)
    // check whether the independent data point
    // falls in the area of the sensitivity
    if (actdir->SED->wavelength[index] < tpass->xmax
	&& actdir->SED->wavelength[index] >  tpass->xmin)
      {
	// fill in the additional independent data point
	// from the SED
	gsl_vector_set(indep_data, act_index, actdir->SED->wavelength[index]);

	// enhance the counter
        act_index += 1;
      }

  // sort the vector
  gsl_sort_vector(indep_data);

  // compose a new interpolator from the independent values
  combine = get_combined_tpass_SED(indep_data, tpass, actdir);

  // release memory in the weight vector
  gsl_vector_free(indep_data);

  // go over all SED data points
  for (index = 0; index < actdir->SED->npoints; index++)
    // transform to nm
    actdir->SED->wavelength[index] /= 10.0;

  // return the interpolator
  return combine;
}

interpolator *
get_combined_tpass_SED(gsl_vector *indep_data, interpolator *tpass, dirobject *actdir)
{
  interpolator *combine;

  gsl_vector_int *indep_weight;

  int n_new=0;
  int index;
  int act_index;

  double *xvals_new;
  double *yvals_new;

  // allocat memory for the weight vector
  indep_weight = gsl_vector_int_alloc(indep_data->size);

  // set all the weights
  gsl_vector_int_set_all(indep_weight, 1);

  // go over the independent data
  for (index = 1; index < (int)indep_data->size; index++)
    // check whether the current entry is equal the previous one
    if (gsl_vector_get(indep_data, index) ==  gsl_vector_get(indep_data, index-1))
      // set the weight of the current entry to zero
      gsl_vector_int_set(indep_weight,index, 0);

  // initialize the
  // final number
  n_new = 0;

  // go over the weight array
  for (index=0; index < (int)indep_weight->size; index++)
    // just count the weights
    n_new += gsl_vector_int_get(indep_weight, index);

  // allocate memory for the interpolator data
  xvals_new = (double *)malloc(n_new * sizeof(double));
  yvals_new = (double *)malloc(n_new * sizeof(double));

  // go over the weight array
  act_index=0;
  for (index=0; index < (int)indep_weight->size; index++)
    // check for weight
    if (gsl_vector_int_get(indep_weight, index))
      {
	// take the independent value from the vector
	xvals_new[act_index] = gsl_vector_get(indep_data, index);

	// compute the new dependent value bye multiplying
	// the brightness with the sensitivity
	yvals_new[act_index] = eval_interp(tpass, xvals_new[act_index])
	  * get_flux_from_SED(actdir->SED, xvals_new[act_index]);

	// enhance the counter
	act_index++;
      }

  // create a new interpolator
  combine = create_interp(n_new, TPASS_INTERP_TYPE, xvals_new, yvals_new);

  // release memory in the weight vector
  gsl_vector_int_free(indep_weight);

  // return the interpolator
  return combine;
}

/*
 * Function: get_filter_sensitivity
 * The function reads in a total passband file from fits format
 * and stroes the data as an interpolation function. The dependent
 * data values are transformed from throughput to sensitivity,
 * and the interpolation function is returned.
 *
 * Parameters:
 * @param tpass_file  - the full name of the total passband
 * @param tel_are     - the collecting area of the telescope
 *
 * Returns:
 * @return tpass    - the filter sensitivity
 */
interpolator *
get_filter_sensitivity(const char tpass_file[], const double tel_area)
{
  interpolator    *tpass;

  double h_erg  = 6.6260693E-27;
  double c_cm   = 2.99792458E+10;

  double factor = 0.0;

  int index=0;

  fprintf(stdout, "Load Total bandpass table :%s\n", tpass_file);
  tpass =  create_interp_ftable(tpass_file, 2,"WAVELENGTH","THROUGHPUT", TPASS_INTERP_TYPE);

  for (index=0; index < tpass->nvals; index++)
    {
      // compute the conversion factor
      factor = tel_area / (h_erg * c_cm / (1.0E-08 * tpass->xvals[index]));

      // apply the conversion factor
      tpass->yvals[index] = tpass->yvals[index] * factor;
    }

  // return the filter sensitivity
  return tpass;
}


/* ---------------------------------------------------------------- *
 * hod.h						            *
 * Martin Kilbinger, Henry J. McCracken, Jean Coupon 2008-2013      *
 * ---------------------------------------------------------------- */

#ifndef __HOD_H
#define __HOD_H

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

#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>

#include "io.h"
#include "errorlist.h"
#include "config.h"
#include "maths.h"

#include "cosmo.h"
#include "nofz.h"
#include "halomodel.h"

/* errors */
#define HOD_BASE        -50000
#define HOD_MSTAR_MAX  HOD_BASE  + 1
#define HOD_NGAL_FIT   HOD_BASE  + 2

#define GM_base      -400000
#define GM_nz_type   GM_base + 1
#define GM_pi_max    GM_base + 2
#define GM_SHMR      GM_base + 3
#define GM_SMF       GM_base + 4
#define GM_GGL       GM_base + 5
#define GM_MSTAR_MAX GM_base + 6
#define GM_ETA       GM_base + 7

/* Limits for xi(r), 1- and 2-halo, in Mpc.h */
#define RMIN1 0.001
#define RMAX1 5.0
#define RMIN2 0.1
#define RMAX2 400.0

#define RMIN  RMIN1
#define RMAX  RMAX2
#define NR    50

#define MAXCHAR 1024
#define NLINES 100

#define NFIELD 100
#define NCHAR 20

/* GG: galaxy-galaxy stuff. GM: galaxy-dar matter stuff (lensing) */
#define GG 0
#define GM 1

#define getDoubleValue(array,col)  atof(array+NCHAR*(col-1))
#define getIntValue(array,col)     atoi(array+NCHAR*(col-1))
#define getCharValue(array,col)    array+NCHAR*(col-1)
#define getLine(array,i)           array+NFIELD*NCHAR*i

#define Nhalodata_t 4
typedef enum {w_of_theta, wp_rp, deltaSigma, smf} halodata_t;
#define shalodata_t(i) ( \
  i==w_of_theta    ? "w_of_theta" : \
  i==wp_rp         ? "wp_rp"      : \
  i==deltaSigma    ? "deltaSigma" : \
  i==smf           ? "smf"        : \
  "")

#define Nhalomode_t 3
typedef enum {galcorr_var, galcorr_cov, galcorr_log} halomode_t;
#define shalomode_t(i) (   \
 i==galcorr_var ? "galcorr_var" : \
 i==galcorr_cov ? "galcorr_cov" : \
 i==galcorr_log ? "galcorr_log" : \
 "")

#define Nngal_fit_t 5
typedef enum {ngal_log_fit, ngal_lin_fit, ngal_no_fit, ngal_lin_fit_only} ngal_fit_t;
#define sngal_fit_t(i) ( \
  i==ngal_log_fit  ? "ngal_log_fit"  : \
  i==ngal_lin_fit  ? "ngal_lin_fit"  : \
  i==ngal_no_fit   ? "ngal_no_fit"   : \
  i==ngal_lin_fit_only ? "ngal_lin_fit_only" : \
  "")

#define Nintconst_t 2
typedef enum {constant, random_file} intconst_t;
#define sintconst_t(i) ( \
  i==constant  ?    "constant"  : \
  i==random_file  ? "random_file"  : \
  "")

typedef struct {
  char WTHETA[500];
  char COVNAME[500];
  char DNDZ[500];
  char OUTFILE[500];
  int nbins;
  double ngal, ngalerr, area_deg, intconst, delta;
  double alpha_min,alpha_max,dalpha;
  double M1_min,M1_max,dM1;
  double Mmin_min,Mmin_max,dMmin;
  double Ngal_min,Ngal_max,dNgal;
  ngal_fit_t ngal_fit_type;
} config_hm_info;

/* ---------------------------------------------------------------- *
 * Global variables and functions                                   *
 * ---------------------------------------------------------------- */

#define ODD 0
#define EVEN 1

#define ABS(a) ((a) < 0 ? -(a) : (a))
#define PARITY(a) (a)%2 ? ODD : EVEN
#define FFTLog_SWAP(a,b) {FFTLog_TMP = (a); (a) = (b); (b) = FFTLog_TMP;}

#define CHANGE(fct) int change_##fct(cosmo_hm*, cosmo_hm*)
CHANGE(w_of_theta);
CHANGE(Pthg);
CHANGE(HOD);
CHANGE(ngd);
CHANGE(vc);
#undef CHANGE

/* ---------------------------------------------------------------- *
 * New types                                                        *
 * ---------------------------------------------------------------- */

/* This structure contains w_theta input data */
typedef struct {
  int nbins, nbins_RR;
  double *th;
  double *w;
  double *werr;   /* Error bars */
  double *wcov;   /* Covariance matrix */
  double *th_RR;  /* theta for random pairs (can be larger than for the data [even recommended]) */
  double *RR;     /* Random pairs (for the integral constraint) */
  double ngal;    /* Galaxy number density*/
  double ngal_err;
} wt_t;

typedef struct Nz_hist
{
  int nbins;
  double *z;
  double *n;
  double zmin;
  double zmax;
  double mean;
  double dz;
  double Norm;
  double max;
} Nz_hist;

/* ---------------------------------------------------------------- *
 * log-likelihood                                                   *
 * ---------------------------------------------------------------- */

double chi2_hm(cosmo_hm *model, halodata_t halodata, halomode_t halomode, const wt_t* data, ngal_fit_t ngal_fit_type,
	       double ngal, double ngalerr, intconst_t intconst_type, error **err);

/* ---------------------------------------------------------------- *
 * w(theta) and wp(rp)                                              *
 * ---------------------------------------------------------------- */

double *woftheta(cosmo_hm *model, pofk_t pofk, double *theta, int Ntheta, int i_bin, int j_bin, error **err);
double *woftheta_FFTLog_total(cosmo_hm *model, double ln_theta_min, double ln_delta_theta, int Ntheta,
			      double **theta, int i_bin, int j_bin, error **err);
double *wp(cosmo_hm *model, pofk_t pofk, const double *rp, int Nrp, double pi_max, int type, error **err);
double int_for_wp(double logr, void *params, error **err);

/* ---------------------------------------------------------------- *
 * Lensing functions (only for leauthaud11 model)                   *
 * ---------------------------------------------------------------- */

double *DeltaSigma(cosmo_hm *model, pofk_t pofk, const double *r, int N, error **err);
double int_for_Sigma(double logr, void *params, error **err);

/* ---------------------------------------------------------------- *
 * Stellar mass function (only for leauthaud11 model)               *
 * ---------------------------------------------------------------- */

double *dNdlogM10stellar(cosmo_hm *model, double *log10Mstellar, int N, error **err);
double *dNdlogM10stellar_c(cosmo_hm *model, double *log10Mstellar, int N, error **err);
double *dNdlogM10stellar_s(cosmo_hm *model, double *log10Mstellar, int N, error **err);

/* ---------------------------------------------------------------- *
 * HOD P(k) and xi(r)                                               *
 * ---------------------------------------------------------------- */

double *xiofr(cosmo_hm *model, pofk_t pofk, const double *r, int N, int type, error **err);
double *xi_1hcs(cosmo_hm *model,double a, const double *r, int N, int type, error **err);
double int_for_xi_1hcs(double logM, void *params, error **err);
double *xi_1hss(cosmo_hm *model,double a, const double *r, int N, int type, error **err);
double P1hss(double k, void *params);
double int_for_P1hss(double logM, void *params, error **err);
double concentration_sat(cosmo_hm *model, double Mh, double a, error **err);
double *xi_2h(cosmo_hm *model,double a, const double *r, int N, int type, error **err);
double *xi_P_NL(cosmo_hm *model, double a, const double *r, int N, error **err);
double FFTLog_P_NL(double k, void *params);
double P2h(double k, void *params);
double int_for_P2h(double logM, void *params, error **err);
double int_for_P2h_dm(double logM, void *params, error **err);
double bias_r(double M, void *params);

/* ---------------------------------------------------------------- *
 * HOD model functions                                              *
 * ---------------------------------------------------------------- */

double log10_fSHMR(double log10Mh, cosmo_hm *model);
double log10_fSHMR_inv_minus_x(double log10Mstar, void *p);
double log10_fSHMR_inv(double log10Mstar, cosmo_hm *model);
double Ngal_c(cosmo_hm *model, double Mh, double Mstellar_min, double Mstellar_max);
double int_for_Ngal_c(double Mstellar, void *params, error **err);
double phi_c_Mstellar(cosmo_hm *model, double log10Mstellar, double Mh, error **err);
double sigma_log_M(cosmo_hm *model, double log10Mstellar, error **err);
double eta_cen(cosmo_hm *model, double Mh, error **err);
double Ngal_s(cosmo_hm *model, double M, double Mstellar_min, double Mstellar_max);
double Ngal(cosmo_hm *model, double M, double Mstellar_min, double Mstellar_max);

double av_Mh_given_Mstar(cosmo_hm *model, double Mstellar, double a, error **err);
double int_for_phi_c_Mh(double log10Mh, void *params, error **err);
double int_for_phi_c_Mh_norm(double log10Mh, void *params, error **err);

/* ---------------------------------------------------------------- *
 * Deduced quantities                                               *
 * ---------------------------------------------------------------- */
double Mstar_tot_c(cosmo_hm *model, double Mh, double Mstellar_min, double Mstellar_max, error **err);
double int_for_Mstar_tot_c(double logMstar, void *params, error **err);
double Mstar_tot_s(cosmo_hm *model, double Mh, double Mstellar_min, double Mstellar_max, error **err);
double int_for_Mstar_tot_s(double logMstar, void *params, error **err);
double av_gal_bias(cosmo_hm *model, double a, error **err);
double int_for_av_gal_bias(double logM, void *intpar, error **err);
double av_halo_mass(cosmo_hm *model, double a, error **err);
double int_for_av_halo_mass(double logM, void *intpar, error **err);
double mass_weighted_av_stellar_mass(cosmo_hm *model, double a, error **err);
double int_for_mass_weighted_av_stellar_mass(double logM, void *params, error **err);
double int_for_mass_denum_weighted_av_stellar_mass(double logM, void *params, error **err);
double av_stellar_mass(cosmo_hm *model, double a, error **err);
double int_for_av_stellar_mass(double logM, void *params, error **err);
double av_frsat(cosmo_hm *model, double a, error **err);
double int_for_av_frsat(double logM, void *intpar, error **err);
double av_halo_bias(cosmo_hm *model, double a, error **err);
double int_for_av_halo_bias(double logM, void *intpar, error **err);
double dn_dlnM_integrand(double logM, void *intpar, error **err);

/* ---------------------------------------------------------------- *
 * Physical quantities                                              *
 * ---------------------------------------------------------------- */

double vc(cosmo_hm *model,double zmin, double zmax, error **err);
double int_for_vc(double z, void *params, error **err);
double logM_lim(cosmo_hm *model, double a, double r, error **err);
double ngal_triax(cosmo_hm *model, double a, double r,  error **err);
double ngal_den(cosmo_hm *model, double a, double logMlim, double Mstellar_min, double Mstellar_max, error **err);
double int_for_ngal_den(double logM, void *params,  error **err);
double ngal_den_c(cosmo_hm *model, double a, double logMlim, double Mstellar_min, double Mstellar_max, error **err);
double int_for_ngal_den_c(double logM, void *params,  error **err);
double ngal_den_s(cosmo_hm *model, double a, double logMlim, double Mstellar_min, double Mstellar_max, error **err);
double int_for_ngal_den_s(double logM, void *params,  error **err);
double ngal_den_vol(cosmo_hm *model, error **err);
double ngal_weighted(cosmo_hm *model, error **err);

/* ---------------------------------------------------------------- *
 * FFTLog                                                           *
 * ---------------------------------------------------------------- */

double xi_from_Pkr(gsl_function *ak, double r_prime, FFTLog_config *fc, error **err);
void FFTLog(FFTLog_config *fc, const gsl_function *ar_in,double *k, double *ak_out, int dir, error **err);
FFTLog_config *FFTLog_init(int N, double min, double max, double q, double mu);
void FFTLog_free(FFTLog_config *fc);
FFTLog_complex FFTLog_U_mu(double mu, FFTLog_complex z);

/* ---------------------------------------------------------------- *
 * Utils                                                            *
 * ---------------------------------------------------------------- */

void updateFrom_hm(cosmo_hm* avant, cosmo_hm* apres, error **err);
int change_vc(cosmo_hm *avant, cosmo_hm *apres);
int change_ngd(cosmo_hm *avant, cosmo_hm *apres);
int change_HOD(cosmo_hm *avant, cosmo_hm *apres);
int change_Pthg(cosmo_hm* avant, cosmo_hm* apres);
int change_w_of_theta(cosmo_hm *avant, cosmo_hm *apres);
wt_t *init_wt_t(double nlines, int wcov, error **err);
void free_wt_t(wt_t **model);
wt_t *read_wtheta(const char *data_file_name, intconst_t intconst_type, double delta, double intconst, const char *ran_file_name, error **err);
int getStrings(char *line, char *strings, char *delimit, size_t *N, error **err);
void read_config_hm_file(config_hm_info *config, char cname[], error **err);
Nz_hist *get_nz(redshift_t *nz_mk, int n_bin, error **err);
void  free_nz( Nz_hist *nz);
double normalize(double *data, size_t Nbins, double binsize,error **err);

/* ---------------------------------------------------------------- *
 * Obsolete stuff                                                   *
 * ---------------------------------------------------------------- */

double *woftheta_total_OBSOLETE(cosmo_hm *model, double log_theta_min, double log_delta_theta, int Ntheta, error **err);
nz_t *read_dndz_OBSOLETE(char *infile,error **err);
double Ngal_mean(cosmo_hm *model, double a, error **err);

double xir_mwolk(cosmo_hm *model, double a, double r, error **err);
wt_t *read_wtheta_mwolk(const char *name, double delta, double intconst, error **err);
double chi2_mwolk(cosmo_hm *model, const wt_t* data, ngal_fit_t ngal_fit_type,
		  double ngal, double ngalerr, double area, error **err);
double wp_mwolk(cosmo_hm *model, double rp, error **err);
double compute_chisq_wp(cosmo_hm *model, const wt_t *wth, double ngd_obs, double ngd_err,
			ngal_fit_t ngal_fit_type, double *ngd, int dologw, error **err);

#endif


/**
 * @file batchg_zgemm.c
 *
 *  @brief BBLAS batchg_zgemm double _Complex routine.
 *
 *  BBLAS is a software package provided by Univ. of Manchester,
 *  Univ. of Tennessee.
 *
 * @version 1.0.0
 * @author  Samuel  D. Relton
 * @author  Pedro   V. Lara
 * @author  Mawussi Zounon
 * @date    2016-06-03
 *
 **/

#ifndef DOXYGEN_SHOULD_SKIP_THIS
/**
 * Code generation
 * @precisions normal z -> c d s
 **/
#endif

#include <cblas.h>
#include "bblas.h"

#define COMPLEX

/**
    Purpose
    -------
    <b>batchg_zgemm</b> is a batch version of zgemm for groups
    of batched with different sizes.
	It performs the matrix-matrix operations

	arrayC[i] = alpha[i]*op( arrayA[i] )*op( arrayB[i] ) + beta[i]*arrayC[i],

	where op( X ) is one of

	op( X ) = X      or
	op( X ) = X**T   or
	op( X ) = X**H,

	alpha[i] and beta[i] are scalars, and arrayA[i], arrayB[i] and C are matrices, with
	op( arrayA[i] ) an m by k matrix, op( arrayB[i] ) a k by n matrix and arrayC[i]
	an m by n matrix.

	Group Batch Operations
	----------------------

    - all parameters that are arrays must have length at least group_count.
	- all parameters that are arrays must have all values set.

	Parameters
	----------
	@param[in]
	transA   Array of <tt>enum BBLAS_TRANS</tt>.
	         On entry, transA[i] specifies the form of op( arrayA[i] ) to be used in
			 the matrix multiplication as follows:
      -     = BblasNoTrans:    op( arrayA[i] ) = arrayA[i].
      -     = BblasTrans:      op( arrayA[i] ) = arrayA[i]**T.
      -     = BblasConjTrans:  op( arrayA[i] ) = arrayA[i]**H.

	@param[in]
	transB   Array of <tt>enum BBLAS_TRANS</tt>.
	         On entry, transB[i] specifies the form of op( arrayB[i] ) to be used in
             the matrix multiplication as follows:
      -     = BblasNoTrans:    op( arrayB[i] ) = arrayB[i].
      -     = BblasTrans:      op( arrayB[i] ) = arrayB[i]**T.
      -     = BblasConjTrans:  op( arrayB[i] ) = arrayB[i]**H.

	@param[in]
	M       Array of <tt>int</tt>.
            Each element M[i] specifies the number of rows of the matrix
            op( arrayA[i] ) and of the matrix arrayC[i]. M[i] must be greater than zero.

	@param[in]
	N       Array of <tt>int</tt>.
            Each element N[i] specifies the number of columns of the matrix
            op( arrayB[i] ) and the number of columns of the matrix arrayC[i].
 		    N[i] must be greater than zero.

    @param[in]
    K      Array of <tt>int</tt>.
	       Each element K[i] specifies the number of columns of the matrix
           op( arrayA[i] ) and the number of rows of the matrix op( arrayB[i] ).
		   K[i] must be greater than zero.

    @param[in]
    alpha   Array of <tt>complex_16</tt>.

    @param[in]
    arrayA  Array of pointers.
            Each element arrayA[i] is a pointer to a COMPLEX_16 matrix of
 		    dimension lda[i] by Ka[i], where Ka[i] is K[i] when transA[i] = BblasNoTrans,
 		    and is M[i] otherwise.
            When using transA[i] = BblasNoTrans the leading M[i] by K[i]
            part of arrayA[i] must contain the matrix elements, otherwise
            the leading  K[i] by M[i] part of arrayA[i] must contain the
            matrix elements.

    @param[in]
    lda     Array of <tt>int</tt>.
            Each element lda[i] specifies the first dimension of arrayA[i] as declared
            in the calling (sub) program. When transA[i] = BblasNoTrans then
            lda[i] must be at least max( 1, M[i] ), otherwise lda[i] must be at
            least max( 1, K[i] ).

    @param[in]
    arrayB  Array of pointers.
  		    Each element arrayB[i] is a pointer to a COMPLEX_16 matrix of
  		    dimension ldb[i] by Kb[i], where Kb[i] is N[i] when transB[i] = BblasNoTrans,
  		    and is K[i] otherwise.
            When using transB[i] = BblasNoTrans the leading K[i] by N[i]
            part of arrayB[i] must contain the matrix elements, otherwise
            the leading N[i] by K[i] part of arrayB[i] must contain the
            matrix elements.

    @param[in]
    ldb     Array of <tt>int</tt>.
  		    Each element ldb[i] specifies the first dimension of arrayB[i] as declared
            in the calling (sub) program. When transB[i] = BblasNoTrans then
            ldb[i] must be at least max( 1, K[i] ), otherwise ldb[i] must be at
            least max( 1, N[i] ).

    @param[in]
    beta    Array of <tt>complex_16</tt>.
            When beta[i] is set to zero arrayC[i] need not be set on input.

    @param[in,out]
    arrayC  Array of pointers.
	        Each element arrayC[i] is a pointer to a COMPLEX_16 matrix of
      	    dimension ldc[i] by N[i].
  	        Before entry, the leading M[i] by N[i] part of the arrayC[i] must
            contain a matrix C, except when beta is zero, in which
            case C need not be set on entry.
            On exit, the matrix arrayC[i] is overwritten by the M[i] by N[i] matrix
            ( alpha[i]*op( arrayA[i] )*op( arrayB[i] ) + beta[i]*arrayC[i] ).

    @param[in]
    ldc     Array of <tt>int</tt>.
  	        Each element ldc[i] specifies the first dimension of arrayC[i] as declared
  	        in the calling (sub) program. The value ldc[i] must be at least
  	        max( 1, M[i] )

    @param[in]
    batch_count  <tt>int</tt>
                 The number of matrices to operate on.

    @param[in,out]
    info    Array of <tt>int</tt>.
            Each element info[i] is the error return code of the ith group,
            these need not be set on entry.
  	    The error codes can be found in bblas_macros.h.
 **/
void batchg_zgemm(
    const enum BBLAS_TRANS *transA, const enum BBLAS_TRANS *transB,
    const int *M,  const int *N, const int *K,
    const BBLAS_Complex64_t *alpha,
    const BBLAS_Complex64_t **arrayA, const int *lda,
    const BBLAS_Complex64_t **arrayB,
    const int *ldb, const BBLAS_Complex64_t *beta,
    BBLAS_Complex64_t **arrayC, const int *ldc,
	const int group_count, const int *group_size,
    int *info)
{
    /* Local variables */
    int group_iter;
    int offset = 0;
    char func_name[15] = "batchg_zgemm";

	/* Check input arguments */
    if (group_count < 0)
    {
	    xerbla_batch(func_name, BBLAS_ERR_BATCH_COUNT, -1);
        return;
    }

	/* Check group_size and call fixed batch computation */
	for (group_iter = 0; group_iter < group_count; group_iter++)
	{
		if (group_size[group_iter] < 0)
		{
			xerbla_batch(func_name, BBLAS_ERR_GROUP_SIZE, group_iter);
		    info[group_iter] = BBLAS_ERR_GROUP_SIZE;
			continue;
		}

        /* Call to bblas_zgemm_batchf */
        batchf_zgemm(
            transA[group_iter],
            transB[group_iter],
            M[group_iter],
            N[group_iter],
            K[group_iter],
            alpha[group_iter],
            arrayA+offset,
            lda[group_iter],
            arrayB+offset,
            ldb[group_iter],
            beta[group_iter],
            arrayC+offset,
            ldc[group_iter],
            group_size[group_iter],
            &info[group_iter]);

        offset += group_size[group_iter];
    }
}
#undef COMPLEX


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:45:10 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -hc2hc-backward 6 */

/*
 * This function contains 72 FP additions, 38 FP multiplications,
 * (or, 54 additions, 20 multiplications, 18 fused multiply/add),
 * 25 stack variables, and 48 memory accesses
 */
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);
static const fftw_real K866025403 = FFTW_KONST(+0.866025403784438646763723170752936183471402627);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);
static const fftw_real K1_732050807 = FFTW_KONST(+1.732050807568877293527446341505872366942805254);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_hc2hc_backward_6(fftw_real *A, const fftw_complex *W, int iostride, int m, int dist)
{
     int i;
     fftw_real *X;
     fftw_real *Y;
     X = A;
     Y = A + (6 * iostride);
     {
	  fftw_real tmp71;
	  fftw_real tmp75;
	  fftw_real tmp80;
	  fftw_real tmp82;
	  fftw_real tmp74;
	  fftw_real tmp76;
	  fftw_real tmp69;
	  fftw_real tmp70;
	  fftw_real tmp77;
	  fftw_real tmp81;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp69 = X[0];
	  tmp70 = X[3 * iostride];
	  tmp71 = tmp69 - tmp70;
	  tmp75 = tmp69 + tmp70;
	  {
	       fftw_real tmp78;
	       fftw_real tmp79;
	       fftw_real tmp72;
	       fftw_real tmp73;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp78 = Y[-2 * iostride];
	       tmp79 = Y[-iostride];
	       tmp80 = K1_732050807 * (tmp78 + tmp79);
	       tmp82 = K1_732050807 * (tmp78 - tmp79);
	       tmp72 = X[2 * iostride];
	       tmp73 = X[iostride];
	       tmp74 = tmp72 - tmp73;
	       tmp76 = tmp72 + tmp73;
	  }
	  X[3 * iostride] = tmp71 + (K2_000000000 * tmp74);
	  tmp77 = tmp71 - tmp74;
	  X[iostride] = tmp77 - tmp80;
	  X[5 * iostride] = tmp77 + tmp80;
	  X[0] = tmp75 + (K2_000000000 * tmp76);
	  tmp81 = tmp75 - tmp76;
	  X[2 * iostride] = tmp81 + tmp82;
	  X[4 * iostride] = tmp81 - tmp82;
     }
     X = X + dist;
     Y = Y - dist;
     for (i = 2; i < m; i = i + 2, X = X + dist, Y = Y - dist, W = W + 5) {
	  fftw_real tmp15;
	  fftw_real tmp46;
	  fftw_real tmp25;
	  fftw_real tmp52;
	  fftw_real tmp22;
	  fftw_real tmp35;
	  fftw_real tmp49;
	  fftw_real tmp62;
	  fftw_real tmp32;
	  fftw_real tmp39;
	  fftw_real tmp55;
	  fftw_real tmp59;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp13;
	       fftw_real tmp14;
	       fftw_real tmp23;
	       fftw_real tmp24;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp13 = X[0];
	       tmp14 = Y[-3 * iostride];
	       tmp15 = tmp13 + tmp14;
	       tmp46 = tmp13 - tmp14;
	       tmp23 = Y[0];
	       tmp24 = X[3 * iostride];
	       tmp25 = tmp23 - tmp24;
	       tmp52 = tmp23 + tmp24;
	  }
	  {
	       fftw_real tmp18;
	       fftw_real tmp47;
	       fftw_real tmp21;
	       fftw_real tmp48;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp16;
		    fftw_real tmp17;
		    fftw_real tmp19;
		    fftw_real tmp20;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp16 = X[2 * iostride];
		    tmp17 = Y[-5 * iostride];
		    tmp18 = tmp16 + tmp17;
		    tmp47 = tmp16 - tmp17;
		    tmp19 = Y[-4 * iostride];
		    tmp20 = X[iostride];
		    tmp21 = tmp19 + tmp20;
		    tmp48 = tmp19 - tmp20;
	       }
	       tmp22 = tmp18 + tmp21;
	       tmp35 = K866025403 * (tmp18 - tmp21);
	       tmp49 = tmp47 + tmp48;
	       tmp62 = K866025403 * (tmp47 - tmp48);
	  }
	  {
	       fftw_real tmp28;
	       fftw_real tmp54;
	       fftw_real tmp31;
	       fftw_real tmp53;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp26;
		    fftw_real tmp27;
		    fftw_real tmp29;
		    fftw_real tmp30;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp26 = Y[-2 * iostride];
		    tmp27 = X[5 * iostride];
		    tmp28 = tmp26 - tmp27;
		    tmp54 = tmp26 + tmp27;
		    tmp29 = Y[-iostride];
		    tmp30 = X[4 * iostride];
		    tmp31 = tmp29 - tmp30;
		    tmp53 = tmp29 + tmp30;
	       }
	       tmp32 = tmp28 + tmp31;
	       tmp39 = K866025403 * (tmp31 - tmp28);
	       tmp55 = tmp53 - tmp54;
	       tmp59 = K866025403 * (tmp54 + tmp53);
	  }
	  X[0] = tmp15 + tmp22;
	  {
	       fftw_real tmp36;
	       fftw_real tmp42;
	       fftw_real tmp40;
	       fftw_real tmp44;
	       fftw_real tmp34;
	       fftw_real tmp38;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp34 = tmp25 - (K500000000 * tmp32);
	       tmp36 = tmp34 - tmp35;
	       tmp42 = tmp35 + tmp34;
	       tmp38 = tmp15 - (K500000000 * tmp22);
	       tmp40 = tmp38 - tmp39;
	       tmp44 = tmp38 + tmp39;
	       {
		    fftw_real tmp33;
		    fftw_real tmp37;
		    fftw_real tmp41;
		    fftw_real tmp43;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp33 = c_re(W[1]);
		    tmp37 = c_im(W[1]);
		    Y[-3 * iostride] = (tmp33 * tmp36) - (tmp37 * tmp40);
		    X[2 * iostride] = (tmp37 * tmp36) + (tmp33 * tmp40);
		    tmp41 = c_re(W[3]);
		    tmp43 = c_im(W[3]);
		    Y[-iostride] = (tmp41 * tmp42) - (tmp43 * tmp44);
		    X[4 * iostride] = (tmp43 * tmp42) + (tmp41 * tmp44);
	       }
	  }
	  Y[-5 * iostride] = tmp25 + tmp32;
	  {
	       fftw_real tmp50;
	       fftw_real tmp56;
	       fftw_real tmp45;
	       fftw_real tmp51;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp50 = tmp46 + tmp49;
	       tmp56 = tmp52 - tmp55;
	       tmp45 = c_re(W[2]);
	       tmp51 = c_im(W[2]);
	       X[3 * iostride] = (tmp45 * tmp50) + (tmp51 * tmp56);
	       Y[-2 * iostride] = (tmp45 * tmp56) - (tmp51 * tmp50);
	  }
	  {
	       fftw_real tmp60;
	       fftw_real tmp66;
	       fftw_real tmp64;
	       fftw_real tmp68;
	       fftw_real tmp58;
	       fftw_real tmp63;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp58 = tmp46 - (K500000000 * tmp49);
	       tmp60 = tmp58 - tmp59;
	       tmp66 = tmp58 + tmp59;
	       tmp63 = tmp52 + (K500000000 * tmp55);
	       tmp64 = tmp62 + tmp63;
	       tmp68 = tmp63 - tmp62;
	       {
		    fftw_real tmp57;
		    fftw_real tmp61;
		    fftw_real tmp65;
		    fftw_real tmp67;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp57 = c_re(W[0]);
		    tmp61 = c_im(W[0]);
		    X[iostride] = (tmp57 * tmp60) + (tmp61 * tmp64);
		    Y[-4 * iostride] = (tmp57 * tmp64) - (tmp61 * tmp60);
		    tmp65 = c_re(W[4]);
		    tmp67 = c_im(W[4]);
		    X[5 * iostride] = (tmp65 * tmp66) + (tmp67 * tmp68);
		    Y[0] = (tmp65 * tmp68) - (tmp67 * tmp66);
	       }
	  }
     }
     if (i == m) {
	  fftw_real tmp1;
	  fftw_real tmp6;
	  fftw_real tmp4;
	  fftw_real tmp5;
	  fftw_real tmp9;
	  fftw_real tmp11;
	  fftw_real tmp12;
	  fftw_real tmp10;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = X[iostride];
	  tmp6 = Y[-iostride];
	  {
	       fftw_real tmp2;
	       fftw_real tmp3;
	       fftw_real tmp7;
	       fftw_real tmp8;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp2 = X[2 * iostride];
	       tmp3 = X[0];
	       tmp4 = tmp2 + tmp3;
	       tmp5 = K1_732050807 * (tmp2 - tmp3);
	       tmp7 = Y[-2 * iostride];
	       tmp8 = Y[0];
	       tmp9 = tmp7 + tmp8;
	       tmp11 = K1_732050807 * (tmp7 - tmp8);
	  }
	  X[0] = K2_000000000 * (tmp1 + tmp4);
	  tmp12 = (K2_000000000 * tmp1) - tmp4;
	  X[2 * iostride] = tmp11 - tmp12;
	  X[4 * iostride] = tmp12 + tmp11;
	  X[3 * iostride] = K2_000000000 * (tmp6 - tmp9);
	  tmp10 = (K2_000000000 * tmp6) + tmp9;
	  X[iostride] = -(tmp5 + tmp10);
	  X[5 * iostride] = tmp5 - tmp10;
     }
}

static const int twiddle_order[] =
{1, 2, 3, 4, 5};
fftw_codelet_desc fftw_hc2hc_backward_6_desc =
{
     "fftw_hc2hc_backward_6",
     (void (*)()) fftw_hc2hc_backward_6,
     6,
     FFTW_BACKWARD,
     FFTW_HC2HC,
     146,
     5,
     twiddle_order,
};


/* interpolation/spline.c
 *
 * Copyright (C) 2001, 2007 Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#include <config.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>

gsl_spline *
gsl_spline_alloc (const gsl_interp_type * T, size_t size)
{
  gsl_spline * spline = (gsl_spline *) malloc (sizeof(gsl_spline));

  if (spline == NULL)
    {
      GSL_ERROR_NULL ("failed to allocate space for spline struct",
                      GSL_ENOMEM);
    }

  spline->interp = gsl_interp_alloc (T, size);

  if (spline->interp == NULL)
    {
      free (spline);
      GSL_ERROR_NULL ("failed to allocate space for interp", GSL_ENOMEM);
    };

  spline->x = (double *) malloc (size * sizeof(double));

  if (spline->x == NULL)
    {
      gsl_interp_free(spline->interp);
      free(spline);
      GSL_ERROR_NULL ("failed to allocate space for x", GSL_ENOMEM);
    }

  spline->y = (double *) malloc (size * sizeof(double));

  if (spline->y == NULL)
    {
      free(spline->x);
      gsl_interp_free(spline->interp);
      free(spline);
      GSL_ERROR_NULL ("failed to allocate space for y", GSL_ENOMEM);
    }

  spline->size = size;

  return spline;
}

int
gsl_spline_init (gsl_spline * spline, const double x_array[], const double y_array[], size_t size)
{
  if (size != spline->size)
    {
      GSL_ERROR ("data must match size of spline object", GSL_EINVAL);
    }

  memcpy (spline->x, x_array, size * sizeof(double));
  memcpy (spline->y, y_array, size * sizeof(double));

  {
    int status = gsl_interp_init (spline->interp, x_array, y_array, size);
    return status;
  }
}

const char *
gsl_spline_name(const gsl_spline * spline)
{
  return gsl_interp_name(spline->interp);
}

unsigned int
gsl_spline_min_size(const gsl_spline * spline)
{
  return gsl_interp_min_size(spline->interp);
}

void
gsl_spline_free (gsl_spline * spline)
{
  RETURN_IF_NULL (spline);
  gsl_interp_free (spline->interp);
  free (spline->x);
  free (spline->y);
  free (spline);
}

int
gsl_spline_eval_e (const gsl_spline * spline,
                   double x,
                   gsl_interp_accel * a, double *y)
{
  return gsl_interp_eval_e (spline->interp,
                            spline->x, spline->y,
                            x, a, y);
}

double
gsl_spline_eval (const gsl_spline * spline,
                 double x,
                 gsl_interp_accel * a)
{
  return gsl_interp_eval (spline->interp,
                          spline->x, spline->y,
                          x, a);
}

int
gsl_spline_eval_deriv_e (const gsl_spline * spline,
                         double x,
                         gsl_interp_accel * a,
                         double *dydx)
{
  return gsl_interp_eval_deriv_e (spline->interp,
                                  spline->x, spline->y,
                                  x, a, dydx);
}

double
gsl_spline_eval_deriv (const gsl_spline * spline,
                       double x,
                       gsl_interp_accel * a)
{
  return gsl_interp_eval_deriv (spline->interp,
                                spline->x, spline->y,
                                x, a);
}

int
gsl_spline_eval_deriv2_e (const gsl_spline * spline,
                          double x,
                          gsl_interp_accel * a,
                          double * d2)
{
  return gsl_interp_eval_deriv2_e (spline->interp,
                                   spline->x, spline->y,
                                   x, a, d2);
}

double
gsl_spline_eval_deriv2 (const gsl_spline * spline,
                        double x,
                        gsl_interp_accel * a)
{
  return gsl_interp_eval_deriv2 (spline->interp,
                                 spline->x, spline->y,
                                 x, a);
}

int
gsl_spline_eval_integ_e (const gsl_spline * spline,
                         double a, double b,
                         gsl_interp_accel * acc,
                         double * result)
{
  return gsl_interp_eval_integ_e (spline->interp,
                                  spline->x, spline->y,
                                  a, b, acc, result);
}

double
gsl_spline_eval_integ (const gsl_spline * spline,
                       double a, double b,
                       gsl_interp_accel * acc)
{
  return gsl_interp_eval_integ (spline->interp,
                                spline->x, spline->y,
                                a, b, acc);
}


// directdebye.c
// Computes the Debye sum from raytracing data.
// See man page for usage information.
#include <assert.h>
#include <complex.h>
#include <errno.h>
#include <error.h>
#include <gsl/gsl_const_mksa.h>
#include <malloc.h>
#include <math.h>
#include <matheval.h>
#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>

#include "hdfio.h"
#include "miscprint.h"
#include "parseargs.h"
#include "rays.h"

int main(int argc, char **argv) {
  /* MPI initialization */
  int rank, nprocs;
  MPI_Init(&argc, &argv);
  MPI_Comm_size(MPI_COMM_WORLD, &nprocs);
  MPI_Comm_rank(MPI_COMM_WORLD, &rank);

  /* parse command line arguments */
  args_t *a = malloc(sizeof(args_t));
  parse_args(argc, argv, a);

  /* read the rays */
  unsigned int nrays; /* number of rays */
  ray_t *ray = NULL;
  ray = readrays(a->filename_in, &nrays);
  if (ray == NULL) {
    error_at_line(-1, errno, __FILE__, __LINE__, "Problem reading ray file.\n");
  }

  /* pulse function */
  void *pfcn_intensity = NULL;
  pfcn_intensity = evaluator_create(a->pfcn_intensity_buffer);
  if (pfcn_intensity == NULL) {
    error_at_line(-1, EINVAL, __FILE__, __LINE__,
                  "Problem creating pulse evaluator function.");
  }

  /* apodization function */
  void *pfcn_g = NULL;
  pfcn_g = evaluator_create(a->pfcn_g_buffer);
  if (pfcn_g == NULL) {
    error_at_line(-1, EINVAL, __FILE__, __LINE__,
                  "Problem creating apodization evaluator function.");
  }

  /* root node prints some useful information */
  if (rank == 0) {
    print_args(a, nrays);
  }

  /* Before allocating what could potentially be a lot of memory, check to
   * see if it's even possible.  */
  unsigned int i;
  a->efield_ss_out = 1;
  for (i = 0; i < 4; ++i) {
    a->efield_ss_out *= a->dim_efield_out[i];
  }

  long page_size = sysconf(_SC_PAGE_SIZE);
  // long pages_avail = sysconf(_SC_AVPHYS_PAGES);
  long pages_tot = sysconf(_SC_PHYS_PAGES);
  if (pages_tot * page_size < 8 * a->efield_ss_out * sizeof(complex double)) {
    error_at_line(-1, ENOMEM, __FILE__, __LINE__,
                  "Size of output arrays exceed avaliable memory.\n");
  }

  /* Allocate memory for the fields.  The _rcv is memory for nodes. */
  complex double *efield_x = NULL;
  complex double *efield_x_rcv = NULL;
  complex double *efield_y = NULL;
  complex double *efield_y_rcv = NULL;
  complex double *efield_z = NULL;
  complex double *efield_z_rcv = NULL;

  double *efield_r = NULL;
  double *efield_r_rcv = NULL;

  /* The root node will be receiving all of the data, so it gets the whole
   * thing.  Calloc here to pre-zero out everything. */
  if (rank == 0) {
    efield_x =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_y =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_z =
        (complex double *)calloc(a->efield_ss_out, sizeof(complex double));
    efield_r = (double *)calloc(4 * (a->efield_ss_out), sizeof(double));
  }

  /* The root node and all the other nodes also get this little chunk to
   * work on.  The root node really has nothing better to do while waiting
   * for information so there's no real waste. */
  efield_x_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_y_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_z_rcv = (complex double *)calloc(a->efield_ss_out / nprocs,
                                          sizeof(complex double));
  efield_r_rcv =
      (double *)calloc(4 * (a->efield_ss_out / nprocs), sizeof(double));

  /* Catch some possible malloc errors*/
  if (errno) {
    fprintf(stderr, "Node %d\n", rank);
    error_at_line(-1, errno, __FILE__, __LINE__, "Error allocating memory.");
  }

  unsigned int n;
  double w;
  /* precompute some ray information */
  for (n = 0; n < nrays; ++n, ++ray) {
    w = 2 * M_PI * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000 / ray->lambda;
    /* ray intensity for gaussian pulse */
    ray->intensity *= evaluator_evaluate_x(pfcn_intensity, w);
    if (isnan(ray->intensity)) {
      error_at_line(-1, errno, __FILE__, __LINE__,
                    "Error with intensity function: got nan.");
    }

    /* analytic apodization factor for parabola */
    ray->g *= evaluator_evaluate_x(pfcn_g, acos(ray->p0[2] / a->fs_radius));
    if (isnan(ray->g)) {
      error_at_line(-1, errno, __FILE__, __LINE__,
                    "Error with apodization function: got nan.");
    }

    /* all of the intensity information is stored in ray->g in order to
     * save a few multiplications in the inner loop */
    ray->g =
        (1.0 / (a->fs_radius * ray->lambda * 0.001)) * ray->g * ray->intensity;

    ray->e0[0] /= ray->lambda;
    ray->e0[1] /= ray->lambda;
    ray->e0[2] /= ray->lambda;
  }
  ray -= nrays;

  /* precompute the position vectors in the efield we'll be dealing with */
  if (rank == 0) {
    double dr[4];

    /* get the spacings of the electric field */
    for (i = 0; i < 4; ++i) {
      if (a->dim_efield_out[i] == 1) {
        dr[i] = (a->dim_efield_ub[i] - a->dim_efield_lb[i]) /
                ((double)a->dim_efield_out[i]);
      } else {
        dr[i] = (a->dim_efield_ub[i] - a->dim_efield_lb[i]) /
                ((double)a->dim_efield_out[i] - 1);
      }
    }

    /* set them */
    unsigned int r_i[4] = {};
    for (r_i[0] = 0; r_i[0] < a->dim_efield_out[0]; r_i[0]++) {
      for (r_i[1] = 0; r_i[1] < a->dim_efield_out[1]; r_i[1]++) {
        for (r_i[2] = 0; r_i[2] < a->dim_efield_out[2]; r_i[2]++) {
          for (r_i[3] = 0; r_i[3] < a->dim_efield_out[3]; r_i[3]++) {
            *efield_r++ = a->dim_efield_lb[0] + dr[0] * (double)r_i[0];
            *efield_r++ = a->dim_efield_lb[1] + dr[1] * (double)r_i[1];
            *efield_r++ = a->dim_efield_lb[2] + dr[2] * (double)r_i[2];
            *efield_r++ = a->dim_efield_lb[3] + dr[3] * (double)r_i[3];
          }
        }
      }
    }
    efield_r -= 4 * a->efield_ss_out;
  }

  /* send this spacing information out to everyone */
  MPI_Scatter(efield_r, 4 * (a->efield_ss_out / nprocs), MPI_DOUBLE,
              efield_r_rcv, 4 * (a->efield_ss_out / nprocs), MPI_DOUBLE, 0,
              MPI_COMM_WORLD);

  /* Main loop over ray data.  This should be fast as possible. */
  /* pre-computing the three efield_r_rcv lines doesn't appear to have a *
   * significant effect on performance */
  complex double tmp;
  for (i = 0; i < a->efield_ss_out / nprocs; ++i) {
    for (n = 0; n < nrays; ++n, ++ray) {
      tmp = -1.0i * ray->g *
            cexp(2.0i * M_PI *
                 ((*(efield_r_rcv + 0) - ray->p0[0]) * ray->e0[0] +
                  (*(efield_r_rcv + 1) - ray->p0[1]) * ray->e0[1] +
                  (*(efield_r_rcv + 2) - ray->p0[2]) * ray->e0[2] - ray->OPL -
                  *(efield_r_rcv + 3) * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000.0 /
                      ray->lambda +
                  a->temporal_offset * GSL_CONST_MKSA_SPEED_OF_LIGHT * 1000.0 /
                      ray->lambda));
      /* three polarization components */
      efield_x_rcv[i] += ray->pol[0] * tmp;
      efield_y_rcv[i] += ray->pol[1] * tmp;
      efield_z_rcv[i] += ray->pol[2] * tmp;
    }
    ray -= nrays;
    efield_r_rcv += 4;
  }
  efield_r_rcv -= 4 * i;

  /* gather all data from nodes */
  MPI_Gather(efield_x_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_x, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);
  MPI_Gather(efield_y_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_y, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);
  MPI_Gather(efield_z_rcv, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX,
             efield_z, a->efield_ss_out / nprocs, MPI_DOUBLE_COMPLEX, 0,
             MPI_COMM_WORLD);

  /* write electric field information to HDF file */
  if (rank == 0) {
    write_efield(a, efield_x, efield_y, efield_z);
  }

  /* clean up bit */
  free(efield_x);
  free(efield_y);
  free(efield_z);
  free(a);

  free(efield_x_rcv);
  free(efield_y_rcv);
  free(efield_z_rcv);
  free(efield_r_rcv);
  if (rank == 0) free(efield_r);
  free(ray);
  evaluator_destroy(pfcn_intensity);
  evaluator_destroy(pfcn_g);
  MPI_Finalize();
  return 0;
}


/*
 *  vbHmmPcFret.c
 *  Model-specific core functions for VB-HMM-PC-FRET.
 *
 *  Created by OKAMOTO Kenji, SAKO Yasushi and RIKEN
 *  Copyright 2011-2015
 *  Cellular Informatics Laboratory, Advance Science Institute, RIKEN, Japan.
 *  All rights reserved.
 *
 *  Ver. 1.1.0
 *  Last modified on 2015.09.17
 */

#include "vbHmmPcFret.h"
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_psi.h>
#include <string.h>
#include "rand.h"

#ifdef _OPENMP
#include "omp.h"
#endif

//#define  DEBUG

//// Uncomment one/both of the following defenitions to activate constraint on I and K.
//#define  INTENSITY_CAP
#ifdef INTENSITY_CAP
#define  maxIntensityRatio  10.0
#endif

#define  MAX(a,b)  ((a)>(b)?(a):(b))
#define  MIN(a,b)  ((a)<(b)?(a):(b))

//static int isGlobalAnalysis = 0;

void setFunctions_pcFret(){
    commonFunctions funcs;
    funcs.newModelParameters    = newModelParameters_pcFret;
    funcs.freeModelParameters   = freeModelParameters_pcFret;
    funcs.newModelStats         = newModelStats_pcFret;
    funcs.freeModelStats        = freeModelStats_pcFret;
    funcs.initializeVbHmm       = initializeVbHmm_pcFret;
    funcs.pTilde_z1             = pTilde_z1_pcFret;
    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pcFret;
    funcs.pTilde_xn_zn          = pTilde_xn_zn_pcFret;
    funcs.calcStatsVars         = calcStatsVars_pcFret;
    funcs.maximization          = maximization_pcFret;
    funcs.varLowerBound         = varLowerBound_pcFret;
    funcs.reorderParameters     = reorderParameters_pcFret;
    funcs.outputResults         = outputResults_pcFret;
    setFunctions( funcs );
}

//void setGFunctions_pcFret(){
//    gCommonFunctions funcs;
//    funcs.newModelParameters    = newModelParameters_pcFret;
//    funcs.freeModelParameters   = freeModelParameters_pcFret;
//    funcs.newModelStats         = newModelStats_pcFret;
//    funcs.freeModelStats        = freeModelStats_pcFret;
//    funcs.newModelStatsG        = newModelStatsG_pcFret;
//    funcs.freeModelStatsG       = freeModelStatsG_pcFret;
//    funcs.initializeVbHmmG      = initializeVbHmmG_pcFret;
//    funcs.pTilde_z1             = pTilde_z1_pcFret;
//    funcs.pTilde_zn_zn1         = pTilde_zn_zn1_pcFret;
//    funcs.pTilde_xn_zn          = pTilde_xn_zn_pcFret;
//    funcs.calcStatsVarsG        = calcStatsVarsG_pcFret;
//    funcs.maximizationG         = maximizationG_pcFret;
//    funcs.varLowerBoundG        = varLowerBoundG_pcFret;
//    funcs.reorderParametersG    = reorderParametersG_pcFret;
//    funcs.outputResultsG        = outputResultsG_pcFret;
//    setGFunctions( funcs );
//    isGlobalAnalysis = 1;
//}

void outputResults_pcFret( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    outputPcFretResults( xn, gv, iv, logFP );
}

//void outputResultsG_pcFret( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//    outputPcFretResultsG( xns, gv, ivs, logFP );
//}

void *newModelParameters_pcFret( xn, sNo )
xnDataSet *xn;
int sNo;
{
    int i;
    pcFretParameters *p = (void*)malloc( sizeof(pcFretParameters) );

    p->uPiArr = (double*)malloc( sNo * sizeof(double) );
    p->sumUPi = 0.0;
    p->uAMat = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->uAMat[i] = (double*)malloc( sNo * sizeof(double) );
    }
    p->sumUAArr = (double*)malloc( sNo * sizeof(double) );

    p->aIArr = (double*)malloc( sNo * sizeof(double) );
    p->bIArr = (double*)malloc( sNo * sizeof(double) );
    p->uEArr = (double*)malloc( sNo * sizeof(double) );
    p->vEArr = (double*)malloc( sNo * sizeof(double) );

    p->avgPi = (double *)malloc( sNo * sizeof(double) );
    p->avgLnPi = (double *)malloc( sNo * sizeof(double) );
    p->avgA = (double **)malloc( sNo * sizeof(double*) );
    p->avgLnA = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ ){
        p->avgA[i] = (double *)malloc( sNo * sizeof(double) );
        p->avgLnA[i] = (double *)malloc( sNo * sizeof(double) );
    }

    p->avgI = (double *)malloc( sNo * sizeof(double) );
    p->avgLnI = (double *)malloc( sNo * sizeof(double) );
    p->avgE = (double *)malloc( sNo * sizeof(double) );
    p->avgLnE = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   p->avgLnE[i] = (double *)malloc( 2 * sizeof(double) );   }

    return p;
}

void freeModelParameters_pcFret( p, xn, sNo )
void **p;
xnDataSet *xn;
int sNo;
{
    pcFretParameters *gp = *p;
    int i;

    free( gp->uPiArr );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->uAMat[i] );
    }
    free( gp->uAMat );
    free( gp->sumUAArr );

    free( gp->aIArr );
    free( gp->bIArr );
    free( gp->uEArr );
    free( gp->vEArr );

    free( gp->avgPi );
    free( gp->avgLnPi );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgA[i] );
        free( gp->avgLnA[i] );
    }
    free( gp->avgA );
    free( gp->avgLnA );

    free( gp->avgI );
    free( gp->avgLnI );
    free( gp->avgE );
    for( i = 0 ; i < sNo ; i++ ){
        free( gp->avgLnE[i] );
    }
    free( gp->avgLnE );

    free( *p );
    *p = NULL;
}

void *newModelStats_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
    int sNo = gv->sNo;
    pcFretStats *s = (pcFretStats*)malloc( sizeof(pcFretStats) );

    int i;
    s->Ni = (double *)malloc( sNo * sizeof(double) );
    s->Ci = (double *)malloc( sNo * sizeof(double) );
    s->Di = (double *)malloc( sNo * sizeof(double) );
    s->Ai = (double *)malloc( sNo * sizeof(double) );
    s->Mi = (double *)malloc( sNo * sizeof(double) );
    s->Nij = (double **)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s->Nij[i] = (double *)malloc( sNo * sizeof(double) );   }

    return s;

//    } else {
//
//        return NULL;
//
//    }
}

void freeModelStats_pcFret( s, xn, gv, iv )
void **s;
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
//    if( isGlobalAnalysis == 0 ){
    int sNo = gv->sNo;
    pcFretStats *gs = *s;
    int i;
    free( gs->Ni );
    free( gs->Ci );
    free( gs->Di );
    free( gs->Ai );
    free( gs->Mi );
    for( i = 0 ; i < sNo ; i++ )
    {   free( gs->Nij[i] );   }
    free( gs->Nij );

    free( gs );
    *s = NULL;
//    }
}

//void *newModelStatsG_pcFret( xns, gv, ivs)
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcFretGlobalStats *gs = (pcFretGlobalStats*)malloc( sizeof(pcFretGlobalStats) );
//
//    return gs;
//}

//void freeModelStatsG_pcFret( gs, xns, gv, ivs )
//void **gs;
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//    int sNo = gv->sNo;
//    pcFretGlobalStats *ggs = *gs;
//
//    free( *gs );
//    *gs = NULL;
//}

void initializeVbHmm_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretData *d = xn->data;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    pcFretParameters *p = gv->params;

    int i, j;
    size_t totalC = 0;
    for( i = 0 ; i < dLen ; i++ ){
        totalC += d->dCounts[i] + d->aCounts[i];
    }
    double meanI = (double)totalC / (double)dLen;

    // hyper parameter for p( pi(i) )
    p->sumUPi = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        p->uPiArr[i] = 1.0;
        p->sumUPi += p->uPiArr[i];
    }

    // hyper parameter for p( A(i,j) )
    for( i = 0 ; i < sNo ; i++ ){
        p->sumUAArr[i] = 0.0;
        for( j = 0 ; j < sNo ; j++ ){
            if( j == i ){
                p->uAMat[i][j] = 100.0;
            } else {
                p->uAMat[i][j] = 1.0;
            }
            p->sumUAArr[i] += p->uAMat[i][j];
        }
    }

    // hyper parameter for p( I(k) )
    for( i = 0 ; i < sNo ; i++ ){
        p->aIArr[i] = 1.0;
        p->bIArr[i] = 1.0 / meanI;
    }

    // hyper parameter for p( E(i) )
    for( i = 0 ; i < sNo ; i++ ){
        p->uEArr[i] = 1.0;
        p->vEArr[i] = 1.0;
    }

    initialize_indVars_pcFret( xn, gv, iv );

    calcStatsVars_pcFret( xn, gv, iv );
    maximization_pcFret( xn, gv, iv );
}

//void initializeVbHmmG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void initialize_indVars_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;

    int i;
    size_t n;
    double sumPar;
    for( n = 0 ; n < dLen ; n++ ){
        sumPar = 0.0;
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] = enoise(1.0) + 1.0;
            sumPar += gmMat[n][i];
        }
        for( i = 0 ; i < sNo ; i++ ){
            gmMat[n][i] /= sumPar;
        }
    }
}

xnDataSet *newXnDataSet_pcFret( filename )
const char *filename;
{
    xnDataSet *xn = (xnDataSet*)malloc( sizeof(xnDataSet) );
    xn->name = (char*)malloc( strlen(filename) + 2 );
    strncpy( xn->name, filename, strlen(filename)+1 );
    xn->data = (pcFretData*)malloc( sizeof(pcFretData) );
    pcFretData *d = (pcFretData*)xn->data;
    d->binSize = 0.0;
    d->dCounts = NULL;
    d->aCounts = NULL;
    return xn;
}

void freeXnDataSet_pcFret( xn )
xnDataSet **xn;
{
    pcFretData *d = (pcFretData*)(*xn)->data;
    free( d->dCounts );
    free( d->aCounts );
    free( (*xn)->data );
    free( (*xn)->name );
    free( *xn );
    *xn = NULL;
}

double pTilde_z1_pcFret( i, params )
int i;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    return exp( p->avgLnPi[i] );
}

double pTilde_zn_zn1_pcFret( i, j, params )
int i, j;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    return exp( p->avgLnA[i][j] );
}

double pTilde_xn_zn_pcFret( xn, n, i, params )
xnDataSet *xn;
size_t n;
int i;
void *params;
{
    pcFretParameters *p = (pcFretParameters*)params;
    pcFretData *d = (pcFretData*)xn->data;
    return exp( d->dCounts[n]*p->avgLnE[i][0] + d->aCounts[n]*p->avgLnE[i][1] + (d->dCounts[n]+d->aCounts[n])*p->avgLnI[i] - p->avgI[i] ) / gsl_sf_fact( d->dCounts[n] ) / gsl_sf_fact( d->aCounts[n] );
}

void calcStatsVars_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretData *d = (pcFretData*)xn->data;
    pcFretStats *s = (pcFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **Nij = s->Nij;
    size_t n;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        Ni[i] = 1e-10;
        Ci[i] = 1e-10;
        Di[i] = 1e-10;
        Ai[i] = 1e-10;
        Mi[i] = 1e-10;
        for( j = 0 ; j < sNo ; j++ ){
            Nij[i][j] = 1e-10;
        }

        for( n = 0 ; n < dLen ; n++ ){
            Ni[i]  += gmMat[n][i];
            Di[i]  += gmMat[n][i] * (double)d->dCounts[n];
            Ai[i]  += gmMat[n][i] * (double)d->aCounts[n];
            for( j = 0 ; j < sNo ; j++ ){
                Mi[i]     += xiMat[n][i][j];
                Nij[i][j] += xiMat[n][i][j];
            }
        }
        Ci[i] = Di[i] + Ai[i];
    }

//#ifdef DEBUG
//#pragma omp critical
//{
//    for( n = 0 ; n < 20 ; n++ ){
//        for( i = 0 ; i < sNo ; i++ ){
//            fprintf(logFP, "%g,", gmMat[n][i]);
//        }
//        fprintf(logFP, "; ");
//    }
//    fprintf(logFP, "\n");
//    for( i = 0 ; i < sNo ; i++ ){
//        fprintf(logFP, "Ni(%d)=%g,  ", i, Ni[i]);
//        fprintf(logFP, "Ti(%d)=%g,  ", i, Ti[i]);
//        fprintf(logFP, "Mi(%d)=%g,  ", i, Mi[i]);
//        for( j = 0 ; j < sNo ; j++ ){
//            if( j != i )
//                fprintf(logFP, "Nij(%d,%d)=%g, ", i, j, Nij[i][j]);
//        }
//        fprintf(logFP, "\n");
//    }
//}
//#endif
}

//void calcStatsVarsG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void maximization_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)iv->stats;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA, **avgLnA = p->avgLnA;
    double *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *avgI = p->avgI, *avgLnI = p->avgLnI;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **Nij = s->Nij;
    int i, j;

    for( i = 0 ; i < sNo ; i++ ){
        avgPi[i] = ( uPiArr[i] + gmMat[0][i] ) / ( sumUPi + 1.0 );
        avgLnPi[i] = gsl_sf_psi( uPiArr[i] + gmMat[0][i] ) - gsl_sf_psi( sumUPi + 1.0 );

        for( j = 0 ; j < sNo ; j++ ){
            avgA[i][j] = ( uAMat[i][j] + Nij[i][j] ) / ( sumUAArr[i] + Mi[i] );
            avgLnA[i][j] = gsl_sf_psi( uAMat[i][j] + Nij[i][j] ) - gsl_sf_psi( sumUAArr[i] + Mi[i] );
        }

        avgI[i] = (Ci[i] + aIArr[i]) / (Ni[i] + bIArr[i]);
        avgLnI[i] = gsl_sf_psi( Ci[i] + aIArr[i] ) - log( Ni[i] + bIArr[i] );
#ifdef  INTENSITY_CAP
        size_t n, totalC = 0;
        pcFretData *pc = xnWv->data;
        for( n = 0 ; n < xnWv->N ; n++ ){
            totalC += pc->dCounts[n] + pc->aCounts[n];
        }
        double meanI = (double)totalC / (double)xnWv->N;
        avgI[i] = MIN( avgI[i], maxIntensityRatio * meanI );
        avgLnI[i] = MIN( avgLnI[i], log(maxIntensityRatio * meanI) );
#endif

        avgE[i] = ( uEArr[i] + Ai[i] ) / ( uEArr[i] + vEArr[i] + Ci[i] );
        // ln(1-E) for donor
        avgLnE[i][0]  = gsl_sf_psi( vEArr[i] + Di[i] ) - gsl_sf_psi( uEArr[i] + vEArr[i] + Ci[i] );
        // ln(E) for acceptor
        avgLnE[i][1]  = gsl_sf_psi( uEArr[i] + Ai[i] ) - gsl_sf_psi( uEArr[i] + vEArr[i] + Ci[i] );
    }
}

//void maximizationG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

double varLowerBound_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, *cn = iv->cn;
    double *uPiArr = p->uPiArr, sumUPi = p->sumUPi, *aIArr = p->aIArr, *bIArr = p->bIArr;
    double **uAMat = p->uAMat, *sumUAArr = p->sumUAArr;
    double *uEArr = p->uEArr, *vEArr = p->vEArr;
    double *avgLnPi = p->avgLnPi, **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI, **avgLnE = p->avgLnE;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    double *Mi = s->Mi, **Nij = s->Nij;
    size_t n;
    int i, j;

    double lnpPi = gsl_sf_lngamma(sumUPi);
    double lnpA = 0.0;
    double lnpI = 0.0;
    double lnpE = 0.0;
    double lnqPi = gsl_sf_lngamma(sumUPi + 1.0);
    double lnqA = 0.0;
    double lnqI = 0.0;
    double lnqE = 0.0;
    for( i = 0 ; i < sNo ; i++ ){
        lnpPi += (uPiArr[i]-1.0) * avgLnPi[i] - gsl_sf_lngamma(uPiArr[i]);

        lnpI += aIArr[i] * log(bIArr[i]) - gsl_sf_lngamma(aIArr[i]);
        lnpI += (aIArr[i] - 1.0) * avgLnI[i] - bIArr[i] * avgI[i];

        lnpE += gsl_sf_lngamma(uEArr[i]+vEArr[i]) - gsl_sf_lngamma(uEArr[i]);
        lnpE += -gsl_sf_lngamma(vEArr[i]);
        lnpE += (uEArr[i]-1.0)*avgLnE[i][1] + (vEArr[i]-1.0)*avgLnE[i][0];

        lnqPi += (uPiArr[i]+gmMat[0][i]-1.0) * (gsl_sf_psi(uPiArr[i]+gmMat[0][i]) - gsl_sf_psi(sumUPi+1.0));
        lnqPi -= gsl_sf_lngamma(uPiArr[i] + gmMat[0][i]);

        lnqI += (Ci[i] + aIArr[i]) * log(Ni[i] + bIArr[i]) - gsl_sf_lngamma(Ci[i] + aIArr[i]);
        lnqI += (Ci[i] + aIArr[i] - 1.0) * avgLnI[i] - (Ni[i] + bIArr[i]) * avgI[i];

        lnqE += gsl_sf_lngamma(uEArr[i] + vEArr[i] + Ci[i]);
        lnqE -= gsl_sf_lngamma(uEArr[i] + Ai[i]);
        lnqE -= gsl_sf_lngamma(vEArr[i] + Di[i]);
        lnqE += (uEArr[i] + Ai[i] - 1.0) * avgLnE[i][1];
        lnqE += (vEArr[i] + Di[i] - 1.0) * avgLnE[i][0];

        lnpA += gsl_sf_lngamma(sumUAArr[i]);
        lnqA += gsl_sf_lngamma(sumUAArr[i] + Mi[i]);
        for( j = 0 ; j < sNo ; j++ ){
            lnpA += (uAMat[i][j]-1.0)*avgLnA[i][j] - gsl_sf_lngamma(uAMat[i][j]);

            lnqA += (uAMat[i][j] + Nij[i][j] - 1.0) * (gsl_sf_psi(uAMat[i][j]+Nij[i][j]) - gsl_sf_psi(sumUAArr[i]+Mi[i]));
            lnqA -= gsl_sf_lngamma( uAMat[i][j] + Nij[i][j] );
        }
    }

    double lnpX = 0.0;
    for( n = 0 ; n < dLen ; n++ ){
        lnpX += log( cn[n] );
    }

    double val;
    val  = lnpPi + lnpA + lnpI + lnpE;
    val -= lnqPi + lnqA + lnqI + lnqE;
    val += lnpX;
    val += log(gsl_sf_fact(sNo));

//#ifdef DEBUG
//#pragma omp critical
//{
//    FILE *logFP = stderr;
//    if( val > 100000 ){
//        fprintf(logFP, "  > %g; %g; %g; %g;", lnpPi, lnpA, lnpI, lnpE);
//        fprintf(logFP, " %g; %g; %g; %g; %g\n", lnqPi, lnqA, lnqI, lnqE, lnpX);
//    }
//}
//#endif

    return val;
}

//double varLowerBoundG_pcFret( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void reorderParameters_pcFret( xn, gv, iv )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    pcFretStats *s = (pcFretStats*)iv->stats;
    size_t dLen = xn->N;
    int sNo = gv->sNo;
    double **gmMat = iv->gmMat, ***xiMat = iv->xiMat;
    double *avgPi = p->avgPi, *avgLnPi = p->avgLnPi, **avgA = p->avgA;
    double **avgLnA = p->avgLnA;
    double *avgI = p->avgI, *avgLnI = p->avgLnI, *avgE = p->avgE, **avgLnE = p->avgLnE;
    double *Ni = s->Ni, *Ci = s->Ci, *Di = s->Di, *Ai = s->Ai;
    size_t n;
    int i, j;

    int *index = (int*)malloc( sNo * sizeof(int) );
    double *store = (double*)malloc( sNo * sizeof(double) );
    double **s2D = (double**)malloc( sNo * sizeof(double*) );
    for( i = 0 ; i < sNo ; i++ )
    {   s2D[i] = (double*)malloc( MAX(sNo,2) * sizeof(double) );   }

    // index indicates order of avgE values (0=biggest avgE -- sNo=smallest avgE).
    for( i = 0 ; i < sNo ; i++ ){
        index[i] = sNo - 1;
        for( j = 0 ; j < sNo ; j++ ){
            if( j != i ){
                if( avgE[i] < avgE[j] ){
                    index[i]--;
                } else if( avgE[i] == avgE[j] ){
                    if( j > i )
                    {   index[i]--;   }
                }
            }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnPi[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnPi[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgI[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgLnI[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgLnI[i] = store[i];   }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgA[i][j] = s2D[i][j];   }
    }

    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = avgLnA[i][j];   }
    }
    for( j = 0 ; j < sNo ; j++ ){
        for( i = 0 ; i < sNo ; i++ ){   avgLnA[i][j] = s2D[i][j];   }
    }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = avgE[i];   }
    for( i = 0 ; i < sNo ; i++ ){   avgE[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ )
    {   s2D[index[i]][0] = avgLnE[i][0];
        s2D[index[i]][1] = avgLnE[i][1];   }
    for( i = 0 ; i < sNo ; i++ )
    {   avgLnE[i][0] = s2D[i][0];
        avgLnE[i][1] = s2D[i][1];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ni[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ni[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ci[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ci[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Di[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Di[i] = store[i];   }

    for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = Ai[i];   }
    for( i = 0 ; i < sNo ; i++ ){   Ai[i] = store[i];   }

    for( n = 0 ; n < dLen ; n++ ){
        for( i = 0 ; i < sNo ; i++ ){   store[index[i]] = gmMat[n][i];   }
        for( i = 0 ; i < sNo ; i++ ){   gmMat[n][i] = store[i];   }
    }

    for( n = 0 ; n < dLen ; n++ ){
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   s2D[index[i]][index[j]] = xiMat[n][i][j];   }
        }
        for( j = 0 ; j < sNo ; j++ ){
            for( i = 0 ; i < sNo ; i++ ){   xiMat[n][i][j] = s2D[i][j];   }
        }
    }

    for( i = 0 ; i < sNo ; i++ ){   free( s2D[i] );   }
    free( s2D );
    free( store );
    free( index );
}

//void reorderParametersG_pc( xns, gv, ivs )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//{
//}

void outputPcFretResults( xn, gv, iv, logFP )
xnDataSet *xn;
globalVars *gv;
indVars *iv;
FILE *logFP;
{
    pcFretParameters *p = (pcFretParameters*)gv->params;
    int sNo = gv->sNo;
    int i, j;
    fprintf(logFP, "  results: K = %d \n", sNo);

    fprintf(logFP, "   intensities: ( %g", p->avgI[0]);
    for( i = 1 ; i < sNo ; i++ )
    {   fprintf(logFP, ", %g", p->avgI[i]);   }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   FRET efficiencies: ( %g", p->avgE[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgE[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   pi: ( %g", p->avgPi[0]);
    for( i = 1 ; i < sNo ; i++ ){
        fprintf(logFP, ", %g", p->avgPi[i]);
    }
    fprintf(logFP, " ) \n");

    fprintf(logFP, "   A_matrix: [");
    for( i = 0 ; i < sNo ; i++ ){
            fprintf(logFP, " ( %g", p->avgA[i][0]);
            for( j = 1 ; j < sNo ; j++ )
            {   fprintf(logFP, ", %g", p->avgA[i][j]);   }
            fprintf(logFP, ")");
    }
    fprintf(logFP, " ] \n\n");

    char fn[256];
    FILE *fp;
    size_t n;

    sprintf( fn, "%s.param%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        fprintf(fp, "I, E, pi");
        for( i = 0 ; i < sNo ; i++ )
        {   fprintf(fp, ", A%dx", i);   }
        fprintf(fp, "\n");

        for( i = 0 ; i < sNo ; i++ ){
            fprintf(fp, "%g, %g", p->avgI[i], p->avgE[i]);
            for( j = 0 ; j < sNo ; j++ )
            {   fprintf(fp, ", %g", p->avgA[j][i]);   }
            fprintf(fp, "\n");
        }
        fclose(fp);
    }

    sprintf( fn, "%s.Lq%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < gv->iteration ; n++ ){
            fprintf( fp, "%24.20e\n", gv->LqArr[n] );
        }
        fclose(fp);
    }

    sprintf( fn, "%s.maxS%03d", xn->name, sNo );
    if( (fp = fopen( fn, "w")) != NULL ){
        for( n = 0 ; n < xn->N ; n++ ){
            fprintf( fp, "%d\n", iv->stateTraj[n] );
        }
        fclose(fp);
    }
}

//void outputPcFretResultsG( xns, gv, ivs, logFP )
//xnDataBundle *xns;
//globalVars *gv;
//indVarBundle *ivs;
//FILE *logFP;
//{
//}

//