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

/* Author:  G. Jungman */

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

#include "error.h"

#define locEPS (1000.0*GSL_DBL_EPSILON)

/* Assumes c != negative integer.
 */
static int
hyperg_2F1_series(const double a, const double b, const double c,
                  const double x,
                  gsl_sf_result * result
                  )
{
  double sum_pos = 1.0;
  double sum_neg = 0.0;
  double del_pos = 1.0;
  double del_neg = 0.0;
  double del = 1.0;
  double k = 0.0;
  int i = 0;

  if(fabs(c) < GSL_DBL_EPSILON) {
    result->val = 0.0; /* FIXME: ?? */
    result->err = 1.0;
    GSL_ERROR ("error", GSL_EDOM);
  }

  do {
    if(++i > 30000) {
      result->val  = sum_pos - sum_neg;
      result->err  = del_pos + del_neg;
      result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
      result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k)+1.0) * fabs(result->val);
      GSL_ERROR ("error", GSL_EMAXITER);
    }
    del *= (a+k)*(b+k) * x / ((c+k) * (k+1.0));  /* Gauss series */

    if(del > 0.0) {
      del_pos  =  del;
      sum_pos +=  del;
    }
    else if(del == 0.0) {
      /* Exact termination (a or b was a negative integer).
       */
      del_pos = 0.0;
      del_neg = 0.0;
      break;
    }
    else {
      del_neg  = -del;
      sum_neg -=  del;
    }

    k += 1.0;
  } while(fabs((del_pos + del_neg)/(sum_pos-sum_neg)) > GSL_DBL_EPSILON);

  result->val  = sum_pos - sum_neg;
  result->err  = del_pos + del_neg;
  result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
  result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k) + 1.0) * fabs(result->val);

  return GSL_SUCCESS;
}

/* a = aR + i aI, b = aR - i aI */
static
int
hyperg_2F1_conj_series(const double aR, const double aI, const double c,
                       double x,
                       gsl_sf_result * result)
{
  if(c == 0.0) {
    result->val = 0.0; /* FIXME: should be Inf */
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EDOM);
  }
  else {
    double sum_pos = 1.0;
    double sum_neg = 0.0;
    double del_pos = 1.0;
    double del_neg = 0.0;
    double del = 1.0;
    double k = 0.0;
    do {
      del *= ((aR+k)*(aR+k) + aI*aI)/((k+1.0)*(c+k)) * x;

      if(del >= 0.0) {
        del_pos  =  del;
        sum_pos +=  del;
      }
      else {
        del_neg  = -del;
        sum_neg -=  del;
      }

      if(k > 30000) {
        result->val  = sum_pos - sum_neg;
        result->err  = del_pos + del_neg;
        result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
        result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k)+1.0) * fabs(result->val);
        GSL_ERROR ("error", GSL_EMAXITER);
      }

      k += 1.0;
    } while(fabs((del_pos + del_neg)/(sum_pos - sum_neg)) > GSL_DBL_EPSILON);

    result->val  = sum_pos - sum_neg;
    result->err  = del_pos + del_neg;
    result->err += 2.0 * GSL_DBL_EPSILON * (sum_pos + sum_neg);
    result->err += 2.0 * GSL_DBL_EPSILON * (2.0*sqrt(k) + 1.0) * fabs(result->val);

    return GSL_SUCCESS;
  }
}

/* Luke's rational approximation. The most accesible
 * discussion is in [Kolbig, CPC 23, 51 (1981)].
 * The convergence is supposedly guaranteed for x < 0.
 * You have to read Luke's books to see this and other
 * results. Unfortunately, the stability is not so
 * clear to me, although it seems very efficient when
 * it works.
 */
static
int
hyperg_2F1_luke(const double a, const double b, const double c,
                const double xin,
                gsl_sf_result * result)
{
  int stat_iter;
  const double RECUR_BIG = 1.0e+50;
  const int nmax = 20000;
  int n = 3;
  const double x  = -xin;
  const double x3 = x*x*x;
  const double t0 = a*b/c;
  const double t1 = (a+1.0)*(b+1.0)/(2.0*c);
  const double t2 = (a+2.0)*(b+2.0)/(2.0*(c+1.0));
  double F = 1.0;
  double prec;

  double Bnm3 = 1.0;                                  /* B0 */
  double Bnm2 = 1.0 + t1 * x;                         /* B1 */
  double Bnm1 = 1.0 + t2 * x * (1.0 + t1/3.0 * x);    /* B2 */

  double Anm3 = 1.0;                                                      /* A0 */
  double Anm2 = Bnm2 - t0 * x;                                            /* A1 */
  double Anm1 = Bnm1 - t0*(1.0 + t2*x)*x + t0 * t1 * (c/(c+1.0)) * x*x;   /* A2 */

  while(1) {
    double npam1 = n + a - 1;
    double npbm1 = n + b - 1;
    double npcm1 = n + c - 1;
    double npam2 = n + a - 2;
    double npbm2 = n + b - 2;
    double npcm2 = n + c - 2;
    double tnm1  = 2*n - 1;
    double tnm3  = 2*n - 3;
    double tnm5  = 2*n - 5;
    double n2 = n*n;
    double F1 =  (3.0*n2 + (a+b-6)*n + 2 - a*b - 2*(a+b)) / (2*tnm3*npcm1);
    double F2 = -(3.0*n2 - (a+b+6)*n + 2 - a*b)*npam1*npbm1/(4*tnm1*tnm3*npcm2*npcm1);
    double F3 = (npam2*npam1*npbm2*npbm1*(n-a-2)*(n-b-2)) / (8*tnm3*tnm3*tnm5*(n+c-3)*npcm2*npcm1);
    double E  = -npam1*npbm1*(n-c-1) / (2*tnm3*npcm2*npcm1);

    double An = (1.0+F1*x)*Anm1 + (E + F2*x)*x*Anm2 + F3*x3*Anm3;
    double Bn = (1.0+F1*x)*Bnm1 + (E + F2*x)*x*Bnm2 + F3*x3*Bnm3;
    double r = An/Bn;

    prec = fabs((F - r)/F);
    F = r;

    if(prec < GSL_DBL_EPSILON || n > nmax) break;

    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;
      Anm3 /= RECUR_BIG;
      Bnm3 /= RECUR_BIG;
    }
    else if(fabs(An) < 1.0/RECUR_BIG || fabs(Bn) < 1.0/RECUR_BIG) {
      An   *= RECUR_BIG;
      Bn   *= RECUR_BIG;
      Anm1 *= RECUR_BIG;
      Bnm1 *= RECUR_BIG;
      Anm2 *= RECUR_BIG;
      Bnm2 *= RECUR_BIG;
      Anm3 *= RECUR_BIG;
      Bnm3 *= RECUR_BIG;
    }

    n++;
    Bnm3 = Bnm2;
    Bnm2 = Bnm1;
    Bnm1 = Bn;
    Anm3 = Anm2;
    Anm2 = Anm1;
    Anm1 = An;
  }

  result->val  = F;
  result->err  = 2.0 * fabs(prec * F);
  result->err += 2.0 * GSL_DBL_EPSILON * (n+1.0) * fabs(F);

  /* FIXME: just a hack: there's a lot of shit going on here */
  result->err *= 8.0 * (fabs(a) + fabs(b) + 1.0);

  stat_iter = (n >= nmax ? GSL_EMAXITER : GSL_SUCCESS );

  return stat_iter;
}

/* Luke's rational approximation for the
 * case a = aR + i aI, b = aR - i aI.
 */
static
int
hyperg_2F1_conj_luke(const double aR, const double aI, const double c,
                     const double xin,
                     gsl_sf_result * result)
{
  int stat_iter;
  const double RECUR_BIG = 1.0e+50;
  const int nmax = 10000;
  int n = 3;
  const double x = -xin;
  const double x3 = x*x*x;
  const double atimesb = aR*aR + aI*aI;
  const double apb     = 2.0*aR;
  const double t0 = atimesb/c;
  const double t1 = (atimesb +     apb + 1.0)/(2.0*c);
  const double t2 = (atimesb + 2.0*apb + 4.0)/(2.0*(c+1.0));
  double F = 1.0;
  double prec;

  double Bnm3 = 1.0;                                  /* B0 */
  double Bnm2 = 1.0 + t1 * x;                         /* B1 */
  double Bnm1 = 1.0 + t2 * x * (1.0 + t1/3.0 * x);    /* B2 */

  double Anm3 = 1.0;                                                      /* A0 */
  double Anm2 = Bnm2 - t0 * x;                                            /* A1 */
  double Anm1 = Bnm1 - t0*(1.0 + t2*x)*x + t0 * t1 * (c/(c+1.0)) * x*x;   /* A2 */

  while(1) {
    double nm1 = n - 1;
    double nm2 = n - 2;
    double npam1_npbm1 = atimesb + nm1*apb + nm1*nm1;
    double npam2_npbm2 = atimesb + nm2*apb + nm2*nm2;
    double npcm1 = nm1 + c;
    double npcm2 = nm2 + c;
    double tnm1  = 2*n - 1;
    double tnm3  = 2*n - 3;
    double tnm5  = 2*n - 5;
    double n2 = n*n;
    double F1 =  (3.0*n2 + (apb-6)*n + 2 - atimesb - 2*apb) / (2*tnm3*npcm1);
    double F2 = -(3.0*n2 - (apb+6)*n + 2 - atimesb)*npam1_npbm1/(4*tnm1*tnm3*npcm2*npcm1);
    double F3 = (npam2_npbm2*npam1_npbm1*(nm2*nm2 - nm2*apb + atimesb)) / (8*tnm3*tnm3*tnm5*(n+c-3)*npcm2*npcm1);
    double E  = -npam1_npbm1*(n-c-1) / (2*tnm3*npcm2*npcm1);

    double An = (1.0+F1*x)*Anm1 + (E + F2*x)*x*Anm2 + F3*x3*Anm3;
    double Bn = (1.0+F1*x)*Bnm1 + (E + F2*x)*x*Bnm2 + F3*x3*Bnm3;
    double r = An/Bn;

    prec = fabs(F - r)/fabs(F);
    F = r;

    if(prec < GSL_DBL_EPSILON || n > nmax) break;

    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;
      Anm3 /= RECUR_BIG;
      Bnm3 /= RECUR_BIG;
    }
    else if(fabs(An) < 1.0/RECUR_BIG || fabs(Bn) < 1.0/RECUR_BIG) {
      An   *= RECUR_BIG;
      Bn   *= RECUR_BIG;
      Anm1 *= RECUR_BIG;
      Bnm1 *= RECUR_BIG;
      Anm2 *= RECUR_BIG;
      Bnm2 *= RECUR_BIG;
      Anm3 *= RECUR_BIG;
      Bnm3 *= RECUR_BIG;
    }

    n++;
    Bnm3 = Bnm2;
    Bnm2 = Bnm1;
    Bnm1 = Bn;
    Anm3 = Anm2;
    Anm2 = Anm1;
    Anm1 = An;
  }

  result->val  = F;
  result->err  = 2.0 * fabs(prec * F);
  result->err += 2.0 * GSL_DBL_EPSILON * (n+1.0) * fabs(F);

  /* FIXME: see above */
  result->err *= 8.0 * (fabs(aR) + fabs(aI) + 1.0);

  stat_iter = (n >= nmax ? GSL_EMAXITER : GSL_SUCCESS );

  return stat_iter;
}

