/* specfunc/legendre_con.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 * Copyright (C) 2010 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.
 */

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_ellint.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_sf_legendre.h>

#include "error.h"
#include "legendre.h"

#define Root_2OverPi_  0.797884560802865355879892
#define locEPS         (1000.0*GSL_DBL_EPSILON)

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

#define RECURSE_LARGE  (1.0e-5*GSL_DBL_MAX)
#define RECURSE_SMALL  (1.0e+5*GSL_DBL_MIN)

/* Continued fraction for f_{ell+1}/f_ell
 * f_ell := P^{-mu-ell}_{-1/2 + I tau}(x),  x < 1.0
 *
 * Uses standard CF method from Temme's book.
 */
static
int
conicalP_negmu_xlt1_CF1(const double mu, const int ell, const double tau,
                        const double x, gsl_sf_result * result)
{
  const double RECUR_BIG = GSL_SQRT_DBL_MAX;
  const int maxiter = 5000;
  int n = 1;
  double xi = x/(sqrt(1.0-x)*sqrt(1.0+x));
  double Anm2 = 1.0;
  double Bnm2 = 0.0;
  double Anm1 = 0.0;
  double Bnm1 = 1.0;
  double a1 = 1.0;
  double b1 = 2.0*(mu + ell + 1.0) * xi;
  double An = b1*Anm1 + a1*Anm2;
  double Bn = b1*Bnm1 + a1*Bnm2;
  double an, bn;
  double fn = An/Bn;

  while(n < maxiter) {
    double old_fn;
    double del;
    n++;
    Anm2 = Anm1;
    Bnm2 = Bnm1;
    Anm1 = An;
    Bnm1 = Bn;
    an = tau*tau + (mu - 0.5 + ell + n)*(mu - 0.5 + ell + n);
    bn = 2.0*(ell + mu + n) * xi;
    An = bn*Anm1 + an*Anm2;
    Bn = bn*Bnm1 + an*Bnm2;

    if(fabs(An) > RECUR_BIG || fabs(Bn) > RECUR_BIG) {
      An /= RECUR_BIG;
      Bn /= RECUR_BIG;
      Anm1 /= RECUR_BIG;
      Bnm1 /= RECUR_BIG;
      Anm2 /= RECUR_BIG;
      Bnm2 /= RECUR_BIG;
    }

    old_fn = fn;
    fn = An/Bn;
    del = old_fn/fn;

    if(fabs(del - 1.0) < 2.0*GSL_DBL_EPSILON) break;
  }

  result->val = fn;
  result->err = 4.0 * GSL_DBL_EPSILON * (sqrt(n) + 1.0) * fabs(fn);

  if(n >= maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Continued fraction for f_{ell+1}/f_ell
 * f_ell := P^{-mu-ell}_{-1/2 + I tau}(x),  x >= 1.0
 *
 * Uses Gautschi (Euler) equivalent series.
 */
static
int
conicalP_negmu_xgt1_CF1(const double mu, const int ell, const double tau,
                        const double x, gsl_sf_result * result)
{
  const int maxk = 20000;
  const double gamma = 1.0-1.0/(x*x);
  const double pre = sqrt(x-1.0)*sqrt(x+1.0) / (x*(2.0*(ell+mu+1.0)));
  double tk   = 1.0;
  double sum  = 1.0;
  double rhok = 0.0;
  int k;

  for(k=1; k<maxk; k++) {
    double tlk = 2.0*(ell + mu + k);
    double l1k = (ell + mu - 0.5 + 1.0 + k);
    double ak = -(tau*tau + l1k*l1k)/(tlk*(tlk+2.0)) * gamma;
    rhok = -ak*(1.0 + rhok)/(1.0 + ak*(1.0 + rhok));
    tk  *= rhok;
    sum += tk;
    if(fabs(tk/sum) < GSL_DBL_EPSILON) break;
  }

  result->val  = pre * sum;
  result->err  = fabs(pre * tk);
  result->err += 2.0 * GSL_DBL_EPSILON * (sqrt(k) + 1.0) * fabs(pre*sum);

  if(k >= maxk)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* Implementation of large negative mu asymptotic
 * [Dunster, Proc. Roy. Soc. Edinburgh 119A, 311 (1991), p. 326]
 */

inline
static double olver_U1(double beta2, double p)
{
  return (p-1.0)/(24.0*(1.0+beta2)) * (3.0 + beta2*(2.0 + 5.0*p*(1.0+p)));
}

inline
static double olver_U2(double beta2, double p)
{
  double beta4 = beta2*beta2;
  double p2    = p*p;
  double poly1 =  4.0*beta4 + 84.0*beta2 - 63.0;
  double poly2 = 16.0*beta4 + 90.0*beta2 - 81.0;
  double poly3 = beta2*p2*(97.0*beta2 - 432.0 + 77.0*p*(beta2-6.0) - 385.0*beta2*p2*(1.0 + p));
  return (1.0-p)/(1152.0*(1.0+beta2)) * (poly1 + poly2 + poly3);
}

static const double U3c1[] = {   -1307.0,   -1647.0,    3375.0,    3675.0 };
static const double U3c2[] = {   29366.0,   35835.0, -252360.0, -272630.0,
                                276810.0,  290499.0 };
static const double U3c3[] = {  -29748.0,   -8840.0, 1725295.0, 1767025.0,
                              -7313470.0, -754778.0, 6309875.0, 6480045.0 };
static const double U3c4[] = {    2696.0,    -16740.0,   -524250.0,  -183975.0,
                              14670540.0,  14172939.0, -48206730.0, -48461985.0,
                              36756720.0,  37182145.0 };
static const double U3c5[] = {       9136.0,      22480.0,     12760.0,
                                  -252480.0,   -4662165.0,   -1705341.0,
                                 92370135.0,   86244015.0, -263678415.0,
                               -260275015.0, 185910725.0,  185910725.0 };

#if 0
static double olver_U3(double beta2, double p)
{
  double beta4 = beta2*beta2;
  double beta6 = beta4*beta2;
  double opb2s = (1.0+beta2)*(1.0+beta2);
  double den   = 39813120.0 * opb2s*opb2s;
  double poly1 = gsl_poly_eval(U3c1, 4, p);
  double poly2 = gsl_poly_eval(U3c2, 6, p);
  double poly3 = gsl_poly_eval(U3c3, 8, p);
  double poly4 = gsl_poly_eval(U3c4, 10, p);
  double poly5 = gsl_poly_eval(U3c5, 12, p);

  return (p-1.0)*(     1215.0*poly1 + 324.0*beta2*poly2
                 + 54.0*beta4*poly3 +  12.0*beta6*poly4
                 + beta4*beta4*poly5
                 ) / den;
}
#endif /* 0 */

/* Large negative mu asymptotic
 * P^{-mu}_{-1/2 + I tau}, mu -> Inf
 * |x| < 1
 *
 * [Dunster, Proc. Roy. Soc. Edinburgh 119A, 311 (1991), p. 326]
 */
int
gsl_sf_conicalP_xlt1_large_neg_mu_e(double mu, double tau, double x,
                                       gsl_sf_result * result, double * ln_multiplier)
{
  double beta  = tau/mu;
  double beta2 = beta*beta;
  double S     = beta * acos((1.0-beta2)/(1.0+beta2));
  double p     = x/sqrt(beta2*(1.0-x*x) + 1.0);
  gsl_sf_result lg_mup1;
  int lg_stat = gsl_sf_lngamma_e(mu+1.0, &lg_mup1);
  double ln_pre_1 =  0.5*mu*(S - log(1.0+beta2) + log((1.0-p)/(1.0+p))) - lg_mup1.val;
  double ln_pre_2 = -0.25 * log(1.0 + beta2*(1.0-x));
  double ln_pre_3 = -tau * atan(p*beta);
  double ln_pre = ln_pre_1 + ln_pre_2 + ln_pre_3;
  double sum   = 1.0 - olver_U1(beta2, p)/mu + olver_U2(beta2, p)/(mu*mu);

  if(sum == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    *ln_multiplier = 0.0;
    return GSL_SUCCESS;
  }
  else {
    int stat_e = gsl_sf_exp_mult_e(ln_pre, sum, result);
    if(stat_e != GSL_SUCCESS) {
      result->val = sum;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(sum);
      *ln_multiplier = ln_pre;
    }
    else {
      *ln_multiplier = 0.0;
    }
    return lg_stat;
  }
}

/* Implementation of large tau asymptotic
 *
 * A_n^{-mu}, B_n^{-mu}  [Olver, p.465, 469]
 */

inline
static double olver_B0_xi(double mu, double xi)
{
  return (1.0 - 4.0*mu*mu)/(8.0*xi) * (1.0/tanh(xi) - 1.0/xi);
}

static double olver_A1_xi(double mu, double xi, double x)
{
  double B = olver_B0_xi(mu, xi);
  double psi;
  if(fabs(x - 1.0) < GSL_ROOT4_DBL_EPSILON) {
    double y = x - 1.0;
    double s = -1.0/3.0 + y*(2.0/15.0 - y *(61.0/945.0 - 452.0/14175.0*y));
    psi = (4.0*mu*mu - 1.0)/16.0 * s;
  }
  else {
    psi = (4.0*mu*mu - 1.0)/16.0 * (1.0/(x*x-1.0) - 1.0/(xi*xi));
  }
  return 0.5*xi*xi*B*B + (mu+0.5)*B - psi + mu/6.0*(0.25 - mu*mu);
}

inline
static double olver_B0_th(double mu, double theta)
{
  return -(1.0 - 4.0*mu*mu)/(8.0*theta) * (1.0/tan(theta) - 1.0/theta);
}

static double olver_A1_th(double mu, double theta, double x)
{
  double B = olver_B0_th(mu, theta);
  double psi;
  if(fabs(x - 1.0) < GSL_ROOT4_DBL_EPSILON) {
    double y = 1.0 - x;
    double s = -1.0/3.0 + y*(2.0/15.0 - y *(61.0/945.0 - 452.0/14175.0*y));
    psi = (4.0*mu*mu - 1.0)/16.0 * s;
  }
  else {
    psi = (4.0*mu*mu - 1.0)/16.0 * (1.0/(x*x-1.0) + 1.0/(theta*theta));
  }
  return -0.5*theta*theta*B*B + (mu+0.5)*B - psi + mu/6.0*(0.25 - mu*mu);
}

/* Large tau uniform asymptotics
 * P^{-mu}_{-1/2 + I tau}
 * 1 < x
 * tau -> Inf
 * [Olver, p. 469]
 */
int
gsl_sf_conicalP_xgt1_neg_mu_largetau_e(const double mu, const double tau,
                                          const double x, double acosh_x,
                                          gsl_sf_result * result, double * ln_multiplier)
{
  double xi = acosh_x;
  double ln_xi_pre;
  double ln_pre;
  double sumA, sumB, sum;
  double arg;
  gsl_sf_result J_mup1;
  gsl_sf_result J_mu;
  double J_mum1;

  if(xi < GSL_ROOT4_DBL_EPSILON) {
    ln_xi_pre = -xi*xi/6.0;           /* log(1.0 - xi*xi/6.0) */
  }
  else {
    gsl_sf_result lnshxi;
    gsl_sf_lnsinh_e(xi, &lnshxi);
    ln_xi_pre = log(xi) - lnshxi.val;     /* log(xi/sinh(xi) */
  }

  ln_pre = 0.5*ln_xi_pre - mu*log(tau);

  arg = tau*xi;

  gsl_sf_bessel_Jnu_e(mu + 1.0,   arg, &J_mup1);
  gsl_sf_bessel_Jnu_e(mu,         arg, &J_mu);
  J_mum1 = -J_mup1.val + 2.0*mu/arg*J_mu.val;      /* careful of mu < 1 */

  sumA = 1.0 - olver_A1_xi(-mu, xi, x)/(tau*tau);
  sumB = olver_B0_xi(-mu, xi);
  sum  = J_mu.val * sumA - xi/tau * J_mum1 * sumB;

  if(sum == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    *ln_multiplier = 0.0;
    return GSL_SUCCESS;
  }
  else {
    int stat_e = gsl_sf_exp_mult_e(ln_pre, sum, result);
    if(stat_e != GSL_SUCCESS) {
      result->val = sum;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(sum);
      *ln_multiplier = ln_pre;
    }
    else {
      *ln_multiplier = 0.0;
    }
    return GSL_SUCCESS;
  }
}

/* Large tau uniform asymptotics
 * P^{-mu}_{-1/2 + I tau}
 * -1 < x < 1
 * tau -> Inf
 * [Olver, p. 473]
 */
int
gsl_sf_conicalP_xlt1_neg_mu_largetau_e(const double mu, const double tau,
                                          const double x, const double acos_x,
                                          gsl_sf_result * result, double * ln_multiplier)
{
  double theta = acos_x;
  double ln_th_pre;
  double ln_pre;
  double sumA, sumB, sum, sumerr;
  double arg;
  gsl_sf_result I_mup1, I_mu;
  double I_mum1;

  if(theta < GSL_ROOT4_DBL_EPSILON) {
    ln_th_pre = theta*theta/6.0;   /* log(1.0 + theta*theta/6.0) */
  }
  else {
    ln_th_pre = log(theta/sin(theta));
  }

  ln_pre = 0.5 * ln_th_pre - mu * log(tau);

  arg = tau*theta;
  gsl_sf_bessel_Inu_e(mu + 1.0,   arg, &I_mup1);
  gsl_sf_bessel_Inu_e(mu,         arg, &I_mu);
  I_mum1 = I_mup1.val + 2.0*mu/arg * I_mu.val; /* careful of mu < 1 */

  sumA = 1.0 - olver_A1_th(-mu, theta, x)/(tau*tau);
  sumB = olver_B0_th(-mu, theta);
  sum  = I_mu.val * sumA - theta/tau * I_mum1 * sumB;
  sumerr  = fabs(I_mu.err * sumA);
  sumerr += fabs(I_mup1.err * theta/tau * sumB);
  sumerr += fabs(I_mu.err   * theta/tau * sumB * 2.0 * mu/arg);

  if(sum == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    *ln_multiplier = 0.0;
    return GSL_SUCCESS;
  }
  else {
    int stat_e = gsl_sf_exp_mult_e(ln_pre, sum, result);
    if(stat_e != GSL_SUCCESS) {
      result->val  = sum;
      result->err  = sumerr;
      result->err += GSL_DBL_EPSILON * fabs(sum);
      *ln_multiplier = ln_pre;
    }
    else {
      *ln_multiplier = 0.0;
    }
    return GSL_SUCCESS;
  }
}

/* Hypergeometric function which appears in the
 * large x expansion below:
 *
 *   2F1(1/4 - mu/2 - I tau/2, 3/4 - mu/2 - I tau/2, 1 - I tau, y)
 *
 * Note that for the usage below y = 1/x^2;
 */
static
int
conicalP_hyperg_large_x(const double mu, const double tau, const double y,
                        double * reF, double * imF)
{
  const int kmax = 1000;
  const double re_a = 0.25 - 0.5*mu;
  const double re_b = 0.75 - 0.5*mu;
  const double re_c = 1.0;
  const double im_a = -0.5*tau;
  const double im_b = -0.5*tau;
  const double im_c = -tau;

  double re_sum = 1.0;
  double im_sum = 0.0;
  double re_term = 1.0;
  double im_term = 0.0;
  int k;

  for(k=1; k<=kmax; k++) {
    double re_ak = re_a + k - 1.0;
    double re_bk = re_b + k - 1.0;
    double re_ck = re_c + k - 1.0;
    double im_ak = im_a;
    double im_bk = im_b;
    double im_ck = im_c;
    double den = re_ck*re_ck + im_ck*im_ck;
    double re_multiplier = ((re_ak*re_bk - im_ak*im_bk)*re_ck + im_ck*(im_ak*re_bk + re_ak*im_bk)) / den;
    double im_multiplier = ((im_ak*re_bk + re_ak*im_bk)*re_ck - im_ck*(re_ak*re_bk - im_ak*im_bk)) / den;
    double re_tmp = re_multiplier*re_term - im_multiplier*im_term;
    double im_tmp = im_multiplier*re_term + re_multiplier*im_term;
    double asum = fabs(re_sum) + fabs(im_sum);
    re_term = y/k * re_tmp;
    im_term = y/k * im_tmp;
    if(fabs(re_term/asum) < GSL_DBL_EPSILON && fabs(im_term/asum) < GSL_DBL_EPSILON) break;
    re_sum += re_term;
    im_sum += im_term;
  }

  *reF = re_sum;
  *imF = im_sum;

  if(k == kmax)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}

/* P^{mu}_{-1/2 + I tau}
 * x->Inf
 */
int
gsl_sf_conicalP_large_x_e(const double mu, const double tau, const double x,
                             gsl_sf_result * result, double * ln_multiplier)
{
  /* 2F1 term
   */
  double y = ( x < 0.5*GSL_SQRT_DBL_MAX ? 1.0/(x*x) : 0.0 );
  double reF, imF;
  int stat_F = conicalP_hyperg_large_x(mu, tau, y, &reF, &imF);

  /* f = Gamma(+i tau)/Gamma(1/2 - mu + i tau)
   * FIXME: shift so it's better for tau-> 0
   */
  gsl_sf_result lgr_num, lgth_num;
  gsl_sf_result lgr_den, lgth_den;
  int stat_gn = gsl_sf_lngamma_complex_e(0.0,tau,&lgr_num,&lgth_num);
  int stat_gd = gsl_sf_lngamma_complex_e(0.5-mu,tau,&lgr_den,&lgth_den);

  double angle = lgth_num.val - lgth_den.val + atan2(imF,reF);

  double lnx   = log(x);
  double lnxp1 = log(x+1.0);
  double lnxm1 = log(x-1.0);
  double lnpre_const = 0.5*M_LN2 - 0.5*M_LNPI;
  double lnpre_comm = (mu-0.5)*lnx - 0.5*mu*(lnxp1 + lnxm1);
  double lnpre_err  =   GSL_DBL_EPSILON * (0.5*M_LN2 + 0.5*M_LNPI)
                      + GSL_DBL_EPSILON * fabs((mu-0.5)*lnx)
                      + GSL_DBL_EPSILON * fabs(0.5*mu)*(fabs(lnxp1)+fabs(lnxm1));

  /*  result = pre*|F|*|f| * cos(angle - tau * (log(x)+M_LN2))
   */
  gsl_sf_result cos_result;
  int stat_cos = gsl_sf_cos_e(angle + tau*(log(x) + M_LN2), &cos_result);
  int status = GSL_ERROR_SELECT_4(stat_cos, stat_gd, stat_gn, stat_F);
  if(cos_result.val == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return status;
  }
  else {
    double lnFf_val = 0.5*log(reF*reF+imF*imF) + lgr_num.val - lgr_den.val;
    double lnFf_err = lgr_num.err + lgr_den.err + GSL_DBL_EPSILON * fabs(lnFf_val);
    double lnnoc_val = lnpre_const + lnpre_comm + lnFf_val;
    double lnnoc_err = lnpre_err + lnFf_err + GSL_DBL_EPSILON * fabs(lnnoc_val);
    int stat_e = gsl_sf_exp_mult_err_e(lnnoc_val, lnnoc_err,
                                          cos_result.val, cos_result.err,
                                          result);
    if(stat_e == GSL_SUCCESS) {
      *ln_multiplier = 0.0;
    }
    else {
      result->val  = cos_result.val;
      result->err  = cos_result.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *ln_multiplier = lnnoc_val;
    }
    return status;
  }
}

/* P^{mu}_{-1/2 + I tau}  first hypergeometric representation
 * -1 < x < 1
 * This is more effective for |x| small, however it will work w/o
 * reservation for any x < 0 because everything is positive
 * definite in that case.
 *
 * [Kolbig,   (3)] (note typo in args of gamma functions)
 * [Bateman, (22)] (correct form)
 */
static
int
conicalP_xlt1_hyperg_A(double mu, double tau, double x, gsl_sf_result * result)
{
  double x2 = x*x;
  double err_amp = 1.0 + 1.0/(GSL_DBL_EPSILON + fabs(1.0-fabs(x)));
  double pre_val = M_SQRTPI / pow(0.5*sqrt(1-x2), mu);
  double pre_err = err_amp * GSL_DBL_EPSILON * (fabs(mu)+1.0) * fabs(pre_val) ;
  gsl_sf_result ln_g1, ln_g2, arg_g1, arg_g2;
  gsl_sf_result F1, F2;
  gsl_sf_result pre1, pre2;
  double t1_val, t1_err;
  double t2_val, t2_err;

  int stat_F1 = gsl_sf_hyperg_2F1_conj_e(0.25 - 0.5*mu, 0.5*tau, 0.5, x2, &F1);
  int stat_F2 = gsl_sf_hyperg_2F1_conj_e(0.75 - 0.5*mu, 0.5*tau, 1.5, x2, &F2);
  int status = GSL_ERROR_SELECT_2(stat_F1, stat_F2);

  gsl_sf_lngamma_complex_e(0.75 - 0.5*mu, -0.5*tau, &ln_g1, &arg_g1);
  gsl_sf_lngamma_complex_e(0.25 - 0.5*mu, -0.5*tau, &ln_g2, &arg_g2);

  gsl_sf_exp_err_e(-2.0*ln_g1.val, 2.0*ln_g1.err, &pre1);
  gsl_sf_exp_err_e(-2.0*ln_g2.val, 2.0*ln_g2.err, &pre2);
  pre2.val *= -2.0*x;
  pre2.err *=  2.0*fabs(x);
  pre2.err +=  GSL_DBL_EPSILON * fabs(pre2.val);

  t1_val = pre1.val * F1.val;
  t1_err = fabs(pre1.val) * F1.err + pre1.err * fabs(F1.val);
  t2_val = pre2.val * F2.val;
  t2_err = fabs(pre2.val) * F2.err + pre2.err * fabs(F2.val);

  result->val  = pre_val * (t1_val + t2_val);
  result->err  = pre_val * (t1_err + t2_err);
  result->err += pre_err * fabs(t1_val + t2_val);
  result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

  return status;
}

/* P^{mu}_{-1/2 + I tau}
 * defining hypergeometric representation
 * [Abramowitz+Stegun, 8.1.2]
 * 1 < x < 3
 * effective for x near 1
 *
 */
#if 0
static
int
conicalP_def_hyperg(double mu, double tau, double x, double * result)
{
  double F;
  int stat_F = gsl_sf_hyperg_2F1_conj_renorm_e(0.5, tau, 1.0-mu, 0.5*(1.0-x), &F);
  *result = pow((x+1.0)/(x-1.0), 0.5*mu) * F;
  return stat_F;
}
#endif /* 0 */

/* P^{mu}_{-1/2 + I tau}  second hypergeometric representation
 * [Zhurina+Karmazina, (3.1)]
 * -1 < x < 3
 * effective for x near 1
 *
 */
#if 0
static
int
conicalP_xnear1_hyperg_C(double mu, double tau, double x, double * result)
{
  double ln_pre, arg_pre;
  double ln_g1, arg_g1;
  double ln_g2, arg_g2;
  double F;

  int stat_F = gsl_sf_hyperg_2F1_conj_renorm_e(0.5+mu, tau, 1.0+mu, 0.5*(1.0-x), &F);

  gsl_sf_lngamma_complex_e(0.5+mu, tau, &ln_g1, &arg_g1);
  gsl_sf_lngamma_complex_e(0.5-mu, tau, &ln_g2, &arg_g2);

  ln_pre  = mu*M_LN2 - 0.5*mu*log(fabs(x*x-1.0)) + ln_g1 - ln_g2;
  arg_pre = arg_g1 - arg_g2;

  *result = exp(ln_pre) * F;
  return stat_F;
}
#endif /* 0 */

/* V0, V1 from Kolbig, m = 0
 */
static
int
conicalP_0_V(const double t, const double f, const double tau, const double sgn,
             double * V0, double * V1)
{
  double C[8];
  double T[8];
  double H[8];
  double V[12];
  int i;
  T[0] = 1.0;
  H[0] = 1.0;
  V[0] = 1.0;
  for(i=1; i<=7; i++) {
    T[i] = T[i-1] * t;
    H[i] = H[i-1] * (t*f);
  }
  for(i=1; i<=11; i++) {
    V[i] = V[i-1] * tau;
  }

  C[0] = 1.0;
  C[1] = (H[1]-1.0)/(8.0*T[1]);
  C[2] = (9.0*H[2] + 6.0*H[1] - 15.0 - sgn*8.0*T[2])/(128.0*T[2]);
  C[3] = 5.0*(15.0*H[3] + 27.0*H[2] + 21.0*H[1] - 63.0 - sgn*T[2]*(16.0*H[1]+24.0))/(1024.0*T[3]);
  C[4] = 7.0*(525.0*H[4] + 1500.0*H[3] + 2430.0*H[2] + 1980.0*H[1] - 6435.0
              + 192.0*T[4] - sgn*T[2]*(720.0*H[2]+1600.0*H[1]+2160.0)
              ) / (32768.0*T[4]);
  C[5] = 21.0*(2835.0*H[5] + 11025.0*H[4] + 24750.0*H[3] + 38610.0*H[2]
               + 32175.0*H[1] - 109395.0 + T[4]*(1984.0*H[1]+4032.0)
               - sgn*T[2]*(4800.0*H[3]+15120.0*H[2]+26400.0*H[1]+34320.0)
               ) / (262144.0*T[5]);
  C[6] = 11.0*(218295.0*H[6] + 1071630.0*H[5] + 3009825.0*H[4] + 6142500.0*H[3]
               + 9398025.0*H[2] + 7936110.0*H[1] - 27776385.0
               + T[4]*(254016.0*H[2]+749952.0*H[1]+1100736.0)
               - sgn*T[2]*(441000.0*H[4] + 1814400.0*H[3] + 4127760.0*H[2]
                         + 6552000.0*H[1] + 8353800.0 + 31232.0*T[4]
                         )
               ) / (4194304.0*T[6]);

  *V0 = C[0] + (-4.0*C[3]/T[1]+C[4])/V[4]
             + (-192.0*C[5]/T[3]+144.0*C[6]/T[2])/V[8]
             + sgn * (-C[2]/V[2]
                      + (-24.0*C[4]/T[2]+12.0*C[5]/T[1]-C[6])/V[6]
                      + (-1920.0*C[6]/T[4])/V[10]
                      );
  *V1 = C[1]/V[1] + (8.0*(C[3]/T[2]-C[4]/T[1])+C[5])/V[5]
                  + (384.0*C[5]/T[4] - 768.0*C[6]/T[3])/V[9]
                  + sgn * ((2.0*C[2]/T[1]-C[3])/V[3]
                           + (48.0*C[4]/T[3]-72.0*C[5]/T[2] + 18.0*C[6]/T[1])/V[7]
                           + (3840.0*C[6]/T[5])/V[11]
                           );

  return GSL_SUCCESS;
}

/* V0, V1 from Kolbig, m = 1
 */
static
int
conicalP_1_V(const double t, const double f, const double tau, const double sgn,
             double * V0, double * V1)
{
  double Cm1;
  double C[8];
  double T[8];
  double H[8];
  double V[12];
  int i;
  T[0] = 1.0;
  H[0] = 1.0;
  V[0] = 1.0;
  for(i=1; i<=7; i++) {
    T[i] = T[i-1] * t;
    H[i] = H[i-1] * (t*f);
  }
  for(i=1; i<=11; i++) {
    V[i] = V[i-1] * tau;
  }

  Cm1  = -1.0;
  C[0] = 3.0*(1.0-H[1])/(8.0*T[1]);
  C[1] = (-15.0*H[2]+6.0*H[1]+9.0+sgn*8.0*T[2])/(128.0*T[2]);
  C[2] = 3.0*(-35.0*H[3] - 15.0*H[2] + 15.0*H[1] + 35.0 + sgn*T[2]*(32.0*H[1]+8.0))/(1024.0*T[3]);
  C[3] = (-4725.0*H[4] - 6300.0*H[3] - 3150.0*H[2] + 3780.0*H[1] + 10395.0
          -1216.0*T[4] + sgn*T[2]*(6000.0*H[2]+5760.0*H[1]+1680.0)) / (32768.0*T[4]);
  C[4] = 7.0*(-10395.0*H[5] - 23625.0*H[4] - 28350.0*H[3] - 14850.0*H[2]
              +19305.0*H[1] + 57915.0 - T[4]*(6336.0*H[1]+6080.0)
              + sgn*T[2]*(16800.0*H[3] + 30000.0*H[2] + 25920.0*H[1] + 7920.0)
              ) / (262144.0*T[5]);
  C[5] = (-2837835.0*H[6] - 9168390.0*H[5] - 16372125.0*H[4] - 18918900*H[3]
          -10135125.0*H[2] + 13783770.0*H[1] + 43648605.0
          -T[4]*(3044160.0*H[2] + 5588352.0*H[1] + 4213440.0)
          +sgn*T[2]*(5556600.0*H[4] + 14817600.0*H[3] + 20790000.0*H[2]
                     + 17297280.0*H[1] + 5405400.0 + 323072.0*T[4]
                     )
          ) / (4194304.0*T[6]);
  C[6] = 0.0;

  *V0 = C[0] + (-4.0*C[3]/T[1]+C[4])/V[4]
             + (-192.0*C[5]/T[3]+144.0*C[6]/T[2])/V[8]
             + sgn * (-C[2]/V[2]
                      + (-24.0*C[4]/T[2]+12.0*C[5]/T[1]-C[6])/V[6]
                      );
  *V1 = C[1]/V[1] + (8.0*(C[3]/T[2]-C[4]/T[1])+C[5])/V[5]
                  + (384.0*C[5]/T[4] - 768.0*C[6]/T[3])/V[9]
                  + sgn * (Cm1*V[1] + (2.0*C[2]/T[1]-C[3])/V[3]
                           + (48.0*C[4]/T[3]-72.0*C[5]/T[2] + 18.0*C[6]/T[1])/V[7]
                           );

  return GSL_SUCCESS;
}

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

/* P^0_{-1/2 + I lambda}
 */
int
gsl_sf_conicalP_0_e(const double lambda, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 1.0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(lambda == 0.0) {
    gsl_sf_result K;
    int stat_K;
    if(x < 1.0) {
      const double th = acos(x);
      const double s  = sin(0.5*th);
      stat_K = gsl_sf_ellint_Kcomp_e(s, GSL_MODE_DEFAULT, &K);
      result->val  = 2.0/M_PI * K.val;
      result->err  = 2.0/M_PI * K.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return stat_K;
    }
    else {
      const double xi = acosh(x);
      const double c  = cosh(0.5*xi);
      const double t  = tanh(0.5*xi);
      stat_K = gsl_sf_ellint_Kcomp_e(t, GSL_MODE_DEFAULT, &K);
      result->val  = 2.0/M_PI / c * K.val;
      result->err  = 2.0/M_PI / c * K.err;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return stat_K;
    }
  }
  else if(   (x <= 0.0 && lambda < 1000.0)
          || (x <  0.1 && lambda < 17.0)
          || (x <  0.2 && lambda < 5.0 )
    ) {
    return conicalP_xlt1_hyperg_A(0.0, lambda, x, result);
  }
  else if(   (x <= 0.2 && lambda < 17.0)
          || (x <= 1.5 && lambda < 20.0)
    ) {
    return gsl_sf_hyperg_2F1_conj_e(0.5, lambda, 1.0, (1.0-x)/2, result);
  }
  else if(1.5 < x && lambda < GSL_MAX(x,20.0)) {
    gsl_sf_result P;
    double lm;
    int stat_P = gsl_sf_conicalP_large_x_e(0.0, lambda, x,
                                              &P, &lm
                                              );
    int stat_e = gsl_sf_exp_mult_err_e(lm, 2.0*GSL_DBL_EPSILON * fabs(lm),
                                          P.val, P.err,
                                          result);
    return GSL_ERROR_SELECT_2(stat_e, stat_P);
  }
  else {
    double V0, V1;
    if(x < 1.0) {
      double th  = acos(x);
      double sth = sqrt(1.0-x*x);  /* sin(th) */
      gsl_sf_result I0, I1;
      int stat_I0 = gsl_sf_bessel_I0_scaled_e(th * lambda, &I0);
      int stat_I1 = gsl_sf_bessel_I1_scaled_e(th * lambda, &I1);
      int stat_I  = GSL_ERROR_SELECT_2(stat_I0, stat_I1);
      int stat_V  = conicalP_0_V(th, x/sth, lambda, -1.0, &V0, &V1);
      double bessterm = V0 * I0.val + V1 * I1.val;
      double besserr  = fabs(V0) * I0.err + fabs(V1) * I1.err;
      double arg1 = th*lambda;
      double sqts = sqrt(th/sth);
      int stat_e = gsl_sf_exp_mult_err_e(arg1, 4.0 * GSL_DBL_EPSILON * fabs(arg1),
                                            sqts * bessterm, sqts * besserr,
                                            result);
      return GSL_ERROR_SELECT_3(stat_e, stat_V, stat_I);
    }
    else {
      double sh = sqrt(x-1.0)*sqrt(x+1.0);  /* sinh(xi)      */
      double xi = log(x + sh);              /* xi = acosh(x) */
      gsl_sf_result J0, J1;
      int stat_J0 = gsl_sf_bessel_J0_e(xi * lambda, &J0);
      int stat_J1 = gsl_sf_bessel_J1_e(xi * lambda, &J1);
      int stat_J  = GSL_ERROR_SELECT_2(stat_J0, stat_J1);
      int stat_V  = conicalP_0_V(xi, x/sh, lambda, 1.0, &V0, &V1);
      double bessterm = V0 * J0.val + V1 * J1.val;
      double besserr  = fabs(V0) * J0.err + fabs(V1) * J1.err;
      double pre_val = sqrt(xi/sh);
      double pre_err = 2.0 * fabs(pre_val);
      result->val  = pre_val * bessterm;
      result->err  = pre_val * besserr;
      result->err += pre_err * fabs(bessterm);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_V, stat_J);
    }
  }
}

/* P^1_{-1/2 + I lambda}
 */
int
gsl_sf_conicalP_1_e(const double lambda, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(lambda == 0.0) {
    gsl_sf_result K, E;
    int stat_K, stat_E;
    if(x == 1.0) {
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else if(x < 1.0) {
      if(1.0-x < GSL_SQRT_DBL_EPSILON) {
        double err_amp = GSL_MAX_DBL(1.0, 1.0/(GSL_DBL_EPSILON + fabs(1.0-x)));
        result->val = 0.25/M_SQRT2 * sqrt(1.0-x) * (1.0 + 5.0/16.0 * (1.0-x));
        result->err = err_amp * 3.0 * GSL_DBL_EPSILON * fabs(result->val);
        return GSL_SUCCESS;
      }
      else {
        const double th = acos(x);
        const double s  = sin(0.5*th);
        const double c2 = 1.0 - s*s;
        const double sth = sin(th);
        const double pre = 2.0/(M_PI*sth);
        stat_K = gsl_sf_ellint_Kcomp_e(s, GSL_MODE_DEFAULT, &K);
        stat_E = gsl_sf_ellint_Ecomp_e(s, GSL_MODE_DEFAULT, &E);
        result->val  = pre * (E.val - c2 * K.val);
        result->err  = pre * (E.err + fabs(c2) * K.err);
        result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
        return GSL_ERROR_SELECT_2(stat_K, stat_E);
      }
    }
    else {
      if(x-1.0 < GSL_SQRT_DBL_EPSILON) {
        double err_amp = GSL_MAX_DBL(1.0, 1.0/(GSL_DBL_EPSILON + fabs(1.0-x)));
        result->val = -0.25/M_SQRT2 * sqrt(x-1.0) * (1.0 - 5.0/16.0 * (x-1.0));
        result->err = err_amp * 3.0 * GSL_DBL_EPSILON * fabs(result->val);
        return GSL_SUCCESS;
      }
      else {
        const double xi = acosh(x);
        const double c  = cosh(0.5*xi);
        const double t  = tanh(0.5*xi);
        const double sxi = sinh(xi);
        const double pre = 2.0/(M_PI*sxi) * c;
        stat_K = gsl_sf_ellint_Kcomp_e(t, GSL_MODE_DEFAULT, &K);
        stat_E = gsl_sf_ellint_Ecomp_e(t, GSL_MODE_DEFAULT, &E);
        result->val  = pre * (E.val - K.val);
        result->err  = pre * (E.err + K.err);
        result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
        return GSL_ERROR_SELECT_2(stat_K, stat_E);
      }
    }
  }
  else if(   (x <= 0.0 && lambda < 1000.0)
          || (x <  0.1 && lambda < 17.0)
          || (x <  0.2 && lambda < 5.0 )
    ) {
    return conicalP_xlt1_hyperg_A(1.0, lambda, x, result);
  }
  else if(   (x <= 0.2 && lambda < 17.0)
          || (x <  1.5 && lambda < 20.0)
    ) {
    const double arg = fabs(x*x - 1.0);
    const double sgn = GSL_SIGN(1.0 - x);
    const double pre = 0.5*(lambda*lambda + 0.25) * sgn * sqrt(arg);
    gsl_sf_result F;
    int stat_F = gsl_sf_hyperg_2F1_conj_e(1.5, lambda, 2.0, (1.0-x)/2, &F);
    result->val  = pre * F.val;
    result->err  = fabs(pre) * F.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_F;
  }
  else if(1.5 <= x && lambda < GSL_MAX(x,20.0)) {
    gsl_sf_result P;
    double lm;
    int stat_P = gsl_sf_conicalP_large_x_e(1.0, lambda, x,
                                              &P, &lm
                                              );
    int stat_e = gsl_sf_exp_mult_err_e(lm, 2.0 * GSL_DBL_EPSILON * fabs(lm),
                                          P.val, P.err,
                                          result);
    return GSL_ERROR_SELECT_2(stat_e, stat_P);
  }
  else {
    double V0, V1;
    if(x < 1.0) {
      const double sqrt_1mx = sqrt(1.0 - x);
      const double sqrt_1px = sqrt(1.0 + x);
      const double th  = acos(x);
      const double sth = sqrt_1mx * sqrt_1px;  /* sin(th) */
      gsl_sf_result I0, I1;
      int stat_I0 = gsl_sf_bessel_I0_scaled_e(th * lambda, &I0);
      int stat_I1 = gsl_sf_bessel_I1_scaled_e(th * lambda, &I1);
      int stat_I  = GSL_ERROR_SELECT_2(stat_I0, stat_I1);
      int stat_V  = conicalP_1_V(th, x/sth, lambda, -1.0, &V0, &V1);
      double bessterm = V0 * I0.val + V1 * I1.val;
      double besserr  =  fabs(V0) * I0.err + fabs(V1) * I1.err
                       + 2.0 * GSL_DBL_EPSILON * fabs(V0 * I0.val)
                       + 2.0 * GSL_DBL_EPSILON * fabs(V1 * I1.val);
      double arg1 = th * lambda;
      double sqts = sqrt(th/sth);
      int stat_e = gsl_sf_exp_mult_err_e(arg1, 2.0 * GSL_DBL_EPSILON * fabs(arg1),
                                            sqts * bessterm, sqts * besserr,
                                            result);
      result->err *= 1.0/sqrt_1mx;
      return GSL_ERROR_SELECT_3(stat_e, stat_V, stat_I);
    }
    else {
      const double sqrt_xm1 = sqrt(x - 1.0);
      const double sqrt_xp1 = sqrt(x + 1.0);
      const double sh = sqrt_xm1 * sqrt_xp1;  /* sinh(xi)      */
      const double xi = log(x + sh);          /* xi = acosh(x) */
      const double xi_lam = xi * lambda;
      gsl_sf_result J0, J1;
      const int stat_J0 = gsl_sf_bessel_J0_e(xi_lam, &J0);
      const int stat_J1 = gsl_sf_bessel_J1_e(xi_lam, &J1);
      const int stat_J  = GSL_ERROR_SELECT_2(stat_J0, stat_J1);
      const int stat_V  = conicalP_1_V(xi, x/sh, lambda, 1.0, &V0, &V1);
      const double bessterm = V0 * J0.val + V1 * J1.val;
      const double besserr  = fabs(V0) * J0.err + fabs(V1) * J1.err
                       + 512.0 * 2.0 * GSL_DBL_EPSILON * fabs(V0 * J0.val)
                       + 512.0 * 2.0 * GSL_DBL_EPSILON * fabs(V1 * J1.val)
                       + GSL_DBL_EPSILON * fabs(xi_lam * V0 * J1.val)
                       + GSL_DBL_EPSILON * fabs(xi_lam * V1 * J0.val);
      const double pre = sqrt(xi/sh);
      result->val  = pre * bessterm;
      result->err  = pre * besserr * sqrt_xp1 / sqrt_xm1;
      result->err += 4.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_ERROR_SELECT_2(stat_V, stat_J);
    }
  }
}

/* P^{1/2}_{-1/2 + I lambda} (x)
 * [Abramowitz+Stegun 8.6.8, 8.6.12]
 * checked OK [GJ] Fri May  8 12:24:36 MDT 1998
 */
int gsl_sf_conicalP_half_e(const double lambda, const double x,
                              gsl_sf_result * result
                              )
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 1.0) {
    double err_amp = 1.0 + 1.0/(GSL_DBL_EPSILON + fabs(1.0-fabs(x)));
    double ac  = acos(x);
    double den = sqrt(sqrt(1.0-x)*sqrt(1.0+x));
    result->val  = Root_2OverPi_ / den * cosh(ac * lambda);
    result->err  = err_amp * 3.0 * GSL_DBL_EPSILON * fabs(result->val);
    result->err *= fabs(ac * lambda) + 1.0;
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* x > 1 */
    double err_amp = 1.0 + 1.0/(GSL_DBL_EPSILON + fabs(1.0-fabs(x)));
    double sq_term = sqrt(x-1.0)*sqrt(x+1.0);
    double ln_term = log(x + sq_term);
    double den = sqrt(sq_term);
    double carg_val = lambda * ln_term;
    double carg_err = 2.0 * GSL_DBL_EPSILON * fabs(carg_val);
    gsl_sf_result cos_result;
    int stat_cos = gsl_sf_cos_err_e(carg_val, carg_err, &cos_result);
    result->val  = Root_2OverPi_ / den * cos_result.val;
    result->err  = err_amp * Root_2OverPi_ / den * cos_result.err;
    result->err += 4.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_cos;
  }
}

/* P^{-1/2}_{-1/2 + I lambda} (x)
 * [Abramowitz+Stegun 8.6.9, 8.6.14]
 * checked OK [GJ] Fri May  8 12:24:43 MDT 1998
 */
int gsl_sf_conicalP_mhalf_e(const double lambda, const double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 1.0) {
    double ac  = acos(x);
    double den = sqrt(sqrt(1.0-x)*sqrt(1.0+x));
    double arg = ac * lambda;
    double err_amp = 1.0 + 1.0/(GSL_DBL_EPSILON + fabs(1.0-fabs(x)));
    if(fabs(arg) < GSL_SQRT_DBL_EPSILON) {
      result->val  = Root_2OverPi_ / den * ac;
      result->err  = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      result->err *= err_amp;
    }
    else {
      result->val  = Root_2OverPi_ / (den*lambda) * sinh(arg);
      result->err  = GSL_DBL_EPSILON * (fabs(arg)+1.0) * fabs(result->val);
      result->err *= err_amp;
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }
    return GSL_SUCCESS;
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* x > 1 */
    double sq_term = sqrt(x-1.0)*sqrt(x+1.0);
    double ln_term = log(x + sq_term);
    double den = sqrt(sq_term);
    double arg_val = lambda * ln_term;
    double arg_err = 2.0 * GSL_DBL_EPSILON * fabs(arg_val);
    if(arg_val < GSL_SQRT_DBL_EPSILON) {
      result->val = Root_2OverPi_ / den * ln_term;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      return GSL_SUCCESS;
    }
    else {
      gsl_sf_result sin_result;
      int stat_sin = gsl_sf_sin_err_e(arg_val, arg_err, &sin_result);
      result->val  = Root_2OverPi_ / (den*lambda) * sin_result.val;
      result->err  = Root_2OverPi_ / fabs(den*lambda) * sin_result.err;
      result->err += 3.0 * GSL_DBL_EPSILON * fabs(result->val);
      return stat_sin;
    }
  }
}

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

  if(x <= -1.0 || l < -1) {
    DOMAIN_ERROR(result);
  }
  else if(l == -1) {
    return gsl_sf_conicalP_half_e(lambda, x, result);
  }
  else if(l == 0) {
    return gsl_sf_conicalP_mhalf_e(lambda, x, result);
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x < 0.0) {
    double c = 1.0/sqrt(1.0-x*x);
    gsl_sf_result r_Pellm1;
    gsl_sf_result r_Pell;
    int stat_0 = gsl_sf_conicalP_half_e(lambda, x, &r_Pellm1);  /* P^( 1/2) */
    int stat_1 = gsl_sf_conicalP_mhalf_e(lambda, x, &r_Pell);   /* P^(-1/2) */
    int stat_P = GSL_ERROR_SELECT_2(stat_0, stat_1);
    double Pellm1 = r_Pellm1.val;
    double Pell   = r_Pell.val;
    double Pellp1;
    int ell;

    for(ell=0; ell<l; ell++) {
      double d = (ell+1.0)*(ell+1.0) + lambda*lambda;
      Pellp1 = (Pellm1 - (2.0*ell+1.0)*c*x * Pell) / d;
      Pellm1 = Pell;
      Pell   = Pellp1;
    }

    result->val  = Pell;
    result->err  = (0.5*l + 1.0) * GSL_DBL_EPSILON * fabs(Pell);
    result->err += GSL_DBL_EPSILON * l * fabs(result->val);
    return stat_P;
  }
  else if(x < 1.0) {
    const double xi = x/(sqrt(1.0-x)*sqrt(1.0+x));
    gsl_sf_result rat;
    gsl_sf_result Phf;
    int stat_CF1 = conicalP_negmu_xlt1_CF1(0.5, l, lambda, x, &rat);
    int stat_Phf = gsl_sf_conicalP_half_e(lambda, x, &Phf);
    double Pellp1 = rat.val * GSL_SQRT_DBL_MIN;
    double Pell   = GSL_SQRT_DBL_MIN;
    double Pellm1;
    int ell;

    for(ell=l; ell>=0; ell--) {
      double d = (ell+1.0)*(ell+1.0) + lambda*lambda;
      Pellm1 = (2.0*ell+1.0)*xi * Pell + d * Pellp1;
      Pellp1 = Pell;
      Pell   = Pellm1;
    }

    result->val  = GSL_SQRT_DBL_MIN * Phf.val / Pell;
    result->err  = GSL_SQRT_DBL_MIN * Phf.err / fabs(Pell);
    result->err += fabs(rat.err/rat.val) * (l + 1.0) * fabs(result->val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_Phf, stat_CF1);
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* x > 1.0 */

    const double xi = x/sqrt((x-1.0)*(x+1.0));
    gsl_sf_result rat;
    int stat_CF1 = conicalP_negmu_xgt1_CF1(0.5, l, lambda, x, &rat);
    int stat_P;
    double Pellp1 = rat.val * GSL_SQRT_DBL_MIN;
    double Pell   = GSL_SQRT_DBL_MIN;
    double Pellm1;
    int ell;

    for(ell=l; ell>=0; ell--) {
      double d = (ell+1.0)*(ell+1.0) + lambda*lambda;
      Pellm1 = (2.0*ell+1.0)*xi * Pell - d * Pellp1;
      Pellp1 = Pell;
      Pell   = Pellm1;
    }

    if(fabs(Pell) > fabs(Pellp1)){
      gsl_sf_result Phf;
      stat_P = gsl_sf_conicalP_half_e(lambda, x, &Phf);
      result->val  =       GSL_SQRT_DBL_MIN * Phf.val / Pell;
      result->err  = 2.0 * GSL_SQRT_DBL_MIN * Phf.err / fabs(Pell);
      result->err += 2.0 * fabs(rat.err/rat.val) * (l + 1.0) * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }
    else {
      gsl_sf_result Pmhf;
      stat_P = gsl_sf_conicalP_mhalf_e(lambda, x, &Pmhf);
      result->val  =       GSL_SQRT_DBL_MIN * Pmhf.val / Pellp1;
      result->err  = 2.0 * GSL_SQRT_DBL_MIN * Pmhf.err / fabs(Pellp1);
      result->err += 2.0 * fabs(rat.err/rat.val) * (l + 1.0) * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }

    return GSL_ERROR_SELECT_2(stat_P, stat_CF1);
  }
}

