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

#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_legendre.h>

#include "fftlog.h"

#include "ccl.h"
#include "ccl_params.h"

/*--------ROUTINE: taper_cl ------
TASK:n Apply cosine tapering to Cls to reduce aliasing
INPUT: number of ell bins for Cl, ell vector, C_ell vector, limits for tapering
       e.g., ell_limits=[low_ell_limit_lower,low_ell_limit_upper,high_ell_limit_lower,high_ell_limit_upper]
*/
static int taper_cl(int n_ell,double *ell,double *cl, double *ell_limits)
{

  for(int i=0;i<n_ell;i++) {
    if(ell[i]<ell_limits[0] || ell[i]>ell_limits[3]) {
      cl[i]=0;//ell outside desirable range
      continue;
    }
    if(ell[i]>=ell_limits[1] && ell[i]<=ell_limits[2])
      continue;//ell within good ell range

    if(ell[i]<ell_limits[1])//tapering low ell
      cl[i]*=cos((ell[i]-ell_limits[1])/(ell_limits[1]-ell_limits[0])*M_PI/2.);

    if(ell[i]>ell_limits[2])//tapering high ell
      cl[i]*=cos((ell[i]-ell_limits[2])/(ell_limits[3]-ell_limits[2])*M_PI/2.);
  }

  return 0;
}

/*--------ROUTINE: ccl_tracer_corr_fftlog ------
TASK: For a given tracer, get the correlation function
      Following function takes a function to calculate angular cl as well.
      By default above function will call it using ccl_angular_cl
INPUT: type of tracer, number of theta values to evaluate = NL, theta vector
 */