/* Do the reflection described in [Moshier, p. 334].
 * Assumes a,b,c != neg integer.
 */
static
int
hyperg_2F1_reflect(const double a, const double b, const double c,
                   const double x, gsl_sf_result * result)
{
  const double d = c - a - b;
  const int intd  = floor(d+0.5);
  const int d_integer = ( fabs(d - intd) < locEPS );

  if(d_integer) {
    const double ln_omx = log(1.0 - x);
    const double ad = fabs(d);
    int stat_F2 = GSL_SUCCESS;
    double sgn_2;
    gsl_sf_result F1;
    gsl_sf_result F2;
    double d1, d2;
    gsl_sf_result lng_c;
    gsl_sf_result lng_ad2;
    gsl_sf_result lng_bd2;
    int stat_c;
    int stat_ad2;
    int stat_bd2;

    if(d >= 0.0) {
      d1 = d;
      d2 = 0.0;
    }
    else {
      d1 = 0.0;
      d2 = d;
    }

    stat_ad2 = gsl_sf_lngamma_e(a+d2, &lng_ad2);
    stat_bd2 = gsl_sf_lngamma_e(b+d2, &lng_bd2);
    stat_c   = gsl_sf_lngamma_e(c,    &lng_c);

    /* Evaluate F1.
     */
    if(ad < GSL_DBL_EPSILON) {
      /* d = 0 */
      F1.val = 0.0;
      F1.err = 0.0;
    }
    else {
      gsl_sf_result lng_ad;
      gsl_sf_result lng_ad1;
      gsl_sf_result lng_bd1;
      int stat_ad  = gsl_sf_lngamma_e(ad,   &lng_ad);
      int stat_ad1 = gsl_sf_lngamma_e(a+d1, &lng_ad1);
      int stat_bd1 = gsl_sf_lngamma_e(b+d1, &lng_bd1);

      if(stat_ad1 == GSL_SUCCESS && stat_bd1 == GSL_SUCCESS && stat_ad == GSL_SUCCESS) {
        /* Gamma functions in the denominator are ok.
         * Proceed with evaluation.
         */
        int i;
        double sum1 = 1.0;
        double term = 1.0;
        double ln_pre1_val = lng_ad.val + lng_c.val + d2*ln_omx - lng_ad1.val - lng_bd1.val;
        double ln_pre1_err = lng_ad.err + lng_c.err + lng_ad1.err + lng_bd1.err + GSL_DBL_EPSILON * fabs(ln_pre1_val);
        int stat_e;

        /* Do F1 sum.
         */
        for(i=1; i<ad; i++) {
          int j = i-1;
          term *= (a + d2 + j) * (b + d2 + j) / (1.0 + d2 + j) / i * (1.0-x);
          sum1 += term;
        }

        stat_e = gsl_sf_exp_mult_err_e(ln_pre1_val, ln_pre1_err,
                                       sum1, GSL_DBL_EPSILON*fabs(sum1),
                                       &F1);
        if(stat_e == GSL_EOVRFLW) {
          OVERFLOW_ERROR(result);
        }
      }
      else {
        /* Gamma functions in the denominator were not ok.
         * So the F1 term is zero.
         */
        F1.val = 0.0;
        F1.err = 0.0;
      }
    } /* end F1 evaluation */

    /* Evaluate F2.
     */
    if(stat_ad2 == GSL_SUCCESS && stat_bd2 == GSL_SUCCESS) {
      /* Gamma functions in the denominator are ok.
       * Proceed with evaluation.
       */
      const int maxiter = 2000;
      double psi_1 = -M_EULER;
      gsl_sf_result psi_1pd;
      gsl_sf_result psi_apd1;
      gsl_sf_result psi_bpd1;
      int stat_1pd  = gsl_sf_psi_e(1.0 + ad, &psi_1pd);
      int stat_apd1 = gsl_sf_psi_e(a + d1,   &psi_apd1);
      int stat_bpd1 = gsl_sf_psi_e(b + d1,   &psi_bpd1);
      int stat_dall = GSL_ERROR_SELECT_3(stat_1pd, stat_apd1, stat_bpd1);

      double psi_val = psi_1 + psi_1pd.val - psi_apd1.val - psi_bpd1.val - ln_omx;
      double psi_err = psi_1pd.err + psi_apd1.err + psi_bpd1.err + GSL_DBL_EPSILON*fabs(psi_val);
      double fact = 1.0;
      double sum2_val = psi_val;
      double sum2_err = psi_err;
      double ln_pre2_val = lng_c.val + d1*ln_omx - lng_ad2.val - lng_bd2.val;
      double ln_pre2_err = lng_c.err + lng_ad2.err + lng_bd2.err + GSL_DBL_EPSILON * fabs(ln_pre2_val);
      int stat_e;

      int j;

      /* Do F2 sum.
       */
      for(j=1; j<maxiter; j++) {
        /* values for psi functions use recurrence; Abramowitz+Stegun 6.3.5 */
        double term1 = 1.0/(double)j  + 1.0/(ad+j);
        double term2 = 1.0/(a+d1+j-1.0) + 1.0/(b+d1+j-1.0);
        double delta = 0.0;
        psi_val += term1 - term2;
        psi_err += GSL_DBL_EPSILON * (fabs(term1) + fabs(term2));
        fact *= (a+d1+j-1.0)*(b+d1+j-1.0)/((ad+j)*j) * (1.0-x);
        delta = fact * psi_val;
        sum2_val += delta;
        sum2_err += fabs(fact * psi_err) + GSL_DBL_EPSILON*fabs(delta);
        if(fabs(delta) < GSL_DBL_EPSILON * fabs(sum2_val)) break;
      }

      if(j == maxiter) stat_F2 = GSL_EMAXITER;

      if(sum2_val == 0.0) {
        F2.val = 0.0;
        F2.err = 0.0;
      }
      else {
        stat_e = gsl_sf_exp_mult_err_e(ln_pre2_val, ln_pre2_err,
                                       sum2_val, sum2_err,
                                       &F2);
        if(stat_e == GSL_EOVRFLW) {
          result->val = 0.0;
          result->err = 0.0;
          GSL_ERROR ("error", GSL_EOVRFLW);
        }
      }
      stat_F2 = GSL_ERROR_SELECT_2(stat_F2, stat_dall);
    }
    else {
      /* Gamma functions in the denominator not ok.
       * So the F2 term is zero.
       */
      F2.val = 0.0;
      F2.err = 0.0;
    } /* end F2 evaluation */

    sgn_2 = ( GSL_IS_ODD(intd) ? -1.0 : 1.0 );
    result->val  = F1.val + sgn_2 * F2.val;
    result->err  = F1.err + F2. err;
    result->err += 2.0 * GSL_DBL_EPSILON * (fabs(F1.val) + fabs(F2.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return stat_F2;
  }
  else {
    /* d not an integer */

    gsl_sf_result pre1, pre2;
    double sgn1, sgn2;
    gsl_sf_result F1, F2;
    int status_F1, status_F2;

    /* These gamma functions appear in the denominator, so we
     * catch their harmless domain errors and set the terms to zero.
     */
    gsl_sf_result ln_g1ca,  ln_g1cb,  ln_g2a,  ln_g2b;
    double sgn_g1ca, sgn_g1cb, sgn_g2a, sgn_g2b;
    int stat_1ca = gsl_sf_lngamma_sgn_e(c-a, &ln_g1ca, &sgn_g1ca);
    int stat_1cb = gsl_sf_lngamma_sgn_e(c-b, &ln_g1cb, &sgn_g1cb);
    int stat_2a  = gsl_sf_lngamma_sgn_e(a, &ln_g2a, &sgn_g2a);
    int stat_2b  = gsl_sf_lngamma_sgn_e(b, &ln_g2b, &sgn_g2b);
    int ok1 = (stat_1ca == GSL_SUCCESS && stat_1cb == GSL_SUCCESS);
    int ok2 = (stat_2a  == GSL_SUCCESS && stat_2b  == GSL_SUCCESS);

    gsl_sf_result ln_gc,  ln_gd,  ln_gmd;
    double sgn_gc, sgn_gd, sgn_gmd;
    gsl_sf_lngamma_sgn_e( c, &ln_gc,  &sgn_gc);
    gsl_sf_lngamma_sgn_e( d, &ln_gd,  &sgn_gd);
    gsl_sf_lngamma_sgn_e(-d, &ln_gmd, &sgn_gmd);

    sgn1 = sgn_gc * sgn_gd  * sgn_g1ca * sgn_g1cb;
    sgn2 = sgn_gc * sgn_gmd * sgn_g2a  * sgn_g2b;

    if(ok1 && ok2) {
      double ln_pre1_val = ln_gc.val + ln_gd.val  - ln_g1ca.val - ln_g1cb.val;
      double ln_pre2_val = ln_gc.val + ln_gmd.val - ln_g2a.val  - ln_g2b.val + d*log(1.0-x);
      double ln_pre1_err = ln_gc.err + ln_gd.err + ln_g1ca.err + ln_g1cb.err;
      double ln_pre2_err = ln_gc.err + ln_gmd.err + ln_g2a.err  + ln_g2b.err;
      if(ln_pre1_val < GSL_LOG_DBL_MAX && ln_pre2_val < GSL_LOG_DBL_MAX) {
        gsl_sf_exp_err_e(ln_pre1_val, ln_pre1_err, &pre1);
        gsl_sf_exp_err_e(ln_pre2_val, ln_pre2_err, &pre2);
        pre1.val *= sgn1;
        pre2.val *= sgn2;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else if(ok1 && !ok2) {
      double ln_pre1_val = ln_gc.val + ln_gd.val - ln_g1ca.val - ln_g1cb.val;
      double ln_pre1_err = ln_gc.err + ln_gd.err + ln_g1ca.err + ln_g1cb.err;
      if(ln_pre1_val < GSL_LOG_DBL_MAX) {
        gsl_sf_exp_err_e(ln_pre1_val, ln_pre1_err, &pre1);
        pre1.val *= sgn1;
        pre2.val = 0.0;
        pre2.err = 0.0;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else if(!ok1 && ok2) {
      double ln_pre2_val = ln_gc.val + ln_gmd.val - ln_g2a.val - ln_g2b.val + d*log(1.0-x);
      double ln_pre2_err = ln_gc.err + ln_gmd.err + ln_g2a.err + ln_g2b.err;
      if(ln_pre2_val < GSL_LOG_DBL_MAX) {
        pre1.val = 0.0;
        pre1.err = 0.0;
        gsl_sf_exp_err_e(ln_pre2_val, ln_pre2_err, &pre2);
        pre2.val *= sgn2;
      }
      else {
        OVERFLOW_ERROR(result);
      }
    }
    else {
      pre1.val = 0.0;
      pre2.val = 0.0;
      UNDERFLOW_ERROR(result);
    }

    status_F1 = hyperg_2F1_series(  a,   b, 1.0-d, 1.0-x, &F1);
    status_F2 = hyperg_2F1_series(c-a, c-b, 1.0+d, 1.0-x, &F2);

    result->val  = pre1.val*F1.val + pre2.val*F2.val;
    result->err  = fabs(pre1.val*F1.err) + fabs(pre2.val*F2.err);
    result->err += fabs(pre1.err*F1.val) + fabs(pre2.err*F2.val);
    result->err += 2.0 * GSL_DBL_EPSILON * (fabs(pre1.val*F1.val) + fabs(pre2.val*F2.val));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);

    return GSL_SUCCESS;
  }
}

static int pow_omx(const double x, const double p, gsl_sf_result * result)
{
  double ln_omx;
  double ln_result;
  if(fabs(x) < GSL_ROOT5_DBL_EPSILON) {
    ln_omx = -x*(1.0 + x*(1.0/2.0 + x*(1.0/3.0 + x/4.0 + x*x/5.0)));
  }
  else {
    ln_omx = log(1.0-x);
  }
  ln_result = p * ln_omx;
  return gsl_sf_exp_err_e(ln_result, GSL_DBL_EPSILON * fabs(ln_result), result);
}

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

int
gsl_sf_hyperg_2F1_e(double a, double b, const double c,
                       const double x,
                       gsl_sf_result * result)
{
  const double d = c - a - b;
  const double rinta = floor(a + 0.5);
  const double rintb = floor(b + 0.5);
  const double rintc = floor(c + 0.5);
  const int a_neg_integer = ( a < 0.0  &&  fabs(a - rinta) < locEPS );
  const int b_neg_integer = ( b < 0.0  &&  fabs(b - rintb) < locEPS );
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  result->val = 0.0;
  result->err = 0.0;

   /* Handle x == 1.0 RJM */

  if (fabs (x - 1.0) < locEPS && (c - a - b) > 0 && c != 0 && !c_neg_integer) {
    gsl_sf_result lngamc, lngamcab, lngamca, lngamcb;
    double lngamc_sgn, lngamca_sgn, lngamcb_sgn;
    int status;
    int stat1 = gsl_sf_lngamma_sgn_e (c, &lngamc, &lngamc_sgn);
    int stat2 = gsl_sf_lngamma_e (c - a - b, &lngamcab);
    int stat3 = gsl_sf_lngamma_sgn_e (c - a, &lngamca, &lngamca_sgn);
    int stat4 = gsl_sf_lngamma_sgn_e (c - b, &lngamcb, &lngamcb_sgn);

    if (stat1 != GSL_SUCCESS || stat2 != GSL_SUCCESS
        || stat3 != GSL_SUCCESS || stat4 != GSL_SUCCESS)
      {
        DOMAIN_ERROR (result);
      }

    status =
      gsl_sf_exp_err_e (lngamc.val + lngamcab.val - lngamca.val - lngamcb.val,
                        lngamc.err + lngamcab.err + lngamca.err + lngamcb.err,
                        result);

    result->val *= lngamc_sgn / (lngamca_sgn * lngamcb_sgn);
      return status;
  }

  if(x < -1.0 || 1.0 <= x) {
    DOMAIN_ERROR(result);
  }

  if(c_neg_integer) {
    /* If c is a negative integer, then either a or b must be a
       negative integer of smaller magnitude than c to ensure
       cancellation of the series. */
    if(! (a_neg_integer && a > c + 0.1) && ! (b_neg_integer && b > c + 0.1)) {
      DOMAIN_ERROR(result);
    }
  }

  if(fabs(c-b) < locEPS || fabs(c-a) < locEPS) {
    return pow_omx(x, d, result);  /* (1-x)^(c-a-b) */
  }

  if(a >= 0.0 && b >= 0.0 && c >=0.0 && x >= 0.0 && x < 0.995) {
    /* Series has all positive definite
     * terms and x is not close to 1.
     */
    return hyperg_2F1_series(a, b, c, x, result);
  }

  if(fabs(a) < 10.0 && fabs(b) < 10.0) {
    /* a and b are not too large, so we attempt
     * variations on the series summation.
     */
    if(a_neg_integer) {
      return hyperg_2F1_series(rinta, b, c, x, result);
    }
    if(b_neg_integer) {
      return hyperg_2F1_series(a, rintb, c, x, result);
    }

    if(x < -0.25) {
      return hyperg_2F1_luke(a, b, c, x, result);
    }
    else if(x < 0.5) {
      return hyperg_2F1_series(a, b, c, x, result);
    }
    else {
      if(fabs(c) > 10.0) {
        return hyperg_2F1_series(a, b, c, x, result);
      }
      else {
        return hyperg_2F1_reflect(a, b, c, x, result);
      }
    }
  }
  else {
    /* Either a or b or both large.
     * Introduce some new variables ap,bp so that bp is
     * the larger in magnitude.
     */
    double ap, bp;
    if(fabs(a) > fabs(b)) {
      bp = a;
      ap = b;
    }
    else {
      bp = b;
      ap = a;
    }

    if(x < 0.0) {
      /* What the hell, maybe Luke will converge.
       */
      return hyperg_2F1_luke(a, b, c, x, result);
    }

    if(GSL_MAX_DBL(fabs(ap),1.0)*fabs(bp)*fabs(x) < 2.0*fabs(c)) {
      /* If c is large enough or x is small enough,
       * we can attempt the series anyway.
       */
      return hyperg_2F1_series(a, b, c, x, result);
    }

    if(fabs(bp*bp*x*x) < 0.001*fabs(bp) && fabs(ap) < 10.0) {
      /* The famous but nearly worthless "large b" asymptotic.
       */
      int stat = gsl_sf_hyperg_1F1_e(ap, c, bp*x, result);
      result->err = 0.001 * fabs(result->val);
      return stat;
    }

    /* We give up. */
    result->val = 0.0;
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EUNIMPL);
  }
}

int
gsl_sf_hyperg_2F1_conj_e(const double aR, const double aI, const double c,
                            const double x,
                            gsl_sf_result * result)
{
  const double ax = fabs(x);
  const double rintc = floor(c + 0.5);
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  result->val = 0.0;
  result->err = 0.0;

  if(ax >= 1.0 || c_neg_integer || c == 0.0) {
    DOMAIN_ERROR(result);
  }

  if(   (ax < 0.25 && fabs(aR) < 20.0 && fabs(aI) < 20.0)
     || (c > 0.0 && x > 0.0)
    ) {
    return hyperg_2F1_conj_series(aR, aI, c, x, result);
  }
  else if(fabs(aR) < 10.0 && fabs(aI) < 10.0) {
    if(x < -0.25) {
      return hyperg_2F1_conj_luke(aR, aI, c, x, result);
    }
    else {
      return hyperg_2F1_conj_series(aR, aI, c, x, result);
    }
  }
  else {
    if(x < 0.0) {
      /* What the hell, maybe Luke will converge.
       */
      return hyperg_2F1_conj_luke(aR, aI, c, x, result);
    }

    /* Give up. */
    result->val = 0.0;
    result->err = 0.0;
    GSL_ERROR ("error", GSL_EUNIMPL);
  }
}

int
gsl_sf_hyperg_2F1_renorm_e(const double a, const double b, const double c,
                              const double x,
                              gsl_sf_result * result
                              )
{
  const double rinta = floor(a + 0.5);
  const double rintb = floor(b + 0.5);
  const double rintc = floor(c + 0.5);
  const int a_neg_integer = ( a < 0.0  &&  fabs(a - rinta) < locEPS );
  const int b_neg_integer = ( b < 0.0  &&  fabs(b - rintb) < locEPS );
  const int c_neg_integer = ( c < 0.0  &&  fabs(c - rintc) < locEPS );

  if(c_neg_integer) {
    if((a_neg_integer && a > c+0.1) || (b_neg_integer && b > c+0.1)) {
      /* 2F1 terminates early */
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else {
      /* 2F1 does not terminate early enough, so something survives */
      /* [Abramowitz+Stegun, 15.1.2] */
      gsl_sf_result g1, g2, g3, g4, g5;
      double s1, s2, s3, s4, s5;
      int stat = 0;
      stat += gsl_sf_lngamma_sgn_e(a-c+1, &g1, &s1);
      stat += gsl_sf_lngamma_sgn_e(b-c+1, &g2, &s2);
      stat += gsl_sf_lngamma_sgn_e(a, &g3, &s3);
      stat += gsl_sf_lngamma_sgn_e(b, &g4, &s4);
      stat += gsl_sf_lngamma_sgn_e(-c+2, &g5, &s5);
      if(stat != 0) {
        DOMAIN_ERROR(result);
      }
      else {
        gsl_sf_result F;
        int stat_F = gsl_sf_hyperg_2F1_e(a-c+1, b-c+1, -c+2, x, &F);
        double ln_pre_val = g1.val + g2.val - g3.val - g4.val - g5.val;
        double ln_pre_err = g1.err + g2.err + g3.err + g4.err + g5.err;
        double sg  = s1 * s2 * s3 * s4 * s5;
        int stat_e = gsl_sf_exp_mult_err_e(ln_pre_val, ln_pre_err,
                                              sg * F.val, F.err,
                                              result);
        return GSL_ERROR_SELECT_2(stat_e, stat_F);
      }
    }
  }
  else {
    /* generic c */
    gsl_sf_result F;
    gsl_sf_result lng;
    double sgn;
    int stat_g = gsl_sf_lngamma_sgn_e(c, &lng, &sgn);
    int stat_F = gsl_sf_hyperg_2F1_e(a, b, c, x, &F);
    int stat_e = gsl_sf_exp_mult_err_e(-lng.val, lng.err,
                                          sgn*F.val, F.err,
                                          result);
    return GSL_ERROR_SELECT_3(stat_e, stat_F, stat_g);
  }
}

int
gsl_sf_hyperg_2F1_conj_renorm_e(const double aR, const double aI, const double c,
                                   const double x,
                                   gsl_sf_result * result
                                   )
{
  const double rintc = floor(c  + 0.5);
  const double rinta = floor(aR + 0.5);
  const int a_neg_integer = ( aR < 0.0 && fabs(aR-rinta) < locEPS && aI == 0.0);
  const int c_neg_integer = (  c < 0.0 && fabs(c - rintc) < locEPS );

  if(c_neg_integer) {
    if(a_neg_integer && aR > c+0.1) {
      /* 2F1 terminates early */
      result->val = 0.0;
      result->err = 0.0;
      return GSL_SUCCESS;
    }
    else {
      /* 2F1 does not terminate early enough, so something survives */
      /* [Abramowitz+Stegun, 15.1.2] */
      gsl_sf_result g1, g2;
      gsl_sf_result g3;
      gsl_sf_result a1, a2;
      int stat = 0;
      stat += gsl_sf_lngamma_complex_e(aR-c+1, aI, &g1, &a1);
      stat += gsl_sf_lngamma_complex_e(aR, aI, &g2, &a2);
      stat += gsl_sf_lngamma_e(-c+2.0, &g3);
      if(stat != 0) {
        DOMAIN_ERROR(result);
      }
      else {
        gsl_sf_result F;
        int stat_F = gsl_sf_hyperg_2F1_conj_e(aR-c+1, aI, -c+2, x, &F);
        double ln_pre_val = 2.0*(g1.val - g2.val) - g3.val;
        double ln_pre_err = 2.0 * (g1.err + g2.err) + g3.err;
        int stat_e = gsl_sf_exp_mult_err_e(ln_pre_val, ln_pre_err,
                                              F.val, F.err,
                                              result);
        return GSL_ERROR_SELECT_2(stat_e, stat_F);
      }
    }
  }
  else {
    /* generic c */
    gsl_sf_result F;
    gsl_sf_result lng;
    double sgn;
    int stat_g = gsl_sf_lngamma_sgn_e(c, &lng, &sgn);
    int stat_F = gsl_sf_hyperg_2F1_conj_e(aR, aI, c, x, &F);
    int stat_e = gsl_sf_exp_mult_err_e(-lng.val, lng.err,
                                          sgn*F.val, F.err,
                                          result);
    return GSL_ERROR_SELECT_3(stat_e, stat_F, stat_g);
  }
}

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

#include "eval.h"

double gsl_sf_hyperg_2F1(double a, double b, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_e(a, b, c, x, &result));
}

double gsl_sf_hyperg_2F1_conj(double aR, double aI, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_conj_e(aR, aI, c, x, &result));
}

double gsl_sf_hyperg_2F1_renorm(double a, double b, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_renorm_e(a, b, c, x, &result));
}

double gsl_sf_hyperg_2F1_conj_renorm(double aR, double aI, double c, double x)
{
  EVAL_RESULT(gsl_sf_hyperg_2F1_conj_renorm_e(aR, aI, c, x, &result));
}


/* Copyright (c) 2014, Giuseppe Argentieri <giuseppe.argentieri@ts.infn.it>

 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 */
/*
 *
 *
 *       Filename:  polar.c
 *
 *    Description:  Cartesian <-> Polar coordinates transformation
 *
 *        Version:  1.0
 *        Created:  06/05/2014 14:21:40
 *       Revision:  none
 *        License:  BSD
 *
 *         Author:  Giuseppe Argentieri (ga), giuseppe.argentieri@ts.infn.it
 *   Organization:  Università degli Studi di Trieste
 *
 *
 */

#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include "funcs.h"

/*
 *      FUNCTION
 *         Name:  bloch_vector
 *  Description:  Return the Bloch vector corresponding to (r,theta,phi) polar coords
 *
 */
int bloch_vector ( gsl_vector* v, double r, double theta, double phi )
{
	/* Check the validity of coordinates */
	if ( r > 1 )                   /* Non physical state */
	{
		v = NULL ;
		return -1;
	}
	theta = fmod (theta, M_PI ) ;          /* Taking the modulo */
	phi   = fmod ( phi, 2*M_PI )  ;

	gsl_vector_set ( v, 1, r*sin(theta)*cos(phi) ) ;
	gsl_vector_set ( v, 2, r*sin(theta)*sin(phi) ) ;
	gsl_vector_set ( v, 3, r*cos(theta) ) ;

	return 0;
}		/* -----  end of function bloch_vector  ----- */

/*
 *      FUNCTION
 *         Name:  polars
 *  Description:  From the Bloch vector representation to the polar coordinates
 *
 */
int polars ( double* r, double* theta, double* phi, const gsl_vector* v )
{
	*r = gsl_hypot3( VECTOR(v,1), VECTOR(v,2), VECTOR(v,3) ) ;
	if ( *r > 1 )
		return -1 ;

	*theta = acos(VECTOR(v,3)) ;

	if ( gsl_fcmp(fabs(*theta),1,1e-9) )
		*phi = 0 ;
	else
		*phi = atan2(VECTOR(v,2),VECTOR(v,1)) ;

	return 0;
}		/* -----  end of function polars  ----- */


/* specfunc/test_hermite.c
 *
 * Copyright (C) 2011, 2012, 2013, 2014 Konrad Griessinger
 *
 * 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_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_sf.h>

#include "test_sf.h"

#define WKB_TOL (1.0e+04 * TEST_SQRT_TOL0)

/*
 * Test the identities:
 *
 * Sum_{k=0}^n (n choose k) (2 y)^{n - k} H_k(x) = H_n(x + y)
 *
 * and
 *
 * Sum_{k=0}^n (n choose k) y^{n-k} He_k(x) = He_n(x + y)
 *
 * see: http://mathworld.wolfram.com/HermitePolynomial.html (Eq. 55)
 */
void
test_hermite_id1(const int n, const double x, const double y)
{
  double *a = malloc((n + 1) * sizeof(double));
  double *b = malloc((n + 1) * sizeof(double));
  double lhs, rhs;
  int k;

  a[0] = gsl_pow_int(2.0 * y, n);
  b[0] = gsl_pow_int(y, n);
  for (k = 1; k <= n; ++k)
    {
      double fac = (n - k + 1.0) / (k * y);
      a[k] = 0.5 * fac * a[k - 1];
      b[k] = fac * b[k - 1];
    }

  lhs = gsl_sf_hermite_phys_series(n, x, a);
  rhs = gsl_sf_hermite_phys(n, x + y);
  gsl_test_rel(lhs, rhs, TEST_TOL4, "identity1 phys n=%d x=%g y=%g", n, x, y);

  lhs = gsl_sf_hermite_prob_series(n, x, b);
  rhs = gsl_sf_hermite_prob(n, x + y);
  gsl_test_rel(lhs, rhs, TEST_TOL3, "identity1 prob n=%d x=%g y=%g", n, x, y);

  free(a);
  free(b);
}

int
test_hermite(void)
{
  gsl_sf_result r;

  int s = 0;
  int m, n, sa;
  double res[256];
  double x;
  const double aizero1 = -2.3381074104597670384891972524467; /* first zero of the Airy function Ai */

  /* test some known identities */
  test_hermite_id1(10, 0.75, 0.33);
  test_hermite_id1(8, -0.75, 1.20);
  test_hermite_id1(7, 2.88, -3.2);

  x = 0.75;

  TEST_SF(s, gsl_sf_hermite_prob_e, (0, 0.75, &r),  1.,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_prob_e, (1, 0.75, &r),  x,  TEST_TOL0, GSL_SUCCESS);

  n = 25;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, 0., &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  n = 28;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, 0., &r),  213458046676875,  TEST_TOL0, GSL_SUCCESS);
  n = 25;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, 0.75, &r),  -1.08128685847680748265939328423e12,  TEST_TOL0, GSL_SUCCESS);
  n = 28;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, 0.75, &r),  -1.60620252094658918105511125135e14,  TEST_TOL0, GSL_SUCCESS);