int gsl_sf_conicalP_cyl_reg_e(const int m, const double lambda,
                                 const double x,
                                 gsl_sf_result * result
                                 )
{
  /* CHECK_POINTER(result) */

  if(x <= -1.0 || m < -1) {
    DOMAIN_ERROR(result);
  }
  else if(m == -1) {
    return gsl_sf_conicalP_1_e(lambda, x, result);
  }
  else if(m == 0) {
    return gsl_sf_conicalP_0_e(lambda, x, result);
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x < 0.0) {
    double c = 1.0/sqrt(1.0-x*x);
    gsl_sf_result r_Pkm1;
    gsl_sf_result r_Pk;
    int stat_0 = gsl_sf_conicalP_1_e(lambda, x, &r_Pkm1);  /* P^1 */
    int stat_1 = gsl_sf_conicalP_0_e(lambda, x, &r_Pk);    /* P^0 */
    int stat_P = GSL_ERROR_SELECT_2(stat_0, stat_1);
    double Pkm1 = r_Pkm1.val;
    double Pk   = r_Pk.val;
    double Pkp1;
    int k;

    for(k=0; k<m; k++) {
      double d = (k+0.5)*(k+0.5) + lambda*lambda;
      Pkp1 = (Pkm1 - 2.0*k*c*x * Pk) / d;
      Pkm1 = Pk;
      Pk   = Pkp1;
    }

    result->val  = Pk;
    result->err  = (m + 2.0) * GSL_DBL_EPSILON * fabs(Pk);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return stat_P;
  }
  else if(x < 1.0) {
    const double xi = x/(sqrt(1.0-x)*sqrt(1.0+x));
    gsl_sf_result rat;
    gsl_sf_result P0;
    int stat_CF1 = conicalP_negmu_xlt1_CF1(0.0, m, lambda, x, &rat);
    int stat_P0  = gsl_sf_conicalP_0_e(lambda, x, &P0);
    double Pkp1 = rat.val * GSL_SQRT_DBL_MIN;
    double Pk   = GSL_SQRT_DBL_MIN;
    double Pkm1;
    int k;

    for(k=m; k>0; k--) {
      double d = (k+0.5)*(k+0.5) + lambda*lambda;
      Pkm1 = 2.0*k*xi * Pk + d * Pkp1;
      Pkp1 = Pk;
      Pk   = Pkm1;
    }

    result->val  = GSL_SQRT_DBL_MIN * P0.val / Pk;
    result->err  = 2.0 * GSL_SQRT_DBL_MIN * P0.err / fabs(Pk);
    result->err += 2.0 * fabs(rat.err/rat.val) * (m + 1.0) * fabs(result->val);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_ERROR_SELECT_2(stat_P0, stat_CF1);
  }
  else if(x == 1.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else {
    /* x > 1.0 */

    const double xi = x/sqrt((x-1.0)*(x+1.0));
    gsl_sf_result rat;
    int stat_CF1 = conicalP_negmu_xgt1_CF1(0.0, m, lambda, x, &rat);
    int stat_P;
    double Pkp1 = rat.val * GSL_SQRT_DBL_MIN;
    double Pk   = GSL_SQRT_DBL_MIN;
    double Pkm1;
    int k;

    for(k=m; k>-1; k--) {
      double d = (k+0.5)*(k+0.5) + lambda*lambda;
      Pkm1 = 2.0*k*xi * Pk - d * Pkp1;
      Pkp1 = Pk;
      Pk   = Pkm1;
    }

    if(fabs(Pk) > fabs(Pkp1)){
      gsl_sf_result P1;
      stat_P = gsl_sf_conicalP_1_e(lambda, x, &P1);
      result->val  = GSL_SQRT_DBL_MIN * P1.val / Pk;
      result->err  = 2.0 * GSL_SQRT_DBL_MIN * P1.err / fabs(Pk);
      result->err += 2.0 * fabs(rat.err/rat.val) * (m+2.0) * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }
    else {
      gsl_sf_result P0;
      stat_P = gsl_sf_conicalP_0_e(lambda, x, &P0);
      result->val  = GSL_SQRT_DBL_MIN * P0.val / Pkp1;
      result->err  = 2.0 * GSL_SQRT_DBL_MIN * P0.err / fabs(Pkp1);
      result->err += 2.0 * fabs(rat.err/rat.val) * (m+2.0) * fabs(result->val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    }

    return GSL_ERROR_SELECT_2(stat_P, stat_CF1);
  }
}

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

#include "eval.h"

double gsl_sf_conicalP_0(const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_0_e(lambda, x, &result));
}

double gsl_sf_conicalP_1(const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_1_e(lambda, x, &result));
}