static void ccl_tracer_corr_fftlog(ccl_cosmology *cosmo,
				   int n_ell,double *ell,double *cls,
				   int n_theta,double *theta,double *wtheta,
				   int corr_type,int do_taper_cl,double *taper_cl_limits,
				   int *status)
{
  int i;
  double *l_arr,*cl_arr,*th_arr,*wth_arr;

  l_arr=ccl_log_spacing(ccl_splines->ELL_MIN_CORR,ccl_splines->ELL_MAX_CORR,ccl_splines->N_ELL_CORR);
  if(l_arr==NULL) {
    *status=CCL_ERROR_LINSPACE;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_fftlog ran out of memory\n");
    return;
  }
  cl_arr=malloc(ccl_splines->N_ELL_CORR*sizeof(double));
  if(cl_arr==NULL) {
    free(l_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_fftlog ran out of memory\n");
    return;
  }

  //Interpolate input Cl into array needed for FFTLog
  SplPar *cl_spl=ccl_spline_init(n_ell,ell,cls,cls[0],0);
  if(cl_spl==NULL) {
    free(l_arr);
    free(cl_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_fftlog ran out of memory\n");
    return;
  }

  double cl_tilt,l_edge,cl_edge;
  l_edge=ell[n_ell-1];
  if((cls[n_ell-1]*cls[n_ell-2]<0) || (cls[n_ell-2]==0)) {
    cl_tilt=0;
    cl_edge=0;
  }
  else {
    cl_tilt=log(cls[n_ell-1]/cls[n_ell-2])/log(ell[n_ell-1]/ell[n_ell-2]);
    cl_edge=cls[n_ell-1];
  }
  for(i=0;i<ccl_splines->N_ELL_CORR;i++) {
    if(l_arr[i]>=l_edge)
      cl_arr[i]=cl_edge*pow(l_arr[i]/l_edge,cl_tilt);
    else
      cl_arr[i]=ccl_spline_eval(l_arr[i],cl_spl);
  }
  ccl_spline_free(cl_spl);

  if (do_taper_cl)
    taper_cl(ccl_splines->N_ELL_CORR,l_arr,cl_arr,taper_cl_limits);

  th_arr=malloc(sizeof(double)*ccl_splines->N_ELL_CORR);
  if(th_arr==NULL) {
    free(l_arr);
    free(cl_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_fftlog ran out of memory\n");
    return;
  }
  wth_arr=(double *)malloc(sizeof(double)*ccl_splines->N_ELL_CORR);
  if(wth_arr==NULL) {
    free(l_arr); free(cl_arr); free(th_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_fftlog ran out of memory\n");
    return;
  }

  for(i=0;i<ccl_splines->N_ELL_CORR;i++)
    th_arr[i]=0;
  //Although set here to 0, theta is modified by FFTlog to obtain the correlation at ~1/l

  int i_bessel=0;
  if(corr_type==CCL_CORR_GG) i_bessel=0;
  if(corr_type==CCL_CORR_GL) i_bessel=2;
  if(corr_type==CCL_CORR_LP) i_bessel=0;
  if(corr_type==CCL_CORR_LM) i_bessel=4;
  fftlog_ComputeXi2D(i_bessel,ccl_splines->N_ELL_CORR,l_arr,cl_arr,th_arr,wth_arr);

  // Interpolate to output values of theta
  SplPar *wth_spl=ccl_spline_init(ccl_splines->N_ELL_CORR,th_arr,wth_arr,wth_arr[0],0);
  for(i=0;i<n_theta;i++)
    wtheta[i]=ccl_spline_eval(theta[i]*M_PI/180.,wth_spl);
  ccl_spline_free(wth_spl);

  free(l_arr); free(cl_arr);
  free(th_arr); free(wth_arr);

  return;
}

typedef struct {
  int nell;
  double ell0;
  double ellf;
  double cl0;
  double clf;
  int extrapol_0;
  int extrapol_f;
  double tilt0;
  double tiltf;
  SplPar *cl_spl;
  int i_bessel;
  double th;
} corr_int_par;

static double corr_bessel_integrand(double l,void *params)
{
  double cl,jbes;
  corr_int_par *p=(corr_int_par *)params;
  double x=l*p->th;

  if(l<p->ell0) {
    if(p->extrapol_0)
      cl=p->cl0*pow(l/p->ell0,p->tilt0);
    else
      cl=0;
  }
  else if(l>p->ellf) {
    if(p->extrapol_f)
      cl=p->clf*pow(l/p->ellf,p->tiltf);
    else
      cl=0;
  }
  else
    cl=ccl_spline_eval(l,p->cl_spl);

  jbes=gsl_sf_bessel_Jn(p->i_bessel,x);

  return l*jbes*cl;
}

static void ccl_tracer_corr_bessel(ccl_cosmology *cosmo,
				   int n_ell,double *ell,double *cls,
				   int n_theta,double *theta,double *wtheta,
				   int corr_type,int *status)
{
  corr_int_par *cp=malloc(sizeof(corr_int_par));
  if(cp==NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_bessel ran out of memory\n");
    return;
  }

  cp->nell=n_ell;
  cp->ell0=ell[0];
  cp->ellf=ell[n_ell-1];
  cp->cl0=cls[0];
  cp->clf=cls[n_ell-1];
  cp->cl_spl=ccl_spline_init(n_ell,ell,cls,cls[0],0);
  if(cp->cl_spl==NULL) {
    free(cp);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_bessel ran out of memory\n");
    return;
  }
  switch(corr_type) {
  case CCL_CORR_GG :
    cp->i_bessel=0;
    break;
  case CCL_CORR_GL :
    cp->i_bessel=2;
    break;
  case CCL_CORR_LP :
    cp->i_bessel=0;
    break;
  case CCL_CORR_LM :
    cp->i_bessel=4;
    break;
  }

  if(cls[0]*cls[1]<=0)
    cp->extrapol_0=0;
  else {
    cp->extrapol_0=1;
    cp->tilt0=log10(cls[1]/cls[0])/log10(ell[1]/ell[0]);
  }

  if(cls[n_ell-2]*cls[n_ell-1]<=0)
    cp->extrapol_f=0;
  else {
    cp->extrapol_f=1;
    cp->tiltf=log10(cls[n_ell-1]/cls[n_ell-2])/log10(ell[n_ell-1]/ell[n_ell-2]);
  }

  int ith, gslstatus;
  double result,eresult;
  gsl_function F;
  gsl_integration_workspace *w=gsl_integration_workspace_alloc(ccl_gsl->N_ITERATION);
  for(ith=0;ith<n_theta;ith++) {
    cp->th=theta[ith]*M_PI/180;
    F.function=&corr_bessel_integrand;
    F.params=cp;
    //TODO: Split into intervals between first bessel zeros before integrating
    //This will help both speed and accuracy of the integral.
    gslstatus = gsl_integration_qag(&F, 0, ccl_splines->ELL_MAX_CORR, 0,
                                    ccl_gsl->INTEGRATION_EPSREL, ccl_gsl->N_ITERATION,
                                    ccl_gsl->INTEGRATION_GAUSS_KRONROD_POINTS,
                                    w, &result, &eresult);
    if(gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_correlation.c: ccl_tracer_corr_bessel():");
      *status |= gslstatus;
    }
    wtheta[ith]=result/(2*M_PI);
  }
  gsl_integration_workspace_free(w);
  ccl_spline_free(cp->cl_spl);
  free(cp);
}

/*--------ROUTINE: ccl_compute_legendre_polynomial ------
TASK: Compute input factor for ccl_tracer_corr_legendre
INPUT: tracer 1, tracer 2, i_bessel, theta array, n_theta, L_max, output Pl_theta
 */
static void ccl_compute_legendre_polynomial(int corr_type,double theta,int ell_max,double *Pl_theta)
{
  int i,j;
  double k=0;
  double cth=cos(theta*M_PI/180);

  //Initialize Pl_theta
  for (j=0;j<=ell_max;j++)
      Pl_theta[j]=0.;

  if(corr_type==CCL_CORR_GG) {
    gsl_sf_legendre_Pl_array(ell_max,cth,Pl_theta);
    for (j=0;j<=ell_max;j++)
      Pl_theta[j]*=(2*j+1);
  }
  else if(corr_type==CCL_CORR_GL) {
    for (j=2;j<=ell_max;j++) {//https://arxiv.org/pdf/1007.4809.pdf
      Pl_theta[j]=gsl_sf_legendre_Plm(j,2,cth);
      Pl_theta[j]*=(2*j+1.)/((j+0.)*(j+1.));
    }
  }
}

/*--------ROUTINE: ccl_tracer_corr_legendre ------
TASK: Compute correlation function via Legendre polynomials
INPUT: cosmology, number of theta bins, theta array, tracer 1, tracer 2, i_bessel, boolean
       for tapering, vector of tapering limits, correlation vector, angular_cl function.
 */
static void ccl_tracer_corr_legendre(ccl_cosmology *cosmo,
				     int n_ell,double *ell,double *cls,
				     int n_theta,double *theta,double *wtheta,
				     int corr_type,int do_taper_cl,double *taper_cl_limits,
				     int *status)
{
  int i;
  double *l_arr,*cl_arr,*Pl_theta;
  SplPar *cl_spl;

  if(corr_type==CCL_CORR_LM || corr_type==CCL_CORR_LP){
    *status=CCL_ERROR_NOT_IMPLEMENTED;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: CCL does not support full-sky xi+- calcuations.\nhttps://arxiv.org/abs/1702.05301 indicates flat-sky to be sufficient.\n");
  }

  if(*status==0) {
    l_arr=malloc(((int)(ccl_splines->ELL_MAX_CORR)+1)*sizeof(double));
    if(l_arr==NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_legendre ran out of memory\n");
    }
  }

  if(*status==0) {
    cl_arr=malloc(((int)(ccl_splines->ELL_MAX_CORR)+1)*sizeof(double));
    if(cl_arr==NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_legendre ran out of memory\n");
    }
  }

  if(*status==0) {
    //Interpolate input Cl into
    cl_spl=ccl_spline_init(n_ell,ell,cls,cls[0],0);
    if(cl_spl==NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_legendre ran out of memory\n");
    }
  }

  if(*status==0) {
    double cl_tilt,l_edge,cl_edge;
    l_edge=ell[n_ell-1];
    if((cls[n_ell-1]*cls[n_ell-2]<0) || (cls[n_ell-2]==0)) {
      cl_tilt=0;
      cl_edge=0;
    }
    else {
      cl_tilt=log(cls[n_ell-1]/cls[n_ell-2])/log(ell[n_ell-1]/ell[n_ell-2]);
      cl_edge=cls[n_ell-1];
    }
    for(i=0;i<=(int)(ccl_splines->ELL_MAX_CORR);i++) {
      double l=(double)i;
      l_arr[i]=l;
      if(l>=l_edge)
	cl_arr[i]=cl_edge*pow(l/l_edge,cl_tilt);
      else
	cl_arr[i]=ccl_spline_eval(l,cl_spl);
    }
    ccl_spline_free(cl_spl);

    if (do_taper_cl)
      *status=taper_cl((int)(ccl_splines->ELL_MAX_CORR)+1,l_arr,cl_arr,taper_cl_limits);
  }

  if(*status==0) {
    Pl_theta=malloc(sizeof(double)*((int)(ccl_splines->ELL_MAX_CORR)+1));
    if(Pl_theta==NULL) {
      *status=CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_tracer_corr_legendre ran out of memory\n");
    }
  }

  if(*status==0) {
    for (int i=0;i<n_theta;i++) {
      wtheta[i]=0;
      ccl_compute_legendre_polynomial(corr_type,theta[i],(int)(ccl_splines->ELL_MAX_CORR),Pl_theta);
      for(int i_L=1;i_L<(int)(ccl_splines->ELL_MAX_CORR);i_L+=1)
	wtheta[i]+=cl_arr[i_L]*Pl_theta[i_L];
      wtheta[i]/=(M_PI*4);
    }
  }

  free(Pl_theta);
  free(l_arr);
  free(cl_arr);
}

/*--------ROUTINE: ccl_tracer_corr ------
TASK: For a given tracer, get the correlation function. Do so by running
      ccl_angular_cls. If you already have Cls calculated, go to the next
      function to pass them directly.
INPUT: cosmology, number of theta values to evaluate = NL, theta vector,
       tracer 1, tracer 2, i_bessel, key for tapering, limits of tapering
       correlation function.
 */
void ccl_correlation(ccl_cosmology *cosmo,
		     int n_ell,double *ell,double *cls,
		     int n_theta,double *theta,double *wtheta,
		     int corr_type,int do_taper_cl,double *taper_cl_limits,int flag_method,
		     int *status)
{
  switch(flag_method) {
  case CCL_CORR_FFTLOG :
    ccl_tracer_corr_fftlog(cosmo,n_ell,ell,cls,n_theta,theta,wtheta,corr_type,
			   do_taper_cl,taper_cl_limits,status);
    break;
  case CCL_CORR_LGNDRE :
    ccl_tracer_corr_legendre(cosmo,n_ell,ell,cls,n_theta,theta,wtheta,corr_type,
			     do_taper_cl,taper_cl_limits,status);
    break;
  case CCL_CORR_BESSEL :
    ccl_tracer_corr_bessel(cosmo,n_ell,ell,cls,n_theta,theta,wtheta,corr_type,status);
    break;
  default :
    *status=CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_correlation. Unknown algorithm\n");
  }

  ccl_check_status(cosmo,status);
}

/*--------ROUTINE: ccl_correlation_3d ------
TASK: Calculate the 3d-correlation function. Do so by using FFTLog.

INPUT: cosmology, scale factor a,
       number of r values, r values,
       key for tapering, limits of tapering

Correlation function result will be in array xi
 */

void ccl_correlation_3d(ccl_cosmology *cosmo, double a,
			int n_r,double *r,double *xi,
			int do_taper_pk,double *taper_pk_limits,
			int *status)
{
  int i,N_ARR;
  double *k_arr,*pk_arr,*r_arr,*xi_arr;

  //number of data points for k and pk array
  N_ARR=(int)(ccl_splines->N_K_3DCOR*log10(ccl_splines->K_MAX/ccl_splines->K_MIN));

  k_arr=ccl_log_spacing(ccl_splines->K_MIN,ccl_splines->K_MAX,N_ARR);
  if(k_arr==NULL) {
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_correlation_3d ran out of memory\n");
    return;
  }

  pk_arr=malloc(N_ARR*sizeof(double));
  if(pk_arr==NULL) {
    free(k_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_correlation_3d ran out of memory\n");
    return;
  }

  for (i=0; i<N_ARR; i++)
    pk_arr[i] = ccl_nonlin_matter_power(cosmo, k_arr[i], a, status);

  if (do_taper_pk)
    taper_cl(N_ARR,k_arr,pk_arr,taper_pk_limits);

  r_arr=malloc(sizeof(double)*N_ARR);
  if(r_arr==NULL) {
    free(k_arr);
    free(pk_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_correlation_3d ran out of memory\n");
    return;
  }
  xi_arr=malloc(sizeof(double)*N_ARR);
  if(xi_arr==NULL) {
    free(k_arr); free(pk_arr); free(r_arr);
    *status=CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(cosmo, "ccl_correlation.c: ccl_correlation_3d ran out of memory\n");
    return;
  }

  for(i=0;i<N_ARR;i++)
    r_arr[i]=0;

  pk2xi(N_ARR,k_arr,pk_arr,r_arr,xi_arr);

  // Interpolate to output values of r
  SplPar *xi_spl=ccl_spline_init(N_ARR,r_arr,xi_arr,xi_arr[0],0);
  for(i=0;i<n_r;i++)
    xi[i]=ccl_spline_eval(r[i],xi_spl);
  ccl_spline_free(xi_spl);

  free(k_arr); free(pk_arr);
  free(r_arr); free(xi_arr);

  ccl_check_status(cosmo,status);

  return;
}


// https://github.com/attractivechaos/matmul
#include <stdlib.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

/**********************************
 * Pseudo-random number generator *
 **********************************/

static uint64_t mat_rng[2] = { 11ULL, 1181783497276652981ULL };

static inline uint64_t xorshift128plus(uint64_t s[2])
{
	uint64_t x, y;
	x = s[0], y = s[1];
	s[0] = y;
	x ^= x << 23;
	s[1] = x ^ y ^ (x >> 17) ^ (y >> 26);
	y += s[1];
	return y;
}

double mat_drand(void)
{
	return (xorshift128plus(mat_rng)>>11) * (1.0/9007199254740992.0);
}

/*******************************************
 * Helper routines for matrix manipulation *
 *******************************************/

float **mat_init(int n_rows, int n_cols)
{
	float **m;
	int i;
	m = (float**)malloc(n_rows * sizeof(float*));
	m[0] = (float*)calloc(n_rows * n_cols, sizeof(float));
	for (i = 1; i < n_rows; ++i)
		m[i] = m[i-1] + n_cols;
	return m;
}

void mat_destroy(float **m)
{
	free(m[0]); free(m);
}

float **mat_gen_random(int n_rows, int n_cols)
{
	float **m;
	int i, j;
	m = mat_init(n_rows, n_cols);
	for (i = 0; i < n_rows; ++i)
		for (j = 0; j < n_cols; ++j)
			m[i][j] = mat_drand();
	return m;
}

float **mat_transpose(int n_rows, int n_cols, float *const* a)
{
	int i, j;
	float **m;
	m = mat_init(n_cols, n_rows);
	for (i = 0; i < n_rows; ++i)
		for (j = 0; j < n_cols; ++j)
			m[j][i] = a[i][j];
	return m;
}

float sdot_1(int n, const float *x, const float *y)
{
	int i;
	float s = 0.0f;
	for (i = 0; i < n; ++i) s += x[i] * y[i];
	return s;
}

float sdot_8(int n, const float *x, const float *y)
{
	int i, n8 = n>>3<<3;
	float s, t[8];
	t[0] = t[1] = t[2] = t[3] = t[4] = t[5] = t[6] = t[7] = 0.0f;
	for (i = 0; i < n8; i += 8) {
		t[0] += x[i+0] * y[i+0];
		t[1] += x[i+1] * y[i+1];
		t[2] += x[i+2] * y[i+2];
		t[3] += x[i+3] * y[i+3];
		t[4] += x[i+4] * y[i+4];
		t[5] += x[i+5] * y[i+5];
		t[6] += x[i+6] * y[i+6];
		t[7] += x[i+7] * y[i+7];
	}
	for (s = 0.0f; i < n; ++i) s += x[i] * y[i];
	s += t[0] + t[1] + t[2] + t[3] + t[4] + t[5] + t[6] + t[7];
	return s;
}

#ifdef __SSE__
#include <xmmintrin.h>

float sdot_sse(int n, const float *x, const float *y)
{
	int i, n8 = n>>3<<3;
	__m128 vs1, vs2;
	float s, t[4];
	vs1 = _mm_setzero_ps();
	vs2 = _mm_setzero_ps();
	for (i = 0; i < n8; i += 8) {
		__m128 vx1, vx2, vy1, vy2;
		vx1 = _mm_loadu_ps(&x[i]);
		vx2 = _mm_loadu_ps(&x[i+4]);
		vy1 = _mm_loadu_ps(&y[i]);
		vy2 = _mm_loadu_ps(&y[i+4]);
		vs1 = _mm_add_ps(vs1, _mm_mul_ps(vx1, vy1));
		vs2 = _mm_add_ps(vs2, _mm_mul_ps(vx2, vy2));
	}
	for (s = 0.0f; i < n; ++i) s += x[i] * y[i];
	_mm_storeu_ps(t, vs1);
	s += t[0] + t[1] + t[2] + t[3];
	_mm_storeu_ps(t, vs2);
	s += t[0] + t[1] + t[2] + t[3];
	return s;
}
#endif

/*************************
 * Matrix multiplication *
 *************************/

float **mat_mul0(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, k;
	float **m;
	m = mat_init(n_a_rows, n_b_cols);
	for (i = 0; i < n_a_rows; ++i) {
		for (j = 0; j < n_b_cols; ++j) {
			float t = 0.0;
			for (k = 0; k < n_a_cols; ++k)
				t += a[i][k] * b[k][j];
			m[i][j] = t;
		}
	}
	return m;
}

float **mat_mul1(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, k, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i) {
		const float *ai = a[i];
		float *mi = m[i];
		for (j = 0; j < n_b_cols; ++j) {
			float t = 0.0f, *bTj = bT[j];
			for (k = 0; k < n_a_cols; ++k)
				t += ai[k] * bTj[k];
			mi[j] = t;
		}
	}
	mat_destroy(bT);
	return m;
}

#ifdef __SSE__
float **mat_mul2(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_sse(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}
float **mat_mul7(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, ii, jj, x = 16, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; i += x) {
		for (j = 0; j < n_b_cols; j += x) {
			int je = n_b_cols < j + x? n_b_cols : j + x;
			int ie = n_a_rows < i + x? n_a_rows : i + x;
			for (ii = i; ii < ie; ++ii)
				for (jj = j; jj < je; ++jj)
					m[ii][jj] += sdot_sse(n_a_cols, a[ii], bT[jj]);
		}
	}
	mat_destroy(bT);
	return m;
}
#endif

float **mat_mul3(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_8(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}

float **mat_mul4(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = sdot_1(n_a_cols, a[i], bT[j]);
	mat_destroy(bT);
	return m;
}

#ifdef HAVE_CBLAS
#include <cblas.h>

float **mat_mul5(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	int i, j, n_b_rows = n_a_cols;
	float **m, **bT;
	m = mat_init(n_a_rows, n_b_cols);
	bT = mat_transpose(n_b_rows, n_b_cols, b);
	for (i = 0; i < n_a_rows; ++i)
		for (j = 0; j < n_b_cols; ++j)
			m[i][j] = cblas_sdot(n_a_cols, a[i], 1, bT[j], 1);
	mat_destroy(bT);
	return m;
}

float **mat_mul6(int n_a_rows, int n_a_cols, float *const *a, int n_b_cols, float *const *b)
{
	float **m, n_b_rows = n_a_cols;
	m = mat_init(n_a_rows, n_b_cols);
	cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n_a_rows, n_b_cols, n_a_cols, 1.0f, a[0], n_a_rows, b[0], n_b_rows, 0.0f, m[0], n_a_rows);
	return m;
}
#endif

/*****************
 * Main function *
 *****************/

#include <unistd.h>
#include <time.h>

int main(int argc, char *argv[])
{
	int c, n = 1000, algo = 2;
	clock_t t;
	float **a, **b, **m = 0;

	while ((c = getopt(argc, argv, "n:a:h")) >= 0) {
		if (c == 'n') n = atoi(optarg);
		else if (c == 'a') algo = atoi(optarg);
		else if (c == 'h') {
			fprintf(stderr, "Usage: mat-eval [options]\n");
			fprintf(stderr, "Options:\n");
			fprintf(stderr, "  -n INT    size of the square matrix [%d]\n", n);
			fprintf(stderr, "  -a INT    matrix multiplication implementation [%d]\n", algo);
			fprintf(stderr, "            0: naive - no optimization\n");
			fprintf(stderr, "            1: transposing the second matrix\n");
#ifdef __SSE__
			fprintf(stderr, "            2: explicitly vectorized sdot() with SSE\n");
			fprintf(stderr, "            7: explicitly SSE sdot() plus loop tiling\n");
#endif
			fprintf(stderr, "            3: implicitly vectorized sdot()\n");
			fprintf(stderr, "            4: no vectorization hints\n");
#ifdef HAVE_CBLAS
			fprintf(stderr, "            5: with sdot() from an external CBLAS library\n");
			fprintf(stderr, "            6: with sgemm() from an external CBLAS library\n");
#endif
			fprintf(stderr, "  -h        this help message\n");
			return 1;
		}
	}

	a = mat_gen_random(n, n);
	b = mat_gen_random(n, n);

	t = clock();
	if (algo == 0) {
		m = mat_mul0(n, n, a, n, b);
	} else if (algo == 1) {
		m = mat_mul1(n, n, a, n, b);
#ifdef __SSE__
	} else if (algo == 2) {
		m = mat_mul2(n, n, a, n, b);
	} else if (algo == 7) {
		m = mat_mul7(n, n, a, n, b);
#endif
	} else if (algo == 3) {
		m = mat_mul3(n, n, a, n, b);
	} else if (algo == 4) {
		m = mat_mul4(n, n, a, n, b);
#ifdef HAVE_CBLAS
	} else if (algo == 5) {
		m = mat_mul5(n, n, a, n, b);
	} else if (algo == 6) {
		m = mat_mul6(n, n, a, n, b);
#endif
	} else {
		fprintf(stderr, "ERROR: unknown algorithm %d\n", algo);
		return 1;
	}
	fprintf(stderr, "CPU time: %g\n", (double)(clock() - t) / CLOCKS_PER_SEC);
	fprintf(stderr, "Central cell: %g\n", m[n/2][n/2]);

	if (m) mat_destroy(m);
	mat_destroy(b); mat_destroy(a);
	return 0;
}


#include <assert.h>
#include <gsl/gsl_sf_gamma.h>
#include <inttypes.h>
#include <math.h>
#include <stdlib.h>

#include "index.h"
#include "parse_mmaps.h"
#include "probability.h"

double reference_probability(const struct mmap_info* mmap_info,
			     struct revision_assignment* parent,
			     struct revision_assignment* child,
			     int disagrees,
			     const struct index_patch* index_patch) {
  double action_prob = 1.0;
  if (parent != NULL && parent->topic >= 0 && parent->pov >= 0
      && child->topic >= 0 && child->pov >= 0) {
    struct revision_assignment_header* revision_assignment_header
      = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
    struct topic_summary* topic_summary;
    struct pov_summary* pov_dist;
    get_topic_summary(mmap_info, child->topic, &topic_summary, &pov_dist, NULL);
    if (parent->topic == child->topic) {
      if (parent->pov != child->pov) {
	// POV reference
	struct pov_summary* pov_summary = get_ant_pov(mmap_info, pov_dist,
						      child->pov, parent->pov);
	double denom = (double)(pov_summary->norevert_count
				+ pov_summary->revert_count)
	  + revision_assignment_header->psi_alpha
	  + revision_assignment_header->psi_beta;
	int correction = 0;
	if (index_patch != NULL
	    && index_patch->topic == child->topic
	    && index_patch->pov == child->pov
	    && index_patch->parent_pov == parent->pov
	    && index_patch->parent_topic == index_patch->topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->disagrees)) {
	    correction += 1;
	  }
	}
	if (index_patch != NULL
	    && index_patch->child_topic == child->topic
	    && index_patch->child_pov == child->pov
	    && index_patch->pov == parent->pov
	    && index_patch->topic == index_patch->child_topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->child_disagrees)) {
	    correction += 1;
	  }
	}
	if (disagrees) {
	  action_prob *= ((double)(pov_summary->revert_count
				   - correction)
			  + revision_assignment_header->psi_alpha) / denom;
	} else {
	  action_prob *= ((double)(pov_summary->norevert_count
				   - correction)
			  + revision_assignment_header->psi_beta) / denom;
	}
      } else {
	// topic reference
	double denom = (double)(topic_summary->revert_topic_count
				+ topic_summary->norevert_topic_count
				+ revision_assignment_header->gamma_alpha
				+ revision_assignment_header->gamma_beta);
	int correction = 0;
	if (index_patch != NULL
	    && index_patch->topic == child->topic
	    && index_patch->parent_pov == index_patch->pov
	    && index_patch->parent_topic == index_patch->topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->disagrees)) {
	    correction += 1;
	  }
	}
	if (index_patch != NULL
	    && index_patch->child_topic == child->topic
	    && index_patch->pov == index_patch->child_pov
	    && index_patch->topic == index_patch->child_topic) {
	  assert(denom -= 1.0 >= 0.0);
	  if (!disagrees == !(index_patch->child_disagrees)) {
	    correction += 1;
	  }
	}
	if (disagrees) {
	  action_prob *= ((double)(topic_summary->revert_topic_count
				   - correction)
			  + revision_assignment_header->gamma_alpha) / denom;
	} else {
	  action_prob *= ((double)(topic_summary->norevert_topic_count
				   - correction)
			  + revision_assignment_header->gamma_beta) / denom;
	}
      }
    } else {
      // general reference
      double denom = (double)(topic_summary->revert_general_count
			      + topic_summary->norevert_general_count)
	+ revision_assignment_header->gamma_alpha
	+ revision_assignment_header->gamma_beta;
      int correction = 0;
      if (index_patch != NULL
	  && index_patch->parent_topic >= 0
	  && index_patch->topic == child->topic
	  && index_patch->parent_topic != index_patch->topic) {
	assert(denom -= 1.0 >= 0.0);
	if (!disagrees == !(index_patch->disagrees)) {
	  correction += 1;
	}
      }
      if (index_patch != NULL
	  && index_patch->topic >= 0
	  && index_patch->child_topic == child->topic
	  && index_patch->topic != index_patch->child_topic) {
	assert(denom -= 1.0 >= 0.0);
	if (!disagrees == !(index_patch->child_disagrees)) {
	  correction = 1;
	}
      }
      if (disagrees) {
	action_prob *= ((double)(topic_summary->revert_general_count
				 - correction)
			+ revision_assignment_header->gamma_alpha) / denom;
      } else {
	action_prob *= ((double)(topic_summary->norevert_general_count
				 - correction)
			+ revision_assignment_header->gamma_beta) / denom;
      }
    }
  } // Else, a constant not dependant on POV or topic
  assert(action_prob > 0.0);
  return action_prob;
}

