/**
 * The driver function and some auxillaries
 * to compute the spectrum from an image and
 * information on the location and distortion of the spectra.
 */

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

#include <gsl/gsl_roots.h>
#include <gsl/gsl_vector.h>

#include "aXe_grism.h"
#include "aXe_utils.h"
#include "spce_sect.h"
#include "spce_is_in.h"
#include "spce_pathlength.h"

#define DEBUG_ME 0x10

/**
  fill in the path length field (xi in ap_pixel) from the abscissas (xs in
  ap_pixel) (helper function for spc_extract).

  @param func the spectrum trace to use for the transformation from xs to xi
  @param table a pointer to the table of aperture pixels, terminated with
  x=-1
*/
static int
transform_to_pathlen (const trace_func * const func, ap_pixel * const table)
{
  int i, len;
  ap_pixel *cur_p = table;
  gsl_vector *section_points;

  while (cur_p->p_x != -1)
    cur_p++;
  len = cur_p - table;
  if (len==0) return -1; // exit now if the table is empty

  section_points = gsl_vector_alloc (len);
  for (i = 0; i < len; i++)
    {
      gsl_vector_set (section_points, i, table[i].xs);
    }

  if (abscissa_to_pathlength (func, section_points))
    {
      gsl_vector_free (section_points);
      return -1;
    }

  for (i = 0; i < len; i++)
    {
      table[i].xi = gsl_vector_get (section_points, i);
    }

  gsl_vector_free (section_points);

  return 0;
}

/**
   creates an ap_pixel.

   @param x x coordinate of pixel relative to beam's reference point
   @param y y coordinate of pixel relative to beam's reference point
   @param px absolute x coordinate of the pixel
   @param py absolute y coordinate of the pixel
   @param ob the observation to get the pixels from
   @param sf pointer to sectionfun structure for this beam
   @param cur_ap a pointer to the next free aperture pixel
   @return a pointer to the next free aperture pixel after the new pixels
    have been added.

*/
static ap_pixel *
handle_one_pixel (const double x, const double y, const int px, const int py,
		  const observation * const obs, sectionfun * const sf,
		  const trace_func * const tracefun, ap_pixel * cur_ap)
{
  double res;
  double sect_y;
  double tmp;
  double phi_trace;

  if (find_section_point (sf, x, y, &res))
    {
      return cur_ap;	/* FIXME: Issue warning here */
    }
  /* set the extraction weight to 1. */
  cur_ap->weight = 1.;
  cur_ap->xs = res;
  phi_trace = atan (tracefun->deriv (cur_ap->xs, tracefun->data));
  cur_ap->dxs = phi_trace;
  cur_ap->ys = tracefun->func (cur_ap->xs, tracefun->data);
  sect_y = tracefun->func (res, tracefun->data);
  tmp = tracefun->func (x, tracefun->data);

  cur_ap->contam = -1.0;
  cur_ap->model = 0.0;
  cur_ap->p_x = px;
  cur_ap->p_y = py;
  cur_ap->x = x;
  cur_ap->y = y;
  cur_ap->dist =
    sqrt ((sect_y - y) * (sect_y - y) +
	  (cur_ap->xs - x) * (cur_ap->xs - x));

  if ( y < tmp ) {
    cur_ap->dist = cur_ap->dist * (-1.); /* If pixel is bwlow the trace, dist is neg. */
  }
  cur_ap->count = gsl_matrix_get (obs->grism, px, py);
  cur_ap->error = gsl_matrix_get (obs->pixerrs, px, py);
  if (obs->dq != NULL) cur_ap->dq = (long) gsl_matrix_get (obs->dq, px, py);
  else cur_ap->dq = 0;

  cur_ap++;
  return cur_ap;
}

/**
   Does some sanity checks on make_spc_table's input.

   @param ob the object to check
   @return 0 if everything is ok, -1 otherwise
*/
static int
sanitycheck (object * const ob)
{
  int i;

  for (i = 0; i < ob->nbeams; i++)
    {
      while (ob->beams[i].orient < 0)
	{
	  ob->beams[i].orient += M_PI;
	}
      while (ob->beams[i].orient > M_PI)
	{
	  ob->beams[i].orient -= M_PI;
	}
    }
  return 0;
}

#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))
/**
   computes a table of aperture pixels, i.e. of tuples containing the
   source coordinates, the distance to the spectrum trace, the path
   length along the trace, and the intensity.  The end of the table
   is marked with an ap_pixel->x==-1.  This routine can do subsampling,
   whereby each pixel is divided into n_sub*n_sub smaller pixels.
   If n_sub==1, a special (faster) handling is enabled.

   @param ob the object struct to examine
   @param beamorder the order of the spectrum to examine
   @param flags warning flags like in the warning field of the
     spectrum structure.  This has to be passed in initialized.
*/
ap_pixel *
make_spc_table (object * const ob, const int beamorder,
		int *const flags)
{
  int bb_x, bb_y, bb_w, bb_h;
  int x, y;
  beam *curbeam = ob->beams + beamorder;
  double dx, dy;
  ap_pixel *table, *cur_ap;
  is_in_descriptor iid;
  sectionfun sf;
  trace_func *tracefun = curbeam->spec_trace;

  quad_to_bbox (curbeam->corners, curbeam->bbox, curbeam->bbox + 1);
  bb_x = curbeam->bbox[0].x;
  bb_y = curbeam->bbox[0].y;
  bb_w = curbeam->bbox[1].x - curbeam->bbox[0].x + 1;
  bb_h = curbeam->bbox[1].y - curbeam->bbox[0].y + 1;

  if (sanitycheck (ob))
    {
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Input data failed sanity check");
      return NULL;
    }

  if (fill_is_in_descriptor (&iid, curbeam->corners))
    return NULL;

  if (fill_in_sectionfun (&sf, curbeam->orient, curbeam))
    return NULL;

  if (!(table =
	malloc ((bb_w * bb_h + 1) * sizeof (ap_pixel))))
    return NULL;

  /* We have a little coordinate confusion here.  There are three
     systems:
     (a) the absolute one, rooted in (0,0) of the image.
     (b) the relative one, rooted in the reference point and used
     when evaluating the spectrum trace.  This one is used
     for finding the section points, etc.
     (c) one relative to the bounding box; x and y below live in this
     system and have to be converted into whatever system is
     required.  dx, dy are used for the conversion to (b)
  */
  dx = bb_x - curbeam->refpoint.x;
  dy = bb_y - curbeam->refpoint.y;
  cur_ap = table;
  for (y = 0; y <= bb_h; y++)
    {
      for (x = 0; x < bb_w; x++)
	{
	  if ((bb_x + x < 0) || (bb_y + y < 0)
	      || (bb_x + x >= (int)ob->grism_obs->grism->size1)
	      || (bb_y + y >= (int)ob->grism_obs->grism->size2))
	    {
	      *flags |= SPC_W_BORDER;
	      continue;
	    }

	  // check whether the trace description has
	  // an order higher than linear
	  if (curbeam->spec_trace->type > 1)
	    {
	      // new criteria based on the true trace distance
	      // which means the true distance from the section point
	      if (!tracedist_criteria(x + dx, y + dy, &sf, tracefun, curbeam->width+2.0))
		{
		  continue;
		}
	    }
	  else
	    {
	      // old criteria based on the box model
	      if (!is_in (bb_x + x, bb_y + y, &iid))
		{
		  continue;
		}
	    }

	  if (isnan
	      (gsl_matrix_get
	       (ob->grism_obs->grism, bb_x + x, bb_y + y)))
	    {
	      continue;
	    }
	  //	  if (ob->ID == 11 && x + bb_x == 59)
	    //	    fprintf(stdout, "xx: %i, yy: %i: %i\n", x + bb_x, y + bb_y, is_in (x + bb_x, y + bb_y, &iid));
	  cur_ap =
	    handle_one_pixel (x + dx, y + dy, x + bb_x,
			      y + bb_y, ob->grism_obs, &sf,
			      tracefun, cur_ap);

	}
    }

  cur_ap->p_x = -1;
  cur_ap->p_y = -1;
  cur_ap->count = -1;

  free_sectionfun (&sf);

  if (transform_to_pathlen (curbeam->spec_trace, table))
    {
      //   free (table);
      //   table = NULL;
      //   return NULL;
    }
  return table;
}