double gsl_sf_conicalP_half(const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_half_e(lambda, x, &result));
}

double gsl_sf_conicalP_mhalf(const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_mhalf_e(lambda, x, &result));
}

double gsl_sf_conicalP_sph_reg(const int l, const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_sph_reg_e(l, lambda, x, &result));
}

double gsl_sf_conicalP_cyl_reg(const int m, const double lambda, const double x)
{
  EVAL_RESULT(gsl_sf_conicalP_cyl_reg_e(m, lambda, x, &result));
}


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

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

/*
 * This module contains routines related to the Cholesky decomposition
 * of a complex Hermitian positive definite matrix.
 */

static void cholesky_complex_conj_vector(gsl_vector_complex *v);

/*
gsl_linalg_complex_cholesky_decomp()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix. See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 4.2.2.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

The lower triangle of A is overwritten with the Cholesky decomposition
*/

int
gsl_linalg_complex_cholesky_decomp(gsl_matrix_complex *A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i, j;
      gsl_complex z;
      double ajj;

      for (j = 0; j < N; ++j)
        {
          z = gsl_matrix_complex_get(A, j, j);
          ajj = GSL_REAL(z);

          if (j > 0)
            {
              gsl_vector_complex_const_view aj =
                gsl_matrix_complex_const_subrow(A, j, 0, j);

              gsl_blas_zdotc(&aj.vector, &aj.vector, &z);
              ajj -= GSL_REAL(z);
            }

          if (ajj <= 0.0)
            {
              GSL_ERROR("matrix is not positive definite", GSL_EDOM);
            }

          ajj = sqrt(ajj);
          GSL_SET_COMPLEX(&z, ajj, 0.0);
          gsl_matrix_complex_set(A, j, j, z);

          if (j < N - 1)
            {
              gsl_vector_complex_view av =
                gsl_matrix_complex_subcolumn(A, j, j + 1, N - j - 1);

              if (j > 0)
                {
                  gsl_vector_complex_view aj =
                    gsl_matrix_complex_subrow(A, j, 0, j);
                  gsl_matrix_complex_view am =
                    gsl_matrix_complex_submatrix(A, j + 1, 0, N - j - 1, j);

                  cholesky_complex_conj_vector(&aj.vector);

                  gsl_blas_zgemv(CblasNoTrans,
                                 GSL_COMPLEX_NEGONE,
                                 &am.matrix,
                                 &aj.vector,
                                 GSL_COMPLEX_ONE,
                                 &av.vector);

                  cholesky_complex_conj_vector(&aj.vector);
                }

              gsl_blas_zdscal(1.0 / ajj, &av.vector);
            }
        }

      /* Now store L^H in upper triangle */
      for (i = 1; i < N; ++i)
        {
          for (j = 0; j < i; ++j)
            {
              z = gsl_matrix_complex_get(A, i, j);
              gsl_matrix_complex_set(A, j, i, gsl_complex_conjugate(z));
            }
        }

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

/*
gsl_linalg_complex_cholesky_solve()
  Solve A x = b where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_solve (const gsl_matrix_complex * cholesky,
                                   const gsl_vector_complex * b,
                                   gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_complex_memcpy (x, b);

      /* solve for y using forward-substitution, L y = b */

      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */

      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

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

/*
gsl_linalg_complex_cholesky_svx()
  Solve A x = b in place where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_svx (const gsl_matrix_complex * cholesky,
                                 gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* solve for y using forward-substitution, L y = b */

      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */

      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

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

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

static void
cholesky_complex_conj_vector(gsl_vector_complex *v)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      gsl_complex z = gsl_vector_complex_get(v, i);
      gsl_vector_complex_set(v, i, gsl_complex_conjugate(z));
    }
} /* cholesky_complex_conj_vector() */


