/* ode-initval2/msadams.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* A variable-coefficient linear multistep Adams method in Nordsieck
   form. This stepper uses explicit Adams-Bashforth (predictor) and
   implicit Adams-Moulton (corrector) methods in P(EC)^m functional
   iteration mode. Method order varies dynamically between 1 and 12.

   References:

   Byrne, G. D., and Hindmarsh, A. C., A Polyalgorithm for the
   Numerical Solution of Ordinary Differential Equations,
   ACM Trans. Math. Software, 1 (1975), pp. 71-96.

   Brown, P. N., Byrne, G. D., and Hindmarsh, A. C., VODE: A
   Variable-coefficient ODE Solver, SIAM J. Sci. Stat. Comput. 10,
   (1989), pp. 1038-1051.

   Hindmarsh, A. C., Brown, P. N., Grant, K. E., Lee, S. L., Serban,
   R., Shumaker, D. E., and Woodward, C. S., SUNDIALS: Suite of
   Nonlinear and Differential/Algebraic Equation Solvers, ACM
   Trans. Math. Software 31 (2005), pp. 363-396.

   Note: The algorithms have been adapted for GSL ode-initval2
   framework.
*/

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>
#include <gsl/gsl_blas.h>

#include "odeiv_util.h"

/* Maximum order of Adams methods */
#define MSADAMS_MAX_ORD 12

typedef struct
{
  /* Nordsieck history matrix. Includes concatenated
     Nordsieck vectors [y_n, h*y_n', (h^2/2!)*y_n'', ...,
     (h^ord/ord!)*d^(ord)(y_n)]. Nordsieck vector number i is located
     at z[i*dim] (i=0..ord).
   */
  double *z;

  double *zbackup;              /* backup of Nordsieck matrix */
  double *ytmp;                 /* work area */
  double *ytmp2;                /* work area */
  double *pc;                   /* product term coefficients */
  double *l;                    /* polynomial coefficients */
  double *hprev;                /* previous step sizes */
  double *hprevbackup;          /* backup of hprev */
  double *errlev;               /* desired error level of y */
  gsl_vector *abscor;           /* absolute y values for correction */
  gsl_vector *relcor;           /* relative y values for correction */
  gsl_vector *svec;             /* saved abscor & work area */
  gsl_vector *tempvec;          /* work area */
  const gsl_odeiv2_driver *driver;      /* pointer to gsl_odeiv2_driver object */

  long int ni;                  /* stepper call counter */
  size_t ord;                   /* current order of method */
  size_t ordprev;               /* order of previous call */
  size_t ordprevbackup;         /* backup of ordprev */
  double tprev;                 /* t point of previous call */
  size_t ordwait;               /* counter for order change */
  size_t ordwaitbackup;         /* backup of ordwait */
  size_t failord;               /* order of convergence failure */
  double failt;                 /* t point of convergence failure */
  double ordm1coeff;            /* saved order-1 coefficiet */
  double ordp1coeffprev;        /* saved order+1 coefficient */
  size_t failcount;             /* counter for rejected steps */
}
msadams_state_t;

/* Introduce msadams_reset for use in msadams_alloc and _apply */

static int msadams_reset (void *, size_t);

static void *
msadams_alloc (size_t dim)
{
  msadams_state_t *state =
    (msadams_state_t *) malloc (sizeof (msadams_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for msadams_state",
                      GSL_ENOMEM);
    }

  state->z =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->z == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for z", GSL_ENOMEM);
    }

  state->zbackup =
    (double *) malloc ((MSADAMS_MAX_ORD + 1) * dim * sizeof (double));

  if (state->zbackup == 0)
    {
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for zbackup", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->ytmp2 = (double *) malloc (dim * sizeof (double));

  if (state->ytmp2 == 0)
    {
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp2", GSL_ENOMEM);
    }

  state->pc = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->pc == 0)
    {
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for pc", GSL_ENOMEM);
    }

  state->l = (double *) malloc ((MSADAMS_MAX_ORD + 1) * sizeof (double));

  if (state->l == 0)
    {
      free (state->pc);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for l", GSL_ENOMEM);
    }

  state->hprev = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprev == 0)
    {
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprev", GSL_ENOMEM);
    }

  state->hprevbackup = (double *) malloc (MSADAMS_MAX_ORD * sizeof (double));

  if (state->hprevbackup == 0)
    {
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for hprevbackup", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->abscor = gsl_vector_alloc (dim);

  if (state->abscor == 0)
    {
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for abscor", GSL_ENOMEM);
    }

  state->relcor = gsl_vector_alloc (dim);

  if (state->relcor == 0)
    {
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for relcor", GSL_ENOMEM);
    }

  state->svec = gsl_vector_alloc (dim);

  if (state->svec == 0)
    {
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for svec", GSL_ENOMEM);
    }

  state->tempvec = gsl_vector_alloc (dim);

  if (state->tempvec == 0)
    {
      gsl_vector_free (state->svec);
      gsl_vector_free (state->relcor);
      gsl_vector_free (state->abscor);
      free (state->errlev);
      free (state->hprevbackup);
      free (state->hprev);
      free (state->l);
      free (state->pc);
      free (state->ytmp2);
      free (state->ytmp);
      free (state->zbackup);
      free (state->z);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for tempvec", GSL_ENOMEM);
    }

  msadams_reset ((void *) state, dim);

  state->driver = NULL;

  return state;
}

static int
msadams_failurehandler (void *vstate, const size_t dim, const double t)
{
  /* Internal failure handler routine for msadams. Adjusted strategy
     for GSL: Decrease order if this is the second time a failure
     has occurred at this order and point.
   */

  msadams_state_t *state = (msadams_state_t *) vstate;

  const size_t ord = state->ord;

  if (ord > 1 && (ord - state->ordprev == 0) &&
      ord == state->failord && t == state->failt)
    {
      state->ord--;
    }

  /* Save information about failure */

  state->failord = ord;
  state->failt = t;
  state->ni++;

  /* Force reinitialization if failure took place at lowest
     order
   */

  if (ord == 1)
    {
      msadams_reset (vstate, dim);
    }

  return GSL_SUCCESS;
}

static int
msadams_calccoeffs (const size_t ord, const size_t ordwait,
                    const double h, const double hprev[],
                    double pc[], double l[],
                    double *errcoeff, double *ordm1coeff,
                    double *ordp1coeff, double *ordp2coeff)
{

  /* Calculates coefficients (l) of polynomial Lambda, error and
     auxiliary order change evaluation coefficients.
   */

  if (ord == 1)
    {
      l[0] = 1.0;
      l[1] = 1.0;
      *errcoeff = 0.5;
      *ordp1coeff = 1.0;
      *ordp2coeff = 12.0;
    }
  else
    {
      size_t i, j;
      double hsum = h;
      double st1 = 0.0;         /* sum term coefficients */
      double st2 = 0.0;

      /* Calculate coefficients (pc) of product terms */

      DBL_ZERO_MEMSET (pc, MSADAMS_MAX_ORD + 1);

      pc[0] = 1.0;

      for (i = 1; i < ord; i++)
        {
          /* Calculate auxiliary coefficient used in evaluation of
             change of order
           */

          if (i == ord - 1 && ordwait == 1)
            {
              int s = 1;

              *ordm1coeff = 0.0;

              for (j = 0; j < ord - 1; j++)
                {
                  *ordm1coeff += s * pc[j] / (j + 2);
                  s = -s;
                }

              *ordm1coeff = pc[ord - 2] / (ord * (*ordm1coeff));
            }

          for (j = i; j > 0; j--)
            {
              pc[j] += pc[j - 1] * h / hsum;
            }

          hsum += hprev[i - 1];
        }

      /* Calculate sum term 1 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st1 += s * pc[i] / (i + 1.0);
            s = -s;
          }
      }

      /* Calculate sum term 2 for error estimation */

      {
        int s = 1;

        for (i = 0; i < ord; i++)
          {
            st2 += s * pc[i] / (i + 2.0);
            s = -s;
          }
      }

      /* Calculate the actual polynomial coefficients (l) */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[0] = 1.0;

      for (i = 1; i < ord + 1; i++)
        {
          l[i] = pc[i - 1] / (i * st1);
        }

#ifdef DEBUG
      {
        size_t di;

        printf ("-- calccoeffs l: ");
        for (di = 0; di < ord + 1; di++)
          {
            printf ("%.5e ", l[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs pc: ");
        for (di = 0; di < ord; di++)
          {
            printf ("%.5e ", pc[di]);
          }
        printf ("\n");

        printf ("-- calccoeffs st1=%.5e, st2=%.5e\n", st1, st2);
      }
#endif

      /* Calculate error coefficient */

      *errcoeff = (h / hsum) * (st2 / st1);

      /* Calculate auxiliary coefficients used in evaluation of change
         of order
       */

      if (ordwait < 2)
        {
          int s = 1;

          *ordp1coeff = hsum / (h * l[ord]);

          *ordp2coeff = 0.0;

          for (i = ord; i > 0; i--)
            {
              pc[i] += pc[i - 1] * (h / hsum);
            }

          for (i = 0; i < ord + 1; i++)
            {
              *ordp2coeff += s * pc[i] / (i + 2);
              s = -s;
            }

          *ordp2coeff = (ord + 1) * st1 / (*ordp2coeff);
        }
    }

#ifdef DEBUG
  printf ("-- calccoeffs ordm1coeff=%.5e ", *ordm1coeff);
  printf ("ordp1coeff=%.5e ", *ordp1coeff);
  printf ("ordp2coeff=%.5e ", *ordp2coeff);
  printf ("errcoeff=%.5e\n", *errcoeff);
#endif

  return GSL_SUCCESS;
}

static int
msadams_corrector (void *vstate, const gsl_odeiv2_system * sys,
                   const double t, const double h, const size_t dim,
                   const double z[], const double errlev[],
                   const double l[], const double errcoeff,
                   gsl_vector * abscor, gsl_vector * relcor,
                   double ytmp[], double ytmp2[])
{
  /* Calculates the correction step (abscor). Non-linear equation
     system is solved by functional iteration.
   */

  size_t mi, i;
  const size_t max_iter = 3;    /* Maximum number of iterations */
  double convrate = 1.0;        /* convergence rate */
  double stepnorm = 0.0;        /* norm of correction step */
  double stepnormprev = 0.0;

  /* Evaluate at predicted values */

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, ytmp);

    if (s == GSL_EBADFUNC)
      {
        return s;
      }

    if (s != GSL_SUCCESS)
      {
        msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
        printf ("-- FAIL at user function evaluation\n");
#endif
        return s;
      }
  }

  /* Calculate correction step (abscor) */

  gsl_vector_set_zero (abscor);

  for (mi = 0; mi < max_iter; mi++)
    {
      const double safety = 0.3;
      const double safety2 = 0.1;

      /* Calculate new y values to ytmp2 */

      for (i = 0; i < dim; i++)
        {
          ytmp[i] *= h;
          ytmp[i] -= z[1 * dim + i];
          ytmp[i] /= l[1];
          ytmp2[i] = z[0 * dim + i] + ytmp[i];
        }

#ifdef DEBUG
      {
        size_t di;
        printf ("-- dstep: ");
        for (di = 0; di < dim; di++)
          {
            printf ("%.5e ", ytmp[di]);
          }
        printf ("\n");
      }
#endif
      /* Convergence test. Norms used are root-mean-square norms. */

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (relcor, i,
                          (ytmp[i] - gsl_vector_get (abscor, i)) / errlev[i]);
          gsl_vector_set (abscor, i, ytmp[i]);
        }

      stepnorm = gsl_blas_dnrm2 (relcor) / sqrt (dim);

      if (mi > 0)
        {
          convrate = GSL_MAX_DBL (safety * convrate, stepnorm / stepnormprev);
        }
      else
        {
          convrate = 1.0;
        }

      {
        const double convtest =
          GSL_MIN_DBL (convrate, 1.0) * stepnorm * errcoeff / safety2;

#ifdef DEBUG
        printf
          ("-- func iter loop %d, errcoeff=%.5e, stepnorm =%.5e, convrate = %.5e, convtest = %.5e\n",
           (int) mi, errcoeff, stepnorm, convrate, convtest);
#endif
        if (convtest <= 1.0)
          {
            break;
          }
      }

      /* Check for divergence during iteration */
      {
        const double div_const = 2.0;

        if (mi > 1 && stepnorm > div_const * stepnormprev)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL, diverging functional iteration\n");
#endif
            return GSL_FAILURE;
          }
      }

      /* Evaluate at new y */

      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, ytmp2, ytmp);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

      stepnormprev = stepnorm;
    }

#ifdef DEBUG
  printf ("-- functional iteration exit at mi=%d\n", (int) mi);
#endif

  /* Handle convergence failure */

  if (mi == max_iter)
    {
      msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
      printf ("-- FAIL, max_iter reached\n");
#endif
      return GSL_FAILURE;
    }

  return GSL_SUCCESS;
}