#if 0
  n = 10025;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2)*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  -3.3090527852387782540121569578e18961,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2)*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  -7.515478445930242044360704363e18967,  TEST_TOL0, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2)*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  4.1369269649092456235914193753e22243,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2)*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  8.363694992558646923734666303e22250,  TEST_TOL0, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-2*(aizero1/pow(8.*n,1/6.)))*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  7.398863979737363164340057757e22273,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-2*(aizero1/pow(8.*n,1/6.)))*M_SQRT2;
  TEST_SF(s, gsl_sf_hermite_prob_e, (n, x, &r),  1.507131397474022356488976968e22281,  TEST_TOL0, GSL_SUCCESS);
#endif

  x = 0.75;

  n = 128;
  m = 225;
  TEST_SF(s, gsl_sf_hermite_prob_der_e, (m, n, x, &r),  0.,  TEST_TOL0, GSL_SUCCESS);

  n = 128;
  m = 5;
  TEST_SF(s, gsl_sf_hermite_prob_der_e, (m, n, x, &r),  -3.0288278964712702882066404e112,  TEST_TOL1, GSL_SUCCESS);

  sa = 0;
  gsl_sf_hermite_prob_array(0, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array(0, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array(1, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   x,   TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array(1, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   823.810509681701660156250, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  1.03749254986255755872498e78, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_prob_array(100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_der(0, 100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   823.810509681701660156250, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  1.03749254986255755872498e78, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_prob_array_der(0, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_der(1000, 100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array_der(1000, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_array_der(23, 100, x, res);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[37], +0.0,  2.3592417210568968566591219172e37, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0, 2.6503570965896336273549197e100, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_array(23, 37, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_prob_der_array(100, 50, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   -3.88338863813139372375411561e31, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   7.9614368698398116765703194e38, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_prob_der_array(100, 50, 0.75)");
  s += sa;

  n = 128;
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_prob_series_e, (n, x, res, &r),  -4.0451066556993485405907339548e68,  TEST_TOL0, GSL_SUCCESS);

  /* phys */

  x = 0.75;

  TEST_SF(s, gsl_sf_hermite_phys_e, (0, 0.75, &r),  1.,  TEST_TOL0, GSL_SUCCESS);

  TEST_SF(s, gsl_sf_hermite_phys_e, (1, 0.75, &r),  2.*x,  TEST_TOL0, GSL_SUCCESS);

  n = 25;
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, 0., &r),  0.,  TEST_TOL0, GSL_SUCCESS);
  n = 28;
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, 0., &r),  3497296636753920000,  TEST_TOL0, GSL_SUCCESS);
  n = 25;
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, 0.75, &r),  -9.7029819451106077507781088352e15,  TEST_TOL0, GSL_SUCCESS);
  n = 28;
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, 0.75, &r),  3.7538457078067672096408339776e18,  TEST_TOL0, GSL_SUCCESS);

#if 0
  n = 10025;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2);
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  -2.7074282783315424535693575770e20470,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2);
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  -1.7392214893577690864150561850e20477,  TEST_TOL0, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2);
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  3.3847852473526979744366379542e23752,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2);
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  1.9355145738418079256435712027e23760,  TEST_TOL0, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-2*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  6.053663953512337293393128307e23782,  TEST_TOL0, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-2*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_phys_e, (n, x, &r),  3.487782358276961096026268141e23790,  TEST_TOL0, GSL_SUCCESS);
#endif

  x = 0.75;

  n = 128;
  m = 225;
  TEST_SF(s, gsl_sf_hermite_phys_der_e, (m, n, x, &r),  0.,  TEST_TOL0, GSL_SUCCESS);

  n = 128;
  m = 5;
  TEST_SF(s, gsl_sf_hermite_phys_der_e, (m, n, x, &r),  2.89461215568095657569833e132,  TEST_TOL0, GSL_SUCCESS);

  sa = 0;
  gsl_sf_hermite_phys_array(0, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array(0, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_array(1, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[1],   +0.0,   2.*x,   TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array(1, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   38740.4384765625, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  -1.4611185395125104593177790757e93, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array(100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_array_der(0, 100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   1.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   38740.4384765625, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,  -1.4611185395125104593177790757e93, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array_der(0, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_array_der(1000, 100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array_der(1000, 100, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_array_der(23, 100, x, res);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.0, TEST_TOL0);
  TEST_SF_VAL(sa, res[37],  +0.0,   1.930387357696033719818118732e46, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   2.957966000491202678467161e118, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_array(23, 37, 0.75)");
  s += sa;

  sa = 0;
  gsl_sf_hermite_phys_der_array(100, 50, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   -8.2663221830586310072686183e38, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   1.52281030265187793605875604e49, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   0.0, TEST_TOL0);
  gsl_test(sa, "gsl_sf_hermite_phys_der_array(100, 50, 0.75)");
  s += sa;

  n = 128;
  /* arbitrary weights */
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_phys_series_e, (n, x, res, &r),  1.07772223811696567390619566842e88,  TEST_TOL0, GSL_SUCCESS);

  x = 0.75;
  n = 28;
  TEST_SF(s, gsl_sf_hermite_func_e, (n, 0, &r),  0.290371943657199641200016132937,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.23526280808621240649319140441,  TEST_TOL0, GSL_SUCCESS);
  n = 100028;
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  -0.02903467369856961147236598086,  TEST_TOL4, GSL_SUCCESS);

  n = 10025;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  -0.05301278920004176459797680403,  TEST_TOL4, GSL_SUCCESS);

  n = 10028;
  x = ((sqrt(2*n+1.)+aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.06992968509693993526829596970,  TEST_TOL3, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.08049000991742150521366671021,  TEST_TOL4, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-(aizero1/pow(8.*n,1/6.))/2.5);
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  0.08048800667512084252723933250,  TEST_TOL4, GSL_SUCCESS);

  n = 10025;
  x = (sqrt(2*n+1.)-2.5*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  7.97206830806663013555068100e-6,  TEST_TOL4, GSL_SUCCESS);

  n = 10028;
  x = (sqrt(2*n+1.)-2.5*(aizero1/pow(8.*n,1/6.)));
  TEST_SF(s, gsl_sf_hermite_func_e, (n, x, &r),  7.97188517397786729928465829e-6,  TEST_TOL4, GSL_SUCCESS);

  x = 0.75;

  sa = 0;
  gsl_sf_hermite_func_array(100, x, res);
  TEST_SF_VAL(sa, res[0],   +0.0,   0.566979307027693616978839335983, TEST_TOL0);
  TEST_SF_VAL(sa, res[10],  +0.0,   0.360329854170806945032958735574, TEST_TOL0);
  TEST_SF_VAL(sa, res[100], +0.0,   -0.07616422890563462489003733382, TEST_TOL1);
  gsl_test(sa, "gsl_sf_hermite_func_array(100, 0.75)");
  s += sa;

  n = 128;
  /* arbitrary weights */
  res[0] = 1.;
  for(m=1; m<=n; m++){
    res[m] = res[m-1]/2.;
  }
  TEST_SF(s, gsl_sf_hermite_func_series_e, (n, x, res, &r),  0.81717103529960997134154552556,  TEST_TOL0, GSL_SUCCESS);

  m = 5;
  n = 28;
  TEST_SF(s, gsl_sf_hermite_func_der_e, (0, n, x, &r),  0.235262808086212406493191404,  TEST_TOL0, GSL_SUCCESS);
  TEST_SF(s, gsl_sf_hermite_func_der_e, (m, n, x, &r),  4035.32788513029308540826835,  TEST_TOL1, GSL_SUCCESS);

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 17 */
    double He17z[8] = { 0.751842600703896170737870774614, 1.50988330779674075905491513417, 2.28101944025298889535537879396,
                        3.07379717532819355851658337833, 3.90006571719800990903311840097, 4.77853158962998382710540812497,
                        5.74446007865940618125547815768,6.88912243989533223256205432938 };

    n = 17;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He17z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 18 */
    double He18z[9] = { 0.365245755507697595916901619097, 1.09839551809150122773848360538, 1.83977992150864548966395498992,
                        2.59583368891124032910545091458, 3.37473653577809099529779309480, 4.18802023162940370448450911428,
                        5.05407268544273984538327527397, 6.00774591135959752029303858752, 7.13946484914647887560975631213 };

    n = 18;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He18z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 23 */
    double He23z[11] = { 0.648471153534495816722576841197, 1.29987646830397886997876116860, 1.95732755293342410739100839243,
                         2.62432363405918177067330340783, 3.30504002175296456723204112903, 4.00477532173330406712238633738,
                         4.73072419745147329426707133987, 5.49347398647179412855289367747, 6.31034985444839982842886078177,
                         7.21465943505186138595859194492, 8.29338602741735258945596770157 };

    n = 23;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He23z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the probabilists' Hermite polynomial of order 24 */
    double He24z[12] = { 0.317370096629452319318170455994, 0.953421922932109084904629632351, 1.59348042981642010695074168129,
                         2.24046785169175236246653790858, 2.89772864322331368932008199475, 3.56930676407356024709649151613,
                         4.26038360501990548884317727406, 4.97804137463912033462166468006, 5.73274717525120114834341822330,
                         6.54167500509863444148277523364, 7.43789066602166310850331715501, 8.50780351919525720508386233432 };

    n = 24;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_prob_zero_e, (n, m, &r),  He24z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 17 */
    double H17z[8] = { 0.531633001342654731349086553718, 1.06764872574345055363045773799, 1.61292431422123133311288254454,
                       2.17350282666662081927537907149, 2.75776291570388873092640349574, 3.37893209114149408338327069289,
                       4.06194667587547430689245559698, 4.87134519367440308834927655662 };

    n = 17;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_phys_zero_e, (n, m, &r),  H17z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 18 */
    double H18z[9] = { 0.258267750519096759258116098711, 0.776682919267411661316659462284, 1.30092085838961736566626555439,
                       1.83553160426162889225383944409, 2.38629908916668600026459301424, 2.96137750553160684477863254906,
                       3.57376906848626607950067599377, 4.24811787356812646302342016090, 5.04836400887446676837203757885 };

    n = 18;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_phys_zero_e, (n, m, &r),  H18z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 23 */
    double H23z[11] = { 0.458538350068104797757887329284, 0.919151465442563765431719239593, 1.38403958568249523732634717118,
                        1.85567703767137106251504753718, 2.33701621147445578644623502174, 2.83180378712615690144806140734,
                        3.34512715994122457247439814585, 3.88447270810610186607248760288, 4.46209117374000667673186157071,
                        5.10153461047667712968749766165, 5.86430949898457256538748413474 };

    n = 23;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_phys_zero_e, (n, m, &r),  H23z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  {
    /* positive zeros of the physicists' Hermite polynomial of order 24 */
    double H24z[12] = { 0.224414547472515585151136715527, 0.674171107037212236000245923730, 1.12676081761124507213306126773,
                        1.58425001096169414850563336202, 2.04900357366169891178708399532, 2.52388101701142697419907602333,
                        3.01254613756556482565453858421, 3.52000681303452471128987227609, 4.05366440244814950394766297923,
                        4.62566275642378726504864923776, 5.25938292766804436743072304398, 6.01592556142573971734857350899 };

    n = 24;
    for (m=1; m<=n/2; m++) {
      TEST_SF(s, gsl_sf_hermite_phys_zero_e, (n, m, &r),  H24z[m-1],  TEST_TOL0, GSL_SUCCESS);
    }
  }

  n = 2121;
  x = -1.*n;
  res[0] = (double) n;
  sa = 0;
  for (m=1; m<=n/2; m++) {
    gsl_sf_hermite_prob_zero_e(n, m, &r);
    if (x>=r.val) {
      sa += TEST_SF_INCONS;
      printf("sanity check failed! (gsl_sf_hermite_prob_zero)\n");
    }
    res[0] = GSL_MIN(res[0],fabs(x-r.val));
    x = r.val;
  }
  gsl_test(sa, "gsl_sf_hermite_prob_zero(n, m, r)");

  n = 2121;
  x = -1.*n;
  res[0] = (double) n;
  sa = 0;
  for (m=1; m<=n/2; m++) {
    gsl_sf_hermite_phys_zero_e(n, m, &r);
    if (x>=r.val) {
      sa += TEST_SF_INCONS;
      printf("sanity check failed! (gsl_sf_hermite_phys_zero)\n");
    }
    res[0] = GSL_MIN(res[0],fabs(x-r.val));
    x = r.val;
  }
  gsl_test(sa, "gsl_sf_hermite_phys_zero(n, m, r)");

  return s;
}


/*
 * applyForceToolFrame.h
 *
 *  Created on: Sep 11, 2015
 *      Author: mas
 */
#ifndef APPLY_FORCE_TOOLFRAME_H_
#define APPLY_FORCE_TOOLFRAME_H_
#include <cmath>
#include <barrett/units.h>
#include <barrett/systems.h>
#include <barrett/products/product_manager.h>
#include <libconfig.h++>
#include <barrett/detail/ca_macro.h>
#include <barrett/cdlbt/calgrav.h>
#include <barrett/systems/abstract/system.h>
#include <barrett/systems/abstract/single_io.h>
#include <barrett/systems/kinematics_base.h>
#include <barrett/math/kinematics.h>
#include <stdio.h>
#include <iostream>
#include <string>
#include <cstdlib>  // For std::atexit()
#include <barrett/os.h>  // For btsleep()
#include <barrett/math.h>  // For barrett::math::saturate()
#include <Eigen/Dense>
#include <Eigen/Core>
#include <barrett/standard_main_function.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#define BARRETT_SMF_VALIDATE_ARGS
#include <barrett/standard_main_function.h>

using namespace barrett;
using namespace systems;
using systems::connect;
BARRETT_UNITS_FIXED_SIZE_TYPEDEFS;

template<size_t DOF>
class applyForceToolFrame : public systems::System
{
BARRETT_UNITS_TEMPLATE_TYPEDEFS(DOF);

// IO  (inputs)
public:
Input<cp_type> CpInput;                 // cart pos. input
Input<Eigen::Quaterniond> OrnInput;     // orientation input (quaternion)
Input<cf_type> FtInput;                 // cart force input (in tool frame)
Input<ct_type> TtInput;                // cart torque input (in tool frame)

// IO  (outputs)
public:
Output<cf_type> CFOutput;    // output Cartesian forces (in base frame)
Output<ct_type> CTOutput;    // output Cartesian torque (in base frame)

protected:
typename Output<cf_type>::Value* cfOutputValue;
typename Output<ct_type>::Value* ctOutputValue;

public:
cf_type computedF;          // can be used to display later in the program
ct_type computedT;          // can be used to display later in the program

public:
applyForceToolFrame(
      const std::string& sysName = "applyForceToolFrame") :
      System(sysName), CpInput(this), OrnInput(this), FtInput(this) , TtInput(this) ,
      CFOutput(this, &cfOutputValue), CTOutput(this, &ctOutputValue)
{
}
virtual ~applyForceToolFrame()
{
this->mandatoryCleanUp();
}
protected:
    cf_type cf;
    ct_type ct;

    cf_type ForceTool;
    ct_type TorqueTool;
    cp_type Xcurr;
    Eigen::Quaterniond OrnCurr;

virtual void operate()
{

  Xcurr      = CpInput.getValue();       // current Cart. Pose
  OrnCurr    = OrnInput.getValue();      // current tool Orientation (quaternion)
  ForceTool  = FtInput.getValue();       // desired tool force
  TorqueTool = TtInput.getValue();       // desired tool torque

  OrnCurr.x() = - OrnCurr.x();           // wam quaternion imaginary part in negative
  OrnCurr.y() = - OrnCurr.y();           // wam quaternion imaginary part in negative
  OrnCurr.z() = - OrnCurr.z();           // wam quaternion imaginary part in negative
//****************************************
//  gsl_matrix* RotMat = gsl_matrix_alloc (3,3);
//  gsl_vector* Ft = gsl_vector_alloc(3);   // Force  in tool frame
//  gsl_vector* Tt = gsl_vector_alloc(3);   // Torque in tool frame
//
//  gsl_vector* Fb = gsl_vector_alloc(3);   // Force  in base frame
//  gsl_vector* Tb = gsl_vector_alloc(3);   // Torque in base frame
//
//  // RotMat = [  1 - 2*( OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z() ) ,       2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z())   ,       2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ;
//  //                 2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z())   ,   1 - 2*(OrnCurr.x()^2 + OrnCurr.z()^2)     ,       2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ;
//  //                 2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y())   ,       2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z())   ,   1 - 2*(OrnCurr.x()^2 + OrnCurr.y()^2)     ] ;
//
// gsl_matrix_set(RotMat, 0 , 0 , 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 0 , 1 ,     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 0 , 2 ,     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) );
//
// gsl_matrix_set(RotMat, 1 , 0 ,     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 1 , 1 , 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) );
// gsl_matrix_set(RotMat, 1 , 2 ,     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x())  );
//
// gsl_matrix_set(RotMat, 2 , 0 ,     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y())  );
// gsl_matrix_set(RotMat, 2 , 1 ,     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z())  );
// gsl_matrix_set(RotMat, 2 , 2 , 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y())  );
//
//
//
// for (int i = 0; i<3 ; i++)
//        {
//           gsl_vector_set (Ft , i , ForceTool[i] ) ;
//           gsl_vector_set (Tt , i , TorqueTool[i] ) ;
//        }
//
// gsl_blas_dgemv(CblasNoTrans, 1.0, RotMat, Ft, 0.0, Fb);    // Fb = 1.0 * RotMat * Ft + 0.0
// gsl_blas_dgemv(CblasNoTrans, 1.0, RotMat, Tt, 0.0, Tb);    // Tb = 1.0 * RotMat * Tt + 0.0
//
// for (int i = 0; i<3 ; i++)
//       {
//           cf[i] = gsl_vector_get ( Fb , i );
//           ct[i] = gsl_vector_get ( Tb , i );
//       }
//****************************************

  cf[0] = ( 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) ) * ForceTool[0] +
          (     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) ) * ForceTool[1] +
          (     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ) * ForceTool[2]   ;

  cf[1] = (     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) ) * ForceTool[0] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) ) * ForceTool[1] +
          (     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ) * ForceTool[2]   ;

  cf[2] = (     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y()) ) * ForceTool[0] +
          (     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z()) ) * ForceTool[1] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y()) ) * ForceTool[2]   ;