/*
 * Simultaneous ML fitting of N 3D Gaussian kernels against a volumetric image
 * MATLAB interface in df_mlfitN.c
 *
 * TODO:
 * - how to handle background, still as a constant over the
 * patch?
 * - bg not handled yet.
 *
 */

#include <assert.h>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>

#include <gsl/gsl_multimin.h>
#include <gsl/gsl_statistics.h>

#include "blit3.h"
#include "mlfit.h"

#ifndef verbose
#define verbose 1
#endif

// When low on bugs, uncomment the following lines for speed
// Try with valgrind and cmocka first ...
//
// replace gsl_vector_get(v,i) by v->data[i*v->stride]
// #define GSL_RANGE_CHECK_OFF

// Globals
uint32_t maxIterations = 5000;
double convCriteria = 1e-6;

// Headers
int    localizeDot(double *, size_t, double *,  double *);
int    localize(double *, size_t, size_t, size_t, double *, size_t, size_t, double *);
double matrix_get(double * , size_t, size_t, size_t, double *);
double my_f (const gsl_vector *, void *);
int    unit_tests(void);

// Optimization constants
typedef struct {
  double * V; // Volumetric image
  double * W; // Temporary space for model during iterations
  double * D; // dots, x,y,z, nphot, sigmax, sigmay, sigmaz
  size_t Vm, Vn, Vp;
  size_t Dm, Dn;
  double * bg;
} optParams;