double revision_probability(const struct mmap_info* mmap_info, int64_t revision_id,
			    int32_t topic, int32_t pov,
			    int include_child,
			    int include_user_topic_pov,
			    const struct index_patch* index_patch) {
  const struct revision* revision = get_revision(mmap_info, revision_id);
  struct revision_assignment current_assignment;
  current_assignment.topic = topic;
  current_assignment.pov = pov;
  double ret = 1.0;
  // Revert probability
  if (revision->parent >= 0) {
    ret *= reference_probability(mmap_info,
				 get_revision_assignment(mmap_info, revision->parent),
				 &current_assignment, revision->disagrees,
				 index_patch);
  }
  if (revision->child >= 0 && include_child) {
    ret *= reference_probability(mmap_info, &current_assignment,
				 get_revision_assignment(mmap_info, revision->child),
				 get_revision(mmap_info, revision->child)->disagrees,
				 index_patch);
  }

  // Topic, pov probability
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  if (include_user_topic_pov) {
    int pov_correction = 0;
    if (index_patch
	&& index_patch->user == revision->user
	&& pov == index_patch->pov
	&& topic == index_patch->topic) {
      pov_correction = 1;
    }

    double* user_pov_dist;
    get_user_topics(mmap_info, revision->user, &user_pov_dist);
    // Alpha is already added to this array. The normalizing constant is
    // independant of topic/pov, so we exclude it here.
    ret *= (user_pov_dist[topic * revision_assignment_header->pov_per_topic + pov]
	    - pov_correction);
  }

  // Page probability
  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  int64_t* page_dist;
  struct topic_summary* topic_summary;
  get_topic_summary(mmap_info, topic, &topic_summary, NULL, &page_dist);
  int64_t topic_page_revisions = page_dist[revision->article];
  int64_t topic_revisions = topic_summary->total_revisions;
  if (index_patch && index_patch->topic == topic) {
    topic_revisions -= 1;
    if (index_patch->page == revision->article) {
      topic_page_revisions -= 1;
    }
  }
  ret *= ((double)(topic_page_revisions) + revision_assignment_header->beta)
    / ((double)(topic_revisions)
       + revision_assignment_header->beta * topic_summary_header->num_pages);
  assert(ret > 0.0);
  return ret;
}

void fill_index_patch(const struct mmap_info* mmap_info,
		      int64_t revision_id,
		      struct index_patch* index_patch) {
  const struct revision* revision = get_revision(mmap_info, revision_id);
  index_patch->user = revision->user;
  index_patch->page = revision->article;
  const struct revision_assignment* revision_assignment
    = get_revision_assignment(mmap_info, revision_id);
  index_patch->topic = revision_assignment->topic;
  index_patch->pov = revision_assignment->pov;
  index_patch->disagrees = revision->disagrees;
  if (revision->parent >= 0) {
    const struct revision_assignment* parent_assignment
      = get_revision_assignment(mmap_info, revision->parent);
    index_patch->parent_topic = parent_assignment->topic;
    index_patch->parent_pov = parent_assignment->pov;
  } else {
    index_patch->parent_topic = -1;
    index_patch->parent_pov = -1;
  }
  if (revision->child >= 0) {
    const struct revision_assignment* child_assignment
      = get_revision_assignment(mmap_info, revision->child);
    index_patch->child_topic = child_assignment->topic;
    index_patch->child_pov = child_assignment->pov;
    index_patch->child_disagrees = get_revision(mmap_info,
						revision->child)->disagrees;
  } else {
    index_patch->child_topic = -1;
    index_patch->child_pov = -1;
    index_patch->child_disagrees = -1;
  }
}

double users_pages_probability_modn(const struct mmap_info* mmap_info, int sample, int modn) {
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  struct user_topic_header* user_topic_header = (struct user_topic_header*)mmap_info->user_topic_mmap;
  double log_likelihood = 0.0;
  int64_t user_edits;
  double* user_topic_pov_dist;
  int topic_pov_count
    = revision_assignment_header->num_topics * revision_assignment_header->pov_per_topic;
  for (int64_t user_num = sample; user_num < user_topic_header->num_users; user_num += modn) {
    get_user(mmap_info, user_num, &user_edits, NULL);
    get_user_topics(mmap_info, user_num, &user_topic_pov_dist);
    if (user_edits <= 0) {
      // This user does not actually exist
      continue;
    }
    log_likelihood -= gsl_sf_lngamma(user_edits + revision_assignment_header->alpha * topic_pov_count);
    for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
      for (int pov = 0; pov < revision_assignment_header->pov_per_topic; ++pov) {
	log_likelihood += gsl_sf_lngamma(user_topic_pov_dist[topic * revision_assignment_header->pov_per_topic
							     + pov]);
      }
    }
  }
  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  int64_t* page_dist;
  for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
    get_topic_summary(mmap_info, topic, NULL, NULL, &page_dist);
    for (int64_t page = sample; page < topic_summary_header->num_pages; page += modn) {
      log_likelihood += gsl_sf_lngamma(page_dist[page] + revision_assignment_header->beta);
    }
  }
  return log_likelihood;
}

double log_likelihood(const struct mmap_info* mmap_info) {
  return log_likelihood_gamma(mmap_info, 1);
}

