/* specfunc/airy.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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_trig.h>
#include <gsl/gsl_sf_airy.h>

#include "error.h"
#include "check.h"

#include "chebyshev.h"
#include "cheb_eval_mode.c"

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

/* chebyshev expansions for Airy modulus and phase
   based on SLATEC r9aimp()

 Series for AM21       on the interval -1.25000D-01 to  0.
                                        with weighted error   2.89E-17
                                         log weighted error  16.54
                               significant figures required  14.15
                                    decimal places required  17.34

 Series for ATH1       on the interval -1.25000D-01 to  0.
                                        with weighted error   2.53E-17
                                         log weighted error  16.60
                               significant figures required  15.15
                                    decimal places required  17.38

 Series for AM22       on the interval -1.00000D+00 to -1.25000D-01
                                        with weighted error   2.99E-17
                                         log weighted error  16.52
                               significant figures required  14.57
                                    decimal places required  17.28

 Series for ATH2       on the interval -1.00000D+00 to -1.25000D-01
                                        with weighted error   2.57E-17
                                         log weighted error  16.59
                               significant figures required  15.07
                                    decimal places required  17.34
*/

static double am21_data[37] = {
  0.0065809191761485,
  0.0023675984685722,
  0.0001324741670371,
  0.0000157600904043,
  0.0000027529702663,
  0.0000006102679017,
  0.0000001595088468,
  0.0000000471033947,
  0.0000000152933871,
  0.0000000053590722,
  0.0000000020000910,
  0.0000000007872292,
  0.0000000003243103,
  0.0000000001390106,
  0.0000000000617011,
  0.0000000000282491,
  0.0000000000132979,
  0.0000000000064188,
  0.0000000000031697,
  0.0000000000015981,
  0.0000000000008213,
  0.0000000000004296,
  0.0000000000002284,
  0.0000000000001232,
  0.0000000000000675,
  0.0000000000000374,
  0.0000000000000210,
  0.0000000000000119,
  0.0000000000000068,
  0.0000000000000039,
  0.0000000000000023,
  0.0000000000000013,
  0.0000000000000008,
  0.0000000000000005,
  0.0000000000000003,
  0.0000000000000001,
  0.0000000000000001
};
static cheb_series am21_cs = {
  am21_data,
  36,
  -1, 1,
  20
};

static double ath1_data[36] = {
  -0.07125837815669365,
  -0.00590471979831451,
  -0.00012114544069499,
  -0.00000988608542270,
  -0.00000138084097352,
  -0.00000026142640172,
  -0.00000006050432589,
  -0.00000001618436223,
  -0.00000000483464911,
  -0.00000000157655272,
  -0.00000000055231518,
  -0.00000000020545441,
  -0.00000000008043412,
  -0.00000000003291252,
  -0.00000000001399875,
  -0.00000000000616151,
  -0.00000000000279614,
  -0.00000000000130428,
  -0.00000000000062373,
  -0.00000000000030512,
  -0.00000000000015239,
  -0.00000000000007758,
  -0.00000000000004020,
  -0.00000000000002117,
  -0.00000000000001132,
  -0.00000000000000614,
  -0.00000000000000337,
  -0.00000000000000188,
  -0.00000000000000105,
  -0.00000000000000060,
  -0.00000000000000034,
  -0.00000000000000020,
  -0.00000000000000011,
  -0.00000000000000007,
  -0.00000000000000004,
  -0.00000000000000002
};
static cheb_series ath1_cs = {
  ath1_data,
  35,
  -1, 1,
  15
};

static double am22_data[33] = {
 -0.01562844480625341,
  0.00778336445239681,
  0.00086705777047718,
  0.00015696627315611,
  0.00003563962571432,
  0.00000924598335425,
  0.00000262110161850,
  0.00000079188221651,
  0.00000025104152792,
  0.00000008265223206,
  0.00000002805711662,
  0.00000000976821090,
  0.00000000347407923,
  0.00000000125828132,
  0.00000000046298826,
  0.00000000017272825,
  0.00000000006523192,
  0.00000000002490471,
  0.00000000000960156,
  0.00000000000373448,
  0.00000000000146417,
  0.00000000000057826,
  0.00000000000022991,
  0.00000000000009197,
  0.00000000000003700,
  0.00000000000001496,
  0.00000000000000608,
  0.00000000000000248,
  0.00000000000000101,
  0.00000000000000041,
  0.00000000000000017,
  0.00000000000000007,
  0.00000000000000002
};
static cheb_series am22_cs = {
  am22_data,
  32,
  -1, 1,
  15
};

static double ath2_data[32] = {
   0.00440527345871877,
  -0.03042919452318455,
  -0.00138565328377179,
  -0.00018044439089549,
  -0.00003380847108327,
  -0.00000767818353522,
  -0.00000196783944371,
  -0.00000054837271158,
  -0.00000016254615505,
  -0.00000005053049981,
  -0.00000001631580701,
  -0.00000000543420411,
  -0.00000000185739855,
  -0.00000000064895120,
  -0.00000000023105948,
  -0.00000000008363282,
  -0.00000000003071196,
  -0.00000000001142367,
  -0.00000000000429811,
  -0.00000000000163389,
  -0.00000000000062693,
  -0.00000000000024260,
  -0.00000000000009461,
  -0.00000000000003716,
  -0.00000000000001469,
  -0.00000000000000584,
  -0.00000000000000233,
  -0.00000000000000093,
  -0.00000000000000037,
  -0.00000000000000015,
  -0.00000000000000006,
  -0.00000000000000002
};
static cheb_series ath2_cs = {
  ath2_data,
  31,
  -1, 1,
  16
};

/* Airy modulus and phase for x < -1 */
static
int
airy_mod_phase(const double x, gsl_mode_t mode, gsl_sf_result * mod, gsl_sf_result * phase)
{
  gsl_sf_result result_m;
  gsl_sf_result result_p;
  double m, p;
  double sqx;

  if(x < -2.0) {
    double z = 16.0/(x*x*x) + 1.0;
    cheb_eval_mode_e(&am21_cs, z, mode, &result_m);
    cheb_eval_mode_e(&ath1_cs, z, mode, &result_p);
  }
  else if(x <= -1.0) {
    double z = (16.0/(x*x*x) + 9.0)/7.0;
    cheb_eval_mode_e(&am22_cs, z, mode, &result_m);
    cheb_eval_mode_e(&ath2_cs, z, mode, &result_p);
  }
  else {
    mod->val = 0.0;
    mod->err = 0.0;
    phase->val = 0.0;
    phase->err = 0.0;
    GSL_ERROR ("x is greater than 1.0", GSL_EDOM);
  }

  m =  0.3125 + result_m.val;
  p = -0.625  + result_p.val;

  sqx = sqrt(-x);

  mod->val   = sqrt(m/sqx);
  mod->err  = fabs(mod->val) * (GSL_DBL_EPSILON + fabs(result_m.err/result_m.val));
  phase->val = M_PI_4 - x*sqx * p;
  phase->err = fabs(phase->val) * (GSL_DBL_EPSILON + fabs(result_p.err/result_p.val));

  return GSL_SUCCESS;
}

/* Chebyshev series for Ai(x) with x in [-1,1]
   based on SLATEC ai(x)

 series for aif        on the interval -1.00000d+00 to  1.00000d+00
                                   with weighted error   1.09e-19
                                    log weighted error  18.96
                          significant figures required  17.76
                               decimal places required  19.44

 series for aig        on the interval -1.00000d+00 to  1.00000d+00
                                   with weighted error   1.51e-17
                                    log weighted error  16.82
                          significant figures required  15.19
                               decimal places required  17.27
 */
static double ai_data_f[9] = {
  -0.03797135849666999750,
   0.05919188853726363857,
   0.00098629280577279975,
   0.00000684884381907656,
   0.00000002594202596219,
   0.00000000006176612774,
   0.00000000000010092454,
   0.00000000000000012014,
   0.00000000000000000010
};
static cheb_series aif_cs = {
  ai_data_f,
  8,
  -1, 1,
  8
};

static double ai_data_g[8] = {
   0.01815236558116127,
   0.02157256316601076,
   0.00025678356987483,
   0.00000142652141197,
   0.00000000457211492,
   0.00000000000952517,
   0.00000000000001392,
   0.00000000000000001
};
static cheb_series aig_cs = {
  ai_data_g,
  7,
  -1, 1,
  7
};

/* Chebvyshev series for Bi(x) with x in [-1,1]
   based on SLATEC bi(x)

 series for bif        on the interval -1.00000d+00 to  1.00000d+00
                                        with weighted error   1.88e-19
                                         log weighted error  18.72
                               significant figures required  17.74
                                    decimal places required  19.20

 series for big        on the interval -1.00000d+00 to  1.00000d+00
                                        with weighted error   2.61e-17
                                         log weighted error  16.58
                               significant figures required  15.17
                                    decimal places required  17.03
 */
static double data_bif[9] = {
  -0.01673021647198664948,
   0.10252335834249445610,
   0.00170830925073815165,
   0.00001186254546774468,
   0.00000004493290701779,
   0.00000000010698207143,
   0.00000000000017480643,
   0.00000000000000020810,
   0.00000000000000000018
};
static cheb_series bif_cs = {
  data_bif,
  8,
  -1, 1,
  8
};

static double data_big[8] = {
   0.02246622324857452,
   0.03736477545301955,
   0.00044476218957212,
   0.00000247080756363,
   0.00000000791913533,
   0.00000000001649807,
   0.00000000000002411,
   0.00000000000000002
};
static cheb_series big_cs = {
  data_big,
  7,
  -1, 1,
  7
};

/* Chebyshev series for Bi(x) with x in [1,8]
   based on SLATEC bi(x)
 */
static double data_bif2[10] = {
  0.0998457269381604100,
  0.4786249778630055380,
  0.0251552119604330118,
  0.0005820693885232645,
  0.0000074997659644377,
  0.0000000613460287034,
  0.0000000003462753885,
  0.0000000000014288910,
  0.0000000000000044962,
  0.0000000000000000111
};
static cheb_series bif2_cs = {
  data_bif2,
  9,
  -1, 1,
  9
};

static double data_big2[10] = {
  0.033305662145514340,
  0.161309215123197068,
  0.0063190073096134286,
  0.0001187904568162517,
  0.0000013045345886200,
  0.0000000093741259955,
  0.0000000000474580188,
  0.0000000000001783107,
  0.0000000000000005167,
  0.0000000000000000011
};
static cheb_series big2_cs = {
  data_big2,
  9,
  -1, 1,
  9
};

/* chebyshev for Ai(x) asymptotic factor
   based on SLATEC aie()

 Series for AIP        on the interval  0.          to  1.00000D+00
                   with weighted error   5.10E-17
                    log weighted error  16.29
          significant figures required  14.41
               decimal places required  17.06

 [GJ] Sun Apr 19 18:14:31 EDT 1998
 There was something wrong with these coefficients. I was getting
 errors after 3 or 4 digits. So I recomputed this table. Now I get
 double precision agreement with Mathematica. But it does not seem
 possible that the small differences here would account for the
 original discrepancy. There must have been something wrong with my
 original usage...
*/
static double data_aip[36] = {
 -0.0187519297793867540198,
 -0.0091443848250055004725,
  0.0009010457337825074652,
 -0.0001394184127221491507,
  0.0000273815815785209370,
 -0.0000062750421119959424,
  0.0000016064844184831521,
 -0.0000004476392158510354,
  0.0000001334635874651668,
 -0.0000000420735334263215,
  0.0000000139021990246364,
 -0.0000000047831848068048,
  0.0000000017047897907465,
 -0.0000000006268389576018,
  0.0000000002369824276612,
 -0.0000000000918641139267,
  0.0000000000364278543037,
 -0.0000000000147475551725,
  0.0000000000060851006556,
 -0.0000000000025552772234,
  0.0000000000010906187250,
 -0.0000000000004725870319,
  0.0000000000002076969064,
 -0.0000000000000924976214,
  0.0000000000000417096723,
 -0.0000000000000190299093,
  0.0000000000000087790676,
 -0.0000000000000040927557,
  0.0000000000000019271068,
 -0.0000000000000009160199,
  0.0000000000000004393567,
 -0.0000000000000002125503,
  0.0000000000000001036735,
 -0.0000000000000000509642,
  0.0000000000000000252377,
 -0.0000000000000000125793
/*
  -.0187519297793868
  -.0091443848250055,
   .0009010457337825,
  -.0001394184127221,
   .0000273815815785,
  -.0000062750421119,
   .0000016064844184,
  -.0000004476392158,
   .0000001334635874,
  -.0000000420735334,
   .0000000139021990,
  -.0000000047831848,
   .0000000017047897,
  -.0000000006268389,
   .0000000002369824,
  -.0000000000918641,
   .0000000000364278,
  -.0000000000147475,
   .0000000000060851,
  -.0000000000025552,
   .0000000000010906,
  -.0000000000004725,
   .0000000000002076,
  -.0000000000000924,
   .0000000000000417,
  -.0000000000000190,
   .0000000000000087,
  -.0000000000000040,
   .0000000000000019,
  -.0000000000000009,
   .0000000000000004,
  -.0000000000000002,
   .0000000000000001,
  -.0000000000000000
*/
};
static cheb_series aip_cs = {
  data_aip,
  35,
  -1, 1,
  17
};