//****************************************
  ct[0] = ( 1 - 2*(OrnCurr.y()*OrnCurr.y() + OrnCurr.z()*OrnCurr.z()) ) * TorqueTool[0] +
          (     2*(OrnCurr.x()*OrnCurr.y() - OrnCurr.w()*OrnCurr.z()) ) * TorqueTool[1] +
          (     2*(OrnCurr.w()*OrnCurr.y() + OrnCurr.x()*OrnCurr.z()) ) * TorqueTool[2]   ;

  ct[1] = (     2*(OrnCurr.x()*OrnCurr.y() + OrnCurr.w()*OrnCurr.z()) ) * TorqueTool[0] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.z()*OrnCurr.z()) ) * TorqueTool[1] +
          (     2*(OrnCurr.y()*OrnCurr.z() - OrnCurr.w()*OrnCurr.x()) ) * TorqueTool[2]   ;

  ct[2] = (     2*(OrnCurr.x()*OrnCurr.z() - OrnCurr.w()*OrnCurr.y()) ) * TorqueTool[0] +
          (     2*(OrnCurr.w()*OrnCurr.x() + OrnCurr.y()*OrnCurr.z()) ) * TorqueTool[1] +
          ( 1 - 2*(OrnCurr.x()*OrnCurr.x() + OrnCurr.y()*OrnCurr.y()) ) * TorqueTool[2]   ;