static int
msadams_eval_order (gsl_vector * abscor, gsl_vector * tempvec,
                    gsl_vector * svec, const double errcoeff,
                    const size_t dim, const double errlev[],
                    const double ordm1coeff, const double ordp1coeff,
                    const double ordp1coeffprev, const double ordp2coeff,
                    const double hprev[],
                    const double h, const double z[],
                    size_t * ord, size_t * ordwait)
{
  /* Evaluates and executes change in method order (current, current-1
     or current+1). Order which maximizes the step length is selected.
   */

  size_t i;

  /* step size estimates at current order, order-1 and order+1 */
  double ordest = 0.0;
  double ordm1est = 0.0;
  double ordp1est = 0.0;

  const double safety = 1e-6;
  const double bias = 6.0;
  const double bias2 = 10.0;

  /* Relative step length estimate for current order */

  ordest = 1.0 / (pow (bias * gsl_blas_dnrm2 (abscor) / sqrt (dim)
                       * errcoeff, 1.0 / (*ord + 1)) + safety);

  /* Relative step length estimate for order ord - 1 */

  if (*ord > 1)
    {
      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (tempvec, i, z[*ord * dim + i] / errlev[i]);
        }

      ordm1est = 1.0 / (pow (bias * gsl_blas_dnrm2 (tempvec) / sqrt (dim)
                             / ordm1coeff, 1.0 / (*ord)) + safety);
    }
  else
    {
      ordm1est = 0.0;
    }

  /* Relative step length estimate for order ord + 1 */

  if (*ord < MSADAMS_MAX_ORD)
    {
      const double c = -ordp1coeff / ordp1coeffprev *
        pow (h / hprev[1], *ord + 1);

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (svec, i) * c +
                          gsl_vector_get (abscor, i));
        }

      ordp1est = 1.0 / (pow (bias2 * gsl_blas_dnrm2 (svec) / sqrt (dim)
                             / ordp2coeff, 1.0 / (*ord + 2)) + safety);
    }
  else
    {
      ordp1est = 0.0;
    }

#ifdef DEBUG
  printf
    ("-- eval_order ord=%d, ordest=%.5e, ordm1est=%.5e, ordp1est=%.5e\n",
     (int) *ord, ordest, ordm1est, ordp1est);
#endif

  /* Choose order that maximises step size and increases step
     size markedly compared to current step
   */

  {
    const double min_incr = 1.5;

    if (ordm1est > ordest && ordm1est > ordp1est && ordm1est > min_incr)
      {
        *ord -= 1;
#ifdef DEBUG
        printf ("-- eval_order order DECREASED to %d\n", (int) *ord);
#endif
      }

    else if (ordp1est > ordest && ordp1est > ordm1est && ordp1est > min_incr)
      {
        *ord += 1;
#ifdef DEBUG
        printf ("-- eval_order order INCREASED to %d\n", (int) *ord);
#endif
      }
  }

  *ordwait = *ord + 2;

  return GSL_SUCCESS;
}

static int
msadams_apply (void *vstate, size_t dim, double t, double h,
               double y[], double yerr[],
               const double dydt_in[], double dydt_out[],
               const gsl_odeiv2_system * sys)
{
  /* Conducts a step by Adams linear multistep methods. */

  msadams_state_t *state = (msadams_state_t *) vstate;

  double *const z = state->z;
  double *const zbackup = state->zbackup;
  double *const ytmp = state->ytmp;
  double *const ytmp2 = state->ytmp2;
  double *const pc = state->pc;
  double *const l = state->l;
  double *const hprev = state->hprev;
  double *const hprevbackup = state->hprevbackup;
  double *const errlev = state->errlev;
  gsl_vector *const abscor = state->abscor;
  gsl_vector *const relcor = state->relcor;
  gsl_vector *const svec = state->svec;
  gsl_vector *const tempvec = state->tempvec;

  size_t ord = state->ord;
  double ordm1coeff = 0.0;
  double ordp1coeff = 0.0;
  double ordp2coeff = 0.0;
  double errcoeff = 0.0;        /* error coefficient */

  int deltaord;

#ifdef DEBUG
  {
    size_t di;

    printf ("msadams_apply: t=%.5e, ord=%d, h=%.5e, y:", t, (int) ord, h);

    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", y[di]);
      }
    printf ("\n");
  }
#endif

  /* Check if t is the same as on previous stepper call (or last
     failed call). This means that calculation of previous step failed
     or the step was rejected, and therefore previous state will be
     restored or the method will be reset.
   */

  if (state->ni > 0 && (t == state->tprev || t == state->failt))
    {
      if (state->ni == 1)
        {
          /* No step has been accepted yet, reset method */

          msadams_reset (vstate, dim);
#ifdef DEBUG
          printf ("-- first step was REJECTED, msadams_reset called\n");
#endif
        }
      else
        {
          /* A succesful step has been saved, restore previous state. */

          /* If previous step suggests order increase, but the step was
             rejected, then do not increase order.
           */

          if (ord > state->ordprev)
            {
              state->ord = state->ordprev;
              ord = state->ord;
            }

          /* Restore previous state */

          DBL_MEMCPY (z, zbackup, (MSADAMS_MAX_ORD + 1) * dim);
          DBL_MEMCPY (hprev, hprevbackup, MSADAMS_MAX_ORD);
          state->ordprev = state->ordprevbackup;
          state->ordwait = state->ordwaitbackup;

#ifdef DEBUG
          printf ("-- previous step was REJECTED, state restored\n");
#endif
        }

      /* If step is repeatedly rejected, then reset method */

      state->failcount++;

      {
        const size_t max_failcount = 3;

        if (state->failcount > max_failcount && state->ni > 1)
          {
            msadams_reset (vstate, dim);
            ord = state->ord;

#ifdef DEBUG
            printf ("-- max_failcount reached, msadams_reset called\n");
#endif
          }
      }
    }
  else
    {
      /* The previous step was accepted. Backup current state. */

      DBL_MEMCPY (zbackup, z, (MSADAMS_MAX_ORD + 1) * dim);
      DBL_MEMCPY (hprevbackup, hprev, MSADAMS_MAX_ORD);
      state->ordprevbackup = state->ordprev;
      state->ordwaitbackup = state->ordwait;

      state->failcount = 0;

#ifdef DEBUG
      if (state->ni > 0)
        {
          printf ("-- previous step was ACCEPTED, state saved\n");
        }
#endif
    }

#ifdef DEBUG
  printf ("-- ord=%d, ni=%ld, ordwait=%d\n", (int) ord, state->ni,
          (int) state->ordwait);
  printf ("-- ordprev: %d\n", (int) state->ordprev);
#endif

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

#ifdef DEBUG
  {
    size_t di;
    printf ("-- errlev: ");
    for (di = 0; di < dim; di++)
      {
        printf ("%.5e ", errlev[di]);
      }
    printf ("\n");
  }
#endif

  /* On first call initialize Nordsieck matrix */

  if (state->ni == 0)
    {
      size_t i;

      DBL_ZERO_MEMSET (z, (MSADAMS_MAX_ORD + 1) * dim);

      if (dydt_in != NULL)
        {
          DBL_MEMCPY (ytmp, dydt_in, dim);
        }
      else
        {
          int s = GSL_ODEIV_FN_EVAL (sys, t, y, ytmp);

          if (s != GSL_SUCCESS)
            {
              return s;
            }
        }

      DBL_MEMCPY (&z[0 * dim], y, dim);
      DBL_MEMCPY (&z[1 * dim], ytmp, dim);

      for (i = 0; i < dim; i++)
        {
          z[1 * dim + i] *= h;
        }
    }

  /* Sanity check  */

  deltaord = ord - state->ordprev;

  if (deltaord > 1 || deltaord < -1)
    {
      printf ("-- order change %d\n", deltaord);
      GSL_ERROR_NULL ("msadams_apply too large order change", GSL_ESANITY);
    }

  /* Modify Nordsieck matrix if order or step length has been changed */

  /* If order increased by 1, initialize new Nordsieck vector */

  if (deltaord == 1)
    {
      DBL_ZERO_MEMSET (&z[ord * dim], dim);

#ifdef DEBUG
      printf ("-- order increase detected, Nordsieck modified\n");
#endif
    }

  /* If order decreased by 1, adjust Nordsieck matrix */

  if (deltaord == -1)
    {
      double hsum = 0.0;
      size_t i, j;

      /* Calculate coefficients used in adjustment to l */

      DBL_ZERO_MEMSET (l, MSADAMS_MAX_ORD + 1);

      l[1] = 1.0;

      for (i = 1; i < ord; i++)
        {
          hsum += hprev[i - 1];

          for (j = i + 1; j > 0; j--)
            {
              l[j] *= hsum / hprev[0];
              l[j] += l[j - 1];
            }
        }

      for (i = 1; i < ord; i++)
        {
          l[i + 1] = (ord + 1) * l[i] / (i + 1);
        }

      /* Scale Nordsieck matrix */

      for (i = 2; i < ord + 1; i++)
        for (j = 0; j < dim; j++)
          {
            z[i * dim + j] += -l[i] * z[(ord + 1) * dim + j];
          }

#ifdef DEBUG
      printf ("-- order decrease detected, Nordsieck modified\n");
#endif
    }

  /* Scale Nordsieck vectors if step size has been changed */

  if (state->ni > 0 && h != hprev[0])
    {
      size_t i, j;
      const double hrel = h / hprev[0];
      double coeff = hrel;

      for (i = 1; i < ord + 1; i++)
        {
          for (j = 0; j < dim; j++)
            {
              z[i * dim + j] *= coeff;
            }

          coeff *= hrel;
        }

#ifdef DEBUG
      printf ("-- h != hprev, Nordsieck modified\n");
#endif
    }

  /* Calculate polynomial coefficients (l), error coefficient and
     auxiliary coefficients
   */

  msadams_calccoeffs (ord, state->ordwait, h, hprev, pc, l, &errcoeff,
                      &ordm1coeff, &ordp1coeff, &ordp2coeff);

  /* Carry out the prediction step */

  {
    size_t i, j, k;

    for (i = 1; i < ord + 1; i++)
      for (j = ord; j > i - 1; j--)
        for (k = 0; k < dim; k++)
          {
            z[(j - 1) * dim + k] += z[j * dim + k];
          }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- predicted y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif
  }

  /* Calculate correction step to abscor */
  {
    int s;
    s = msadams_corrector (vstate, sys, t, h, dim, z, errlev, l, errcoeff,
                           abscor, relcor, ytmp, ytmp2);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Add accepted final correction step to Nordsieck matrix */

    size_t i, j;

    for (i = 0; i < ord + 1; i++)
      for (j = 0; j < dim; j++)
        {
          z[i * dim + j] += l[i] * gsl_vector_get (abscor, j);
        }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- corrected y: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", z[di]);
        }
      printf ("\n");
    }
#endif

    /* Derivatives at output */

    if (dydt_out != NULL)
      {
        int s = GSL_ODEIV_FN_EVAL (sys, t + h, z, dydt_out);

        if (s == GSL_EBADFUNC)
          {
            return s;
          }

        if (s != GSL_SUCCESS)
          {
            msadams_failurehandler (vstate, dim, t);

#ifdef DEBUG
            printf ("-- FAIL at user function evaluation\n");
#endif
            return s;
          }
      }

    /* Calculate error estimate */

    for (i = 0; i < dim; i++)
      {
        yerr[i] = fabs (gsl_vector_get (abscor, i)) * errcoeff;
      }

#ifdef DEBUG
    {
      size_t di;
      printf ("-- yerr: ");
      for (di = 0; di < dim; di++)
        {
          printf ("%.5e ", yerr[di]);
        }
      printf ("\n");
    }
#endif

    /* Save y values */

    for (i = 0; i < dim; i++)
      {
        y[i] = z[0 * dim + i];
      }
  }

  /* Scale abscor with errlev for later use in norm calculations */
  {
    size_t i;

    for (i = 0; i < dim; i++)
      {
        gsl_vector_set (abscor, i, gsl_vector_get (abscor, i) / errlev[i]);
      }
  }

  /* Save items needed for evaluation of order increase on next
     call, if needed
   */

  if (state->ordwait == 1 && ord < MSADAMS_MAX_ORD)
    {
      size_t i;

      state->ordp1coeffprev = ordp1coeff;
      state->ordm1coeff = ordm1coeff;

      for (i = 0; i < dim; i++)
        {
          gsl_vector_set (svec, i, gsl_vector_get (abscor, i));
        }
    }

  /* Consider and execute order change for next step */

  if (state->ordwait == 0)
    {
      msadams_eval_order (abscor, tempvec, svec, errcoeff, dim, errlev,
                          state->ordm1coeff, ordp1coeff,
                          state->ordp1coeffprev, ordp2coeff,
                          hprev, h, z, &(state->ord), &(state->ordwait));
    }

  /* Save information about current step in state and update counters */
  {
    size_t i;

    state->ordprev = ord;

    for (i = MSADAMS_MAX_ORD - 1; i > 0; i--)
      {
        hprev[i] = hprev[i - 1];
      }
    hprev[0] = h;

#ifdef DEBUG
    {
      size_t di;
      printf ("-- hprev: ");
      for (di = 0; di < MSADAMS_MAX_ORD; di++)
        {
          printf ("%.5e ", hprev[di]);
        }
      printf ("\n");
    }
#endif

    state->tprev = t;
    state->ordwait--;
    state->ni++;
  }

  return GSL_SUCCESS;
}