/* chebyshev for Bi(x) asymptotic factor
   based on SLATEC bie()

 Series for BIP        on the interval  1.25000D-01 to  3.53553D-01
                   with weighted error   1.91E-17
                    log weighted error  16.72
          significant figures required  15.35
               decimal places required  17.41

 Series for BIP2       on the interval  0.          to  1.25000D-01
                   with weighted error   1.05E-18
                    log weighted error  17.98
          significant figures required  16.74
               decimal places required  18.71
*/
static double data_bip[24] = {
  -0.08322047477943447,
   0.01146118927371174,
   0.00042896440718911,
  -0.00014906639379950,
  -0.00001307659726787,
   0.00000632759839610,
  -0.00000042226696982,
  -0.00000019147186298,
   0.00000006453106284,
  -0.00000000784485467,
  -0.00000000096077216,
   0.00000000070004713,
  -0.00000000017731789,
   0.00000000002272089,
   0.00000000000165404,
  -0.00000000000185171,
   0.00000000000059576,
  -0.00000000000012194,
   0.00000000000001334,
   0.00000000000000172,
  -0.00000000000000145,
   0.00000000000000049,
  -0.00000000000000011,
   0.00000000000000001
};
static cheb_series bip_cs = {
  data_bip,
  23,
  -1, 1,
  14
};

static double data_bip2[29] = {
  -0.113596737585988679,
   0.0041381473947881595,
   0.0001353470622119332,
   0.0000104273166530153,
   0.0000013474954767849,
   0.0000001696537405438,
  -0.0000000100965008656,
  -0.0000000167291194937,
  -0.0000000045815364485,
   0.0000000003736681366,
   0.0000000005766930320,
   0.0000000000621812650,
  -0.0000000000632941202,
  -0.0000000000149150479,
   0.0000000000078896213,
   0.0000000000024960513,
  -0.0000000000012130075,
  -0.0000000000003740493,
   0.0000000000002237727,
   0.0000000000000474902,
  -0.0000000000000452616,
  -0.0000000000000030172,
   0.0000000000000091058,
  -0.0000000000000009814,
  -0.0000000000000016429,
   0.0000000000000005533,
   0.0000000000000002175,
  -0.0000000000000001737,
  -0.0000000000000000010
};
static cheb_series bip2_cs = {
  data_bip2,
  28,
  -1, 1,
  10
};

/* assumes x >= 1.0 */
inline static int
airy_aie(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  double sqx = sqrt(x);
  double z   = 2.0/(x*sqx) - 1.0;
  double y   = sqrt(sqx);
  gsl_sf_result result_c;
  cheb_eval_mode_e(&aip_cs, z, mode, &result_c);
  result->val = (0.28125 + result_c.val)/y;
  result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  return GSL_SUCCESS;
}

/* assumes x >= 2.0 */
static int airy_bie(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  const double ATR =  8.7506905708484345;
  const double BTR = -2.0938363213560543;

  if(x < 4.0) {
    double sqx = sqrt(x);
    double z   = ATR/(x*sqx) + BTR;
    double y   = sqrt(sqx);
    gsl_sf_result result_c;
    cheb_eval_mode_e(&bip_cs, z, mode, &result_c);
    result->val = (0.625 + result_c.val)/y;
    result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  }
  else {
    double sqx = sqrt(x);
    double z   = 16.0/(x*sqx) - 1.0;
    double y   = sqrt(sqx);
    gsl_sf_result result_c;
    cheb_eval_mode_e(&bip2_cs, z, mode, &result_c);
    result->val = (0.625 + result_c.val)/y;
    result->err = result_c.err/y + GSL_DBL_EPSILON * fabs(result->val);
  }

  return GSL_SUCCESS;
}

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

int
gsl_sf_airy_Ai_e(const double x, const gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result cos_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_cos = gsl_sf_cos_err_e(theta.val, theta.err, &cos_result);
    result->val  = mod.val * cos_result.val;
    result->err  = fabs(mod.val * cos_result.err) + fabs(cos_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_cos);
  }
  else if(x <= 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&aif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&aig_cs, z, mode, &result_c1);
    result->val  = 0.375 + (result_c0.val - x*(0.25 + result_c1.val));
    result->err  = result_c0.err + fabs(x*result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    double x32 = x * sqrt(x);
    double s   = exp(-2.0*x32/3.0);
    gsl_sf_result result_aie;
    int stat_aie = airy_aie(x, mode, &result_aie);
    result->val  = result_aie.val * s;
    result->err  = result_aie.err * s + result->val * x32 * GSL_DBL_EPSILON;
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    CHECK_UNDERFLOW(result);
    return stat_aie;
  }
}

int
gsl_sf_airy_Ai_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result cos_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_cos = gsl_sf_cos_err_e(theta.val, theta.err, &cos_result);
    result->val  = mod.val * cos_result.val;
    result->err  = fabs(mod.val * cos_result.err) + fabs(cos_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_cos);
  }
  else if(x <= 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&aif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&aig_cs, z, mode, &result_c1);
    result->val  = 0.375 + (result_c0.val - x*(0.25 + result_c1.val));
    result->err  = result_c0.err + fabs(x*result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);

    if(x > 0.0) {
      const double scale = exp(2.0/3.0 * sqrt(z));
      result->val *= scale;
      result->err *= scale;
    }

    return GSL_SUCCESS;
  }
  else {
    return airy_aie(x, mode, result);
  }
}

int gsl_sf_airy_Bi_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */
  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result sin_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_sin = gsl_sf_sin_err_e(theta.val, theta.err, &sin_result);
    result->val  = mod.val * sin_result.val;
    result->err  = fabs(mod.val * sin_result.err) + fabs(sin_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_sin);
  }
  else if(x < 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big_cs, z, mode, &result_c1);
    result->val  = 0.625 + result_c0.val + x*(0.4375 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x <= 2.0) {
    const double z = (2.0*x*x*x - 9.0)/7.0;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif2_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big2_cs, z, mode, &result_c1);
    result->val  = 1.125 + result_c0.val + x*(0.625 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    const double y = 2.0*x*sqrt(x)/3.0;
    const double s = exp(y);

    if(y > GSL_LOG_DBL_MAX - 1.0) {
      OVERFLOW_ERROR(result);
    }
    else {
      gsl_sf_result result_bie;
      int stat_bie = airy_bie(x, mode, &result_bie);
      result->val  = result_bie.val * s;
      result->err  = result_bie.err * s + fabs(1.5*y * (GSL_DBL_EPSILON * result->val));
      result->err += GSL_DBL_EPSILON * fabs(result->val);
      return stat_bie;
    }
  }
}

int
gsl_sf_airy_Bi_scaled_e(const double x, gsl_mode_t mode, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(x < -1.0) {
    gsl_sf_result mod;
    gsl_sf_result theta;
    gsl_sf_result sin_result;
    int stat_mp  = airy_mod_phase(x, mode, &mod, &theta);
    int stat_sin = gsl_sf_sin_err_e(theta.val, theta.err, &sin_result);
    result->val  = mod.val * sin_result.val;
    result->err  = fabs(mod.val * sin_result.err) + fabs(sin_result.val * mod.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_2(stat_mp, stat_sin);
  }
  else if(x < 1.0) {
    const double z = x*x*x;
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big_cs, z, mode, &result_c1);
    result->val  = 0.625 + result_c0.val + x*(0.4375 + result_c1.val);
    result->err  = result_c0.err + fabs(x * result_c1.err);
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    if(x > 0.0) {
      const double scale = exp(-2.0/3.0 * sqrt(z));
      result->val *= scale;
      result->err *= scale;
    }
    return GSL_SUCCESS;
  }
  else if(x <= 2.0) {
    const double x3 = x*x*x;
    const double z  = (2.0*x3 - 9.0)/7.0;
    const double s  = exp(-2.0/3.0 * sqrt(x3));
    gsl_sf_result result_c0;
    gsl_sf_result result_c1;
    cheb_eval_mode_e(&bif2_cs, z, mode, &result_c0);
    cheb_eval_mode_e(&big2_cs, z, mode, &result_c1);
    result->val  = s * (1.125 + result_c0.val + x*(0.625 + result_c1.val));
    result->err  = s * (result_c0.err + fabs(x * result_c1.err));
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    return airy_bie(x, mode, result);
  }
}

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

#include "eval.h"

double gsl_sf_airy_Ai(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Ai_e(x, mode, &result));
}

double gsl_sf_airy_Ai_scaled(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Ai_scaled_e(x, mode, &result));
}

double gsl_sf_airy_Bi(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Bi_e(x, mode, &result));
}

double gsl_sf_airy_Bi_scaled(const double x, gsl_mode_t mode)
{
  EVAL_RESULT(gsl_sf_airy_Bi_scaled_e(x, mode, &result));
}


/* DomSelfAdaptFWD.c

Forward-in-time simulation of an adaptive alleles with dominance and selfing, with linked neutral fragment.

This is the BATCH version - to be used on cluster machine to produce many simulation replicates at once.

Reps program: Reads in pre-calculated tables and introduces neutral mutations based on them. See README for more information.

Simulation uses routines found with the GNU Scientific Library (GSL)
(http://www.gnu.org/software/gsl/)
Since GSL is distributed under the GNU General Public License
(http://www.gnu.org/copyleft/gpl.html), you must download it
separately from this file.

*/

/* Preprocessor statements */
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <math.h>
#include <stddef.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <sys/stat.h>
#include <sys/types.h>

/* Function prototypes */
void Wait();
void fitness(unsigned int N, double h, double s, unsigned int *inpop, double *outfit, double *outcf, double *fitsum);
double sumT_UI(unsigned int *Tin, unsigned int nrow);
unsigned int parchoose(unsigned int N, double *culfit, double *fitsum, const gsl_rng *r);
unsigned int bitswitch(unsigned int x);
void rec_sort(double *index, unsigned int nrow);
void generation(unsigned int N, double self, double R, unsigned int *nums, unsigned int *selin, unsigned int *selout, unsigned int **neutin, unsigned int **neutout, double *culfit, double *fitsum, unsigned int npoly, double *polypos, const gsl_rng *r);
void addpoly(unsigned int N, unsigned int **neutin, double *polyloc, unsigned int *npoly, double theta, unsigned int maxsize, const gsl_rng *r);
void reassign(unsigned int **neutin, unsigned int **neutout, unsigned int *selin, unsigned int *selout, unsigned int npoly, unsigned int N);
void reassign2(unsigned int **neutin, unsigned int **neutout, double *posin, double *posout, unsigned int npoly, unsigned int N);
void polyprint(unsigned int **neutin, double *posin, unsigned int npoly, unsigned int N);
void ptrim(unsigned int size, unsigned int **neutin, unsigned int **neutout, double *posin, double *posout, unsigned int npoly, unsigned int *npolyT, double *avpi);
void mutsamp(unsigned int N, unsigned int samps, unsigned int *selin, double *polypos, unsigned int **neutin, unsigned int *nums2, unsigned int npoly, unsigned int ei, unsigned int rep, const gsl_rng *r);
void popread(unsigned int **neutin, double *posin, unsigned int npoly, unsigned int N, unsigned int ei, unsigned int suffix);