//****************************************

  computedF =  cf;
  computedT =  ct;

  cfOutputValue->setData(&cf);
  ctOutputValue->setData(&ct);
}

private:
DISALLOW_COPY_AND_ASSIGN(applyForceToolFrame);
};

#endif /*APPLY_FORCE_TOOLFRAME_H_*/


/**
 *
 * @file example_dgelqf.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example using LQ factorization
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated d Tue Jan  7 11:45:20 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

#include <plasma.h>
#include <cblas.h>
#include <lapacke.h>
#include <core_blas.h>

#ifndef max
#define max(a, b) ((a) > (b) ? (a) : (b))
#endif
#ifndef min
#define min(a, b) ((a) < (b) ? (a) : (b))
#endif

int check_orthogonality(int, int, int, double*);
int check_factorization(int, int, double*, double*, int, double*);

int IONE=1;
int ISEED[4] = {0,0,0,1};   /* initial seed for dlarnv() */

int main ()
{

    int cores = 2;
    int M     = 10;
    int N     = 15;
    int LDA   = 10;
    int K = min(M, N);
    int info;
    int info_ortho, info_factorization;
    int i,j;
    int LDAxN = LDA*N;

    double *A1 = (double *)malloc(LDA*N*sizeof(double));
    double *A2 = (double *)malloc(LDA*N*sizeof(double));
    double *Q  = (double *)malloc(LDA*N*sizeof(double));
    PLASMA_desc *T;

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!Q)){
        printf("Out of Memory \n ");
        return EXIT_SUCCESS;
    }

    /* Plasma Initialization */
    PLASMA_Init(cores);
    printf("-- PLASMA is initialized to run on %d cores. \n",cores);

    /* Allocate T */
    PLASMA_Alloc_Workspace_dgelqf(M, N, &T);

    /* Initialize A1 and A2 */
    LAPACKE_dlarnv_work(IONE, ISEED, LDAxN, A1);
    for (i = 0; i < M; i++)
        for (j = 0; j < N; j++)
            A2[LDA*j+i] = A1[LDA*j+i] ;

    /* Factorization QR of the matrix A2 */
    info = PLASMA_dgelqf(M, N, A2, LDA, T);

    /* Building the economy-size Q */
    memset((void*)Q, 0, LDA*N*sizeof(double));
    for (i = 0; i < K; i++)
        Q[LDA*i+i] = 1.0;

    PLASMA_dorglq(M, N, K, A2, LDA, T, Q, LDA);

    /* Check the orthogonality, factorization and the solution */
    info_ortho = check_orthogonality(M, N, LDA, Q);
    info_factorization = check_factorization(M, N, A1, A2, LDA, Q);

    if ((info_ortho != 0)|(info_factorization != 0)|(info != 0))
       printf("-- Error in DGELQF example ! \n");
    else
       printf("-- Run of DGELQF example successful ! \n");

    free(A1); free(A2); free(Q); free(T);

    PLASMA_Finalize();

    return EXIT_SUCCESS;
}

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int check_orthogonality(int M, int N, int LDQ, double *Q)
{
    double alpha, beta;
    double normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    double eps;
    double *work = (double *)malloc(minMN*sizeof(double));

    eps = LAPACKE_dlamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    double *Id = (double *) malloc(minMN*minMN*sizeof(double));
    memset((void*)Id, 0, minMN*minMN*sizeof(double));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (double)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_dlansy_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int check_factorization(int M, int N, double *A1, double *A2, int LDA, double *Q)
{
    double Anorm, Rnorm;
    double alpha, beta;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Ql       = (double *)malloc(M*N*sizeof(double));
    double *Residual = (double *)malloc(M*N*sizeof(double));
    double *work              = (double *)malloc(max(M,N)*sizeof(double));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        double *R = (double *)malloc(N*N*sizeof(double));
        memset((void*)R, 0, N*N*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        double *L = (double *)malloc(M*M*sizeof(double));
        memset((void*)L, 0, M*M*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), M, N, Residual, M, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}


#ifndef _WAVELET_H_
#define _WAVELET_H_

#include "gsl/gsl_wavelet.h"
//#include <gsl/gsl_wavelet2d.h>

// N-to-M conversion helper functions
template <typename T> T *** arrayto3D(T * data, size_t x, size_t y, size_t z);
template <typename T> T ** arrayto2D(T * data, size_t x, size_t y);
template <typename T> T * arrayto1D(T *** data, size_t x, size_t y, size_t z);
template <typename T> T * arrayto1D(T ** data, size_t x, size_t y);

// Helper functions to compute size of wavelet hierarchy at a level
static int binary_logn (const size_t n);
int binary_upto_logn (const size_t n);
int binary_upto_value (const size_t n, const int log_n);

// Debug output of discrete wavelet signal
void gsl_wavelet_print (const gsl_wavelet * w);

// Native stepping functions that apply signal to data
template <typename T> static void dwt_step (const gsl_wavelet * w, T *a, size_t stride, size_t n, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);
template <typename T> static void dwt_step_proto(const gsl_wavelet * w, T *a, size_t stride, size_t n, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);

// 1D-support
template <typename T> int wavelet_transform(const gsl_wavelet * w, T *&data, size_t stride, size_t n, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);

// 2D-support
template <typename T> int wavelet2d_transform(const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);
template <typename T> int wavelet2d_nstransform(const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);

// 3D-support versions of (GSL) wavelet transform
template <typename T> int gsl_wavelet3d_transform (const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2,size_t size3, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);
template <typename T> int gsl_wavelet3d_nstransform (const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2, size_t size3, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);
template <typename T> int gsl_wavelet3d_nstransform (const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2, size_t size3, gsl_wavelet_direction dir, gsl_wavelet_workspace * work, int levels);
template <typename T> int gsl_wavelet3d_nstransform_proto(const gsl_wavelet * w, T *data, size_t tda, size_t size1, size_t size2, size_t size3, gsl_wavelet_direction dir, gsl_wavelet_workspace * work);

// Functions to compute dimensions for each coefficient block (quadrant) in a wavelet hierarchy
int *** getCoeffDims (int dimx, int dimy, int dimz);
int *** getCoeffDims (int dimx, int dimy);

// Set/Extract functions used in conjunction with above to extract coefficient subregions
template <typename T> T * getCutout3d(T * in, int * codims, int * dims);
template <typename T> T * getCutout2d(T * in, int * codims, int * dims);
template <typename T> void setCutout3d(T * in, T * cutout, int * codims, int * dims);
template <typename T> void setCutout2d(T * in, T * cutout, int * codims, int * dims);

template <typename T> int grow(int val, T *** data, size_t x, size_t y, size_t z);
template <typename T> int shrink(int val, T *** data, size_t x, size_t y, size_t z);

#endif

#include "wavelet.inl"


/* stable/stable_fit.c
 *
 * Functions employed by different methods of estimation implemented
 * in Libstable.
 *
 * Copyright (C) 2013. Javier Royuela del Val
 *                     Federico Simmross Wattenberg
 *
 * 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; version 3 of the License.
 *
 * 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, see <http://www.gnu.org/licenses/>.
 *
 *
 *  Javier Royuela del Val.
 *  E.T.S.I. Telecomunicación
 *  Universidad de Valladolid
 *  Paseo de Belén 15, 47002 Valladolid, Spain.
 *  jroyval@lpi.tel.uva.es
 */
#include "stable.h"
#include "mcculloch.h"

#include <gsl/gsl_complex.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_fft_real.h>

void stable_fft(double *data, const unsigned int length, double * y)
{
  //int i;

  memcpy ( (void *)y, (const void *) data, length*sizeof(double));

  gsl_fft_real_radix2_transform (y, 1, length);

  return;
}

double stable_loglikelihood(StableDist *dist, double *data, const unsigned int length)
{
  double *pdf=NULL;
  double l=0.0;
  int i;

  pdf=(double*)malloc(sizeof(double)*length);

  stable_pdf(dist,data,length,pdf,NULL);

  for(i=0;i<length;i++)
    {
      if (pdf[i]>0.0) l+=log(pdf[i]);
    }

  free(pdf);
  return l;
}

double stable_loglike_p(stable_like_params *params)
{
  double *pdf;
  double l=0.0;
  int i;

  pdf=(double*)malloc(sizeof(double)*(params->length));

  stable_pdf(params->dist,params->data,params->length,pdf,NULL);

  for(i=0;i<params->length;i++)
    {
      if (pdf[i]>0.0) {l+=log(pdf[i]);}
    }

  free(pdf);

  return l;
}

double stable_minusloglikelihood(const gsl_vector * theta, void * p)
{
/* Cost function to minimize, with the estimation of sigma and mu given by McCulloch at each iteration*/
  double alpha=1, beta=0, sigma=1.0, mu=0.0;
  double minusloglike=0;
  stable_like_params * params = (stable_like_params *) p;

  alpha = gsl_vector_get(theta,0);
  beta = gsl_vector_get(theta,1);

  /* Update sigma and mu with McCulloch. It needs nu_c nu_z*/
  czab(alpha, beta, params->nu_c, params->nu_z, &sigma, &mu);

  /* Check that the parameters are valid */
  if(stable_setparams(params->dist, alpha, beta, sigma, mu, 0) < 0)
    {
      return GSL_NAN;
    }
  else minusloglike = -stable_loglike_p(params);

  if (isinf(minusloglike) || isnan(minusloglike)) minusloglike=GSL_NAN;

  return minusloglike;
}

int compare (const void * a, const void * b)
{
/* qsort compare function */
  return ((*(double *)b < *(double *)a) - (*(double *)a < *(double *)b));
}

inline void get_original(const gsl_vector *s,double *a,double *b,double *c,double *m)
{
  *a = M_2_PI*atan(gsl_vector_get(s,0))+1.0;
  *b = M_2_PI*atan(gsl_vector_get(s,1));
  *c = exp(gsl_vector_get(s,2));
  *m = gsl_vector_get(s,3);
}
inline void set_expanded(gsl_vector *s,const double a,const double b,const double c,const double m)
{
  gsl_vector_set(s,0,tan(M_PI_2*(a-1.0)));
  gsl_vector_set(s,1,tan(M_PI_2*b));
  gsl_vector_set(s,2,log(c));
  gsl_vector_set(s,3,m);
}

double stable_minusloglikelihood_whole(const gsl_vector * theta, void * p)
{
/* Whole cost function to minimize in a 4D parameter space */
  double alpha=1, beta=0, sigma=1.0, mu=0.0;
  double minusloglike=0;
  stable_like_params * params = (stable_like_params *) p;

  get_original(theta,&alpha,&beta,&sigma,&mu);

  /* Check that the parameters are valid */
  if(stable_setparams(params->dist, alpha, beta, sigma, mu, 0) < 0)
    {
      perror("setparams error");
      return GSL_NAN;
    }
  else minusloglike = -stable_loglike_p(params);

  if (isinf(minusloglike) || isnan(minusloglike)) minusloglike=GSL_NAN;

  return minusloglike;
}

void stable_fit_init(StableDist *dist, const double * data, const unsigned int length, double *pnu_c,double *pnu_z)
{
  /* McCulloch estimation */

  double *sorted=NULL;
  double alpha0, beta0, sigma0, mu0;

  /* We need to sort the data to get percentiles */
  sorted = (double*)malloc(length*sizeof(double));
  memcpy ( (void *)sorted, (const void *) data, length*sizeof(double));
  qsort  ( sorted, length, sizeof(double), compare);

  /* Estimate the parameters. */
  stab((const double *) sorted,length,0,&alpha0,&beta0,&sigma0,&mu0);

  /* Set parameters in the distribution */
  if(stable_setparams(dist,alpha0,beta0,sigma0,mu0, 0)<0)
    {
      perror("INITIAL ESTIMATED PARAMETER ARE NOT VALID");
      return;
    }

  /* Get pnu_c and pnu_z needed for mle2d estimation */
  cztab(sorted, length, pnu_c, pnu_z);

  free(sorted);
  return;
}

int stable_fit_iter(StableDist *dist, const double * data, const unsigned int length,const double nu_c,const double nu_z)
{
  const gsl_multimin_fminimizer_type *T;
  gsl_multimin_fminimizer *s;

  gsl_multimin_function likelihood_func;

  gsl_vector *theta, *ss;

  unsigned int iter = 0;
  int status=0;
  double size=0;

  double a=1,b=0.0,c=1,m=0.0;
  stable_like_params par;

  par.dist=dist;
  par.data=(double *)data;
  par.length=length;
  par.nu_c=nu_c;
  par.nu_z=nu_z;

  /* Inicio: Debe haberse inicializado dist con alpha y beta de McCulloch */
  theta=gsl_vector_alloc(2);
  gsl_vector_set (theta, 0, dist->alpha);
  gsl_vector_set (theta, 1, dist->beta);

  #ifdef DEBUG
    printf("%lf, %lf\n",gsl_vector_get (theta, 0),gsl_vector_get (theta, 1));
  #endif

  /* Saltos iniciales */
  ss = gsl_vector_alloc (2);
  gsl_vector_set_all (ss, 0.01);

  /* Funcion a minimizar */
  likelihood_func.n = 2; // Dimension 2 (alpha y beta)
  likelihood_func.f = &stable_minusloglikelihood;
  likelihood_func.params = (void *) (&par);  // Parametros de la funcion

   /* Creacion del minimizer */
  T = gsl_multimin_fminimizer_nmsimplex2rand;

  s = gsl_multimin_fminimizer_alloc (T, 2); /* Dimension 2*/

  /* Poner funcion, estimacion inicial, saltos iniciales */
  gsl_multimin_fminimizer_set (s, &likelihood_func, theta, ss);

  #ifdef DEBUG
    printf("5\n");
  #endif

  /* Iterar */
  do
    {
      iter++;
      status = gsl_multimin_fminimizer_iterate(s);
 //     if (status!=GSL_SUCCESS) {
 //       printf("Minimizer warning: %s\n",gsl_strerror(status));
 //       fflush(stdout);
 //      }

      size = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, 0.02);
/*
      if (status == GSL_SUCCESS)
        {
          printf ("              converged to minimum at\n");
        }

      printf ("%5d %1.5f %1.5f %1.5f %1.5f f() = %1.8e size = %.5f\n",
              (int)iter,
              gsl_vector_get (s->x, 0),
              gsl_vector_get (s->x, 1),
              p->dist->sigma,
              p->dist->mu_1,
              s->fval, size);
        //}
*/
    } while (status == GSL_CONTINUE && iter < 200);

//  if (status!=GSL_SUCCESS)
//    {
//      printf("Minimizer warning: %s\n",gsl_strerror(status));
//      fflush(stdout);
//    }

  /* Se recupera la estimacion alpha y beta */
  gsl_vector_free(theta);
/*
  theta = gsl_multimin_fminimizer_x (s);
  a = gsl_vector_get (theta, 0);
  b = gsl_vector_get (theta, 1);
*/
  a = gsl_vector_get (s->x, 0);
  b = gsl_vector_get (s->x, 1);

  /* Y se estima sigma y mu para esos alpha y beta */
  czab(a, b, nu_c, nu_z, &c, &m);

  //printf("%5d %10.3e %10.3e %10.3e %10.3e\n",(int)iter,a,b,c,m);

  // Se almacena el punto estimado en la distribucion, comprobando que es valido
  if (stable_setparams(dist,a,b,c,m,0)<0)
   {
    perror("FINAL ESTIMATED PARAMETER ARE NOT VALID\n");
   }

  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  return status;
}

int stable_fit(StableDist *dist, const double *data, const unsigned int length)
{
  double nu_c=0.0,nu_z=0.0;
  int status = 0;

  stable_fit_init(dist,data,length,&nu_c,&nu_z);
  status=stable_fit_iter(dist,data,length,nu_c,nu_z);

  return status;
}

int stable_fit_iter_whole(StableDist *dist, const double * data, const unsigned int length)
{
  const gsl_multimin_fminimizer_type *T;
  gsl_multimin_fminimizer *s;

  gsl_multimin_function likelihood_func;

  gsl_vector *theta, *ss;

  unsigned int iter = 0;
  int status=0;
  double size=0;

  double a=1,b=0.0,c=1,m=0.0;
  stable_like_params par;

  par.dist=dist;
  par.data=(double *)data;
  par.length=length;
  par.nu_c=0;
  par.nu_z=0;

  /* Inital params (with McCulloch) */
  theta=gsl_vector_alloc(4);
  set_expanded(theta,dist->alpha,dist->beta,dist->sigma,dist->mu_1);

  #ifdef DEBUG
    printf("%lf, %lf, %lf, %lf\n",gsl_vector_get (theta, 0),gsl_vector_get (theta, 1),gsl_vector_get (theta, 2),gsl_vector_get (theta, 3));
  #endif

  /* Initial steps */
  ss = gsl_vector_alloc (4);
  gsl_vector_set_all (ss, 0.01);

  /* Cost function to minimize */
  likelihood_func.n = 4; // 4 Dimensions (alpha, beta, sigma, mu_0)
  likelihood_func.f = &stable_minusloglikelihood_whole;
  likelihood_func.params = (void *) (&par);  // Cost function arguments

  /* Minimizer creation */
  T = gsl_multimin_fminimizer_nmsimplex2rand;

  s = gsl_multimin_fminimizer_alloc (T, 4); /* 4 dimensions */

  /* Set cost function, initial guess and initial steps */
  gsl_multimin_fminimizer_set (s, &likelihood_func, theta, ss);

  #ifdef DEBUG
    printf("5\n");
  #endif

  /* Start iterations */
  do
    {
      iter++;
      status = gsl_multimin_fminimizer_iterate(s);
      if (status!=GSL_SUCCESS) {
        perror("Minimizer warning:\n");
      }

      size   = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, 0.002);
      /*
      printf(" %03d\t size = %f a_ = %f  b_ = %f  c_ = %f  m_ = %f f_ = %f \n",iter,size,gsl_vector_get (s->x, 0),gsl_vector_get (s->x, 1),
                                                           gsl_vector_get (s->x, 2),gsl_vector_get (s->x, 3), gsl_multimin_fminimizer_minimum(s));
      */

    } while (status == GSL_CONTINUE && iter < 200);

  if (status!=GSL_SUCCESS)
    {
      perror("Minimizer warning");
    }

  /* Get last estimation */

  gsl_vector_free(theta);
  theta = gsl_multimin_fminimizer_x (s);
  get_original(theta,&a,&b,&c,&m);

  /* Set estimated parameters to the distribution and check if their have valid values*/
  if (stable_setparams(dist,a,b,c,m,0)<0)
   {
    perror("FINAL ESTIMATED PARAMETER ARE NOT VALID\n");
   }

  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  return status;
}