static int
msadams_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
msadams_reset (void *vstate, size_t dim)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  state->ni = 0;
  state->ord = 1;
  state->ordprev = 1;
  state->ordprevbackup = 1;
  state->ordwait = 2;
  state->ordwaitbackup = 2;
  state->failord = 0;
  state->failt = GSL_NAN;
  state->failcount = 0;

  DBL_ZERO_MEMSET (state->hprev, MSADAMS_MAX_ORD);
  DBL_ZERO_MEMSET (state->z, (MSADAMS_MAX_ORD + 1) * dim);

#ifdef DEBUG
  printf ("-- msadams_reset called\n");
#endif

  return GSL_SUCCESS;
}

static unsigned int
msadams_order (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  return state->ord;
}

static void
msadams_free (void *vstate)
{
  msadams_state_t *state = (msadams_state_t *) vstate;

  gsl_vector_free (state->tempvec);
  gsl_vector_free (state->svec);
  gsl_vector_free (state->relcor);
  gsl_vector_free (state->abscor);
  free (state->errlev);
  free (state->hprevbackup);
  free (state->hprev);
  free (state->l);
  free (state->pc);
  free (state->ytmp2);
  free (state->ytmp);
  free (state->zbackup);
  free (state->z);
  free (state);
}

static const gsl_odeiv2_step_type msadams_type = {
  "msadams",                    /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &msadams_alloc,
  &msadams_apply,
  &msadams_set_driver,
  &msadams_reset,
  &msadams_order,
  &msadams_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_msadams = &msadams_type;


#include <gsl/gsl_complex.h>

#define RVEC(A) int A##n, double*A##p
#define RMAT(A) int A##r, int A##c, double* A##p
#define KRVEC(A) int A##n, const double*A##p
#define KRMAT(A) int A##r, int A##c, const double* A##p

#define CVEC(A) int A##n, gsl_complex*A##p
#define CMAT(A) int A##r, int A##c, gsl_complex* A##p
#define KCVEC(A) int A##n, const gsl_complex*A##p
#define KCMAT(A) int A##r, int A##c, const gsl_complex* A##p

#define FVEC(A) int A##n, float*A##p
#define FMAT(A) int A##r, int A##c, float* A##p
#define KFVEC(A) int A##n, const float*A##p
#define KFMAT(A) int A##r, int A##c, const float* A##p

#define QVEC(A) int A##n, gsl_complex_float*A##p
#define QMAT(A) int A##r, int A##c, gsl_complex_float* A##p
#define KQVEC(A) int A##n, const gsl_complex_float*A##p
#define KQMAT(A) int A##r, int A##c, const gsl_complex_float* A##p

#include <gsl/gsl_blas.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_multimin.h>
#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_roots.h>
#include <gsl/gsl_multifit_nlin.h>
#include <string.h>
#include <stdio.h>

#define MACRO(B) do {B} while (0)
#define ERROR(CODE) MACRO(return CODE;)
#define REQUIRES(COND, CODE) MACRO(if(!(COND)) {ERROR(CODE);})
#define OK return 0;

#define MIN(A,B) ((A)<(B)?(A):(B))
#define MAX(A,B) ((A)>(B)?(A):(B))

#ifdef DBG
#define DEBUGMSG(M) printf("*** calling aux C function: %s\n",M);
#else
#define DEBUGMSG(M)
#endif

#define CHECK(RES,CODE) MACRO(if(RES) return CODE;)

#ifdef DBG
#define DEBUGMAT(MSG,X) printf(MSG" = \n"); gsl_matrix_fprintf(stdout,X,"%f"); printf("\n");
#else
#define DEBUGMAT(MSG,X)
#endif

#ifdef DBG
#define DEBUGVEC(MSG,X) printf(MSG" = \n"); gsl_vector_fprintf(stdout,X,"%f"); printf("\n");
#else
#define DEBUGVEC(MSG,X)
#endif

#define DVVIEW(A) gsl_vector_view A = gsl_vector_view_array(A##p,A##n)
#define DMVIEW(A) gsl_matrix_view A = gsl_matrix_view_array(A##p,A##r,A##c)
#define CVVIEW(A) gsl_vector_complex_view A = gsl_vector_complex_view_array((double*)A##p,A##n)
#define CMVIEW(A) gsl_matrix_complex_view A = gsl_matrix_complex_view_array((double*)A##p,A##r,A##c)
#define KDVVIEW(A) gsl_vector_const_view A = gsl_vector_const_view_array(A##p,A##n)
#define KDMVIEW(A) gsl_matrix_const_view A = gsl_matrix_const_view_array(A##p,A##r,A##c)
#define KCVVIEW(A) gsl_vector_complex_const_view A = gsl_vector_complex_const_view_array((double*)A##p,A##n)
#define KCMVIEW(A) gsl_matrix_complex_const_view A = gsl_matrix_complex_const_view_array((double*)A##p,A##r,A##c)

#define FVVIEW(A) gsl_vector_float_view A = gsl_vector_float_view_array(A##p,A##n)
#define FMVIEW(A) gsl_matrix_float_view A = gsl_matrix_float_view_array(A##p,A##r,A##c)
#define QVVIEW(A) gsl_vector_complex_float_view A = gsl_vector_float_complex_view_array((float*)A##p,A##n)
#define QMVIEW(A) gsl_matrix_complex_float_view A = gsl_matrix_float_complex_view_array((float*)A##p,A##r,A##c)
#define KFVVIEW(A) gsl_vector_float_const_view A = gsl_vector_float_const_view_array(A##p,A##n)
#define KFMVIEW(A) gsl_matrix_float_const_view A = gsl_matrix_float_const_view_array(A##p,A##r,A##c)
#define KQVVIEW(A) gsl_vector_complex_float_const_view A = gsl_vector_complex_float_const_view_array((float*)A##p,A##n)
#define KQMVIEW(A) gsl_matrix_complex_float_const_view A = gsl_matrix_complex_float_const_view_array((float*)A##p,A##r,A##c)

#define V(a) (&a.vector)
#define M(a) (&a.matrix)

#define GCVEC(A) int A##n, gsl_complex*A##p
#define KGCVEC(A) int A##n, const gsl_complex*A##p

#define GQVEC(A) int A##n, gsl_complex_float*A##p
#define KGQVEC(A) int A##n, const gsl_complex_float*A##p

#define BAD_SIZE 2000
#define BAD_CODE 2001
#define MEM      2002
#define BAD_FILE 2003

void no_abort_on_error() {
    gsl_set_error_handler_off();
}

int sumF(KFVEC(x),FVEC(r)) {
    DEBUGMSG("sumF");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    float res = 0;
    for (i = 0; i < xn; i++) res += xp[i];
    rp[0] = res;
    OK
}

int sumR(KRVEC(x),RVEC(r)) {
    DEBUGMSG("sumR");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    double res = 0;
    for (i = 0; i < xn; i++) res += xp[i];
    rp[0] = res;
    OK
}

int sumQ(KQVEC(x),QVEC(r)) {
    DEBUGMSG("sumQ");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    gsl_complex_float res;
    res.dat[0] = 0;
    res.dat[1] = 0;
    for (i = 0; i < xn; i++) {
      res.dat[0] += xp[i].dat[0];
      res.dat[1] += xp[i].dat[1];
    }
    rp[0] = res;
    OK
}

int sumC(KCVEC(x),CVEC(r)) {
    DEBUGMSG("sumC");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    gsl_complex res;
    res.dat[0] = 0;
    res.dat[1] = 0;
    for (i = 0; i < xn; i++)  {
      res.dat[0] += xp[i].dat[0];
      res.dat[1] += xp[i].dat[1];
    }
    rp[0] = res;
    OK
}

int prodF(KFVEC(x),FVEC(r)) {
    DEBUGMSG("prodF");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    float res = 1;
    for (i = 0; i < xn; i++) res *= xp[i];
    rp[0] = res;
    OK
}

int prodR(KRVEC(x),RVEC(r)) {
    DEBUGMSG("prodR");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    double res = 1;
    for (i = 0; i < xn; i++) res *= xp[i];
    rp[0] = res;
    OK
}

int prodQ(KQVEC(x),QVEC(r)) {
    DEBUGMSG("prodQ");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    gsl_complex_float res;
    float temp;
    res.dat[0] = 1;
    res.dat[1] = 0;
    for (i = 0; i < xn; i++) {
      temp       = res.dat[0] * xp[i].dat[0] - res.dat[1] * xp[i].dat[1];
      res.dat[1] = res.dat[0] * xp[i].dat[1] + res.dat[1] * xp[i].dat[0];
      res.dat[0] = temp;
    }
    rp[0] = res;
    OK
}

int prodC(KCVEC(x),CVEC(r)) {
    DEBUGMSG("prodC");
    REQUIRES(rn==1,BAD_SIZE);
    int i;
    gsl_complex res;
    double temp;
    res.dat[0] = 1;
    res.dat[1] = 0;
    for (i = 0; i < xn; i++)  {
      temp       = res.dat[0] * xp[i].dat[0] - res.dat[1] * xp[i].dat[1];
      res.dat[1] = res.dat[0] * xp[i].dat[1] + res.dat[1] * xp[i].dat[0];
      res.dat[0] = temp;
    }
    rp[0] = res;
    OK
}

int dotF(KFVEC(x), KFVEC(y), FVEC(r)) {
    DEBUGMSG("dotF");
    REQUIRES(xn==yn,BAD_SIZE);
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("dotF");
    KFVVIEW(x);
    KFVVIEW(y);
    gsl_blas_sdot(V(x),V(y),rp);
    OK
}

int dotR(KRVEC(x), KRVEC(y), RVEC(r)) {
    DEBUGMSG("dotR");
    REQUIRES(xn==yn,BAD_SIZE);
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("dotR");
    KDVVIEW(x);
    KDVVIEW(y);
    gsl_blas_ddot(V(x),V(y),rp);
    OK
}

int dotQ(KQVEC(x), KQVEC(y), QVEC(r)) {
    DEBUGMSG("dotQ");
    REQUIRES(xn==yn,BAD_SIZE);
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("dotQ");
    KQVVIEW(x);
    KQVVIEW(y);
    gsl_blas_cdotu(V(x),V(y),rp);
    OK
}

int dotC(KCVEC(x), KCVEC(y), CVEC(r)) {
    DEBUGMSG("dotC");
    REQUIRES(xn==yn,BAD_SIZE);
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("dotC");
    KCVVIEW(x);
    KCVVIEW(y);
    gsl_blas_zdotu(V(x),V(y),rp);
    OK
}

int toScalarR(int code, KRVEC(x), RVEC(r)) {
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("toScalarR");
    KDVVIEW(x);
    double res;
    switch(code) {
        case 0: { res = gsl_blas_dnrm2(V(x)); break; }
        case 1: { res = gsl_blas_dasum(V(x));  break; }
        case 2: { res = gsl_vector_max_index(V(x));  break; }
        case 3: { res = gsl_vector_max(V(x));  break; }
        case 4: { res = gsl_vector_min_index(V(x)); break; }
        case 5: { res = gsl_vector_min(V(x)); break; }
        default: ERROR(BAD_CODE);
    }
    rp[0] = res;
    OK
}

int toScalarF(int code, KFVEC(x), FVEC(r)) {
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("toScalarF");
    KFVVIEW(x);
    float res;
    switch(code) {
        case 0: { res = gsl_blas_snrm2(V(x)); break; }
        case 1: { res = gsl_blas_sasum(V(x));  break; }
        case 2: { res = gsl_vector_float_max_index(V(x));  break; }
        case 3: { res = gsl_vector_float_max(V(x));  break; }
        case 4: { res = gsl_vector_float_min_index(V(x)); break; }
        case 5: { res = gsl_vector_float_min(V(x)); break; }
        default: ERROR(BAD_CODE);
    }
    rp[0] = res;
    OK
}

int toScalarC(int code, KCVEC(x), RVEC(r)) {
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("toScalarC");
    KCVVIEW(x);
    double res;
    switch(code) {
        case 0: { res = gsl_blas_dznrm2(V(x)); break; }
        case 1: { res = gsl_blas_dzasum(V(x));  break; }
        default: ERROR(BAD_CODE);
    }
    rp[0] = res;
    OK
}

int toScalarQ(int code, KQVEC(x), FVEC(r)) {
    REQUIRES(rn==1,BAD_SIZE);
    DEBUGMSG("toScalarQ");
    KQVVIEW(x);
    float res;
    switch(code) {
        case 0: { res = gsl_blas_scnrm2(V(x)); break; }
        case 1: { res = gsl_blas_scasum(V(x));  break; }
        default: ERROR(BAD_CODE);
    }
    rp[0] = res;
    OK
}

inline double sign(double x) {
    if(x>0) {
        return +1.0;
    } else if (x<0) {
        return -1.0;
    } else {
        return 0.0;
    }
}

inline float float_sign(float x) {
    if(x>0) {
        return +1.0;
    } else if (x<0) {
        return -1.0;
    } else {
        return 0.0;
    }
}

inline gsl_complex complex_abs(gsl_complex z) {
    gsl_complex r;
    r.dat[0] = gsl_complex_abs(z);
    r.dat[1] = 0;
    return r;
}

inline gsl_complex complex_signum(gsl_complex z) {
    gsl_complex r;
    double mag;
    if (z.dat[0] == 0 && z.dat[1] == 0) {
        r.dat[0] = 0;
        r.dat[1] = 0;
    } else {
        mag = gsl_complex_abs(z);
        r.dat[0] = z.dat[0]/mag;
        r.dat[1] = z.dat[1]/mag;
    }
    return r;
}

#define OP(C,F) case C: { for(k=0;k<xn;k++) rp[k] = F(xp[k]); OK }
#define OPV(C,E) case C: { for(k=0;k<xn;k++) rp[k] = E; OK }
int mapR(int code, KRVEC(x), RVEC(r)) {
    int k;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapR");
    switch (code) {
        OP(0,sin)
        OP(1,cos)
        OP(2,tan)
        OP(3,fabs)
        OP(4,asin)
        OP(5,acos)
        OP(6,atan) /* atan2 mediante vectorZip */
        OP(7,sinh)
        OP(8,cosh)
        OP(9,tanh)
        OP(10,gsl_asinh)
        OP(11,gsl_acosh)
        OP(12,gsl_atanh)
        OP(13,exp)
        OP(14,log)
        OP(15,sign)
        OP(16,sqrt)
        default: ERROR(BAD_CODE);
    }
}

int mapF(int code, KFVEC(x), FVEC(r)) {
    int k;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapF");
    switch (code) {
        OP(0,sin)
        OP(1,cos)
        OP(2,tan)
        OP(3,fabs)
        OP(4,asin)
        OP(5,acos)
        OP(6,atan) /* atan2 mediante vectorZip */
        OP(7,sinh)
        OP(8,cosh)
        OP(9,tanh)
        OP(10,gsl_asinh)
        OP(11,gsl_acosh)
        OP(12,gsl_atanh)
        OP(13,exp)
        OP(14,log)
        OP(15,sign)
        OP(16,sqrt)
        default: ERROR(BAD_CODE);
    }
}

int mapCAux(int code, KGCVEC(x), GCVEC(r)) {
    int k;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapC");
    switch (code) {
        OP(0,gsl_complex_sin)
        OP(1,gsl_complex_cos)
        OP(2,gsl_complex_tan)
        OP(3,complex_abs)
        OP(4,gsl_complex_arcsin)
        OP(5,gsl_complex_arccos)
        OP(6,gsl_complex_arctan)
        OP(7,gsl_complex_sinh)
        OP(8,gsl_complex_cosh)
        OP(9,gsl_complex_tanh)
        OP(10,gsl_complex_arcsinh)
        OP(11,gsl_complex_arccosh)
        OP(12,gsl_complex_arctanh)
        OP(13,gsl_complex_exp)
        OP(14,gsl_complex_log)
        OP(15,complex_signum)
        OP(16,gsl_complex_sqrt)

        // gsl_complex_arg
        // gsl_complex_abs
        default: ERROR(BAD_CODE);
    }
}

int mapC(int code, KCVEC(x), CVEC(r)) {
    return mapCAux(code, xn, (gsl_complex*)xp, rn, (gsl_complex*)rp);
}

gsl_complex_float complex_float_math_fun(gsl_complex (*cf)(gsl_complex), gsl_complex_float a)
{
  gsl_complex c;
  gsl_complex r;

  gsl_complex_float float_r;

  c.dat[0] = a.dat[0];
  c.dat[1] = a.dat[1];

  r = (*cf)(c);

  float_r.dat[0] = r.dat[0];
  float_r.dat[1] = r.dat[1];

  return float_r;
}

gsl_complex_float complex_float_math_op(gsl_complex (*cf)(gsl_complex,gsl_complex),
					gsl_complex_float a,gsl_complex_float b)
{
  gsl_complex c1;
  gsl_complex c2;
  gsl_complex r;

  gsl_complex_float float_r;

  c1.dat[0] = a.dat[0];
  c1.dat[1] = a.dat[1];

  c2.dat[0] = b.dat[0];
  c2.dat[1] = b.dat[1];

  r = (*cf)(c1,c2);

  float_r.dat[0] = r.dat[0];
  float_r.dat[1] = r.dat[1];

  return float_r;
}

#define OPC(C,F) case C: { for(k=0;k<xn;k++) rp[k] = complex_float_math_fun(&F,xp[k]); OK }
#define OPCA(C,F,A,B) case C: { for(k=0;k<xn;k++) rp[k] = complex_float_math_op(&F,A,B); OK }
int mapQAux(int code, KGQVEC(x), GQVEC(r)) {
    int k;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapQ");
    switch (code) {
        OPC(0,gsl_complex_sin)
        OPC(1,gsl_complex_cos)
        OPC(2,gsl_complex_tan)
        OPC(3,complex_abs)
        OPC(4,gsl_complex_arcsin)
        OPC(5,gsl_complex_arccos)
        OPC(6,gsl_complex_arctan)
        OPC(7,gsl_complex_sinh)
        OPC(8,gsl_complex_cosh)
        OPC(9,gsl_complex_tanh)
        OPC(10,gsl_complex_arcsinh)
        OPC(11,gsl_complex_arccosh)
        OPC(12,gsl_complex_arctanh)
        OPC(13,gsl_complex_exp)
        OPC(14,gsl_complex_log)
        OPC(15,complex_signum)
        OPC(16,gsl_complex_sqrt)

        // gsl_complex_arg
        // gsl_complex_abs
        default: ERROR(BAD_CODE);
    }
}

int mapQ(int code, KQVEC(x), QVEC(r)) {
    return mapQAux(code, xn, (gsl_complex_float*)xp, rn, (gsl_complex_float*)rp);
}

int mapValR(int code, double* pval, KRVEC(x), RVEC(r)) {
    int k;
    double val = *pval;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapValR");
    switch (code) {
        OPV(0,val*xp[k])
        OPV(1,val/xp[k])
        OPV(2,val+xp[k])
        OPV(3,val-xp[k])
        OPV(4,pow(val,xp[k]))
        OPV(5,pow(xp[k],val))
        default: ERROR(BAD_CODE);
    }
}

int mapValF(int code, float* pval, KFVEC(x), FVEC(r)) {
    int k;
    float val = *pval;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapValF");
    switch (code) {
        OPV(0,val*xp[k])
        OPV(1,val/xp[k])
        OPV(2,val+xp[k])
        OPV(3,val-xp[k])
        OPV(4,pow(val,xp[k]))
        OPV(5,pow(xp[k],val))
        default: ERROR(BAD_CODE);
    }
}

int mapValCAux(int code, gsl_complex* pval, KGCVEC(x), GCVEC(r)) {
    int k;
    gsl_complex val = *pval;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapValC");
    switch (code) {
        OPV(0,gsl_complex_mul(val,xp[k]))
        OPV(1,gsl_complex_div(val,xp[k]))
        OPV(2,gsl_complex_add(val,xp[k]))
        OPV(3,gsl_complex_sub(val,xp[k]))
        OPV(4,gsl_complex_pow(val,xp[k]))
        OPV(5,gsl_complex_pow(xp[k],val))
        default: ERROR(BAD_CODE);
    }
}

int mapValC(int code, gsl_complex* val, KCVEC(x), CVEC(r)) {
    return mapValCAux(code, val, xn, (gsl_complex*)xp, rn, (gsl_complex*)rp);
}

int mapValQAux(int code, gsl_complex_float* pval, KQVEC(x), GQVEC(r)) {
    int k;
    gsl_complex_float val = *pval;
    REQUIRES(xn == rn,BAD_SIZE);
    DEBUGMSG("mapValQ");
    switch (code) {
        OPCA(0,gsl_complex_mul,val,xp[k])
	OPCA(1,gsl_complex_div,val,xp[k])
	OPCA(2,gsl_complex_add,val,xp[k])
	OPCA(3,gsl_complex_sub,val,xp[k])
	OPCA(4,gsl_complex_pow,val,xp[k])
	OPCA(5,gsl_complex_pow,xp[k],val)
        default: ERROR(BAD_CODE);
    }
}

int mapValQ(int code, gsl_complex_float* val, KQVEC(x), QVEC(r)) {
    return mapValQAux(code, val, xn, (gsl_complex_float*)xp, rn, (gsl_complex_float*)rp);
}

#define OPZE(C,msg,E) case C: {DEBUGMSG(msg) for(k=0;k<an;k++) rp[k] = E(ap[k],bp[k]); OK }
#define OPZV(C,msg,E) case C: {DEBUGMSG(msg) res = E(V(r),V(b)); CHECK(res,res); OK }
int zipR(int code, KRVEC(a), KRVEC(b), RVEC(r)) {
    REQUIRES(an == bn && an == rn, BAD_SIZE);
    int k;
    switch(code) {
        OPZE(4,"zipR Pow",pow)
        OPZE(5,"zipR ATan2",atan2)
    }
    KDVVIEW(a);
    KDVVIEW(b);
    DVVIEW(r);
    gsl_vector_memcpy(V(r),V(a));
    int res;
    switch(code) {
        OPZV(0,"zipR Add",gsl_vector_add)
        OPZV(1,"zipR Sub",gsl_vector_sub)
        OPZV(2,"zipR Mul",gsl_vector_mul)
        OPZV(3,"zipR Div",gsl_vector_div)
        default: ERROR(BAD_CODE);
    }
}

int zipF(int code, KFVEC(a), KFVEC(b), FVEC(r)) {
    REQUIRES(an == bn && an == rn, BAD_SIZE);
    int k;
    switch(code) {
        OPZE(4,"zipF Pow",pow)
        OPZE(5,"zipF ATan2",atan2)
    }
    KFVVIEW(a);
    KFVVIEW(b);
    FVVIEW(r);
    gsl_vector_float_memcpy(V(r),V(a));
    int res;
    switch(code) {
        OPZV(0,"zipF Add",gsl_vector_float_add)
        OPZV(1,"zipF Sub",gsl_vector_float_sub)
        OPZV(2,"zipF Mul",gsl_vector_float_mul)
        OPZV(3,"zipF Div",gsl_vector_float_div)
        default: ERROR(BAD_CODE);
    }
}

int zipCAux(int code, KGCVEC(a), KGCVEC(b), GCVEC(r)) {
    REQUIRES(an == bn && an == rn, BAD_SIZE);
    int k;
    switch(code) {
        OPZE(0,"zipC Add",gsl_complex_add)
        OPZE(1,"zipC Sub",gsl_complex_sub)
        OPZE(2,"zipC Mul",gsl_complex_mul)
        OPZE(3,"zipC Div",gsl_complex_div)
        OPZE(4,"zipC Pow",gsl_complex_pow)
        //OPZE(5,"zipR ATan2",atan2)
    }
    //KCVVIEW(a);
    //KCVVIEW(b);
    //CVVIEW(r);
    //gsl_vector_memcpy(V(r),V(a));
    //int res;
    switch(code) {
        default: ERROR(BAD_CODE);
    }
}

int zipC(int code, KCVEC(a), KCVEC(b), CVEC(r)) {
    return zipCAux(code, an, (gsl_complex*)ap, bn, (gsl_complex*)bp, rn, (gsl_complex*)rp);
}

#define OPCZE(C,msg,E) case C: {DEBUGMSG(msg) for(k=0;k<an;k++) rp[k] = complex_float_math_op(&E,ap[k],bp[k]); OK }
int zipQAux(int code, KGQVEC(a), KGQVEC(b), GQVEC(r)) {
    REQUIRES(an == bn && an == rn, BAD_SIZE);
    int k;
    switch(code) {
        OPCZE(0,"zipQ Add",gsl_complex_add)
        OPCZE(1,"zipQ Sub",gsl_complex_sub)
        OPCZE(2,"zipQ Mul",gsl_complex_mul)
        OPCZE(3,"zipQ Div",gsl_complex_div)
        OPCZE(4,"zipQ Pow",gsl_complex_pow)
        //OPZE(5,"zipR ATan2",atan2)
    }
    //KCVVIEW(a);
    //KCVVIEW(b);
    //CVVIEW(r);
    //gsl_vector_memcpy(V(r),V(a));
    //int res;
    switch(code) {
        default: ERROR(BAD_CODE);
    }
}

int zipQ(int code, KQVEC(a), KQVEC(b), QVEC(r)) {
    return zipQAux(code, an, (gsl_complex_float*)ap, bn, (gsl_complex_float*)bp, rn, (gsl_complex_float*)rp);
}

int fft(int code, KCVEC(X), CVEC(R)) {
    REQUIRES(Xn == Rn,BAD_SIZE);
    DEBUGMSG("fft");
    int s = Xn;
    gsl_fft_complex_wavetable * wavetable = gsl_fft_complex_wavetable_alloc (s);
    gsl_fft_complex_workspace * workspace = gsl_fft_complex_workspace_alloc (s);
    gsl_vector_const_view X = gsl_vector_const_view_array((double*)Xp, 2*Xn);
    gsl_vector_view R = gsl_vector_view_array((double*)Rp, 2*Rn);
    gsl_blas_dcopy(&X.vector,&R.vector);
    if(code==0) {
        gsl_fft_complex_forward ((double*)Rp, 1, s, wavetable, workspace);
    } else {
        gsl_fft_complex_inverse ((double*)Rp, 1, s, wavetable, workspace);
    }
    gsl_fft_complex_wavetable_free (wavetable);
    gsl_fft_complex_workspace_free (workspace);
    OK
}

int deriv(int code, double f(double, void*), double x, double h, double * result, double * abserr)
{
    gsl_function F;
    F.function = f;
    F.params = 0;

    if(code==0) return gsl_deriv_central (&F, x, h, result, abserr);

    if(code==1) return gsl_deriv_forward (&F, x, h, result, abserr);

    if(code==2) return gsl_deriv_backward (&F, x, h, result, abserr);

    return 0;
}

int integrate_qng(double f(double, void*), double a, double b, double prec,
                   double *result, double*error) {
    DEBUGMSG("integrate_qng");
    gsl_function F;
    F.function = f;
    F.params = NULL;
    size_t neval;
    int res = gsl_integration_qng (&F, a,b, 0, prec, result, error, &neval);
    CHECK(res,res);
    OK
}

int integrate_qags(double f(double,void*), double a, double b, double prec, int w,
               double *result, double* error) {
    DEBUGMSG("integrate_qags");
    gsl_integration_workspace * wk = gsl_integration_workspace_alloc (w);
    gsl_function F;
    F.function = f;
    F.params = NULL;
    int res = gsl_integration_qags (&F, a,b, 0, prec, w,wk, result, error);
    CHECK(res,res);
    gsl_integration_workspace_free (wk);
    OK
}

int integrate_qagi(double f(double,void*), double prec, int w,
               double *result, double* error) {
    DEBUGMSG("integrate_qagi");
    gsl_integration_workspace * wk = gsl_integration_workspace_alloc (w);
    gsl_function F;
    F.function = f;
    F.params = NULL;
    int res = gsl_integration_qagi (&F, 0, prec, w,wk, result, error);
    CHECK(res,res);
    gsl_integration_workspace_free (wk);
    OK
}

int integrate_qagiu(double f(double,void*), double a, double prec, int w,
               double *result, double* error) {
    DEBUGMSG("integrate_qagiu");
    gsl_integration_workspace * wk = gsl_integration_workspace_alloc (w);
    gsl_function F;
    F.function = f;
    F.params = NULL;
    int res = gsl_integration_qagiu (&F, a, 0, prec, w,wk, result, error);
    CHECK(res,res);
    gsl_integration_workspace_free (wk);
    OK
}

int integrate_qagil(double f(double,void*), double b, double prec, int w,
               double *result, double* error) {
    DEBUGMSG("integrate_qagil");
    gsl_integration_workspace * wk = gsl_integration_workspace_alloc (w);
    gsl_function F;
    F.function = f;
    F.params = NULL;
    int res = gsl_integration_qagil (&F, b, 0, prec, w,wk, result, error);
    CHECK(res,res);
    gsl_integration_workspace_free (wk);
    OK
}

int polySolve(KRVEC(a), CVEC(z)) {
    DEBUGMSG("polySolve");
    REQUIRES(an>1,BAD_SIZE);
    gsl_poly_complex_workspace * w = gsl_poly_complex_workspace_alloc (an);
    int res = gsl_poly_complex_solve ((double*)ap, an, w, (double*)zp);
    CHECK(res,res);
    gsl_poly_complex_workspace_free (w);
    OK;
}

int vector_fscanf(char*filename, RVEC(a)) {
    DEBUGMSG("gsl_vector_fscanf");
    DVVIEW(a);
    FILE * f = fopen(filename,"r");
    CHECK(!f,BAD_FILE);
    int res = gsl_vector_fscanf(f,V(a));
    CHECK(res,res);
    fclose (f);
    OK
}

int vector_fprintf(char*filename, char*fmt, RVEC(a)) {
    DEBUGMSG("gsl_vector_fprintf");
    DVVIEW(a);
    FILE * f = fopen(filename,"w");
    CHECK(!f,BAD_FILE);
    int res = gsl_vector_fprintf(f,V(a),fmt);
    CHECK(res,res);
    fclose (f);
    OK
}

int vector_fread(char*filename, RVEC(a)) {
    DEBUGMSG("gsl_vector_fread");
    DVVIEW(a);
    FILE * f = fopen(filename,"r");
    CHECK(!f,BAD_FILE);
    int res = gsl_vector_fread(f,V(a));
    CHECK(res,res);
    fclose (f);
    OK
}

int vector_fwrite(char*filename, RVEC(a)) {
    DEBUGMSG("gsl_vector_fwrite");
    DVVIEW(a);
    FILE * f = fopen(filename,"w");
    CHECK(!f,BAD_FILE);
    int res = gsl_vector_fwrite(f,V(a));
    CHECK(res,res);
    fclose (f);
    OK
}

int matrix_fprintf(char*filename, char*fmt, int ro, RMAT(m)) {
    DEBUGMSG("matrix_fprintf");
    FILE * f = fopen(filename,"w");
    CHECK(!f,BAD_FILE);
    int i,j,sr,sc;
    if (ro==1) { sr = mc; sc = 1;} else { sr = 1; sc = mr;}
    #define AT(M,r,c) (M##p[(r)*sr+(c)*sc])
    for (i=0; i<mr; i++) {
        for (j=0; j<mc-1; j++) {
            fprintf(f,fmt,AT(m,i,j));
            fprintf(f," ");
        }
        fprintf(f,fmt,AT(m,i,j));
        fprintf(f,"\n");
    }
    fclose (f);
    OK
}

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

typedef double Trawfun(int, double*);

double only_f_aux_min(const gsl_vector*x, void *pars) {
    Trawfun * f = (Trawfun*) pars;
    double* p = (double*)calloc(x->size,sizeof(double));
    int k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }
    double res = f(x->size,p);
    free(p);
    return res;
}