void Wait(){
	printf("Press Enter to Continue");
	while( getchar() != '\n' );
	printf("\n");
}

/* Fitness calculation routine */
void fitness(unsigned int N, double h, double s, unsigned int *inpop, double *outfit, double *outcf, double *fitsum){
	unsigned int i;				/* Pop counter */
	unsigned int ac = 0;		/* Allele copies in indv */

	/* Calculating new fitnesses */
    *fitsum = 0;
	for(i = 0; i < N; i++){
		ac = 0;
		ac = (*(inpop + 2*i)) + (*(inpop + 2*i + 1));
		if(ac == 0){
			*(outfit + i) = 1;
		}else if(ac == 1){
			*(outfit + i) = 1 + h*s;
		}else if(ac == 2){
			*(outfit + i) = 1 + s;
		}
		*fitsum += *(outfit + i);
		*(outcf + i) = *fitsum;
    }
}	/* End of fitness routine */

/* Summing entire table (unsigned int) */
double sumT_UI(unsigned int *Tin, unsigned int nrow){
	unsigned int i;
	double res = 0;
	for(i = 0; i < nrow; i++){
		res += (*(Tin + i));
	}
	return res;
}

/* Parent choosing */
unsigned int parchoose(unsigned int N, double *culfit, double *fitsum, const gsl_rng *r){

	double a;					/* Random number */
	unsigned int done = 0;		/* Signal to determine whether selection complete */
	unsigned int cases = 0;		/* Determines whether one should add up or decrease */
	unsigned int choose1 = 0;	/* Chromosome to be selected */
	unsigned int firstgo = 0;	/* First try at choosing progeny */

	done = 0;
	cases = 0;
	choose1 = 0;
	a = gsl_ran_flat(r,0.0,1.0);
	firstgo = (int)N*a;

	/* Choosing case */
	if (((*(culfit + firstgo)/(*(fitsum))) >= a) && firstgo == 0){
		cases = 3;
	} else if (((*(culfit + firstgo)/(*(fitsum))) >= a) && firstgo != 0){
		cases = 2;
	} else {
		cases = 1;
	}

	switch(cases)
	{
		case 1:
			while(done != 1){
				firstgo++;
				if((*(culfit + firstgo)/(*(fitsum))) >= a){
					choose1 = firstgo;
					done = 1;
				}
			}
			break;
		case 2:
			while(done != 1){
				firstgo--;
				if((*(culfit + firstgo)/(*(fitsum))) < a || firstgo == 0){
					choose1 = (firstgo + 1);
					done = 1;
				}
			}
		break;
		case 3:
			choose1 = 0;
			break;
	}

	return choose1;
}

/* Bit-switching routine */
unsigned int bitswitch(unsigned int x){
	unsigned int ret;
	switch(x)
	{
		case 0:
			ret = 1;
			break;
		case 1:
			ret = 0;
			break;
		default:	/* If none of these cases chosen, exit with error message */
			fprintf(stderr,"Error in bitswitch: input is not 0 or 1.\n");
			exit(1);
            break;
	}
	return ret;
}

/* Sorting rec events after choosing them */
void rec_sort(double *index, unsigned int nrow){
	unsigned int j, i;		/* Sorting indices */
	double temp0;		/* For swapping */

	for(j = 1; j < nrow; j++){
		i = j;
		while( (i > 0) && ( *(index + (i - 1) ) > *(index + i) )){
			/* Swapping entries */
			temp0 = *(index + (i - 1));
			*(index + (i - 1)) =  *(index + (i));
			*(index + (i)) = temp0;
			i--;
		}
	}

}

/* Reproduction routine */
void generation(unsigned int N, double self, double R, unsigned int *nums, unsigned int *selin, unsigned int *selout, unsigned int **neutin, unsigned int **neutout, double *culfit, double *fitsum, unsigned int npoly, double *polypos, const gsl_rng *r){
	unsigned int i, j, k;		/* Pop counter, neutral marker counter, rec event counter */
	unsigned int isself = 0;	/* Is this a selfing reproduction? */
	unsigned int choose1 = 0;	/* Chromosome to be selected */
	unsigned int choose2 = 0;	/* Chromosome to be selected (2nd with outcrossing) */
	unsigned int wc1 = 0;		/* Which chrom (parent 1) */
	unsigned int wc2 = 0;		/* Which chrom (parent 2) */
	unsigned int index1 = 0;	/* Index of chosen sample 1 */
	unsigned int index2 = 0;	/* Index of chosen sample 2 */
	unsigned int nself = 0;		/* Number of selfing events */
	unsigned int selfix = 0;	/* Selfing index */
	unsigned int recix1 = 0;	/* Rec index chr 1 */
	unsigned int recix2 = 0;	/* Rec index chr 2 */
	unsigned int nrec1 = 0;		/* Number rec events chr 1 */
	unsigned int nrec2 = 0;		/* Number rec events chr 2 */

	/* First deciding number of selfing events; creating vector of events + choosing */
	nself = gsl_ran_binomial(r, self, N);
	unsigned int *selfev = calloc(nself,sizeof(unsigned int));
	gsl_ran_choose(r, selfev, nself, nums, N, sizeof(unsigned int));

	for(i = 0; i < N; i++){		/* Regenerating population */

    	/* Choosing first parent, and relevant chromosomes */
		choose1 = parchoose(N, culfit, fitsum, r);
		wc1 = gsl_ran_bernoulli(r,0.5);
		wc2 = gsl_ran_bernoulli(r,0.5);
		index1 = 2*choose1 + wc1;

		/* Copying over selected sites, first checking if selfing occurs */
		isself = 0;
		if(nself != 0){
			if(*(selfev + selfix) == i){
				isself = 1;
				selfix++;
			}
		}
		if(isself == 1){
			index2 = 2*choose1 + wc2;
		}else if(isself == 0){
			choose2 = parchoose(N, culfit, fitsum, r);
			index2 = 2*choose2 + wc2;
		}
		(*(selout + 2*i)) = (*(selin + index1));
		(*(selout + 2*i + 1)) = (*(selin + index2));

		/* Now copying over neutral fragment, accounting for recombination */
		if(R != 0){
			/* Choosing number of recombination events on arms 1, 2 */
			recix1 = 0;
			recix2 = 0;
			nrec1 = gsl_ran_poisson(r,R);
			nrec2 = gsl_ran_poisson(r,R);
			double *recev1 = calloc(nrec1 + 1,sizeof(double));
			double *recev2 = calloc(nrec2 + 1,sizeof(double));
			for(k = 0; k < nrec1; k++){
				*(recev1 + k) = gsl_ran_flat(r,0,1);
			}
			for(k = 0; k < nrec2; k++){
				*(recev2 + k) = gsl_ran_flat(r,0,1);
			}
			*(recev1 + nrec1) = 1.01;
			*(recev2 + nrec2) = 1.01;
			rec_sort(recev1,nrec1 + 1);
			rec_sort(recev2,nrec2 + 1);

			for(j = 0; j < npoly; j++){
				if( (*(polypos + j)) >= *(recev1 + recix1)){
					wc1 = bitswitch(wc1);
					index1 = 2*choose1 + wc1;
					recix1++;
				}
				if( (*(polypos + j)) >= *(recev2 + recix2)){
					wc2 = bitswitch(wc2);
					if(isself == 1){
						index2 = 2*choose1 + wc2;
					}else if(isself == 0){
						index2 = 2*choose2 + wc2;
					}
					recix2++;
				}

				*((*(neutout + 2*i)) + j) = *((*(neutin + index1)) + j);
				*((*(neutout + 2*i + 1)) + j) = *((*(neutin + index2)) + j);

			}

			free(recev1);
			free(recev2);

		}else if (R == 0){
			for(j = 0; j < npoly; j++){
				*((*(neutout + 2*i)) + j) = *((*(neutin + index1)) + j);
				*((*(neutout + 2*i + 1)) + j) = *((*(neutin + index2)) + j);
			}
		}
	}

	free(selfev);

}	/* End of reproduction routine */

/* Adding neutral polymorphism routine */
void addpoly(unsigned int N, unsigned int **neutin, double *polyloc, unsigned int *npoly, double theta, unsigned int maxsize, const gsl_rng *r){
	unsigned int i, j;
	int k;
	unsigned int newpoly = 0;			/* Number of new polymorphisms added */
	unsigned int pchr = 0;				/* Chromosome of appearance */
	unsigned int pfound = 0;			/* Flag to denote if right index found */
	double ploc = 0;					/* Location of polymorphism */
/*	FILE *ofp_poly;				 Pointer for polymorphisms output */

	newpoly = gsl_ran_poisson(r,(0.5*theta));
/*	printf("Add %d new polymorphisms\n",newpoly);*/

	for(j = 0; j < newpoly; j++){
		ploc = gsl_ran_flat(r,0.0,1.0);
/*		printf("ploc is %lf\n",ploc);		*/
		pchr = gsl_rng_uniform_int(r,2*N);
/*		printf("pchr is %d\n",pchr);		*/

		/* Inserting new polymorphism */
		if(*(npoly) == 0){
/*			printf("zero entry here\n");	*/
			*(polyloc + 0) = ploc;
				for(i = 0; i < 2*N; i++){
					*((*(neutin + i)) + 0) = 0;
				}
			*((*(neutin + pchr)) + 0) = 1;
		}else if(*(npoly) > 0){
/*			printf("non-zero entry here\n");		*/
			pfound = 0;
			for(k = (*(npoly) - 1); k >= 0; k--){
/*				printf("k is %d\n",k);*/
				if(*(polyloc + k) > ploc){
/*					printf("is here 1\n");*/
					*(polyloc + k + 1) = *(polyloc + k);
					for(i = 0; i < 2*N; i++){
						*((*(neutin + i)) + k + 1) = *((*(neutin + i)) + k);
					}
				}else if(*(polyloc + k) <= ploc){
/*					printf("is here 2\n");				*/
					pfound = 1;
					*(polyloc + k + 1) = ploc;
					for(i = 0; i < 2*N; i++){
						*((*(neutin + i)) + k + 1) = 0;
					}
					*((*(neutin + pchr)) + k + 1) = 1;
					break;
				}
			}

			if(pfound == 0){		/* In this case polymorphism is inserted at the start */
				*(polyloc + 0) = ploc;
				for(i = 0; i < 2*N; i++){
					*((*(neutin + i)) + 0) = 0;
				}
				*((*(neutin + pchr)) + 0) = 1;
			}

		}

		*(npoly) += 1;
/*		printf("n poly is %d\n",*(npoly));			*/
		if(*(npoly) == maxsize){
			fprintf(stderr,"Too many neutral polymorphisms.\n");
			exit(1);
		}

	}
}

/* Reassigning new population routine */
void reassign(unsigned int **neutin, unsigned int **neutout, unsigned int *selin, unsigned int *selout, unsigned int npoly, unsigned int N){
	unsigned int i, j;		/* pop counter, poly counter */

	for(i = 0; i < 2*N; i++){
		*(selout + i) = *(selin + i);
		for(j = 0; j < npoly; j++){
			*((*(neutout + i)) + j) = *((*(neutin + i)) + j);
		}
	}

}	/* End of reassignment routine */

/* Reassigning new population routine (after trimming) */
void reassign2(unsigned int **neutin, unsigned int **neutout, double *posin, double *posout, unsigned int npoly, unsigned int N){
	unsigned int i, j;		/* pop counter, poly counter */

	for(i = 0; i < 2*N; i++){
		for(j = 0; j < npoly; j++){
			*((*(neutout + i)) + j) = *((*(neutin + i)) + j);
			if(i == 0){
				*(posout + j) = *(posin + j);
			}
		}
	}

}	/* End of reassignment routine */