double log_likelihood_gamma(const struct mmap_info* mmap_info, int include_users_pages) {
  struct user_topic_header* user_topic_header = (struct user_topic_header*)mmap_info->user_topic_mmap;
  struct revision_assignment_header* revision_assignment_header
    = (struct revision_assignment_header*)mmap_info->revision_assignment_mmap;
  int topic_pov_count
    = revision_assignment_header->num_topics * revision_assignment_header->pov_per_topic;
  double log_likelihood = 0.0;
  log_likelihood += user_topic_header->num_users
    * gsl_sf_lngamma(revision_assignment_header->alpha * topic_pov_count);
  log_likelihood -= user_topic_header->num_users * topic_pov_count
    * gsl_sf_lngamma(revision_assignment_header->alpha);
  if (include_users_pages) {
    log_likelihood += users_pages_probability_modn(mmap_info, 0, 1);
  }

  struct topic_summary_header* topic_summary_header
    = (struct topic_summary_header*)mmap_info->topic_index_mmap;
  struct topic_summary* topic_summary;
  struct pov_summary* pov_dist;
  struct pov_summary* pov_summary;
  log_likelihood += revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->beta * topic_summary_header->num_pages);
  log_likelihood -= revision_assignment_header->num_topics * topic_summary_header->num_pages
    * gsl_sf_lngamma(revision_assignment_header->beta);
  for (int topic = 0; topic < revision_assignment_header->num_topics; ++topic) {
    get_topic_summary(mmap_info, topic, &topic_summary, &pov_dist, NULL);
    log_likelihood -= gsl_sf_lngamma(topic_summary->total_revisions + revision_assignment_header->beta
				     * topic_summary_header->num_pages);
    // General reverts
    log_likelihood += gsl_sf_lngamma(topic_summary->revert_general_count
				     + revision_assignment_header->gamma_alpha);
    log_likelihood += gsl_sf_lngamma(topic_summary->norevert_general_count
				     + revision_assignment_header->gamma_beta);
    log_likelihood -= gsl_sf_lngamma(topic_summary->revert_general_count
				     + topic_summary->norevert_general_count
				     + revision_assignment_header->gamma_alpha
				     + revision_assignment_header->gamma_beta);
    // Topic reverts
    log_likelihood += gsl_sf_lngamma(topic_summary->revert_topic_count
				     + revision_assignment_header->gamma_alpha);
    log_likelihood += gsl_sf_lngamma(topic_summary->norevert_topic_count
				     + revision_assignment_header->gamma_beta);
    log_likelihood -= gsl_sf_lngamma(topic_summary->revert_topic_count
				     + topic_summary->norevert_topic_count
				     + revision_assignment_header->gamma_alpha
				     + revision_assignment_header->gamma_beta);

    // POV reverts
    for (int pov = 0; pov < revision_assignment_header->pov_per_topic; ++pov) {
      for (int ant_pov = 0; ant_pov < revision_assignment_header->pov_per_topic - 1; ++ant_pov) {
	pov_summary = pov_dist + pov * (revision_assignment_header->pov_per_topic - 1) + ant_pov;
	log_likelihood += gsl_sf_lngamma(pov_summary->revert_count
					 + revision_assignment_header->psi_alpha);
	log_likelihood += gsl_sf_lngamma(pov_summary->norevert_count
					 + revision_assignment_header->psi_beta);
	log_likelihood -= gsl_sf_lngamma(pov_summary->revert_count
					 + pov_summary->norevert_count
					 + revision_assignment_header->psi_alpha
					 + revision_assignment_header->psi_beta);
      }
    }
  }
  log_likelihood += 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_alpha + revision_assignment_header->gamma_beta);
  log_likelihood -= 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_alpha);
  log_likelihood -= 2 * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->gamma_beta);

  log_likelihood += revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_alpha + revision_assignment_header->psi_beta);
  log_likelihood -= revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_alpha);
  log_likelihood -= revision_assignment_header->pov_per_topic
    * (revision_assignment_header->pov_per_topic - 1)
    * revision_assignment_header->num_topics
    * gsl_sf_lngamma(revision_assignment_header->psi_beta);

  return log_likelihood;
}


/* specfunc/zeta.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2004 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 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.
 */

/* Author:  G. Jungman */

/* This file was taken from the GNU Scientific Library. Some modifications
 * were done in order to make it independent from the rest of GSL
 */

/*
#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_elementary.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_zeta.h>

#include "error.h"

#include "chebyshev.h"
#include "cheb_eval.c"
*/

#include <math.h>
#include <stdio.h>
#include "error.h"

/*-*-*-*-*-*-*-*-*-*- From gsl_machine.h -*-*-*-*-*-*-*-*-*-*-*-*-*/

#define GSL_LOG_DBL_MIN   (-7.0839641853226408e+02)
#define GSL_LOG_DBL_MAX    7.0978271289338397e+02
#define GSL_DBL_EPSILON        2.2204460492503131e-16

/*-*-*-*-*-*-*-*-*-* From gsl_sf_result.h *-*-*-*-*-*-*-*-*-*-*-*/

struct gsl_sf_result_struct {
  double val;
  double err;
};
typedef struct gsl_sf_result_struct gsl_sf_result;

/*-*-*-*-*-*-*-*-*-*-*-* Private Section *-*-*-*-*-*-*-*-*-*-*-*/

/* coefficients for Maclaurin summation in hzeta()
 * B_{2j}/(2j)!
 */
static double hzeta_c[15] = {
  1.00000000000000000000000000000,
  0.083333333333333333333333333333,
 -0.00138888888888888888888888888889,
  0.000033068783068783068783068783069,
 -8.2671957671957671957671957672e-07,
  2.0876756987868098979210090321e-08,
 -5.2841901386874931848476822022e-10,
  1.3382536530684678832826980975e-11,
 -3.3896802963225828668301953912e-13,
  8.5860620562778445641359054504e-15,
 -2.1748686985580618730415164239e-16,
  5.5090028283602295152026526089e-18,
 -1.3954464685812523340707686264e-19,
  3.5347070396294674716932299778e-21,
 -8.9535174270375468504026113181e-23
};

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

static int gsl_sf_hzeta_e(const double s, const double q, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(s <= 1.0 || q <= 0.0) {
	PLFIT_ERROR("s must be larger than 1.0 and q must be larger than zero", PLFIT_EINVAL);
  }
  else {
    const double max_bits = 54.0;
    const double ln_term0 = -s * log(q);

    if(ln_term0 < GSL_LOG_DBL_MIN + 1.0) {
	  PLFIT_ERROR("underflow", PLFIT_UNDRFLOW);
    }
    else if(ln_term0 > GSL_LOG_DBL_MAX - 1.0) {
	  PLFIT_ERROR("overflow", PLFIT_OVERFLOW);
    }
    else if((s > max_bits && q < 1.0) || (s > 0.5*max_bits && q < 0.25)) {
      result->val = pow(q, -s);
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return PLFIT_SUCCESS;
    }
    else if(s > 0.5*max_bits && q < 1.0) {
      const double p1 = pow(q, -s);
      const double p2 = pow(q/(1.0+q), s);
      const double p3 = pow(q/(2.0+q), s);
      result->val = p1 * (1.0 + p2 + p3);
      result->err = GSL_DBL_EPSILON * (0.5*s + 2.0) * fabs(result->val);
      return PLFIT_SUCCESS;
    }
    else {
      /* Euler-Maclaurin summation formula
       * [Moshier, p. 400, with several typo corrections]
       */
      const int jmax = 12;
      const int kmax = 10;
      int j, k;
      const double pmax  = pow(kmax + q, -s);
      double scp = s;
      double pcp = pmax / (kmax + q);
      double ans = pmax*((kmax+q)/(s-1.0) + 0.5);

      for(k=0; k<kmax; k++) {
        ans += pow(k + q, -s);
      }

      for(j=0; j<=jmax; j++) {
        double delta = hzeta_c[j+1] * scp * pcp;
        ans += delta;
        if(fabs(delta/ans) < 0.5*GSL_DBL_EPSILON) break;
        scp *= (s+2*j+1)*(s+2*j+2);
        pcp /= (kmax + q)*(kmax + q);
      }

      result->val = ans;
      result->err = 2.0 * (jmax + 1.0) * GSL_DBL_EPSILON * fabs(ans);
      return PLFIT_SUCCESS;
    }
  }
}

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

double gsl_sf_hzeta(const double s, const double a)
{
  gsl_sf_result result;
  gsl_sf_hzeta_e(s, a, &result);
  return result.val;
}


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

#include <gsl/gsl_sf.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "LSAbundance.h"

static int  verbose  = FALSE;

static int  nLines   = 9;

static char *usage[] = {"LSAbundance - \n",
                        "Required parameters:\n",
			"   -a    abundance data\n",
			"   -in   filename      parameter file  \n",
                        "Optional:\n",
			"   -b    integer       length of sample file to ignore\n",
			"   -s    integer       sampling frequency\n",
			"   -seed long          seed random number generator\n",
			"   -v                  verbose\n"};

void updateExpectations(double* adExpect, int nMax, double dMDash, double dV, double dNu, int nS, t_Data *ptData)
{
  int i = 0;

  for(i = 1; i <= nMax; i++){
    int nA = i;
    double dLog = 0.0, dP = 0.0;

    if(nA < MAX_QUAD){
      dLog = logLikelihoodQuad(nA, dMDash, dV, dNu);
    }
    else{
      dLog = logLikelihoodRampal(nA, dMDash, dV, dNu);
    }

    dP = exp(dLog);

    adExpect[i - 1]+= dP*nS;
  }
}