// this version returns info about intermediate steps
int minimize(int method, double f(int, double*), double tolsize, int maxit,
                 KRVEC(xi), KRVEC(sz), RMAT(sol)) {
    REQUIRES(xin==szn && solr == maxit && solc == 3+xin,BAD_SIZE);
    DEBUGMSG("minimizeList (nmsimplex)");
    gsl_multimin_function my_func;
    // extract function from pars
    my_func.f = only_f_aux_min;
    my_func.n = xin;
    my_func.params = f;
    size_t iter = 0;
    int status;
    double size;
    const gsl_multimin_fminimizer_type *T;
    gsl_multimin_fminimizer *s = NULL;
    // Initial vertex size vector
    KDVVIEW(sz);
    // Starting point
    KDVVIEW(xi);
    // Minimizer nmsimplex, without derivatives
    switch(method) {
        case 0 : {T = gsl_multimin_fminimizer_nmsimplex; break; }
#ifdef GSL110
        case 1 : {T = gsl_multimin_fminimizer_nmsimplex; break; }
#else
        case 1 : {T = gsl_multimin_fminimizer_nmsimplex2; break; }
#endif
        default: ERROR(BAD_CODE);
    }
    s = gsl_multimin_fminimizer_alloc (T, my_func.n);
    gsl_multimin_fminimizer_set (s, &my_func, V(xi), V(sz));
    do {
        status = gsl_multimin_fminimizer_iterate (s);
        size = gsl_multimin_fminimizer_size (s);

        solp[iter*solc+0] = iter+1;
        solp[iter*solc+1] = s->fval;
        solp[iter*solc+2] = size;

        int k;
        for(k=0;k<xin;k++) {
            solp[iter*solc+k+3] = gsl_vector_get(s->x,k);
        }
        iter++;
        if (status) break;
        status = gsl_multimin_test_size (size, tolsize);
    } while (status == GSL_CONTINUE && iter < maxit);
    int i,j;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        for(j=1;j<solc;j++) {
            solp[i*solc+j]=0.;
        }
    }
    gsl_multimin_fminimizer_free(s);
    OK
}