/* Printing polymorphism routines */
void polyprint(unsigned int **neutin, double *posin, unsigned int npoly, unsigned int N){
	unsigned int i, j;		/* pop counter, poly counter */

	for(j = 0; j < npoly; j++){
		printf("%lf ",*(posin + j));
		for(i = 0; i < 2*N; i++){
			printf("%d ",*((*(neutin + i)) + j));
		}
		printf("\n");
	}
	Wait();

}	/* End of reassignment routine */

/* Cutting out non-polymorphic sites */
void ptrim(unsigned int size, unsigned int **neutin, unsigned int **neutout, double *posin, double *posout, unsigned int npoly, unsigned int *npolyT, double *avpi){
	unsigned int i, j;
	unsigned int newp = 0;		/* New number of polymorphisms */
	unsigned int count = 0;		/* Polymorphism count */
	double cumpi = 0;			/* Cumulative pi (diversity) */

/*	printf("Trimming routine activated\n");	*/
	for(j = 0; j < npoly; j++){
		count = 0;
		*(posout + newp) = *(posin + j);
		for(i = 0; i < size; i++){
			*((*(neutout + i)) + newp) = *((*(neutin + i)) + j);
			count += *((*(neutin + i)) + j);
		}
/*		printf("For poly %d, count is %d\n",j,count);*/
		if((count > 0) && (count < size)){
/*			printf("Keeping poly %d\n",j);	*/
			newp++;
			cumpi += ((count*(size-count))/(1.0*size*size));
/*			printf("Count is %d, freq is %lf, cumulative value is %lf\n",count,count/(1.0*size),cumpi);	*/
		}
	}

	*npolyT = newp;
	*avpi = (cumpi/(1.0*newp));
}

/* Producing mutational samples */
void mutsamp(unsigned int N, unsigned int samps, unsigned int *selin, double *polypos, unsigned int **neutin, unsigned int *nums2, unsigned int npoly, unsigned int ei, unsigned int rep, const gsl_rng *r){

	unsigned int a, j, x;
	unsigned int currsamp;
	unsigned int npolyT;
	double avpi;
	char Mout[32];				 /* String to hold filename in (Mutations) */
	FILE *ofp_mut = NULL;		 /* Pointer for data output */

	for(x = 0; x < rep; x++){

		double *polyposF = calloc(npoly,sizeof(double));						/* Position of neutral mutations (final for polymorphism sampling) */
		unsigned int *selsamp = calloc(samps,sizeof(unsigned int *));			/* State of selected locus after sim stops */
		unsigned int **nsamp = calloc(samps,sizeof(unsigned int *));			/* Table of neutral markers per individual (final for sampling) */
		unsigned int **nsampT = calloc(samps,sizeof(unsigned int *));			/* Table of neutral markers per individual (final for sampling) AFTER TRIMMING */
		for(a = 0; a < samps; a++){
			nsamp[a] = calloc(npoly,sizeof(unsigned int));
			nsampT[a] = calloc(npoly,sizeof(unsigned int));
		}

		npolyT = npoly;

		/* Choosing which haplotypes to sample */
		unsigned int *thesamp = calloc(samps,sizeof(unsigned int));
		gsl_ran_choose(r, thesamp, samps, nums2, 2*N, sizeof(unsigned int));

		for(a = 0; a < samps; a++){
			currsamp = *(thesamp + a);
			*(selsamp + a) = *(selin + currsamp);
			for(j = 0; j < npoly; j++){
				*((*(nsamp + a)) + j) = *((*(neutin + currsamp)) + j);
			}
		}

		/* Check, then include code to ONLY print out polymorphic sites in table? */
		ptrim(samps, nsamp, nsampT, polypos, polyposF, npoly, &npolyT, &avpi);

		/* Printing out sample table */
		sprintf(Mout,"Mutations/Muts_%d.dat",ei*rep + x);
		ofp_mut = fopen(Mout,"w");

		/* State of selected site */
		fprintf(ofp_mut,"0 ");
		for(a = 0; a < samps; a++){
			fprintf(ofp_mut,"%d ",*(selsamp + a));
		}
		fprintf(ofp_mut,"\n");

		for(j = 0; j < npolyT; j++){
			fprintf(ofp_mut,"%lf ",*(polyposF + j));
			for(a = 0; a < samps; a++){
				fprintf(ofp_mut,"%d ",*((*(nsampT + a)) + j));
			}
			fprintf(ofp_mut,"\n");
		}
		fclose(ofp_mut);

		for(a = 0; a < samps; a++){
			free(nsampT[a]);
			free(nsamp[a]);
		}
		free(nsampT);
		free(nsamp);
		free(thesamp);
		free(selsamp);
		free(polyposF);

	}
}

/* Reading population state from file */
void popread(unsigned int **neutin, double *posin, unsigned int npoly, unsigned int N, unsigned int ei, unsigned int suffix){
	unsigned int i, j;		/* pop counter, poly counter */
	char Pin[32];				 /* String to hold filename in (Mutations) */
	FILE *ifp_poly = NULL;		 /* Pointer for data output */

	/* Printing out polymorphism table */
	sprintf(Pin,"Pop_%d.dat",ei + suffix);
	ifp_poly = fopen(Pin,"r");
	for(j = 0; j < npoly; j++){
		/* Is this the right way of reading in the first column of file? */
		for(i = 0; i < (2*N + 1); i++){
			if(i == 0){
				fscanf(ifp_poly,"%lf",&(posin[j]));
			}else if(i > 0){
				fscanf(ifp_poly,"%d",&(neutin[(i-1)][j]));
			}
		}
		fprintf(ifp_poly,"\n");
	}
	fclose(ifp_poly);

}	/* End of population reading routine */

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

	/* Declare variables here */
	unsigned int a, i, x;		/* Counters */
	unsigned int N = 0;			/* Population Size */
	unsigned int initp = 0;		/* Initial location of mutant (individual) */
	unsigned int done = 0;		/* Is simulation done or not? */
	unsigned int dcopies = 0;	/* Number of copies of derived allele */
	unsigned int afix = 0;		/* Has allele fixed? */
	unsigned int npoly = 0;		/* Number of neutral alleles */
	unsigned int npolyB = 0;	/* BASELINE number of neutral alleles at simulation initiation */
	unsigned int npolyT = 0;	/* Number of neutral alleles (after cutting out fixed sites) */
	unsigned int nowsel = 0;	/* If derived allele selected for? */
	unsigned int samps = 0;		/* Number of samples to take */
	unsigned int suffix = 0;	/* Number of times to subsample from population */
	unsigned int base = 0;		/* Number of baseline forward-in-time simulations */
	unsigned int rps = 0;		/* Samples taken per baseline simulation */
	unsigned int INITTS = 0;	/* Initial size of mutation array */