/**
   Similar to make_spc_table, it just works only
   on one spot given in the parameters.
*/
ap_pixel *
make_gps_table (object * const ob, const int beamorder,
		int *const flags, int xval, int yval)
{
  int bb_x, bb_y, bb_w, bb_h;
  int x, y;
  beam *curbeam = ob->beams + beamorder;
  double dx, dy;
  ap_pixel *table, *cur_ap;
  is_in_descriptor iid;
  sectionfun sf;
  trace_func *tracefun = curbeam->spec_trace;

  quad_to_bbox (curbeam->corners, curbeam->bbox, curbeam->bbox + 1);
  bb_x = curbeam->bbox[0].x;
  bb_y = curbeam->bbox[0].y;
  bb_w = curbeam->bbox[1].x - curbeam->bbox[0].x + 1;
  bb_h = curbeam->bbox[1].y - curbeam->bbox[0].y + 1;

  if (sanitycheck (ob))
    {
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Input data failed sanity check");
      return NULL;
    }

  if (fill_is_in_descriptor (&iid, curbeam->corners))
    return NULL;

  if (fill_in_sectionfun (&sf, curbeam->orient, curbeam))
    return NULL;

  if (!
      (table =
       malloc (2 * sizeof (ap_pixel))))
    return NULL;

  /* We have a little coordinate confusion here.  There are three
     systems:
     (a) the absolute one, rooted in (0,0) of the image.
     (b) the relative one, rooted in the reference point and used
     when evaluating the spectrum trace.  This one is used
     for finding the section points, etc.
     (c) one relative to the bounding box; x and y below live in this
     system and have to be converted into whatever system is
     required.  dx, dy are used for the conversion to (b)
  */
  dx = bb_x - curbeam->refpoint.x;
  dy = bb_y - curbeam->refpoint.y;
  x  = xval - bb_x;
  y  = yval - bb_y;

  cur_ap = table;

  cur_ap =
    handle_one_pixel (x + dx, y + dy, x + bb_x,
		      y + bb_y, ob->grism_obs, &sf,
		      tracefun, cur_ap);

  cur_ap->p_x = -1;
  cur_ap->p_y = -1;
  cur_ap->count = -1;

  free_sectionfun (&sf);

  transform_to_pathlen (curbeam->spec_trace, table);

  return table;
}

void
print_ap_pixel_table (const ap_pixel * ap_p)
{
  printf ("# x y pathlen distance lambda count error\n");
  while (ap_p->p_x != -1)
    {
      printf ("%d %d %f %f %f %f %f %f %f %ld\n", ap_p->p_x, ap_p->p_y,
	      ap_p->x, ap_p->y, ap_p->xi, ap_p->dist, ap_p->lambda,
	      ap_p->count, ap_p->error, ap_p->dq);
      ap_p++;
    }
}

/*
int
tracedist_criteria(const double x, const double y, sectionfun * const sf,
		   const trace_func *tracefun, const double width)
{
  double x_sect, y_sect;
  double dist, max_dist;

  int ireturn=0;

  find_section_point (sf, x, y, &x_sect);
  y_sect = tracefun->func (x_sect, tracefun->data);

  dist =  sqrt((y_sect-y)*(y_sect-y)+(x_sect-x)*(x_sect-x));

  if (dist < width)
    ireturn=1;

  return ireturn;
}
*/


/*
** hemodynamic modelling for single-subject LISA
**
** G.Lohmann, MPI-KYB,  2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_spline.h>

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

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

#define LEN     10000   /* buffer length        */
#define NTRIALS 10000   /* max number of trials */

/* standard parameter values for gamma function, Glover 99 */
double a1 = 6;
double b1 = 0.9;
double a2 = 12;
double b2 = 0.9;
double cc = 0.35;

typedef struct TrialStruct {
  int   id;
  float onset;
  float duration;
  float height;
} Trial;