// working with the gradient

typedef struct {double (*f)(int, double*); int (*df)(int, double*, int, double*);} Tfdf;

double f_aux_min(const gsl_vector*x, void *pars) {
    Tfdf * fdf = ((Tfdf*) pars);
    double* p = (double*)calloc(x->size,sizeof(double));
    int k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }
    double res = fdf->f(x->size,p);
    free(p);
    return res;
}

void df_aux_min(const gsl_vector * x, void * pars, gsl_vector * g) {
    Tfdf * fdf = ((Tfdf*) pars);
    double* p = (double*)calloc(x->size,sizeof(double));
    double* q = (double*)calloc(g->size,sizeof(double));
    int k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }

    fdf->df(x->size,p,g->size,q);

    for(k=0;k<x->size;k++) {
        gsl_vector_set(g,k,q[k]);
    }
    free(p);
    free(q);
}

void fdf_aux_min(const gsl_vector * x, void * pars, double * f, gsl_vector * g) {
    *f = f_aux_min(x,pars);
    df_aux_min(x,pars,g);
}

int minimizeD(int method, double f(int, double*), int df(int, double*, int, double*),
              double initstep, double minimpar, double tolgrad, int maxit,
              KRVEC(xi), RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 2+xin,BAD_SIZE);
    DEBUGMSG("minimizeWithDeriv (conjugate_fr)");
    gsl_multimin_function_fdf my_func;
    // extract function from pars
    my_func.f = f_aux_min;
    my_func.df = df_aux_min;
    my_func.fdf = fdf_aux_min;
    my_func.n = xin;
    Tfdf stfdf;
    stfdf.f = f;
    stfdf.df = df;
    my_func.params = &stfdf;
    size_t iter = 0;
    int status;
    const gsl_multimin_fdfminimizer_type *T;
    gsl_multimin_fdfminimizer *s = NULL;
    // Starting point
    KDVVIEW(xi);
    // conjugate gradient fr
    switch(method) {
        case 0 : {T = gsl_multimin_fdfminimizer_conjugate_fr; break; }
        case 1 : {T = gsl_multimin_fdfminimizer_conjugate_pr; break; }
        case 2 : {T = gsl_multimin_fdfminimizer_vector_bfgs; break; }
        case 3 : {T = gsl_multimin_fdfminimizer_vector_bfgs2; break; }
        case 4 : {T = gsl_multimin_fdfminimizer_steepest_descent; break; }
        default: ERROR(BAD_CODE);
    }
    s = gsl_multimin_fdfminimizer_alloc (T, my_func.n);
    gsl_multimin_fdfminimizer_set (s, &my_func, V(xi), initstep, minimpar);
    do {
        status = gsl_multimin_fdfminimizer_iterate (s);
        solp[iter*solc+0] = iter+1;
        solp[iter*solc+1] = s->f;
        int k;
        for(k=0;k<xin;k++) {
            solp[iter*solc+k+2] = gsl_vector_get(s->x,k);
        }
        iter++;
        if (status) break;
        status = gsl_multimin_test_gradient (s->gradient, tolgrad);
    } while (status == GSL_CONTINUE && iter < maxit);
    int i,j;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        for(j=1;j<solc;j++) {
            solp[i*solc+j]=0.;
        }
    }
    gsl_multimin_fdfminimizer_free(s);
    OK
}

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

double only_f_aux_root(double x, void *pars) {
    double (*f)(double) = (double (*)(double)) pars;
    return f(x);
}

int root(int method, double f(double),
         double epsrel, int maxit,
         double xl, double xu, RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 4,BAD_SIZE);
    DEBUGMSG("root_only_f");
    gsl_function my_func;
    // extract function from pars
    my_func.function = only_f_aux_root;
    my_func.params = f;
    size_t iter = 0;
    int status;
    const gsl_root_fsolver_type *T;
    gsl_root_fsolver *s;
    // Starting point
    switch(method) {
        case 0 : {T = gsl_root_fsolver_bisection; printf("7\n"); break; }
        case 1 : {T = gsl_root_fsolver_falsepos; break; }
        case 2 : {T = gsl_root_fsolver_brent; break; }
        default: ERROR(BAD_CODE);
    }
    s = gsl_root_fsolver_alloc (T);
    gsl_root_fsolver_set (s, &my_func, xl, xu);
    do {
           double best, current_lo, current_hi;
           status = gsl_root_fsolver_iterate (s);
           best = gsl_root_fsolver_root (s);
           current_lo = gsl_root_fsolver_x_lower (s);
           current_hi = gsl_root_fsolver_x_upper (s);
           solp[iter*solc] = iter + 1;
           solp[iter*solc+1] = best;
           solp[iter*solc+2] = current_lo;
           solp[iter*solc+3] = current_hi;
           iter++;
           if (status)   /* check if solver is stuck */
             break;

           status =
               gsl_root_test_interval (current_lo, current_hi, 0, epsrel);
        }
        while (status == GSL_CONTINUE && iter < maxit);
    int i;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        solp[i*solc+1]=0.;
        solp[i*solc+2]=0.;
        solp[i*solc+3]=0.;
    }
    gsl_root_fsolver_free(s);
    OK
}