/*	unsigned int n = 0;			sprintf counter */
	double s = 0;				/* Strength of selection */
	double h = 0;				/* Dominance level */
	double self = 0;			/* Selfing rate */
	double Rin = 0;				/* Recombination rate (input) */
	double R = 0;				/* Recombination rate (at a time) */
	double x0 = 0;				/* Initial allele frequency */
	double xfix = 0;			/* Final allele frequency (i.e. stop when allele reaches this frequency) */
	double theta = 0;			/* Rate of neutral mutation */
	double fitsum = 0;			/* Summed Fitness */
	double scurr = 0;			/* Current fitness of mutant (to account for initial neutrality) */
	double avpi = 0;
	char Pout[32];				/* String to hold filename in (Mutations) */
	char Sout[32];				/* String to hold filename in (Seed) */
	FILE *ofp_sd;				/* Pointer for seed output */
	FILE *ofp_poly;				/* Pointer for polymorphisms output */

	/* GSL random number definitions */
	const gsl_rng_type * T;
	gsl_rng * r;

	/* Reading in data from command line */
	if(argc != 14){
		fprintf(stderr,"Not enough inputs (need: N s h self R x0 xfix theta basereps reps-per-sim samples suffix npoly).\n");
		exit(1);
	}

	N = atoi(argv[1]);
	if(N <= 0){
		fprintf(stderr,"Total Population size N is zero or negative, not allowed.\n");
		exit(1);
	}

	s = strtod(argv[2],NULL);
	if(s < 0){
		fprintf(stderr,"Allele strength s is negative, not allowed.\n");
		exit(1);
	}

	h = strtod(argv[3],NULL);
	if(h < 0 || h > 1){
		fprintf(stderr,"Dominance value must lie between 0 and 1.\n");
		exit(1);
	}

	self = strtod(argv[4],NULL);
	if(self < 0 || self > 1){
		fprintf(stderr,"Selfing rate must lie between 0 and 1 (inclusive).\n");
		exit(1);
	}

	Rin = strtod(argv[5],NULL);
	if(Rin < 0){
		fprintf(stderr,"Recombination rate must be positive or zero.\n");
		exit(1);
	}
	Rin /= 2.0*N;

	x0 = strtod(argv[6],NULL);
	if(x0 < (1.0/(2.0*N)) || x0 > 1.0-(1.0/(2.0*N)) ){
		fprintf(stderr,"Initial mutant frequency must lie between 1/2N and 1-1/2N.\n");
		exit(1);
	}

	xfix = strtod(argv[7],NULL);
	if(xfix < 0 || xfix > 1.0 ){
		fprintf(stderr,"Final mutant frequency must lie between 0 and 1.\n");
		exit(1);
	}
	if(xfix <= x0){
		fprintf(stderr,"Final mutant frequency must be greater than initial mutant frequency.\n");
		exit(1);
	}

	theta = strtod(argv[8],NULL);
	if(theta <= 0){
		fprintf(stderr,"Mutation rate must be a positive value.\n");
		exit(1);
	}

	base = atoi(argv[9]);
	if(base <= 0){
		fprintf(stderr,"Number of baseline simulations must be greater than zero.\n");
		exit(1);
	}

	rps = atoi(argv[10]);
	if(rps <= 0){
		fprintf(stderr,"Number of reps per simulation must be > 0.\n");
		exit(1);
	}

	samps = atoi(argv[11]);
	if(samps <= 0){
		fprintf(stderr,"Number of samples taken must be > 0.\n");
		exit(1);
	}

	suffix = atoi(argv[12]);
	if(argv[12] < 0){
		fprintf(stderr,"File index must be greater than or equal to zero.\n");
		exit(1);
	}

	npolyB = atoi(argv[13]);
	INITTS = 2*npolyB;

	/* create a generator chosen by the
    environment variable GSL_RNG_TYPE */

    mkdir("SeedsRep/", 0777);
	gsl_rng_env_setup();
	if (!getenv("GSL_RNG_SEED")) gsl_rng_default_seed = time(0);
	T = gsl_rng_default;
	r = gsl_rng_alloc(T);
	sprintf(Sout,"SeedsRep/Seed_%d.dat",suffix);
	ofp_sd = fopen(Sout,"w");
	fprintf(ofp_sd,"%lu\n",gsl_rng_default_seed);
	fclose(ofp_sd);

	mkdir("Polymorphisms/", 0777);
	mkdir("Mutations/", 0777);

	/* Vector of 0 to (N-1) for sampling random numbers */
	unsigned int *nums = calloc(N,sizeof(unsigned int));
	unsigned int *nums2 = calloc(2*N,sizeof(unsigned int));
	for(a = 0; a < 2*N; a++){
		*(nums2 + a) = a;
		if(a < N){
			*(nums + a) = a;
		}
	}

	/* Executing simulation */
	for(i = 0; i < base; i++){

/*		printf("Starting run %d\n",i);*/

		afix = 0;
		R = Rin;
		while(afix != 1){

			/* Setting up selection, neutral tables per individual */
			double *fit = calloc(N,sizeof(double));									/* Fitness of each individual */
			double *cumfit = calloc(N,sizeof(double));								/* Cumulative fitness of each individual */
			double *polypos = calloc(INITTS,sizeof(double));						/* Position of neutral mutations */
			double *polyposF = calloc(INITTS,sizeof(double));						/* Position of neutral mutations (final for polymorphism sampling) */
			unsigned int *selindvP = calloc(2*N,sizeof(unsigned int));				/* Table of derived allele states per individual (parents) */
			unsigned int *selindvO = calloc(2*N,sizeof(unsigned int));				/* Table of derived allele states per individual (offspring) */
			unsigned int **neutindvP = calloc(2*N,sizeof(unsigned int *));			/* Table of neutral markers per individual (parents) */
			unsigned int **neutindvO = calloc(2*N,sizeof(unsigned int *));			/* Table of neutral markers per individual (offspring) */
			unsigned int **neutindvF = calloc(2*N,sizeof(unsigned int *));			/* Table of neutral markers per individual (final for sampling) */
			for(a = 0; a < 2*N; a++){
				neutindvP[a] = calloc(INITTS,sizeof(unsigned int));
				neutindvO[a] = calloc(INITTS,sizeof(unsigned int));
				neutindvF[a] = calloc(INITTS,sizeof(unsigned int));
			}

			/* Reassigning ancestral polymorphism state */
			npoly = npolyB;
			popread(neutindvP, polypos, npoly, N, i, suffix);
/*
			polyprint(neutindvP, polypos, npoly, N);
			exit(1);
*/
			/* Assigning initial mutant, calculating fitness */
			initp = gsl_rng_uniform_int(r,2*N);
			*(selindvP + initp) = 1;

			if(x0 == 1.0/(2.0*N)){
				scurr = s;
				nowsel = 1;
			}else if(x0 > 1.0/(2.0*N)){
				scurr = 0;
				nowsel = 0;
			}
	/*		printf("scurr is %lf\n",scurr);	*/
			fitness(N, h, scurr, selindvP, fit, cumfit, &fitsum);

			done = 0;
			while(done != 1){

				/* Generating new population */
				generation(N,self,R,nums,selindvP,selindvO,neutindvP,neutindvO,cumfit,&fitsum,npoly,polypos,r);

				/* Neutral Mutation phase */
				addpoly(N, neutindvO, polypos, &npoly, theta, INITTS, r);

				/* Reassigning matrices */
				reassign(neutindvO, neutindvP, selindvO, selindvP, npoly, N);

				/* Garbage collection of non-polymorphic sites */
				ptrim(2*N, neutindvP, neutindvF, polypos, polyposF, npoly, &npolyT,&avpi);
				npoly = npolyT;
				reassign2(neutindvF, neutindvP, polyposF, polypos, npoly, N);

				/* Checking derived allele copies and whether it has fixed or lost */
				dcopies = sumT_UI(selindvP, 2.0*N);
				/*
				printf("Dcopies are %d\n",dcopies);
				printf("copies are %d, npoly are %d\n",dcopies,npoly);
				*/
				if(dcopies == 0){
					done = 1;
/*					printf("Selected allele lost\n");*/
				}
				if(dcopies >= (int)(2.0*N*xfix)){
					done = 1;
					afix = 1;
				}

				/* Deciding if derived allele becomes selected for, then updating fitness */
				if(((dcopies/(2.0*N)) >= x0) && nowsel == 0){
					nowsel = 1;
					scurr = s;
/*					printf("scurr is %lf\n",scurr);	*/
				}
				fitness(N, h, scurr, selindvP, fit, cumfit, &fitsum);
			}

			/* Routine to print out polymorphisms if allele fixed */
			if(afix == 1){

				printf("Selected allele fixed\n");

				/* First, remove non-polymorphic sites */
				ptrim(2*N, neutindvP, neutindvF, polypos, polyposF, npoly, &npolyT,&avpi);
				npoly = npolyT;

				/* Printing out polymorphism table */
				sprintf(Pout,"Polymorphisms/Poly_%d.dat",i + suffix);
				ofp_poly = fopen(Pout,"w");

				/* State of selected site */
				fprintf(ofp_poly,"0 ");
				for(x = 0; x < 2*N; x++){
					fprintf(ofp_poly,"%d ",*(selindvP + x));
				}
				fprintf(ofp_poly,"\n");

				/* State of neutral sites */
				for(a = 0; a < npoly; a++){
					fprintf(ofp_poly,"%lf ",*(polyposF + a));
					for(x = 0; x < 2*N; x++){
						fprintf(ofp_poly,"%d ",*((*(neutindvF + x)) + a));
					}
					fprintf(ofp_poly,"\n");
				}
				fclose(ofp_poly);

				/* Then, sample from this table to produce pseudo-coalescent results */
				mutsamp(N, samps, selindvP, polyposF, neutindvF, nums2, npoly, i + suffix, rps, r);

			}

			/* End of run; freeing memory */
			for(a = 0; a < 2*N; a++){
				free(neutindvF[a]);
				free(neutindvO[a]);
				free(neutindvP[a]);
			}
			free(neutindvF);
			free(neutindvO);
			free(neutindvP);
			free(selindvO);
			free(selindvP);
			free(polyposF);
			free(polypos);
			free(cumfit);
			free(fit);
		}

	}

	free(nums2);
	free(nums);
	gsl_rng_free(r);
	return 0;

}	/* End of main program */

/* End of File */


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

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

#include "ccl.h"

/*
 * Spline the linear power spectrum for mu-Sigma MG cosmologies.
 * @param cosmo Cosmological parameters
 ^ @param psp The linear power spectrum to spline.
 * @param status, integer indicating the status
 */
void ccl_rescale_musigma_s8(ccl_cosmology* cosmo, ccl_f2d_t *psp,
                            int mg_rescale, int* status) {

  int do_mg = mg_rescale && (fabs(cosmo->params.mu_0) > 1e-14);
  int do_s8 = isfinite(cosmo->params.sigma8) && (!isfinite(cosmo->params.A_s));

  if (!do_mg && !do_s8)
    return;

  size_t na;
  double *aa;
  double rescale_extra_musig = 1;
  double *rescale_factor = NULL;

  if (*status == 0) {
    // Get array of scale factors
    if(psp->fa != NULL) {
      na = psp->fa->size;
      aa = psp->fa->x;
    }
    else if(psp->fka != NULL) {
      na = psp->fka->interp_object.ysize;
      aa = psp->fka->yarr;
    }
    else {
      *status = CCL_ERROR_SPLINE;
      ccl_cosmology_set_status_message(cosmo,
             "ccl_musigma.c: ccl_rescale_musigma_s8(): "
             "input pk2d has no splines.\n");
    }
  }

  // Alloc rescale_factor
  if(*status == 0) {
    rescale_factor = malloc(na * sizeof(double));
    if(rescale_factor == NULL) {
      *status = CCL_ERROR_MEMORY;
      ccl_cosmology_set_status_message(cosmo,
             "ccl_musigma.c: ccl_rescale_musigma_s8(): memory allocation\n");
    }
  }

  // set to 1 by default
  if(*status == 0) {
    for(int i=0; i<na; i++)
      rescale_factor[i]=1;
  }

  // If scale-independent mu / Sigma modified gravity is in use
  // and mu ! = 0 : get the unnormalized growth factor in MG and for
  // corresponding GR case, to rescale CLASS power spectrum
  if(do_mg) {
    // Set up another cosmology which is exactly the same as the
    // current one but with mu_0 and Sigma_0=0, for scaling P(k)

    // Get a list of the three neutrino masses already calculated
    double norm_pk;
    double mnu_list[3] = {0, 0, 0};
    ccl_parameters params_GR;
    params_GR.m_nu = NULL;
    params_GR.z_mgrowth = NULL;
    params_GR.df_mgrowth = NULL;
    ccl_cosmology * cosmo_GR = NULL;

    for (int i=0; i< cosmo->params.N_nu_mass; i=i+1)
      mnu_list[i] = cosmo->params.m_nu[i];

    if (isfinite(cosmo->params.A_s)) {
      norm_pk = cosmo->params.A_s;
    }
    else if (isfinite(cosmo->params.sigma8)) {
      norm_pk = cosmo->params.sigma8;
    }
    else {
      *status = CCL_ERROR_PARAMETERS;
      ccl_cosmology_set_status_message(cosmo,
             "ccl_musigma.c: ccl_rescale_musigma_s8(): "
             "neither A_s nor sigma8 defined.\n");
    }

    if (*status == 0) {
      params_GR = ccl_parameters_create(
        cosmo->params.Omega_c, cosmo->params.Omega_b, cosmo->params.Omega_k,
        cosmo->params.Neff, mnu_list, cosmo->params.N_nu_mass,
        cosmo->params.w0, cosmo->params.wa, cosmo->params.h,
        norm_pk, cosmo->params.n_s,
        cosmo->params.bcm_log10Mc, cosmo->params.bcm_etab,
        cosmo->params.bcm_ks, 0., 0., cosmo->params.nz_mgrowth,
        cosmo->params.z_mgrowth, cosmo->params.df_mgrowth, status);

      if (*status) {
        *status = CCL_ERROR_PARAMETERS;
        ccl_cosmology_set_status_message(cosmo,
               "ccl_musigma.c: ccl_rescale_musigma_s8(): "
               "could not make MG params.\n");
      }
    }

    if (*status == 0) {
      cosmo_GR = ccl_cosmology_create(params_GR, cosmo->config);

      if (cosmo_GR == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(cosmo,
               "ccl_musigma.c: ccl_rescale_musigma_s8(): "
               "error initializing cosmology\n");
      }
    }

    if (*status == 0) {
      ccl_cosmology_compute_growth(cosmo_GR, status);

      if (*status) {
        *status = CCL_ERROR_PARAMETERS;
        ccl_cosmology_set_status_message(cosmo,
               "ccl_musigma.c: ccl_rescale_musigma_s8(): "
               "could not init GR growth.\n");
      }
    }

    // Populate rescale_factor
    if (*status == 0) {
      for (int i=0; i<na; i++) {
        double D_mu = ccl_growth_factor_unnorm(cosmo, aa[i], status);
        double D_GR = ccl_growth_factor_unnorm(cosmo_GR, aa[i], status);
        double renorm = D_mu/D_GR;
        rescale_factor[i] *= renorm*renorm;
      }
      rescale_extra_musig = rescale_factor[na-1];

      if (*status) {
        *status = CCL_ERROR_PARAMETERS;
        ccl_cosmology_set_status_message(cosmo,
               "ccl_musigma.c: ccl_rescale_musigma_s8(): "
               "could not make MG and GR growth.\n");
      }
    }

    ccl_parameters_free(&params_GR);
    ccl_cosmology_free(cosmo_GR);
  }

  if(do_s8) {
    if (*status == 0) {
      double renorm = cosmo->params.sigma8/ccl_sigma8(cosmo, psp, status);
      renorm *= renorm;
      renorm /= rescale_extra_musig;

      for (int i=0; i<na; i++)
        rescale_factor[i] *= renorm;
    }
  }

  if (*status == 0) {
    size_t nk=0;
    if (psp->fka != NULL)
      nk=psp->fka->interp_object.xsize;

    // Rescale
    for (int i=0; i<na; i++) {
      if (psp->fa != NULL) {
        if(psp->is_log)
          psp->fa->y[i] += log(rescale_factor[i]);
        else
          psp->fa->y[i] *= rescale_factor[i];
      }
      else {
        for(int j=0; j<nk; j++) {
          if(psp->is_log)
            psp->fka->zarr[i*nk+j] += log(rescale_factor[i]);
          else
            psp->fka->zarr[i*nk+j] *= rescale_factor[i];
        }
      }
    }

    //Respline
    if(psp->fa != NULL) {
      gsl_spline *fa = gsl_spline_alloc(gsl_interp_cspline,
                                        psp->fa->size);
      if(fa == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(cosmo,
                                         "ccl_musigma.c: ccl_rescale_musigma_s8(): "
                                         "memory allocation\n");
      }
      if(*status==0) {
        int spstatus = gsl_spline_init(fa, psp->fa->x,
                                       psp->fa->y, psp->fa->size);
        if(spstatus) {
          *status = CCL_ERROR_MEMORY;
          ccl_cosmology_set_status_message(cosmo,
                                           "ccl_musigma.c: ccl_rescale_musigma_s8(): "
                                           "Error initializing spline\n");
        }
      }
      if(*status==0) {
        gsl_spline_free(psp->fa);
        psp->fa=fa;
      }
    }
    else {
      gsl_spline2d *fka = gsl_spline2d_alloc(gsl_interp2d_bicubic,
                                             psp->fka->interp_object.xsize,
                                             psp->fka->interp_object.ysize);
      if(fka == NULL) {
        *status = CCL_ERROR_MEMORY;
        ccl_cosmology_set_status_message(cosmo,
                                         "ccl_musigma.c: ccl_rescale_musigma_s8(): "
                                         "memory allocation\n");
      }
      if(*status==0) {
        int spstatus = gsl_spline2d_init(fka, psp->fka->xarr,
                                         psp->fka->yarr, psp->fka->zarr,
                                         psp->fka->interp_object.xsize,
                                         psp->fka->interp_object.ysize);
        if(spstatus) {
          *status = CCL_ERROR_MEMORY;
          ccl_cosmology_set_status_message(cosmo,
                                           "ccl_musigma.c: ccl_rescale_musigma_s8(): "
                                           "Error initializing spline\n");
        }
      }
      if(*status==0) {
        gsl_spline2d_free(psp->fka);
        psp->fka=fka;
      }
    }
  }

  free(rescale_factor);
}