int stable_fit_whole(StableDist *dist, const double *data, const unsigned int length)
{
//  double nu_c=0.0,nu_z=0.0;
  int status=0;
//  stable_fit_init(dist,data,length,&nu_c,&nu_z);
//  printf("McCulloch %d sampless: %f %f %f %f\n",length,dist->alpha,dist->beta,dist->sigma,dist->mu_1);

  status = stable_fit_iter_whole(dist,data,length);

  return status;
}

double * load_rand_data(char * filename, int N)
{
  FILE * f_data;
  double * data;
  int i;

  if ((f_data = fopen(filename,"rt")) == NULL)
   {
    perror("Error when opening file with random data");
   }

  data=(double*)malloc(N*sizeof(double));

  for(i=0;i<N;i++)
   {
    if (EOF==fscanf(f_data,"%le\n",data+i))
     {
      perror("Error when reading data");
     }
   }
  return data;
}

int stable_fit_mle(StableDist *dist, const double *data, const unsigned int length) {
  return stable_fit_whole(dist,data,length);
}

int stable_fit_mle2d(StableDist *dist, const double *data, const unsigned int length) {
  return stable_fit(dist,data,length);
}


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

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_ieee_utils.h>

#include "roots.h"
#include "test.h"

/* stopping parameters */
const double EPSREL = (10 * GSL_DBL_EPSILON);
const double EPSABS = (10 * GSL_DBL_EPSILON);
const unsigned int MAX_ITERATIONS = 150;

