/* statistics/gsl_statistics_uint.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Jim Davies, Brian Gough
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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.
 */

#ifndef __GSL_STATISTICS_UINT_H__
#define __GSL_STATISTICS_UINT_H__

#include <stddef.h>
#include <gsl/gsl_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

GSL_EXPORT double gsl_stats_uint_mean (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_variance (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_sd (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_variance_with_fixed_mean (const unsigned int data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_uint_sd_with_fixed_mean (const unsigned int data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_uint_absdev (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_skew (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_kurtosis (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT double gsl_stats_uint_lag1_autocorrelation (const unsigned int data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_uint_covariance (const unsigned int data1[], const size_t stride1,const unsigned int data2[], const size_t stride2, const size_t n);

GSL_EXPORT double gsl_stats_uint_variance_m (const unsigned int data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_uint_sd_m (const unsigned int data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_uint_absdev_m (const unsigned int data[], const size_t stride, const size_t n, const double mean);
GSL_EXPORT double gsl_stats_uint_skew_m_sd (const unsigned int data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_uint_kurtosis_m_sd (const unsigned int data[], const size_t stride, const size_t n, const double mean, const double sd);
GSL_EXPORT double gsl_stats_uint_lag1_autocorrelation_m (const unsigned int data[], const size_t stride, const size_t n, const double mean);

GSL_EXPORT double gsl_stats_uint_covariance_m (const unsigned int data1[], const size_t stride1,const unsigned int data2[], const size_t stride2, const size_t n, const double mean1, const double mean2);

GSL_EXPORT double gsl_stats_uint_pvariance (const unsigned int data1[], const size_t stride1, const size_t n1, const unsigned int data2[], const size_t stride2, const size_t n2);
GSL_EXPORT double gsl_stats_uint_ttest (const unsigned int data1[], const size_t stride1, const size_t n1, const unsigned int data2[], const size_t stride2, const size_t n2);

GSL_EXPORT unsigned int gsl_stats_uint_max (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT unsigned int gsl_stats_uint_min (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_uint_minmax (unsigned int * min, unsigned int * max, const unsigned int data[], const size_t stride, const size_t n);

GSL_EXPORT size_t gsl_stats_uint_max_index (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT size_t gsl_stats_uint_min_index (const unsigned int data[], const size_t stride, const size_t n);
GSL_EXPORT void gsl_stats_uint_minmax_index (size_t * min_index, size_t * max_index, const unsigned int data[], const size_t stride, const size_t n);

GSL_EXPORT double gsl_stats_uint_median_from_sorted_data (const unsigned int sorted_data[], const size_t stride, const size_t n) ;
GSL_EXPORT double gsl_stats_uint_quantile_from_sorted_data (const unsigned int sorted_data[], const size_t stride, const size_t n, const double f) ;

__END_DECLS

#endif /* __GSL_STATISTICS_UINT_H__ */


/* convolvedexponential.c -- model functions for multiple exponentials + background, convolved with an instrument response */
/////////////////////////////////////////////////////////////////////////////////
//
//  Calculation of multiple exponentials convolved with an instrument response.
//  using GNU Scientific Library.  Also, calls Levenberg-Marquardt minimization routine
//  for fitting event counting histograms.  Options include using the Maximum Likelihood Estimator
//  for Poisson deviates and least squares fitting.
//
//  Copyright (c) 2010, Lawrence Livermore National Security, LLC. Produced at the Lawrence Livermore National Laboratory. Written by Ted Laurence (laurence2@llnl.gov)
//  LLNL-CODE-424602 All rights reserved.
//  This file is part of dlevmar_mle_der
//
//  Please also read Our Notice and GNU General Public License.
//
//  This program is free software; you can redistribute it and/or modify
//  it under the terms of the GNU General Public License as published by
//  the Free Software Foundation; either version 2 of the License, or
//  (at your option) any later version.
//
//  This program is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU General Public License for more details.
//
//  You should have received a copy of the GNU General Public License
//  along with this program; if not, write to the
//  Free Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
//
/////////////////////////////////////////////////////////////////////////////////

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include <gsl/gsl_vector.h>
#include "levmar_mle.h"

struct data {
	double deltaT;	/*Spacing between bins*/
	size_t n;		/*Number of data elements*/
	size_t nRadix2; /*Next higher power of 2 for n, giving size of irf and fWork arrays*/
	double *irfWork;/*Instrument Response Function*/
	double *fWork;  /*Workspace for function calculation*/
	size_t m_total;	/*Number of parameters (number of exponentials = (m-1)/2 ) */
	double *a;		/*Parameter Array; fitted parameters x are subsituted into this array */
	size_t m;		/*Number of fitted parameters*/
	int *ip;		/*Mapping of fitted parameter array x to parameter array a*/
};

void complex_convolution_multiply(double *a, const double *b, int n)
{
	int i,j;
	double re,im,factor;

	factor = 1.0/n; /* to normalize inverse FFT */

	a[0]*=b[0]*factor;
	a[n/2]*=b[n/2]*factor;

	for (i=1;i<n/2;i++)
	{
		j=n-i;
		re=a[i];
		im=a[j];
		a[i]=(re*b[i]-im*b[j])*factor;
		a[j]=(im*b[i]+re*b[j])*factor;
	}
}

void convolved_exp_f(double *p, double *f, int m, int n, void *data)
{
	double deltaT=((struct data *)data)->deltaT;
	size_t nRadix2 = ((struct data *)data)->nRadix2;
	double *irfWork = ((struct data *) data)->irfWork;
	double *fWork = ((struct data *) data)->fWork;
	size_t m_total = ((struct data *)data)->m_total;
	double *a = ((struct data *) data)->a;
	int *ip = ((struct data *) data)->ip;

	int i,j;
	int numExponentials = (m_total-1)/2;
	double tau, amp, sqrtTau,sqrtAmp;

	double constantValue;
	double totalT=deltaT*n;
	double factor1;
	double factor2;

	if ( (n != ((struct data *)data)->n ) || (m != ((struct data *)data)->m) ) return;

	for (i=0; i<m; i++)
		a[ ip[i] ] = p[i];

	constantValue=a[0]*a[0]/n;

	for (i=0; i<n; i++) fWork[i] =  constantValue;
	for (i=n; i<nRadix2; i++) fWork[i] = 0.0;

	for (j=0;j<numExponentials;j++)
	{
		sqrtAmp=a[2*j+1];
		sqrtTau=a[2*j+2];
		amp=sqrtAmp*sqrtAmp;
		tau=sqrtTau*sqrtTau;
		factor1=1-exp(-deltaT/tau);
		factor2=1-exp(-totalT/tau);
		for (i=0;i<n;i++)
			fWork[i]+=amp*factor1/factor2*exp(-deltaT*i/tau);
	}

	gsl_fft_real_radix2_transform(fWork,1,nRadix2);
	complex_convolution_multiply(fWork,irfWork,nRadix2);
	gsl_fft_halfcomplex_radix2_backward(fWork,1,nRadix2);

	// This loop uses 0 offset indexing
	for (i=n;i<nRadix2;i++) fWork[i%n]+=fWork[i];
	for (i=0;i<n;i++) f[i]=fWork[i];

	return;
}

void convolved_exp_df(double *p, double *jac, int m, int n, void *data)
{
	double deltaT=((struct data *)data)->deltaT;
	size_t nRadix2 = ((struct data *)data)->nRadix2;
	double *irfWork = ((struct data *) data)->irfWork;
	double *fWork = ((struct data *) data)->fWork;
	size_t m_total = ((struct data *)data)->m_total;
	double *a = ((struct data *) data)->a;
	int *ip = ((struct data *) data)->ip;
	double *jac_row;

	int i,k;

	if ( (n != ((struct data *)data)->n ) || (m != ((struct data *)data)->m) ) return;

	for (k=0; k<m; k++)
	{
		double tau, amp, sqrtTau,sqrtAmp;
		double temp, totalT=deltaT*n;
		double factor1;
		double factor2;

		if (ip[k]==0)
		{
			double d_constant=2*a[0]/n;
			jac_row=jac;
			for (i=0;i<n;i++,jac_row+=m)
				jac_row[k]=d_constant;
			continue;
		}

		for (i=0; i<nRadix2; i++) fWork[i] = 0.0;

		if ((ip[k]-1)%2==0) /* Amplitude */
		{
			sqrtAmp=a[ip[k]];
			sqrtTau=a[ip[k]+1];
			amp=sqrtAmp*sqrtAmp;
			tau=sqrtTau*sqrtTau;
			factor1=1-exp(-deltaT/tau);
			factor2=1-exp(-totalT/tau);
			for (i=0;i<n;i++)
				fWork[i]=2*sqrtAmp*factor1/factor2*exp(-deltaT*i/tau);
		}
		else /* lifetime */
		{
			sqrtAmp=a[ip[k]-1];
			sqrtTau=a[ip[k]];
			amp=sqrtAmp*sqrtAmp;
			tau=sqrtTau*sqrtTau;
			factor1=1-exp(-deltaT/tau);
			factor2=1-exp(-totalT/tau);
			for (i=0;i<n;i++)
			{
				temp=factor1/factor2*exp(-deltaT*i/tau);
				fWork[i]= -2*amp*deltaT*exp(-(deltaT*i+deltaT)/tau)/(factor2*tau*sqrtTau)
							+2*temp*amp*deltaT*i/(tau*sqrtTau)
							+2*temp*amp*totalT*exp(-totalT/tau)/(factor2*tau*sqrtTau);
			}
		}

		gsl_fft_real_radix2_transform(fWork,1,nRadix2);
		complex_convolution_multiply(fWork,irfWork,nRadix2);
		gsl_fft_halfcomplex_radix2_backward(fWork,1,nRadix2);

		// This loop uses 0 offset indexing
		for (i=n;i<nRadix2;i++) fWork[i%n]+=fWork[i];

		jac_row=jac;
		for (i=0;i<n;i++,jac_row+=m)
			jac_row[k]=fWork[i];
	}
	return;
}

int fit_convolved_exponential(int n,			// number of measurement bins
							  double deltaT,	// time resolution for each bin
							  double *x,		// Data Measured
							  double *f,		// Space for function
							  double *irf,		// Instrument Response
							int m_total,		// Total number of parameters (including those not fitted)
							double *a,			// Parameter array (length m_total)
							int m,				// Number of fitted parameters
							int *ip,			// Array of length m with indices of fitted parameters in a
							double *chisq,		// Result for chisq
							int *nIterations,	// Number of iterations performed
							int fitType,		// 0 = MLE, 1 = Neyman weighting, 2 = Equal Weighting (sigma=1)
							double deltaChisqLimit, // Stop criterion for change in chisq
							int maxIterations)	//  maximum number of iterations
{
	struct data importedData;

	double tempN,nRadix2;
	double *irfWork, *fWork;
	double opts[LM_OPTS_SZ],info[LM_INFO_SZ];

	int i;
	int status;
	unsigned iter = 0;

	double *p;

	opts[0]=LM_INIT_MU;
	opts[1]=LM_STOP_THRESH;
	opts[2]=LM_STOP_THRESH;
	opts[3]=LM_STOP_THRESH;
	opts[4]=deltaChisqLimit;

	importedData.n=n;
	importedData.deltaT=deltaT;
	importedData.m_total=m_total;
	importedData.a=a;
	importedData.m=m;
	importedData.ip=ip;

	for (i=0; i<m_total; i++) a[i]=sqrt(a[i]);

	/* Determine power of 2 higher than n required for performing convolutions via FFT */
	tempN = pow(2, 1.0+ceil( log((double)n)/log(2.0) ) );
	nRadix2 = ( (int) tempN );
	irfWork=malloc(nRadix2*sizeof(double));
	if (irfWork==NULL) return GSL_ENOMEM;
	fWork=malloc(nRadix2*sizeof(double));
	if (fWork==NULL)
	{
		free(irfWork);
		return GSL_ENOMEM;
	}
	p=malloc(m*sizeof(double));
	if (p==NULL)
	{
		free(irfWork);
		free(fWork);
		return GSL_ENOMEM;
	}

	for (i=0; i<n; i++)
		irfWork[i]=irf[i];
	for (i=n; i<nRadix2; i++)
		irfWork[i]=0.0;
	gsl_fft_real_radix2_transform(irfWork,1,nRadix2);

	importedData.nRadix2=nRadix2;
	importedData.irfWork=irfWork;
	importedData.fWork=fWork;

	for (i=0; i<m; i++)
		p[i]=a[ip[i]];

	status = dlevmar_mle_der( &convolved_exp_f, &convolved_exp_df, p, x, m, n, maxIterations,opts,info,NULL,NULL,&importedData,fitType);

	*chisq = info[1]/(n-m);
	*nIterations=info[5];

	convolved_exp_f(p, f, m, n, &importedData);  /* Evaluate function for external use in array f */

	for (i=0; i<m; i++)
		a[ip[i]]=p[i];

	free(p);
	free(fWork);
	free(irfWork);

	for (i=0; i<m_total; i++) a[i]=a[i]*a[i];

	return info[6];
}


/* Simple Travelling Salesman Problem heuristics.
 *
 * Krzysztof Chalupka, 2017.
 */
#include <limits.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include "../utils/utils.h"
#include "../datastructures/datastructures.h"

int *tsp_bs(gsl_vector **, int); /* brute-force search */
int *tsp_nn(gsl_vector **, int); /* nearest-neighbor */
int *tsp_cp(gsl_vector **, int); /* closest-pair */
double pathlen(gsl_vector **, int *, int); /* path length of a permutation. */

double pathlen(gsl_vector **data, int *path, int n){
  /* Compute the length of the path defined by the vectors
   * in `data`, traversed in order given in `path`,
   * and closing the loop by traversing path[n-1] -- path[0]
   * at the end.
   */
  int i;
  double total = 0.;
  for (i = 0; i < n-1; i++)
    total += vector_dist(data[path[i]], data[path[i+1]]);
  if (n > 0)
    total += vector_dist(data[path[n-1]], data[path[0]]);
  return total;
}

int *tsp_nn(gsl_vector **data, int n)
{
  /* Solve the TSP using the nearest-neighbor heuristic. */
  if (n == 0)
    return NULL;
  int i, j, best_id;
  int usedup = 0; /* Flag already-used elements. */
  gsl_vector *current = data[0];
  double cur_dist;
  double best_dist;
  int *nn_path = (int *) calloc(n, sizeof(int));

  nn_path[0] = 0;
  usedup |= 1;
  for (i = 1; i < n; i++){
    best_dist = 0;
    for (j = 1; j < n; j++){
      if (!(usedup & (2 << (j-1)))){
    	cur_dist = vector_dist(current, data[j]);
    	if (cur_dist < best_dist || best_dist == 0){
    	  best_dist = cur_dist;
	  best_id = j;
    	}
      }
    }
    current = data[best_id];
    nn_path[i] = best_id;
    usedup |= (2 << (best_id-1));
  }

  return nn_path;
}

int *tsp_cp(gsl_vector **data, int n)
{
  /* Solve the TSP using the closest pair heuristic. */
  int i, j, ch1_id, ch2_id, best_ch1, best_ch2;
  int match_flag;
  double dist, best_dist;
  dlist_node *node;
  int *best_path = (int *) calloc(n, sizeof(int));

  /* Create an array where each element corresponds to a chain. */
  dlist **chains = (dlist **) calloc(n, sizeof(dlist *));
  for (i = 0; i < n; i++){
    chains[i] = dlist_new(data[i]);
  }

  /* Merge the chains according to the heuristic. */
  for (i = 0; i < n-1; i++){
    /* Find the smallest-distance chain ends pair. */
    best_dist = 0.;
    for (ch1_id = 0; ch1_id < n; ch1_id++){
      if (chains[ch1_id] == NULL){
	continue;
      }
      for (ch2_id = 0; ch2_id < n; ch2_id++){
	if (chains[ch2_id] == NULL || ch1_id == ch2_id){
	  continue;
	}
	dist = vector_dist(chains[ch1_id]->head->item,
			   chains[ch2_id]->head->item);
	if (dist < best_dist || best_dist == 0){
	  /* Heads match. */
	  best_dist = dist;
	  best_ch1 = ch1_id;
	  best_ch2 = ch2_id;
	  match_flag = 0;
	}

	dist = vector_dist(chains[ch1_id]->head->item,
			   chains[ch2_id]->tail->item);
	if (dist < best_dist || best_dist == 0){
	  /* Head1 matches tail2. */
	  best_dist = dist;
	  best_ch1 = ch1_id;
	  best_ch2 = ch2_id;
	  match_flag = 1;
	}

	dist = vector_dist(chains[ch1_id]->tail->item,
			   chains[ch2_id]->head->item);
	if (dist < best_dist || best_dist == 0){
	  /* Tail1 matches head2. */
	  best_dist = dist;
	  best_ch1 = ch1_id;
	  best_ch2 = ch2_id;
	  match_flag = 2;
	}

	dist = vector_dist(chains[ch1_id]->tail->item,
			   chains[ch2_id]->head->item);
	if (dist < best_dist || best_dist == 0){
	  /* Tail1 matches tail2. */
	  best_dist = dist;
	  best_ch1 = ch1_id;
	  best_ch2 = ch2_id;
	  match_flag = 3;
	}
      }
    }
    printf("Iter %d, best match (%d, %d) %f\n",
	   i, best_ch1, best_ch2, best_dist);

    /* Merge the smallest-distance chain ends. */
    if (match_flag == 0){
      /* Heads match: push ch2 to the front of ch1 in reverse-order. */
      while(chains[best_ch2]->head){
	dlist_insert(chains[best_ch1],
		     chains[best_ch2]->head->item);
	dlist_delete(chains[best_ch2], chains[best_ch2]->head);
      }
    }
    else if (match_flag == 1){
      /* Head1 to tail2 match: push ch2 to the front of ch1 in-order. */
      while(chains[best_ch2]->tail){
	dlist_insert(chains[best_ch1],
		     chains[best_ch2]->tail->item);
	dlist_delete(chains[best_ch2], chains[best_ch2]->tail);
      }
    }
    else if (match_flag == 2){
      /* Head2 to tail1 match: push ch2 to the end of ch1 in-order. */
      while(chains[best_ch2]->head){
	dlist_insert_tail(chains[best_ch1],
			  chains[best_ch2]->head->item);
	dlist_delete(chains[best_ch2], chains[best_ch2]->head);
      }
    }
    else if (match_flag == 3){
      /* Tails match: push ch2 to the end of ch1 in reverse-order. */
      while(chains[best_ch2]->tail){
	dlist_insert_tail(chains[best_ch1],
			  chains[best_ch2]->tail->item);
	dlist_delete(chains[best_ch2], chains[best_ch2]->tail);
      }
    }
    chains[best_ch2] = NULL;
  }

  /* All the chains except for one should be null. The non-null
     chain contains the best (heuristic) path. */
  for (i = 0; i < n; i++)
    if (chains[i])
      node = chains[i]->head;

  for(i = 0; node; i++){
    for (j = 0; j < n; j++)
      if (data[j] == node->item)
	best_path[i] = j;
    node = node->next;
  }
  return best_path;
}

int *tsp_bs(gsl_vector **data, int n)
{
  /* Find the shortest loop between `n` points. stored in `data`. */
  if (n == 0)
    return NULL;
  double best_dist = 0;
  double cur_dist = 0;
  int i_perm, i_iter;
  int *cur_path;
  int *best_path = calloc(n, sizeof(int));

  int **perm = permutations(n);
  for (i_iter = 0; i_iter < factorial(n); i_iter++){
    /* Retrieve the permutation into an array. */
    cur_path = perm[i_iter];

    /* Compute the path length corresponding to this permutation. */
    printf("Iter %d, permutation ", i_iter);
    print_intarray(cur_path, n);
    cur_dist = pathlen(data, cur_path, n);
    printf(" dist %.10f.\n", cur_dist);

    /* Save best pathlen to-date. */
    if (best_dist > cur_dist || best_dist == 0){
      best_dist = cur_dist;
      for (i_perm = 0; i_perm < n; i_perm++)
	best_path[i_perm] = cur_path[i_perm];
    }
  }
  printf("\n");
  free(cur_path);
  free(perm);
  return best_path;
}

int main(){
  char *fname;
  int npoints;
  int *bestpath;
  gsl_vector **vecs;

  /* Load data into a vector array. */
  fname = "tsp.dat";
  npoints = get_nlines(fname);
  vecs = load_vectors_from_file("tsp.dat", npoints);

  /* Compute the best path using the brute-force method. */
  bestpath = tsp_bs(vecs, npoints);
  printf("Best path (brute-force) len = %f, for path ",
	 pathlen(vecs, bestpath, npoints));
  print_intarray(bestpath, npoints);
  printf("\n");

  /* Compute the best path using the nearest-neighbor heuristic. */
  bestpath = tsp_nn(vecs, npoints);
  printf("Best path (nearest-neighbor) len = %f, for path ",
	 pathlen(vecs, bestpath, npoints));
  print_intarray(bestpath, npoints);
  printf("\n");

  /* Compute the best path using the closest-pairs heuristic. */
  bestpath = tsp_cp(vecs, npoints);
  printf("Best path (closest-pair) len = %f, for path ",
	 pathlen(vecs, bestpath, npoints));
  print_intarray(bestpath, npoints);
  printf("\n");

  return 0;
}


// Copyright [1999-2017] EMBL-European Bioinformatics Institute
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_cdf.h>

#include "multiSet.h"

typedef struct setComparisonData_st {
	Multiset * multi;
} SetComparisonData;

void SetComparisonSeek(WiggleIterator * iter, const char * chrom, int start, int finish) {
	SetComparisonData * data = (SetComparisonData* ) iter->data;
	seekMultiset(data->multi, chrom, start, finish);
	pop(iter);
}

////////////////////////////////////////////////////////
// T-test
////////////////////////////////////////////////////////

void TTestReductionPop(WiggleIterator * wi) {
	if (wi->done)
		return;

	SetComparisonData * data = (SetComparisonData *) wi->data;
	Multiset * multi = data->multi;

	if (multi->done) {
		wi->done = true;
		return;
	}

	// Go to first position where both of the sets have at least one value
	while (!multi->inplay[0] || !multi->inplay[1]) {
		popMultiset(multi);
		if (multi->done) {
			wi->done = true;
			return;
		}
	}
	wi->chrom = multi->chrom;
	wi->start = multi->start;
	wi->finish = multi->finish;

	// Compute measurements
	double sum1, sum2, sumSq1, sumSq2;
	int count1, count2;
	int index;

	sum1 = sum2 = 0;
	sumSq1 = sumSq2 = 0;
	count1 = multi->multis[0]->count;
	count2 = multi->multis[1]->count;

	for (index = 0; index < multi->multis[0]->count; index++) {
		if (multi->multis[0]->inplay[index]) {
			sum1 += multi->values[0][index];
			sumSq1 += multi->values[0][index] * multi->values[0][index];
		}
	}

	for (index = 0; index < multi->multis[1]->count; index++) {
		if (multi->multis[1]->inplay[index]) {
			sum2 += multi->values[1][index];
			sumSq2 += multi->values[1][index] * multi->values[1][index];
		}
	}

	// To avoid divisions by 0:
	if (count1 == 0 || count2 == 0) {
		wi->value = NAN;
		popMultiset(multi);
		return;
	}

	double mean1 = sum1 / count1;
	double mean2 = sum2 / count2;
	double meanSq1 = sumSq1 / count1;
	double meanSq2 = sumSq2 / count2;
	double var1 = meanSq1 - mean1 * mean1;
	double var2 = meanSq2 - mean2 * mean2;

	// To avoid divisions by 0:
	if (var1 + var2 == 0) {
		wi->value = NAN;
		popMultiset(multi);
		return;
	}

	// T-statistic

	double t = (mean1 - mean2) / sqrt(var1 / count1 + var2 / count2);

	if (t < 0)
		t = -t;

	// Degrees of freedom

	double nu = (var1 / count1 + var2 / count2) * (var1 / count1 + var2 / count2) / ((var1 * var1) / (count1 * count1 * (count1 - 1)) + (var2 * var2) / (count2 * count2 * (count2 - 1)));

	// P-value

	wi->value = 2 * gsl_cdf_tdist_Q(t, nu);

	// Update inputs
	popMultiset(multi);
}

WiggleIterator * TTestReduction(Multiset * multi) {
	SetComparisonData * data = (SetComparisonData *) calloc(1, sizeof(SetComparisonData));
	if (multi->count != 2 || multi->multis[0]->count < 3 || multi->multis[1]->count < 3) {
		puts("The t-test function only works for two sets with enough elements to compute variance");
		exit(1);
	}
	data->multi = multi;
	return newWiggleIterator(data, &TTestReductionPop, &SetComparisonSeek, NAN);
}

////////////////////////////////////////////////////////
// F-test
////////////////////////////////////////////////////////

typedef struct ftestData_st {
	Multiset * multi;
	int * counts;
	double * means;
	int total_count;
} FTestData;

void FTestSeek(WiggleIterator * iter, const char * chrom, int start, int finish) {
	FTestData * data = (FTestData* ) iter->data;
	seekMultiset(data->multi, chrom, start, finish);
	pop(iter);
}

void FTestReductionPop(WiggleIterator * wi) {
	if (wi->done)
		return;

	FTestData * data = (FTestData *) wi->data;
	Multiset * multi = data->multi;

	if (multi->done) {
		wi->done = true;
		return;
	}

	// Go to first position where both of the sets have at least one value
	while (!multi->inplay[0] || !multi->inplay[1]) {
		popMultiset(multi);
		if (multi->done) {
			wi->done = true;
			return;
		}
	}
	wi->chrom = multi->chrom;
	wi->start = multi->start;
	wi->finish = multi->finish;

	// Compute means
	double mean = 0;
	int groups = multi->count;
	int index, index2;
	for (index = 0; index < groups; index++) {
		Multiplexer * mplx = multi->multis[index];
		data->means[index] = 0;
		for (index2 = 0; index < mplx->count; index++) {
			if (mplx->inplay[index2])
				data->means[index] += mplx->values[index2];
			else
				data->means[index] += mplx->iters[index2]->default_value;
		}
		mean += data->means[index];
		data->means[index] /= data->counts[index];
	}
	mean /= data->total_count;

	double inter = 0;
	double intra = 0;
	for (index = 0; index < groups; index++) {
		Multiplexer * mplx = multi->multis[index];
		inter += mplx->count * (data->means[index] - mean) * (data->means[index] - mean);
		for (index2 = 0; index < mplx->count; index++) {
			if (mplx->inplay[index2])
				intra += (mplx->values[index2] - data->means[index]) * (mplx->values[index2] - data->means[index]);
			else
				intra += (mplx->iters[index2]->default_value - data->means[index]) * (mplx->iters[index2]->default_value - data->means[index]);
		}
	}

	// F-statistic
	inter /= groups - 1;
	intra /= data->total_count - groups;
	double f = inter / intra;

	// P-value
	wi->value = 2 * gsl_cdf_fdist_Q(f, multi->count - 1, data->total_count - multi->count);

	// Update inputs
	popMultiset(multi);
}

WiggleIterator * FTestReduction(Multiset * multi) {
	FTestData * data = (FTestData *) calloc(1, sizeof(FTestData));
	data->multi = multi;
	data->means = calloc(multi->count, sizeof(double));
	data->counts = calloc(multi->count, sizeof(int));
	int index;
	for (index = 0; index < multi->count; index++) {
		data->counts[index] = multi->multis[index]->count;
		data->total_count += data->counts[index];
	}
	return newWiggleIterator(data, &FTestReductionPop, &FTestSeek, NAN);
}

////////////////////////////////////////////////////////
// Mann-Whitney U (Wilcoxon rank-sum test)
////////////////////////////////////////////////////////

typedef struct valueSetPair_st {
	double value;
	bool set;
} ValueSetPair;

typedef struct mwuData_st {
	Multiset * multi;
	int n1;
	int n2;
	int N;
	// Pre-allocated table for sorting
	ValueSetPair * rankingTable;
	// For normal approximation
	bool normalApproximation;
	double mu_U, sigma_U;
} MWUData;

void MWUSeek(WiggleIterator * iter, const char * chrom, int start, int finish) {
	MWUData * data = (MWUData* ) iter->data;
	seekMultiset(data->multi, chrom, start, finish);
	pop(iter);
}

static int compareValueSetPairs(const void * A, const void * B) {
	ValueSetPair * vspA = (ValueSetPair *) A;
	ValueSetPair * vspB = (ValueSetPair *) B;
	if (vspA->value < vspB->value)
		return -1;
	if (vspA->value > vspB->value)
		return 1;
	return 0;
}

void MWUReductionPop(WiggleIterator * wi) {
	if (wi->done)
		return;

	MWUData * data = (MWUData *) wi->data;
	Multiset * multi = data->multi;

	if (multi->done) {
		wi->done = true;
		return;
	}

	// Go to first position where both of the sets have at least one value
	while (!multi->inplay[0] || !multi->inplay[1]) {
		popMultiset(multi);
		if (multi->done) {
			wi->done = true;
			return;
		}
	}
	wi->chrom = multi->chrom;
	wi->start = multi->start;
	wi->finish = multi->finish;

	// Compute measurements
	int index;
	ValueSetPair * vspPtr = data->rankingTable;

	for (index = 0; index < data->n1; index++) {
		if (multi->multis[0]->inplay[index])
			vspPtr->value = multi->values[0][index];
		else
			vspPtr->value = multi->multis[0]->iters[index]->default_value;
		if (isnan(vspPtr->value)) {
			wi->value = NAN;
			popMultiset(multi);
			return;
		}
		vspPtr->set = false;
		vspPtr++;
	}

	for (index = 0; index < data->n2; index++) {
		if (multi->multis[1]->inplay[index])
			vspPtr->value = multi->values[1][index];
		else
			vspPtr->value = multi->multis[1]->iters[index]->default_value;
		if (isnan(vspPtr->value)) {
			wi->value = NAN;
			popMultiset(multi);
			return;
		}
		vspPtr->set = true;
		vspPtr++;
	}

	qsort(data->rankingTable, data->N, sizeof(ValueSetPair), compareValueSetPairs);

	// Sum of ranks of elements of set 1
	double U1 = 0;
	// Rolling count of elements of set 1 seen prior on the list
	int prev = 0;
	// Warns you when you have a tie with the previously visited Value-Set pairs
	int ties = 0;
	int previousTies = 0;
	vspPtr = data->rankingTable;
	for (index = 0; index < data->N && prev < data->n1; index++) {
		if (!vspPtr->set) {
			U1 += index - prev;
			if (ties) {
				// Look for ties on the table prior to the current position and after the last occurence of an element of set 1.
				int index2;
				for (index2 = index + 1; index2 < data->N && data->rankingTable[index2].value == vspPtr->value && data->rankingTable[index2].set; index2++)
					previousTies++;
				U1 -= previousTies / 2.0;
				U1 += (ties - previousTies) / 2.0;
				if (previousTies == ties)
					previousTies = ties = 0;
			} else {
				int index2;
				// Look for ties with next values
				for (index2 = index + 1; index2 < data->N && data->rankingTable[index2].value == vspPtr->value; index2++)
					if (data->rankingTable[index2].set)
						ties++;
				if (ties)
					U1 += ties / 2.0;
			}
			prev++;
		}
		vspPtr++;
	}

	if (data->normalApproximation) {
		if (U1 > data->mu_U)
			wi->value = 2 * erf((data->mu_U - U1) / data->sigma_U);
		else
			wi->value = 2 * erf((U1 - data->mu_U) / data->sigma_U);
	}

	// Update inputs
	popMultiset(multi);
}

WiggleIterator * MWUReduction(Multiset * multi) {
	MWUData * data = (MWUData *) calloc(1, sizeof(MWUData));
	if (multi->count != 2 || multi->multis[0]->count == 0 || multi->multis[1]->count == 0) {
		puts("The Mann-Whitney U function only works for two non-empty sets");
		exit(1);
	}
	data->multi = multi;
	data->n1 = multi->multis[0]->count;
	data->n2 = multi->multis[1]->count;
	data->N = data->n1 + data->n2;
	data->rankingTable = calloc(data->N, sizeof(ValueSetPair));
	if (true) {
		// Ideally, tables could be used for small values of n1 and n2
		data->normalApproximation = true;
		data->mu_U = data->n1 * data->n2 / 2;
		data->sigma_U = sqrt(data->n1 * data->n2 * (data->n1 + data->n2 + 1) / 12);
	}
	return newWiggleIterator(data, &MWUReductionPop, &MWUSeek, NAN);
}


/**
 *
 * @file core_cpltmg_condex.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Mathieu Faverge
 * @date 2010-11-15
 * @generated c Tue Jan  7 11:44:47 2014
 *
 **/
#include <math.h>
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex32_t
 *
 *  CORE_cpltmg_condexq generates the Q used in condex matrix generation
 *
 *  See http://www.mathworks.fr/fr/help/matlab/ref/gallery.html#f84-999898
 *  gallery('condex',n,4,100)
 *
 *  Returns a "counter-example" matrix to a condition estimator. It has order n
 *  and scalar parameter theta (default 100).
 *
 *  LAPACK (RCOND): It is the inverse of this matrix that is a counter-example.
 *
 *******************************************************************************
 *
 * @param[in] M
 *         The number of rows of the matrix Q used in condex generation. M >= 0.
 *
 * @param[in] N
 *         The number of columns of the matrix A to be generated. N >= 0.
 *
 * @param[out] Q
 *         On entry, the M-by-3 matrix to be initialized.
 *         On exit, the housholder reflectors required for condex generation.
 *
 * @param[in] LDQ
 *         The leading dimension of the matrix Q. LDQ >= max(1,M).
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_cpltmg_condexq = PCORE_cpltmg_condexq
#define CORE_cpltmg_condexq PCORE_cpltmg_condexq
#endif
void CORE_cpltmg_condexq( int M, int N, PLASMA_Complex32_t *Q, int LDQ )
{
    PLASMA_Complex32_t tau[3];
    PLASMA_Complex32_t *tQ = Q;
    int i;

    /* First column is [ 1 ... 1 ] */
    for( i=0; i < M; i++, tQ++ )
        *tQ = (PLASMA_Complex32_t)1.0;

    /* Second column is [1 0 0 ... 0] */
    tQ = Q + LDQ;
    *tQ = (PLASMA_Complex32_t)1.;
    tQ++;
    memset( tQ, 0, (M-1) * sizeof(PLASMA_Complex32_t) );

    /* Third column is  (-1)^i * (1. + i / (N-1)) */
    tQ = Q + 2 * LDQ;
    for( i=0; i<M; i++, tQ++ )
        *tQ = (PLASMA_Complex32_t)( cpowf( -1.0, (float)i ) * (1.0 + (float)i/(N-1) ) );

    /* Generate orthogonal projector */
    LAPACKE_cgeqrf( LAPACK_COL_MAJOR, M, 3,    Q, LDQ, tau );
    LAPACKE_cungqr( LAPACK_COL_MAJOR, M, 3, 3, Q, LDQ, tau );

    return;
}


//
//  mcmc_solar.c
//
//
//  Created by Juan Pablo Molano on 11/11/15.
//
//

#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <math.h>
#define USAGE "./mcmc_solar.x n_burn n_steps"

//------------------------------------------------------------------------------------------------------------

float *load_matrix(char *filename, int *n, int *m);
void get_data( float *array , float *x, int n_row);
float *reserva(int n);
void print_data(float *array, float *array2, float *array3,  float *array4 , float *array5 , float *array6 , float *array7 , float *array8,  int n_puntos);
void get_patch( float *array,  float *x , int n_row);
void my_model( float *x_obs, float *array, float a, float b, float c , float d,  int n_row);
float likelihood(float *y_obs, float *y_model, int n_row);
int min_likelihood( float *array , int n_row);

//------------------------------------------------------------------------------------------------------------

int main(int argc, char **argv){
    float *matrix , *tim, *man, *count, *a_walk, *b_walk, *c_walk, *d_walk, *l_walk, *y_init, *y_prime, *best_y;
    srand48(time(NULL));
    float a_prime, b_prime, c_prime, d_prime, l_prime, l_init, gamma, alpha, beta , best_a , best_b, best_c, best_d;
    int n_row, n_cols , i, rlin = 0;

    a_walk = reserva(atof(argv[1])+atof(argv[2]));
    b_walk = reserva(atof(argv[1])+atof(argv[2]));
    c_walk = reserva(atof(argv[1])+atof(argv[2]));
    d_walk = reserva(atof(argv[1])+atof(argv[2]));
    l_walk = reserva(atof(argv[1])+atof(argv[2]));
    y_init = reserva(n_row);
    y_prime = reserva(n_row);

    a_walk[0] = drand48();
    b_walk[0] = drand48();
    c_walk[0] = drand48();
    d_walk[0] = drand48();

    matrix = load_matrix( "sol.dat" , &n_row, &n_cols);

    man = reserva( n_row);
    tim = reserva(n_row);
    man = reserva(n_row);
    best_y = reserva( n_row);
    get_data( matrix , tim , n_row);
    get_patch( matrix ,  man  ,  n_row);

    for (i=0;i<n_row;i++) {
        if (man[i] == -99) {
            man[i] = 0.0;
        }
    }

    my_model( tim , y_init , a_walk[0] , b_walk[0] , c_walk[0] , d_walk[0] , n_row );

    l_walk[0] = likelihood( man , y_init, n_row);

    const gsl_rng_type * T;
    gsl_rng * r;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    for (i=0; i < atof(argv[1]) + atof(argv[2]); i++) {

        a_prime = gsl_ran_gaussian(r, 0.1) + a_walk[i];
        b_prime = gsl_ran_gaussian(r, 0.1) + b_walk[i];
        c_prime = gsl_ran_gaussian(r, 0.1) + c_walk[i];
        d_prime = gsl_ran_gaussian(r, 0.1) + d_walk[i];

        my_model( tim, y_init, a_walk[i], b_walk[i], c_walk[i], d_walk[i], n_row);
        my_model(tim, y_prime ,a_prime, b_prime, c_prime, d_prime, n_row);

        l_prime = likelihood( man, y_prime , n_row);
        l_init = likelihood( man, y_init ,  n_row);

        gamma = l_init - l_prime;
        if (gamma>=0.0) {
            a_walk[i+1] = a_prime;
            b_walk[i+1] = b_prime;
            c_walk[i+1] = c_prime;
            d_walk[i+1] = d_prime;
            l_walk[i+1] = l_prime;
        }
        else{

            beta = drand48();
            alpha = exp(gamma);
            if (beta <= alpha) {

                a_walk[i+1]  = a_prime;
                b_walk[i+1]  = b_prime;
                c_walk[i+1]  = c_prime;
                d_walk[i+1]  = d_prime;
                l_walk[i+1]  = l_prime;
            }
            else{

                a_walk[i+1] = a_walk[i];
                b_walk[i+1] = b_walk[i];
                c_walk[i+1] = c_walk[i];
                d_walk[i+1] = d_walk[i];
                l_walk[i+1] = l_init;

            }

        }
    }

    i = min_likelihood( l_walk , n_row);
    best_a = a_walk[i];
    best_b = b_walk[i];
    best_c = c_walk[i];
    best_d = d_walk[i];

    my_model( tim , best_y , best_a, best_b , best_c , best_d , n_row);

    print_data( tim , man,  l_walk , a_walk , b_walk , c_walk , d_walk , best_y, n_row);

    gsl_rng_free (r);

    return 0;
}

//------------------------------------------------------------------------------------------------------------

float *load_matrix(char *filename, int *n, int *m){
    float *matrix;
    FILE *in;
    int n_row = 4632, n_cols = 5;
    int i;
    int j;

    if(!(in=fopen(filename, "r"))){
        printf("Problem opening file %s\n", filename);
        exit(1);
    }

    matrix = malloc(n_row * n_cols * sizeof(float));

    for(i=0;i<n_row;i++){
        for(j=0;j<n_cols;j++){
            fscanf(in, "%f", &matrix[i*n_cols + j]);
        }
    }
    *n = n_row;
    *m = n_cols;
    return matrix;
}

//------------------------------------------------------------------------------------------------------------

void get_data( float *array , float *x , int n_row){

    int i,j;

    for(i=0;i<n_row;i++){

            x[i] = array[i*5] + array[i*5 + 1]/12 + array[i*5 + 2]/365;

    }

}

//------------------------------------------------------------------------------------------------------------

void get_patch( float *array,  float *x , int n_row){

    int i,j;

    for(i=0;i<n_row;i++){

        x[i] = array[i*5 + 3];
    }

}

//------------------------------------------------------------------------------------------------------------

float *reserva(int n){
    float *array;
    int i;
    if(!(array = malloc(n * sizeof(float)))){
        printf("Problema en reserva\n");
        exit(1);
    }
    for(i=0;i<n ;i++){
        array[i] = 0.0;
    }
    return array;
}

//------------------------------------------------------------------------------------------------------------

void print_data(float *array, float *array2 , float *array3 , float *array4 , float *array5 , float *array6, float *array7, float *array8 , int n_puntos){
    int i;
    for(i=0;i<n_puntos;i++){
        printf("%f \t %f \t %f \t %f \t %f \t %f \t %f \t %f \n", array[i] , array2[i] , array3[i] , array4[i] , array5[i] , array6[i], array7[i], array8[i]);
    }
}

//------------------------------------------------------------------------------------------------------------

float likelihood(float *y_obs, float *y_model, int n_row){

    int i;
    float c=0;

    for (i=0; i<n_row; i++) {

        c = c + ( y_obs[i] - y_model[i]);
    }
    c = (1.0/2.0)*pow(c,2);

    return c;

}

//------------------------------------------------------------------------------------------------------------

void my_model( float *x_obs, float *array , float a, float b, float c , float d,  int n_row){

    int i;
    for (i=0; i<n_row; i++) {
        array[i] = a*cos((2*3.141592653589793/d)*x_obs[i] + b) + c;
    }
}

//------------------------------------------------------------------------------------------------------------

int min_likelihood( float *array , int n_row){

    int i;
    int ind = 0;

    for(i = 1; i < n_row; i ++){
        if ( array[i] < ind) {
            ind = i;
        }

    }

    return ind;
}


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

int
func (double t, const double y[], double f[],
      void *params)
{
  double mu = *(double *)params;
  f[0] = y[1];
  f[1] = -y[0] - mu*y[1]*(y[0]*y[0] - 1);
  return GSL_SUCCESS;
}

int
jac (double t, const double y[], double *dfdy,
     double dfdt[], void *params)
{
  double mu = *(double *)params;
  gsl_matrix_view dfdy_mat
    = gsl_matrix_view_array (dfdy, 2, 2);
  gsl_matrix * m = &dfdy_mat.matrix;
  gsl_matrix_set (m, 0, 0, 0.0);
  gsl_matrix_set (m, 0, 1, 1.0);
  gsl_matrix_set (m, 1, 0, -2.0*mu*y[0]*y[1] - 1.0);
  gsl_matrix_set (m, 1, 1, -mu*(y[0]*y[0] - 1.0));
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  return GSL_SUCCESS;
}

int
main (void)
{
  double mu = 10;
  gsl_odeiv2_system sys = {func, jac, 2, &mu};

  gsl_odeiv2_driver * d =
    gsl_odeiv2_driver_alloc_y_new (&sys, gsl_odeiv2_step_rk8pd,
				  1e-6, 1e-6, 0.0);
  int i;
  double t = 0.0, t1 = 100.0;
  double y[2] = { 1.0, 0.0 };

  for (i = 1; i <= 100; i++)
    {
      double ti = i * t1 / 100.0;
      int status = gsl_odeiv2_driver_apply (d, &t, ti, y);

      if (status != GSL_SUCCESS)
	{
	  printf ("error, return value=%d\n", status);
	  break;
	}

      printf ("%.5e %.5e %.5e\n", t, y[0], y[1]);
    }

  gsl_odeiv2_driver_free (d);
  return 0;
}


/**
 * Subroutines used in the "aXe_AF2PET.c" for the task aXe_AF2PET
 */

#include <stdarg.h>

#include <gsl/gsl_vector.h>
#include <gsl/gsl_nan.h>
#include <gsl/gsl_sys.h>

#include "spc_wl_calib.h"
#include "aXe_grism.h"
#include "aXe_errors.h"
#include "disp_conf.h"

/**
 * The function performs a wavelength calibration on an allready
 * existing ap_pixel table. It fills in the lambda and dlambda fields.
 *
 * @param ap_p  - the ap_pixel table to work on, with the pathlength field
 *                filled out
 * @param calib - the calibration function
 */
void
wl_calib (ap_pixel * ap_p, const calib_function * const calib)
{
  double l1,l2;

  while (ap_p->p_x != -1)
    {
      ap_p->lambda = calib->func (ap_p->xi, calib->order, calib->coeffs);

      l1 = calib->func (ap_p->xi - .5, calib->order, calib->coeffs);
      l2 = calib->func (ap_p->xi + .5, calib->order, calib->coeffs);
      ap_p->dlambda = fabs(l2-l1);
      ap_p++;
    }
}

/*
 * Function: pwise_wl_calib
 * Make for every PET pixel an individual wavelength
 * calibration. The function was developed for FORS2 MXU spectra
 * and makes especially sense for long spectra and/or fast
 * variations of the 2D calibration solution.
 */
void
pwise_wl_calib(const global_disp *gdisp, const d_point pixel,
	       const beam actbeam, const int for_grism, ap_pixel *ap_p, const calib_function *old_calib)
{

  calib_function *calib;
  d_point actpix;

  double l1,l2;
  double tmp;

  int i=0;

  while (ap_p[i].p_x != -1)
    {
      // get the current xy-position of the pixel
      actpix.x = pixel.x + ap_p[i].dist*cos(actbeam.orient);
      actpix.y = pixel.y + ap_p[i].dist*sin(actbeam.orient);

      // create a calibration for that position
      calib = create_calib_from_gsl_vector(for_grism, get_calvector_from_gdisp(gdisp, actpix));

      // determine the wavelength value
      ap_p[i].lambda = calib->func (ap_p[i].xi, calib->order, calib->coeffs);

      // determine the dlambda value
      l1 = calib->func (ap_p[i].xi - .5, calib->order, calib->coeffs);
      l2 = calib->func (ap_p[i].xi + .5, calib->order, calib->coeffs);
      ap_p[i].dlambda = fabs(l2-l1);

      // determine the trace length
      tmp =  old_calib->func (ap_p[i].xi, old_calib->order, old_calib->coeffs);
      ap_p[i].xi = ap_p[i].xi + (ap_p[i].lambda - tmp) / ap_p[i].dlambda;

      // free the calibration
      // structure
      free_calib(calib);

      // enhance the counter
      i++;
    }
}

/**
 *  The function trims down an ap_pixel table to only allowed values
 *  for the trace path length or trace distance. The input ap_pixel
 *  is only changed. Valid entries are moved to the front side,
 *  and a new endmark is set after all valid entries
 *
 *  @param  ap_pixel       the input ap_pixel table
 *  @param  prange         the valid trace range
 *  @param  wl_calibration the calibration structure, needed to get the
 *                         first dispersion coefficient
 *
 *  @return ret            ap_pixel table output
 */
ap_pixel *
prange_cut(ap_pixel * in_p, const gsl_vector * prange,
           const calib_function * wl_calibration)
{

  //long nelem;
  ap_pixel * ret, *cur_ap, *ap_p;
  double prel;
  double trmin=1.0E+06, trmax=-1.0E6;

  // set all PET list pointers
  // to the beginning of the array
  ret    = in_p;
  cur_ap = in_p;
  ap_p   = in_p;

  while (ap_p->p_x != -1)
    {
      // search for new minimum
      // and amximum in tracelength
      if (ap_p->xi > trmax)
        trmax = ap_p->xi;
      if (ap_p->xi < trmin)
        trmin = ap_p->xi;

      // compute the tracelength with
      // respect to the zeropoint
      prel = ap_p->xi - wl_calibration->coeffs[0];

      // check whether the realitive zeropoint is within the allowed range
      if (prel > gsl_vector_get(prange,0) && prel < gsl_vector_get(prange,1))
        {
          // transfer the properties
          // of the good pixel to the next
          // PET element in the list of good PET's
          cur_ap->p_x    = ap_p->p_x;
          cur_ap->p_y    = ap_p->p_y;
          cur_ap->x      = ap_p->x;
          cur_ap->y      = ap_p->y;
          cur_ap->dist   = ap_p->dist;
          cur_ap->xs     = ap_p->xs;
          cur_ap->ys     = ap_p->ys;
          cur_ap->dxs    = ap_p->dxs;
          cur_ap->xi     = ap_p->xi;
          cur_ap->count  = ap_p->count;
          cur_ap->error  = ap_p->error;
          cur_ap->weight = ap_p->weight;
          cur_ap->contam = ap_p->contam;
          cur_ap->dq     = ap_p->dq;

          // forward the pointer
          // of good pixels
          cur_ap++;
        }

      // always forward the
      // general pointer
      ap_p++;
    }

  // make a new endmark
  cur_ap->p_x = -1;
  cur_ap->p_y = -1;
  cur_ap->count = -1;

  // return the pointer
  return ret;
}

/**
 *  Calibration function for the grism: simple polynomial
 *
 * @param xi     the path length of the sample point
 * @param order  the order of the polynomial, i.e. the number of coefficients
 *               minus one
 * @param coeffs a pointer to order+1 doubles, starting with the zeroth
 *               coefficent
 *
 * @return res   wavelength at xi
 */
static double
grism_calib_func (const double xi, const int order,
		  const double *const coeffs)
{
  int i;
  double res = 0;

  for (i = 0; i <= order; i++)
    {
      res += coeffs[i] * pow(xi,i*1.0);
    }

  return res;
}

/**
 * Calibration function for the prism: reciprocal polynomial in the
 * form: lambda = a1 + a2/(xi-a0) + a3/(xi-a0)**2 + a4/(xi-a0)**3
 *
 * @param  xi     the path length of the sample point
 * @param  order  the order of the polynomial, i.e. the number of coefficients
 *                minus one
 * @param  coeffs a pointer to order+1 doubles, starting with the zeroth
 *                coefficent
 * @return res    wavelength at xi
 */
static double
prism_calib_func (const double xi, const int order,
		  const double *const coeffs)
{
  int i;
  double res = 0;

  if (xi==0.) return GSL_NAN;
  for (i = 0; i <= order-1; i++)
    {
      //	    res += coeffs[i] / pow(xi,i*1.0);
      res += coeffs[i+1] / pow((xi-coeffs[0]),i*1.0);
    }

  return res;
}

/**
 * Create a prism or grism calibration function, i.e. a function of one of
 * the forms "sum(a_i 1/x^i, i=0..order)" or "sum(a_i x^i, i=0..order)"
 * for transforming pathlength along the trace to lambda.
 *
 *@param for_grism if true, a the second form of the calibration function
 *                 is used (normal polynom), otherwise the first one.
 *@param order     order of the polynom
 *@param a         a GSL vector of length n containing the n coefficients
 *                 describing the wavelength calibration polynomial.
 *@return calib    an allocated structure containing an allocated array
 *                 for the coefficients.  Free using calib_free.
 */
calib_function *
create_calib_from_gsl_vector (const int for_grism, const gsl_vector * a)
{
  calib_function *calib;
  int i, order;

  // set the polynomial order
  order = a->size - 1;

  // allocate memory
  calib = malloc(sizeof (calib_function));

  // complain if memory
  // allocation failed
  if (!calib)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__, "Out of memory");

  // store the order of the
  // calibration
  calib->order = order;

  // transfer the calibration functions
  if (for_grism)
    calib->func = &grism_calib_func;
  else
    calib->func = &prism_calib_func;

  // allocate memory; complain if problems occur
  calib->coeffs = malloc ((order + 1) * sizeof (double));
  if (!calib->coeffs)
    aXe_message (aXe_M_FATAL, __FILE__, __LINE__, "Out of memory");

  // transfer the coefficients
  calib->coeffs[0] = gsl_vector_get (a, 0);
  for (i = 1; i <= order; i++)
      calib->coeffs[i] = gsl_vector_get (a, i);

  // set the allowed range for
  // the trace lengths to NULL
  calib->pr_range = NULL;

  // return the calibration
  // structure
  return calib;
}

/**
 * Function: free_calib
 * Free a calibration function and its assoicated data.
 *
 * @param  calib the calibration structure to be freed
 *
 */
void
free_calib(calib_function * calib)
{
  free (calib->coeffs);
  calib->coeffs = NULL;
  if (calib->pr_range != NULL)
    {
      gsl_vector_free(calib->pr_range);
      calib->pr_range = NULL;
    }
  free (calib);
  calib = NULL;
}


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>

static void
initialise (double * ri, double * rj, double * rg, double * rh,
            double alpha, double beta);

gsl_integration_qaws_table *
gsl_integration_qaws_table_alloc (double alpha, double beta, int mu, int nu)
{
  gsl_integration_qaws_table * t;

  if (alpha < -1.0)
    {
      GSL_ERROR_VAL ("alpha must be greater than -1.0", GSL_EINVAL, 0);
    }

  if (beta < -1.0)
    {
      GSL_ERROR_VAL ("beta must be greater than -1.0", GSL_EINVAL, 0);
    }

  if (mu != 0 && mu != 1)
    {
      GSL_ERROR_VAL ("mu must be 0 or 1", GSL_EINVAL, 0);
    }

  if (nu != 0 && nu != 1)
    {
      GSL_ERROR_VAL ("nu must be 0 or 1", GSL_EINVAL, 0);
    }

  t = (gsl_integration_qaws_table *)
    malloc(sizeof(gsl_integration_qaws_table));

  if (t == 0)
    {
      GSL_ERROR_VAL ("failed to allocate space for qaws_table struct",
                        GSL_ENOMEM, 0);
    }

  t->alpha = alpha;
  t->beta = beta;
  t->mu = mu;
  t->nu = nu;

  initialise (t->ri, t->rj, t->rg, t->rh, alpha, beta);

  return t;
}

int
gsl_integration_qaws_table_set (gsl_integration_qaws_table * t,
                                double alpha, double beta, int mu, int nu)
{
  if (alpha < -1.0)
    {
      GSL_ERROR ("alpha must be greater than -1.0", GSL_EINVAL);
    }

  if (beta < -1.0)
    {
      GSL_ERROR ("beta must be greater than -1.0", GSL_EINVAL);
    }

  if (mu != 0 && mu != 1)
    {
      GSL_ERROR ("mu must be 0 or 1", GSL_EINVAL);
    }

  if (nu != 0 && nu != 1)
    {
      GSL_ERROR ("nu must be 0 or 1", GSL_EINVAL);
    }

  t->alpha = alpha;
  t->beta = beta;
  t->mu = mu;
  t->nu = nu;

  initialise (t->ri, t->rj, t->rg, t->rh, alpha, beta);

  return GSL_SUCCESS;
}

void
gsl_integration_qaws_table_free (gsl_integration_qaws_table * t)
{
  RETURN_IF_NULL (t);
  free (t);
}

static void
initialise (double * ri, double * rj, double * rg, double * rh,
            double alpha, double beta)
{
  const double alpha_p1 = alpha + 1.0;
  const double beta_p1 = beta + 1.0;

  const double alpha_p2 = alpha + 2.0;
  const double beta_p2 = beta + 2.0;

  const double r_alpha = pow (2.0, alpha_p1);
  const double r_beta = pow (2.0, beta_p1);

  size_t i;

  double an, anm1;

  ri[0] = r_alpha / alpha_p1;
  ri[1] = ri[0] * alpha / alpha_p2;

  an = 2.0;
  anm1 = 1.0;

  for (i = 2; i < 25; i++)
    {
      ri[i] = -(r_alpha + an * (an - alpha_p2) * ri[i - 1])
        / (anm1 * (an + alpha_p1));
      anm1 = an;
      an = an + 1.0;
    }

  rj[0] = r_beta / beta_p1;
  rj[1] = rj[0] * beta / beta_p2;

  an = 2.0;
  anm1 = 1.0;

  for (i = 2; i < 25; i++)
    {
      rj[i] = -(r_beta + an * (an - beta_p2) * rj[i - 1])
        / (anm1 * (an + beta_p1));
      anm1 = an;
      an = an + 1.0;
    }

  rg[0] = -ri[0] / alpha_p1;
  rg[1] = -rg[0] - 2.0 * r_alpha / (alpha_p2 * alpha_p2);

  an = 2.0;
  anm1 = 1.0;

  for (i = 2; i < 25; i++)
    {
      rg[i] = -(an * (an - alpha_p2) * rg[i - 1] - an * ri[i - 1]
                + anm1 * ri[i]) / (anm1 * (an + alpha_p1));
      anm1 = an;
      an = an + 1.0;
    }

  rh[0] = -rj[0] / beta_p1;
  rh[1] = -rh[0] - 2.0 * r_beta / (beta_p2 * beta_p2);

  an = 2.0;
  anm1 = 1.0;

  for (i = 2; i < 25; i++)
    {
      rh[i] = -(an * (an - beta_p2) * rh[i - 1] - an * rj[i - 1]
                + anm1 * rj[i]) / (anm1 * (an + beta_p1));
      anm1 = an;
      an = an + 1.0;
    }

  for (i = 1; i < 25; i += 2)
    {
      rj[i] *= -1;
      rh[i] *= -1;
    }
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_legendre.h>

#include "error.h"

/* Calculate P_m^m(x) from the analytic result:
 *   P_m^m(x) = (-1)^m (2m-1)!! (1-x^2)^(m/2) , m > 0
 *            = 1 , m = 0
 */
static double legendre_Pmm(int m, double x)
{
  if(m == 0)
  {
    return 1.0;
  }
  else
  {
    double p_mm = 1.0;
    double root_factor = sqrt(1.0-x)*sqrt(1.0+x);
    double fact_coeff = 1.0;
    int i;
    for(i=1; i<=m; i++)
    {
      p_mm *= -fact_coeff * root_factor;
      fact_coeff += 2.0;
    }
    return p_mm;
  }
}

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

int
gsl_sf_legendre_P1_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    result->val = x;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_P2_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    result->val = 0.5*(3.0*x*x - 1.0);
    result->err = GSL_DBL_EPSILON * (fabs(3.0*x*x) + 1.0);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_P3_e(double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  {
    result->val = 0.5*x*(5.0*x*x - 3.0);
    result->err = GSL_DBL_EPSILON * (fabs(result->val) + 0.5 * fabs(x) * (fabs(5.0*x*x) + 3.0));
    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_Pl_e(const int l, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(l < 0 || x < -1.0 || x > 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(l == 0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(l == 1) {
    result->val = x;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(l == 2) {
    result->val = 0.5 * (3.0*x*x - 1.0);
    result->err = GSL_DBL_EPSILON * (fabs(3.0*x*x) + 1.0);
    /*result->err = 3.0 * GSL_DBL_EPSILON * fabs(result->val);
      removed this old bogus estimate [GJ]
      */
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x == -1.0) {
    result->val = ( GSL_IS_ODD(l) ? -1.0 : 1.0 );
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(l < 100000) {
    /* upward recurrence: l P_l = (2l-1) z P_{l-1} - (l-1) P_{l-2} */

    double p_ellm2 = 1.0;    /* P_0(x) */
    double p_ellm1 = x;      /* P_1(x) */
    double p_ell = p_ellm1;

    double e_ellm2 = GSL_DBL_EPSILON;
    double e_ellm1 = fabs(x)*GSL_DBL_EPSILON;
    double e_ell = e_ellm1;

    int ell;

    for(ell=2; ell <= l; ell++){
      p_ell = (x*(2*ell-1)*p_ellm1 - (ell-1)*p_ellm2) / ell;
      p_ellm2 = p_ellm1;
      p_ellm1 = p_ell;

      e_ell = 0.5*(fabs(x)*(2*ell-1.0) * e_ellm1 + (ell-1.0)*e_ellm2)/ell;
      e_ellm2 = e_ellm1;
      e_ellm1 = e_ell;
    }

    result->val = p_ell;
    result->err = e_ell + l*fabs(p_ell)*GSL_DBL_EPSILON;
    return GSL_SUCCESS;
  }
  else {
    /* Asymptotic expansion.
     * [Olver, p. 473]
     */
    double u  = l + 0.5;
    double th = acos(x);
    gsl_sf_result J0;
    gsl_sf_result Jm1;
    int stat_J0  = gsl_sf_bessel_J0_e(u*th, &J0);
    int stat_Jm1 = gsl_sf_bessel_Jn_e(-1, u*th, &Jm1);
    double pre;
    double B00;
    double c1;

    /* B00 = 1/8 (1 - th cot(th) / th^2
     * pre = sqrt(th/sin(th))
     */
    if(th < GSL_ROOT4_DBL_EPSILON) {
      B00 = (1.0 + th*th/15.0)/24.0;
      pre = 1.0 + th*th/12.0;
    }
    else {
      double sin_th = sqrt(1.0 - x*x);
      double cot_th = x / sin_th;
      B00 = 1.0/8.0 * (1.0 - th * cot_th) / (th*th);
      pre = sqrt(th/sin_th);
    }

    c1 = th/u * B00;

    result->val  = pre * (J0.val + c1 * Jm1.val);
    result->err  = pre * (J0.err + fabs(c1) * Jm1.err);
    result->err += GSL_SQRT_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_J0, stat_Jm1);
  }
}

int
gsl_sf_legendre_Pl_array(const int lmax, const double x, double * result_array)
{
  /* CHECK_POINTER(result_array) */

  if(lmax < 0 || x < -1.0 || x > 1.0) {
    GSL_ERROR ("domain error", GSL_EDOM);
  }
  else if(lmax == 0) {
    result_array[0] = 1.0;
    return GSL_SUCCESS;
  }
  else if(lmax == 1) {
    result_array[0] = 1.0;
    result_array[1] = x;
    return GSL_SUCCESS;
  }
  else {
    /* upward recurrence: l P_l = (2l-1) z P_{l-1} - (l-1) P_{l-2} */

    double p_ellm2 = 1.0;    /* P_0(x) */
    double p_ellm1 = x;    /* P_1(x) */
    double p_ell = p_ellm1;
    int ell;

    result_array[0] = 1.0;
    result_array[1] = x;

    for(ell=2; ell <= lmax; ell++){
      p_ell = (x*(2*ell-1)*p_ellm1 - (ell-1)*p_ellm2) / ell;
      p_ellm2 = p_ellm1;
      p_ellm1 = p_ell;
      result_array[ell] = p_ell;
    }

    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_Pl_deriv_array(const int lmax, const double x, double * result_array, double * result_deriv_array)
{
  int stat_array = gsl_sf_legendre_Pl_array(lmax, x, result_array);

  if(lmax >= 0) result_deriv_array[0] = 0.0;
  if(lmax >= 1) result_deriv_array[1] = 1.0;

  if(stat_array == GSL_SUCCESS)
  {
    int ell;

    if(fabs(x - 1.0)*(lmax+1.0)*(lmax+1.0) <  GSL_SQRT_DBL_EPSILON)
    {
      /* x is near 1 */
      for(ell = 2; ell <= lmax; ell++)
      {
        const double pre = 0.5 * ell * (ell+1.0);
        result_deriv_array[ell] = pre * (1.0 - 0.25 * (1.0-x) * (ell+2.0)*(ell-1.0));
      }
    }
    else if(fabs(x + 1.0)*(lmax+1.0)*(lmax+1.0) <  GSL_SQRT_DBL_EPSILON)
    {
      /* x is near -1 */
      for(ell = 2; ell <= lmax; ell++)
      {
        const double sgn = ( GSL_IS_ODD(ell) ? 1.0 : -1.0 ); /* derivative is odd in x for even ell */
        const double pre = sgn * 0.5 * ell * (ell+1.0);
        result_deriv_array[ell] = pre * (1.0 - 0.25 * (1.0+x) * (ell+2.0)*(ell-1.0));
      }
    }
    else
    {
      const double diff_a = 1.0 + x;
      const double diff_b = 1.0 - x;
      for(ell = 2; ell <= lmax; ell++)
      {
        result_deriv_array[ell] = - ell * (x * result_array[ell] - result_array[ell-1]) / (diff_a * diff_b);
      }
    }

    return GSL_SUCCESS;
  }
  else
  {
    return stat_array;
  }
}

int
gsl_sf_legendre_Plm_e(const int l, const int m, const double x, gsl_sf_result * result)
{
  /* If l is large and m is large, then we have to worry
   * about overflow. Calculate an approximate exponent which
   * measures the normalization of this thing.
   */
  const double dif = l-m;
  const double sum = l+m;
  const double t_d = ( dif == 0.0 ? 0.0 : 0.5 * dif * (log(dif)-1.0) );
  const double t_s = ( dif == 0.0 ? 0.0 : 0.5 * sum * (log(sum)-1.0) );
  const double exp_check = 0.5 * log(2.0*l+1.0) + t_d - t_s;

  /* CHECK_POINTER(result) */

  if(m < 0 || l < m || x < -1.0 || x > 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(exp_check < GSL_LOG_DBL_MIN + 10.0){
    /* Bail out. */
    OVERFLOW_ERROR(result);
  }
  else {
    /* Account for the error due to the
     * representation of 1-x.
     */
    const double err_amp = 1.0 / (GSL_DBL_EPSILON + fabs(1.0-fabs(x)));

    /* P_m^m(x) and P_{m+1}^m(x) */
    double p_mm   = legendre_Pmm(m, x);
    double p_mmp1 = x * (2*m + 1) * p_mm;

    if(l == m){
      result->val = p_mm;
      result->err = err_amp * 2.0 * GSL_DBL_EPSILON * fabs(p_mm);
      return GSL_SUCCESS;
    }
    else if(l == m + 1) {
      result->val = p_mmp1;
      result->err = err_amp * 2.0 * GSL_DBL_EPSILON * fabs(p_mmp1);
      return GSL_SUCCESS;
    }
    else{
      /* upward recurrence: (l-m) P(l,m) = (2l-1) z P(l-1,m) - (l+m-1) P(l-2,m)
       * start at P(m,m), P(m+1,m)
       */

      double p_ellm2 = p_mm;
      double p_ellm1 = p_mmp1;
      double p_ell = 0.0;
      int ell;

      for(ell=m+2; ell <= l; ell++){
        p_ell = (x*(2*ell-1)*p_ellm1 - (ell+m-1)*p_ellm2) / (ell-m);
        p_ellm2 = p_ellm1;
        p_ellm1 = p_ell;
      }

      result->val = p_ell;
      result->err = err_amp * (0.5*(l-m) + 1.0) * GSL_DBL_EPSILON * fabs(p_ell);

      return GSL_SUCCESS;
    }
  }
}

int
gsl_sf_legendre_Plm_array(const int lmax, const int m, const double x, double * result_array)
{
  /* If l is large and m is large, then we have to worry
   * about overflow. Calculate an approximate exponent which
   * measures the normalization of this thing.
   */
  const double dif = lmax-m;
  const double sum = lmax+m;
  const double t_d = ( dif == 0.0 ? 0.0 : 0.5 * dif * (log(dif)-1.0) );
  const double t_s = ( dif == 0.0 ? 0.0 : 0.5 * sum * (log(sum)-1.0) );
  const double exp_check = 0.5 * log(2.0*lmax+1.0) + t_d - t_s;

  /* CHECK_POINTER(result_array) */

  if(m < 0 || lmax < m || x < -1.0 || x > 1.0) {
    GSL_ERROR ("domain error", GSL_EDOM);
  }
  else if(m > 0 && (x == 1.0 || x == -1.0)) {
    int ell;
    for(ell=m; ell<=lmax; ell++) result_array[ell-m] = 0.0;
    return GSL_SUCCESS;
  }
  else if(exp_check < GSL_LOG_DBL_MIN + 10.0){
    /* Bail out. */
    GSL_ERROR ("overflow", GSL_EOVRFLW);
  }
  else {
    double p_mm   = legendre_Pmm(m, x);
    double p_mmp1 = x * (2.0*m + 1.0) * p_mm;

    if(lmax == m){
      result_array[0] = p_mm;
      return GSL_SUCCESS;
    }
    else if(lmax == m + 1) {
      result_array[0] = p_mm;
      result_array[1] = p_mmp1;
      return GSL_SUCCESS;
    }
    else {
      double p_ellm2 = p_mm;
      double p_ellm1 = p_mmp1;
      double p_ell = 0.0;
      int ell;

      result_array[0] = p_mm;
      result_array[1] = p_mmp1;

      for(ell=m+2; ell <= lmax; ell++){
        p_ell = (x*(2.0*ell-1.0)*p_ellm1 - (ell+m-1)*p_ellm2) / (ell-m);
        p_ellm2 = p_ellm1;
        p_ellm1 = p_ell;
        result_array[ell-m] = p_ell;
      }

      return GSL_SUCCESS;
    }
  }
}

int
gsl_sf_legendre_Plm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array)
{
  if(m < 0 || m > lmax)
  {
    GSL_ERROR("m < 0 or m > lmax", GSL_EDOM);
  }
  else if(m == 0)
  {
    /* It is better to do m=0 this way, so we can more easily
     * trap the divergent case which can occur when m == 1.
     */
    return gsl_sf_legendre_Pl_deriv_array(lmax, x, result_array, result_deriv_array);
  }
  else
  {
    int stat_array = gsl_sf_legendre_Plm_array(lmax, m, x, result_array);

    if(stat_array == GSL_SUCCESS)
    {
      int ell;

      if(m == 1 && (1.0 - fabs(x) < GSL_DBL_EPSILON))
      {
        /* This divergence is real and comes from the cusp-like
         * behaviour for m = 1. For example, P[1,1] = - Sqrt[1-x^2].
         */
        GSL_ERROR("divergence near |x| = 1.0 since m = 1", GSL_EOVRFLW);
      }
      else if(m == 2 && (1.0 - fabs(x) < GSL_DBL_EPSILON))
      {
        /* m = 2 gives a finite nonzero result for |x| near 1 */
        if(fabs(x - 1.0) < GSL_DBL_EPSILON)
        {
          for(ell = m; ell <= lmax; ell++) result_deriv_array[ell-m] = -0.25 * x * (ell - 1.0)*ell*(ell+1.0)*(ell+2.0);
        }
        else if(fabs(x + 1.0) < GSL_DBL_EPSILON)
        {
          for(ell = m; ell <= lmax; ell++)
          {
            const double sgn = ( GSL_IS_ODD(ell) ? 1.0 : -1.0 );
            result_deriv_array[ell-m] = -0.25 * sgn * x * (ell - 1.0)*ell*(ell+1.0)*(ell+2.0);
          }
        }
        return GSL_SUCCESS;
      }
      else
      {
        /* m > 2 is easier to deal with since the endpoints always vanish */
        if(1.0 - fabs(x) < GSL_DBL_EPSILON)
        {
          for(ell = m; ell <= lmax; ell++) result_deriv_array[ell-m] = 0.0;
          return GSL_SUCCESS;
        }
        else
        {
          const double diff_a = 1.0 + x;
          const double diff_b = 1.0 - x;
          result_deriv_array[0] = - m * x / (diff_a * diff_b) * result_array[0];
          if(lmax-m >= 1) result_deriv_array[1] = (2.0 * m + 1.0) * (x * result_deriv_array[0] + result_array[0]);
          for(ell = m+2; ell <= lmax; ell++)
          {
            result_deriv_array[ell-m] = - (ell * x * result_array[ell-m] - (ell+m) * result_array[ell-1-m]) / (diff_a * diff_b);
          }
          return GSL_SUCCESS;
        }
      }
    }
    else
    {
      return stat_array;
    }
  }
}

int
gsl_sf_legendre_sphPlm_e(const int l, int m, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(m < 0 || l < m || x < -1.0 || x > 1.0) {
    DOMAIN_ERROR(result);
  }
  else if(m == 0) {
    gsl_sf_result P;
    int stat_P = gsl_sf_legendre_Pl_e(l, x, &P);
    double pre = sqrt((2.0*l + 1.0)/(4.0*M_PI));
    result->val  = pre * P.val;
    result->err  = pre * P.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_P;
  }
  else if(x == 1.0 || x == -1.0) {
    /* m > 0 here */
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* m > 0 and |x| < 1 here */

    /* Starting value for recursion.
     * Y_m^m(x) = sqrt( (2m+1)/(4pi m) gamma(m+1/2)/gamma(m) ) (-1)^m (1-x^2)^(m/2) / pi^(1/4)
     */
    gsl_sf_result lncirc;
    gsl_sf_result lnpoch;
    double lnpre_val;
    double lnpre_err;
    gsl_sf_result ex_pre;
    double sr;
    const double sgn = ( GSL_IS_ODD(m) ? -1.0 : 1.0);
    const double y_mmp1_factor = x * sqrt(2.0*m + 3.0);
    double y_mm, y_mm_err;
    double y_mmp1, y_mmp1_err;
    gsl_sf_log_1plusx_e(-x*x, &lncirc);
    gsl_sf_lnpoch_e(m, 0.5, &lnpoch);  /* Gamma(m+1/2)/Gamma(m) */
    lnpre_val = -0.25*M_LNPI + 0.5 * (lnpoch.val + m*lncirc.val);
    lnpre_err = 0.25*M_LNPI*GSL_DBL_EPSILON + 0.5 * (lnpoch.err + fabs(m)*lncirc.err);
    /* Compute exp(ln_pre) with error term, avoiding call to gsl_sf_exp_err BJG */
    ex_pre.val = exp(lnpre_val);
    ex_pre.err = 2.0*(sinh(lnpre_err) + GSL_DBL_EPSILON)*ex_pre.val;
    sr     = sqrt((2.0+1.0/m)/(4.0*M_PI));
    y_mm   = sgn * sr * ex_pre.val;
    y_mm_err  = 2.0 * GSL_DBL_EPSILON * fabs(y_mm) + sr * ex_pre.err;
    y_mm_err *= 1.0 + 1.0/(GSL_DBL_EPSILON + fabs(1.0-x));
    y_mmp1 = y_mmp1_factor * y_mm;
    y_mmp1_err=fabs(y_mmp1_factor) * y_mm_err;

    if(l == m){
      result->val  = y_mm;
      result->err  = y_mm_err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(y_mm);
      return GSL_SUCCESS;
    }
    else if(l == m + 1) {
      result->val  = y_mmp1;
      result->err  = y_mmp1_err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(y_mmp1);
      return GSL_SUCCESS;
    }
    else{
      double y_ell = 0.0;
      double y_ell_err;
      int ell;

      /* Compute Y_l^m, l > m+1, upward recursion on l. */
      for(ell=m+2; ell <= l; ell++){
        const double rat1 = (double)(ell-m)/(double)(ell+m);
        const double rat2 = (ell-m-1.0)/(ell+m-1.0);
        const double factor1 = sqrt(rat1*(2.0*ell+1.0)*(2.0*ell-1.0));
        const double factor2 = sqrt(rat1*rat2*(2.0*ell+1.0)/(2.0*ell-3.0));
        y_ell = (x*y_mmp1*factor1 - (ell+m-1.0)*y_mm*factor2) / (ell-m);
        y_mm   = y_mmp1;
        y_mmp1 = y_ell;

        y_ell_err = 0.5*(fabs(x*factor1)*y_mmp1_err + fabs((ell+m-1.0)*factor2)*y_mm_err) / fabs(ell-m);
        y_mm_err = y_mmp1_err;
        y_mmp1_err = y_ell_err;
      }

      result->val  = y_ell;
      result->err  = y_ell_err + (0.5*(l-m) + 1.0) * GSL_DBL_EPSILON * fabs(y_ell);

      return GSL_SUCCESS;
    }
  }
}

int
gsl_sf_legendre_sphPlm_array(const int lmax, int m, const double x, double * result_array)
{
  /* CHECK_POINTER(result_array) */

  if(m < 0 || lmax < m || x < -1.0 || x > 1.0) {
    GSL_ERROR ("error", GSL_EDOM);
  }
  else if(m > 0 && (x == 1.0 || x == -1.0)) {
    int ell;
    for(ell=m; ell<=lmax; ell++) result_array[ell-m] = 0.0;
    return GSL_SUCCESS;
  }
  else {
    double y_mm;
    double y_mmp1;

    if(m == 0) {
      y_mm   = 0.5/M_SQRTPI;          /* Y00 = 1/sqrt(4pi) */
      y_mmp1 = x * M_SQRT3 * y_mm;
    }
    else {
      /* |x| < 1 here */

      gsl_sf_result lncirc;
      gsl_sf_result lnpoch;
      double lnpre;
      const double sgn = ( GSL_IS_ODD(m) ? -1.0 : 1.0);
      gsl_sf_log_1plusx_e(-x*x, &lncirc);
      gsl_sf_lnpoch_e(m, 0.5, &lnpoch);  /* Gamma(m+1/2)/Gamma(m) */
      lnpre = -0.25*M_LNPI + 0.5 * (lnpoch.val + m*lncirc.val);
      y_mm   = sqrt((2.0+1.0/m)/(4.0*M_PI)) * sgn * exp(lnpre);
      y_mmp1 = x * sqrt(2.0*m + 3.0) * y_mm;
    }

    if(lmax == m){
      result_array[0] = y_mm;
      return GSL_SUCCESS;
    }
    else if(lmax == m + 1) {
      result_array[0] = y_mm;
      result_array[1] = y_mmp1;
      return GSL_SUCCESS;
    }
    else{
      double y_ell;
      int ell;

      result_array[0] = y_mm;
      result_array[1] = y_mmp1;

      /* Compute Y_l^m, l > m+1, upward recursion on l. */
      for(ell=m+2; ell <= lmax; ell++){
        const double rat1 = (double)(ell-m)/(double)(ell+m);
        const double rat2 = (ell-m-1.0)/(ell+m-1.0);
        const double factor1 = sqrt(rat1*(2*ell+1)*(2*ell-1));
        const double factor2 = sqrt(rat1*rat2*(2*ell+1)/(2*ell-3));
        y_ell = (x*y_mmp1*factor1 - (ell+m-1)*y_mm*factor2) / (ell-m);
        y_mm   = y_mmp1;
        y_mmp1 = y_ell;
        result_array[ell-m] = y_ell;
      }
    }

    return GSL_SUCCESS;
  }
}

int
gsl_sf_legendre_sphPlm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array)
{
  if(m < 0 || lmax < m || x < -1.0 || x > 1.0)
  {
    GSL_ERROR ("domain", GSL_EDOM);
  }
  else if(m == 0)
  {
    /* m = 0 is easy to trap */
    const int stat_array = gsl_sf_legendre_Pl_deriv_array(lmax, x, result_array, result_deriv_array);
    int ell;
    for(ell = 0; ell <= lmax; ell++)
    {
      const double prefactor = sqrt((2.0 * ell + 1.0)/(4.0*M_PI));
      result_array[ell] *= prefactor;
      result_deriv_array[ell] *= prefactor;
    }
    return stat_array;
  }
  else if(m == 1)
  {
    /* Trapping m = 1 is necessary because of the possible divergence.
     * Recall that this divergence is handled properly in ..._Plm_deriv_array(),
     * and the scaling factor is not large for small m, so we just scale.
     */
    const int stat_array = gsl_sf_legendre_Plm_deriv_array(lmax, m, x, result_array, result_deriv_array);
    int ell;
    for(ell = 1; ell <= lmax; ell++)
    {
      const double prefactor = sqrt((2.0 * ell + 1.0)/(ell + 1.0) / (4.0*M_PI*ell));
      result_array[ell-1] *= prefactor;
      result_deriv_array[ell-1] *= prefactor;
    }
    return stat_array;
  }
  else
  {
    /* as for the derivative of P_lm, everything is regular for m >= 2 */

    int stat_array = gsl_sf_legendre_sphPlm_array(lmax, m, x, result_array);

    if(stat_array == GSL_SUCCESS)
    {
      int ell;

      if(1.0 - fabs(x) < GSL_DBL_EPSILON)
      {
        for(ell = m; ell <= lmax; ell++) result_deriv_array[ell-m] = 0.0;
        return GSL_SUCCESS;
      }
      else
      {
        const double diff_a = 1.0 + x;
        const double diff_b = 1.0 - x;
        result_deriv_array[0] = - m * x / (diff_a * diff_b) * result_array[0];
        if(lmax-m >= 1) result_deriv_array[1] = sqrt(2.0 * m + 3.0) * (x * result_deriv_array[0] + result_array[0]);
        for(ell = m+2; ell <= lmax; ell++)
        {
          const double c1 = sqrt(((2.0*ell+1.0)/(2.0*ell-1.0)) * ((double)(ell-m)/(double)(ell+m)));
          result_deriv_array[ell-m] = - (ell * x * result_array[ell-m] - c1 * (ell+m) * result_array[ell-1-m]) / (diff_a * diff_b);
        }
        return GSL_SUCCESS;
      }
    }
    else
    {
      return stat_array;
    }
  }
}

int
gsl_sf_legendre_array_size(const int lmax, const int m)
{
  return lmax-m+1;
}

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

#include "eval.h"

double gsl_sf_legendre_P1(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_P1_e(x, &result));
}

double gsl_sf_legendre_P2(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_P2_e(x, &result));
}

double gsl_sf_legendre_P3(const double x)
{
  EVAL_RESULT(gsl_sf_legendre_P3_e(x, &result));
}

double gsl_sf_legendre_Pl(const int l, const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Pl_e(l, x, &result));
}

double gsl_sf_legendre_Plm(const int l, const int m, const double x)
{
  EVAL_RESULT(gsl_sf_legendre_Plm_e(l, m, x, &result));
}

double gsl_sf_legendre_sphPlm(const int l, const int m, const double x)
{
  EVAL_RESULT(gsl_sf_legendre_sphPlm_e(l, m, x, &result));
}


/*
** gsl routines for double data type
**
** G.Lohmann, July 2004
*/

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

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>

#define dvset gsl_vector_set
#define dvget gsl_vector_get
#define dmset gsl_matrix_set
#define dmget gsl_matrix_get

#define fvset gsl_vector_float_set
#define fvget gsl_vector_float_get
#define fmset gsl_matrix_float_set
#define fmget gsl_matrix_float_get

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

/*
**  y = Ax
*/
gsl_vector *
dmat_x_vector(gsl_matrix *A, gsl_vector *x, gsl_vector *y)
{
  int i,j,nrows,ncols;
  double *ptr1,*ptr2,*ptr3,sum;

  nrows = A->size1;
  ncols = A->size2;

  if (y == NULL) {
     y = gsl_vector_alloc (nrows);
  }

  if (x->size != ncols || y->size != nrows) {
    fprintf(stderr," dmat_x_vector: incongruent dimensions\n");
    exit(0);
  }

  ptr1 = A->data;
  ptr3 = y->data;

  for (i=0; i<nrows; i++) {
    sum = 0;
    ptr2 = x->data;
    for (j=0; j<ncols; j++) {
      sum += (*ptr1++) * (*ptr2++);
    }
    *ptr3++ = sum;
  }
  return y;
}

/*
**  y = x^T A
*/
gsl_vector *
dvector_x_mat(gsl_vector *x, gsl_matrix *A, gsl_vector *y)
{
  int i,j,nrows,ncols;
  double *ptr1,*ptr2,*ptr3,sum;

  nrows = A->size1;
  ncols = A->size2;

  if (y == NULL) {
     y = gsl_vector_alloc (nrows);
  }

  if (x->size != ncols || y->size != nrows) {
    fprintf(stderr," dmat_x_vector: incongruent dimensions\n");
    exit(0);
  }

  ptr1 = A->data;
  ptr3 = y->data;

  for (i=0; i<nrows; i++) {
    sum = 0;
    ptr2 = x->data;
    for (j=0; j<ncols; j++) {
      sum += (*ptr1++) * (*ptr2++);
    }
    *ptr3++ = sum;
  }
  return y;
}

/*
**  z = x^T y
*/
double
dskalarproduct(gsl_vector *x,gsl_vector *y)
{
  int i,n;
  double *ptr1,*ptr2,sum;

  n = x->size;
  if (y->size != n) {
    fprintf(stderr," dskalarproduct: incongruent vector sizes: %d %d",n,y->size);
    exit(0);
  }

  ptr1 = x->data;
  ptr2 = y->data;
  sum = 0;
  for (i=0; i<n; i++) {
    sum += (*ptr1) * (*ptr2);
    ptr1++;
    ptr2++;
  }
  return sum;
}

/*
**    B = A^-1 = A^+
*/
gsl_matrix *
dmat_PseudoInv(gsl_matrix *A,gsl_matrix *B)
{
  int j,k,l,n,m;
  double u,x;
  static gsl_matrix *U=NULL,*V=NULL;
  static gsl_vector *w=NULL;

  m = A->size1;
  n = A->size2;

  if (B == NULL) {
    B = gsl_matrix_alloc (n,m);
  }
  else if (B->size1 != n || B->size2 != m) {
    gsl_matrix_free(B);
    B = gsl_matrix_alloc (n, m);
  }

  if (U == NULL) {
    U = gsl_matrix_alloc (m, n);
    V = gsl_matrix_alloc (n, n);
    w = gsl_vector_alloc (n);
  }
  else if (U->size1 != m || w->size != n) {
    gsl_matrix_free(U);
    gsl_matrix_free(V);
    gsl_vector_free(w);
    U = gsl_matrix_alloc (m, n);
    V = gsl_matrix_alloc (n, n);
    w = gsl_vector_alloc (n);
  }

  /* singular value decomposition */
  gsl_matrix_memcpy(U,A);
  gsl_linalg_SV_decomp_jacobi(U,V,w);

  gsl_matrix_set_zero(B);

  for (k=0; k<n; k++) {
    for (l=0; l<m; l++) {
      for (j=0; j<n; j++) {
	u = dvget(w,j);
        if (ABS(u) > 1.0e-6) {
          x = dmget(B,k,l);
	  x += dmget(V,k,j)*dmget(U,l,j)/u;
          dmset(B,k,l,x);
        }
      }
    }
  }
  return B;
}

/*
**    C = A x B^T
*/
gsl_matrix *
dmat_x_matT(gsl_matrix *A,gsl_matrix *B,gsl_matrix *C)
{
  int i,j,k;
  int n,r,m;
  double *ptr1,*ptr2,*ptr3,sum;

  n = A->size1;
  r = A->size2;
  m = B->size1;

  if (B->size2 != r) {
    fprintf(stderr,"fmat_x_matT: incongruent matrix dimensions (A,B).\n");
    exit(0);
  }

  if (C == NULL) {
    C = gsl_matrix_alloc (n,m);
  }
  else {
    if (C->size1 != n || C->size2 != m) {
      fprintf(stderr,"fmat_x_matT: incongruent matrix dimensions(C).\n");
      exit(0);
    }
  }
  ptr1 = C->data;
  for (i=0; i<n; i++) {
    for (j=0; j<m; j++) {

      ptr2 = gsl_matrix_ptr (A,i,0);
      ptr3 = gsl_matrix_ptr (B,j,0);

      sum = 0;
      for (k=0; k<r; k++) {
	sum  += (*ptr2) * (*ptr3);
	ptr2++;
	ptr3++;
      }
      *ptr1++ = sum;
    }
  }
  return C;
}

/*
**    C = A^T x B
*/
gsl_matrix *
dmatT_x_mat(gsl_matrix *A,gsl_matrix *B,gsl_matrix *C)
{
  int i,j,k;
  int n,m,r;
  double *ptr1,*ptr2,*ptr3,sum;

  n = A->size1;
  r = A->size2;
  m = B->size2;

  if (B->size1 != n) {
    fprintf(stderr,"dmatT_x_mat: incongruent matrix dimensions (A,B).\n");
    exit(0);
  }

  if (C == NULL) {
    C = gsl_matrix_alloc (r,m);
  }
  else {
    if (C->size1 != r || C->size2 != m) {
      fprintf(stderr,"dmatT_x_mat: incongruent matrix dimensions (C, %d %d, %d %d).\n",
	      C->size1,C->size2,r,m);
      exit(0);
    }
  }

  ptr1 = C->data;
  for (i=0; i<r; i++) {
    for (j=0; j<m; j++) {

      ptr2 = gsl_matrix_ptr (A,0,i);
      ptr3 = gsl_matrix_ptr (B,0,j);

      sum = 0;
      for (k=0; k<n; k++) {
	sum  += (*ptr2) * (*ptr3);
	ptr2 += A->tda;
	ptr3 += B->tda;
      }
      *ptr1++ = sum;
    }
  }

  return C;
}

/*
**    C = A x B
*/
gsl_matrix *
dmat_x_mat(gsl_matrix *A,gsl_matrix *B,gsl_matrix *C)
{
  int i,j,k,m,n,r;
  int nrowsA,ncolsA;
  int nrowsB,ncolsB;
  int nrowsC,ncolsC;
  double *ptr1,*ptr2,*ptr3,sum;

  nrowsA = A->size1;
  ncolsA = A->size2;

  nrowsB = B->size1;
  ncolsB = B->size2;

  if (C == NULL)
    C = gsl_matrix_alloc (nrowsA,ncolsB);

  nrowsC = C->size1;
  ncolsC = C->size2;

  if (ncolsA != nrowsB || nrowsA != nrowsC || ncolsB != ncolsC) {
    fprintf(stderr,"dmat_x_mat: incongruent matrix dimensions.\n");
    exit(0);
  }

  m = A->size1;
  n = A->size2;
  r = B->size2;

  ptr1 = C->data;

  for (i=0; i<m; i++) {
    for (j=0; j<r; j++) {

      ptr2 = gsl_matrix_ptr (A,i,0);
      ptr3 = gsl_matrix_ptr (B,0,j);

      sum = 0;
      for (k=0; k<n; k++) {
	sum += (*ptr2++) * (*ptr3);
	ptr3 += B->tda;
      }
      *ptr1++ = sum;
    }
  }
  return C;
}

gsl_matrix *
dtranspose(gsl_matrix *A,gsl_matrix *B)
{
  int i,j,n,m;

  n = A->size1;
  m = A->size2;

  if (B == NULL) {
    B = gsl_matrix_alloc(m,n);
  }
  else if (B->size1 != m || B->size2 != n) {
    gsl_matrix_free(B);
    B = gsl_matrix_alloc(m,n);
  }

  for (i=0; i<n; i++) {
    for (j=0; j<m; j++) {
      dmset(B,j,i,dmget(A,i,j));
    }
  }
  return B;
}

/*

int main (void)
{
  int i,j;
  int m,n,r;
  gsl_matrix *a=NULL,*b=NULL,*c=NULL;
  double x;

  n = 648;
  a = gsl_matrix_alloc(n,n);

  for (i=0; i<n; i++) {
    for (j=0; j<n; j++) {
      x = i+j+1;
      dmset(a,i,j,x);
    }
  }

  b = gsl_matrix_alloc(n,n);
  for (i=0; i<n; i++) {
    for (j=0; j<n; j++) {
      x = i-j+1;
      dmset(b,i,j,x);
    }
  }

  c = dmat_x_mat(a,b,NULL);
  exit(0);
}
*/


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

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

#include "ccl.h"

// Global variable to hold the neutrino phase-space spline
gsl_spline* nu_spline = NULL;

// these are NOT adjustable
// this phase space integral is only done once and the following is accurate
// enough according to tests done by the devs
/**
 * Absolute precision in neutrino root finding
 */
#define GSL_EPSABS_NU 1E-7

/**
 * Relative precision in neutrino root finding
 */
#define GSL_EPSREL_NU 1E-7

/**
 * Number of iterations for neutrino root finding
 */
#define GSL_N_ITERATION_NU 1000

/* ------- ROUTINE: nu_integrand ------
INPUTS: x: dimensionless momentum, *r: pointer to a dimensionless mass / temperature
TASK: Integrand of phase-space massive neutrino integral
*/
static double nu_integrand(double x, void *r) {
  double rat = *((double*)(r));
  double x2 = x*x;
  return sqrt(x2 + rat*rat) / (exp(x)+1.0) * x2;
}

/* ------- ROUTINE: ccl_calculate_nu_phasespace_spline ------
TASK: Get the spline of the result of the phase-space integral required for massive neutrinos.
*/

gsl_spline* calculate_nu_phasespace_spline(int *status) {
  int N = CCL_NU_MNUT_N;
  double *mnut = NULL;
  double *y = NULL;
  gsl_spline* spl = NULL;
  gsl_integration_cquad_workspace * workspace = NULL;
  int stat = 0, gslstatus;
  gsl_function F;

  mnut = ccl_linear_spacing(log(CCL_NU_MNUT_MIN), log(CCL_NU_MNUT_MAX), N);
  y = malloc(sizeof(double)*CCL_NU_MNUT_N);
  if ((y == NULL) || (mnut == NULL)) {
    // Not setting a status_message here because we can't easily pass a
    // cosmology to this function - message printed in ccl_error.c.
    *status = CCL_ERROR_NU_INT;
  }

  if (*status == 0) {
    workspace = gsl_integration_cquad_workspace_alloc(GSL_N_ITERATION_NU);
    if (workspace == NULL)
      *status = CCL_ERROR_NU_INT;
  }

  if (*status == 0) {
    F.function = &nu_integrand;
    for (int i=0; i < CCL_NU_MNUT_N; i++) {
      double mnut_ = exp(mnut[i]);
      F.params = &(mnut_);
      gslstatus = gsl_integration_cquad(&F, 0, 1000.0,
                                        GSL_EPSABS_NU,
                                        GSL_EPSREL_NU,
                                        workspace, &y[i], NULL, NULL);
      if (gslstatus != GSL_SUCCESS) {
        ccl_raise_gsl_warning(gslstatus, "ccl_neutrinos.c: calculate_nu_phasespace_spline():");
        stat |= gslstatus;
      }
    }

    double renorm = 1./y[0];
    for (int i=0; i < CCL_NU_MNUT_N; i++)
      y[i] *= renorm;

    if (stat) {
      *status = CCL_ERROR_NU_INT;
    }
  }

  if (*status == 0) {
    spl = gsl_spline_alloc(gsl_interp_akima, CCL_NU_MNUT_N);
    if (spl == NULL)
      *status = CCL_ERROR_NU_INT;
  }

  if (*status == 0) {
    stat |= gsl_spline_init(spl, mnut, y, CCL_NU_MNUT_N);
    if (stat) {
      ccl_raise_gsl_warning(gslstatus, "ccl_neutrinos.c: calculate_nu_phasespace_spline():");
      *status = CCL_ERROR_NU_INT;
    }
  }

  // Check for errors in creating the spline
  if (stat || (*status)) {
    // Not setting a status_message here because we can't easily pass a
    // cosmology to this function - message printed in ccl_error.c.
    *status = CCL_ERROR_NU_INT;
    gsl_spline_free(spl);
  }

  gsl_integration_cquad_workspace_free(workspace);
  free(mnut);
  free(y);

  return spl;
}

/* ------- ROUTINE: ccl_nu_phasespace_intg ------
INPUTS: mnuOT: the dimensionless mass / temperature of a single massive neutrino
TASK: Get the value of the phase space integral at mnuOT
*/
double nu_phasespace_intg(double mnuOT, int* status)
{
  // Check if the global variable for the phasespace spline has been defined yet:
  if (nu_spline == NULL)
    nu_spline = calculate_nu_phasespace_spline(status);

  if (*status) {
    return NAN;
  }

  double integral_value = 0.;

  // First check the cases where we are in the limits.
  if (mnuOT < CCL_NU_MNUT_MIN)
    return 7./8.;
  else if (mnuOT > CCL_NU_MNUT_MAX)
    return 0.2776566337 * mnuOT;

  int gslstatus = gsl_spline_eval_e(nu_spline, log(mnuOT), NULL, &integral_value);
  if (gslstatus != GSL_SUCCESS) {
    ccl_raise_gsl_warning(gslstatus, "ccl_neutrinos.c: nu_phasespace_intg():");
    *status |= gslstatus;
  }
  return integral_value * 7./8.;
}

/* -------- ROUTINE: Omeganuh2 ---------
INPUTS: a: scale factor, Nnumass: number of massive neutrino species,
        mnu: total mass in eV of neutrinos, T_CMB: CMB temperature, status: pointer to status integer.
TASK: Compute Omeganu * h^2 as a function of time.
!! To all practical purposes, Neff is simply N_nu_mass !!
*/
double ccl_Omeganuh2 (double a, int N_nu_mass, double* mnu, double T_CMB, int* status) {
  double Tnu, a4, prefix_massless, mnuone, OmNuh2;
  double Tnu_eff, mnuOT, intval, prefix_massive;
  double total_mass; // To check if this is the massless or massive case.

  // First check if N_nu_mass is 0
  if (N_nu_mass == 0) return 0.0;

  Tnu = T_CMB*pow(4./11.,1./3.);
  a4 = a*a*a*a;
  // Check if mnu=0. We assume that in the massless case mnu is a
  // pointer to a single element and that element is 0. This should in principle never be called.
  if (mnu[0] < 0.00017) {  // Limit taken from Lesgourges et al. 2012
    prefix_massless = NU_CONST  * Tnu * Tnu * Tnu * Tnu;
    return N_nu_mass*prefix_massless*7./8./a4;
  }

  // And the remaining massive case.
  // Tnu_eff is used in the massive case because CLASS uses an effective
  // temperature of nonLCDM components to match to mnu / Omeganu =93.14eV. Tnu_eff = T_ncdm * T_CMB = 0.71611 * T_CMB
  Tnu_eff = Tnu * ccl_constants.TNCDM / (pow(4./11.,1./3.));

  // Define the prefix using the effective temperature (to get mnu / Omega = 93.14 eV) for the massive case:
  prefix_massive = NU_CONST * Tnu_eff * Tnu_eff * Tnu_eff * Tnu_eff;

  OmNuh2 = 0.; // Initialize to 0 - we add to this for each massive neutrino species.
  for(int i=0; i < N_nu_mass; i++) {
    // Get mass over T (mass (eV) / ((kb eV/s/K) Tnu_eff (K))
    // This returns the density normalized so that we get nuh2 at a=0
    mnuOT = mnu[i] / (Tnu_eff/a) * (ccl_constants.EV_IN_J / (ccl_constants.KBOLTZ));

    // Get the value of the phase-space integral
    intval = nu_phasespace_intg(mnuOT, status);
    OmNuh2 = intval*prefix_massive/a4 + OmNuh2;
  }

  return OmNuh2;
}

/* -------- ROUTINE: Omeganuh2_to_Mnu ---------
INPUTS: OmNuh2: neutrino mass density today Omeganu * h^2,
        label: how you want to split up the masses, see ccl_neutrinos.h for options,
        T_CMB: CMB temperature, status: pointer to status integer.
TASK: Given Omeganuh2 today, the method of splitting into masses, and the temperature of the
      CMB, output a pointer to the array of neutrino masses (may be length 1 if label asks for sum)
*/
double* ccl_nu_masses(double OmNuh2, ccl_neutrino_mass_splits mass_split,
                      double T_CMB,  int* status) {
  double sumnu;
  double *mnu = NULL;

  sumnu = 93.14 * OmNuh2;

  // Now split the sum up into three masses depending on the label given:
  if (mass_split == ccl_nu_normal) {
    mnu = malloc(3*sizeof(double));
    if (mnu == NULL) {
      *status = CCL_ERROR_MEMORY;
    }

    if (*status == 0) {
      // See CCL note for how we get these expressions for the neutrino masses in
      // normal and inverted hierarchy.
      mnu[0] = (
        2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. * ccl_constants.DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)
        - 0.25 * ccl_constants.DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. *
        ccl_constants.DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)));
      mnu[1] = (
        2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. * ccl_constants.DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)
        + 0.25 * ccl_constants.DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. *
        ccl_constants.DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5)));
      mnu[2] = (
        -1./3. * sumnu + 1./3 * pow(-6. * ccl_constants.DELTAM12_sq +
          12. * ccl_constants.DELTAM13_sq_pos + 4. * sumnu*sumnu, 0.5));

      if (mnu[0] < 0 || mnu[1] < 0 || mnu[2] < 0) {
        // The user has provided a sum that is below the physical limit.
        if (sumnu < 1e-14) {
          mnu[0] = 0.;
          mnu[1] = 0.;
          mnu[2] = 0.;
        }
        else {
          *status = CCL_ERROR_MNU_UNPHYSICAL;
          ccl_raise_warning(
              *status,
              "CCL_ERROR_MNU_UNPHYSICAL: Sum of neutrinos masses for this Omeganu "
              "value is incompatible with the requested mass hierarchy.");
        }
      }
    }
  }
  else if (mass_split == ccl_nu_inverted) {
    mnu = malloc(3*sizeof(double));
    if (mnu == NULL) {
      *status = CCL_ERROR_MEMORY;
    }

    if (*status == 0) {
      mnu[0] = (
        2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. * ccl_constants.DELTAM13_sq_neg + 4. * sumnu * sumnu, 0.5)
        - 0.25 * ccl_constants.DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. *
        ccl_constants.DELTAM13_sq_neg + 4. * sumnu * sumnu, 0.5)));
      mnu[1] = (
        2./3.* sumnu - 1./6. * pow(-6. * ccl_constants.DELTAM12_sq + 12. * ccl_constants.DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5)
        + 0.25 * ccl_constants.DELTAM12_sq / (2./3.* sumnu - 1./6. * pow(-6. *
        ccl_constants.DELTAM12_sq + 12. * ccl_constants.DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5)));
      mnu[2] = (
        -1./3. * sumnu + 1./3 * pow(-6. * ccl_constants.DELTAM12_sq +
          12. * ccl_constants.DELTAM13_sq_neg + 4. * sumnu*sumnu, 0.5));

      if (mnu[0] < 0 || mnu[1] < 0 || mnu[2] < 0) {
        // The user has provided a sum that is below the physical limit.
        if (sumnu < 1e-14) {
          mnu[0] = 0.;
          mnu[1] = 0.;
          mnu[2] = 0.;;
        }
        else {
          *status = CCL_ERROR_MNU_UNPHYSICAL;
          ccl_raise_warning(
              *status,
              "CCL_ERROR_MNU_UNPHYSICAL: Sum of neutrinos masses for this Omeganu "
              "value is incompatible with the requested mass hierarchy.");
        }
      }
    }
  }
  else if (mass_split == ccl_nu_equal) {
    mnu = malloc(3*sizeof(double));
    if (mnu == NULL) {
      *status = CCL_ERROR_MEMORY;
    }

    if (*status == 0) {
      mnu[0] = sumnu/3.;
      mnu[1] = sumnu/3.;
      mnu[2] = sumnu/3.;
    }
  }
  else if (mass_split == ccl_nu_sum) {
    mnu = malloc(sizeof(double));
    if (mnu == NULL) {
      *status = CCL_ERROR_MEMORY;
    }

    if (*status == 0) {
      mnu[0] = sumnu;
    }
  }
  else {
    *status = CCL_ERROR_MNU_UNPHYSICAL;
    ccl_raise_warning(
        *status,
        "mass option = %d not yet supported!", mass_split);
  }

  return mnu;
}

#undef GSL_EPSABS_NU
#undef GSL_EPSREL_NU
#undef GSL_N_ITERATION_NU


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <cblas.h>
#endif

/*!
 * @brief Computes the omega angles between two moment tensors.
 *
 * @param[in] nmt1    Number of moment tensors in M1.  This can be 1 otherwise
 *                    it must be equal to nmt2.  If it is 1 then the angle will
 *                    be between M1 and all the moment tensors in M2.
 * @param[in] M1      First array of moment tensors.  M1 is packed
 *                    \f$ M = \{M_{11} M_{22} M_{33} M_{12} M_{13} M_{23}\} \f$.
 *                    This is an array of dimension [6 x nmt1] with leading
 *                    dimension 6.
 * @param[in] nmt2    Number of moment tensors in M2.  This can be 1 otherwise
 *                    it must be equal to nmt1.  If it is 1 then the angle will
 *                    be between M2 and all the moment tensors in M1.
 * @param[in] M2      Second array of moment tensors.  M2 is packed
 *                    \f$ M = \{M_{11} M_{22} M_{33} M_{12} M_{13} M_{23}\} \f$.
 *                    This is an array of dimension [6 x nmt2] with leading
 *                    dimension 6.
 *
 * @param[out] omega  This is a measure between two moment tensors and combines
 *                    differences in eigenvalues (source types) and orientation
 *                    but not magnitude.
 *                    This is an array of dimension [MAX(nmt1, nmt2)].
 *
 * @author Carl Tape and translated to C by Ben Baker
 *
 * @copyright MIT
 *
 */
int compearth_CMT2omega(const int nmt1, const double *__restrict__ M1,
                        const int nmt2, const double *__restrict__ M2,
                        double *__restrict__ omega)
{
    double Mwork[6*CE_CHUNKSIZE] __attribute__((aligned(64)));
    int ierr, imt, nmtLoc;
    const double pi180i = 180.0/M_PI;
    ierr = 0;
    if (nmt1 < 1 || M1 == NULL || nmt2 < 1 || M2 == NULL || omega == NULL)
    {
        if (nmt1 < 1)
        {
            fprintf(stderr, "%s: Error nmt1 must be positive\n", __func__);
        }
        if (nmt2 < 1)
        {
            fprintf(stderr, "%s: Error nmt2 must be positive\n", __func__);
        }
        if (M1 == NULL){fprintf(stderr, "%s: Error M1 is NULL\n", __func__);}
        if (M2 == NULL){fprintf(stderr, "%s: Error M2 is NULL\n", __func__);}
        if (omega == NULL)
        {
            fprintf(stderr, "%s: Error omega is NULL\n", __func__);
        }
        return -1;
    }
    // Straight compuation
    if (nmt1 == nmt2)
    {
        ierr = compearth_angleMT(nmt1, M1, M2, omega);
        if (ierr != 0)
        {
            fprintf(stderr, "%s: Error computing angleMT 1\n", __func__);
        }
        // Convert to degrees
        cblas_dscal(nmt1, pi180i, omega, 1);
        return ierr;
    }
    else
    {
        if (nmt1 == 1 || nmt2 == 1)
        {
            // Copy nmt1 and repeat computation with it
            if (nmt1 == 1)
            {
                for (imt=0; imt<CE_CHUNKSIZE; imt++)
                {
                    cblas_dcopy(6, &M1[0], 1, &Mwork[6*imt], 1);
                }
                for (imt=0; imt<nmt2; imt=imt+CE_CHUNKSIZE)
                {
                    nmtLoc = MIN(CE_CHUNKSIZE, nmt2 - imt);
                    ierr = compearth_angleMT(nmtLoc, Mwork, &M2[6*imt],
                                             &omega[imt]);
                    if (ierr != 0)
                    {
                        fprintf(stderr, "%s: Error calling angleMT at %d\n",
                                __func__, __LINE__);
                        return ierr;
                    }
                    // Convert to degrees
                    cblas_dscal(nmtLoc, pi180i, &omega[imt], 1);
                }
            }
            // Copy nmt2 and repeat computation with it
            else
            {
                for (imt=0; imt<CE_CHUNKSIZE; imt++)
                {
                    cblas_dcopy(6, &M2[0], 1, &Mwork[6*imt], 1);
                }
                for (imt=0; imt<nmt1; imt=imt+CE_CHUNKSIZE)
                {
                    nmtLoc = MIN(CE_CHUNKSIZE, nmt1 - imt);
                    ierr = compearth_angleMT(nmtLoc, &M1[6*imt], Mwork,
                                             &omega[imt]);
                    if (ierr != 0)
                    {
                        fprintf(stderr, "%s: Error calling angleMT at %d\n",
                                __func__, __LINE__);
                        return ierr;
                    }
                    // Convert to degrees
                    cblas_dscal(nmtLoc, pi180i, &omega[imt], 1);
                }
            }
        }
        else
        {
            fprintf(stderr, "%s: nmt1=%d /= nmt2=%d; this is invalid\n",
                    __func__, nmt1, nmt2);
            return -1;
        }
    }
    return ierr;
}