double my_f (const gsl_vector *v, void *params)
  // The function to optimize. mlsimplex controls/varies the variables
  // in v. params are to set up the context
{

  optParams *p = (optParams *) params;

  // Size of volumetric image
  const size_t Vm = p->Vm; // Size of image
  const size_t Vn = p->Vn;
  const size_t Vp = p->Vp;
  const size_t Dn = p->Dn; // number of dots
  const size_t Dm = p->Dm;  // and number of features
  double * D = p->D;
  const double * restrict V = p->V; // Volumetric data to fit against
  double * W = p->W; // Pre allocated memory for the model
  double * bg = p->bg;

#if verbose > 0
  printf("-> my_f, %d dots\n", (int) Dn);
#endif

  // Get the other parameters ...

  double photsum = 0;

  // Copy the cordinates and nphot at the current iteration to D
  for(size_t dd = 0; dd<Dn; dd++)
  {
#if verbose > 0
    printf("%lu: (%.1f %.1f %.1f) %.0f (%.1f %.1f %.1f)\n", dd, D[Dm*dd+0],D[Dm*dd+1], D[Dm*dd+2], D[Dm*dd+3], D[Dm*dd+4], D[Dm*dd+5], D[Dm*dd+6]);
#endif
    D[Dm*dd+0] = gsl_vector_get(v, 4*dd+0); // x
    D[Dm*dd+1] = gsl_vector_get(v, 4*dd+1); // y
    D[Dm*dd+2] = gsl_vector_get(v, 4*dd+2); // z
    D[Dm*dd+3] = gsl_vector_get(v, 4*dd+3); // nphot

#if verbose>0
    printf("%lu: xyz (%.1f %.1f %.1f) %.0f (%.1f %.1f %.1f)\n", dd, D[Dm*dd+0],D[Dm*dd+1], D[Dm*dd+2], D[Dm*dd+3], D[Dm*dd+4], D[Dm*dd+5], D[Dm*dd+6]);
#endif
    if(D[Dm*dd+3]<0)
      return INFINITY;

    photsum = photsum + D[7*dd+3];
  }

#if verbose > 0
  printf("phosum of dot parameters: %f\n", photsum);
#endif

  // Copy background map to model
  memcpy(W, bg, sizeof(double)*Vm*Vn*Vp);
  //  memset(W, 0, sizeof(double)*Vm*Vn*Vp); // W = 0;
  blit3g(W, Vm, Vn, Vp, D, Dn, mid_point,0);

  // set bg as well with blit3(g)
  for(size_t kk = 0; kk<Vn*Vm*Vp; kk++) {
    //W[kk]++;
  }

  double vmin = 10e99;
  double vmax = -10e99;
  double wmin = 10e99;
  double wmax = -10e99;
  double diffmax = 0;
  for(size_t kk = 0; kk<Vn*Vm*Vp; kk++)
  {
    vmin = GSL_MIN(vmin, V[kk]);
    vmax = GSL_MAX(vmax, V[kk]);
    wmin = GSL_MIN(wmin, W[kk]);
    wmax = GSL_MAX(wmax, W[kk]);
    diffmax = GSL_MAX(diffmax, fabs(V[kk]-W[kk]));
  }
#if verbose>0
  printf("V -- min: %f max %f\n", vmin, vmax);
  printf("W -- min: %f max %f\n", wmin, wmax);
  printf("diffmax: %f\n", diffmax);
#endif

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum(mask.*(-(patch-model).^2./model - .5*log(model))));
   */

  double E = 0;
  for (size_t kk=0; kk<Vm*Vn*Vp; kk++) {
    //E+= (V[kk]-W[kk])*(V[kk]-W[kk])/V[kk] - .5*log(W[kk]);
    E+= (V[kk]-W[kk])*(V[kk]-W[kk]);
  }
  //  E = -E;

  /* Quadratic
   * for (size_t kk=0; kk<Rw*Rw; kk++)
   * E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]);
   */
#if verbose >0
  printf("E: %f\n", E);
#endif
  return E;
}
double matrix_get(double * V, size_t Vm, size_t Vn, size_t Vp, double * D)
  // Safely get V(D), i.e. check bounds. Returns 0 if outside
  //
{

  double x = D[0];
  double y = D[1];
  double z = D[2];

  if(x<0)
    return 0;
  if(x>Vm-1)
    return 0;
  if(y<0)
    return 0;
  if(y>Vn-1)
    return 0;
  if(z<0)
    return 0;
  if(z>Vp-1)
    return 1;

  size_t pos = (size_t) nearbyint(x) + nearbyint(y)*Vm +  nearbyint(z)*Vm*Vn;
  assert(pos<Vm*Vn*Vp);
  return V[pos];
}

int localize(double * V, size_t Vm, size_t Vn, size_t Vp,
    double * D, size_t Dm, size_t Dn, double * F)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{

  // avoid crashing matlab
  gsl_set_error_handler_off();

#if verbose > 0
  printf("-> Localize\n");
  printf("V: %lu %lu %lu\n", Vm, Vn, Vp);
  printf("D: %lu %lu\n", Dm, Dn);
#endif
  assert(Dm == 7); // x y z nphot sigmax sigmay sigmaz

  // Non-optimized parameters
  optParams par;
  par.V = V;
  par.Vm = Vm;
  par.Vn = Vn;
  par.Vp = Vp;
  par.Dn = Dn;
  par.Dm = Dm;
  par.W = malloc(Vm*Vn*Vp*sizeof(double));
  assert(par.W != NULL);
  par.D = D;
  par.bg = malloc(Vm*Vn*Vp*sizeof(double));

#if verbose > 0
  printf("Estimating local background\n");
#endif
  // Estimating a constant from the surroundings of the first dot
  // printf("Vm: %lu %Vn: %lu Vp: %lu, D: %f %f %f\n", Vm, Vn, Vp, D[0], D[1], D[2]);
  double bg = estimateBGV(V, Vm, Vn, Vp, D);
#if verbose>0
  printf("Background estimated to %f\n", bg);
#endif
  printf("Background estimated to %f\n", bg);

  for(size_t kk=0; kk<Vm*Vn*Vp; kk++)
    par.bg[kk] = bg;

  const gsl_multimin_fminimizer_type *T =
    gsl_multimin_fminimizer_nmsimplex2;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *ss, *x;
  gsl_multimin_function minex_func;

  size_t iter = 0;
  int status;
  double size;

  /* Starting point */
#if verbose > 0
  printf("Setting up starting vector\n");
#endif
  const int nf = 4; // # of features to be optimized: x, y, z, nphot
  int nParameters = nf*Dn;
  x = gsl_vector_alloc (nParameters);
  for(size_t dd = 0; dd<Dn; dd++)
  {
    gsl_vector_set(x, dd*nf+0, D[dd*Dm+0]); // x position
    gsl_vector_set(x, dd*nf+1, D[dd*Dm+1]); // y position
    gsl_vector_set(x, dd*nf+2, D[dd*Dm+2]); // z position

    // number of photons
    // Since the blitting is scaled by the central pixel, the number
    // of photons can be estimated as the central value of each peak -
    // background from the surrounding
    double nphot = gsl_max(0, matrix_get(V, Vm, Vn, Vp, D+ dd*Dm)-par.bg[0]);
    printf("nphot: %f\n", nphot);
    gsl_vector_set(x, dd*nf+3, nphot);
  }
#if verbose > 0
  printf("Setting up initial step sizes\n");
#endif
  /* Set initial step sizes */
  ss = gsl_vector_alloc(nParameters);
  gsl_vector_set_all(ss, 0.01);
  for(size_t dd = 0; dd<Dn; dd++)
  {
    gsl_vector_set(ss, dd*nf+3, 5); // Number of photons
  }

#if verbose > 0
  printf("Testing the fitting with the initial data\n");
#endif
  my_f(x,(void*) &par);

  /* Initialize method and iterate */
  minex_func.n = nParameters;
  minex_func.f = my_f;
  minex_func.params = ∥
#if verbose > 0
  printf("Allocating for the solver\n");
#endif
  s = gsl_multimin_fminimizer_alloc (T, nParameters);
#if verbose > 0
  printf("Initializing solver\n");
#endif
  gsl_multimin_fminimizer_set (s, &minex_func, x, ss);

  do
  {
    iter++;
#if verbose>0
    printf("----------------------------- Iteration: %lu\n", iter);
#endif
    status = gsl_multimin_fminimizer_iterate(s);

    if (status)
      break;

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, convCriteria);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu x:%10.3e y:%10.3e z:%10.3e NP:%6.1f f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->x, 2),
          gsl_vector_get (s->x, 3),
          s->fval, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxIterations);

  for(size_t dd = 0; dd<Dn; dd++)
  {
    F[5*dd+0] = gsl_vector_get (s->x, nf*dd+0); // x
    F[5*dd+1] = gsl_vector_get (s->x, nf*dd+1); // y
    F[5*dd+2] = gsl_vector_get (s->x, nf*dd+2); // z
    F[5*dd+3] = gsl_vector_get (s->x, nf*dd+3); // nphot
    F[5*dd+4] = status;                            // status
  }

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  free(par.W);
  free(par.bg);
  return status;
}

#ifndef _MAIN
#define _MAIN
int unit_tests(){
  double * V; // image
  int Vm = 102; int Vn = 107; int Vp = 60;
  double * D; // list of dots
  size_t Dm = 7; // number of features of the dots
  size_t Dn = 10; // number of dots
  double * F; // fitted dots

  printf("Image size: %dx%dx%d\n", Vm, Vn, Vp);
  printf("Localizing %lu dots\n", Dn);

  V = malloc(Vm*Vn*Vp*sizeof(double));
  D = malloc(Dm*Dn*sizeof(double));
  F = malloc(5*Dn*sizeof(double));

  // Initialize the data
  for(int kk=0; kk<Vm*Vn*Vp; kk++)
    V[kk] = rand_range(2,9);

  for(uint32_t kk=0; kk<Dn; kk++)
  {
    size_t pos = kk*Dm;
    D[pos] =   rand_range(6, Vm-7);
    D[pos+1] = rand_range(6, Vn-7);
    D[pos+2] = rand_range(6, Vp-7);
    D[pos+3] = 1000;
    D[pos+4] = 1.2;
    D[pos+5] = 1.2;
    D[pos+6] = 1.4;

    if(D[pos] < 6)
      D[pos] = 6;
    if(D[pos+1] < 6)
      D[pos+1] = 6;
    if(D[pos+2] < 6)
      D[pos+2] = 6;
#if verbose > 0
    printf("D %03d %f %f %f\n", kk, D[pos], D[pos+1], D[pos+2]);
#endif
  }

  for(uint32_t kk=0; kk<Dn; kk++)
  {
    size_t pos = nearbyint(D[kk*7]) + nearbyint(D[kk*7+1])*Vm + nearbyint(D[kk*7+2])*Vm*Vn;
    assert(pos<(size_t) Vm*Vn*Vp);
    assert(pos>0);
    V[pos] = 5;
  }

  // Run the optimization
  localize(V, Vm, Vn, Vp, D, Dm, Dn, F);

  // In next version, also supply clustering information

#if verbose >0
  for(size_t kk = 0; kk<Dm; kk++)
  {
    size_t Dpos = kk*7;
    size_t Fpos = kk*5;
    printf("%6lu (%f, %f, %f) -> (%f, %f, %f)\n", kk,
        D[Dpos], D[Dpos+1], D[Dpos+2],
        F[Fpos], F[Fpos+1], F[Fpos+2]);
  }
#endif
  free(F);
  free(D);
  free(V);
  return 0;
}

int main(int argc, char ** argv)
  // For testing, not used any more, see the MATLAB interface in
  // df_mlfit.c
{

  printf("%s\n", argv[0]);

  if(argc == 1)
    return unit_tests();

  return 0;
}
#endif


/**
  *
  * For reference: Pearson's rho...
  *
  *               sum( (x_i-E[x])(y_i-E[y]) )
  * rho = -------------------------------------------
  *       sqrt( sum((x_i-E[x])^2) sum((y_i-E[y])^2) )
  */

#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <limits.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <alloca.h>

#include <gsl/gsl_cdf.h>

#include "stattest.h"
#include "mix.h"
#include "bvr.h"
#include "limits.h"

struct Pair {
	float cv; // Continuous Variable
	unsigned int dv; // Discrete Variable
};

static int _cmp_pair( const void *pvl, const void *pvr ) {
	const struct Pair *l = (const struct Pair *)pvl;
	const struct Pair *r = (const struct Pair *)pvr;
	const float delta = l->cv - r->cv;
	if( delta == 0.0 )
		return 0.0;
	else
		return  delta < 0.0 ? -1 : +1;
}

struct MixCovars {

	/**
	  * Maximum number of samples
	  */
	unsigned int SAMPLE_CAPACITY;

	/**
	  * All category labels pushed into this accumulator must
	  * be in [0,category_capacity).
	 */
	unsigned int CATEGORY_CAPACITY;

	/**
	  * Count of samples pushed since last cat_clear.
	  */
	unsigned int sample_count;

	/**
	  * This essentially bounds the allowed category LABELS.
	  * Any category label push'ed in must be in [0,expected_categories).
	  * Only this member, NOT category_capacity, should be used to size
	  * counts arrays!
	  */
	unsigned int expected_categories;

	/**
	  * This is an count of DISTINCT LABELS observed by push.
	  */
	unsigned int observed_categories;

	// Calculation state ///////////////////////////////////////////////////

	double mean_rank;  // necessarily of both covariates
	double sum_sq_dev; // used for Kruskal-Wallis and Spearman

	/**
	  * Following struct array is ONLY for Spearman rho calculation.
	  * Note that the number of non-empty categories after data entry
	  * says NOTHING about WHICH categories (indices) are non-zero.
	  * An earlier implementation was assuming category_count[0,1] > 0,
	  * BUT THIS NEED NOT BE TRUE! Thus, all this hack...
	  */
	struct {
		unsigned int index;
		unsigned int count;
		unsigned int meanRank;
	} edge[2];

	double sum_dev_prod; // used only for Spearman-rho

	// Buffer //////////////////////////////////////////////////////////////

	/**
	  * The TOTAL amount of malloc'ed space (for the purposes
	  * of fast clearing with memset).
	  */
	size_t SIZEOF_BUFFERS;

	/**
	  * Two buffers
	  */
	struct Pair *samples;

	/**
	  * A buffer allocated once in construction and reused for
	  * all subsequent analyses.
	  * Reallocation is not currently supported.
	  */
	unsigned int *category_count;
};

#ifdef _UNITTEST_MIX_
unsigned arg_min_mixb_count = 1;
#else
extern unsigned arg_min_mixb_count;
#endif

/**
  * Both minCat and maxCat assume that samples is non-empty,
  * but it's entirely possible that there were NO samples.
  */