/**
 *
 * @file core_zpemv.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Dulceneia Becker
 * @date 2011-06-29
 * @precisions normal z -> c d s
 *
 **/
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex64_t
 *
 *  CORE_zpemv  performs one of the matrix-vector operations
 *
 *     y = alpha*op( A )*x + beta*y
 *
 *  where  op( A ) is one of
 *
 *     op( A ) = A   or   op( A ) = A**T   or   op( A ) = A**H,
 *
 *  alpha and beta are scalars, x and y are vectors and A is a
 *  pentagonal matrix (see further details).
 *
 *
 *  Arguments
 *  ==========
 *
 * @param[in] storev
 *
 *         @arg PlasmaColumnwise :  array A stored columwise
 *         @arg PlasmaRowwise    :  array A stored rowwise
 *
 * @param[in] trans
 *
 *         @arg PlasmaNoTrans   :  y := alpha*A*x    + beta*y.
 *         @arg PlasmaTrans     :  y := alpha*A**T*x + beta*y.
 *         @arg PlasmaConjTrans :  y := alpha*A**H*x + beta*y.
 *
 * @param[in] M
 *         Number of rows of the matrix A.
 *         M must be at least zero.
 *
 * @param[in] N
 *         Number of columns of the matrix A.
 *         N must be at least zero.
 *
 * @param[in] L
 *         Order of triangle within the matrix A (L specifies the shape
 *         of the matrix A; see further details).
 *
 * @param[in] ALPHA
 *         Scalar alpha.
 *
 * @param[in] A
 *         Array of size LDA-by-N.  On entry, the leading M by N part
 *         of the array A must contain the matrix of coefficients.
 *
 * @param[in] LDA
 *         Leading dimension of array A.
 *
 * @param[in] X
 *         On entry, the incremented array X must contain the vector x.
 *
 * @param[in] INCX
 *         Increment for the elements of X. INCX must not be zero.
 *
 * @param[in] BETA
 *         Scalar beta.
 *
 * @param[in,out] Y
 *         On entry, the incremented array Y must contain the vector y.
 *
 * @param[out] INCY
 *         Increment for the elements of Y. INCY must not be zero.
 *
 * @param[out] WORK
 *         Workspace array of size at least L.
 *
 *  Further Details
 *  ===============
 *
 *               |     N    |
 *            _   ___________   _
 *               |          |
 *     A:        |          |
 *          M-L  |          |
 *               |          |  M
 *            _  |.....     |
 *               \    :     |
 *            L    \  :     |
 *            _      \:_____|  _
 *
 *               |  L | N-L |
 *
 *
 *******************************************************************************
 *
 * @return
 *          \retval PLASMA_SUCCESS successful exit
 *          \retval <0 if -i, the i-th argument had an illegal value
 *
 ******************************************************************************/

#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zpemv = PCORE_zpemv
#define CORE_zpemv PCORE_zpemv
#endif
int CORE_zpemv(PLASMA_enum trans, int storev,
               int M, int N, int L,
               PLASMA_Complex64_t ALPHA,
               const PLASMA_Complex64_t *A, int LDA,
               const PLASMA_Complex64_t *X, int INCX,
               PLASMA_Complex64_t BETA,
               PLASMA_Complex64_t *Y, int INCY,
               PLASMA_Complex64_t *WORK)
{

   /*
    *  y = alpha * op(A) * x + beta * y
    */

    int K;
    static PLASMA_Complex64_t zzero = 0.0;

    /* Check input arguments */
    if ((trans != PlasmaNoTrans) && (trans != PlasmaTrans) && (trans != PlasmaConjTrans)) {
        coreblas_error(1, "Illegal value of trans");
        return -1;
    }
    if ((storev != PlasmaColumnwise) && (storev != PlasmaRowwise)) {
        coreblas_error(2, "Illegal value of storev");
        return -2;
    }
    if (!( ((storev == PlasmaColumnwise) && (trans != PlasmaNoTrans)) ||
           ((storev == PlasmaRowwise) && (trans == PlasmaNoTrans)) )) {
        coreblas_error(2, "Illegal values of trans/storev");
        return -2;
    }
    if (M < 0) {
        coreblas_error(3, "Illegal value of M");
        return -3;
    }
    if (N < 0) {
        coreblas_error(4, "Illegal value of N");
        return -4;
    }
    if (L > min(M ,N)) {
        coreblas_error(5, "Illegal value of L");
        return -5;
    }
    if (LDA < max(1,M)) {
        coreblas_error(8, "Illegal value of LDA");
        return -8;
    }
    if (INCX < 1) {
        coreblas_error(10, "Illegal value of INCX");
        return -10;
    }
    if (INCY < 1) {
        coreblas_error(13, "Illegal value of INCY");
        return -13;
    }

    /* Quick return */
    if ((M == 0) || (N == 0))
        return PLASMA_SUCCESS;
    if ((ALPHA == zzero) && (BETA == zzero))
        return PLASMA_SUCCESS;

    /* If L < 2, there is no triangular part */
    if (L == 1) L = 0;

    /* Columnwise */
    if (storev == PlasmaColumnwise) {
        /*
         *        ______________
         *        |      |     |    A1: A[ 0 ]
         *        |      |     |    A2: A[ M-L ]
         *        |  A1  |     |    A3: A[ (N-L) * LDA ]
         *        |      |     |
         *        |______| A3  |
         *        \      |     |
         *          \ A2 |     |
         *            \  |     |
         *              \|_____|
         *
         */

        /* Columnwise / NoTrans */
        if (trans == PlasmaNoTrans) {
            coreblas_error(1, "The case PlasmaNoTrans / PlasmaColumnwise is not yet implemented");
            return -1;
        }
        /* Columnwise / [Conj]Trans */
        else {

            /* L top rows of y */
            if (L > 0) {

                /* w = A_2' * x_2 */
                cblas_zcopy(
                            L, &X[INCX*(M-L)], INCX, WORK, 1);
                cblas_ztrmv(
                            CblasColMajor, (CBLAS_UPLO)PlasmaUpper,
                            (CBLAS_TRANSPOSE)trans,
                            (CBLAS_DIAG)PlasmaNonUnit,
                            L, &A[M-L], LDA, WORK, 1);

                if (M > L) {

                    /* y_1 = beta * y_1 [ + alpha * A_1 * x_1 ] */
                    cblas_zgemv(
                                CblasColMajor, (CBLAS_TRANSPOSE)trans,
                                M-L, L, CBLAS_SADDR(ALPHA), A, LDA,
                                X, INCX, CBLAS_SADDR(BETA), Y, INCY);

                    /* y_1 = y_1 + alpha * w */
                    cblas_zaxpy(L, CBLAS_SADDR(ALPHA), WORK, 1, Y, INCY);

                } else {

                    /* y_1 = y_1 + alpha * w */
                    if (BETA == zzero) {
                        cblas_zscal(L, CBLAS_SADDR(ALPHA), WORK, 1);
                        cblas_zcopy(L, WORK, 1, Y, INCY);
                    } else {
                        cblas_zscal(L, CBLAS_SADDR(BETA), Y, INCY);
                        cblas_zaxpy(L, CBLAS_SADDR(ALPHA), WORK, 1, Y, INCY);
                    }
                }
            }

            /* N-L bottom rows of Y */
            if (N > L) {
                K = N - L;
                cblas_zgemv(
                            CblasColMajor, (CBLAS_TRANSPOSE)trans,
                            M, K, CBLAS_SADDR(ALPHA), &A[LDA*L], LDA,
                            X, INCX, CBLAS_SADDR(BETA), &Y[INCY*L], INCY);
            }
        }
    }
    /* Rowwise */
    else {
        /*
         * --------------
         * |            | \           A1:  A[ 0 ]
         * |    A1      |   \         A2:  A[ (N-L) * LDA ]
         * |            | A2  \       A3:  A[ L ]
         * |--------------------\
         * |        A3          |
         * ----------------------
         *
         */

        /* Rowwise / NoTrans */
        if (trans == PlasmaNoTrans) {
            /* L top rows of A and y */
            if (L > 0) {

                /* w = A_2 * x_2 */
                cblas_zcopy(
                            L, &X[INCX*(N-L)], INCX, WORK, 1);
                cblas_ztrmv(
                            CblasColMajor, (CBLAS_UPLO)PlasmaLower,
                            (CBLAS_TRANSPOSE)PlasmaNoTrans,
                            (CBLAS_DIAG)PlasmaNonUnit,
                            L, &A[LDA*(N-L)], LDA, WORK, 1);

                if (N > L) {

                    /* y_1 = beta * y_1 [ + alpha * A_1 * x_1 ] */
                    cblas_zgemv(
                                CblasColMajor, (CBLAS_TRANSPOSE)PlasmaNoTrans,
                                L, N-L, CBLAS_SADDR(ALPHA), A, LDA,
                                X, INCX, CBLAS_SADDR(BETA), Y, INCY);

                    /* y_1 = y_1 + alpha * w */
                    cblas_zaxpy(L, CBLAS_SADDR(ALPHA), WORK, 1, Y, INCY);

                } else {

                    /* y_1 = y_1 + alpha * w */
                    if (BETA == zzero) {
                        cblas_zscal(L, CBLAS_SADDR(ALPHA), WORK, 1);
                        cblas_zcopy(L, WORK, 1, Y, INCY);
                    } else {
                        cblas_zscal(L, CBLAS_SADDR(BETA), Y, INCY);
                        cblas_zaxpy(L, CBLAS_SADDR(ALPHA), WORK, 1, Y, INCY);
                    }
                }
            }

            /* M-L bottom rows of Y */
            if (M > L) {
                cblas_zgemv(
                        CblasColMajor, (CBLAS_TRANSPOSE)PlasmaNoTrans,
                        M-L, N, CBLAS_SADDR(ALPHA), &A[L], LDA,
                        X, INCX, CBLAS_SADDR(BETA), &Y[INCY*L], INCY);
            }
        }
        /* Rowwise / [Conj]Trans */
        else {
            coreblas_error(1, "The case Plasma[Conj]Trans / PlasmaRowwise is not yet implemented");
            return -1;
        }
    }

    return PLASMA_SUCCESS;
}


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

#include "error.h"
#include "check.h"

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