void my_error_handler (const char *reason, const char *file,
                       int line, int err);

#define WITHIN_TOL(a, b, epsrel, epsabs) \
 ((fabs((a) - (b)) < (epsrel) * GSL_MIN(fabs(a), fabs(b)) + (epsabs)))

int
main (void)
{
  gsl_function F_sin, F_cos, F_func1, F_func2, F_func3, F_func4,
    F_func5, F_func6;

  gsl_function_fdf FDF_sin, FDF_cos, FDF_func1, FDF_func2, FDF_func3, FDF_func4,
    FDF_func5, FDF_func6;

  const gsl_root_fsolver_type * fsolver[4] ;
  const gsl_root_fdfsolver_type * fdfsolver[4] ;

  const gsl_root_fsolver_type ** T;
  const gsl_root_fdfsolver_type ** S;

  gsl_ieee_env_setup();

  fsolver[0] = gsl_root_fsolver_bisection;
  fsolver[1] = gsl_root_fsolver_brent;
  fsolver[2] = gsl_root_fsolver_falsepos;
  fsolver[3] = 0;

  fdfsolver[0] = gsl_root_fdfsolver_newton;
  fdfsolver[1] = gsl_root_fdfsolver_secant;
  fdfsolver[2] = gsl_root_fdfsolver_steffenson;
  fdfsolver[3] = 0;

  F_sin = create_function (sin_f) ;
  F_cos = create_function (cos_f) ;
  F_func1 = create_function (func1) ;
  F_func2 = create_function (func2) ;
  F_func3 = create_function (func3) ;
  F_func4 = create_function (func4) ;
  F_func5 = create_function (func5) ;
  F_func6 = create_function (func6) ;

  FDF_sin = create_fdf (sin_f, sin_df, sin_fdf) ;
  FDF_cos = create_fdf (cos_f, cos_df, cos_fdf) ;
  FDF_func1 = create_fdf (func1, func1_df, func1_fdf) ;
  FDF_func2 = create_fdf (func2, func2_df, func2_fdf) ;
  FDF_func3 = create_fdf (func3, func3_df, func3_fdf) ;
  FDF_func4 = create_fdf (func4, func4_df, func4_fdf) ;
  FDF_func5 = create_fdf (func5, func5_df, func5_fdf) ;
  FDF_func6 = create_fdf (func6, func6_df, func6_fdf) ;

  gsl_set_error_handler (&my_error_handler);

  for (T = fsolver ; *T != 0 ; T++)
    {
      test_f (*T, "sin(x) [3, 4]", &F_sin, 3.0, 4.0, M_PI);
      test_f (*T, "sin(x) [-4, -3]", &F_sin, -4.0, -3.0, -M_PI);
      test_f (*T, "sin(x) [-1/3, 1]", &F_sin, -1.0 / 3.0, 1.0, 0.0);
      test_f (*T, "cos(x) [0, 3]", &F_cos, 0.0, 3.0, M_PI / 2.0);
      test_f (*T, "cos(x) [-3, 0]", &F_cos, -3.0, 0.0, -M_PI / 2.0);
      test_f (*T, "x^20 - 1 [0.1, 2]", &F_func1, 0.1, 2.0, 1.0);
      test_f (*T, "sqrt(|x|)*sgn(x)", &F_func2, -1.0 / 3.0, 1.0, 0.0);
      test_f (*T, "x^2 - 1e-8 [0, 1]", &F_func3, 0.0, 1.0, sqrt (1e-8));
      test_f (*T, "x exp(-x) [-1/3, 2]", &F_func4, -1.0 / 3.0, 2.0, 0.0);
      test_f (*T, "(x - 1)^7 [0.9995, 1.0002]", &F_func6, 0.9995, 1.0002, 1.0);

      test_f_e (*T, "invalid range check [4, 0]", &F_sin, 4.0, 0.0, M_PI);
      test_f_e (*T, "invalid range check [1, 1]", &F_sin, 1.0, 1.0, M_PI);
      test_f_e (*T, "invalid range check [0.1, 0.2]", &F_sin, 0.1, 0.2, M_PI);
    }

  for (S = fdfsolver ; *S != 0 ; S++)
    {
      test_fdf (*S,"sin(x) {3.4}", &FDF_sin, 3.4, M_PI);
      test_fdf (*S,"sin(x) {-3.3}", &FDF_sin, -3.3, -M_PI);
      test_fdf (*S,"sin(x) {0.5}", &FDF_sin, 0.5, 0.0);
      test_fdf (*S,"cos(x) {0.6}", &FDF_cos, 0.6, M_PI / 2.0);
      test_fdf (*S,"cos(x) {-2.5}", &FDF_cos, -2.5, -M_PI / 2.0);
      test_fdf (*S,"x^{20} - 1 {0.9}", &FDF_func1, 0.9, 1.0);
      test_fdf (*S,"x^{20} - 1 {1.1}", &FDF_func1, 1.1, 1.0);
      test_fdf (*S,"sqrt(|x|)*sgn(x) {1.001}", &FDF_func2, 0.001, 0.0);
      test_fdf (*S,"x^2 - 1e-8 {1}", &FDF_func3, 1.0, sqrt (1e-8));
      test_fdf (*S,"x exp(-x) {-2}", &FDF_func4, -2.0, 0.0);
      test_fdf_e (*S,"max iterations x -> +Inf, x exp(-x) {2}", &FDF_func4, 2.0, 0.0);
      test_fdf_e (*S,"max iterations x -> -Inf, 1/(1 + exp(-x)) {0}", &FDF_func5, 0.0, 0.0);
    }

  test_fdf (gsl_root_fdfsolver_steffenson,
            "(x - 1)^7 {0.9}", &FDF_func6, 0.9, 1.0);

  /* now summarize the results */

  exit (gsl_test_summary ());
}