static unsigned _minCat( struct MixCovars *co ) {
	unsigned int i = 0;
	while( i < co->expected_categories ) {
		if( co->category_count[i] > 0 )
			return i;
		i++;
	}
	return INT_MAX; // just to silence compiler.
}

static unsigned _maxCat( struct MixCovars *co ) {

	int i = co->expected_categories;
	while( i-- > 0 ) {
		if( co->category_count[i] > 0 )
			return (unsigned int)i;
	}
	return INT_MAX; // just to silence compiler
}

/**
  * Notice that all calcuations are performed without actually requiring
  * an array of ranks.
  */
static unsigned int _rank_sums( struct MixCovars *co,
	double *sums ) {

	const unsigned int N = co->sample_count;
	const bool PRECALC_SPEARMAN_PRODUCT
		= co->observed_categories == 2;
	const unsigned UPPER
		= co->edge[1].index;
	unsigned int ties = 0;
	unsigned int until = 0;
	float rank = 0;
	double diff;

	// The default pair::operator< automatically uses the first element.

	qsort( co->samples, co->sample_count, sizeof(struct Pair), _cmp_pair );

	for(unsigned int i = 0; i < N; i++ ) {

		const unsigned int cat
			= co->samples[i].dv;

		// The rank of the current sample is i+1 UNLESS we're in
		// the midst of a run of samples with the same value in
		// which case the last computed rank (a mean) is reused.

		if( ! ( i < until ) ) {

			until = i + 1;
			while( until < N
					&& ( co->samples[i].cv == co->samples[until].cv ) ) {
				ties++;
				until++;
			}

			if( i == 0 && until == N ) {
				// degenerate case: all ties.
			}

			// Mean of n integers starting on r:
			// [ (r+0) + (r+1) + (r+2) + ... + (r+(n-1)) ] / n
			// ... = \frac{nr + \sum_{i=0}^{n-1} i}{n}
			// ... = r + (1/n)[ (n-1)n / 2 ]
			// ... = r + (n-1)/2

			rank = (1.0+i) + ((until-i)-1)/2.0;

			// ...notice if the very next sample -is- different
			// then rank == i+1, the trivial case.
		}

		/**
		  * Warning: If the numeric feature is constant then rank==meanRank
		  * and diff == 0
		  */
		diff = rank - co->mean_rank;

		co->sum_sq_dev += ( diff * diff );

		if( PRECALC_SPEARMAN_PRODUCT ) {
			co->sum_dev_prod += ( diff * ( co->edge[UPPER==cat?1:0].meanRank - co->mean_rank ) );
		}

		sums[ cat ] += rank;
	}

	return ties;
}

#if defined(_UNITTEST_MIX_)
static void dbg_dump( struct MixCovars *co, FILE *fp ) {
	for(unsigned int i = 0; i < co->sample_count; i++ )
		fprintf( fp, "%.3f\t%d\n", co->samples[i].cv, co->samples[i].dv );
}
#endif

/***************************************************************************
  * Publics
  */

void mix_destroy( void *pv ) {

	if( pv ) {
		struct MixCovars *co = (struct MixCovars *)pv;
		if( co->samples )
			free( co->samples );
		free( pv );
	}
}

/**
  * Pre-allocate a set of working buffers large enough for all anticipated
  * calculations (max feature length) and a struct to wrap them.
  */
void *mix_create( unsigned int sample_capacity, unsigned int category_capacity ) {

	struct MixCovars *co
		= calloc( 1, sizeof(struct MixCovars) );
	if( co ) {
		co->SAMPLE_CAPACITY   = sample_capacity;
		co->CATEGORY_CAPACITY = category_capacity;
		co->SIZEOF_BUFFERS
			= ( sample_capacity * sizeof(struct Pair) )
			+ ( category_capacity * sizeof(unsigned int) );

		// Allocate one large buffer and partition it up.
		co->samples
			= calloc( co->SIZEOF_BUFFERS, sizeof(char) );
		co->category_count
			= (unsigned int*)(co->samples
			+ sample_capacity);
		// If -anything- failed clean up any successes.
		if( NULL == co->samples ) {
			mix_destroy( co );
			return NULL;
		}
		return co;
	}
	return NULL;
}

void mix_clear( void *pv, unsigned expcat ) {

	struct MixCovars *co = (struct MixCovars *)pv;

	assert( expcat <= co->CATEGORY_CAPACITY );

	co->sample_count = 0;
	co->expected_categories = expcat;
	co->observed_categories = 0;
	co->mean_rank    = 0.0;
	co->sum_sq_dev   = 0.0;
	co->sum_dev_prod = 0.0;
	memset( co->edge, 0, sizeof(co->edge) );
	co->sum_dev_prod = 0.0;

	memset( co->samples, 0, co->SIZEOF_BUFFERS );
}

void mix_push( void *pv, float num, unsigned int cat ) {

	struct MixCovars *co = (struct MixCovars *)pv;

	assert( cat              < co->expected_categories );
	assert( co->sample_count < co->SAMPLE_CAPACITY     );

	if( 0 == co->category_count[ cat ] )
		co->observed_categories++;

	co->category_count[ cat ] += 1;

	// Not updating edges in here because the number of conditionals
	// executed for sample counts > 32 exceeds the work to find the
	// edges post-sample accumulation.
	co->samples[ co->sample_count ].cv = num;
	co->samples[ co->sample_count ].dv = cat;
	co->sample_count++;
}

size_t mix_size( void *pv ) {
	struct MixCovars *co = (struct MixCovars *)pv;
	return co->sample_count;
}

/**
  * - Establish whether or not Spearman is even sensible (according to
  *   whether or not the categorical variable is binary).
  * - Compute the mean rank used throughout subsequent calcs.
  * - Compute the mean ranks of the categorical values assuming
  *   they -are- binary.
  */
bool mix_complete( void *pv ) {

	struct MixCovars *co = (struct MixCovars *)pv;
	if( mix_degenerate( co ) ) return false;

	co->mean_rank = (1.0 + co->sample_count)/2.0;

	co->edge[1].index = _maxCat( co ); // ...this is used by kruskal_wallis, but

	assert( co->edge[1].index < co->expected_categories );

	// ...the remainder of the edge[] struct is only used if...

	if( co->observed_categories == 2 ) {

		// All the following is relevant only to Spearman rho calculation.

		co->edge[0].index = _minCat( co );
		assert( co->edge[0].index < co->expected_categories );

		co->edge[0].count = co->category_count[ co->edge[0].index ];
		co->edge[0].meanRank = MEAN_RANK_OF_TIES( 0, co->edge[0].count );

		co->edge[1].count = co->category_count[ co->edge[1].index ];

		if( co->edge[0].count < arg_min_mixb_count ||
			co->edge[1].count < arg_min_mixb_count )
			return false; // another degeneracy class

		co->edge[1].meanRank = MEAN_RANK_OF_TIES( co->edge[0].count, co->sample_count );

		assert( (co->edge[0].count + co->edge[1].count) == co->sample_count );
	}
	return true;
}

bool mix_degenerate( void *pv ) {
	struct MixCovars *co = (struct MixCovars *)pv;
	return co->observed_categories < 2 || co->sample_count < 2;
}

bool mix_categoricalIsBinary( void *pv ) {
	struct MixCovars *co = (struct MixCovars *)pv;
	return co->observed_categories == 2;
}

/**
 * Calculates the Kruskal-Wallis statistic and optionally a p-value.
 * Importantly, it does it in a one pass iteration over the data.
 */
int mix_kruskal_wallis( void *pv, struct Statistic *result ) {

	struct MixCovars *co = (struct MixCovars *)pv;
	const unsigned int N
		= co->sample_count ;
	const size_t SIZEOF_SUMS
		= co->expected_categories * sizeof(double);
	double *rank_sum
		= (double*)alloca( SIZEOF_SUMS );
	double numerator = 0.0;

	memset( rank_sum, 0, SIZEOF_SUMS   );

	result->extra_value[0] = _rank_sums( co, rank_sum );

	for(unsigned int i = 0; i <= co->edge[1].index; i++ ) {
		if( co->category_count[i] > 0 ) {
			rank_sum[i] /= co->category_count[i];
			double delta
				= rank_sum[i] - co->mean_rank;
			numerator
				+= ( co->category_count[i] * delta * delta );
		}
	}

	result->name
		= "Kruskal-Wallis_K";
	result->sample_count
		= N;
	result->value
		= (N-1) * ( numerator / co->sum_sq_dev );
	result->probability
		= gsl_cdf_chisq_Q( result->value, co->observed_categories-1 );

	return 0;
}

/**
 * P-value returned is ONE-SIDED. Double it for two-sided.
 */
#ifdef HAVE_MANN_WHITNEY
int mix_mann_whitney( void *pv, struct Statistic *result ) {

	struct MixCovars *co = (struct MixCovars *)pv;
	const size_t SIZEOF_SUMS
		= co->expected_categories * sizeof(double);
	double *rank_sum
		= (double*)alloca( SIZEOF_SUMS );
	memset( rank_sum, 0, SIZEOF_SUMS   );

	ms->ties
		= _rank_sums( co, rank_sum );

	// TODO: If too many ties, the following method of calculating U
	//       becomes invalid. What to do?

	const unsigned int SQ
		= category_count[0]*category_count[1];

	// If either group count is 0 we're done...

	if( SQ ) {

		const double U1
			= rank_sum[0] - (category_count[0]*(category_count[0]+1.0))/2.0;
		const double U2
			= rank_sum[1] - (category_count[1]*(category_count[1]+1.0))/2.0;

		assert( U1+U2 == SQ );

		ms->U = U1 < U2 ? U1 : U2;

		const double mean_U
			= SQ / 2.0;
		const double sigma_U
			= sqrt( SQ*( category_count[0] + category_count[1] + 1.0 ) / 12.0 );

		cs->P = gsl_cdf_ugaussian_P( ( ms->U - mean_U ) / sigma_U );
	}
	return 0;
}
#endif

/**
  * This calculation depends on a quantities calculated within
  * kruskal_wallis (or mann_whitney), so must be called after
  * either of them.
  * (This is also the only reason this method can be const.)
  */
double mix_spearman_rho( void *pv ) {

	struct MixCovars *co = (struct MixCovars *)pv;
	/**
	  * denominator requires sum of square deviation for
	  * the binary variables which collapses to...
	  */
	double SQD0 = (co->edge[0].meanRank - co->mean_rank ); SQD0 *= SQD0;
	double SQD1 = (co->edge[1].meanRank - co->mean_rank ); SQD1 *= SQD1;

	const double SUMSQD
		= co->edge[0].count * SQD0 + co->edge[1].count * SQD1;
	/**
	  * And the sum of squared deviation of the numeric covariate's
	  * ranks (sum_sq_dev) was (necessarily) calculated earlier!
	  */
	return co->sum_dev_prod / sqrt( co->sum_sq_dev * SUMSQD );
}

#ifdef _UNITTEST_MIX_

/**
 * This is intended to be exercised with the following R script
 * that generates a small table of grouped floating-point values
 * executes a Kruskal-Wallis test on it, and dumps the table
 * to a tab-delimited file with the test results as a comment on
 * the first line.
 *
 * x <- data.frame(
 * 		cat=as.integer(gl(3,4))-1,
 * 		num=c( runif(4)*10, runif(4)*20, 10+runif(4)*10 ) );
 * k <- with( x, kruskal.test( num, cat ) );
 * cat( sprintf( "# K=%f p-value=%f\n", k$statistic, k$p.value ), file="foo.tab" );
 * write.table( x, 'foo.tab', quote=F, sep='\t', row.names=F, col.names=F, append=TRUE );
 */
#include <err.h>

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

	if( argc >= 3 ) {

		struct Statistic result;
		const int EXPCAT
			= atoi( argv[1] );
		struct MixCovars *accum
		   = mix_create( atoi( argv[2] ), MAX_CATEGORY_COUNT );
		FILE *fp
			= argc > 3
			? fopen( argv[3], "r" )
			: stdin;
		char *line = NULL;
		size_t n = 0;

		mix_clear( accum, EXPCAT );

		while( getline( &line, &n, fp ) > 0 ) {
			unsigned int cat;
			float        num;
			if( line[0] == '#' ) {
				fputs( line, stdout );
				continue;
			}
			if( 2 == sscanf( line, "%d\t%f\n", &cat, &num ) ) {
				mix_push( accum, num, cat );
			} else {
				fprintf( stderr, "Failure parsing line: %s", line );
			}
		}
		free( line );
		fclose( fp );

		// Always calculate Kruskal-Wallis since it's valid for any groups
		// count >= 2...

		if( mix_complete( accum ) ) {
			memset( &result, 0, sizeof(struct Statistic) );
			if( mix_kruskal_wallis( accum, &result ) == 0 ) {
				printf( "K=%f, p-value=%f", result.value, result.probability );
				if( mix_categoricalIsBinary( accum ) )
					printf( ", spearman=%f\n", mix_spearman_rho(accum) );
				else
					fputc( '\n', stdout );
			} else {
				printf( "error\n" );
			}
		}

#ifdef HAVE_MANN_WHITNEY
		// Do a Mann-Whitney, too, if there are only 2 groups.
		if( mix_categoricalIsBinary(accum) == 2 ) {
		}
#endif
		mix_destroy( accum );
	} else
		err( -1, "%s <categories> <sample count> [ <input file> ]", argv[0] );
	return 0;
}
#endif


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

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

#include "ccl.h"

ccl_cl_tracer_collection_t *ccl_cl_tracer_collection_t_new(int *status) {
  ccl_cl_tracer_collection_t *trc = NULL;
  trc = malloc(sizeof(ccl_cl_tracer_collection_t));
  if (trc == NULL)
    *status = CCL_ERROR_MEMORY;

  if (*status == 0) {
    trc->n_tracers = 0;
    // Currently CCL_MAX_TRACERS_PER_COLLECTION is hard-coded to 100.
    // It should be enough for any practical application with minimal memory overhead
    trc->ts = malloc(CCL_MAX_TRACERS_PER_COLLECTION*sizeof(ccl_cl_tracer_t *));
    if (trc->ts == NULL) {
      *status = CCL_ERROR_MEMORY;
      free(trc);
      trc = NULL;
    }
  }

  return trc;
}

void ccl_cl_tracer_collection_t_free(ccl_cl_tracer_collection_t *trc) {
  if (trc != NULL) {
    if (trc->ts != NULL)
      free(trc->ts);
    free(trc);
  }
}

void ccl_add_cl_tracer_to_collection(ccl_cl_tracer_collection_t *trc,
                                     ccl_cl_tracer_t *tr, int *status) {
  if (trc->n_tracers >= CCL_MAX_TRACERS_PER_COLLECTION) {
    *status = CCL_ERROR_MEMORY;
    return;
  }
  trc->ts[trc->n_tracers] = tr;
  trc->n_tracers++;
}