static double transport2_data[18] = {
   1.671760446434538503,
  -0.147735359946794490,
   0.148213819946936338e-01,
  -0.14195330326305613e-02,
   0.1306541324415708e-03,
  -0.117155795867579e-04,
   0.10333498445756e-05,
  -0.901911304223e-07,
   0.78177169833e-08,
  -0.6744565684e-09,
   0.579946394e-10,
  -0.49747619e-11,
   0.425961e-12,
  -0.36422e-13,
   0.3111e-14,
  -0.265e-15,
   0.23e-16,
  -0.19e-17
};
static cheb_series transport2_cs = {
  transport2_data,
  17,
  -1, 1,
  9
};

static double transport3_data[18] = {
   0.762012543243872007,
  -0.105674387705058533,
   0.119778084819657810e-01,
  -0.12144015203698307e-02,
   0.1155099769392855e-03,
  -0.105815992124423e-04,
   0.9474663385302e-06,
  -0.836221212858e-07,
   0.73109099278e-08,
  -0.6350594779e-09,
   0.549118282e-10,
  -0.47321395e-11,
   0.4067695e-12,
  -0.348971e-13,
   0.29892e-14,
  -0.256e-15,
   0.219e-16,
  -0.19e-17
};
static cheb_series transport3_cs = {
  transport3_data,
  17,
  -1, 1,
  9
};

static double transport4_data[18] = {
  0.4807570994615110579,
 -0.8175378810321083956e-01,
  0.1002700665975162973e-01,
 -0.10599339359820151e-02,
  0.1034506245030405e-03,
 -0.96442705485899e-05,
  0.8745544408515e-06,
 -0.779321207981e-07,
  0.68649886141e-08,
 -0.5999571076e-09,
  0.521366241e-10,
 -0.45118382e-11,
  0.3892159e-12,
 -0.334936e-13,
  0.28767e-14,
 -0.2467e-15,
  0.211e-16,
 -0.18e-17
};
static cheb_series transport4_cs = {
  transport4_data,
  17,
  -1, 1,
  9
};

static double transport5_data[18] = {
   0.347777777133910789,
  -0.66456988976050428e-01,
   0.8611072656883309e-02,
  -0.9396682223755538e-03,
   0.936324806081513e-04,
  -0.88571319340833e-05,
   0.811914989145e-06,
  -0.72957654233e-07,
   0.646971455e-08,
  -0.568490283e-09,
   0.49625598e-10,
  -0.4310940e-11,
   0.373100e-12,
  -0.32198e-13,
   0.2772e-14,
  -0.238e-15,
   0.21e-16,
  -0.18e-17
};
static cheb_series transport5_cs = {
  transport5_data,
  17,
  -1, 1,
  9
};

static
double
transport_sumexp(const int numexp, const int order, const double t, double x)
{
  double rk = (double)numexp;
  double sumexp = 0.0;
  int k;
  for(k=1; k<=numexp; k++) {
    double sum2 = 1.0;
    double xk  = 1.0/(rk*x);
    double xk1 = 1.0;
    int j;
    for(j=1; j<=order; j++) {
      sum2 = sum2*xk1*xk + 1.0;
      xk1 += 1.0;
    }
    sumexp *= t;
    sumexp += sum2;
    rk -= 1.0;
  }
  return sumexp;
}

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

int
gsl_sf_transport_2_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 3.289868133696452873;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x < 3.0*GSL_SQRT_DBL_EPSILON) {
    result->val = x;
    result->err = GSL_DBL_EPSILON*fabs(x) + x*x/2.0;
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    double t = (x*x/8.0 - 0.5) - 0.5;
    gsl_sf_result result_c;
    cheb_eval_e(&transport2_cs, t, &result_c);
    result->val  = x * result_c.val;
    result->err  = x * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -GSL_LOG_DBL_EPSILON) {
    const int    numexp = (int)((-GSL_LOG_DBL_EPSILON)/x) + 1;
    const double sumexp = transport_sumexp(numexp, 2, exp(-x), x);
    const double t = 2.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val  = val_infinity;
      result->err  = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + fabs(t) * et);
    }
    return GSL_SUCCESS;
  }
  else if(x < 2.0/GSL_DBL_EPSILON) {
    const int    numexp = 1;
    const double sumexp = transport_sumexp(numexp, 2, 1.0, x);
    const double t = 2.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else {
    const double t = 2.0 * log(x) - x;
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
}

int
gsl_sf_transport_3_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 7.212341418957565712;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x < 3.0*GSL_SQRT_DBL_EPSILON) {
    result->val = 0.5*x*x;
    result->err = 2.0 * GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double x2 = x*x;
    const double t = (x2/8.0 - 0.5) - 0.5;
    gsl_sf_result result_c;
    cheb_eval_e(&transport3_cs, t, &result_c);
    result->val  = x2 * result_c.val;
    result->err  = x2 * result_c.err;
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -GSL_LOG_DBL_EPSILON) {
    const int    numexp = (int)((-GSL_LOG_DBL_EPSILON)/x) + 1;
    const double sumexp = transport_sumexp(numexp, 3, exp(-x), x);
    const double t = 3.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + fabs(t) * et);
    }
    return GSL_SUCCESS;
  }
  else if(x < 3.0/GSL_DBL_EPSILON) {
    const int    numexp = 1;
    const double sumexp = transport_sumexp(numexp, 3, 1.0, x);
    const double t = 3.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else {
    const double t = 3.0 * log(x) - x;
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
}

int
gsl_sf_transport_4_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 25.97575760906731660;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x < 3.0*GSL_SQRT_DBL_EPSILON) {
    result->val = x*x*x/3.0;
    result->err = 3.0 * GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double x2 = x*x;
    const double t = (x2/8.0 - 0.5) - 0.5;
    gsl_sf_result result_c;
    cheb_eval_e(&transport4_cs, t, &result_c);
    result->val  = x2*x * result_c.val;
    result->err  = x2*x * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -GSL_LOG_DBL_EPSILON) {
    const int    numexp = (int)((-GSL_LOG_DBL_EPSILON)/x) + 1;
    const double sumexp = transport_sumexp(numexp, 4, exp(-x), x);
    const double t = 4.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else if(x < 3.0/GSL_DBL_EPSILON) {
    const int    numexp = 1;
    const double sumexp = transport_sumexp(numexp, 4, 1.0, x);
    const double t = 4.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else {
    const double t = 4.0 * log(x) - x;
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
}

int
gsl_sf_transport_5_e(const double x, gsl_sf_result * result)
{
  const double val_infinity = 124.4313306172043912;

  /* CHECK_POINTER(result) */

  if(x < 0.0) {
    DOMAIN_ERROR(result);
  }
  else if(x == 0.0) {
    result->val = 0.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(x < 3.0*GSL_SQRT_DBL_EPSILON) {
    result->val = x*x*x*x/4.0;
    result->err = 4.0 * GSL_DBL_EPSILON * result->val;
    CHECK_UNDERFLOW(result);
    return GSL_SUCCESS;
  }
  else if(x <= 4.0) {
    const double x2 = x*x;
    const double t = (x2/8.0 - 0.5) - 0.5;
    gsl_sf_result result_c;
    cheb_eval_e(&transport5_cs, t, &result_c);
    result->val  = x2*x2 * result_c.val;
    result->err  = x2*x2 * result_c.err;
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else if(x < -GSL_LOG_DBL_EPSILON) {
    const int    numexp = (int)((-GSL_LOG_DBL_EPSILON)/x) + 1;
    const double sumexp = transport_sumexp(numexp, 5, exp(-x), x);
    const double t = 5.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else if(x < 3.0/GSL_DBL_EPSILON) {
    const int    numexp = 1;
    const double sumexp = transport_sumexp(numexp, 5, 1.0, x);
    const double t = 5.0 * log(x) - x + log(sumexp);
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
  else {
    const double t = 5.0 * log(x) - x;
    if(t < GSL_LOG_DBL_EPSILON) {
      result->val = val_infinity;
      result->err = 2.0 * GSL_DBL_EPSILON * val_infinity;
    }
    else {
      const double et = exp(t);
      result->val = val_infinity - et;
      result->err = 2.0 * GSL_DBL_EPSILON * (val_infinity + (fabs(t)+1.0) * et);
    }
    return GSL_SUCCESS;
  }
}

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

#include "eval.h"

double gsl_sf_transport_2(const double x)
{
  EVAL_RESULT(gsl_sf_transport_2_e(x, &result));
}

double gsl_sf_transport_3(const double x)
{
  EVAL_RESULT(gsl_sf_transport_3_e(x, &result));
}

double gsl_sf_transport_4(const double x)
{
  EVAL_RESULT(gsl_sf_transport_4_e(x, &result));
}

double gsl_sf_transport_5(const double x)
{
  EVAL_RESULT(gsl_sf_transport_5_e(x, &result));
}


#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>

#include "tests.h"

void
test_spr (void) {
const double flteps = 1e-4, dbleps = 1e-6;
  {
   int order = 101;
   int uplo = 121;
   int N = 2;
   float alpha = -0.3f;
   float Ap[] = { -0.764f, -0.257f, -0.064f };
   float X[] = { 0.455f, -0.285f };
   int incX = -1;
   float Ap_expected[] = { -0.788367f, -0.218097f, -0.126108f };
   cblas_sspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], flteps, "sspr(case 1426)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 2;
   float alpha = -0.3f;
   float Ap[] = { -0.764f, -0.257f, -0.064f };
   float X[] = { 0.455f, -0.285f };
   int incX = -1;
   float Ap_expected[] = { -0.788367f, -0.218097f, -0.126108f };
   cblas_sspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], flteps, "sspr(case 1427)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 2;
   float alpha = -0.3f;
   float Ap[] = { -0.764f, -0.257f, -0.064f };
   float X[] = { 0.455f, -0.285f };
   int incX = -1;
   float Ap_expected[] = { -0.788367f, -0.218097f, -0.126108f };
   cblas_sspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], flteps, "sspr(case 1428)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 2;
   float alpha = -0.3f;
   float Ap[] = { -0.764f, -0.257f, -0.064f };
   float X[] = { 0.455f, -0.285f };
   int incX = -1;
   float Ap_expected[] = { -0.788367f, -0.218097f, -0.126108f };
   cblas_sspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], flteps, "sspr(case 1429)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 121;
   int N = 2;
   double alpha = -1;
   double Ap[] = { 0.819, 0.175, -0.809 };
   double X[] = { -0.645, -0.222 };
   int incX = -1;
   double Ap_expected[] = { 0.769716, 0.03181, -1.225025 };
   cblas_dspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], dbleps, "dspr(case 1430)");
     }
   };
  };

  {
   int order = 101;
   int uplo = 122;
   int N = 2;
   double alpha = -1;
   double Ap[] = { 0.819, 0.175, -0.809 };
   double X[] = { -0.645, -0.222 };
   int incX = -1;
   double Ap_expected[] = { 0.769716, 0.03181, -1.225025 };
   cblas_dspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], dbleps, "dspr(case 1431)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 121;
   int N = 2;
   double alpha = -1;
   double Ap[] = { 0.819, 0.175, -0.809 };
   double X[] = { -0.645, -0.222 };
   int incX = -1;
   double Ap_expected[] = { 0.769716, 0.03181, -1.225025 };
   cblas_dspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], dbleps, "dspr(case 1432)");
     }
   };
  };

  {
   int order = 102;
   int uplo = 122;
   int N = 2;
   double alpha = -1;
   double Ap[] = { 0.819, 0.175, -0.809 };
   double X[] = { -0.645, -0.222 };
   int incX = -1;
   double Ap_expected[] = { 0.769716, 0.03181, -1.225025 };
   cblas_dspr(order, uplo, N, alpha, X, incX, Ap);
   {
     int i;
     for (i = 0; i < 3; i++) {
       gsl_test_rel(Ap[i], Ap_expected[i], dbleps, "dspr(case 1433)");
     }
   };
  };

}


#ifndef SCANMATCHERPROCESSOR_H
#define SCANMATCHERPROCESSOR_H

// #include <log/sensorlog.h>
#include "sensor/sensor_range/rangereading.h"
#include "sensor/sensor_range/rangesensor.h"
//#include <gsl/gsl_eigen.h>
#include "scanmatcher.h"