void printmat(gsl_matrix *R,char *str)
{
  int i,j;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<R->size1; i++) {
    for (j=0; j<R->size2; j++) {
      fprintf(stderr," %9.6f",gsl_matrix_get(R,i,j));
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void tprintmat(gsl_matrix *R,char *str)
{
  int i,j;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<R->size2; i++) {
    for (j=0; j<R->size1; j++) {
      fprintf(stderr," %9.6f",gsl_matrix_get(R,j,i));
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void printvec(gsl_vector *x,char *str)
{
  int i;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<x->size; i++) {
    fprintf(stderr," %f\n",x->data[i]);
  }
  fprintf(stderr,"\n");
}

/* output txt file for plotting */
void PlotDesign(gsl_matrix *X,double tr,VString filename)
{
  int i,j;
  double u=0;

  FILE *fp = fopen(filename,"w");
  if (fp == NULL) VError("err opening plot file");
  double t = 0;
  for (i=0; i<X->size1; i++) {
    fprintf(fp," %8.2f",t);
    for (j=0; j<X->size2; j++) {
      u = gsl_matrix_get(X,i,j);
      fprintf(fp," %10.6f",u);
    }
    t += tr;
    fprintf(fp,"\n");
  }
  fclose(fp);
}

/* create design X */
gsl_matrix *VCreateDesign(int ntimesteps,int nevents,int hemomodel,VBoolean firstcol,gsl_matrix *covariates)
{
  int dim = nevents;
  if (hemomodel == 1) dim = nevents*2;
  if (hemomodel == 2) dim = nevents*3;
  if (firstcol == TRUE) dim++;
  if (covariates != NULL) dim += covariates->size2;  /* nuisance covariates without task labels */

  gsl_matrix *X = gsl_matrix_calloc(ntimesteps,dim);
  return X;
}

/* Glover kernel, gamma function */
double xgamma(double xx, double t0)
{
  double x, y, scale = 20;
  double y1, y2;
  double d1, d2;
  x = xx - t0;
  if(x < 0 || x > 50)
    return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(x / d1, a1) * exp(-(x - d1) / b1);
  y2 = pow(x / d2, a2) * exp(-(x - d2) / b2);
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* Glover kernel, gamma function, parameters changed for block designs */
double bgamma(double xx, double t0)
{
  double x, y, scale = 120;
  double y1, y2;
  double d1, d2;
  double aa1 = 6;
  double bb1 = 0.9;
  double aa2 = 12;
  double bb2 = 0.9;
  double cx  = 0.1;
  x = xx - t0;
  if(x < 0 || x > 50)
    return 0;
  d1 = aa1 * bb1;
  d2 = aa2 * bb2;
  y1 = pow(x / d1, aa1) * exp(-(x - d1) / bb1);
  y2 = pow(x / d2, aa2) * exp(-(x - d2) / bb2);
  y = y1 - cx * y2;
  y /= scale;
  return y;
}

/* first derivative */
double deriv1_gamma(double x, double t0)
{
  double d1, d2, y1, y2, y, xx;
  double scale = 20.0;
  xx = x - t0;
  if(xx < 0 || xx > 50) return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(d1, -a1) * a1 * pow(xx, (a1 - 1.0)) * exp(-(xx - d1) / b1)
    - (pow((xx / d1), a1) * exp(-(xx - d1) / b1)) / b1;
  y2 = pow(d2, -a2) * a2 * pow(xx, (a2 - 1.0)) * exp(-(xx - d2) / b2)
    - (pow((xx / d2), a2) * exp(-(xx - d2) / b2)) / b2;
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* second derivative */
double deriv2_gamma(double x, double t0)
{
  double d1, d2, y1, y2, y3, y4, y, xx;
  double scale = 20.0;
  xx = x - t0;
  if (xx < 0 || xx > 50) return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(d1, -a1) * a1 * (a1 - 1) * pow(xx, a1 - 2) * exp(-(xx - d1) / b1)
    - pow(d1, -a1) * a1 * pow(xx, (a1 - 1)) * exp(-(xx - d1) / b1) / b1;
  y2 = pow(d1, -a1) * a1 * pow(xx, a1 - 1) * exp(-(xx - d1) / b1) / b1
    - pow((xx / d1), a1) * exp(-(xx - d1) / b1) / (b1 * b1);
  y1 = y1 - y2;
  y3 = pow(d2, -a2) * a2 * (a2 - 1) * pow(xx, a2 - 2) * exp(-(xx - d2) / b2)
    - pow(d2, -a2) * a2 * pow(xx, (a2 - 1)) * exp(-(xx - d2) / b2) / b2;
  y4 = pow(d2, -a2) * a2 * pow(xx, a2 - 1) * exp(-(xx - d2) / b2) / b2
    - pow((xx / d2), a2) * exp(-(xx - d2) / b2) / (b2 * b2);
  y2 = y3 - y4;
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* Gaussian function */
double xgauss(double xx, double t0)
{
  double sigma=1.0;
  double x, y, z, a = 2.506628273;
  x = (xx - t0);
  z = x / sigma;
  y = exp(-0.5*z*z) / (sigma * a);
  return y;
}

void XConvolve(double *src,double *dst,double *kernel,int nt,int kernelsize)
{
  int i,j,jj,k;
  double sum=0;

  for (i=0; i<nt; i++) {
    sum = 0;
    k=0;
    for (j=i; j<i+kernelsize; j++) {
      jj = i-k;
      if (jj >= 0 && jj < nt) {
	sum += src[jj] * kernel[k];
      }
      k++;
    }
    dst[i] = sum;
  }
}

/* hemodynamic modelling at high temporal resolution */
void VHemoModel(Trial *trial,int ntrials,int nevents,int nt,double xtr,int hemomodel,VBoolean firstcol,
		gsl_matrix *X,gsl_matrix *covariates)
{
  int i,j,jj,k;
  double t,t0,t1,h;

  /* set temporal resolution to 0.5 sec, reduce discretization artefacts */
  double hfactor = floor(xtr/0.5);
  if (hfactor < 1.0) hfactor = 1.0;

  double tr = xtr/hfactor;
  int jfactor = (int)hfactor;
  int ntimesteps = jfactor*nt;

  for(i = 0; i < nevents; i++) {
    double xmin = VRepnMaxValue(VFloatRepn);
    for(j = 0; j < ntrials; j++) {
      if(trial[j].id != i) continue;
      if(trial[j].duration < xmin) xmin = trial[j].duration;
    }
  }

  /* get kernels */
  t1 = 30.0;    /* kernel duration = 30 secs */
  int kernelsize = t1 / tr;
  if (tr < 0.01) VWarning(" implausible TR (%f seconds)",tr);

  /* fprintf(stderr," hf= %f,  tr= %f  %f, kernelsize: %d\n",hfactor,xtr,tr,kernelsize); */

  double *kernel1=NULL,*kernel2=NULL;
  double *bkernel  = (double *)VCalloc(kernelsize,sizeof(double));
  double *kernel0  = (double *)VCalloc(kernelsize,sizeof(double));
  if (hemomodel == 1 || hemomodel == 2) {
    kernel1 = (double *)VCalloc(kernelsize,sizeof(double));
  }
  if (hemomodel == 2) {
    kernel2 = (double *)VCalloc(kernelsize,sizeof(double));
  }

  i = 0;
  for (t = 0; t < t1; t += tr) {
    if (i >= kernelsize) break;

    bkernel[i] = xgauss(t, 5.0*tr);
    kernel0[i] = xgamma(t, 0.0);

    if(hemomodel == 1 || hemomodel == 2)
      kernel1[i] = deriv1_gamma(t, 0);
    if(hemomodel == 2)
      kernel2[i] = deriv2_gamma(t, 0);
    i++;
  }

  /* tmp storage */
  double *x = (double *) VCalloc(ntimesteps,sizeof(double));
  double *y = (double *) VCalloc(ntimesteps,sizeof(double));

  /* constant in column 0 */
  int col=0;
  if (firstcol == TRUE) {
    gsl_matrix_set_zero(X);
    for (j=0; j<nt; j++) gsl_matrix_set(X,j,0,1.0);
    col=1;
  }

  /* for each trial,event, do... */
  for (i = 0; i < nevents; i++) {
    for (k=0; k<ntimesteps; k++) x[k] = y[k] = 0;

    /* read design info */
    int trialcount = 0;
    for (j = 0; j < ntrials; j++) {
      if(trial[j].id != i+1) continue;
      trialcount++;
      t0 = trial[j].onset;
      t1 = trial[j].onset + trial[j].duration;
      h  = trial[j].height;
      int k0 = (int) (t0/tr + 0.5);
      int k1 = (int) (t1/tr + 0.5);

      int klen=0;
      for (k=k0; k<=k1; k++) {
	if (k < 0 || k >= ntimesteps) continue;
	x[k] = h;
	klen++;
      }
      if(trialcount < 1)
	VError(" no trials in event %d, please re-number event-ids,  ntrials= %d", i + 1,ntrials);
    }

    /* convolve */
    if (hemomodel == 3)  {       /* block design, gaussian kernel */
      XConvolve(x,y,bkernel,ntimesteps,kernelsize);
    }
    else {                       /* gamma function kernel */
      XConvolve(x,y,kernel0,ntimesteps,kernelsize);
    }

    for (j=0; j<X->size1; j++) {
      jj = j*jfactor;
      if (jj >= ntimesteps) continue;
      gsl_matrix_set(X,j,col,y[jj]/hfactor);
    }
    col++;

    if (hemomodel == 1 || hemomodel == 2) {   /* gamma function kernel, first derivative */
      XConvolve(x,y,kernel1,ntimesteps,kernelsize);
      for (j=0; j<X->size1; j++) {
	jj = j*jfactor;
	if (jj >= ntimesteps) continue;
	gsl_matrix_set(X,j,col,y[jj]/hfactor);
      }
      col++;
    }
    if (hemomodel == 2) {   /* gamma function kernel, second derivative */
      XConvolve(x,y,kernel2,ntimesteps,kernelsize);
      for (j=0; j<X->size1; j++) {
	jj = j*jfactor;
	if (jj >= ntimesteps) continue;
	gsl_matrix_set(X,j,col,y[jj]/hfactor);
      }
      col++;
    }
  }

  /* add further covariates that have no task labels, will not be permuted */
  if (covariates != NULL) {
    for (i=0; i<covariates->size2; i++) {
      for (j=0; j<covariates->size1; j++) {
	if (col >= X->size2) VError(" VHemoModel, column= %d  %d\n",col,(int)X->size2);
	gsl_matrix_set(X,j,col,gsl_matrix_get(covariates,j,i));
      }
      col++;
    }
  }

  /* cleanup */
  VFree(x);
  VFree(y);
  VFree(kernel0);
  VFree(bkernel);
  if (kernel1 != NULL) VFree(kernel1);
  if (kernel2 != NULL) VFree(kernel2);
}


//Causal FIR filtering of each vector in X along dim.
//FIR impulse response is given in vector B with length Q+1.
//(I use P for IIR filter order, since same as polynomial order.)

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

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int fir_s (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_d (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_c (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_z (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);

int fir_s (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in fir_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Initialize Y
    float b = *B++;
    for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    X -= N; Y -= N - 1u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fmaf(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=Q, ++Y)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fmaf(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=L, Y+=L-Q)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += K - 1u;
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, B-=Q, Y-=K*Q-1u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, Y+=K) { cblas_saxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

int fir_d (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in fir_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Initialize Y
    double b = *B++;
    for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    X -= N; Y -= N - 1u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fma(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=Q, ++Y)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fma(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=L, Y+=L-Q)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += K - 1u;
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, B-=Q, Y-=K*Q-1u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, Y+=K) { cblas_daxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

int fir_c (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in fir_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    float xr, xi, br, bi;

    //Initialize Y
    br = *B++; bi = *B++;
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = br*xr - bi*xi;
        *Y++ = br*xi + bi*xr;
    }
    X -= 2u*N; Y -= 2u*N - 2u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
            {
                br = *B++; bi = *B++;
                for (size_t l=q; l<L; ++l)
                {
                    xr = *X++; xi = *X++;
                    *Y++ += br*xr - bi*xi;
                    *Y++ += br*xi + bi*xr;
                }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_caxpy((int)(L-q),B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=2u*Q, X+=2u*Q, Y+=2u)
                {
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l)
                        {
                            xr = *X++; xi = *X++;
                            *Y++ += br*xr - bi*xi;
                            *Y++ += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, X+=2u*L, B-=2u*Q, Y+=2u*(L-Q-1u))
                {
                    for (size_t q=0u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_caxpy((int)(L-q),B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += 2u*K - 2u;
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X+=2u, B-=2u*Q, Y-=2u*K*Q-2u)
                {
                    for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u*K) { cblas_caxpy((int)(L-q),B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

int fir_z (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim)
{
    if (dim>3u) { fprintf(stderr,"error in fir_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    double xr, xi, br, bi;

    //Initialize Y
    br = *B++; bi = *B++;
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = br*xr - bi*xi;
        *Y++ = br*xi + bi*xr;
    }
    X -= 2u*N; Y -= 2u*N - 2u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
            {
                br = *B++; bi = *B++;
                for (size_t l=q; l<L; ++l)
                {
                    xr = *X++; xi = *X++;
                    *Y++ += br*xr - bi*xi;
                    *Y++ += br*xi + bi*xr;
                }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_zaxpy((int)(L-q),B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=2u*Q, X+=2u*Q, Y+=2u)
                {
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l)
                        {
                            xr = *X++; xi = *X++;
                            *Y++ += br*xr - bi*xi;
                            *Y++ += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, X+=2u*L, B-=2u*Q, Y+=2u*(L-Q-1u))
                {
                    for (size_t q=0u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_zaxpy((int)(L-q),B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += 2u*K - 2u;
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X+=2u, B-=2u*Q, Y-=2u*K*Q-2u)
                {
                    for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u*K) { cblas_zaxpy((int)(L-q),B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <stdio.h>
#include <gsl/gsl_sf_bessel.h>

int
main (void)
{
  double x = 5.0;
  double y = gsl_sf_bessel_J0 (x);
  printf ("J0(%g) = %.18e\n", x, y);
  return 0;
}


/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file utils.c
 * \brief Source file with the useful data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include <glib.h>
#include "config.h"
#include "equation.h"

#define DEBUG_UTILS 0           ///< macro to debug the useful functions.

char *error_message = NULL;     ///< error message string.

/**
 * Function to add an error message.
 */
void
error_add (const char *message) ///< error message.
{
	char *buffer;
  buffer = error_message;
  error_message = (char *) g_strconcat (message, "\n", buffer, NULL);
  g_free (buffer);
}

/**
 * Function to calculate the distance between two vectors.
 *
 * \return vectors distance.
 */
long double
distance (long double *r1,      ///< 1st vector.
          long double *r2)      ///< 2nd vector.
{
  long double d, dr[3];
#if DEBUG_EQUATION
  fprintf (stderr, "distance: start\n");
  fprintf (stderr, "distance: x1=%Lg y1=%Lg z1=%Lg\n", r1[0], r1[1], r1[2]);
  fprintf (stderr, "distance: x2=%Lg y2=%Lg z2=%Lg\n", r2[0], r2[1], r2[2]);
#endif
  dr[0] = r1[0] - r2[0];
  dr[1] = r1[1] - r2[1];
  dr[2] = r1[2] - r2[2];
  d = sqrtl (dr[0] * dr[0] + dr[1] * dr[1] + dr[2] * dr[2]);
#if DEBUG_EQUATION
  fprintf (stderr, "distance: d=%Lg\n", d);
  fprintf (stderr, "distance: end\n");
#endif
  return d;
}

/**
 * Function to calculate the solution of a reduced 2nd order equation.
 *
 * This function calculates the solution of a reduced 2nd order equation in the
 * form:
 * \f[x^2+a\,x+b=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_quadratic_reduced (long double a, ///< a equation coefficient.
                         long double b, ///< b equation coefficient.
                         long double x1,        ///< lower solution limit.
                         long double x2)        ///< higher solution limit.
{
  long double a2, k, x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic_reduced: start\n");
  fprintf (stderr, "solve_quadratic_reduced: a=%Lg b=%Lg\n", a, b);
  fprintf (stderr, "solve_quadratic_reduced: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  a2 = -0.5L * a;
  k = sqrtl (a2 * a2 - b);
  x = a2 + k;
  if (x < x1 || x > x2)
    x = a2 - k;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic_reduced: x=%Lg\n", x);
  fprintf (stderr, "solve_quadratic_reduced: end\n");
#endif
  return x;
}

/**
 * Function to calculate the solution of a 2nd order equation.
 *
 * This function calculates the solution of a 2nd order equation in the
 * form:
 * \f[a\,x^2+b\,x+c=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_quadratic (long double a, ///< a equation coefficient.
                 long double b, ///< b equation coefficient.
                 long double c, ///< c equation coefficient.
                 long double x1,        ///< lower solution limit.
                 long double x2)        ///< higher solution limit.
{
  long double x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic: start\n");
  fprintf (stderr, "solve_quadratic: a=%Lg b=%Lg c=%Lg\n", a, b, c);
  fprintf (stderr, "solve_quadratic: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  if (a == 0.L)
    x = -c / b;
  else
    x = solve_quadratic_reduced (b / a, c / a, x1, x2);
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic: x=%Lg\n", x);
  fprintf (stderr, "solve_quadratic: end\n");
#endif
  return x;
}

/**
 * Function to calculate the solution of a reduced 3rd order equation.
 *
 * This function calculates the solution of a reduced 3rd order equation in the
 * form:
 * \f[x^3+a\,x^2+b\,x+c=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_cubic_reduced (long double a,     ///< a equation coefficient.
                     long double b,     ///< b equation coefficient.
                     long double c,     ///< c equation coefficient.
                     long double x1,    ///< lower solution limit.
                     long double x2)    ///< higher solution limit.
{
  long double k0, k1, k2;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic_reduced: start\n");
  fprintf (stderr, "solve_cubic_reduced: a=%Lg b=%Lg c=%Lg\n", a, b, c);
  fprintf (stderr, "solve_cubic_reduced: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  a /= 3.L;
  k0 = a * a;
  k1 = b / 3.L - k0;
  k0 = (b * a - c) / 2.L - a * k0;
  k2 = k1 * k1 * k1 + k0 * k0;
  if (k2 < 0.L)
    {
      k1 = sqrtl (-k1);
      k0 = acosl (k0 / (k1 * k1 * k1)) / 3.L;
      k1 *= 2.L;
      k2 = k1 * cosl (k0) - a;
      if (k2 < x1 || k2 > x2)
        {
          k2 = k1 * cosl (k0 + 2.L * M_PIl / 3.L) - a;
          if (k2 < x1 || k2 > x2)
            k2 = k1 * cosl (k0 - 2.L * M_PIl / 3.L) - a;
        }
    }
  else
    {
      k1 = sqrtl (k2);
      k2 = k0 + k1;
      k2 = cbrtl (k2);
      k0 -= k1;
      k2 += cbrtl (k0);
      k2 -= a;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic_reduced: x=%Lg\n", k2);
  fprintf (stderr, "solve_cubic_reduced: end\n");
#endif
  return k2;
}

/**
 * Function to calculate the solution of a 3rd order equation.
 *
 * This function calculates the solution of a 3rd order equation in the
 * form:
 * \f[a\,x^3+b\,x^2+c\,x+d=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_cubic (long double a,     ///< a equation coefficient.
             long double b,     ///< b equation coefficient.
             long double c,     ///< c equation coefficient.
             long double d,     ///< d equation coefficient.
             long double x1,    ///< lower solution limit.
             long double x2)    ///< higher solution limit.
{
  long double x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic: start\n");
  fprintf (stderr, "solve_cubic: a=%Lg b=%Lg c=%Lg d=%Lg\n", a, b, c, d);
  fprintf (stderr, "solve_cubic: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  if (a == 0.L)
    x = solve_quadratic (b, c, d, x1, x2);
  else
    x = solve_cubic_reduced (b / a, c / a, d / a, x1, x2);
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic: x=%Lg\n", x);
  fprintf (stderr, "solve_cubic: end\n");
#endif
  return x;
}

/**
 * Function to get an integer number of a XML node property.
 *
 * \return Integer number value.
 */
int
xml_node_get_int (xmlNode * node,       ///< XML node.
                  const xmlChar * prop, ///< XML property.
                  int *error_code)      ///< Error code.
{
  int i = 0;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%d", &i) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return i;
}

/**
 * Function to get an unsigned integer number of a XML node property.
 *
 * \return Unsigned integer number value.
 */
unsigned int
xml_node_get_uint (xmlNode * node,      ///< XML node.
                   const xmlChar * prop,        ///< XML property.
                   int *error_code)     ///< Error code.
{
  unsigned int i = 0;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%u", &i) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return i;
}

/**
 * Function to get an unsigned integer number of a XML node property with a
 *   default value.
 *
 * \return Unsigned integer number value.
 */
unsigned int
xml_node_get_uint_with_default (xmlNode * node, ///< XML node.
                                const xmlChar * prop,   ///< XML property.
                                unsigned int default_value,
                                ///< default value.
                                int *error_code)        ///< Error code.
{
  unsigned int i;
  if (xmlHasProp (node, prop))
    i = xml_node_get_uint (node, prop, error_code);
  else
    {
      i = default_value;
      *error_code = 0;
    }
  return i;
}

/**
 * Function to get a floating point number of a XML node property.
 *
 * \return Floating point number value.
 */
long double
xml_node_get_float (xmlNode * node,     ///< XML node.
                    const xmlChar * prop,       ///< XML property.
                    int *error_code)    ///< Error code.
{
  long double x = 0.L;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%Lf", &x) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return x;
}

/**
 * Function to get a floating point number of a XML node property with a
 *   default value.
 *
 * \return Floating point number value.
 */
long double
xml_node_get_float_with_default (xmlNode * node,        ///< XML node.
                                 const xmlChar * prop,  ///< XML property.
                                 long double default_value,
                                 ///< default value.
                                 int *error_code)       ///< Error code.
{
  long double x;
  if (xmlHasProp (node, prop))
    x = xml_node_get_float (node, prop, error_code);
  else
    {
      x = default_value;
      *error_code = 0;
    }
  return x;
}


//
// Created by david on 2018-11-30.
//

#ifndef DMRG_CLASS_XDMRG_FULL_FUNCTOR_H
#define DMRG_CLASS_XDMRG_FULL_FUNCTOR_H
#ifdef OpenMP_AVAILABLE
#include <omp.h>
#endif

#ifdef OpenBLAS_AVAILABLE
#include <cblas.h>
#endif

#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>
#include <general/nmspc_tensor_extra.h>
#include <general/class_tic_toc.h>
template<typename Scalar>
class class_xDMRG_full_functor {
private:
    double variance;
    double energy  ;
    double energy_lower_bound;
    double energy_upper_bound;
    double energy_target;
    double energy_window;
public:
    template <typename T>
    int sgn(const T val) const {
        return (T(0) < val) - (val < T(0));
    }

    using MatrixType_ = Eigen::Matrix<Scalar,Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType_ = Eigen::Matrix<Scalar,Eigen::Dynamic, 1>;
    size_t   counter = 0;
//    const size_t shape;
    void set_energy_bounds(double E_lower, double E_upper);
    bool have_bounds_on_energy = false;
    double get_variance(){return variance;}
    double get_energy  (){return energy  ;}
    size_t get_count   (){return counter;}
    Eigen::Tensor<double,4> HA_MPO;
    Eigen::Tensor<double,4> HB_MPO;
    Eigen::Tensor<double,3> Lblock;
    Eigen::Tensor<double,3> Rblock;
    Eigen::Tensor<double,4> Lblock2;
    Eigen::Tensor<double,4> Rblock2;

    Eigen::Tensor<double,6> HAHB;
    Eigen::Tensor<double,8> HAHB2;
    Eigen::DSizes<long,4>   dsizes;
    class_tic_toc t_lbfgs;
    class_xDMRG_full_functor(
            const Eigen::Tensor<Scalar,4> &HA_MPO_,
            const Eigen::Tensor<Scalar,4> &HB_MPO_,
            const Eigen::Tensor<Scalar,3> &Lblock_,
            const Eigen::Tensor<Scalar,3> &Rblock_,
            const Eigen::Tensor<Scalar,4> &Lblock2_,
            const Eigen::Tensor<Scalar,4> &Rblock2_,
            const Eigen::DSizes<long,4>   &dsizes_
            );

    double get_vH2v(const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    double get_vHv(const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    Eigen::VectorXd get_vH2 (const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    Eigen::VectorXd get_vH (const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    double operator()(const Eigen::Matrix<double,Eigen::Dynamic,1> &v, Eigen::Matrix<double,Eigen::Dynamic,1> &grad);

};

#endif //DMRG_CLASS_XDMRG_FULL_FUNCTOR_H


/*
    learn.c
*/
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_multifit.h>
#include "learn.h"
#include "feature.h"
#include "imatrix.h"
#include "dmatrix.h"
#include "util.h"
#include "likelihood.h"
#include "hyper.h"

void mvslda_learn(document *data, double **resp, double *alpha, double beta, double nu2, double sigma2,
    int nclass, int nlex, int dlenmax, int nresp, int maxiter, double **phi, double **theta, double **eta,
    int **n_mz, int **n_zw, FILE *likp, FILE *hyperp, unsigned long int random_seed){
    document *dp;
    int ndocs;
    int *n_m;
    int *n_z;
    int ***topics;
    int word_index;
    int word_num;
    double sum_alpha;
    double *left;
    double *center;
    double *log_right;
    double *p_z;
    double *log_p_z;
    double *cum_sum_p_z;
    double log_Z, sum_p_r, sum_empirical_z;
    double temp_prediction;
    double lik;
    double **temp_phi;
    double **temp_theta;
    double **empirical_z;
    int z;
    int it;
    int m, w, t, i, j, k;
    const gsl_rng_type *T;
    gsl_rng *r;

    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc(T);

    // count data length
    for(dp = data, ndocs = 0;(dp->len) != -1;dp++, ndocs++)
        ;

    // initialize buffers
    if((n_m = calloc(ndocs,sizeof(int))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate n_m.\n");
        return;
    }
    if((n_z = calloc(nclass,sizeof(int))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate n_z.\n");
        return;
    }
    if((left = calloc(nclass,sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate left.\n");
        return;
    }
    if((center = calloc(nclass,sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate center.\n");
        return;
    }
    if((log_right = calloc(nclass,sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate log_cright.\n");
        return;
    }
    if((p_z = calloc(nclass,sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate p_z.\n");
        return;
    }
    if((log_p_z = calloc(nclass,sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate log_p_z.\n");
        return;
    }
    if((cum_sum_p_z = calloc((nclass+1),sizeof(double))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate cum_sum_p_z.\n");
        return;
    }
    if((topics = calloc(ndocs,sizeof(int **))) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate topics.\n");
        return;
    }
    if((empirical_z = dmatrix(ndocs, nclass)) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate empirical_z.\n");
        return;
    }
    if((temp_phi = dmatrix(nlex, nclass)) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate temp_phi.\n");
        exit(1);
    }
    if((temp_theta = dmatrix(ndocs, nclass)) == NULL){
        fprintf(stderr,"mvslda_learn:: cannot allocate temp_theta.\n");
        exit(1);
    }

    printf("Number of documents          = %d\n",ndocs);
    printf("Number of unique words       = %d\n",nlex);
    printf("Number of latent classes     = %d\n",nclass);
    printf("Number of responses          = %d\n",nresp);
    printf("Number of iteration          = %d\n",maxiter);

    // choose an arbitrary topic as first topic for word
    gsl_rng_set(r, random_seed);
    for(dp = data, m = 0;(dp->len) != -1;dp++, m++){
        if((topics[m] = calloc((dp->len), sizeof(int *))) == NULL){
            fprintf(stderr,"mvslda_learn:: cannot allocate topics[m].\n");
            return;
        }
        for(w = 0;w < (dp->len);w++){
            if((topics[m][w] = calloc((dp->cnt[w]), sizeof(int))) == NULL){
                fprintf(stderr,"mvslda_learn:: cannot allocate topics[m][w].\n");
                return;
            }
            word_index = dp->id[w];
            word_num = dp->cnt[w];
            for(i = 0;i < word_num;i++){
                z = (int)gsl_rng_uniform_int(r, nclass);
                n_mz[m][z] += 1;
                n_m[m] += 1;
                n_zw[z][word_index] += 1;
                n_z[z] += 1;
                topics[m][w][i] = z;
            }
        }
    }
    // initialize eta ([nclass, nresp])
    for(k = 0;k < nclass;k++)
        for(t = 0;t < nresp;t++)
            eta[k][t] = 0.0;

    // learning main
    for(it = 0;it < maxiter;it++){
        printf("iteration %2d/%3d..\n", it + 1, maxiter);
        fflush(stdout);
        sum_alpha = 0.0;
        for(k = 0;k < nclass;k++)
            sum_alpha += alpha[k];
        for (dp = data, m = 0; (dp->len) != -1; dp++, m++){
            // for words
            for(w = 0;w < (dp->len);w++){
                word_index = dp->id[w];
                word_num = dp->cnt[w];
                for(i = 0;i < word_num;i++){
                    z = topics[m][w][i];
                    n_mz[m][z] -= 1;
                    n_m[m] -= 1;
                    n_zw[z][word_index] -= 1;
                    n_z[z] -= 1;

                    // compute conditional distribution log_p_z
                    // log_p_z left ... theta term
                    for(k = 0;k < nclass;k++){
                        left[k] = (double)n_mz[m][k] + alpha[k];
                        left[k] /= ((double)n_m[m] + sum_alpha);
                    }
                    // log_p_z center ... phi term
                    for(k = 0;k < nclass;k++){
                        center[k] = (double)n_zw[k][word_index] + beta;
                        center[k] /= ((double)n_z[k] + (double)nlex * beta);
                    }
                    // temporal log_p_z (left and center)
                    for(k = 0; k < nclass;k++){
                        log_p_z[k] = log(left[k]) + log(center[k]);
                    }
                    // p_z right ... eta term
                    sum_empirical_z = 0.0;
                    for(k = 0; k < nclass;k++){
                        empirical_z[m][k] = (double)n_mz[m][k];
                        sum_empirical_z += (double)n_mz[m][k];
                    }
                    for(k = 0; k < nclass;k++){
                        empirical_z[m][k] = empirical_z[m][k] / sum_empirical_z;
                    }
                    for(t = 0; t < nresp; t++){
                        temp_prediction = 0.0;
                        for(k = 0;k < nclass;k++){
                            temp_prediction += eta[k][t] * empirical_z[m][k]; // dot(eta, z_d)
                        }
                        for(k = 0;k < nclass;k++){
                            log_right[k] = 1.0;
                            log_right[k] *= 1.0 / (2 * sigma2);
                            log_right[k] *= (eta[k][t] / (double)n_m[m]);
                            log_right[k] *= (2 * (resp[m][t] - temp_prediction) - (eta[k][t] / (double)n_m[m]));
                            log_p_z[k] += log_right[k];
                        }
                    }
                    // conditional distribution log_p_z
                    // log_Z = logsumexp(logP_k1 + logP_k2 + ... logP_kK)
                    log_Z = logsumexp(log_p_z, nclass);
                    for(k = 0;k < nclass;k++){
                        p_z[k] = exp(log_p_z[k] - log_Z); // normalize to obtain probabilities
                    }
                    // random sampling from p_z
                    z = sampling_multinomial(r, p_z, cum_sum_p_z, nclass);
                    // update buffers
                    n_mz[m][z] += 1;
                    n_m[m] += 1;
                    n_zw[z][word_index] += 1;
                    n_z[z] += 1;
                    topics[m][w][i] = z;
                }
            }
        }

        // for eta update
        //least squares for dot(Z, eta) = resp
        for(m = 0; m < ndocs; m++){
            sum_empirical_z = 0.0;
            for(k = 0; k < nclass;k++){
                empirical_z[m][k] = (double)n_mz[m][k];
                sum_empirical_z += (double)n_mz[m][k];
            }
            for(k = 0; k < nclass;k++){
                empirical_z[m][k] = empirical_z[m][k] / sum_empirical_z;
            }
        }
        for(t = 0; t < nresp; t++){
            double chisq;
            gsl_matrix *Z, *cov;
            gsl_vector *y, *c;
            gsl_multifit_linear_workspace * work = gsl_multifit_linear_alloc (ndocs, nclass);
            Z = gsl_matrix_alloc(ndocs, nclass);
            cov = gsl_matrix_alloc(nclass, nclass);
            y = gsl_vector_alloc(ndocs);
            c = gsl_vector_alloc(nclass);
            for(m = 0;m < ndocs;m++){
                gsl_vector_set(y, m, resp[m][t]);
                for(k = 0;k < nclass;k++){
                    gsl_matrix_set(Z, m, k, empirical_z[m][k]);
                }
            }
            gsl_multifit_linear(Z, y, c, cov, &chisq, work);
            for(k = 0;k < nclass;k++){
                eta[k][t] = gsl_vector_get(c, k);
            }
            gsl_multifit_linear_free(work);
            gsl_matrix_free(Z);
            gsl_matrix_free(cov);
            gsl_vector_free(y);
            gsl_vector_free(c);
        }

        // update hyperparameters.
        update_alpha(alpha, n_m, n_mz, ndocs, nclass);
        beta = update_beta(beta, n_z, n_zw, nclass, nlex);

        // compute likelihood.
        lik = loglikelihood(n_mz, n_zw, n_m, nclass, nlex, ndocs, nresp, resp, alpha, beta, eta, empirical_z, nu2, sigma2);
        printf("\tlikelihood ... %.8f\n",lik);
        printf("\talpha = \n\t");
        for(k = 0;k < nclass;k++)
            printf("%.8f ",alpha[k]);
        printf("\n\tbeta ... %.2f\n",beta);
        fprintf(likp,"%.8f\n",lik);
        for(k = 0;k < nclass;k++)
            fprintf(hyperp,"%.8f,",alpha[k]);
        fprintf(hyperp,"%.8f\n",beta);
    }

    // compute matrix phi ([nlex, nclass] matrix)
    for(w = 0;w < nlex;w++)
        for(k = 0;k < nclass;k++)
            temp_phi[w][k] = (double)n_zw[k][w] + beta;
    normalize_matrix_col(phi, temp_phi, nlex, nclass);

    // compute matrix theta ([ndocs, nclass])
    for(m = 0;m < ndocs;m++)
        for(k = 0;k < nclass;k++)
            temp_theta[m][k] = (double)n_mz[m][k] + alpha[k];
    normalize_matrix_row(theta, temp_theta, ndocs, nclass);

    free(n_m);
    free(n_z);
    free(left);
    free(center);
    free(log_right);
    free(p_z);
    free(log_p_z);
    free(cum_sum_p_z);

    for(dp = data, m = 0;(dp->len) != -1;dp++, m++){
        for(w = 0;w < (dp->len);w++){
            free(topics[m][w]);
        }
        free(topics[m]);
    }
    free(topics);
    free_dmatrix(temp_phi, nlex);
    free_dmatrix(temp_theta, ndocs);
    free_dmatrix(empirical_z, ndocs);

    return;
}

int sampling_multinomial(gsl_rng *r, double *p, double *cum_sum_p, int len_p){
    int k, z;
    double sampling;

    cum_sum_p[0] = 0.0;
    for(k = 0;k < len_p;k++){
        cum_sum_p[k+1] = cum_sum_p[k] + p[k];
    }
    sampling = gsl_rng_uniform(r);
    for(k = 0;k < len_p;k++){
        if((sampling >= cum_sum_p[k]) && (sampling < cum_sum_p[k+1])){
            z = k;
            break;
        }
    }
    return z;
}


/* specfunc/poch.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_exp.h"
#include "gsl_sf_log.h"
#include "gsl_sf_pow_int.h"
#include "gsl_sf_psi.h"
#include "gsl_sf_gamma.h"

#include "error.h"

static const double bern[21] = {
   0.0   /* no element 0 */,
  +0.833333333333333333333333333333333e-01,
  -0.138888888888888888888888888888888e-02,
  +0.330687830687830687830687830687830e-04,
  -0.826719576719576719576719576719576e-06,
  +0.208767569878680989792100903212014e-07,
  -0.528419013868749318484768220217955e-09,
  +0.133825365306846788328269809751291e-10,
  -0.338968029632258286683019539124944e-12,
  +0.858606205627784456413590545042562e-14,
  -0.217486869855806187304151642386591e-15,
  +0.550900282836022951520265260890225e-17,
  -0.139544646858125233407076862640635e-18,
  +0.353470703962946747169322997780379e-20,
  -0.895351742703754685040261131811274e-22,
  +0.226795245233768306031095073886816e-23,
  -0.574472439520264523834847971943400e-24,
  +0.145517247561486490186626486727132e-26,
  -0.368599494066531017818178247990866e-28,
  +0.933673425709504467203255515278562e-30,
  -0.236502241570062993455963519636983e-31
};

/* ((a)_x - 1)/x in the "small x" region where
 * cancellation must be controlled.
 *
 * Based on SLATEC DPOCH1().
 */
/*
C When ABS(X) is so small that substantial cancellation will occur if
C the straightforward formula is used, we use an expansion due
C to Fields and discussed by Y. L. Luke, The Special Functions and Their
C Approximations, Vol. 1, Academic Press, 1969, page 34.
C
C The ratio POCH(A,X) = GAMMA(A+X)/GAMMA(A) is written by Luke as
C        (A+(X-1)/2)**X * polynomial in (A+(X-1)/2)**(-2) .
C In order to maintain significance in POCH1, we write for positive a
C        (A+(X-1)/2)**X = EXP(X*LOG(A+(X-1)/2)) = EXP(Q)
C                       = 1.0 + Q*EXPREL(Q) .
C Likewise the polynomial is written
C        POLY = 1.0 + X*POLY1(A,X) .
C Thus,
C        POCH1(A,X) = (POCH(A,X) - 1) / X
C                   = EXPREL(Q)*(Q/X + Q*POLY1(A,X)) + POLY1(A,X)
C
*/
static
int
pochrel_smallx(const double a, const double x, gsl_sf_result * result)
{
  /*
   SQTBIG = 1.0D0/SQRT(24.0D0*D1MACH(1))
   ALNEPS = LOG(D1MACH(3))
   */
  const double SQTBIG = 1.0/(2.0*M_SQRT2*M_SQRT3*GSL_SQRT_DBL_MIN);
  const double ALNEPS = GSL_LOG_DBL_EPSILON - M_LN2;

  if(x == 0.0) {
    return gsl_sf_psi_e(a, result);
  }
  else {
    const double bp   = (  (a < -0.5) ? 1.0-a-x : a );
    const int	 incr = ( (bp < 10.0) ? 11.0-bp : 0 );
    const double b    = bp + incr;
    double dpoch1;
    gsl_sf_result dexprl;
    int stat_dexprl;
    int i;

    double var    = b + 0.5*(x-1.0);
    double alnvar = log(var);
    double q = x*alnvar;

    double poly1 = 0.0;

    if(var < SQTBIG) {
      const int nterms = (int)(-0.5*ALNEPS/alnvar + 1.0);
      const double var2 = (1.0/var)/var;
      const double rho  = 0.5 * (x + 1.0);
      double term = var2;
      double gbern[24];
      int k, j;

      gbern[1] = 1.0;
      gbern[2] = -rho/12.0;
      poly1 = gbern[2] * term;

      if(nterms > 20) {
        /* NTERMS IS TOO BIG, MAYBE D1MACH(3) IS BAD */
        /* nterms = 20; */
	result->val = 0.0;
	result->err = 0.0;
	GSL_ERROR ("error", GSL_ESANITY);
      }

      for(k=2; k<=nterms; k++) {
        double gbk = 0.0;
        for(j=1; j<=k; j++) {
          gbk += bern[k-j+1]*gbern[j];
        }
        gbern[k+1] = -rho*gbk/k;

        term  *= (2*k-2-x)*(2*k-1-x)*var2;
        poly1 += gbern[k+1]*term;
      }
    }

    stat_dexprl = gsl_sf_expm1_e(q, &dexprl);
    if(stat_dexprl != GSL_SUCCESS) {
      result->val = 0.0;
      result->err = 0.0;
      return stat_dexprl;
    }
    dexprl.val = dexprl.val/q;
    poly1 *= (x - 1.0);
    dpoch1 = dexprl.val * (alnvar + q * poly1) + poly1;

    for(i=incr-1; i >= 0; i--) {
      /*
       C WE HAVE DPOCH1(B,X), BUT BP IS SMALL, SO WE USE BACKWARDS RECURSION
       C TO OBTAIN DPOCH1(BP,X).
       */
      double binv = 1.0/(bp+i);
      dpoch1 = (dpoch1 - binv) / (1.0 + x*binv);
    }

    if(bp == a) {
      result->val = dpoch1;
      result->err = 2.0 * GSL_DBL_EPSILON * (fabs(incr) + 1.0) * fabs(result->val);
      return GSL_SUCCESS;
    }
    else {
      /*
       C WE HAVE DPOCH1(BP,X), BUT A IS LT -0.5.  WE THEREFORE USE A
       C REFLECTION FORMULA TO OBTAIN DPOCH1(A,X).
       */
      double sinpxx = sin(M_PI*x)/x;
      double sinpx2 = sin(0.5*M_PI*x);
      double t1 = sinpxx/tan(M_PI*b);
      double t2 = 2.0*sinpx2*(sinpx2/x);
      double trig  = t1 - t2;
      result->val  = dpoch1 * (1.0 + x*trig) + trig;
      result->err  = (fabs(dpoch1*x) + 1.0) * GSL_DBL_EPSILON * (fabs(t1) + fabs(t2));
      result->err += 2.0 * GSL_DBL_EPSILON * (fabs(incr) + 1.0) * fabs(result->val);
      return GSL_SUCCESS;
    }
  }
}

/* Assumes a>0 and a+x>0.
 */
static
int
lnpoch_pos(const double a, const double x, gsl_sf_result * result)
{
  double absx = fabs(x);

  if(absx > 0.1*a || absx*log(GSL_MAX_DBL(a,2.0)) > 0.1) {
    if(a < GSL_SF_GAMMA_XMAX && a+x < GSL_SF_GAMMA_XMAX) {
      /* If we can do it by calculating the gamma functions
       * directly, then that will be more accurate than
       * doing the subtraction of the logs.
       */
      gsl_sf_result g1;
      gsl_sf_result g2;
      gsl_sf_gammainv_e(a,   &g1);
      gsl_sf_gammainv_e(a+x, &g2);
      result->val  = -log(g2.val/g1.val);
      result->err  = g1.err/fabs(g1.val) + g2.err/fabs(g2.val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_SUCCESS;
    }
    else {
      /* Otherwise we must do the subtraction.
       */
      gsl_sf_result lg1;
      gsl_sf_result lg2;
      int stat_1 = gsl_sf_lngamma_e(a,   &lg1);
      int stat_2 = gsl_sf_lngamma_e(a+x, &lg2);
      result->val  = lg2.val - lg1.val;
      result->err  = lg2.err + lg1.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_1, stat_2);
    }
  }
  else if(absx < 0.1*a && a > 15.0) {
    /* Be careful about the implied subtraction.
     * Note that both a+x and and a must be
     * large here since a is not small
     * and x is not relatively large.
     * So we calculate using Stirling for Log[Gamma(z)].
     *
     *   Log[Gamma(a+x)/Gamma(a)] = x(Log[a]-1) + (x+a-1/2)Log[1+x/a]
     *                              + (1/(1+eps)   - 1) / (12 a)
     *                              - (1/(1+eps)^3 - 1) / (360 a^3)
     *                              + (1/(1+eps)^5 - 1) / (1260 a^5)
     *                              - (1/(1+eps)^7 - 1) / (1680 a^7)
     *                              + ...
     */
    const double eps = x/a;
    const double den = 1.0 + eps;
    const double d3 = den*den*den;
    const double d5 = d3*den*den;
    const double d7 = d5*den*den;
    const double c1 = -eps/den;
    const double c3 = -eps*(3.0+eps*(3.0+eps))/d3;
    const double c5 = -eps*(5.0+eps*(10.0+eps*(10.0+eps*(5.0+eps))))/d5;
    const double c7 = -eps*(7.0+eps*(21.0+eps*(35.0+eps*(35.0+eps*(21.0+eps*(7.0+eps))))))/d7;
    const double p8 = gsl_sf_pow_int(1.0+eps,8);
    const double c8 = 1.0/p8             - 1.0;  /* these need not   */
    const double c9 = 1.0/(p8*(1.0+eps)) - 1.0;  /* be very accurate */
    const double a4 = a*a*a*a;
    const double a6 = a4*a*a;
    const double ser_1 = c1 + c3/(30.0*a*a) + c5/(105.0*a4) + c7/(140.0*a6);
    const double ser_2 = c8/(99.0*a6*a*a) - 691.0/360360.0 * c9/(a6*a4);
    const double ser = (ser_1 + ser_2)/ (12.0*a);

    double term1 = x * log(a/M_E);
    double term2;
    gsl_sf_result ln_1peps;
    gsl_sf_log_1plusx_e(eps, &ln_1peps);  /* log(1 + x/a) */
    term2 = (x + a - 0.5) * ln_1peps.val;

    result->val  = term1 + term2 + ser;
    result->err  = GSL_DBL_EPSILON*fabs(term1);
    result->err += fabs((x + a - 0.5)*ln_1peps.err);
    result->err += fabs(ln_1peps.val) * GSL_DBL_EPSILON * (fabs(x) + fabs(a) + 0.5);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result poch_rel;
    int stat_p = pochrel_smallx(a, x, &poch_rel);
    double eps = x*poch_rel.val;
    int stat_e = gsl_sf_log_1plusx_e(eps, result);
    result->err  = 2.0 * fabs(x * poch_rel.err / (1.0 + eps));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_e, stat_p);
  }
}

/*-*-*-*-*-*-*-*-*-*-*-* Functions with Error Codes *-*-*-*-*-*-*-*-*-*-*-*/

int
gsl_sf_lnpoch_e(const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(a <= 0.0 || a+x <= 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    return lnpoch_pos(a, x, result);
  }
}

int
gsl_sf_lnpoch_sgn_e(const double a, const double x,
                       gsl_sf_result * result, double * sgn)
{
  if(a == 0.0 || a+x == 0.0) {
    *sgn = 0.0;
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    *sgn = 1.0;
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(a > 0.0 && a+x > 0.0) {
    *sgn = 1.0;
    return lnpoch_pos(a, x, result);
  }
  else if(a < 0.0 && a+x < 0.0) {
    /* Reduce to positive case using reflection.
     */
    double sin_1 = sin(M_PI * (1.0 - a));
    double sin_2 = sin(M_PI * (1.0 - a - x));
    if(sin_1 == 0.0 || sin_2 == 0.0) {
      *sgn = 0.0;
      DOMAIN_ERROR(result);
    }
    else {
      gsl_sf_result lnp_pos;
      int stat_pp   = lnpoch_pos(1.0-a, -x, &lnp_pos);
      double lnterm = log(fabs(sin_1/sin_2));
      result->val  = lnterm - lnp_pos.val;
      result->err  = lnp_pos.err;
      result->err += 2.0 * GSL_DBL_EPSILON * (fabs(1.0-a) + fabs(1.0-a-x)) * fabs(lnterm);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *sgn = GSL_SIGN(sin_1*sin_2);
      return stat_pp;
    }
  }
  else {
    /* Evaluate gamma ratio directly.
     */
    gsl_sf_result lg_apn;
    gsl_sf_result lg_a;
    double s_apn, s_a;
    int stat_apn = gsl_sf_lngamma_sgn_e(a+x, &lg_apn, &s_apn);
    int stat_a   = gsl_sf_lngamma_sgn_e(a,   &lg_a,   &s_a);
    if(stat_apn == GSL_SUCCESS && stat_a == GSL_SUCCESS) {
      result->val  = lg_apn.val - lg_a.val;
      result->err  = lg_apn.err + lg_a.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *sgn = s_a * s_apn;
      return GSL_SUCCESS;
    }
    else if(stat_apn == GSL_EDOM || stat_a == GSL_EDOM){
      *sgn = 0.0;
      DOMAIN_ERROR(result);
    }
    else {
      result->val = 0.0;
      result->err = 0.0;
      *sgn = 0.0;
      return GSL_FAILURE;
    }
  }
}

int
gsl_sf_poch_e(const double a, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x == 0.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    gsl_sf_result lnpoch;
    double sgn;
    int stat_lnpoch = gsl_sf_lnpoch_sgn_e(a, x, &lnpoch, &sgn);
    int stat_exp    = gsl_sf_exp_err_e(lnpoch.val, lnpoch.err, result);
    result->val *= sgn;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_exp, stat_lnpoch);
  }
}

int
gsl_sf_pochrel_e(const double a, const double x, gsl_sf_result * result)
{
  const double absx = fabs(x);
  const double absa = fabs(a);

  /* CHECK_POINTER(result) */

  if(absx > 0.1*absa || absx*log(GSL_MAX(absa,2.0)) > 0.1) {
    gsl_sf_result lnpoch;
    double sgn;
    int stat_poch = gsl_sf_lnpoch_sgn_e(a, x, &lnpoch, &sgn);
    if(lnpoch.val > GSL_LOG_DBL_MAX) {
      OVERFLOW_ERROR(result);
    }
    else {
      const double el = exp(lnpoch.val);
      result->val  = (sgn*el - 1.0)/x;
      result->err  = fabs(result->val) * (lnpoch.err + 2.0 * GSL_DBL_EPSILON);
      result->err += 2.0 * GSL_DBL_EPSILON * (fabs(sgn*el) + 1.0) / fabs(x);
      return stat_poch;
    }
  }
  else {
    return pochrel_smallx(a, x, result);
  }
}

/*-*-*-*-*-*-*-*-*-* Functions w/ Natural Prototypes *-*-*-*-*-*-*-*-*-*-*/

#include "eval.h"

double gsl_sf_lnpoch(const double a, const double x)
{
  EVAL_RESULT(gsl_sf_lnpoch_e(a, x, &result));
}

double gsl_sf_poch(const double a, const double x)
{
  EVAL_RESULT(gsl_sf_poch_e(a, x, &result));
}

double gsl_sf_pochrel(const double a, const double x)
{
  EVAL_RESULT(gsl_sf_pochrel_e(a, x, &result));
}