int main(int argc, char* argv[]){
  t_Params    tParams;
  t_LSParams* atLSParams;
  int         i = 0, nSamples = 0, nMax = 0;
  t_Data      tData;
  double*     adExpect = NULL;

  gsl_set_error_handler_off();

  getCommandLineParams(&tParams, argc, argv);

  /*allocate memory for samples*/
  atLSParams = (t_LSParams *) malloc(MAX_SAMPLES*sizeof(t_LSParams));
  if(!atLSParams)
    goto memoryError;

  /*read in Monte-Carlo samples*/
  readSamples(&tParams, atLSParams, &nSamples);

  readAbundanceData(tParams.szAbundFile, &tData);

  nMax = tData.aanAbund[tData.nNA - 1][0];
  nMax = floor(pow(2.0,ceil(log((double) nMax)/log(2.0)) + 2.0) + 1.0e-7);

  adExpect = (double *) malloc(sizeof(double)*nMax);

  for(i = 0; i < nMax; i++){
    adExpect[i] = 0.0;
  }

  for(i = 0; i < nSamples; i++){
    updateExpectations(adExpect, nMax,
		       atLSParams[i].dMDash,
		       atLSParams[i].dV,
		       atLSParams[i].dNu,
		       atLSParams[i].nS,
		       &tData);
  }

  for(i = 1; i <= nMax; i++){
    printf("%d %f\n",i,adExpect[i - 1]/((double) nSamples));
  }

  free(adExpect);
  exit(EXIT_SUCCESS);

 memoryError:
  fprintf(stderr, "Failed to allocate memory in main aborting ...\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

int intCompare(const void *pvA, const void *pvB)
{
  int* pnA = (int *) pvA, *pnB = (int *) pvB;

  if(*pnA < *pnB)
    return +1;
  else if(*pnA == *pnB){
    return 0;
  }
  else{
    return -1;
  }
}

int doubleCompare(const void *pvA, const void *pvB)
{
  double* pnA = (double *) pvA, *pnB = (double *) pvB;

  if(*pnA < *pnB)
    return +1;
  else if(*pnA == *pnB){
    return 0;
  }
  else{
    return -1;
  }
}

void writeUsage(FILE* ofp)
{
  int i = 0;
  char *line;

  for(i = 0; i < nLines; i++){
    line = usage[i];
    fputs(line,ofp);
  }
}

char *extractParameter(int argc, char **argv, char *param,int when)
{
  int i = 0;

  while((i < argc) && (strcmp(param,argv[i]))){
    i++;
  }

  if(i < argc - 1){
    return(argv[i + 1]);
  }

  if((i == argc - 1) && (when == OPTION)){
    return "";
  }

  if(when == ALWAYS){
    fprintf(stdout,"Can't find asked option %s\n",param);
  }

  return (char *) NULL;
}

void getCommandLineParams(t_Params *ptParams,int argc,char *argv[])
{
  char *szTemp = NULL;
  char *cError = NULL;

  /*get parameter file name*/
  ptParams->szInputFile  = extractParameter(argc,argv, INPUT_FILE,ALWAYS);
  if(ptParams->szInputFile == NULL)
    goto error;

 /*get abundance file name*/
  ptParams->szAbundFile  = extractParameter(argc,argv, ABUND_FILE,ALWAYS);
  if(ptParams->szAbundFile == NULL)
    goto error;

  /*get long seed*/
  szTemp  = extractParameter(argc,argv,SEED,OPTION);
  if(szTemp != NULL){
    ptParams->lSeed = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->lSeed = 0;
  }

  /*get burn*/
  szTemp  = extractParameter(argc, argv, BURN, OPTION);
  if(szTemp != NULL){
    ptParams->nBurn = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->nBurn = DEF_BURN;
  }

 /*get long seed*/
  szTemp  = extractParameter(argc, argv, SAMPLE, OPTION);
  if(szTemp != NULL){
    ptParams->nSample = strtol(szTemp,&cError,10);
    if(*cError != '\0'){
      goto error;
    }
  }
  else{
    ptParams->nSample = DEF_SAMPLE;
  }

  /*verbosity*/
  szTemp = extractParameter(argc, argv, VERBOSE, OPTION);
  if(szTemp != NULL){
    verbose = TRUE;
  }

  return;

 error:
  writeUsage(stdout);
  exit(EXIT_FAILURE);
}

void readSamples(t_Params *ptParams, t_LSParams *atLSParams, int *pnSamples)
{
  int  nSamples = 0;
  char *szInputFile = ptParams->szInputFile;
  char szLine[MAX_LINE_LENGTH];
  FILE *ifp = NULL;

  ifp = fopen(szInputFile, "r");

  if(ifp){
    while(fgets(szLine, MAX_LINE_LENGTH, ifp)){
      char *szTok = NULL, *szBrk = NULL, *pcError = NULL;
      int  nTime  = 0;

      /*remove trailing new line*/
      szBrk = strpbrk(szLine, "\n"); (*szBrk) = '\0';

      szTok = strtok(szLine, DELIM);

      nTime = strtol(szTok, &pcError, 10);
      if(*pcError != '\0') goto fileFormatError;

      if(nTime > ptParams->nBurn && nTime % ptParams->nSample == 0){

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dMDash = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dV = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].dNu = strtod(szTok, &pcError);
	if(*pcError != '\0') goto fileFormatError;

	szTok = strtok(NULL,DELIM);
	atLSParams[nSamples].nS = strtol(szTok, &pcError, 10);
	if(*pcError != '\0') goto fileFormatError;

	nSamples++;
      }
    }

    fclose(ifp);
  }
  else{
    fprintf(stderr, "Failed to open file %s aborting\n", szInputFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  (*pnSamples) = nSamples;
  return;

 fileFormatError:
  fprintf(stderr, "Incorrectly formatted input file aborting\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}

double f1(double x, void *pvParams)
{
  t_LSParams *ptLSParams = (t_LSParams *) pvParams;
  double dMDash = ptLSParams->dMDash, dV = ptLSParams->dV, dNu = ptLSParams->dNu;
  int n = ptLSParams->n;
  double t = ((x - dMDash)*(x - dMDash))/dV;
  double dExp  = x*((double) n) - exp(x);
  double dF  = pow(1.0 + t/dNu, -0.5*(dNu + 1.0));

  return exp(dExp)*dF;
}

double f1Log(double x, void *pvParams)
{
  t_LNParams *ptLNParams = (t_LNParams *) pvParams;
  double dMDash = ptLNParams->dMDash, dV = ptLNParams->dV;
  int n = ptLNParams->n;
  double dTemp = (x - dMDash);
  double dExp  = x*((double) n) - exp(x) - 0.5*((dTemp*dTemp)/dV);
  double dRet  = exp(dExp);

  return dRet;
}

double derivExponent(double x, void *pvParams)
{
  t_LNParams *ptLNParams = (t_LNParams *) pvParams;
  double dMDash = ptLNParams->dMDash, dV = ptLNParams->dV, n = ptLNParams->n;
  double dTemp = (x - dMDash)/dV, dRet = 0.0;

  dRet = ((double) n) - exp(x) - dTemp;

  return dRet;
}

double logStirlingsGamma(double dZ)
{
  return 0.5*log(2.0*M_PI) + (dZ - 0.5)*log(dZ) - dZ;
}

double logLikelihoodQuad(int n, double dMDash, double dV, double dNu)
{
  gsl_integration_workspace *ptGSLWS =
    gsl_integration_workspace_alloc(1000);
  double dLogFac1   = 0.0, dLogFacN  = 0.0;
  double dN = (double) n, dResult = 0.0, dError = 0.0, dPrecision = 0.0;
  gsl_function tGSLF;
  t_LSParams tLSParams;
  double dEst = dMDash + ((double)n)*dV, dA = 0.0, dB = 0.0;

  tLSParams.n = n; tLSParams.dMDash = dMDash; tLSParams.dV = dV; tLSParams.dNu = dNu;

  tGSLF.function = &f1;
  tGSLF.params   = (void *) &tLSParams;

  if(dNu < MAX_MU_GAMMA){
    dLogFac1 = gsl_sf_lngamma(0.5*(dNu + 1.0)) - gsl_sf_lngamma(0.5*dNu) - 0.5*log(M_PI*dNu);
  }
  else{
    dLogFac1 = 0.5*dNu*(log(0.5*(dNu + 1.0)) - log(0.5*dNu)) -0.5*log(2.0*M_PI) - 0.5;
  }

  if(n < 50){
    dLogFacN = gsl_sf_fact(n);
    dLogFacN = log(dLogFacN);
  }
  else if(n < 100){
    dLogFacN = gsl_sf_lngamma(dN + 1.0);
  }
  else{
    dLogFacN = logStirlingsGamma(dN + 1.0);
  }

  dA = -100.0; dB = 100.0;

  if(n < 10){
    dPrecision = HI_PRECISION;
  }
  else{
    dPrecision = LO_PRECISION;
  }

  gsl_integration_qag(&tGSLF, dA, dB, dPrecision, 0.0, 1000, GSL_INTEG_GAUSS61, ptGSLWS, &dResult, &dError);

  //printf("%f %f\n", dResult, dError);

  gsl_integration_workspace_free(ptGSLWS);

  return log(dResult) - dLogFacN + dLogFac1 - 0.5*log(dV);
}

int solveF(double x_lo, double x_hi, double (*f)(double, void*),
	   void* params, double tol, double *xsolve)
{
  int status, iter = 0, max_iter = 100;
  const gsl_root_fsolver_type *T;
  gsl_root_fsolver *s;
  double r = 0;
  gsl_function F;
  t_LNParams *ptLNParams = (t_LNParams *) params;
  F.function = f;
  F.params = params;

  //printf("%f %f %d %f %f\n",ptLNParams->dMDash, ptLNParams->dV, ptLNParams->n, x_lo, x_hi);
  T = gsl_root_fsolver_brent;
  s = gsl_root_fsolver_alloc (T);
  gsl_root_fsolver_set (s, &F, x_lo, x_hi);

  do{
    iter++;
    status = gsl_root_fsolver_iterate (s);
    r = gsl_root_fsolver_root (s);
    x_lo = gsl_root_fsolver_x_lower (s);
    x_hi = gsl_root_fsolver_x_upper (s);

    status = gsl_root_test_interval (x_lo, x_hi, 0, tol);
  }
  while (status == GSL_CONTINUE && iter < max_iter);

  (*xsolve) = gsl_root_fsolver_root (s);
  gsl_root_fsolver_free (s);

  return status;
}

double logLikelihoodLNQuad(int n, double dMDash, double dV)
{
  gsl_integration_workspace *ptGSLWS =
    gsl_integration_workspace_alloc(1000);
  double dLogFac1   = 0.0, dLogFacN  = 0.0;
  double dResult = 0.0, dError = 0.0, dPrecision = 0.0;
  gsl_function tGSLF;
  t_LNParams tLNParams;
  double dEst = dMDash + ((double)n)*dV, dA = 0.0, dB = 0.0;

  tLNParams.n = n; tLNParams.dMDash = dMDash; tLNParams.dV = dV;

  tGSLF.function = &f1Log;
  tGSLF.params   = (void *) &tLNParams;

  dLogFac1 = log(2.0*M_PI*dV);

  if(n < 50){
    dLogFacN = gsl_sf_fact(n);
    dLogFacN = log(dLogFacN);
  }
  else{
    dLogFacN = gsl_sf_lngamma(((double) n) + 1.0);
  }

  if(dEst > dV){
    double dMax = 0.0;
    double dUpper = (((double) n) + (dMDash/dV) - 1.0)/(1.0 + 1.0/dV);
    double dVar   = 0.0;

    if(fabs(dUpper) > 1.0e-7){
      solveF(0.0, dUpper, derivExponent, (void *) &tLNParams, 1.0e-5, &dMax);
    }

    dVar = sqrt(1.0/((1.0/dV) + exp(dMax)));

    dA = dMax - V_MULT*dVar; dB = dMax + V_MULT*dVar;
  }
  else{
    double dMax = 0.0;
    double dLower = dEst - dV;
    double dUpper = (((double) n) + (dMDash/dV) - 1.0)/(1.0 + 1.0/dV);
    double dVar   = 0.0;

    if(fabs(dUpper - dLower) > 1.0e-7){
      solveF(dLower, dUpper, derivExponent, (void *) &tLNParams, 1.0e-5, &dMax);
    }
    else{
      dMax = 0.5*(dLower + dUpper);
    }
    dVar = sqrt(1.0/((1.0/dV) + exp(dMax)));

    dA = dMax - V_MULT*dVar; dB = dMax + V_MULT*dVar;
  }

  if(n < 10){
    dPrecision = HI_PRECISION;
  }
  else{
    dPrecision = LO_PRECISION;
  }

  gsl_integration_qag(&tGSLF, dA, dB, dPrecision, 0.0, 1000, GSL_INTEG_GAUSS61, ptGSLWS, &dResult, &dError);

  gsl_integration_workspace_free(ptGSLWS);

  return log(dResult) - dLogFacN -0.5*dLogFac1;
}

double logLikelihoodLNRampal(int n, double dMDash, double dV)
{
  double dN = (double) n;
  double dLogLik = 0.0, dTemp = gsl_pow_int(log(dN) - dMDash,2), dTemp3 = gsl_pow_int(log(dN) - dMDash,3);

  dLogLik = -0.5*log(2.0*M_PI*dV) - log(dN) - (dTemp/(2.0*dV));

  dLogLik += log(1.0 + 1.0/(2.0*dN*dV)*(dTemp/dV + log(dN) - dMDash - 1.0)
		 + 1.0/(6.0*dN*dN*dV*dV*dV)*(3.0*dV*dV - (3.0*dV - 2.0*dV*dV)*(dMDash - log(dN))
		 - 3.0*dV*dTemp + dTemp3));

  return dLogLik;
}

double logLikelihoodRampal(int n, double dMDash, double dV, double dNu)
{
  double dGamma = 0.5*(dNu + 1.0), dN = (double) n, dRN = 1.0/dN, dRSV = 1.0/(sqrt(dV)*sqrt(dNu));
  double dZ = (log(dN) - dMDash)*dRSV;
  double dDZDX = dRN*dRSV, dDZDX2 = -dRN*dRN*dRSV;
  double dF = (1.0 + dZ*dZ);
  double dA = 0.0, dB = 0.0, dTemp = 0.0;
  double dLogFac1 = 0.0;

  if(dNu < MAX_MU_GAMMA){
    dLogFac1 = gsl_sf_lngamma(0.5*(dNu + 1.0)) - gsl_sf_lngamma(0.5*dNu) - 0.5*log(M_PI*dNu);
  }
  else{
    dLogFac1 = 0.5*dNu*(log(0.5*(dNu + 1.0)) - log(0.5*dNu)) -0.5*log(2.0*M_PI) - 0.5;
  }

  dA = 4.0*dZ*dZ*dDZDX*dDZDX*dGamma*(dGamma + 1.0);
  dA /= dF*dF;

  dB = -2.0*dGamma*(dDZDX*dDZDX + dZ*dDZDX2);
  dB /= dF;

  dTemp = dRN + dA + dB;

  return -dGamma*log(dF) + log(dTemp) + dLogFac1 - 0.5*log(dV);
}

void readAbundanceData(const char *szFile, t_Data *ptData)
{
  int **aanAbund = NULL;
  int  i = 0, nNA = 0, nA = 0, nC = 0;
  int  nL = 0, nJ = 0;
  char szLine[MAX_LINE_LENGTH];
  FILE* ifp = NULL;

  ifp = fopen(szFile, "r");

  if(ifp){
    char* szTok   = NULL;
    char* pcError = NULL;

    fgets(szLine, MAX_LINE_LENGTH, ifp);

    szTok = strtok(szLine, DELIM2);

    nNA = strtol(szTok,&pcError,10);
    if(*pcError != '\0'){
      goto formatError;
    }

    aanAbund = (int **) malloc(nNA*sizeof(int*));

    for(i = 0; i < nNA; i++){
      aanAbund[i] = (int *) malloc(sizeof(int)*2);

      fgets(szLine, MAX_LINE_LENGTH, ifp);

      szTok = strtok(szLine, DELIM2);

      nA = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      szTok = strtok(NULL, DELIM2);

      nC = strtol(szTok,&pcError,10);
      if(*pcError != '\0'){
	goto formatError;
      }

      nL += nC;
      nJ += nC*nA;

      aanAbund[i][0]  = nA;
      aanAbund[i][1]  = nC;
    }
  }
  else{
    fprintf(stderr, "Failed to open abundance data file %s aborting\n", szFile);
    fflush(stderr);
    exit(EXIT_FAILURE);
  }

  ptData->nJ          = nJ;
  ptData->nL          = nL;
  ptData->aanAbund    = aanAbund;
  ptData->nNA         = nNA;
  return;

 formatError:
  fprintf(stderr, "Incorrectly formatted abundance data file\n");
  fflush(stderr);
  exit(EXIT_FAILURE);
}


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

/* Factorise a (p,q) banded M x N matrix A into
 *
 *   A = Q R
 *
 * where Q is orthogonal (M x M) and R is upper triangular (M x N).
 *
 * Example with M = 7, N = 6, (p,q) = (2,1)
 *
 * A = [ A11 A12  0   0   0   0  ]
 *     [ A21 A22 A23  0   0   0  ]
 *     [ A31 A32 A33 A34  0   0  ]
 *     [  0  A42 A43 A44 A45  0  ]
 *     [  0   0  A53 A54 A55 A56 ]
 *     [  0   0   0  A64 A65 A66 ]
 *     [  0   0   0   0  A75 A76 ]
 *
 * AB has dimensions N-by-(2p + q + 1)
 *
 * INPUT:                          OUTPUT:
 *
 * AB = [ *  *  *  A11 A21 A31 ]   AB = [  *   *   *  R11 V21 V31 ]
 *      [ *  * A12 A22 A32 A42 ]        [  *   *  R12 R22 V32 V42 ]
 *      [ *  0 A23 A33 A43 A53 ]        [  *  R13 R23 R33 V43 V53 ]
 *      [ 0  0 A34 A44 A54 A64 ]        [ R14 R24 R34 R44 V54 V64 ]
 *      [ 0  0 A45 A55 A65 A75 ]        [ R25 R35 R45 R55 V65 V75 ]
 *      [ 0  0 A56 A66 A76  *  ]        [ R36 R46 R56 R66 V76  *  ]
 *        -p-- -q- -1- ---p---            ---p--- -q- -1- ---p---
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 */

int
gsl_linalg_QR_band_decomp_L2 (const size_t M, const size_t p, const size_t q, gsl_matrix * AB, gsl_vector * tau)
{
  const size_t N = AB->size1;

  if (tau->size != N)
    {
      GSL_ERROR ("tau must have length N", GSL_EBADLEN);
    }
  else if (AB->size2 != 2*p + q + 1)
    {
      GSL_ERROR ("dimensions of AB are inconsistent with (p,q)", GSL_EBADLEN);
    }
  else
    {
      const size_t minMN = GSL_MIN(M, N);
      size_t j;

      /* set AB(:,1:p) to zero */
      if (p > 0)
        {
          gsl_matrix_view m = gsl_matrix_submatrix(AB, 0, 0, N, p);
          gsl_matrix_set_zero(&m.matrix);
        }

      for (j = 0; j < minMN; ++j)
        {
          /* Compute the Householder transformation to reduce the j-th
             column of the matrix to a multiple of the j-th unit vector */

          size_t k1 = GSL_MIN(p + 1, M - j);     /* number of non-zero elements of this column, including diagonal element */
          size_t k2 = GSL_MIN(p + q, N - j - 1); /* number of columns to update */
          gsl_vector_view c = gsl_matrix_subrow(AB, j, p + q, k1);
          double tau_j = gsl_linalg_householder_transform (&(c.vector));
          double * ptr = gsl_vector_ptr(&(c.vector), 0);

          gsl_vector_set (tau, j, tau_j);

          /* apply the transformation to the remaining columns */
          if (k2 > 0)
            {
              gsl_matrix_view m = gsl_matrix_submatrix (AB, j + 1, p + q - 1, k2, k1);
              gsl_vector_view work = gsl_vector_subvector(tau, j + 1, k2);
              double tmp = *ptr;

              m.matrix.tda -= 1; /* unskew matrix */

              /* we want to compute H*A(j:j+k1-1,j+1:j+k2), but due to our using row-major order, the
               * matrix m contains A(j:j+k1-1,j+1:j+k2)^T. So therefore we apply H from the right,
               *
               * [H*A]^T = A^T H^T = A^T H
               */

              *ptr = 1.0;
              gsl_linalg_householder_right(tau_j, &(c.vector), &(m.matrix), &(work.vector));
              *ptr = tmp;
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_QR_band_unpack_L2 (const size_t p, const size_t q, const gsl_matrix * QRB, const gsl_vector * tau,
                              gsl_matrix * Q, gsl_matrix * R)
{
  const size_t M = Q->size1;
  const size_t N = QRB->size1;

  if (Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be square", GSL_ENOTSQR);
    }
  else if (R->size1 != M || R->size2 != N)
    {
      GSL_ERROR ("R matrix must be M x N", GSL_ENOTSQR);
    }
  else if (tau->size < GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau must be at least MIN(M,N)", GSL_EBADLEN);
    }
  else if (QRB->size2 != 2*p + q + 1)
    {
      GSL_ERROR ("dimensions of QRB are inconsistent with (p,q)", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* form matrix Q */
      gsl_matrix_set_identity (Q);

      for (i = GSL_MIN (M, N); i-- > 0;)
        {
          size_t k1 = GSL_MIN(p + 1, M - i);     /* number of non-zero elements of this column, including diagonal element */
          gsl_vector_const_view h = gsl_matrix_const_subrow(QRB, i, p + q, k1);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, k1, M - i);
          double ti = gsl_vector_get (tau, i);
          gsl_vector_view work = gsl_matrix_subcolumn(R, 0, 0, M - i);
          double * ptr = gsl_vector_ptr((gsl_vector *) &h.vector, 0);
          double tmp = *ptr;

          *ptr = 1.0;
          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
          *ptr = tmp;
        }

      /* form matrix R */
      gsl_matrix_set_zero(R);

      for (i = 0; i <= GSL_MIN(p + q, N - 1); ++i)
        {
          gsl_vector_const_view src = gsl_matrix_const_subcolumn(QRB, p + q - i, i, GSL_MIN(M, N - i));
          gsl_vector_view dest = gsl_matrix_superdiagonal(R, i);
          gsl_vector_memcpy(&dest.vector, &src.vector);
        }

      return GSL_SUCCESS;
    }
}


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

#include <gsl/gsl_sf_bessel.h>

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


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

/* Fit
 *
 * y = X c
 *
 * where X is an n x p matrix of n observations for p variables.
 *
 * The solution includes a possible standard form Tikhonov regularization:
 *
 * c = (X^T X + lambda^2 I)^{-1} X^T y
 *
 * where lambda^2 is the Tikhonov regularization parameter.
 *
 * The function multifit_linear_svd() must first be called to
 * compute the SVD decomposition of X
 *
 * Inputs: X        - least squares matrix
 *         y        - right hand side vector
 *         tol      - singular value tolerance
 *         lambda   - Tikhonov regularization parameter lambda;
 *                    ignored if <= 0
 *         rank     - (output) effective rank
 *         c        - (output) model coefficient vector
 *         rnorm    - (output) residual norm ||y - X c||
 *         snorm    - (output) solution norm ||c||
 *         work     - workspace
 *
 * Notes:
 * 1) The dimensions of X must match work->n and work->p which are set
 *    by multifit_linear_svd()
 * 2) On input:
 *    work->A contains U
 *    work->Q contains Q
 *    work->S contains singular values
 * 3) If this function is called from gsl_multifit_wlinear(), then
 *    the input y points to work->t, which contains sqrt(W) y. Since
 *    work->t is also used as scratch workspace by this function, we
 *    do the necessary computations with y first to avoid problems.
 * 4) When lambda <= 0, singular values are truncated when:
 *    s_j <= tol * s_0
 */

static int
multifit_linear_solve (const gsl_matrix * X,
                       const gsl_vector * y,
                       const double tol,
                       const double lambda,
                       size_t * rank,
                       gsl_vector * c,
                       double *rnorm,
                       double *snorm,
                       gsl_multifit_linear_workspace * work)
{
  const size_t n = X->size1;
  const size_t p = X->size2;

  if (n != work->n || p != work->p)
    {
      GSL_ERROR("observation matrix does not match workspace", GSL_EBADLEN);
    }
  else if (n != y->size)
    {
      GSL_ERROR("number of observations in y does not match matrix",
                GSL_EBADLEN);
    }
  else if (p != c->size)
    {
      GSL_ERROR ("number of parameters c does not match matrix",
                 GSL_EBADLEN);
    }
  else if (tol <= 0)
    {
      GSL_ERROR ("tolerance must be positive", GSL_EINVAL);
    }
  else
    {
      const double lambda_sq = lambda * lambda;

      double rho_ls = 0.0;     /* contribution to rnorm from OLS */

      size_t j, p_eff;

      /* these inputs are previously computed by multifit_linear_svd() */
      gsl_matrix_view A = gsl_matrix_submatrix(work->A, 0, 0, n, p);
      gsl_matrix_view Q = gsl_matrix_submatrix(work->Q, 0, 0, p, p);
      gsl_vector_view S = gsl_vector_subvector(work->S, 0, p);

      /* workspace */
      gsl_matrix_view QSI = gsl_matrix_submatrix(work->QSI, 0, 0, p, p);
      gsl_vector_view xt = gsl_vector_subvector(work->xt, 0, p);
      gsl_vector_view D = gsl_vector_subvector(work->D, 0, p);
      gsl_vector_view t = gsl_vector_subvector(work->t, 0, n);

      /*
       * Solve y = A c for c
       * c = Q diag(s_i / (s_i^2 + lambda_i^2)) U^T y
       */

      /* compute xt = U^T y */
      gsl_blas_dgemv (CblasTrans, 1.0, &A.matrix, y, 0.0, &xt.vector);

      if (n > p)
        {
          /*
           * compute OLS residual norm = || y - U U^T y ||;
           * for n = p, U U^T = I, so no need to calculate norm
           */
          gsl_vector_memcpy(&t.vector, y);
          gsl_blas_dgemv(CblasNoTrans, -1.0, &A.matrix, &xt.vector, 1.0, &t.vector);
          rho_ls = gsl_blas_dnrm2(&t.vector);
        }

      if (lambda > 0.0)
        {
          /* xt <-- [ s(i) / (s(i)^2 + lambda^2) ] .* U^T y */
          for (j = 0; j < p; ++j)
            {
              double sj = gsl_vector_get(&S.vector, j);
              double f = (sj * sj) / (sj * sj + lambda_sq);
              double *ptr = gsl_vector_ptr(&xt.vector, j);

              /* use D as workspace for residual norm */
              gsl_vector_set(&D.vector, j, (1.0 - f) * (*ptr));

              *ptr *= sj / (sj*sj + lambda_sq);
            }

          /* compute regularized solution vector */
          gsl_blas_dgemv (CblasNoTrans, 1.0, &Q.matrix, &xt.vector, 0.0, c);

          /* compute solution norm */
          *snorm = gsl_blas_dnrm2(c);

          /* compute residual norm */
          *rnorm = gsl_blas_dnrm2(&D.vector);

          if (n > p)
            {
              /* add correction to residual norm (see eqs 6-7 of [1]) */
              *rnorm = sqrt((*rnorm) * (*rnorm) + rho_ls * rho_ls);
            }

          /* reset D vector */
          gsl_vector_set_all(&D.vector, 1.0);
        }
      else
        {
          /* Scale the matrix Q, QSI = Q S^{-1} */

          gsl_matrix_memcpy (&QSI.matrix, &Q.matrix);

          {
            double s0 = gsl_vector_get (&S.vector, 0);
            p_eff = 0;

            for (j = 0; j < p; j++)
              {
                gsl_vector_view column = gsl_matrix_column (&QSI.matrix, j);
                double sj = gsl_vector_get (&S.vector, j);
                double alpha;

                if (sj <= tol * s0)
                  {
                    alpha = 0.0;
                  }
                else
                  {
                    alpha = 1.0 / sj;
                    p_eff++;
                  }

                gsl_vector_scale (&column.vector, alpha);
              }

            *rank = p_eff;
          }

          gsl_blas_dgemv (CblasNoTrans, 1.0, &QSI.matrix, &xt.vector, 0.0, c);

          /* Unscale the balancing factors */
          gsl_vector_div (c, &D.vector);

          *snorm = gsl_blas_dnrm2(c);
          *rnorm = rho_ls;
        }

      return GSL_SUCCESS;
    }
}


/*
** Implementation of LISA algorithm
** for statistical inference of fMRI images
**
** generic plug-in.
** A file containing a list of all 3D permutation images must be supplied.
**
** G.Lohmann, April 2017
*/

#include <viaio/Vlib.h>
#include <viaio/file.h>
#include <viaio/mu.h>
#include <viaio/option.h>
#include <viaio/os.h>
#include <viaio/VImage.h>
#include <via/via.h>

#include <gsl/gsl_cdf.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_histogram.h>

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

#ifdef _OPENMP
#include <omp.h>
#endif /*_OPENMP*/

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

extern void VIsolatedVoxels(VImage src,float threshold);
extern void VHistogram(gsl_histogram *histogram,VString filename);
extern void VCheckImage(VImage src);
extern void FDR(VImage src,VImage dest,gsl_histogram *nullhist,gsl_histogram *realhist,double alpha);
extern void ImageStats(VImage src,double *,double *,double *hmin,double *hmax);
extern void VBilateralFilter(VImage src,VImage dest,int radius,double var1,double var2,int);
extern double VImageVar(VImage src);
extern void  VImageCount(VImage src);
extern void  VGetHistRange(VImage src,double *hmin,double *hmax);
extern float VGetMode(VImage src);
extern void  VZScale(VImage src,float mode,float stddev);
extern void HistoUpdate(VImage,gsl_histogram *);

/* make sure all input images are in float and have the same number of pixels */
void CheckImageTypes(VImage zmap,VImage *permimages,int numperm)
{
  size_t npixels = VImageNPixels(zmap);
  int i;
  for (i=0; i<numperm; i++) {
    if (npixels != VImageNPixels(permimages[i]))
      VError(" inconsistent number of pixels in permutation image %d",i);
    if (VPixelRepn(permimages[i]) != VFloatRepn) VError(" perm image %d is not in float repn");
  }
}

int main (int argc, char *argv[])
{
  static VString  filename = "";
  static VFloat   alpha = 0.05;
  static VShort   radius = 2;
  static VFloat   rvar = 2.0;
  static VFloat   svar = 2.0;
  static VShort   numiter = 2;
  static VBoolean centering = FALSE;
  static VBoolean cleanup = TRUE;
  static VShort   nproc = 0;
  static VOptionDescRec  options[] = {
    {"permutations",VStringRepn,1,(VPointer) &filename,VRequiredOpt,NULL,"List of all permutation images"},
    {"alpha",VFloatRepn,1,(VPointer) &alpha,VOptionalOpt,NULL,"FDR significance level"},
    {"radius",VShortRepn,1,(VPointer) &radius,VOptionalOpt,NULL,"Bilateral parameter (radius in voxels)"},
    {"rvar",VFloatRepn,1,(VPointer) &rvar,VOptionalOpt,NULL,"Bilateral parameter (radiometric)"},
    {"svar",VFloatRepn,1,(VPointer) &svar,VOptionalOpt,NULL,"Bilateral parameter (spatial)"},
    {"filteriterations",VShortRepn,1,(VPointer) &numiter,VOptionalOpt,NULL,"Bilateral parameter (number of iterations)"},
    {"cleanup",VBooleanRepn,1,(VPointer) &cleanup,VOptionalOpt,NULL,"Whether to remove isolated voxels"},
    {"j",VShortRepn,1,(VPointer) &nproc,VOptionalOpt,NULL,"Number of processors to use, '0' to use all"},
  };
  FILE *out_file=NULL,*in_file=NULL;
  VString in_filename=NULL;
  char *prg_name=GetLipsiaName("vlisa_generic");
  fprintf (stderr, "%s\n", prg_name);
  gsl_set_error_handler_off ();

  /* Parse command line arguments and identify files: */
  VParseFilterCmdZ (VNumber (options), options, argc, argv,&in_file,&out_file,&in_filename);

  /* Read the input zmap file: */
  VAttrList list = VReadAttrListZ(in_file,in_filename,0L,TRUE,FALSE);
  VImage zmap1 = VReadImage(list);
  if (zmap1 == NULL) VError(" no input zmap image found");
  if (VPixelRepn(zmap1) != VFloatRepn) VError(" input pixel repn must be float");
  VAttrList geolist = VGetGeoInfo(list);

  /* Read permutation file containing a list of 3D images */
  VAttrList listperm = VReadAttrList(filename,0L,FALSE,FALSE);
  int numperm  = VAttrListNumImages(listperm);
  VImage *zmap = VAttrListGetImages(listperm,numperm);
  CheckImageTypes(zmap1,zmap,numperm);
  fprintf(stderr," Number of permutation images: %d\n",(int)numperm);

  /* estimate null variance to adjust radiometric parameter, use first 30 permutations */
  double zvar=0,hmin=0,hmax=0;
  float stddev=1.0;
  int nperm=0;
  if (numperm > 0) {
    int tstperm = 30;
    if (tstperm > numperm) tstperm = numperm;
    double varsum=0,nx=0;
    for (nperm = 0; nperm < tstperm; nperm++) {
      zvar = VImageVar(zmap[nperm]);
      varsum += zvar;
      nx++;
    }
    double meanvar = varsum/nx;
    stddev = sqrt(meanvar);
  }

  /* get non-permuted hotspot map */
  float mode=0;
  if (centering) mode = VGetMode(zmap1);
  if (numperm > 0) VZScale(zmap1,mode,stddev);

  VImage dst1 = VCreateImageLike(zmap1);
  VBilateralFilter(zmap1,dst1,(int)radius,(double)rvar,(double)svar,(int)numiter);

  /* ini histograms */
  VGetHistRange(dst1,&hmin,&hmax);
  size_t nbins = 20000;
  gsl_histogram *hist0 = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (hist0,hmin,hmax);
  gsl_histogram *histz = gsl_histogram_alloc (nbins);
  gsl_histogram_set_ranges_uniform (histz,hmin,hmax);
  HistoUpdate(dst1,histz);

  /* omp-stuff */
#ifdef _OPENMP
  int num_procs=omp_get_num_procs();
  if (nproc > 0 && nproc < num_procs) num_procs = nproc;
  fprintf(stderr," using %d cores\n",(int)num_procs);
  omp_set_num_threads(num_procs);
#endif /* _OPENMP */

  /* do random permutations */
#pragma omp parallel for shared(zmap) schedule(dynamic)
  for (nperm = 0; nperm < numperm; nperm++) {
    if (nperm%20 == 0) fprintf(stderr," perm  %4d  of  %d\r",nperm,(int)numperm);

    float mode=0;
    if (centering) mode = VGetMode(zmap[nperm]);
    VZScale(zmap[nperm],mode,stddev);

    VImage dst = VCreateImageLike (zmap1);
    VBilateralFilter(zmap[nperm],dst,(int)radius,(double)rvar,(double)svar,(int)numiter);

    #pragma omp critical
    {
      HistoUpdate(dst,hist0);
    }
    VDestroyImage(dst);
  }

  /* apply fdr */
  VImage fdrimage = VCopyImage (dst1,NULL,VAllBands);
  if (numperm > 0) {
    FDR(dst1,fdrimage,hist0,histz,(double)alpha);

    if (cleanup && alpha < 1.0) {
      VIsolatedVoxels(fdrimage,(float)(1.0-alpha));
    }
  }

  /* output */
  VAttrList out_list = VCreateAttrList ();
  VHistory(VNumber(options),options,prg_name,&list,&out_list);
  VSetGeoInfo(geolist,out_list);
  VAppendAttr (out_list,"image",NULL,VImageRepn,fdrimage);
  if (! VWriteFile (out_file, out_list)) exit (1);
  fprintf (stderr, "\n%s: done.\n", argv[0]);
  exit(0);
}


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

/* #include <cblas.h> */
#include <lapacke_utils.h>

#include <jni.h>
#include "org_bitbucket_ioplus_openblas_Lapacke.h"

/*
 * Class:     org_bitbucket_ioplus_openblas_Lapacke
 * Method:    sgetrf
 * Signature: (II[FI[I)V
 */
    JNIEXPORT void JNICALL Java_org_bitbucket_ioplus_openblas_Lapacke_sgetrf
    (JNIEnv * env, jclass class,
     jint m, jint n, jfloatArray a, jint lda, jintArray ipiv){

        float *A;
        A = (*env)->GetPrimitiveArrayCritical(env, a, 0);
        int *IPIV;
        IPIV = (*env)->GetPrimitiveArrayCritical(env, ipiv, 0);

        LAPACKE_sgetrf(LAPACK_ROW_MAJOR,
                       m, n, A, lda, IPIV);

        (*env)->ReleasePrimitiveArrayCritical(env, a, A, 0);
        (*env)->ReleasePrimitiveArrayCritical(env, ipiv, IPIV, 0);

    }

/*
 * Class:     org_bitbucket_ioplus_openblas_Lapacke
 * Method:    sgetri
 * Signature: (I[FI[I)V
 */
    JNIEXPORT void JNICALL
    Java_org_bitbucket_ioplus_openblas_Lapacke_sgetri
    (JNIEnv * env, jclass class,
     jint n, jfloatArray a, jint lda, jintArray ipiv){

        float *A;
        A = (*env)->GetPrimitiveArrayCritical(env, a, 0);
        int *IPIV;
        IPIV = (*env)->GetPrimitiveArrayCritical(env, ipiv, 0);

        LAPACKE_sgetri(LAPACK_ROW_MAJOR,
                       n, A, lda, IPIV);

        (*env)->ReleasePrimitiveArrayCritical(env, a, A, 0);
        (*env)->ReleasePrimitiveArrayCritical(env, ipiv, IPIV, 0);

    }

/*
 * Class:     org_bitbucket_ioplus_openblas_Lapacke
 * Method:    sdeev
 * Signature: (CCI[FI[F[F[FI[FI)V
 */
JNIEXPORT void JNICALL Java_org_bitbucket_ioplus_openblas_Lapacke_sdeev
(JNIEnv * env, jclass class,
 jchar jobvl,
 jchar jobvr,
 jint n,
 jfloatArray a,
 jint lda,
 jfloatArray wr,
 jfloatArray wi,
 jfloatArray vl,
 jint ldvl,
 jfloatArray vr,
 jint ldvr){
    /* jni args -> c mapping  */

    float *A;
    float *WR;
    float *WI;
    float *VL;
    float *VR;

    A = (*env)->GetPrimitiveArrayCritical(env, a, 0);
    WR = (*env)->GetPrimitiveArrayCritical(env, wr, 0);
    WI = (*env)->GetPrimitiveArrayCritical(env, wi, 0);
    VL = (*env)->GetPrimitiveArrayCritical(env, vl, 0);
    VR = (*env)->GetPrimitiveArrayCritical(env, vr, 0);

    LAPACKE_sgeev(LAPACK_ROW_MAJOR,
                  jobvl,
                  jobvr,
                  n,
                  A,
                  lda,
                  WR, // values
                  WI,
                  VL,
                  ldvl,
                  VR, // vectors
                  ldvr);

    (*env)->ReleasePrimitiveArrayCritical(env, a, A, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, wr, WR, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, wi, WI, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, vr, VR, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, vl, VL, 0);

}

/*
 * Class:     org_bitbucket_ioplus_openblas_Lapacke
 * Method:    sgehrd
 * Signature: (III[FI[F)V
 */
JNIEXPORT void JNICALL Java_org_bitbucket_ioplus_openblas_Lapacke_sgehrd
(JNIEnv * env, jclass class,
 jint n,
 jint ilo,
 jint ihi,
 jfloatArray a,
 jint lda,
 jfloatArray tau
    ){
    float *A  = (*env)->GetPrimitiveArrayCritical(env, a, NULL);
    float *TAU = (*env)->GetPrimitiveArrayCritical(env, tau, NULL);
    LAPACKE_sgehrd(LAPACK_ROW_MAJOR,n, ilo, ihi, A, lda, TAU);
    (*env)->ReleasePrimitiveArrayCritical(env, a, A, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, tau, TAU, 0);
}

/*
 * Class:     org_bitbucket_ioplus_openblas_Lapacke
 * Method:    shseqr
 * Signature: (CCIII[FI[F[F[FI)V
 */
JNIEXPORT void JNICALL Java_org_bitbucket_ioplus_openblas_Lapacke_shseqr
(JNIEnv * env, jclass class,
 jchar job,
 jchar compz,
 jint n,
 jint ilo,
 jint ihi,
 jfloatArray h,
 jint ldh,
 jfloatArray wr,
 jfloatArray wi,
 jfloatArray z,
 jint ldz){

    float *H = (*env)->GetPrimitiveArrayCritical(env, h, 0);
    float *WR = (*env)->GetPrimitiveArrayCritical(env, wr, 0);
    float *WI = (*env)->GetPrimitiveArrayCritical(env, wi, 0);
    float *Z = (*env)->GetPrimitiveArrayCritical(env, z, 0);

    LAPACKE_shseqr(LAPACK_ROW_MAJOR,
                   job,
                   compz,
                   n,
                   ilo,
                   ihi,
                   H,
                   ldh,
                   WR,
                   WI,
                   Z,
                   ldz);

    (*env)->ReleasePrimitiveArrayCritical(env, h, H, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, wr, WR, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, wi, WI, 0);
    (*env)->ReleasePrimitiveArrayCritical(env, z, Z, 0);

}


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

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>

/*
 * This module contains routines related to calculating B-splines.
 * The algorithms used are described in
 *
 * [1] Carl de Boor, "A Practical Guide to Splines", Springer
 *     Verlag, 1978.
 */

static int bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                            gsl_bspline_workspace *w);

static inline size_t bspline_find_interval(const double x, int *flag,
                                           gsl_bspline_workspace *w);

/*
gsl_bspline_alloc()
  Allocate space for a bspline workspace. The size of the
workspace is O(5k + nbreak)

Inputs: k      - spline order (cubic = 4)
        nbreak - number of breakpoints

Return: pointer to workspace
*/

gsl_bspline_workspace *
gsl_bspline_alloc(const size_t k, const size_t nbreak)
{
  if (k == 0)
    {
      GSL_ERROR_NULL("k must be at least 1", GSL_EINVAL);
    }
  else if (nbreak < 2)
    {
      GSL_ERROR_NULL("nbreak must be at least 2", GSL_EINVAL);
    }
  else
    {
      gsl_bspline_workspace *w;

      w = (gsl_bspline_workspace *)
          calloc(1, sizeof(gsl_bspline_workspace));
      if (w == 0)
        {
          GSL_ERROR_NULL("failed to allocate space for workspace", GSL_ENOMEM);
        }

      w->k = k;
      w->km1 = k - 1;
      w->nbreak = nbreak;
      w->l = nbreak - 1;
      w->n = w->l + k - 1;

      w->knots = gsl_vector_alloc(w->n + k);
      if (w->knots == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for knots vector", GSL_ENOMEM);
        }

      w->deltal = gsl_vector_alloc(k);
      w->deltar = gsl_vector_alloc(k);
      if (!w->deltal || !w->deltar)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for delta vectors", GSL_ENOMEM);
        }

      w->B = gsl_vector_alloc(k);
      if (w->B == 0)
        {
          gsl_bspline_free(w);
          GSL_ERROR_NULL("failed to allocate space for temporary spline vector", GSL_ENOMEM);
        }

      return (w);
    }
} /* gsl_bspline_alloc() */

/* Return number of coefficients */
size_t
gsl_bspline_ncoeffs (gsl_bspline_workspace * w)
{
  return w->n;
}

/* Return order */
size_t
gsl_bspline_order (gsl_bspline_workspace * w)
{
  return w->k;
}

/* Return number of breakpoints */
size_t
gsl_bspline_nbreak (gsl_bspline_workspace * w)
{
  return w->nbreak;
}

double
gsl_bspline_breakpoint (size_t i, gsl_bspline_workspace * w)
{
  size_t j = i + w->k - 1;
  return gsl_vector_get(w->knots, j);
}

/*
gsl_bspline_free()
  Free a bspline workspace

Inputs: w - workspace to free

Return: none
*/

void
gsl_bspline_free(gsl_bspline_workspace *w)
{
  if (!w)
    return;

  if (w->knots)
    gsl_vector_free(w->knots);

  if (w->deltal)
    gsl_vector_free(w->deltal);

  if (w->deltar)
    gsl_vector_free(w->deltar);

  if (w->B)
    gsl_vector_free(w->B);

  free(w);
} /* gsl_bspline_free() */

/*
gsl_bspline_knots()
  Compute the knots from the given breakpoints:

knots(1:k) = breakpts(1)
knots(k+1:k+l-1) = breakpts(i), i = 2 .. l
knots(n+1:n+k) = breakpts(l + 1)

where l is the number of polynomial pieces (l = nbreak - 1) and
n = k + l - 1
(using matlab syntax for the arrays)

The repeated knots at the beginning and end of the interval
correspond to the continuity condition there. See pg. 119
of [1].

Inputs: breakpts - breakpoints
        w        - bspline workspace

Return: success or error
*/

int
gsl_bspline_knots(const gsl_vector *breakpts, gsl_bspline_workspace *w)
{
  if (breakpts->size != w->nbreak)
    {
      GSL_ERROR("breakpts vector has wrong size", GSL_EBADLEN);
    }
  else
    {
      size_t i; /* looping */

      for (i = 0; i < w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, 0));

      for (i = 1; i < w->l; ++i)
        {
          gsl_vector_set(w->knots, w->k - 1 + i,
                         gsl_vector_get(breakpts, i));
        }

      for (i = w->n; i < w->n + w->k; ++i)
        gsl_vector_set(w->knots, i, gsl_vector_get(breakpts, w->l));

      return GSL_SUCCESS;
    }
} /* gsl_bspline_knots() */

/*
gsl_bspline_knots_uniform()
  Construct uniformly spaced knots on the interval [a,b] using
the previously specified number of breakpoints. 'a' is the position
of the first breakpoint and 'b' is the position of the last
breakpoint.

Inputs: a - left side of interval
        b - right side of interval
        w - bspline workspace

Return: success or error

Notes: 1) w->knots is modified to contain the uniformly spaced
          knots

       2) The knots vector is set up as follows (using octave syntax):

          knots(1:k) = a
          knots(k+1:k+l-1) = a + i*delta, i = 1 .. l - 1
          knots(n+1:n+k) = b
*/

int
gsl_bspline_knots_uniform(const double a, const double b,
                          gsl_bspline_workspace *w)
{
  size_t i;     /* looping */
  double delta; /* interval spacing */
  double x;

  delta = (b - a) / (double) w->l;

  for (i = 0; i < w->k; ++i)
    gsl_vector_set(w->knots, i, a);

  x = a + delta;
  for (i = 0; i < w->l - 1; ++i)
    {
      gsl_vector_set(w->knots, w->k + i, x);
      x += delta;
    }

  for (i = w->n; i < w->n + w->k; ++i)
    gsl_vector_set(w->knots, i, b);

  return GSL_SUCCESS;
} /* gsl_bspline_knots_uniform() */

/*
gsl_bspline_eval()
  Evaluate the basis functions B_i(x) for all i. This is
basically a wrapper function for bspline_eval_all()
which formats the output in a nice way.

Inputs: x - point for evaluation
        B - (output) where to store B_i(x) values
            the length of this vector is
            n = nbreak + k - 2 = l + k - 1 = w->n
        w - bspline workspace

Return: success or error

Notes: The w->knots vector must be initialized prior to calling
       this function (see gsl_bspline_knots())
*/

int
gsl_bspline_eval(const double x, gsl_vector *B,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->n)
    {
      GSL_ERROR("B vector length does not match n", GSL_EBADLEN);
    }
  else
    {
      size_t i;     /* looping */
      size_t idx = 0;
      size_t start; /* first non-zero spline */

      /* find all non-zero B_i(x) values */
      bspline_eval_all(x, w->B, &idx, w);

      /* store values in appropriate part of given vector */

      start = idx - w->k + 1;
      for (i = 0; i < start; ++i)
        gsl_vector_set(B, i, 0.0);

      for (i = start; i <= idx; ++i)
        gsl_vector_set(B, i, gsl_vector_get(w->B, i - start));

      for (i = idx + 1; i < w->n; ++i)
        gsl_vector_set(B, i, 0.0);

      return GSL_SUCCESS;
    }
} /* gsl_bspline_eval() */

/****************************************
 *          INTERNAL ROUTINES           *
 ****************************************/

/*
bspline_eval_all()
  Evaluate all non-zero B-splines B_i(x) using algorithm (8)
of chapter X of [1]

The idea is something like this. Given x \in [t_i, t_{i+1}]
with t_i < t_{i+1} and the t_i are knots, the values of the
B-splines not automatically zero fit into a triangular
array as follows:
                         0
            0
0                        B_{i-2,3}
            B_{i-1,2}
B_{i,1}                  B_{i-1,3}       ...
            B_{i,2}
0                        B_{i,3}
            0
                         0

where B_{i,k} is the ith B-spline of order k. The boundary 0s
indicate that those B-splines not in the table vanish at x.

To compute the non-zero B-splines of a given order k, we use
Eqs. (4) and (5) of chapter X of [1]:

(4) B_{i,1}(x) = { 1, t_i <= x < t_{i+1}
                   0, else }

(5) B_{i,k}(x) =     (x - t_i)
                 ----------------- B_{i,k-1}(x)
                 (t_{i+k-1} - t_i)

                      t_{i+k} - x
                 + ----------------- B_{i+1,k-1}(x)
                   t_{i+k} - t_{i+1}

So (4) gives us the first column of the table and we can use
the recurrence relation (5) to get the rest of the columns.

Inputs: x   - point at which to evaluate splines
        B   - (output) where to store B-spline values (length k)
        idx - (output) B-spline function index of last output
              value (B_{idx}(x) is stored in the last slot of 'B')
        w   - bspline workspace

Return: success or error

Notes: 1) the w->knots vector must be initialized before calling
          this function

       2) On output, B contains:

       B = [B_{i-k+1,k}, B_{i-k+2,k}, ..., B_{i-1,k}, B_{i,k}]

          where 'i' is stored in 'idx' on output

       3) based on PPPACK bsplvb
*/

static int
bspline_eval_all(const double x, gsl_vector *B, size_t *idx,
                 gsl_bspline_workspace *w)
{
  if (B->size != w->k)
    {
      GSL_ERROR("B vector not of length k", GSL_EBADLEN);
    }
  else
    {
      size_t i;  /* spline index */
      size_t j;  /* looping */
      size_t ii; /* looping */
      int flag = 0;  /* error flag */
      double saved;
      double term;

      i = bspline_find_interval(x, &flag, w);

      if (flag == -1)
        {
          GSL_ERROR("x outside of knot interval", GSL_EINVAL);
        }
      else if (flag == 1)
        {
          if (x <= gsl_vector_get(w->knots, i) + GSL_DBL_EPSILON)
            {
              --i;
            }
          else
            {
              GSL_ERROR("x outside of knot interval", GSL_EINVAL);
            }
        }

      *idx = i;

      gsl_vector_set(B, 0, 1.0);

      for (j = 0; j < w->k - 1; ++j)
        {
          gsl_vector_set(w->deltar, j,
                         gsl_vector_get(w->knots, i + j + 1) - x);
          gsl_vector_set(w->deltal, j,
                         x - gsl_vector_get(w->knots, i - j));

          saved = 0.0;

          for (ii = 0; ii <= j; ++ii)
            {
              term = gsl_vector_get(B, ii) /
                     (gsl_vector_get(w->deltar, ii) +
                      gsl_vector_get(w->deltal, j - ii));

              gsl_vector_set(B, ii,
                             saved +
                             gsl_vector_get(w->deltar, ii) * term);

              saved = gsl_vector_get(w->deltal, j - ii) * term;
            }

          gsl_vector_set(B, j + 1, saved);
        }

      return GSL_SUCCESS;
    }
} /* bspline_eval_all() */

/*
bspline_find_interval()
  Find knot interval such that

  t_i <= x < t_{i + 1}

where the t_i are knot values.

Inputs: x    - x value
        flag - (output) error flag
        w    - bspline workspace

Return: i (index in w->knots corresponding to left limit of interval)

Notes: The error conditions are reported as follows:

Condition             Return value        Flag
---------             ------------        ----
x < t_0                    0               -1
t_i <= x < t_{i+1}         i                0
t_{n+k-1} <= x           l+k-1             +1
*/

static inline size_t
bspline_find_interval(const double x, int *flag,
                      gsl_bspline_workspace *w)
{
  size_t i;

  if (x < gsl_vector_get(w->knots, 0))
    {
      *flag = -1;
      return 0;
    }

  /* find i such that t_i <= x < t_{i+1} */
  for (i = w->k - 1; i < w->k + w->l - 1; ++i)
    {
      const double ti = gsl_vector_get(w->knots, i);
      const double tip1 = gsl_vector_get(w->knots, i + 1);

      if (tip1 < ti)
        {
          GSL_ERROR("knots vector is not increasing", GSL_EINVAL);
        }

      if (ti <= x && x < tip1)
        break;
    }

  if (i == w->k + w->l - 1)
    *flag = 1;
  else
    *flag = 0;

  return i;
} /* bspline_find_interval() */