typedef struct {
    double (*f)(double);
    double (*jf)(double);
} uniTfjf;

double f_aux_uni(double x, void *pars) {
    uniTfjf * fjf = ((uniTfjf*) pars);
    return (fjf->f)(x);
}

double jf_aux_uni(double x, void * pars) {
    uniTfjf * fjf = ((uniTfjf*) pars);
    return (fjf->jf)(x);
}

void fjf_aux_uni(double x, void * pars, double * f, double * g) {
    *f = f_aux_uni(x,pars);
    *g = jf_aux_uni(x,pars);
}

int rootj(int method, double f(double),
          double df(double),
         double epsrel, int maxit,
         double x, RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 2,BAD_SIZE);
    DEBUGMSG("root_fjf");
    gsl_function_fdf my_func;
    // extract function from pars
    my_func.f = f_aux_uni;
    my_func.df = jf_aux_uni;
    my_func.fdf = fjf_aux_uni;
    uniTfjf stfjf;
    stfjf.f = f;
    stfjf.jf = df;
    my_func.params = &stfjf;
    size_t iter = 0;
    int status;
    const gsl_root_fdfsolver_type *T;
    gsl_root_fdfsolver *s;
    // Starting point
    switch(method) {
        case 0 : {T = gsl_root_fdfsolver_newton;; break; }
        case 1 : {T = gsl_root_fdfsolver_secant; break; }
        case 2 : {T = gsl_root_fdfsolver_steffenson; break; }
        default: ERROR(BAD_CODE);
    }
    s = gsl_root_fdfsolver_alloc (T);

    gsl_root_fdfsolver_set (s, &my_func, x);

    do {
           double x0;
           status = gsl_root_fdfsolver_iterate (s);
           x0 = x;
           x = gsl_root_fdfsolver_root(s);
           solp[iter*solc+0] = iter+1;
           solp[iter*solc+1] = x;

           iter++;
           if (status)   /* check if solver is stuck */
             break;

           status =
               gsl_root_test_delta (x, x0, 0, epsrel);
        }
        while (status == GSL_CONTINUE && iter < maxit);

    int i;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        solp[i*solc+1]=0.;
    }
    gsl_root_fdfsolver_free(s);
    OK
}

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

typedef void TrawfunV(int, double*, int, double*);

int only_f_aux_multiroot(const gsl_vector*x, void *pars, gsl_vector*y) {
    TrawfunV * f = (TrawfunV*) pars;
    double* p = (double*)calloc(x->size,sizeof(double));
    double* q = (double*)calloc(y->size,sizeof(double));
    int k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }
    f(x->size,p,y->size,q);
    for(k=0;k<y->size;k++) {
        gsl_vector_set(y,k,q[k]);
    }
    free(p);
    free(q);
    return 0; //hmmm
}

int multiroot(int method, void f(int, double*, int, double*),
         double epsabs, int maxit,
         KRVEC(xi), RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 1+2*xin,BAD_SIZE);
    DEBUGMSG("root_only_f");
    gsl_multiroot_function my_func;
    // extract function from pars
    my_func.f = only_f_aux_multiroot;
    my_func.n = xin;
    my_func.params = f;
    size_t iter = 0;
    int status;
    const gsl_multiroot_fsolver_type *T;
    gsl_multiroot_fsolver *s;
    // Starting point
    KDVVIEW(xi);
    switch(method) {
        case 0 : {T = gsl_multiroot_fsolver_hybrids;; break; }
        case 1 : {T = gsl_multiroot_fsolver_hybrid; break; }
        case 2 : {T = gsl_multiroot_fsolver_dnewton; break; }
        case 3 : {T = gsl_multiroot_fsolver_broyden; break; }
        default: ERROR(BAD_CODE);
    }
    s = gsl_multiroot_fsolver_alloc (T, my_func.n);
    gsl_multiroot_fsolver_set (s, &my_func, V(xi));

    do {
           status = gsl_multiroot_fsolver_iterate (s);

           solp[iter*solc+0] = iter+1;

           int k;
           for(k=0;k<xin;k++) {
               solp[iter*solc+k+1] = gsl_vector_get(s->x,k);
           }
           for(k=xin;k<2*xin;k++) {
               solp[iter*solc+k+1] = gsl_vector_get(s->f,k-xin);
           }

           iter++;
           if (status)   /* check if solver is stuck */
             break;

           status =
             gsl_multiroot_test_residual (s->f, epsabs);
        }
        while (status == GSL_CONTINUE && iter < maxit);

    int i,j;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        for(j=1;j<solc;j++) {
            solp[i*solc+j]=0.;
        }
    }
    gsl_multiroot_fsolver_free(s);
    OK
}

// working with the jacobian

typedef struct {int (*f)(int, double*, int, double *);
                int (*jf)(int, double*, int, int, double*);} Tfjf;

int f_aux(const gsl_vector*x, void *pars, gsl_vector*y) {
    Tfjf * fjf = ((Tfjf*) pars);
    double* p = (double*)calloc(x->size,sizeof(double));
    double* q = (double*)calloc(y->size,sizeof(double));
    int k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }
    (fjf->f)(x->size,p,y->size,q);
    for(k=0;k<y->size;k++) {
        gsl_vector_set(y,k,q[k]);
    }
    free(p);
    free(q);
    return 0;
}

int jf_aux(const gsl_vector * x, void * pars, gsl_matrix * jac) {
    Tfjf * fjf = ((Tfjf*) pars);
    double* p = (double*)calloc(x->size,sizeof(double));
    double* q = (double*)calloc((jac->size1)*(jac->size2),sizeof(double));
    int i,j,k;
    for(k=0;k<x->size;k++) {
        p[k] = gsl_vector_get(x,k);
    }

    (fjf->jf)(x->size,p,jac->size1,jac->size2,q);

    k=0;
    for(i=0;i<jac->size1;i++) {
        for(j=0;j<jac->size2;j++){
            gsl_matrix_set(jac,i,j,q[k++]);
        }
    }
    free(p);
    free(q);
    return 0;
}

int fjf_aux(const gsl_vector * x, void * pars, gsl_vector * f, gsl_matrix * g) {
    f_aux(x,pars,f);
    jf_aux(x,pars,g);
    return 0;
}

int multirootj(int method, int f(int, double*, int, double*),
                      int jac(int, double*, int, int, double*),
         double epsabs, int maxit,
         KRVEC(xi), RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 1+2*xin,BAD_SIZE);
    DEBUGMSG("root_fjf");
    gsl_multiroot_function_fdf my_func;
    // extract function from pars
    my_func.f = f_aux;
    my_func.df = jf_aux;
    my_func.fdf = fjf_aux;
    my_func.n = xin;
    Tfjf stfjf;
    stfjf.f = f;
    stfjf.jf = jac;
    my_func.params = &stfjf;
    size_t iter = 0;
    int status;
    const gsl_multiroot_fdfsolver_type *T;
    gsl_multiroot_fdfsolver *s;
    // Starting point
    KDVVIEW(xi);
    switch(method) {
        case 0 : {T = gsl_multiroot_fdfsolver_hybridsj;; break; }
        case 1 : {T = gsl_multiroot_fdfsolver_hybridj; break; }
        case 2 : {T = gsl_multiroot_fdfsolver_newton; break; }
        case 3 : {T = gsl_multiroot_fdfsolver_gnewton; break; }
        default: ERROR(BAD_CODE);
    }
    s = gsl_multiroot_fdfsolver_alloc (T, my_func.n);

    gsl_multiroot_fdfsolver_set (s, &my_func, V(xi));

    do {
           status = gsl_multiroot_fdfsolver_iterate (s);

           solp[iter*solc+0] = iter+1;

           int k;
           for(k=0;k<xin;k++) {
               solp[iter*solc+k+1] = gsl_vector_get(s->x,k);
           }
           for(k=xin;k<2*xin;k++) {
               solp[iter*solc+k+1] = gsl_vector_get(s->f,k-xin);
           }

           iter++;
           if (status)   /* check if solver is stuck */
             break;

           status =
             gsl_multiroot_test_residual (s->f, epsabs);
        }
        while (status == GSL_CONTINUE && iter < maxit);

    int i,j;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        for(j=1;j<solc;j++) {
            solp[i*solc+j]=0.;
        }
    }
    gsl_multiroot_fdfsolver_free(s);
    OK
}

//-------------- non linear least squares fitting -------------------

int nlfit(int method, int f(int, double*, int, double*),
                      int jac(int, double*, int, int, double*),
         double epsabs, double epsrel, int maxit, int p,
         KRVEC(xi), RMAT(sol)) {
    REQUIRES(solr == maxit && solc == 2+xin,BAD_SIZE);
    DEBUGMSG("nlfit");
    const gsl_multifit_fdfsolver_type *T;
    gsl_multifit_fdfsolver *s;
    gsl_multifit_function_fdf my_f;
    // extract function from pars
    my_f.f = f_aux;
    my_f.df = jf_aux;
    my_f.fdf = fjf_aux;
    my_f.n = p;
    my_f.p = xin;  // !!!!
    Tfjf stfjf;
    stfjf.f = f;
    stfjf.jf = jac;
    my_f.params = &stfjf;
    size_t iter = 0;
    int status;

    KDVVIEW(xi);
    //DMVIEW(cov);

    switch(method) {
        case 0 : { T = gsl_multifit_fdfsolver_lmsder; break; }
        case 1 : { T = gsl_multifit_fdfsolver_lmder; break; }
        default: ERROR(BAD_CODE);
    }

    s = gsl_multifit_fdfsolver_alloc (T, my_f.n, my_f.p);
    gsl_multifit_fdfsolver_set (s, &my_f, V(xi));

    do {   status = gsl_multifit_fdfsolver_iterate (s);

           solp[iter*solc+0] = iter+1;
           solp[iter*solc+1] = gsl_blas_dnrm2 (s->f);

           int k;
           for(k=0;k<xin;k++) {
               solp[iter*solc+k+2] = gsl_vector_get(s->x,k);
           }

           iter++;
           if (status)   /* check if solver is stuck */
             break;

           status = gsl_multifit_test_delta (s->dx, s->x, epsabs, epsrel);
        }
        while (status == GSL_CONTINUE && iter < maxit);

    int i,j;
    for (i=iter; i<solr; i++) {
        solp[i*solc+0] = iter;
        for(j=1;j<solc;j++) {
            solp[i*solc+j]=0.;
        }
    }

    //gsl_multifit_covar (s->J, 0.0, M(cov));

    gsl_multifit_fdfsolver_free (s);
    OK
}

//////////////////////////////////////////////////////

#define RAN(C,F) case C: { for(k=0;k<rn;k++) { rp[k]= F(gen); }; OK }

int random_vector(int seed, int code, RVEC(r)) {
    DEBUGMSG("random_vector")
    static gsl_rng * gen = NULL;
    if (!gen) { gen = gsl_rng_alloc (gsl_rng_mt19937);}
    gsl_rng_set (gen, seed);
    int k;
    switch (code) {
        RAN(0,gsl_rng_uniform)
        RAN(1,gsl_ran_ugaussian)
        default: ERROR(BAD_CODE);
    }
}
#undef RAN

//////////////////////////////////////////////////////

#include "gsl-ode.c"

//////////////////////////////////////////////////////


#pragma once

#ifndef _NOGSL
#include <gsl/gsl_vector.h>
#else
#include "FakeGSL.h"
#endif

#include <fstream>
#include <iostream>
#include <vector>
#include <map>
#include <math.h>
#include "FloatSupport.h"
#include "BooleanDAG.h"
#include "Interface.h"
#include "ConflictGenerator.h"
#include "ActualEvaluators.h"
#include "CommandLineArgs.h"
#include "SmoothEvaluators.h"
#include "NumDebugger.h"

using namespace std;

class NumericalSolver {
protected:
	BooleanDAG* dag;
	Interface* interf;

    int ncontrols;
    map<string, int>& ctrls;
    SmoothEvaluators* smoothEval;
    ActualEvaluators* actualEval;
    OptimizationWrapper* opt;

    SimpleEvaluator* seval;

    set<int> assertConstraints;
    int minimizeNode;

    const vector<vector<int>>& dependentInputs;
    const vector<vector<int>>& dependentCtrls;

    gsl_vector* result;
    LocalState* localState;

	NumDebugger* debugger;

public:
    NumericalSolver(BooleanDAG* _dag, map<string, int>& _ctrls, Interface* _interface, SmoothEvaluators* _smoothEval, ActualEvaluators* _actualEval, OptimizationWrapper* _opt, const vector<vector<int>>& _dependentInputs, const vector<vector<int>>& _dependentCtrls, NumDebugger* _debugger);
    ~NumericalSolver(void);