/* Using gsl_root_bisection, find the root of the function pointed to by f,
   using the interval [lower_bound, upper_bound]. Check if f succeeded and
   that it was accurate enough. */

void
test_f (const gsl_root_fsolver_type * T, const char * description, gsl_function *f,
        double lower_bound, double upper_bound, double correct_root)
{
  int status;
  size_t iterations = 0;
  double r, a, b;
  double x_lower, x_upper;
  gsl_root_fsolver * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_root_fsolver_alloc(T);
  gsl_root_fsolver_set(s, f, x_lower, x_upper) ;

  do
    {
      iterations++ ;

      gsl_root_fsolver_iterate (s);

      r = gsl_root_fsolver_root(s);

      a = gsl_root_fsolver_x_lower(s);
      b = gsl_root_fsolver_x_upper(s);

      if (a > b)
        gsl_test (GSL_FAILURE, "interval is invalid (%g,%g)", a, b);

      if (r < a || r > b)
        gsl_test (GSL_FAILURE, "r lies outside interval %g (%g,%g)", r, a, b);

      status = gsl_root_test_interval (a,b, EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (status, "%s, %s (%g obs vs %g expected) ",
            gsl_root_fsolver_name(s), description,
            gsl_root_fsolver_root(s), correct_root);

  if (iterations == MAX_ITERATIONS)
    {
      gsl_test (GSL_FAILURE, "exceeded maximum number of iterations");
    }

  /* check the validity of the returned result */

  if (!WITHIN_TOL (r, correct_root, EPSREL, EPSABS))
    {
      gsl_test (GSL_FAILURE, "incorrect precision (%g obs vs %g expected)",
                r, correct_root);

    }

  gsl_root_fsolver_free(s);
}

void
test_f_e (const gsl_root_fsolver_type * T,
          const char * description, gsl_function *f,
          double lower_bound, double upper_bound, double correct_root)
{
  int status;
  size_t iterations = 0;
  double x_lower, x_upper;
  gsl_root_fsolver * s;

  x_lower = lower_bound;
  x_upper = upper_bound;

  s = gsl_root_fsolver_alloc(T);
  status = gsl_root_fsolver_set(s, f, x_lower, x_upper) ;

  gsl_test (status != GSL_EINVAL, "%s (set), %s", T->name, description);

  if (status == GSL_EINVAL)
    {
      gsl_root_fsolver_free(s);
      return ;
    }

  do
    {
      iterations++ ;
      gsl_root_fsolver_iterate (s);
      x_lower = gsl_root_fsolver_x_lower(s);
      x_upper = gsl_root_fsolver_x_lower(s);
      status = gsl_root_test_interval (x_lower, x_upper,
                                      EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (!status, "%s, %s", gsl_root_fsolver_name(s), description,
            gsl_root_fsolver_root(s) - correct_root);

  gsl_root_fsolver_free(s);
}

void
test_fdf (const gsl_root_fdfsolver_type * T, const char * description,
        gsl_function_fdf *fdf, double root, double correct_root)
{
  int status;
  size_t iterations = 0;
  double prev = 0 ;

  gsl_root_fdfsolver * s = gsl_root_fdfsolver_alloc(T);
  gsl_root_fdfsolver_set (s, fdf, root) ;

  do
    {
      iterations++ ;
      prev = gsl_root_fdfsolver_root(s);
      gsl_root_fdfsolver_iterate (s);
      status = gsl_root_test_delta(gsl_root_fdfsolver_root(s), prev,
                                   EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (status, "%s, %s (%g obs vs %g expected) ",
            gsl_root_fdfsolver_name(s), description,
            gsl_root_fdfsolver_root(s), correct_root);

  if (iterations == MAX_ITERATIONS)
    {
      gsl_test (GSL_FAILURE, "exceeded maximum number of iterations");
    }

  /* check the validity of the returned result */

  if (!WITHIN_TOL (gsl_root_fdfsolver_root(s), correct_root,
                   EPSREL, EPSABS))
    {
      gsl_test (GSL_FAILURE, "incorrect precision (%g obs vs %g expected)",
                gsl_root_fdfsolver_root(s), correct_root);

    }
  gsl_root_fdfsolver_free(s);
}

void
test_fdf_e (const gsl_root_fdfsolver_type * T,
            const char * description, gsl_function_fdf *fdf,
            double root, double correct_root)
{
  int status;
  size_t iterations = 0;
  double prev = 0 ;

  gsl_root_fdfsolver * s = gsl_root_fdfsolver_alloc(T);
  status = gsl_root_fdfsolver_set (s, fdf, root) ;

  gsl_test (status, "%s (set), %s", T->name, description);

  do
    {
      iterations++ ;
      prev = gsl_root_fdfsolver_root(s);
      gsl_root_fdfsolver_iterate (s);
      status = gsl_root_test_delta(gsl_root_fdfsolver_root(s), prev,
                                   EPSABS, EPSREL);
    }
  while (status == GSL_CONTINUE && iterations < MAX_ITERATIONS);

  gsl_test (!status, "%s, %s", gsl_root_fdfsolver_name(s),
            description, gsl_root_fdfsolver_root(s) - correct_root);
  gsl_root_fdfsolver_free(s);
}

void
my_error_handler (const char *reason, const char *file, int line, int err)
{
  if (0)
    printf ("(caught [%s:%d: %s (%d)])\n", file, line, reason, err);
}