namespace GMapping {

class ScanMatcherProcessor {
 public:
  ScanMatcherProcessor(const ScanMatcherMap& m);
  ScanMatcherProcessor(double xmin, double ymin, double xmax, double ymax,
                       double delta, double patchdelta, bool use_icp);
  virtual ~ScanMatcherProcessor();
  virtual void processScan(const RangeReading& reading);
  void setSensorMap(const SensorMap& smap, std::string sensorName = "FLASER");
  void init();
  void setMatchingParameters(double urange, double range, double sigma,
                             int kernsize, double lopt, double aopt,
                             int iterations, bool computeCovariance = false);
  void setRegistrationParameters(double regScore, double critScore);
  OrientedPoint getPose() const;
  inline const ScanMatcherMap& getMap() const { return m_map; }
  inline ScanMatcher& matcher() { return m_matcher; }
  inline void setmaxMove(double mmove) { m_maxMove = mmove; }
  bool useICP;

 protected:
  ScanMatcher m_matcher;
  bool m_computeCovariance;
  bool m_first;
  SensorMap m_sensorMap;
  double m_regScore, m_critScore;
  unsigned int m_beams;
  double m_maxMove;
  // state
  ScanMatcherMap m_map;
  OrientedPoint m_pose;
  OrientedPoint m_odoPose;
  int m_count;
  // gsl_eigen_symmv_workspace * m_eigenspace;
};

};  // namespace GMapping

#endif


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

/***************************************************************************//**
 *
 **/
void QUARK_CORE_claswp(Quark *quark, Quark_Task_Flags *task_flags,
                       int n, PLASMA_Complex32_t *A, int lda,
                       int i1,  int i2, const int *ipiv, int inc)
{
    DAG_CORE_LASWP;
    QUARK_Insert_Task(
        quark, CORE_claswp_quark, task_flags,
        sizeof(int),                      &n,    VALUE,
        sizeof(PLASMA_Complex32_t)*lda*n,  A,        INOUT | LOCALITY,
        sizeof(int),                      &lda,  VALUE,
        sizeof(int),                      &i1,   VALUE,
        sizeof(int),                      &i2,   VALUE,
        sizeof(int)*n,                     ipiv,     INPUT,
        sizeof(int),                      &inc,  VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_claswp_quark = PCORE_claswp_quark
#define CORE_claswp_quark PCORE_claswp_quark
#endif
void CORE_claswp_quark(Quark *quark)
{
    int n, lda, i1, i2, inc;
    int *ipiv;
    PLASMA_Complex32_t *A;

    quark_unpack_args_7(quark, n, A, lda, i1, i2, ipiv, inc);
    LAPACKE_claswp_work(LAPACK_COL_MAJOR, n, A, lda, i1, i2, ipiv, inc );
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_claswp_f2(Quark *quark, Quark_Task_Flags *task_flags,
                          int n, PLASMA_Complex32_t *A, int lda,
                          int i1,  int i2, const int *ipiv, int inc,
                          PLASMA_Complex32_t *fake1, int szefake1, int flag1,
                          PLASMA_Complex32_t *fake2, int szefake2, int flag2)
{
    DAG_CORE_LASWP;
    QUARK_Insert_Task(
        quark, CORE_claswp_f2_quark, task_flags,
        sizeof(int),                        &n,     VALUE,
        sizeof(PLASMA_Complex32_t)*lda*n,    A,         INOUT | LOCALITY,
        sizeof(int),                        &lda,   VALUE,
        sizeof(int),                        &i1,    VALUE,
        sizeof(int),                        &i2,    VALUE,
        sizeof(int)*n,                       ipiv,      INPUT,
        sizeof(int),                        &inc,   VALUE,
        sizeof(PLASMA_Complex32_t)*szefake1, fake1,     flag1,
        sizeof(PLASMA_Complex32_t)*szefake2, fake2,     flag2,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_claswp_f2_quark = PCORE_claswp_f2_quark
#define CORE_claswp_f2_quark PCORE_claswp_f2_quark
#endif
void CORE_claswp_f2_quark(Quark* quark)
{
    int n, lda, i1, i2, inc;
    int *ipiv;
    PLASMA_Complex32_t *A;
    void *fake1, *fake2;

    quark_unpack_args_9(quark, n, A, lda, i1, i2, ipiv, inc, fake1, fake2);
    LAPACKE_claswp_work(LAPACK_COL_MAJOR, n, A, lda, i1, i2, ipiv, inc );
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_claswp_ontile(Quark *quark, Quark_Task_Flags *task_flags,
                              PLASMA_desc descA, PLASMA_Complex32_t *Aij,
                              int i1,  int i2, const int *ipiv, int inc, PLASMA_Complex32_t *fakepanel)
{
    DAG_CORE_LASWP;
    if (fakepanel == Aij) {
        QUARK_Insert_Task(
            quark, CORE_claswp_ontile_quark, task_flags,
            sizeof(PLASMA_desc),              &descA,     VALUE,
            sizeof(PLASMA_Complex32_t)*1,      Aij,           INOUT | LOCALITY,
            sizeof(int),                      &i1,        VALUE,
            sizeof(int),                      &i2,        VALUE,
            sizeof(int)*(i2-i1+1)*abs(inc),   ipiv,           INPUT,
            sizeof(int),                      &inc,       VALUE,
            sizeof(PLASMA_Complex32_t)*1,      fakepanel,     SCRATCH,
            0);
    } else {
        QUARK_Insert_Task(
            quark, CORE_claswp_ontile_quark, task_flags,
            sizeof(PLASMA_desc),              &descA,     VALUE,
            sizeof(PLASMA_Complex32_t)*1,      Aij,           INOUT | LOCALITY,
            sizeof(int),                      &i1,        VALUE,
            sizeof(int),                      &i2,        VALUE,
            sizeof(int)*(i2-i1+1)*abs(inc),   ipiv,           INPUT,
            sizeof(int),                      &inc,       VALUE,
            sizeof(PLASMA_Complex32_t)*1,      fakepanel,     INOUT,
            0);
    }
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_claswp_ontile_quark = PCORE_claswp_ontile_quark
#define CORE_claswp_ontile_quark PCORE_claswp_ontile_quark
#endif
void CORE_claswp_ontile_quark(Quark *quark)
{
    int i1, i2, inc;
    int *ipiv;
    PLASMA_Complex32_t *A, *fake;
    PLASMA_desc descA;

    quark_unpack_args_7(quark, descA, A, i1, i2, ipiv, inc, fake);
    CORE_claswp_ontile(descA, i1, i2, ipiv, inc);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_claswp_ontile_f2(Quark *quark, Quark_Task_Flags *task_flags,
                                 PLASMA_desc descA, PLASMA_Complex32_t *Aij,
                                 int i1,  int i2, const int *ipiv, int inc,
                                 PLASMA_Complex32_t *fake1, int szefake1, int flag1,
                                 PLASMA_Complex32_t *fake2, int szefake2, int flag2)
{
    DAG_CORE_LASWP;
    QUARK_Insert_Task(
        quark, CORE_claswp_ontile_f2_quark, task_flags,
        sizeof(PLASMA_desc),                &descA, VALUE,
        sizeof(PLASMA_Complex32_t)*1,        Aij,       INOUT | LOCALITY,
        sizeof(int),                        &i1,    VALUE,
        sizeof(int),                        &i2,    VALUE,
        sizeof(int)*(i2-i1+1)*abs(inc),      ipiv,      INPUT,
        sizeof(int),                        &inc,   VALUE,
        sizeof(PLASMA_Complex32_t)*szefake1, fake1, flag1,
        sizeof(PLASMA_Complex32_t)*szefake2, fake2, flag2,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_claswp_ontile_f2_quark = PCORE_claswp_ontile_f2_quark
#define CORE_claswp_ontile_f2_quark PCORE_claswp_ontile_f2_quark
#endif
void CORE_claswp_ontile_f2_quark(Quark *quark)
{
    int i1, i2, inc;
    int *ipiv;
    PLASMA_Complex32_t *A;
    PLASMA_desc descA;
    void *fake1, *fake2;

    quark_unpack_args_8(quark, descA, A, i1, i2, ipiv, inc, fake1, fake2);
    CORE_claswp_ontile(descA, i1, i2, ipiv, inc);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_cswptr_ontile(Quark *quark, Quark_Task_Flags *task_flags,
                              PLASMA_desc descA, PLASMA_Complex32_t *Aij,
                              int i1,  int i2, const int *ipiv, int inc,
                              const PLASMA_Complex32_t *Akk, int ldak)
{
    DAG_CORE_TRSM;
    QUARK_Insert_Task(
        quark, CORE_cswptr_ontile_quark, task_flags,
        sizeof(PLASMA_desc),              &descA, VALUE,
        sizeof(PLASMA_Complex32_t)*1,      Aij,       INOUT | LOCALITY,
        sizeof(int),                      &i1,    VALUE,
        sizeof(int),                      &i2,    VALUE,
        sizeof(int)*(i2-i1+1)*abs(inc),    ipiv,      INPUT,
        sizeof(int),                      &inc,   VALUE,
        sizeof(PLASMA_Complex32_t)*ldak,   Akk,       INPUT,
        sizeof(int),                      &ldak,  VALUE,
        0);
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_cswptr_ontile_quark = PCORE_cswptr_ontile_quark
#define CORE_cswptr_ontile_quark PCORE_cswptr_ontile_quark
#endif
void CORE_cswptr_ontile_quark(Quark *quark)
{
    int i1, i2, inc, ldak;
    int *ipiv;
    PLASMA_Complex32_t *A, *Akk;
    PLASMA_desc descA;

    quark_unpack_args_8(quark, descA, A, i1, i2, ipiv, inc, Akk, ldak);
    CORE_cswptr_ontile(descA, i1, i2, ipiv, inc, Akk, ldak);
}

/***************************************************************************//**
 *
 **/
void QUARK_CORE_claswpc_ontile(Quark *quark, Quark_Task_Flags *task_flags,
                              PLASMA_desc descA, PLASMA_Complex32_t *Aij,
                              int i1,  int i2, const int *ipiv, int inc, PLASMA_Complex32_t *fakepanel)
{
    DAG_CORE_LASWP;
    if (fakepanel == Aij) {
        QUARK_Insert_Task(
            quark, CORE_claswpc_ontile_quark, task_flags,
            sizeof(PLASMA_desc),              &descA,     VALUE,
            sizeof(PLASMA_Complex32_t)*1,      Aij,           INOUT | LOCALITY,
            sizeof(int),                      &i1,        VALUE,
            sizeof(int),                      &i2,        VALUE,
            sizeof(int)*(i2-i1+1)*abs(inc),   ipiv,           INPUT,
            sizeof(int),                      &inc,       VALUE,
            sizeof(PLASMA_Complex32_t)*1,      fakepanel,     SCRATCH,
            0);
    } else {
        QUARK_Insert_Task(
            quark, CORE_claswpc_ontile_quark, task_flags,
            sizeof(PLASMA_desc),              &descA,     VALUE,
            sizeof(PLASMA_Complex32_t)*1,      Aij,           INOUT | LOCALITY,
            sizeof(int),                      &i1,        VALUE,
            sizeof(int),                      &i2,        VALUE,
            sizeof(int)*(i2-i1+1)*abs(inc),   ipiv,           INPUT,
            sizeof(int),                      &inc,       VALUE,
            sizeof(PLASMA_Complex32_t)*1,      fakepanel,     INOUT,
            0);
    }
}

/***************************************************************************//**
 *
 **/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_claswpc_ontile_quark = PCORE_claswpc_ontile_quark
#define CORE_claswpc_ontile_quark PCORE_claswpc_ontile_quark
#endif
void CORE_claswpc_ontile_quark(Quark *quark)
{
    int i1, i2, inc;
    int *ipiv;
    PLASMA_Complex32_t *A, *fake;
    PLASMA_desc descA;

    quark_unpack_args_7(quark, descA, A, i1, i2, ipiv, inc, fake);
    CORE_claswpc_ontile(descA, i1, i2, ipiv, inc);
}