	// Called by the NumericalSolver
    bool checkSAT(int level, gsl_vector* initState = NULL);

    bool checkFullSAT(gsl_vector* state);

    gsl_vector* getResult();
    LocalState* getLocalState();

};


/** @file kjg_fpca.h
 * @brief Runs fastPCA.
 * This module also has methods to multiply a genotype matrix against the GSL
 * matrices.
 */

#ifndef KJG_FPCA_H_
#define KJG_FPCA_H_

#include <gsl/gsl_matrix.h>

extern size_t KJG_FPCA_ROWS;	// number of rows to process at once

/** Performs a fast PCA
 * @param *eval eigenvalues
 * @param *evec eigenvectors
 * @param K number of eigenvalues/vectors
 * @param L width of projection matrix
 * @param I iterations to do exponentiation
 */

void kjg_fpca (size_t K, size_t L, size_t I, double *eval, double *evec);

/** Multiplies B=X*A1 and A2 = XT*B = XT*X*A1
 * @param *A1 some matrix
 * @param *B intermediate matrix
 * @param *A2 next matrix
 */

void kjg_fpca_XTXA (const gsl_matrix * A1, gsl_matrix * B, gsl_matrix * A2);

/** Multiplies B = X*A
 * @param *A some matrix
 * @param *B another matrix
 */

void kjg_fpca_XA (const gsl_matrix * A, gsl_matrix * B);

/** Multiplies A = XT*B
 * @param *B some matrix
 * @param *A another matrix
 */

void kjg_fpca_XTB (const gsl_matrix * B, gsl_matrix * A);

#endif /* KJG_FPCA_H_ */


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:44:36 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -twiddle 16 */

/*
 * This function contains 174 FP additions, 84 FP multiplications,
 * (or, 136 additions, 46 multiplications, 38 fused multiply/add),
 * 50 stack variables, and 64 memory accesses
 */
static const fftw_real K382683432 = FFTW_KONST(+0.382683432365089771728459984030398866761344562);
static const fftw_real K923879532 = FFTW_KONST(+0.923879532511286756128183189396788286822416626);
static const fftw_real K707106781 = FFTW_KONST(+0.707106781186547524400844362104849039284835938);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_twiddle_16(fftw_complex *A, const fftw_complex *W, int iostride, int m, int dist)
{
     int i;
     fftw_complex *inout;
     inout = A;
     for (i = m; i > 0; i = i - 1, inout = inout + dist, W = W + 15) {
	  fftw_real tmp7;
	  fftw_real tmp91;
	  fftw_real tmp180;
	  fftw_real tmp193;
	  fftw_real tmp18;
	  fftw_real tmp194;
	  fftw_real tmp94;
	  fftw_real tmp177;
	  fftw_real tmp77;
	  fftw_real tmp88;
	  fftw_real tmp161;
	  fftw_real tmp128;
	  fftw_real tmp144;
	  fftw_real tmp162;
	  fftw_real tmp163;
	  fftw_real tmp164;
	  fftw_real tmp123;
	  fftw_real tmp143;
	  fftw_real tmp30;
	  fftw_real tmp152;
	  fftw_real tmp100;
	  fftw_real tmp136;
	  fftw_real tmp41;
	  fftw_real tmp153;
	  fftw_real tmp105;
	  fftw_real tmp137;
	  fftw_real tmp54;
	  fftw_real tmp65;
	  fftw_real tmp156;
	  fftw_real tmp117;
	  fftw_real tmp141;
	  fftw_real tmp157;
	  fftw_real tmp158;
	  fftw_real tmp159;
	  fftw_real tmp112;
	  fftw_real tmp140;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp1;
	       fftw_real tmp179;
	       fftw_real tmp6;
	       fftw_real tmp178;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp1 = c_re(inout[0]);
	       tmp179 = c_im(inout[0]);
	       {
		    fftw_real tmp3;
		    fftw_real tmp5;
		    fftw_real tmp2;
		    fftw_real tmp4;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp3 = c_re(inout[8 * iostride]);
		    tmp5 = c_im(inout[8 * iostride]);
		    tmp2 = c_re(W[7]);
		    tmp4 = c_im(W[7]);
		    tmp6 = (tmp2 * tmp3) - (tmp4 * tmp5);
		    tmp178 = (tmp4 * tmp3) + (tmp2 * tmp5);
	       }
	       tmp7 = tmp1 + tmp6;
	       tmp91 = tmp1 - tmp6;
	       tmp180 = tmp178 + tmp179;
	       tmp193 = tmp179 - tmp178;
	  }
	  {
	       fftw_real tmp12;
	       fftw_real tmp92;
	       fftw_real tmp17;
	       fftw_real tmp93;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp9;
		    fftw_real tmp11;
		    fftw_real tmp8;
		    fftw_real tmp10;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp9 = c_re(inout[4 * iostride]);
		    tmp11 = c_im(inout[4 * iostride]);
		    tmp8 = c_re(W[3]);
		    tmp10 = c_im(W[3]);
		    tmp12 = (tmp8 * tmp9) - (tmp10 * tmp11);
		    tmp92 = (tmp10 * tmp9) + (tmp8 * tmp11);
	       }
	       {
		    fftw_real tmp14;
		    fftw_real tmp16;
		    fftw_real tmp13;
		    fftw_real tmp15;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp14 = c_re(inout[12 * iostride]);
		    tmp16 = c_im(inout[12 * iostride]);
		    tmp13 = c_re(W[11]);
		    tmp15 = c_im(W[11]);
		    tmp17 = (tmp13 * tmp14) - (tmp15 * tmp16);
		    tmp93 = (tmp15 * tmp14) + (tmp13 * tmp16);
	       }
	       tmp18 = tmp12 + tmp17;
	       tmp194 = tmp12 - tmp17;
	       tmp94 = tmp92 - tmp93;
	       tmp177 = tmp92 + tmp93;
	  }
	  {
	       fftw_real tmp71;
	       fftw_real tmp124;
	       fftw_real tmp87;
	       fftw_real tmp121;
	       fftw_real tmp76;
	       fftw_real tmp125;
	       fftw_real tmp82;
	       fftw_real tmp120;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp68;
		    fftw_real tmp70;
		    fftw_real tmp67;
		    fftw_real tmp69;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp68 = c_re(inout[15 * iostride]);
		    tmp70 = c_im(inout[15 * iostride]);
		    tmp67 = c_re(W[14]);
		    tmp69 = c_im(W[14]);
		    tmp71 = (tmp67 * tmp68) - (tmp69 * tmp70);
		    tmp124 = (tmp69 * tmp68) + (tmp67 * tmp70);
	       }
	       {
		    fftw_real tmp84;
		    fftw_real tmp86;
		    fftw_real tmp83;
		    fftw_real tmp85;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp84 = c_re(inout[11 * iostride]);
		    tmp86 = c_im(inout[11 * iostride]);
		    tmp83 = c_re(W[10]);
		    tmp85 = c_im(W[10]);
		    tmp87 = (tmp83 * tmp84) - (tmp85 * tmp86);
		    tmp121 = (tmp85 * tmp84) + (tmp83 * tmp86);
	       }
	       {
		    fftw_real tmp73;
		    fftw_real tmp75;
		    fftw_real tmp72;
		    fftw_real tmp74;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp73 = c_re(inout[7 * iostride]);
		    tmp75 = c_im(inout[7 * iostride]);
		    tmp72 = c_re(W[6]);
		    tmp74 = c_im(W[6]);
		    tmp76 = (tmp72 * tmp73) - (tmp74 * tmp75);
		    tmp125 = (tmp74 * tmp73) + (tmp72 * tmp75);
	       }
	       {
		    fftw_real tmp79;
		    fftw_real tmp81;
		    fftw_real tmp78;
		    fftw_real tmp80;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp79 = c_re(inout[3 * iostride]);
		    tmp81 = c_im(inout[3 * iostride]);
		    tmp78 = c_re(W[2]);
		    tmp80 = c_im(W[2]);
		    tmp82 = (tmp78 * tmp79) - (tmp80 * tmp81);
		    tmp120 = (tmp80 * tmp79) + (tmp78 * tmp81);
	       }
	       {
		    fftw_real tmp126;
		    fftw_real tmp127;
		    fftw_real tmp119;
		    fftw_real tmp122;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp77 = tmp71 + tmp76;
		    tmp88 = tmp82 + tmp87;
		    tmp161 = tmp77 - tmp88;
		    tmp126 = tmp124 - tmp125;
		    tmp127 = tmp82 - tmp87;
		    tmp128 = tmp126 + tmp127;
		    tmp144 = tmp126 - tmp127;
		    tmp162 = tmp124 + tmp125;
		    tmp163 = tmp120 + tmp121;
		    tmp164 = tmp162 - tmp163;
		    tmp119 = tmp71 - tmp76;
		    tmp122 = tmp120 - tmp121;
		    tmp123 = tmp119 - tmp122;
		    tmp143 = tmp119 + tmp122;
	       }
	  }
	  {
	       fftw_real tmp24;
	       fftw_real tmp96;
	       fftw_real tmp29;
	       fftw_real tmp97;
	       fftw_real tmp98;
	       fftw_real tmp99;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp21;
		    fftw_real tmp23;
		    fftw_real tmp20;
		    fftw_real tmp22;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp21 = c_re(inout[2 * iostride]);
		    tmp23 = c_im(inout[2 * iostride]);
		    tmp20 = c_re(W[1]);
		    tmp22 = c_im(W[1]);
		    tmp24 = (tmp20 * tmp21) - (tmp22 * tmp23);
		    tmp96 = (tmp22 * tmp21) + (tmp20 * tmp23);
	       }
	       {
		    fftw_real tmp26;
		    fftw_real tmp28;
		    fftw_real tmp25;
		    fftw_real tmp27;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp26 = c_re(inout[10 * iostride]);
		    tmp28 = c_im(inout[10 * iostride]);
		    tmp25 = c_re(W[9]);
		    tmp27 = c_im(W[9]);
		    tmp29 = (tmp25 * tmp26) - (tmp27 * tmp28);
		    tmp97 = (tmp27 * tmp26) + (tmp25 * tmp28);
	       }
	       tmp30 = tmp24 + tmp29;
	       tmp152 = tmp96 + tmp97;
	       tmp98 = tmp96 - tmp97;
	       tmp99 = tmp24 - tmp29;
	       tmp100 = tmp98 - tmp99;
	       tmp136 = tmp99 + tmp98;
	  }
	  {
	       fftw_real tmp35;
	       fftw_real tmp102;
	       fftw_real tmp40;
	       fftw_real tmp103;
	       fftw_real tmp101;
	       fftw_real tmp104;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp32;
		    fftw_real tmp34;
		    fftw_real tmp31;
		    fftw_real tmp33;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp32 = c_re(inout[14 * iostride]);
		    tmp34 = c_im(inout[14 * iostride]);
		    tmp31 = c_re(W[13]);
		    tmp33 = c_im(W[13]);
		    tmp35 = (tmp31 * tmp32) - (tmp33 * tmp34);
		    tmp102 = (tmp33 * tmp32) + (tmp31 * tmp34);
	       }
	       {
		    fftw_real tmp37;
		    fftw_real tmp39;
		    fftw_real tmp36;
		    fftw_real tmp38;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp37 = c_re(inout[6 * iostride]);
		    tmp39 = c_im(inout[6 * iostride]);
		    tmp36 = c_re(W[5]);
		    tmp38 = c_im(W[5]);
		    tmp40 = (tmp36 * tmp37) - (tmp38 * tmp39);
		    tmp103 = (tmp38 * tmp37) + (tmp36 * tmp39);
	       }
	       tmp41 = tmp35 + tmp40;
	       tmp153 = tmp102 + tmp103;
	       tmp101 = tmp35 - tmp40;
	       tmp104 = tmp102 - tmp103;
	       tmp105 = tmp101 + tmp104;
	       tmp137 = tmp101 - tmp104;
	  }
	  {
	       fftw_real tmp48;
	       fftw_real tmp108;
	       fftw_real tmp64;
	       fftw_real tmp115;
	       fftw_real tmp53;
	       fftw_real tmp109;
	       fftw_real tmp59;
	       fftw_real tmp114;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp45;
		    fftw_real tmp47;
		    fftw_real tmp44;
		    fftw_real tmp46;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp45 = c_re(inout[iostride]);
		    tmp47 = c_im(inout[iostride]);
		    tmp44 = c_re(W[0]);
		    tmp46 = c_im(W[0]);
		    tmp48 = (tmp44 * tmp45) - (tmp46 * tmp47);
		    tmp108 = (tmp46 * tmp45) + (tmp44 * tmp47);
	       }
	       {
		    fftw_real tmp61;
		    fftw_real tmp63;
		    fftw_real tmp60;
		    fftw_real tmp62;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp61 = c_re(inout[13 * iostride]);
		    tmp63 = c_im(inout[13 * iostride]);
		    tmp60 = c_re(W[12]);
		    tmp62 = c_im(W[12]);
		    tmp64 = (tmp60 * tmp61) - (tmp62 * tmp63);
		    tmp115 = (tmp62 * tmp61) + (tmp60 * tmp63);
	       }
	       {
		    fftw_real tmp50;
		    fftw_real tmp52;
		    fftw_real tmp49;
		    fftw_real tmp51;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp50 = c_re(inout[9 * iostride]);
		    tmp52 = c_im(inout[9 * iostride]);
		    tmp49 = c_re(W[8]);
		    tmp51 = c_im(W[8]);
		    tmp53 = (tmp49 * tmp50) - (tmp51 * tmp52);
		    tmp109 = (tmp51 * tmp50) + (tmp49 * tmp52);
	       }
	       {
		    fftw_real tmp56;
		    fftw_real tmp58;
		    fftw_real tmp55;
		    fftw_real tmp57;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp56 = c_re(inout[5 * iostride]);
		    tmp58 = c_im(inout[5 * iostride]);
		    tmp55 = c_re(W[4]);
		    tmp57 = c_im(W[4]);
		    tmp59 = (tmp55 * tmp56) - (tmp57 * tmp58);
		    tmp114 = (tmp57 * tmp56) + (tmp55 * tmp58);
	       }
	       {
		    fftw_real tmp113;
		    fftw_real tmp116;
		    fftw_real tmp110;
		    fftw_real tmp111;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp54 = tmp48 + tmp53;
		    tmp65 = tmp59 + tmp64;
		    tmp156 = tmp54 - tmp65;
		    tmp113 = tmp48 - tmp53;
		    tmp116 = tmp114 - tmp115;
		    tmp117 = tmp113 - tmp116;
		    tmp141 = tmp113 + tmp116;
		    tmp157 = tmp108 + tmp109;
		    tmp158 = tmp114 + tmp115;
		    tmp159 = tmp157 - tmp158;
		    tmp110 = tmp108 - tmp109;
		    tmp111 = tmp59 - tmp64;
		    tmp112 = tmp110 + tmp111;
		    tmp140 = tmp110 - tmp111;
	       }
	  }
	  {
	       fftw_real tmp107;
	       fftw_real tmp131;
	       fftw_real tmp202;
	       fftw_real tmp204;
	       fftw_real tmp130;
	       fftw_real tmp203;
	       fftw_real tmp134;
	       fftw_real tmp199;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp95;
		    fftw_real tmp106;
		    fftw_real tmp200;
		    fftw_real tmp201;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp95 = tmp91 - tmp94;
		    tmp106 = K707106781 * (tmp100 - tmp105);
		    tmp107 = tmp95 + tmp106;
		    tmp131 = tmp95 - tmp106;
		    tmp200 = K707106781 * (tmp137 - tmp136);
		    tmp201 = tmp194 + tmp193;
		    tmp202 = tmp200 + tmp201;
		    tmp204 = tmp201 - tmp200;
	       }
	       {
		    fftw_real tmp118;
		    fftw_real tmp129;
		    fftw_real tmp132;
		    fftw_real tmp133;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp118 = (K923879532 * tmp112) + (K382683432 * tmp117);
		    tmp129 = (K382683432 * tmp123) - (K923879532 * tmp128);
		    tmp130 = tmp118 + tmp129;
		    tmp203 = tmp129 - tmp118;
		    tmp132 = (K382683432 * tmp112) - (K923879532 * tmp117);
		    tmp133 = (K382683432 * tmp128) + (K923879532 * tmp123);
		    tmp134 = tmp132 - tmp133;
		    tmp199 = tmp132 + tmp133;
	       }
	       c_re(inout[11 * iostride]) = tmp107 - tmp130;
	       c_re(inout[3 * iostride]) = tmp107 + tmp130;
	       c_re(inout[15 * iostride]) = tmp131 - tmp134;
	       c_re(inout[7 * iostride]) = tmp131 + tmp134;
	       c_im(inout[3 * iostride]) = tmp199 + tmp202;
	       c_im(inout[11 * iostride]) = tmp202 - tmp199;
	       c_im(inout[7 * iostride]) = tmp203 + tmp204;
	       c_im(inout[15 * iostride]) = tmp204 - tmp203;
	  }
	  {
	       fftw_real tmp139;
	       fftw_real tmp147;
	       fftw_real tmp196;
	       fftw_real tmp198;
	       fftw_real tmp146;
	       fftw_real tmp197;
	       fftw_real tmp150;
	       fftw_real tmp191;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp135;
		    fftw_real tmp138;
		    fftw_real tmp192;
		    fftw_real tmp195;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp135 = tmp91 + tmp94;
		    tmp138 = K707106781 * (tmp136 + tmp137);
		    tmp139 = tmp135 + tmp138;
		    tmp147 = tmp135 - tmp138;
		    tmp192 = K707106781 * (tmp100 + tmp105);
		    tmp195 = tmp193 - tmp194;
		    tmp196 = tmp192 + tmp195;
		    tmp198 = tmp195 - tmp192;
	       }
	       {
		    fftw_real tmp142;
		    fftw_real tmp145;
		    fftw_real tmp148;
		    fftw_real tmp149;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp142 = (K382683432 * tmp140) + (K923879532 * tmp141);
		    tmp145 = (K923879532 * tmp143) - (K382683432 * tmp144);
		    tmp146 = tmp142 + tmp145;
		    tmp197 = tmp145 - tmp142;
		    tmp148 = (K923879532 * tmp140) - (K382683432 * tmp141);
		    tmp149 = (K923879532 * tmp144) + (K382683432 * tmp143);
		    tmp150 = tmp148 - tmp149;
		    tmp191 = tmp148 + tmp149;
	       }
	       c_re(inout[9 * iostride]) = tmp139 - tmp146;
	       c_re(inout[iostride]) = tmp139 + tmp146;
	       c_re(inout[13 * iostride]) = tmp147 - tmp150;
	       c_re(inout[5 * iostride]) = tmp147 + tmp150;
	       c_im(inout[iostride]) = tmp191 + tmp196;
	       c_im(inout[9 * iostride]) = tmp196 - tmp191;
	       c_im(inout[5 * iostride]) = tmp197 + tmp198;
	       c_im(inout[13 * iostride]) = tmp198 - tmp197;
	  }
	  {
	       fftw_real tmp155;
	       fftw_real tmp167;
	       fftw_real tmp188;
	       fftw_real tmp190;
	       fftw_real tmp166;
	       fftw_real tmp189;
	       fftw_real tmp170;
	       fftw_real tmp185;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp151;
		    fftw_real tmp154;
		    fftw_real tmp186;
		    fftw_real tmp187;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp151 = tmp7 - tmp18;
		    tmp154 = tmp152 - tmp153;
		    tmp155 = tmp151 + tmp154;
		    tmp167 = tmp151 - tmp154;
		    tmp186 = tmp41 - tmp30;
		    tmp187 = tmp180 - tmp177;
		    tmp188 = tmp186 + tmp187;
		    tmp190 = tmp187 - tmp186;
	       }
	       {
		    fftw_real tmp160;
		    fftw_real tmp165;
		    fftw_real tmp168;
		    fftw_real tmp169;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp160 = tmp156 + tmp159;
		    tmp165 = tmp161 - tmp164;
		    tmp166 = K707106781 * (tmp160 + tmp165);
		    tmp189 = K707106781 * (tmp165 - tmp160);
		    tmp168 = tmp159 - tmp156;
		    tmp169 = tmp161 + tmp164;
		    tmp170 = K707106781 * (tmp168 - tmp169);
		    tmp185 = K707106781 * (tmp168 + tmp169);
	       }
	       c_re(inout[10 * iostride]) = tmp155 - tmp166;
	       c_re(inout[2 * iostride]) = tmp155 + tmp166;
	       c_re(inout[14 * iostride]) = tmp167 - tmp170;
	       c_re(inout[6 * iostride]) = tmp167 + tmp170;
	       c_im(inout[2 * iostride]) = tmp185 + tmp188;
	       c_im(inout[10 * iostride]) = tmp188 - tmp185;
	       c_im(inout[6 * iostride]) = tmp189 + tmp190;
	       c_im(inout[14 * iostride]) = tmp190 - tmp189;
	  }
	  {
	       fftw_real tmp43;
	       fftw_real tmp171;
	       fftw_real tmp182;
	       fftw_real tmp184;
	       fftw_real tmp90;
	       fftw_real tmp183;
	       fftw_real tmp174;
	       fftw_real tmp175;
	       ASSERT_ALIGNED_DOUBLE;
	       {
		    fftw_real tmp19;
		    fftw_real tmp42;
		    fftw_real tmp176;
		    fftw_real tmp181;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp19 = tmp7 + tmp18;
		    tmp42 = tmp30 + tmp41;
		    tmp43 = tmp19 + tmp42;
		    tmp171 = tmp19 - tmp42;
		    tmp176 = tmp152 + tmp153;
		    tmp181 = tmp177 + tmp180;
		    tmp182 = tmp176 + tmp181;
		    tmp184 = tmp181 - tmp176;
	       }
	       {
		    fftw_real tmp66;
		    fftw_real tmp89;
		    fftw_real tmp172;
		    fftw_real tmp173;
		    ASSERT_ALIGNED_DOUBLE;
		    tmp66 = tmp54 + tmp65;
		    tmp89 = tmp77 + tmp88;
		    tmp90 = tmp66 + tmp89;
		    tmp183 = tmp89 - tmp66;
		    tmp172 = tmp157 + tmp158;
		    tmp173 = tmp162 + tmp163;
		    tmp174 = tmp172 - tmp173;
		    tmp175 = tmp172 + tmp173;
	       }
	       c_re(inout[8 * iostride]) = tmp43 - tmp90;
	       c_re(inout[0]) = tmp43 + tmp90;
	       c_re(inout[12 * iostride]) = tmp171 - tmp174;
	       c_re(inout[4 * iostride]) = tmp171 + tmp174;
	       c_im(inout[0]) = tmp175 + tmp182;
	       c_im(inout[8 * iostride]) = tmp182 - tmp175;
	       c_im(inout[4 * iostride]) = tmp183 + tmp184;
	       c_im(inout[12 * iostride]) = tmp184 - tmp183;
	  }
     }
}