// Takes an array of z-dependent numbers and the corresponding z values
// and returns an array of a values and the corresponding a-dependent values.
// The order of the original arrays is assumed to be ascending in z, and
// the order of the returned arrays is swapped (so it has ascending a).
static void from_z_to_a(ccl_cosmology *cosmo,
                        int nz, double *z_arr, double *fz_arr,
                        double **a_arr, double **fa_arr, int *status) {
  *a_arr = NULL;
  *fa_arr = NULL;
  *a_arr = malloc(nz*sizeof(double));
  *fa_arr = malloc(nz*sizeof(double));

  if ((a_arr == NULL) || (fa_arr == NULL)) {
    *status = CCL_ERROR_MEMORY;
    free(*a_arr);
    free(*fa_arr);
    ccl_cosmology_set_status_message(
      cosmo, "ccl_tracers.c: from_z_to_a(): memory allocation error\n");
  }

  if (*status == 0) {
    int ia;
    // Populate array of scale factors in reverse order
    for (ia=0; ia < nz; ia++) {
      (*a_arr)[ia] = 1./(1+z_arr[nz-1-ia]);
      (*fa_arr)[ia] = fz_arr[nz-1-ia];
    }
  }
}

//Integrand for N(z) integrator
static double nz_integrand(double z, void *pars) {
  ccl_f1d_t *nz_f = (ccl_f1d_t *)pars;

  return ccl_f1d_t_eval(nz_f,z);
}

// Gets area of N(z) curve
static double get_nz_norm(ccl_cosmology *cosmo, ccl_f1d_t *nz_f,
                          double z0, double zf, int *status) {
  double nz_norm = -1, nz_enorm;

  // Get N(z) norm
  gsl_function F;
  gsl_integration_workspace *w = NULL;
  F.function = &nz_integrand;
  F.params = nz_f;

  w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);

  if (w == NULL) {
    *status = CCL_ERROR_MEMORY;
    ccl_cosmology_set_status_message(
      cosmo, "ccl_tracers.c: get_nz_norm(): out of memory");
  }
  else {
    int gslstatus = gsl_integration_qag(
      &F, z0, zf, 0,
      cosmo->gsl_params.INTEGRATION_EPSREL,
      cosmo->gsl_params.N_ITERATION,
      cosmo->gsl_params.INTEGRATION_GAUSS_KRONROD_POINTS,
      w, &nz_norm, &nz_enorm);

    if (gslstatus != GSL_SUCCESS) {
      ccl_raise_gsl_warning(gslstatus, "ccl_tracers.c: get_nz_norm():");
      *status = CCL_ERROR_INTEG;
      ccl_cosmology_set_status_message(
        cosmo,
        "ccl_tracers.c: get_nz_norm(): "
        "integration error when normalizing N(z)\n");
    }
  }

  gsl_integration_workspace_free(w);

  return nz_norm;
}

static void from_z_to_chi(ccl_cosmology *cosmo, int nz, double *z_arr,
                          double *chi_arr, int *status) {
  for (int ichi=0; ichi < nz; ichi++)
    chi_arr[ichi] = ccl_comoving_radial_distance(cosmo, 1./(1+z_arr[ichi]), status);
}

void ccl_get_number_counts_kernel(ccl_cosmology *cosmo,
                                  int nz, double *z_arr, double *nz_arr,
                                  int normalize_nz,
                                  double *pchi_arr, int *status) {
  // Returns dn/dchi normalized to unit area from an unnormalized dn/dz.
  // Prepare N(z) spline
  ccl_f1d_t *nz_f = NULL;

  nz_f = ccl_f1d_t_new(nz, z_arr, nz_arr, 0, 0);
  if (nz_f == NULL) {
    *status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_get_number_counts_kernel: "
      "error initializing spline\n");
  }

  // Get N(z) normalization
  double i_nz_norm = -1;
  if (*status == 0) {
    if (normalize_nz)
      i_nz_norm = 1./get_nz_norm(cosmo, nz_f, z_arr[0], z_arr[nz-1], status);
    else
      i_nz_norm = 1;
  }

  if (*status == 0) {
    // Populate arrays
    for(int ichi=0; ichi < nz; ichi++) {
      double a = 1./(1+z_arr[ichi]);
      double h = cosmo->params.h*ccl_h_over_h0(cosmo,a,status)/ccl_constants.CLIGHT_HMPC;
      // H(z) * dN/dz * 1/Ngal
      pchi_arr[ichi] = h*nz_arr[ichi]*i_nz_norm;
    }
  }

  ccl_f1d_t_free(nz_f);
}

//3 H0^2 Omega_M / 2
static double get_lensing_prefactor(ccl_cosmology *cosmo,int *status) {
  double hub = cosmo->params.h/ccl_constants.CLIGHT_HMPC;
  return 1.5*hub*hub*cosmo->params.Omega_m;
}

typedef struct {
  ccl_cosmology *cosmo;
  double z_max;
  double z_end;
  double chi_end;
  double i_nz_norm;
  ccl_f1d_t *nz_f;
  ccl_f1d_t *sz_f;
  int *status;
} integ_lensing_pars;

// Integrand for lensing kernel.
// Returns N(z) * (1 - 5*s(z)/2) * (chi(z)-chi) / chi(z)
static double lensing_kernel_integrand(double z, void *pars) {
  integ_lensing_pars *p = (integ_lensing_pars *)pars;
  double pz = ccl_f1d_t_eval(p->nz_f, z);
  double qz;
  if (p->sz_f == NULL) // No magnification factor
    qz = 1;
  else // With magnification factor
    qz = (1 - 2.5*ccl_f1d_t_eval(p->sz_f, z));

  if (z == 0)
    return pz * qz;
  else {
    double chi = ccl_comoving_radial_distance(p->cosmo, 1./(1+z), p->status);
    return (
      pz * qz *
      ccl_sinn(p->cosmo, chi-p->chi_end, p->status) /
      ccl_sinn(p->cosmo, chi, p->status));
  }
}

// Returns
// Integral[ p(z) * (1-5s(z)/2) * chi_end * (chi(z)-chi_end)/chi(z) , {z',z_end,z_max} ]
static double lensing_kernel_integrate(ccl_cosmology *cosmo,
                                       integ_lensing_pars *pars,
                                       gsl_integration_workspace *w) {
  int gslstatus = 0;
  double result, eresult;
  gsl_function F;
  F.function = &lensing_kernel_integrand;
  F.params = pars;
  gslstatus = gsl_integration_qag(
    &F, pars->z_end, pars->z_max, 0,
    cosmo->gsl_params.INTEGRATION_EPSREL,
    cosmo->gsl_params.N_ITERATION,
    cosmo->gsl_params.INTEGRATION_GAUSS_KRONROD_POINTS,
    w, &result, &eresult);

  if ((gslstatus != GSL_SUCCESS) || (*(pars->status))) {
    ccl_raise_gsl_warning(gslstatus, "ccl_tracers.c: lensing_kernel_integrate():");
    return -1;
  }

  return result * pars->i_nz_norm * pars->chi_end;
}

//Returns number of divisions on which
//the lensing kernel should be calculated
int ccl_get_nchi_lensing_kernel(int nz, double *z_arr, int *status) {
  int nchi;
  double dz = -1, z_max = -1;
  //Compute redshift step
  dz = (z_arr[nz-1]-z_arr[0])/(nz-1);

  //How many steps to z=0?
  return (int)(z_arr[nz-1]/dz+0.5);
}

//Return array with the values of chi at
//the which the lensing kernel will be
//calculated.
void ccl_get_chis_lensing_kernel(ccl_cosmology *cosmo,
                                 int nchi, double z_max,
                                 double *chis, int *status) {
  double dz = z_max/nchi;
  for(int ichi=0; ichi < nchi; ichi++) {
    double z = dz*ichi+1E-15;
    double a = 1./(1+z);
    chis[ichi] = ccl_comoving_radial_distance(cosmo, a, status);
  }
}

//Returns array with lensing kernel:
//3 * H0^2 * Omega_M / 2 / a *
// Integral[ p(z) * (1-5s(z)/2) * chi_end * (chi(z)-chi_end)/chi(z) ,
//          {z',z_end,z_max} ]
void ccl_get_lensing_mag_kernel(ccl_cosmology *cosmo,
                                int nz, double *z_arr, double *nz_arr,
                                int normalize_nz, double z_max,
                                int nz_s, double *zs_arr, double *sz_arr,
                                int nchi, double *chi_arr, double *wL_arr,
                                int *status) {
  ccl_f1d_t *nz_f = NULL;
  ccl_f1d_t *sz_f = NULL;
  gsl_integration_workspace *w;
  integ_lensing_pars *ipar;
  int local_status;

  // Prepare N(z) spline
  nz_f = ccl_f1d_t_new(nz, z_arr, nz_arr, 0, 0);
  if (nz_f == NULL) {
    *status = CCL_ERROR_SPLINE;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: get_lensing_mag_kernel: error initializing spline\n");
  }

  // Get N(z) normalization
  double i_nz_norm = -1;
  if (*status == 0) {
    if (normalize_nz)
      i_nz_norm = 1./get_nz_norm(cosmo, nz_f, z_arr[0], z_arr[nz-1], status);
    else
      i_nz_norm = 1.;
  }

  // Prepare magnification bias spline if needed
  if (*status == 0) {
    if ((nz_s > 0) && (zs_arr != NULL) && (sz_arr != NULL)) {
      sz_f = ccl_f1d_t_new(nz_s, zs_arr, sz_arr, sz_arr[0], sz_arr[nz_s-1]);
      if (sz_f == NULL) {
        *status = CCL_ERROR_SPLINE;
        ccl_cosmology_set_status_message(
          cosmo,
          "ccl_tracers.c: get_lensing_mag_kernel: error initializing spline\n");
      }
    }
  }

  double lens_prefac = get_lensing_prefactor(cosmo, status);
  double chi, a, z, mgfac;
  int ichi;

  w = NULL;
  ipar = NULL;
  local_status = *status;

  if (local_status == 0) {
    ipar = malloc(sizeof(integ_lensing_pars));
    w = gsl_integration_workspace_alloc(cosmo->gsl_params.N_ITERATION);

    if ((ipar == NULL) || (w == NULL)) {
      local_status = CCL_ERROR_MEMORY;
    }
  }

  if (local_status == 0) {
    ipar->cosmo = cosmo;
    ipar->z_max = z_max;
    ipar->i_nz_norm = i_nz_norm;
    ipar->sz_f = sz_f;
    ipar->nz_f = nz_f;
    ipar->status = &local_status;
  }

  //Populate arrays
  for (ichi=0; ichi < nchi; ichi++) {
    if (local_status == 0) {
      chi = chi_arr[ichi];
      a = ccl_scale_factor_of_chi(cosmo, chi, &local_status);
      z = 1./a-1;
      // Add MG correction if needed
      mgfac = 1.0;
      if (fabs(cosmo->params.sigma_0))
        mgfac += ccl_Sig_MG(cosmo, a, &local_status);
      ipar->z_end = z;
      ipar->chi_end = chi;

      wL_arr[ichi] = lensing_kernel_integrate(cosmo, ipar, w)*(1+z)*lens_prefac*mgfac;
    } else {
      wL_arr[ichi] = NAN;
    }
  }

  gsl_integration_workspace_free(w);
  free(ipar);

  if (local_status != 0) {
    *status = CCL_ERROR_INTEG;
  }

  ccl_f1d_t_free(nz_f);
  ccl_f1d_t_free(sz_f);
}

// Returns kernel for CMB lensing
// 3H0^2Om/2 * chi * (chi_s - chi) / chi_s / a
void ccl_get_kappa_kernel(ccl_cosmology *cosmo, double chi_source,
                          int nchi, double *chi_arr,
                          double *wchi, int *status) {
  double lens_prefac = get_lensing_prefactor(cosmo, status) / ccl_sinn(cosmo, chi_source, status);

  for (int ichi=0; ichi < nchi; ichi++) {
    double chi = chi_arr[ichi];
    double a = ccl_scale_factor_of_chi(cosmo, chi, status);
    double mgfac = 1;
    // Add MG correction if needed
    if (fabs(cosmo->params.sigma_0))
      mgfac += ccl_Sig_MG(cosmo, a, status);
    wchi[ichi] = lens_prefac*(ccl_sinn(cosmo,chi_source-chi,status))*chi*mgfac/a;
  }
}

ccl_cl_tracer_t *ccl_cl_tracer_t_new(ccl_cosmology *cosmo,
                                     int der_bessel,
                                     int der_angles,
                                     int n_w, double *chi_w, double *w_w,
                                     int na_ka, double *a_ka,
                                     int nk_ka, double *lk_ka,
                                     double *fka_arr,
                                     double *fk_arr,
                                     double *fa_arr,
                                     int is_fka_log,
                                     int is_factorizable,
                                     int extrap_order_lok,
                                     int extrap_order_hik,
                                     int *status) {
  ccl_cl_tracer_t *tr = NULL;

  // Check der_bessel and der_angles are sensible
  if ((der_angles < 0) || (der_angles > 2)) {
    *status = CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_cl_tracer_new: der_angles must be between 0 and 2\n");
  }
  if ((der_bessel < -1) || (der_bessel > 2)) {
    *status = CCL_ERROR_INCONSISTENT;
    ccl_cosmology_set_status_message(
      cosmo,
      "ccl_tracers.c: ccl_cl_tracer_new: der_bessel must be between -1 and 2\n");
  }

  if (*status == 0) {
    tr = malloc(sizeof(ccl_cl_tracer_t));
    if (tr == NULL)
      *status = CCL_ERROR_MEMORY;
  }

  // Initialize everythin
  if (*status == 0) {
    tr->der_angles = der_angles;
    tr->der_bessel = der_bessel;
    tr->kernel = NULL; // Initialize these to NULL
    tr->transfer = NULL; // Initialize these to NULL
    tr->chi_min = 0;
    tr->chi_max = 1E15;
  }

  if (*status == 0) {
    // Initialize radial kernel
    if ((n_w > 0) && (chi_w != NULL) && (w_w != NULL)) {
      tr->kernel = ccl_f1d_t_new(n_w,chi_w,w_w,0,0);
      if (tr->kernel == NULL)
        *status=CCL_ERROR_MEMORY;
    }
  }

  // Find kernel edges
  if (*status == 0) {
    // If no radial kernel, set limits to zero and maximum distance
    if (tr->kernel == NULL) {
      tr->chi_min = 0;
      tr->chi_max = ccl_comoving_radial_distance(cosmo, cosmo->spline_params.A_SPLINE_MIN, status);
    }
    else {
      int ichi;
      double w_max = fabs(w_w[0]);

      // Find maximum of radial kernel
      for (ichi=0; ichi < n_w; ichi++) {
        if (fabs(w_w[ichi]) >= w_max)
          w_max = fabs(w_w[ichi]);
      }

      // Multiply by fraction
      w_max *= CCL_FRAC_RELEVANT;

      // Initialize as the original edges in case we don't find an interval
      tr->chi_min = chi_w[0];
      tr->chi_max = chi_w[n_w-1];

      // Find minimum
      for (ichi=0; ichi < n_w; ichi++) {
        if (fabs(w_w[ichi]) >= w_max) {
          tr->chi_min = chi_w[ichi];
          break;
        }
      }

      // Find maximum
      for (ichi=n_w-1; ichi >= 0; ichi--) {
        if (fabs(w_w[ichi]) >= w_max) {
          tr->chi_max = chi_w[ichi];
          break;
        }
      }
    }
  }

  if (*status == 0) {
    if ((fka_arr != NULL) || (fk_arr != NULL) || (fa_arr != NULL)) {
      tr->transfer = ccl_f2d_t_new(
        na_ka,a_ka, // na, a_arr
        nk_ka,lk_ka, // nk, lk_arr
        fka_arr, // fka_arr
        fk_arr, // fk_arr
        fa_arr, // fa_arr
        is_factorizable, // is factorizable
        extrap_order_lok, // extrap_order_lok
        extrap_order_hik, // extrap_order_hik
        ccl_f2d_constantgrowth, // extrap_linear_growth
        is_fka_log, // is_fka_log
        NULL, // growth (function)
        1, // growth_factor_0 -> will assume constant transfer function
        0, // growth_exponent
        ccl_f2d_3, // interp_type
        status);
      if (tr->transfer == NULL)
        *status=CCL_ERROR_MEMORY;
    }
  }

  return tr;
}

void ccl_cl_tracer_t_free(ccl_cl_tracer_t *tr) {
  if (tr != NULL) {
    if (tr->transfer != NULL)
      ccl_f2d_t_free(tr->transfer);
    if (tr->kernel != NULL)
      ccl_f1d_t_free(tr->kernel);
    free(tr);
  }
}

double ccl_cl_tracer_t_get_f_ell(ccl_cl_tracer_t *tr, double ell, int *status) {
  if (tr != NULL) {
    if (tr->der_angles == 1)
      return ell*(ell+1.);
    else if (tr->der_angles == 2) {
      if (ell <= 1) // This is identically 0
        return 0;
      else if (ell <= 10) // Use full expression in this case
        return sqrt((ell+2)*(ell+1)*ell*(ell-1));
      else {
        double lp1h = ell+0.5;
        double lp1h2 = lp1h*lp1h;
        if (ell <= 1000)  // This is accurate to 5E-5 for l>10
          return lp1h2*(1-1.25/lp1h2);
        else // This is accurate to 1E-6 for l>1000
          return lp1h2;
      }
    }
    else
      return 1;
  }
  else
    return 1;
}

double ccl_cl_tracer_t_get_kernel(ccl_cl_tracer_t *tr, double chi, int *status) {
  if (tr != NULL) {
    if (tr->kernel != NULL)
      return ccl_f1d_t_eval(tr->kernel, chi);
    else
      return 1;
  }
  else
    return 1;
}

double ccl_cl_tracer_t_get_transfer(ccl_cl_tracer_t *tr,
                                    double lk, double a, int *status) {
  if (tr != NULL) {
    if (tr->transfer != NULL)
      return ccl_f2d_t_eval(tr->transfer, lk, a, NULL, status);
    else
      return 1;
  }
  else
    return 1;
}


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

#include <mpi.h>

/* Pull in the CBLAS header. */
#ifdef _MACOSX
#include <Accelerate/Accelerate.h>
#else
#ifdef _ATLAS
#include <cblas.h>
#else
#include <gsl_cblas.h>
#endif /* _ATLAS */
#endif /* _MACOSX */

/* These headers are provided by ScaleME. */
#include "ScaleME.h"

#include "precision.h"

#include "mlfma.h"
#include "direct.h"
#include "itsolver.h"
#include "util.h"

int matvec (cplx *out, cplx *in, cplx *cur, int id) {
	long i, nelt = (long)fmaconf.numbases * (long)fmaconf.bspboxvol;

	/* Compute the contrast pressure. */
#pragma omp parallel for default(shared) private(i)
	for (i = 0; i < nelt; ++i) cur[i] = in[i] * fmaconf.contrast[i];

	/* Reset the direct-interaction buffer and compute
	 * the matrix-vector product for the Green's matrix. */
	clrdircache();
	ScaleME_applyParFMA (cur, out);

	if (!id) return 0;

	/* Add in the identity portion. */
#pragma omp parallel for default(shared) private(i)
	for (i = 0; i < nelt; ++i) out[i] = fmaconf.cellvol * in[i] - out[i];

	return 0;
}

int gmres (cplx *rhs, cplx *sol, int guess,
		int mit, real tol, int quiet, augspace *aug) {
	long j, nelt = (long)fmaconf.numbases * (long)fmaconf.bspboxvol, lwork;
	int i, rank, one = 1, mred = mit;
	cplx *h, *v, *mvp, *beta, *vp, *hp, *s, cr,
		cone = 1., czero = 0., *azp, *zp;
	real rhn, err, *c;

	MPI_Comm_rank (MPI_COMM_WORLD, &rank);

	/* Allocate space for all required complex vectors. */
	lwork = (mit + 1) * (mit + nelt + 1) + nelt + mit;
	v = calloc (lwork, sizeof(cplx));	/* The Krylov subspace. */
	beta = v + nelt * (mit + 1);		/* The least-squares RHS. */
	mvp = beta + mit + 1;			/* Buffer for matrix-vector product. */
	h = mvp + nelt;				/* The upper Hessenberg matrix. */
	s = h + (mit + 1) * mit;		/* Givens rotation sines. */

	/* Allocate space for the Givens rotation cosines. */
	c = malloc (mit * sizeof(real));

	/* Compute the norm of the RHS for residual scaling. */
	rhn = parnorm(rhs, nelt);

	/* Compute the initial matrix-vector product for the input guess. */
	if (guess) matvec (v, sol, mvp, 1);

	/* Subtract from the RHS to form the residual. */
#pragma omp parallel for default(shared) private(j)
	for (j = 0; j < nelt; ++j) v[j] = rhs[j] - v[j];

	/* Zero the initial guess if one wasn't provided. */
	if (!guess) memset (sol, 0, nelt * sizeof(cplx));

	/* Find the norm of the initial residual. */
	err = parnorm(v, nelt);

	/* Construct the initial Arnoldi vector by normalizing the residual. */
#pragma omp parallel for default(shared) private(j)
	for (j = 0; j < nelt; ++j) v[j] /= err;

	/* Construct the vector beta for the minimization problem. */
	beta[0] = err;

	/* Report the RRE. */
	err /= rhn;
	if (!rank && !quiet) printf ("True residual: %g\n", err);

	/* The reduced number of iterations, if the space is augmented. */
	if (aug) mred = mit - aug->ntot;

	for (i = 0; i < mit && err > tol; ++i) {
		/* Point to the working space for this iteration. */
		vp = v + i * nelt;
		hp = h + i * (mit + 1);

		/* Compute the next expansion of the Krylov space. */
		if (!aug || i < mred) matvec (vp + nelt, vp, mvp, 1);
		else {
			/* Update with the next augmented vector. */
			azp = aug->az + nelt *
				((aug->nmax + aug->start + mred - i) % aug->nmax);
			/* Use the augmented space. */
			memcpy (vp + nelt, azp, nelt * sizeof(cplx));
		}

		/* Perform modified Gram-Schmidt to orthogonalize the basis. */
		/* This also builds the Hessenberg matrix column, including
		 * the 2-norm of the next basis vector. */
		cmgs (vp + nelt, hp, v, nelt, i + 1);

		/* Watch for breakdown. */
		if (cabs(hp[i + 1]) < REAL_EPSILON) {
			++i;
			break;
		}

		/* Apply previous Givens rotations to the Hessenberg column. */
		for (j = 0; j < i; ++j)
			ROT (&one, hp + j, &one, hp + j + 1, &one, c + j, s + j);

		/* Compute the Givens rotation for the current iteration. */
		LARTG (hp + i, hp + i + 1, c + i, s + i, &cr);
		/* Apply the current Givens rotation to the Hessenberg column. */
		hp[i] = cr;
		hp[i + 1] = 0;
		/* Perform the rotation on the vector beta. */
		ROT (&one, beta + i, &one, beta + i + 1, &one, c + i, s + i);

		/* Estimate the RRE for this iteration. */
		err = cabs(beta[i + 1]) / rhn;
		if (!rank && !quiet) printf ("GMRES(%d): %g\n", i, err);

		/* Flush the output buffers. */
		fflush (stdout);
		fflush (stderr);
	}

	/* If there were any GMRES iterations, update the solution. */
	if (i > 0 && aug) {
		/* Compute the optimum solution in the Krylov basis. */
		cplx *ys;
		ys = malloc(i * sizeof(cplx));
		memcpy (ys, beta, i * sizeof(cplx));
		TRSV (CblasColMajor, CblasUpper, CblasNoTrans,
				CblasNonUnit, i, h, mit + 1, ys, 1);

		/* Compute the next Krylov vector in the augmented space. */
		aug->start = (aug->start + 1) % aug->nmax;
		if (aug->ntot < aug->nmax) ++(aug->ntot);
		azp = aug->az + nelt * aug->start;

		beta[i] = 0;
		for (j = i - 1; j >= 0; --j) {
			/* Invert the Givens rotations. */
			s[j] = -s[j];
			ROT (&one, beta + j, &one, beta + j + 1, &one, c + j, s + j);
		}
		GEMV (CblasColMajor, CblasNoTrans, nelt, i + 1,
				&cone, v, nelt, beta, 1, &czero, azp, 1);

		/* Overwrite the Krylov subspace with the augmented subspace.
		 * The start pointer has already been incremented! */
		for (j = mred; j < i; ++j) {
			zp = aug->z + nelt *
				((aug->nmax + aug->start + mred - j - 1) % aug->nmax);
			memcpy(v + j * nelt, zp, nelt * sizeof(cplx));
		}

		/* Compute the solution update. */
		zp = aug->z + nelt * aug->start;
		GEMV (CblasColMajor, CblasNoTrans, nelt, i,
				&cone, v, nelt, ys, 1, &czero, zp, 1);
		for (j = 0; j < nelt; ++j) sol[j] += zp[j];

		free(ys);
	} else if (i > 0) {
		/* Compute the minimizer of the least-squares problem. */
		TRSV (CblasColMajor, CblasUpper, CblasNoTrans,
				CblasNonUnit, i, h, mit + 1, beta, 1);

		/* Compute the solution update in place. */
		GEMV (CblasColMajor, CblasNoTrans, nelt, i,
				&cone, v, nelt, beta, 1, &cone, sol, 1);
	}

	free (v);
	free (c);

	return i;
}

int bicgstab (cplx *rhs, cplx *sol,
		int guess, int mit, real tol, int quiet) {
	long j, nelt = (long)fmaconf.numbases * (long)fmaconf.bspboxvol;
	int i, rank;
	cplx *r, *rhat, *v, *p, *mvp, *t;
	cplx rho, alpha, omega, beta;
	real err, rhn;

	MPI_Comm_rank (MPI_COMM_WORLD, &rank);

	rho = alpha = omega = 1.;

	/* Allocate and zero the work arrays. */
	r = calloc (6L * nelt, sizeof(cplx));
	rhat = r + nelt;
	v = rhat + nelt;
	p = v + nelt;
	t = p + nelt;
	mvp = t + nelt;

	/* Compute the norm of the right-hand side for residual scaling. */
	rhn = parnorm(rhs, nelt);

	/* Compute the inital matrix-vector product for the input guess. */
	if (guess) matvec (r, sol, mvp, 1);

	/* Subtract from the RHS to form the residual. */
#pragma omp parallel for default(shared) private(j)
	for (j = 0; j < nelt; ++j) r[j] = rhs[j] - r[j];

	if (!guess) memset (sol, 0, nelt * sizeof(cplx));

	/* Copy the initial residual as the test vector. */
	memcpy (rhat, r, nelt * sizeof(cplx));

	/* Find the norm of the initial residual. */
	err = parnorm(r, nelt) / rhn;
	if (!rank && !quiet) printf ("True residual: %g\n", err);

	/* Run iterations until convergence or the maximum is reached. */
	for (i = 0; i < mit && err > tol; ++i) {
		/* Pre-compute portion of beta from previous iteration. */
		beta = alpha / (rho * omega);
		/* Compute rho for this iteration. */
		rho = pardot (rhat, r, nelt);
		/* Include the missing factor in beta. */
		beta *= rho;

		/* Update the search vector. */
#pragma omp parallel for default(shared) private(j)
		for (j = 0; j < nelt; ++j)
			p[j] = r[j] + beta * (p[j] - omega * v[j]);

		/* Compute the first search step, v = A * p. */
		matvec (v, p, mvp, 1);

		/* Compute the next alpha. */
		alpha = rho / pardot (rhat, v, nelt);

#pragma omp parallel for default(shared) private(j)
		for (j = 0; j < nelt; ++j) {
			/* Update the solution vector. */
			sol[j] += alpha * p[j];
			/* Update the residual vector. */
			r[j] -= alpha * v[j];
		}

		/* Compute the scaled residual norm and stop if convergence
		 * has been achieved. */
		err = parnorm(r, nelt) / rhn;
		if (!rank && !quiet) printf ("BiCG-STAB(%0.1f): %g\n", 0.5 + i, err);

		/* Flush the output buffers. */
		fflush (stdout);
		fflush (stderr);

		if (err < tol) break;

		/* Compute the next search step, t = A * r. */
		matvec (t, r, mvp, 1);

		/* Compute the update direction. */
		omega = pardot (t, r, nelt) / pardot (t, t, nelt);

		/* Update both the residual and the solution guess. */
#pragma omp parallel for default(shared) private(j)
		for (j = 0; j < nelt; ++j) {
			/* Update the solution vector. */
			sol[j] += omega * r[j];
			/* Update the residual vector. */
			r[j] -= omega * t[j];
		}

		/* Compute the scaled residual norm. */
		err = parnorm(r, nelt) / rhn;
		if (!rank && !quiet) printf ("BiCG-STAB(%d): %g\n", i + 1, err);

		fflush (stdout);
		fflush (stderr);
	}

	free (r);
	return i;
}