static const int twiddle_order[] =
{1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};
fftw_codelet_desc fftw_twiddle_16_desc =
{
     "fftw_twiddle_16",
     (void (*)()) fftw_twiddle_16,
     16,
     FFTW_FORWARD,
     FFTW_TWIDDLE,
     352,
     15,
     twiddle_order,
};


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

int main(void)
{
  int i, j;
  gsl_matrix * m = gsl_matrix_alloc (10,3);

  for (i=0; i<10; i++)
    for (j=0; j<3; j++)
      gsl_matrix_set (m, i, j, 0.23 + 100*i + j);

  for (i=0; i< 10; i++) /* OUT OF RANGE ERROR */
    for (j=0; j<3; j++)
      printf("m(%d,%d) = %g\n", i, j,
              gsl_matrix_get (m, i, j));

  gsl_matrix_free(m);

  return 0;
}


/*
 * File:   Random.h
 * Author: nguyentran
 *
 * Created on May 27, 2013, 10:46 AM
 */

#ifndef RANDOM_H
#define    RANDOM_H

#include <gsl/gsl_rng.h>
#include "PropertyMacro.h"
#include "Strategies/AdaptiveCyclingStrategy.h"

class Model;

class Random {
 DISALLOW_COPY_AND_ASSIGN(Random)

 DISALLOW_MOVE(Random)

 VIRTUAL_PROPERTY(unsigned long, seed)

 public:
  gsl_rng *G_RNG;

  explicit Random(gsl_rng *g_rng = nullptr);

  virtual ~Random();

  void initialize(const unsigned long &seed = 0);

  void release() const;

  virtual int random_poisson(const double &poisson_mean);

  virtual unsigned long random_uniform(unsigned long range);

  virtual unsigned long random_uniform_int(const unsigned long &from, const unsigned long &to);

  virtual double random_uniform_double(const double &from, const double &to);

  /*
   * This function will return a random number in [0,1)
   */
  virtual double random_uniform();

  virtual double random_normal(const double &mean, const double &sd);

  virtual double random_normal_truncated(const double &mean, const double &sd);

  virtual int random_normal(const int &mean, const int &sd);

  virtual int random_normal_truncated(const int &mean, const int &sd);

  virtual double random_beta(const double &alpha, const double &beta);

  virtual double random_gamma(const double &shape, const double &scale);

  virtual double cdf_gamma_distribution(const double &x, const double &alpha, const double &beta);

  virtual double cdf_gamma_distribution_inverse(const double &p, const double &alpha, const double &beta);

  virtual double random_flat(const double &from, const double &to);

  virtual void random_multinomial(const size_t &K, const unsigned &N, double p[], unsigned n[]);

  virtual void random_shuffle(void *base, size_t base_length, size_t size_of_type);

  virtual double cdf_standard_normal_distribution(const double &p);

  virtual int random_binomial(const double &p, const unsigned int &n);

  void shuffle(void *base, const size_t &n, const size_t &size);
};

#endif    /* RANDOM_H */