/* $Header$ */

/* Purpose: Description (definition) of spherical geometry functions */

/* This file includes BSD-licensed code whose copyright is held by another author
   The copyright owner and license terms for the NCO modifications to that code are
   Copyright (C) 2018--present Charlie Zender
   This file is part of NCO, the netCDF Operators. NCO is free software.
   You may redistribute and/or modify NCO under the terms of the
   3-Clause BSD License with exceptions described in the LICENSE file */

/* This copyright statement and license terms for modification and redistribution
   of the original code were agreed to by the original author, Joseph O'Rourke, on 20190517:

   This code is described in "Computational Geometry in C" (Second Edition),
   Chapter 7.  It is not written to be comprehensible without the
   explanation in that book.

   Written by Joseph O'Rourke.
   Last modified: December 1997
   Questions to jorourke@smith.edu.
   -------------------------------------------------------------------------
   Copyright 1997 by Joseph O'Rourke <jorourke@smith.edu>

   Redistribution and use in source and binary forms, with or without
   modification, are permitted provided that the following conditions are
   met:

   1. Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.

   2. Redistributions in binary form must reproduce the above copyright
   notice, this list of conditions and the following disclaimer in the
   documentation and/or other materials provided with the distribution.

   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
   TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
   PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER
   OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
   EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
   PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
   PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
   LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
   NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
   SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
   ------------------------------------------------------------------------- */

/* Usage:
   #include "nco_sph.h" *//* Spherical geometry intersections */

#ifndef NCO_SPH_H /* Contents have not yet been inserted in current source file */
#define NCO_SPH_H

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

#ifdef HAVE_CONFIG_H
# include <config.h> /* Autotools tokens */
#endif /* !HAVE_CONFIG_H */

#ifdef ENABLE_GSL
  #include <gsl/gsl_vector.h>
  #include <gsl/gsl_matrix.h>
  #include <gsl/gsl_permutation.h>
  #include <gsl/gsl_linalg.h>
#endif

/* Personal headers */
#include "nco.h" /* netCDF Operator (NCO) definitions */
#include "nco_mmr.h"     /* Memory management */
#include "nco_omp.h"     /* OpenMP utilities */
#include "nco_rgr.h"     /* Regridding */
#include "nco_sld.h"     /* Swath-Like Data */
#include "nco_sng_utl.h" /* String utilities */

#include "nco_crt.h" /* Cartesian geometry intersections */
#include "nco_ply.h" /* Polygon structure & utilities */

#define NBR_SPH (5)
#define NBR_RLL (5)

#define VP_MAX    1000            /* Max # of pts in polygon */

/* this is 1.0e-20 * PI / 180.0 */
#define ARC_MIN_LENGTH_RAD (1.0e-15)

/* smallest RADIAN */
#define SIGMA_RAD (1.0e-12)

#define SIGMA_TOLERANCE (1.0e-16)
#define DOT_TOLERANCE (1.0e-14)

/* this value plays nice with edges on grids/ne120np4_pentagons.100310.nc */
#define DIST_TOLERANCE (1.0e-14)

/* convert Degrees to Radians */
#define D2R(x)  ((x) * M_PI / 180.0)
/* convert Radians to degrees */
#define R2D(x)  ((x) * 180.0 / M_PI)

/* if true then longitude 0-360 */
/* we need this to convert 3D back to 2D */
#define IS_LON_360 (1)

// #define DEBUG_SPH (1)

#ifdef __cplusplus
/* Use C-bindings so C++-compiled and C-compiled libraries are compatible */
extern "C" {
#endif /* !__cplusplus */

/*---------------------------------------------------------------------
Structures
---------------------------------------------------------------------*/

/* vertex info  */
typedef struct {
  poly_vrl_flg_enm  in_flag;  /* if P_in - vertex from a, Q_in Vertex from b */
  int p_vrt;                  /* if -1 then genuine intersection */
  int q_vrt;
  double p0[NBR_SPH];           /* actual point - used for debuggging */
}vrt_info_sct;

/*---------------------------------------------------------------------
Function prototypes.
---------------------------------------------------------------------*/

int
nco_sph_intersect(poly_sct *P, poly_sct *Q, poly_sct *R, int *r, int flg_snp_to, const char *pq_pre);

char
nco_sph_seg_int_old(double *a, double *b, double *c, double *d, double *p, double *q);

nco_bool
nco_sph_seg_int(double *p0, double *p1, double *q0, double *q1, double *r0, double *r1, int *pq_cross, int flg_snp_to, char *codes);

int
nco_sph_mk_pqcross( double *p0, double *p1, double *pCross, double *q0, double  *q1, double *qCross, int pqCross[], nco_edg_typ_enm rgr_edg_typ,   nco_edg_typ_enm *p_edg_typ, nco_edg_typ_enm *q_edg_typ);

nco_bool
nco_sph_seg_parallel(double *p0, double *p1, double *q0, double *q1, double *r0, double *r1, poly_vrl_flg_enm *inflag, char *codes );

nco_bool
nco_sph_seg_smc(double *p0, double *p1, double *q0, double *q1, double *r0, double *r1, int *pq_cross, int flg_snp_to, char *codes);

nco_bool
nco_sph_seg_vrt_int(double *a, double *b, double *c);

int
nco_sph_lhs(double *Pi, double *Qi);

nco_bool
nco_sph_face(int iLHS, int iRHS, int jRHS);

double
nco_sph_dot(double *a, double *b);

double
nco_sph_dot_nm(double *a, double *b);

double
nco_sph_cross(double *a, double *b, double *c);

double
nco_sph_cross_sub(double *a, double *b, double *c);

double
nco_sph_cross2(double *a, double *b, double *c);

double
nco_sph_trp(double *a, double *b, double *c);

void
nco_sph_add(double *a, double *b, double *c);

void
nco_sph_sub(double *a, double *b, double *c);

void
nco_sph_mlt(double *a, double m);

double
nco_sph_dist(double *a, double *b);

double
nco_sph_rad(double *a);

double
nco_sph_rad2(double *a);

double
nco_sph_sxcross(double *a, double *b, double *c);

void
nco_sph_adi(double *a, double *b);

void
nco_sph_add_pnt(double **R, int *r, double *P);

bool
nco_sph_vrt_info_cmp( vrt_info_sct *info_a, vrt_info_sct *info_b);

void
nco_sph_add_pnt_chk( vrt_info_sct *vrt_info, poly_vrl_flg_enm inflag, int p_vrt, int q_vrt,  double **R, int *r, double *P);

nco_bool
nco_sph_between(double a, double b, double x);

void
nco_sph_prn_pnt(const char *sMsg, double *p, int style, nco_bool bRet);

nco_bool
nco_sph_is_convex(double **sP, int np);

void
nco_sph_prn(double **sR, int r, int istyle);

int
nco_sph_pnt_in_poly(double **sP, int n, double *pControl, double *pVertex);

nco_bool
nco_sph_poly_in_poly(poly_sct *sP,poly_sct *sQ);

void
nco_sph_set_domain(double lon_min_rad, double lon_max_rad, double lat_min_rad, double lat_max_rad);

void
nco_sph_add_lonlat(double *ds);

int
nco_sph_mk_control(poly_sct *sP, nco_bool bInside,  double* pControl  ); /* make a control point that is outside polygon */

nco_bool
nco_sph_intersect_pre(poly_sct *sP,poly_sct *sQ, char *sq_sng  );

int
nco_sph_process_pre(poly_sct *sQ, char *sq_sng, nco_bool *bGenuine);

void
nco_sph_centroid_mk(poly_sct *sP, double *pControl);

nco_bool
nco_sph_inside_mk(poly_sct *sP, double *pControl);

nco_bool
nco_sph_metric( double *p, double *q);

int
nco_sph_metric_int(double *c, double *d, double *Icross);

double
nco_sph_area_karney( double **sP, int np);

double
nco_sph_area_quadrature(double  **sP, int np);

/***************** nco_geo functions these manimpulate lat & lon  ***************************/
void
nco_geo_sph_2_lonlat(double *a, double *lon, double *lat, nco_bool bDeg);

void
nco_geo_lonlat_2_sph(double lon, double lat, double *b, nco_bool bSimple, nco_bool bDeg);

double
nco_geo_lat_correct(double lat1, double lon1, double lon2);

void
nco_geo_get_lat_correct(double lon1, double lat1, double lon2, double lat2, double *dp_min, double *dp_max, nco_bool bDeg);

/**************** functions for RLL grids ***************************************************/
int
nco_rll_intersect(poly_sct *P, poly_sct *Q, poly_sct *R, int *r);

char
nco_rll_seg_int(double *a, double *b, double *c, double *d, double *p, double *q);

nco_bool
nco_rll_seg_parallel(double *p0, double *p1, double *q0, double *q1, double *r0, double *r1, poly_vrl_flg_enm *inflag, char *codes );

void
nco_rll_area(poly_sct *pl);

nco_bool
nco_rll_is_lat_circle(double *p0, double *p1);

int
nco_rll_lhs(double *p0, double *QCross);

int
nco_rll_lhs_lat(double *p0, double *q0, double *q1);

void
nco_rll_add_pnt(double **R, int *r, double *P);

/*********************** functions for matrix*****************************************************/
void
nco_mat_mlt
(double mat[], double vec[], double vec_out[]);

nco_bool
nco_mat_inv(double *mat, double *mat_inv);

nco_bool
nco_mat_int_pl(const double *p0, const double *p1, const double *q0, const double *q1, double *r0);

#ifdef __cplusplus
} /* end extern "C" */
#endif /* !__cplusplus */

#endif /* NCO_SPH_H */


/***************************************************************************
 *   Copyright (C) 2007 by Mikhail Zaslavskiy   *
 *      *
 *                                                                         *
 *   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.             *
 ***************************************************************************/
#ifndef GRAPH_H
#define GRAPH_H

#include <iostream>
#include "rpc.h"
#include <math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <vector>

/**
Graph class, it implements graph matching algorithms

	@author Mikhail Zaslavskiy
*/
class graph : public rpc
{
public:
    //class constructor
    graph(std::string fconfig="config.txt");
    graph(graph & _graph);
    const graph & operator =(graph &);

    //load graph routine
    int load_graph(std::string fgraph,char ftype='A',char cformat='D',std::string fvertexlst_name="");
    //adjacency matrix control
    const gsl_matrix*  get_adjmatrix(){return gm_A;};
    int set_adjmatrix(const gsl_matrix* _gm_A);
    gsl_matrix* get_descmatrix(char dmt);
    int printout(std::string fname_out);
    void printdot(std::string fname_out, gsl_matrix* =NULL);
    ~graph();
    int add_dummy_nodes(int id);
    long long getN();
protected:
    gsl_matrix* gm_A;//adjacency matrix
    long long N;
};
double gsl_matrix_norm(const gsl_matrix* gm,double p);
int gsl_matrix_abs(gsl_matrix* gm);
double gsl_matrix_min(gsl_matrix* gm_A,double ic);//minimum greater than ic
double gsl_vector_sum(gsl_vector* gv);
int gsl_matrix_printout(const gsl_matrix * gm,std::string,std::string);
int gsl_matrix_printout(gsl_vector * gv,std::string,std::string);
int gsl_matrix_printout(gsl_permutation * gv,std::string,std::string);
int gsl_matrix_printout(std::string sout,std::string );
double abs(double);
void gsl_matrix_sum(gsl_matrix* A,int idim,gsl_vector* gv_res);
double gsl_matrix_sum(gsl_matrix* A);
double gsl_matrix_max_abs(gsl_matrix* A);
double gsl_matrix_min_abs(gsl_matrix* gm);
//global min in [0,1]^2 of a quadratic function
void qglob_min(double a_11,double a_12,double a_22,double a_1,double a_2,double &dalpha_1,double &dalpha_2);
#endif


/* ode-initval/rkck.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Runge-Kutta 4(5), Cash-Karp */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>

#include "odeiv_util.h"

/* Cash-Karp constants */
static const double ah[] = { 1.0 / 5.0, 0.3, 3.0 / 5.0, 1.0, 7.0 / 8.0 };
static const double b21 = 1.0 / 5.0;
static const double b3[] = { 3.0 / 40.0, 9.0 / 40.0 };
static const double b4[] = { 0.3, -0.9, 1.2 };
static const double b5[] = { -11.0 / 54.0, 2.5, -70.0 / 27.0, 35.0 / 27.0 };
static const double b6[] =
  { 1631.0 / 55296.0, 175.0 / 512.0, 575.0 / 13824.0, 44275.0 / 110592.0,
    253.0 / 4096.0 };
static const double c1 = 37.0 / 378.0;
static const double c3 = 250.0 / 621.0;
static const double c4 = 125.0 / 594.0;
static const double c6 = 512.0 / 1771.0;
static const double ec[] = { 0.0,
  /* the first value is the same as c1, above */
  37.0 / 378.0 - 2825.0 / 27648.0,
  0.0,
  /* the first value is the same as c3, above */
  250.0 / 621.0 - 18575.0 / 48384.0,
  /* the first value is the same as c4, above */
  125.0 / 594.0 - 13525.0 / 55296.0,
  -277.00 / 14336.0,
  /* the first value is the same as c6, above */
  512.0 / 1771.0 - 0.25
};

typedef struct
{
  double *k1;
  double *k2;
  double *k3;
  double *k4;
  double *k5;
  double *k6;
  double *y0;
  double *ytmp;
}
rkck_state_t;

static void *
rkck_alloc (size_t dim)
{
  rkck_state_t *state = (rkck_state_t *) malloc (sizeof (rkck_state_t));

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

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

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

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

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

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

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

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

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

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

  if (state->k5 == 0)
    {
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k5", GSL_ENOMEM);
    }

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

  if (state->k6 == 0)
    {
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for k6", GSL_ENOMEM);
    }

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

  if (state->y0 == 0)
    {
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

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

  if (state->ytmp == 0)
    {
      free (state->y0);
      free (state->k6);
      free (state->k5);
      free (state->k4);
      free (state->k3);
      free (state->k2);
      free (state->k1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  return state;
}

static int
rkck_apply (void *vstate,
            size_t dim,
            double t,
            double h,
            double y[],
            double yerr[],
            const double dydt_in[],
            double dydt_out[], const gsl_odeiv_system * sys)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  size_t i;
  int status = 0;

  double *const k1 = state->k1;
  double *const k2 = state->k2;
  double *const k3 = state->k3;
  double *const k4 = state->k4;
  double *const k5 = state->k5;
  double *const k6 = state->k6;
  double *const ytmp = state->ytmp;

  /* k1 step */
  if (dydt_in != NULL)
    {
      DBL_MEMCPY (k1, dydt_in, dim);
    }
  else
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t, y, k1);
      GSL_STATUS_UPDATE (&status, s);
    }

  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + b21 * h * k1[i];

  /* k2 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[0] * h, ytmp, k2);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b3[0] * k1[i] + b3[1] * k2[i]);

  /* k3 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[1] * h, ytmp, k3);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] = y[i] + h * (b4[0] * k1[i] + b4[1] * k2[i] + b4[2] * k3[i]);

  /* k4 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[2] * h, ytmp, k4);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b5[0] * k1[i] + b5[1] * k2[i] + b5[2] * k3[i] +
                  b5[3] * k4[i]);

  /* k5 step */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[3] * h, ytmp, k5);
    GSL_STATUS_UPDATE (&status, s);
  }
  for (i = 0; i < dim; i++)
    ytmp[i] =
      y[i] + h * (b6[0] * k1[i] + b6[1] * k2[i] + b6[2] * k3[i] +
                  b6[3] * k4[i] + b6[4] * k5[i]);

  /* k6 step and final sum */
  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + ah[4] * h, ytmp, k6);
    GSL_STATUS_UPDATE (&status, s);
  }

  for (i = 0; i < dim; i++)
    {
      const double d_i = c1 * k1[i] + c3 * k3[i] + c4 * k4[i] + c6 * k6[i];
      y[i] += h * d_i;
      if (dydt_out != NULL)
        dydt_out[i] = d_i;
    }

  /* difference between 4th and 5th order */
  for (i = 0; i < dim; i++)
    yerr[i] =
      h * (ec[1] * k1[i] + ec[3] * k3[i] + ec[4] * k4[i] + ec[5] * k5[i] +
           ec[6] * k6[i]);

  return status;
}

static int
rkck_reset (void *vstate, size_t dim)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  DBL_ZERO_MEMSET (state->k1, dim);
  DBL_ZERO_MEMSET (state->k2, dim);
  DBL_ZERO_MEMSET (state->k3, dim);
  DBL_ZERO_MEMSET (state->k4, dim);
  DBL_ZERO_MEMSET (state->k5, dim);
  DBL_ZERO_MEMSET (state->k6, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);

  return GSL_SUCCESS;
}

static unsigned int
rkck_order (void *vstate)
{
  rkck_state_t *state = (rkck_state_t *) vstate;
  state = 0; /* prevent warnings about unused parameters */
  return 5;
}

static void
rkck_free (void *vstate)
{
  rkck_state_t *state = (rkck_state_t *) vstate;

  free (state->ytmp);
  free (state->y0);
  free (state->k6);
  free (state->k5);
  free (state->k4);
  free (state->k3);
  free (state->k2);
  free (state->k1);
  free (state);
}

static const gsl_odeiv_step_type rkck_type = { "rkck",  /* name */
  1,                            /* can use dydt_in */
  0,                            /* gives exact dydt_out */
  &rkck_alloc,
  &rkck_apply,
  &rkck_reset,
  &rkck_order,
  &rkck_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rkck = &rkck_type;


#pragma once

#include <random>
#include <type_traits>

#include <gsl-lite/gsl-lite.hpp>

namespace thrustshift {

namespace random {

template <class Range, class Distribution, class Engine>
void generate(Range&& range, Distribution& dist, Engine& rng) {
	for (size_t i = 0; i < range.size(); ++i) {
		range[i] = dist(rng);
	}
}

template <class Range,
          typename T = typename std::remove_reference<Range>::type::value_type>
void generate_uniform_real(Range&& range,
                           T min = T{0},
                           T max = T{1},
                           unsigned long long seed = 0) {
	gsl_Expects(min <= max);
	std::default_random_engine rng(seed);
	std::uniform_real_distribution<T> dist(min, max);
	generate(std::forward<Range>(range), dist, rng);
}

template <class Range,
          typename T = typename std::remove_reference<
              Range>::type::value_type::value_type>
void generate_uniform_complex(Range&& range,
                              T min = T{0},
                              T max = T{1},
                              unsigned long long seed = 0) {
	gsl_Expects(min <= max);
	using Complex = typename std::remove_reference<Range>::type::value_type;
	std::default_random_engine rng(seed);
	std::uniform_real_distribution<T> dist(min, max);
	auto cdist = [&](auto& rng) -> Complex { return {dist(rng), dist(rng)}; };
	generate(std::forward<Range>(range), cdist, rng);
}

template <class Range,
          typename T = typename std::remove_reference<Range>::type::value_type>
void generate_normal_real(Range&& range,
                          T mean = T{0},
                          T stddev = T{1},
                          unsigned long long seed = 0) {
	gsl_Expects(stddev >= 0);
	std::default_random_engine rng(seed);
	std::normal_distribution<T> dist(mean, stddev);
	generate(std::forward<Range>(range), dist, rng);
}

template <class Range,
          typename T = typename std::remove_reference<
              Range>::type::value_type::value_type>
void generate_normal_complex(Range&& range,
                             T mean = T{0},
                             T stddev = T{1},
                             unsigned long long seed = 0) {
	gsl_Expects(stddev >= 0);
	using Complex = typename std::remove_reference<Range>::type::value_type;
	std::default_random_engine rng(seed);
	std::normal_distribution<T> dist(mean, stddev);
	auto cdist = [&](auto& rng) -> Complex { return {dist(rng), dist(rng)}; };
	generate(std::forward<Range>(range), cdist, rng);
}

} // namespace random

} // namespace thrustshift


/*
 * Copyright (c) 2018-2021 Aleksas Mazeliauskas, Stefan Floerchinger,
 *                    Eduardo Grossi, and Derek Teaney
 * All rights reserved.
 *
 * FastReso is distributed under MIT license;
 * see the LICENSE file that should be present in the root
 * of the source distribution, or alternately available at:
 * https://github.com/amazeliauskas/FastReso/
 */
#ifndef FASTRESO_TFastReso_formulas_h
#define FASTRESO_TFastReso_formulas_h
#include <gsl/gsl_integration.h>
#include "TParticle.h"
#include <vector>
// List of parameters for GSL integration procedure.
#include "qag_params.h"
#include "grid_params.h"

#include <gsl/gsl_errno.h>
//using namespace std;
//! Eabc -- energy of b in restframe of a in a->b+c decay
double get_Eabc(double ma,double mb,double mc) { return (ma*ma+mb*mb-mc*mc)/(2*ma);}
//! particle momenta
double get_p(double E, double m) { return sqrt(E*E-m*m);}
//! pabc -- momentum of b or c in restframe of a in a->b+c decay
double get_pabc(double ma,double mb,double mc) { return get_p(get_Eabc(ma,mb,mc),mb);}

//! returns thermal Bose-Einstein, Fermi-Dirac or Boltzmann distribution
double get_thermal_F(double Ebar, double T, double QMu, EParticleType type);
//! returns derivative of thermal Bose-Einstein, Fermi-Dirac or Boltzmann distribution
double get_thermal_dF(double Ebar, double T, double QMu, EParticleType type);

//! Initialize f_j on the freeze-out surface
//! Here you can add additional fj's
double get_initial_Fj(EFjIndex j, double Ebar, double T, double QMu, EParticleType type, double m, double Cs2)
{
  double pbar = sqrt(Ebar*Ebar-m*m);
  switch(j){
    case EFjIndex::kFeq1: //feq 1
      return pbar*get_thermal_F(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFeq2: //feq 2
      return pbar*get_thermal_F(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear1: //fshear 1
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear2: //fshear 2
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFshear3: //fshear 3
      return pbar*pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFbulk1: //fbulk 1
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*(1./3.*m*m/T/Ebar-Ebar/T*(1./3.-Cs2));
      break;
    case EFjIndex::kFbulk2: //fbulk 2
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*(1./3.*m*m/T/Ebar-Ebar/T*(1./3.-Cs2));
      break;
    case EFjIndex::kFtemp1: //ftemperature. 1
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*Ebar/T;
      break;
    case EFjIndex::kFtemp2: //ftemperature 2
      return pbar*get_thermal_dF(Ebar,T, QMu, type)*Ebar/T;
      break;
    case EFjIndex::kFvel1: //fvelocity  1
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFvel2: //fvelocity 2
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    case EFjIndex::kFvel3: //fvelocity 3
      return pbar*pbar*get_thermal_dF(Ebar,T, QMu, type);
      break;
    default:
      std::cerr  << "\033[1mTFastReso.cpp::get_initial_Fj\033[0m : \033[1;31merror\033[0m :  wrong index "  << (int) j << std::endl;
      exit(EXIT_FAILURE);
  }
}
//! Transformation rule factor A for Fj in a 2-body decay
//! Here you can add additional rules for fj's
double get_factor_Aj(EFjIndex j, double Qw, double Ew, double Ebar, double pbar, double Ma, double pw)
{

//  double A= (Ma*Eabc/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar/pbar);
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);

  switch( j){
    case EFjIndex::kFeq1: // feq 1
      return Qw/pw;
      break;
    case EFjIndex::kFeq2: // feq 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFshear1: // fshear 1
      return Qw/pw*(2.5*Qw/pw*Qw/pw-1.5);
      break;
    case EFjIndex::kFshear2: // fshear 2
      return Qw/pw;
      break;
    case EFjIndex::kFshear3: // fshear 3
      return (1.5*Qw/pw*Qw/pw-0.5)*Ew/Ebar*pbar/pw;
      break;
    case EFjIndex::kFbulk1: // fbulk 1
      return Qw/pw;
      break;
    case EFjIndex::kFbulk2: // fbulk 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFtemp1: // ftemperature 1
      return Qw/pw;
      break;
    case EFjIndex::kFtemp2: // ftemperature 2
      return pbar/pw*Ew/Ebar;
      break;
    case EFjIndex::kFvel1: // fvelocity 1
      return 1.5*Qw/pw*Qw/pw-0.5;
      break;
    case EFjIndex::kFvel2: // fvelocity 2
      return 1;
      break;
    case EFjIndex::kFvel3: // fvelocity 3
      return pbar/pw*Qw/pw*Ew/Ebar;
      break;
    default:
      std::cerr  << "\033[1mTFastReso.cpp::get_factor_Aj\033[0m : \033[1;31merror\033[0m :  wrong index "  << (int) j << std::endl;
      exit(EXIT_FAILURE);
  }
}

//! Parameters for integration of thermal distribution
struct thermal_params{
  double m;
  double Tfo;
  double QMu;
  EParticleType type;
};
//! Thermal distribution function for integration over vbar=[0,1]
double get_F_p_dvbar(double vbar, void * p) {
  double &m           = ((struct thermal_params *) p)->m;
  double &Tfo           = ((struct thermal_params *) p)->Tfo;
  double &QMu           = ((struct thermal_params *) p)->QMu;
  EParticleType &type           = ((struct thermal_params *) p)->type;
  double Ebar=m/sqrt(1-vbar*vbar);
  double pbar=vbar*Ebar;
  double F_p = get_thermal_F(Ebar,Tfo,QMu, type);
  return F_p*pbar*pbar*4*M_PI*m/pow(2*M_PI*sqrt(1-vbar*vbar),3);
}
//! Parameters for integration of two body decay function
struct twobody_params{
  double Ebar;
  double Ma;
  double Mb;
  double Mc;
  TParticle * Parent;
  EFjIndex index;
};
//! Integrand of two body decay formula for f_j components
double get_F_pu_dw(double w, void * p) {
  double &Ebar           = ((struct twobody_params *) p)->Ebar;
  double &Ma             = ((struct twobody_params *) p)->Ma;
  double &Mb             = ((struct twobody_params *) p)->Mb;
  double &Mc             = ((struct twobody_params *) p)->Mc;
  TParticle *Parent      = ((struct twobody_params *) p)->Parent;
  EFjIndex &index             = ((struct twobody_params *) p)->index;

  double Eabc = get_Eabc(Ma,Mb,Mc);
  double pabc = get_p(Eabc, Mb);
  //double pbar = GSL_MAX(get_p(Ebar, Mb),DBL_EPSILON);
  double pbar = get_p(Ebar, Mb);// ,DBL_EPSILON);
  double Ebar_old = Ebar*Eabc*Ma/Mb/Mb-w*pbar*pabc*Ma/Mb/Mb;
  //double Ebar_old = pbar*pabc*Ma/Mb/Mb*(1-w);
  //double Ebar_old = pbar*pabc/Ma*u;
  double F_old=Parent->get_Fj( (int) index, Ebar_old);
  double Qw= (Ma*Eabc*pbar/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar);
  //double Qw = pabc/Ma*u;
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);
  double A = get_factor_Aj(index, Qw, Ebar_old, Ebar, pbar, Ma,sqrt(Qw*Qw+(1-w*w)*Ma*Ma/Mb/Mb*pabc*pabc));
  return F_old*A;
}
double get_F_pu_du(double u, void * p) {
  double &Ebar           = ((struct twobody_params *) p)->Ebar;
  double &Ma             = ((struct twobody_params *) p)->Ma;
  double &Mb             = ((struct twobody_params *) p)->Mb;
  double &Mc             = ((struct twobody_params *) p)->Mc;
  TParticle *Parent      = ((struct twobody_params *) p)->Parent;
  EFjIndex &index             = ((struct twobody_params *) p)->index;

  double Eabc = get_Eabc(Ma,Mb,Mc);
  double pabc = get_p(Eabc, Mb);
  //double pbar = GSL_MAX(get_p(Ebar, Mb),DBL_EPSILON);
  double pbar = get_p(Ebar, Mb);// ,DBL_EPSILON);
  //double Ebar_old = Ebar*Eabc*Ma/Mb/Mb-w*pbar*pabc*Ma/Mb/Mb;
  //double Ebar_old = pbar*pabc*Ma/Mb/Mb*(1-w);
  if (pbar==0) { return 0;};
  double Ebar_old = Ma/2*(pbar/pabc+pabc/pbar)+u*Ma;
  double F_old=Parent->get_Fj((int) index, Ebar_old);
  //double Qw= (Ma*Eabc/Mb/Mb-w*Ma*pabc/Mb/Mb*Ebar/pbar);
  double Qw = Ma/2/pbar*(pbar*pbar/pabc-pabc)+Ma*u;
//  double Ew= (Ma*Eabc*Ebar/Mb/Mb-w*Ma*pabc*pbar/Mb/Mb);
  double A = get_factor_Aj(index, Qw, Ebar_old, Ebar, pbar, Ma,sqrt(Qw*Qw+2*u*pabc/pbar*Ma*Ma));
  return F_old*A/(pbar*pabc/Ma/Ma);
}

//! Parameters for three body decay mass integral
struct threebody_params{
  gsl_function  *twobody_function;
  double Mc;
  double Md;
  gsl_integration_workspace *twobody_workspace;
};

//! Integrand for three body decay normalization
double get_B_dm(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  return get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc, Md);
}
//! Integrand for three body decay function
double get_F_pu_dm(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  ((struct twobody_params *) twobody_function->params)->Mc = mct;
  gsl_integration_workspace *twobody_workspace = ((struct threebody_params *) p)->twobody_workspace;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  double result,error;

  using namespace qag_params;
  int status = gsl_integration_qag (twobody_function, -1, 1, fEpsAbs,fEpsRel, fLimit,fKey ,twobody_workspace, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
    status = gsl_integration_qag (twobody_function, -1, 1, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15,twobody_workspace, &result, &error);
    if (status) {
      status = gsl_integration_qag (twobody_function, -1, 1, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15,twobody_workspace, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " <<gsl_strerror ( status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }
  return result*get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc,Md);
}
double get_F_pu_dm_u(double mct, void * p) {
  gsl_function *twobody_function = ((struct threebody_params *) p)->twobody_function;
  ((struct twobody_params *) twobody_function->params)->Mc = mct;
  gsl_integration_workspace *twobody_workspace = ((struct threebody_params *) p)->twobody_workspace;
  double &Ma = ((struct twobody_params *) twobody_function->params)->Ma;
  double &Mb = ((struct twobody_params *) twobody_function->params)->Mb;
  double &Mc             = ((struct threebody_params *) p)->Mc;
  double &Md             = ((struct threebody_params *) p)->Md;
  double result,error;

  using namespace qag_params;
  int status = gsl_integration_qagiu (twobody_function, 0.0, fEpsAbs,fEpsRel, fLimit,twobody_workspace, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
  status = gsl_integration_qagiu (twobody_function, 0.0, fEpsAbs,1e-4, fLimit,twobody_workspace, &result, &error);
    if (status) {
  status = gsl_integration_qagiu (twobody_function, 0.0, 1e-4,1e-4, fLimit,twobody_workspace, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

  return result*get_pabc(Ma,Mb,mct)*get_pabc(mct, Mc,Md);
}

void do_2bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, double BranchingRatio);
void do_3bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, TParticle * Child3, double BranchingRatio);

//! Initialize thermal distribution
double get_thermal_F(double Ebar, double T, double QMu, EParticleType type)
{
  switch(type){
    case EParticleType::kBoson:
      return 1.0/(exp((Ebar-QMu)/T)-1);
      break;
    case EParticleType::kFermion:
      return 1.0/(exp((Ebar-QMu)/T)+1);
      break;
    case EParticleType::kBoltzman:
      return exp(-(Ebar-QMu)/T);
      break;
    default:
      std::cerr << "\033[1mTFastReso.cpp::get_thermal_F\033[0m : \033[1;31merror\033[0m : unrecognined particle type!" <<std::endl;
      exit(EXIT_FAILURE);}
}
//! Initialize  derivative of t hermal distribution (With a minus sign)
double get_thermal_dF(double Ebar, double T, double QMu, EParticleType type)
{
  switch(type){
    case EParticleType::kBoson:
      return 1.0/(exp((Ebar-QMu)/T)-1)*(1+1.0/(exp((Ebar-QMu)/T)-1));
      break;
    case EParticleType::kFermion:
      return 1.0/(exp((Ebar-QMu)/T)+1)*(1-1.0/(exp((Ebar-QMu)/T)+1));
      break;
    case EParticleType::kBoltzman:
      return exp(-(Ebar-QMu)/T);
      break;
    default:
      std::cerr << "\033[1mTFastReso.cpp::get_thermal_F_pu\033[0m : \033[1;31merror\033[0m : unrecognined particle type!" <<std::endl;
      exit(EXIT_FAILURE);}
}

//! Perform two body decay integral: Parent->Child1 + Child2
void do_2bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, double BranchingRatio){
  using namespace qag_params;

  gsl_integration_workspace * fWorkspace2body ;
  fWorkspace2body = gsl_integration_workspace_alloc (fLimit);
  Parent->lock();
  double Ma = Parent->getM();
  double Mb = Child1->getM();
  double Mc = Child2->getM();
  double nua = Parent->getNu();
  double nub = Child1->getNu();
  double Ebar=0.0;
  Child1->clean_buffer();
  if (Ma < Mb+Mc) {
    std::cerr  << "\033[1mTFastReso.cpp::do_2bodydecays\033[0m : \033[1;31merror\033[0m : father particle lighter than childern! " << Ma << " < " << Mb+Mc << std::endl;
    exit(EXIT_FAILURE);
  }
  gsl_function func2body;
      if (Mb==0) {
  func2body.function = get_F_pu_du;
      } else {
  func2body.function = get_F_pu_dw;
      }
  twobody_params params = {Ebar, Ma, Mb, Mc, Parent, EFjIndex::kFeq1};
  func2body.params = &params;
  double error, result ;
  int sym=1 ;
  if (Child1==Child2 ){ sym=2; } else { sym=1; }

  double Nfather = Parent->getN();
  //! add fraction of the total yield to child
  Child1->addN(BranchingRatio*Nfather*sym);
  for (int i=0; i < Child1->getNpbar(); i++){
    double Ebar =Child1->getEbar(i);
    params.Ebar =Ebar;

    //for (int j=0; j < grid_params::fNf ; j++){
    for(auto j: Parent->fComponents) {
      params.index =j;
      double fac = 0;
      if (Mb==0) {
int status = gsl_integration_qagiu (&func2body, 0.0, fEpsAbs,fEpsRel, fLimit ,fWorkspace2body, &result, &error);

  // Check status of integration and reduce accuracy if failing
  if (status) {
status = gsl_integration_qagiu (&func2body, 0.0, fEpsAbs,1e-4, fLimit ,fWorkspace2body, &result, &error);
    if (status) {
status = gsl_integration_qagiu (&func2body, 0.0, 1e-4,1e-4, fLimit ,fWorkspace2body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      fac = sym*BranchingRatio*nua/nub/2;
      }
      else {
int status = gsl_integration_qag (&func2body, -1, 1, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace2body, &result, &error);

  // Check status of integration and reduce accuracy if failing
  if (status) {
status = gsl_integration_qag (&func2body, -1, 1, fEpsAbs,1e-4, fLimit, GSL_INTEG_GAUSS15,fWorkspace2body, &result, &error);
    if (status) {
status = gsl_integration_qag (&func2body, -1, 1, 1e-4,1e-4, fLimit, GSL_INTEG_GAUSS15 ,fWorkspace2body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status) << ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      fac = sym*BranchingRatio*nua/nub*pow(Ma/Mb,2)/2;

      }
      Child1->addFj((int) j,i, fac*result);

    }
  }
  //if (Child1=="pi0139plu"  ) { getParticle(Child1)->print_buffer(Child1+"_"+Parent+"_"+Child2);}
  gsl_integration_workspace_free (fWorkspace2body);

}

//! Perform three body decay integral Parent->Child1 + Child2 + Child3
void do_3bodydecay(TParticle * Parent, TParticle * Child1, TParticle * Child2, TParticle * Child3, double BranchingRatio){
  using namespace qag_params;
  gsl_integration_workspace * fWorkspace2body ;
  gsl_integration_workspace * fWorkspace3body ;
  fWorkspace2body = gsl_integration_workspace_alloc (fLimit);
  fWorkspace3body = gsl_integration_workspace_alloc (fLimit);
  Parent->lock();
  double Ma = Parent->getM();
  double Mb = Child1->getM();
  double Mc = Child2->getM();
  double Md = Child3->getM();
  double nua = Parent->getNu();
  double nub = Child1->getNu();
  double Ebar=0.0;

  Child1->clean_buffer();
  if (Ma < Mb+Mc+Md) {
    std::cerr  << "\033[1mTFastReso.cpp::do_3bdoydecays\033[0m : \033[1;31merror\033[0m : father particle lighter than childern! " << Ma << " < " << Mb+Mc+Md << std::endl;
    exit(EXIT_FAILURE);
  }
  gsl_function func2body;
  gsl_function func3body;
      if (Mb==0) {
  func2body.function = get_F_pu_du;
      } else {
  func2body.function = get_F_pu_dw;
      }
  func3body.function = get_B_dm;
  twobody_params params2 = {Ebar, Ma, Mb, Mc+Md, Parent, EFjIndex::kFeq1};
  func2body.params = &params2;
  threebody_params params3 = {&func2body, Mc,Md, fWorkspace2body};
  func3body.params = &params3;
  double error, result ;
  int status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace3body, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
  status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
    if (status) {
  status =gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

  double B3body = result;
      if (Mb==0) {
  func3body.function = get_F_pu_dm_u;
      } else {
  func3body.function = get_F_pu_dm;
      }

  int sym = 1 ;
  if (Child1==Child2 and Child2==Child3 ){ sym=3; } else if (Child1==Child2 or Child1==Child3 ){ sym=2;} else { sym=1; }
  double Nfather = Parent->getN();
  // add fraction of yield to child
  Child1->addN(BranchingRatio*Nfather*sym);
  for (int i=0; i < Child1->getNpbar(); i++){
    double Ebar =Child1->getEbar(i);
    params2.Ebar =Ebar;
    //for (int j=0; j < grid_params::fNf ; j++){
    for(auto j: Parent->fComponents) {
      params2.index =j;
      int status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,fEpsRel, fLimit,fKey ,fWorkspace3body, &result, &error);
  // Check status of integration and reduce accuracy if failing
  if (status) {
      status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, fEpsAbs,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
    if (status) {
      status = gsl_integration_qag (&func3body, Mc+Md, Ma-Mb, 1e-4,1e-4, fLimit,GSL_INTEG_GAUSS15 ,fWorkspace3body, &result, &error);
      if (status) {      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31merror\033[0m : integration failure! status = " << gsl_strerror (status )<< ". " << result << "+_" << error << std::endl;
        exit(EXIT_FAILURE);
      }else {
        std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative and absolute error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
      }
    } else {
      std::cerr << "\033[1mTFastReso_formulas.h\033[0m : \033[1;31mwarning\033[0m : reduced relative error (1e-4) integration. Result = " << result << "+-" << error <<std::endl;
    }
  }

      double fac =0;
      if (Mb==0) {
        fac = sym*BranchingRatio*nua/nub/2/B3body;
      }
      else {
        fac = sym*BranchingRatio*nua/nub*pow(Ma/Mb,2)/2/B3body;
      }
      Child1->addFj((int) j,i, fac*result);

    }
  }

  //if (Child1=="pi0139plu" ) { getParticle(Child1)->print_buffer(Child1+"_"+Parent+"_"+Child2+"_"+Child3);}
  gsl_integration_workspace_free (fWorkspace2body);
  gsl_integration_workspace_free (fWorkspace3body);
}

#endif


/*
 * Adapted from bits and pieces in https://code.google.com/p/eq-polar-alignment/
 * Could not find copyright statement from there, but license seems to be GPLv3...
 *
 * Copyright 2014, Kalle Vahlman, zuh@iki.fi
 * Licensed under the GNU GPL v3
 *
 */

#include <stdio.h>
#include <time.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <astrometry/anwcs.h>
#include <astrometry/sip.h>
#include <astrometry/sip_qfits.h>

struct coord
{
    double ra;
    double dec;
    double x;
    double y;
};

struct state
{
    anwcs_t* h_wcs;
    anwcs_t* v_wcs;
    double pixel_scale;
    struct coord ncp;
    struct coord polaris;
    struct coord lambda;
    struct coord axis;

};

double julian_date()
{
    time_t now = time(NULL);
    struct tm *t = localtime(&now);
    int day = t->tm_mday;
    int month = t->tm_mon+1;
    int year = 1900 + t->tm_year;

    return 367*year-7*(year+(month+9)/12)/4-3*((year+(month-9)/7)/100+1)/4+275*month/9+day+1721029;
}

int calculate_ncp (double julian, struct coord *c)
{
    double t, zeta, theta;
    if (c == NULL)
        return 0;

    t = (julian-2451545)/36525;
    zeta = (2306.2181*t+0.30188*t*t+0.017998*t*t*t)/3600;
    theta = (2004.3109*t-0.42665*t*t-0.041833*t*t*t)/3600;
    c->dec = 90 - theta;
    c->ra = -zeta;

    return 1;
}

int solve_field(struct state *ps, const char *dir, const char *in, const char *out)
{
    char run[512];
    // TODO: Run the solving with code rather than system()
#define SOLVE "solve-field -B none -M none -S none -R none -U none -N none " \
              "--no-plots --temp-dir %s --dir %s --out %s " \
              "--downsample 2 --ra %.4f --dec %.4f --radius 10 %s"
    sprintf(run, SOLVE, dir, dir, out, ps->ncp.ra, ps->ncp.dec, in);
    printf("%s\n", run);
    return system(run);
}

int fvec(const gsl_vector *x, void *params, gsl_vector *f)
{
    struct state *s = (struct state *)params;
    double ra, dec, xp, yp;
    double xi = gsl_vector_get(x, 0);
    double yi = gsl_vector_get(x, 1);

    anwcs_pixelxy2radec(s->h_wcs, xi, yi, &ra, &dec);
    anwcs_radec2pixelxy(s->v_wcs, ra, dec, &xp, &yp);
    xp = xp - xi;
    yp = yp - yi;
    gsl_vector_set(f, 0, xp);
    gsl_vector_set(f, 1, yp);

    return GSL_SUCCESS;
}

int solve_axis(struct state *ps)
{
    int s;
    size_t iter=0;
    const size_t n=2;
    gsl_multiroot_fsolver *solver;
    gsl_multiroot_function f = { &fvec, n, ps};
    gsl_vector *x = gsl_vector_alloc(n);

    gsl_vector_set(x, 0, ps->polaris.x);
    gsl_vector_set(x, 1, ps->polaris.y);
    solver = gsl_multiroot_fsolver_alloc(gsl_multiroot_fsolver_hybrids, 2);
    gsl_multiroot_fsolver_set(solver, &f, x);
    do {
        iter++;
        s = gsl_multiroot_fsolver_iterate(solver);
        if (s) break;
        s = gsl_multiroot_test_residual(solver->f,1e-7);
    } while (s == GSL_CONTINUE && iter < 1000);
    ps->axis.x = gsl_vector_get(solver->x, 0);
    ps->axis.y = gsl_vector_get(solver->x, 1);

    gsl_multiroot_fsolver_free(solver);
    gsl_vector_free(x);
    return s;
}

#define MAX(a, b) a < b ? b : a

int plot(struct state *ps, char *in, char *out)
{
    char convert[2048];
    double arcmin = 60/ps->pixel_scale;
    double cropwidth = MAX(400, 120 * arcmin);
    double cropheight = MAX(400, 120 * arcmin);
    double cropx = ps->ncp.x - cropwidth/2;
    double cropy = ps->ncp.y - cropheight/2;
    int fontsize = ps->pixel_scale < 24 ? 16 : 8;

#define CONVERT "convert -fill none " \
    "-pointsize %d " \
    "-font Courier -stroke cyan " \
    "-draw 'point %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-stroke red " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-draw 'circle %.0f,%.0f %.0f,%.0f' " \
    "-stroke white " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-font Symbol -fill white " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-fill none " \
    "-stroke orange " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-draw 'line %.0f,%.0f %.0f,%.0f' " \
    "-font Symbol -fill orange " \
    "-draw \"text %.0f,%.0f '%s'\" " \
    "-fill none " \
    "-font Courier -stroke white -fill white " \
    "-draw \"text %.0f,%.0f 'Current pixel offset from NCP: %.0f, %.0f'\" " \
    "-draw \"text %.0f,%.0f '%s %s %s %s'\" " \
    "-crop '%.0fx%.0f+%.0f+%.0f' " \
    "%s %s"

    sprintf(convert, CONVERT, fontsize,
        // Target circles
        ps->ncp.x, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+2*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+5*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+10*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+20*arcmin, ps->ncp.y,
        ps->ncp.x, ps->ncp.y, ps->ncp.x+40*arcmin, ps->ncp.y,
        ps->ncp.x-2*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "2\\'" : "",
        ps->ncp.x-5*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "5\\'" : "",
        ps->ncp.x-10*arcmin+2, ps->ncp.y, ps->pixel_scale < 24 ? "10\\'" : "",
        ps->ncp.x-20*arcmin+2, ps->ncp.y, "20\\'",
        ps->ncp.x-40*arcmin+2, ps->ncp.y, "40\\'",
        // Current axis
        ps->axis.x-5*arcmin, ps->axis.y-5*arcmin, ps->axis.x+5*arcmin, ps->axis.y+5*arcmin,
        ps->axis.x-5*arcmin, ps->axis.y+5*arcmin, ps->axis.x+5*arcmin, ps->axis.y-5*arcmin,
        ps->axis.x, ps->axis.y, ps->axis.x+2*arcmin, ps->axis.y,
        ps->axis.x, ps->axis.y, ps->axis.x+5*arcmin, ps->axis.y,
        // Polaris
        ps->polaris.x, ps->polaris.y-1*arcmin,
            ps->polaris.x, ps->polaris.y+1*arcmin,
        ps->polaris.x-1*arcmin, ps->polaris.y,
            ps->polaris.x+1*arcmin, ps->polaris.y,
        ps->polaris.x+1*arcmin, ps->polaris.y-1*arcmin, "a",
        // λ UMi
        ps->lambda.x, ps->lambda.y-1*arcmin,
            ps->lambda.x, ps->lambda.y+1*arcmin,
        ps->lambda.x-1*arcmin, ps->lambda.y,
            ps->lambda.x+1*arcmin, ps->lambda.y,
        ps->lambda.x+1*arcmin, ps->lambda.y-1*arcmin, "l",
        // Offset report
        cropx + 16, cropy + 32, ps->ncp.x-ps->axis.x, ps->ncp.y-ps->axis.y,
        cropx + 16, cropy + 64,
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? "Well done, perfect alignment!"
                : "Adjust mount alignment to",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? ""
                : ps->ncp.x < ps->axis.x ? "left" : "right",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y) ? "" : "and",
            (ps->ncp.x == ps->axis.x && ps->ncp.y == ps->axis.y)
                ? ""
                : ps->ncp.y < ps->axis.y ? "up" : "down",
        // Crop
        cropwidth, cropheight, cropx, cropy,
        in, out);

    printf("%s\n", convert);
    return system(convert);
}

int main (int argc, char **argv)
{
    char dir[20] = "/tmp/polar-XXXXXX";
    char h_dst[30];
    char v_dst[30];
    sip_t *sip;
    struct state ps = { NULL, NULL, 0,
                      { 0, 0, 0, 0 }, // NCP
                      { 37.9529, 89.2642, 0, 0 }, // Polaris
                      { 259.2367, 89.0378, 0, 0 }, // λ UMi
                      { 0, 0, 0, 0 } // axis
                    };

    if (argc < 3) {
        printf("E: Not enough parameters!\n");
        printf("Usage: %s <horizontal image> <vertical image> [output]\n", argv[0]);
        return -1;
    }

    if (mkdtemp(dir) == NULL) {
        printf("E: Could not create working directory!\n");
        return -1;
    }

    // Calculate current ra,dec position of North Celestial Pole
    calculate_ncp(julian_date(), &ps.ncp);

    // Solve horizontal and vertical images of Polaris and Lambda UMi
    sprintf(h_dst, "%s/h.wcs", dir);
    sprintf(v_dst, "%s/v.wcs", dir);
    solve_field(&ps, dir, argv[1], "h.wcs");
    solve_field(&ps, dir, argv[2], "v.wcs");

    // Pick pixel coordinates from horizontal image
    ps.h_wcs = anwcs_open(h_dst, 0);
    anwcs_radec2pixelxy(ps.h_wcs, ps.polaris.ra, ps.polaris.dec, &ps.polaris.x, &ps.polaris.y);
    anwcs_radec2pixelxy(ps.h_wcs, ps.lambda.ra, ps.lambda.dec, &ps.lambda.x, &ps.lambda.y);
    anwcs_radec2pixelxy(ps.h_wcs, ps.ncp.ra, ps.ncp.dec, &ps.ncp.x, &ps.ncp.y);

    // Solve rotational axis in pixel coordinates of the horizontal image
    ps.v_wcs = anwcs_open(v_dst, 0);
    solve_axis(&ps);
    anwcs_pixelxy2radec(ps.h_wcs, ps.axis.x, ps.axis.y, &ps.axis.ra, &ps.axis.dec);
    anwcs_free(ps.v_wcs);

    // Grab pixel scale from horizontal image
    sip = anwcs_get_sip(ps.h_wcs);
    ps.pixel_scale = sip_pixel_scale(sip);
    anwcs_free(ps.h_wcs);

    // Print what we found
    printf("Pixel scale: %.4f\n", ps.pixel_scale);
    printf("Polaris: %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.lambda.ra, ps.lambda.dec, ps.lambda.x, ps.lambda.y);
    printf("λ UMi  : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.polaris.ra, ps.polaris.dec, ps.polaris.x, ps.polaris.y);
    printf("NCP    : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.ncp.ra, ps.ncp.dec, ps.ncp.x, ps.ncp.y);
    printf("Axis   : %+9.4f ra, %+9.4f dec (%4.0f, %4.0f)\n",
        ps.axis.ra, ps.axis.dec, ps.axis.x, ps.axis.y);
    printf("Offset : %4.0f x, %4.0f y\n",
        ps.ncp.x-ps.axis.x, ps.ncp.y-ps.axis.y);

    // Plot a chart of the polar alignment, if user asks for it
    if (argc == 4)
        plot(&ps, argv[1], argv[3]);

    return 0;
}


/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file multi-steps.c
 * \brief Source file to define the multi-steps method data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include "config.h"
#include "utils.h"
#include "equation.h"
#include "method.h"
#include "runge-kutta.h"
#include "multi-steps.h"

#define DEBUG_MULTI_STEPS 0     ///< macro to debug the multi-steps functions.

///> array of a coefficients of the 2nd order multi-steps method.
const long double ms_a2[3] = { 0.75L, 0.L, 0.25L };

///> array of c coefficients of the 2nd order multi-steps method.
const long double ms_c2[3] = { 2.L, 0.L, 0.L };

///> array of error a coefficients of the 2nd order multi-steps method.
const long double ms_ea2[3] = { 0.25L, 0.L, -0.25L };

///> array of error b coefficients of the 2nd order multi-steps method.
const long double ms_eb2[3] = { 0.5L, 0.L, 0.L };

///> array of a coefficients of the 3rd order multi-steps method.
const long double ms_a3[4] = { 16.L / 27.L, 0.L, 0.L, 11.L / 27.L };

///> array of c coefficients of the 3rd order multi-steps method.
const long double ms_c3[4] = { 3.L, 0.L, 0.L, 12.L / 11.L };

///> array of error a coefficients of the 3rd order multi-steps method.
const long double ms_ea3[4] = { 17.L / 108.L, 0.L, 0.25L, -11.L / 27.L };

///> array of error b coefficients of the 3rd order multi-steps method.
const long double ms_eb3[4] = { -5.L / 18.L, 0.L, 0.L, -4.L / 9.L };

/**
 * Function to init the multi-steps method coefficients.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_init (MultiSteps * ms)      ///< MultiSteps struct.
{
  Method *m;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  switch (ms->type)
    {
    case 1:
      method_init (m, 3, 2);
      ms->a = ms_a2;
      ms->c = ms_c2;
      ms->ea = ms_ea2;
      ms->eb = ms_eb2;
      break;
    case 2:
      method_init (m, 4, 3);
      ms->a = ms_a3;
      ms->c = ms_c3;
      ms->ea = ms_ea3;
      ms->eb = ms_eb3;
      break;
    default:
      printf ("Error reading multi-steps data\n");
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_init: error\n");
      fprintf (stderr, "multi_steps_init: end\n");
#endif
      return 0;
    }
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init: success\n");
  fprintf (stderr, "multi_steps_init: end\n");
#endif
  return 1;
}

/**
 * Function to init the variables used by the multi-steps methods.
 */
void
multi_steps_init_variables (MultiSteps * ms)
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: start\n");
#endif
  runge_kutta_init_variables (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_init_variables (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_init_variables: end\n");
#endif
}

/**
 * Function to perform a step of the multi-steps method.
 */
static inline void
multi_steps_step (MultiSteps * ms,      ///< MultiSteps struct.
                  Equation * eq,        ///< Equation struct.
                  long double t,        ///< actual time.
                  long double dt)       ///< time step size.
{
  long double msr0[3], msr1[3];
  Method *m;
  const long double *a;
  const long double *c;
  unsigned int i, n;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_step: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, m->r0[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, m->r1[0][i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r2[0][%u]=%Lg\n", i, m->r2[0][i]);
#endif
  n = m->nsteps;
  a = ms->a;
  c = ms->c;
#if DEBUG_MULTI_STEPS
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: a%u=%Lg\n", i, a[i]);
  for (i = 0; i < n; ++i)
    fprintf (stderr, "multi_steps_step: c%u=%Lg\n", i, c[i]);
#endif
  msr0[0] = a[0] * (r0[0] + dt * c[0] * r1[0]);
  msr0[1] = a[0] * (r0[1] + dt * c[0] * r1[1]);
  msr0[2] = a[0] * (r0[2] + dt * c[0] * r1[2]);
  msr1[0] = a[0] * (r1[0] + dt * c[0] * r2[0]);
  msr1[1] = a[0] * (r1[1] + dt * c[0] * r2[1]);
  msr1[2] = a[0] * (r1[2] + dt * c[0] * r2[2]);
  for (i = 1; i < n; ++i)
    {
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_step: r0[%u][0]=%Lg\n", i, m->r0[i][0]);
      fprintf (stderr, "multi_steps_step: r0[%u][1]=%Lg\n", i, m->r0[i][1]);
      fprintf (stderr, "multi_steps_step: r0[%u][2]=%Lg\n", i, m->r0[i][2]);
      fprintf (stderr, "multi_steps_step: r1[%u][0]=%Lg\n", i, m->r1[i][0]);
      fprintf (stderr, "multi_steps_step: r1[%u][1]=%Lg\n", i, m->r1[i][1]);
      fprintf (stderr, "multi_steps_step: r1[%u][2]=%Lg\n", i, m->r1[i][2]);
      fprintf (stderr, "multi_steps_step: r2[%u][0]=%Lg\n", i, m->r2[i][0]);
      fprintf (stderr, "multi_steps_step: r2[%u][1]=%Lg\n", i, m->r2[i][1]);
      fprintf (stderr, "multi_steps_step: r2[%u][2]=%Lg\n", i, m->r2[i][2]);
#endif
      msr0[0] += a[i] * (m->r0[i][0] + dt * c[i] * m->r1[i][0]);
      msr0[1] += a[i] * (m->r0[i][1] + dt * c[i] * m->r1[i][1]);
      msr0[2] += a[i] * (m->r0[i][2] + dt * c[i] * m->r1[i][2]);
      msr1[0] += a[i] * (m->r1[i][0] + dt * c[i] * m->r2[i][0]);
      msr1[1] += a[i] * (m->r1[i][1] + dt * c[i] * m->r2[i][1]);
      msr1[2] += a[i] * (m->r1[i][2] + dt * c[i] * m->r2[i][2]);
    }
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, msr0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, msr1[i]);
#endif
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));
  for (i = n; --i > 0;)
    {
      memcpy (m->r0[i], m->r0[i - 1], 3 * sizeof (long double));
      memcpy (m->r1[i], m->r1[i - 1], 3 * sizeof (long double));
      memcpy (m->r2[i], m->r2[i - 1], 3 * sizeof (long double));
    }
  memcpy (r0, msr0, 3 * sizeof (long double));
  memcpy (r1, msr1, 3 * sizeof (long double));
  equation_acceleration (eq, r0, r1, r2, t + dt);
#if DEBUG_MULTI_STEPS
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r0[0][%u]=%Lg\n", i, r0[i]);
  for (i = 0; i < 3; ++i)
    fprintf (stderr, "multi_steps_step: r1[0][%u]=%Lg\n", i, r1[i]);
  fprintf (stderr, "multi_steps_step: end\n");
#endif
}

/**
 * Function to estimate the error on a multi-steps method step.
 */
static inline void
multi_steps_error (MultiSteps * ms,     ///< MultiSteps struct.
                   long double dt)      ///< time step size.
{
  long double e0[3], e1[3];
  Method *m;
  unsigned int i;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: start\n");
#endif
  m = MULTI_STEPS_METHOD (ms);
  e0[0] = e0[1] = e0[2] = e1[0] = e1[1] = e1[2] = 0.L;
  for (i = 0; i < m->nsteps; ++i)
    {
      e0[0] += ms->ea[i] * m->r0[i][0] + dt * ms->eb[i] * m->r1[i][0];
      e0[1] += ms->ea[i] * m->r0[i][1] + dt * ms->eb[i] * m->r1[i][1];
      e0[2] += ms->ea[i] * m->r0[i][2] + dt * ms->eb[i] * m->r1[i][2];
      e1[0] += ms->ea[i] * m->r1[i][0] + dt * ms->eb[i] * m->r2[i][0];
      e1[1] += ms->ea[i] * m->r1[i][1] + dt * ms->eb[i] * m->r2[i][1];
      e1[2] += ms->ea[i] * m->r1[i][2] + dt * ms->eb[i] * m->r2[i][2];
    }
  m->e0 = sqrtl (e0[0] * e0[0] + e0[1] * e0[1] + e0[2] * e0[2]);
  m->e1 = sqrtl (e1[0] * e1[0] + e1[1] * e1[1] + e1[2] * e1[2]);
  m->et0 += m->e0;
  m->et1 += m->e1;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_error: e0=%Lg et0=%Lg\n", m->e0, m->et0);
  fprintf (stderr, "multi_steps_error: e1=%Lg et1=%Lg\n", m->e1, m->et1);
  fprintf (stderr, "multi_steps_error: end\n");
#endif
}

/**
 * Function to run the multi-steps method bucle.
 *
 * \return final time.
 */
long double
multi_steps_run (MultiSteps * ms,       ///< MultiSteps struct.
                 Equation * eq) ///< Equation struct.
{
  RungeKutta *rk;
  Method *m, *mrk;
  long double t, dt, to, dto, et0o, et1o;
  unsigned int i, n;

#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: start\n");
#endif

  // variables backup
  rk = MULTI_STEPS_RUNGE_KUTTA (ms);
  mrk = RUNGE_KUTTA_METHOD (rk);
  m = MULTI_STEPS_METHOD (ms);
  memcpy (ro0, r0, 3 * sizeof (long double));
  memcpy (ro1, r1, 3 * sizeof (long double));
  memcpy (ro2, r2, 3 * sizeof (long double));

  // Runge-Kutta first steps
  n = m->nsteps;
  for (t = 0.L, i = n; --i > 0;)
    {

      // time step size
      if (t > 0.L && mrk->error_dt)
        {
          dto = dt;
          dt = method_dt (mrk, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              mrk->et0 = et0o;
              mrk->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      if (equation_land (eq, to, &t, &dt))
        goto end;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // saving step
      memcpy (m->r0[i], r0, 3 * sizeof (long double));
      memcpy (m->r1[i], r1, 3 * sizeof (long double));
      memcpy (m->r2[i], r2, 3 * sizeof (long double));
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // Runge-Kutta step
      runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (mrk->error_dt)
        {
          et0o = mrk->et0;
          et1o = mrk->et1;
          runge_kutta_error (rk, dt);
        }
    }

  // saving last step
  memcpy (m->r0[0], r0, 3 * sizeof (long double));
  memcpy (m->r1[0], r1, 3 * sizeof (long double));
  memcpy (m->r2[0], r2, 3 * sizeof (long double));

  // initing errors
  m->et0 = mrk->et0;
  m->et1 = mrk->et1;

  // temporal bucle
  while (1)
    {

      // time step size
      if (m->error_dt)
        {
          dto = dt;
          dt = method_dt (m, dt);

          // revert the step if big error
          if (dt < mrk->beta * dto)
            {
              ++i;
              t = to;
              m->et0 = et0o;
              m->et1 = et1o;
              memcpy (r0, ro0, 3 * sizeof (long double));
              memcpy (r1, ro1, 3 * sizeof (long double));
              memcpy (r2, ro2, 3 * sizeof (long double));
            }
        }
      else
        dt = equation_step_size (eq);

      // checking trajectory end
      to = t;
      dto = dt;
      if (equation_land (eq, to, &t, &dt))
        break;
#if DEBUG_MULTI_STEPS
      fprintf (stderr, "multi_steps_run: t=%Lg dt=%Lg\n", t, dt);
#endif

      // variables backup
      memcpy (ro0, r0, 3 * sizeof (long double));
      memcpy (ro1, r1, 3 * sizeof (long double));
      memcpy (ro2, r2, 3 * sizeof (long double));

      // multi-steps step
      if (dto == dt)
        multi_steps_step (ms, eq, to, dt);
      else
        runge_kutta_step (rk, eq, to, dt);

      // error estimate
      if (m->error_dt)
        {
          et0o = m->et0;
          et1o = m->et1;
          multi_steps_error (ms, dt);
        }
    }
end:
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_run: end\n");
#endif
  return t;
}

/**
 * Function to free the memory used by a MultiSteps struct.
 */
void
multi_steps_delete (MultiSteps * ms)    ///< RungeKutta struct.
{
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: start\n");
#endif
  runge_kutta_delete (MULTI_STEPS_RUNGE_KUTTA (ms));
  method_delete (MULTI_STEPS_METHOD (ms));
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_delete: end\n");
#endif
}

/**
 * Function to read on a XML node the multi-steps method input data.
 *
 * \return 1 on success, 0 on error.
 */
int
multi_steps_read_xml (MultiSteps * ms,  ///< MultiSteps struct.
                      xmlNode * node)   ///< XML node.
{
	char *message[] = {
		NULL,
		"Bad steps number",
		"Bad numerical method data",
		"No Runge-Kutta XML node",
		"Bad Runge-Kutta XML node",
		"Bad Runge-Kutta method data",
		"Bad multi-steps data"
	};
  int e, error_code;
#if DEBUG_MULTI_STEPS
  fprintf (stderr, "multi_steps_read_xml: start\n");
#endif
  ms->type = xml_node_get_uint (node, XML_TYPE, &error_code);
  if (error_code || !ms->type)
    e = 1;
	else if (!method_read_xml (MULTI_STEPS_METHOD (ms), node))
    e = 2;
  else
	  {
			node = node->children;
			if (!node)
				e = 3;
			else if (xmlStrcmp (node->name, XML_RUNGE_KUTTA))
				e = 4;
			else if (!runge_kutta_read_xml (MULTI_STEPS_RUNGE_KUTTA (ms), node))
				e = 5;
			else if (!multi_steps_init (ms))
				e = 6;
			else
				e = 0;
		}
	if (e)
	{
		error_add (message[e]);
		e = 0;
	}
	else
		e = 1;
#if DEBUG_MULTI_STEPS
  if (e)
    fprintf (stderr, "multi_steps_read: success\n");
  else
    fprintf (stderr, "multi_steps_read: error\n");
  fprintf (stderr, "multi_steps_read: end\n");
#endif
  return e;
}


/*
** get transformation matrix for co-registration
** Translation, rotation and one scaling parameter (7 degrees of freedom)
**
** G.Lohmann, 2015
*/
#include <viaio/VImage.h>
#include <viaio/Vlib.h>
#include <viaio/mu.h>
#include <viaio/os.h>

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

#include <gsl/gsl_errno.h>
#include <gsl/gsl_cblas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_multimin.h>

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

#define PI 3.14159265358979323846

double **MatrixAlloc(int n,int m)
{
  int i,j;
  double **A = (double **)VCalloc(n,sizeof(double));
  for(i=0; i<n; i++) {
    A[i]=(double *)VCalloc(m,sizeof(double));
  }
  for (i=0; i<n; i++) {
    for (j=0; j<m; j++) {
      A[i][j] = 0.0;
    }
  }
  return A;
}

void MatrixFree(double **A,int n,int m)
{
  int i;
  for(i=0; i<n; i++) VFree(A[i]);
  VFree(A);
}

/* matrix mult of 3x3 rotation matrices */
void MatrixMult3x3(double **A, double **B, double **C)
{
  int i,j,k,n=3;
  for (i=0; i<n; i++) {
    for (j=0; j<n; j++) {
      double sum = 0;
      for (k=0; k<n; k++) {
        sum += A[i][k] * B[k][j];
      }
      C[i][j] = sum;
    }
  }
}

/* matrix ini */
void MatrixIni3x3(double **A)
{
  int i,j,n=3;
  for (i=0; i<n; i++) {
    for (j=0; j<n; j++) {
      A[i][j] = 0.0;
    }
    for (i=0; i<n; i++) A[i][i] = 1.0;
  }
}

/* 3x3 matrix times vector */
void MatrixVector3x3(double **A, double *x,double *y)
{
  int i,j,n=3;
  for (i=0; i<n; i++) {
    double sum = 0;
    for (j=0; j<n; j++) {
      sum += A[i][j]*x[j];
    }
    y[i] = sum;
  }
}

void MatrixCopy3x3(double **A,double **B)
{
  int i,j,n=3;
  for (i=0; i<n; i++) {
    for (j=0; j<n; j++) {
      B[i][j] = A[i][j];
    }
  }
}

void MatrixScale3x3(double **R,double sx,double sy,double sz)
{
  int i;
  for (i=0; i<3; i++) {
    R[i][0] *= sx;
    R[i][1] *= sy;
    R[i][2] *= sz;
  }
}

void printmat(VImage src)
{
  int i,j;
  fprintf(stderr,"printmat:\n");
  for (i=0; i<VImageNRows(src); i++) {
    for (j=0; j<VImageNColumns(src); j++) {
      float u = VGetPixel(src,0,i,j);
      fprintf(stderr," %8.3f",u);
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void xprintmat(double **A,int n,int m)
{
  int i,j;
  fprintf(stderr,"printmat:\n");
  for (i=0; i<n; i++) {
    for (j=0; j<m; j++) {
      fprintf(stderr," %8.3f",A[i][j]);
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void VCheckImage(VImage src)
{
  VAttrList out_list = VCreateAttrList();
  VAppendAttr(out_list,"image",NULL,VImageRepn,src);
  FILE *out_file = fopen("test.v","w");
  VWriteFile (out_file, out_list);
}

double clamp(double v)
{
  v = fmod(v, 2.0*PI);
  return v>=0 ? v : 2.0*PI+v;
}

void EulerAngles(double **R,double *ea)
{
  int i;

  double u = -R[0][2];
  if (u < -1.0) u = u + ceil(u);
  if (u >  1.0) u = u - floor(u);
  if (u < -1.0) u = -1.0;
  if (u >  1.0) u =  1.0;
  ea[1] = asin(u);

  double xcos = cos(ea[1]);
  if (ABS(xcos)<0.001*PI/180.0) {   /* Gimball lock? */
    ea[0] = atan2(R[1][0], R[1][1]);
    ea[2] = 0;
  }
  else {
    ea[0] = atan2(R[1][2], R[2][2]);
    ea[2] = atan2(R[0][1], R[0][0]);
  }
  for (i=0; i<3; i++) ea[i] = fmod(ea[i],2.0*PI);
  /*   for (i=0; i<3; i++) ea[i] = clamp(ea[i]); */
}

void Euler2RotationMatrix(double **R,double *ea)
{
  double c1 = cos(ea[0]);
  double c2 = cos(ea[1]);
  double c3 = cos(ea[2]);
  double s1 = sin(ea[0]);
  double s2 = sin(ea[1]);
  double s3 = sin(ea[2]);

  R[0][0] = c2*c1;
  R[1][0] = -c3*s1+s3*s2*c1;
  R[2][0] = s3*s1+c3*s2*c1;
  R[0][1] = c2*s1;
  R[1][1] = c3*c1+s3*s2*s1;
  R[2][1] = -s3*c1+c3*s2*s1;
  R[0][2] = -s2;
  R[1][2] = s3*c2;
  R[2][2] = c3*c2;

  double qfac = -1.0;
  R[0][2] *= qfac;
  R[1][2] *= qfac;
  R[2][2] *= qfac;
}

void Normalize(VImage src)
{
  int b,r,c;
  double tiny = 1.0e-6;
  if (VPixelRepn(src) == VUByteRepn || VPixelRepn(src) == VShortRepn
      || VPixelRepn(src) == VSByteRepn)tiny = 1.0;

  double sum1=0,sum2=0,nx=0;
  for (b=0; b<VImageNBands(src); b++) {
    for (r=0; r<VImageNRows(src); r++) {
      for (c=0; c<VImageNColumns(src); c++) {
	double u = VGetPixel(src,b,r,c);
	if (u >= tiny) {
	  sum1 += u;
	  sum2 += u*u;
	  nx++;
	}
      }
    }
  }
  double mean = sum1/nx;
  double sigma = sqrt((sum2 - nx * mean * mean) / (nx - 1.0));

  double xmax = VPixelMaxValue(src);
  double xmin = VPixelMinValue(src);
  double mean0 = (xmax + xmin)/2.0;
  double sig0 = xmax*0.2;
  fprintf(stderr," normalize, mean,sd= %f %f\n",mean0,sig0);

  for (b=0; b<VImageNBands(src); b++) {
    for (r=0; r<VImageNRows(src); r++) {
      for (c=0; c<VImageNColumns(src); c++) {
	double u = VGetPixel(src,b,r,c);
	if (u >= tiny) {
	  u = (u-mean)/sigma;
	  u = u*sig0 + mean0;
	  if (u > xmax) u = xmax;
	  if (u < xmin) u = xmin;
	  VSetPixel(src,b,r,c,u);
	}
      }
    }
  }
}

/* set up transformation info */
void ReadTransformation(VImage transimage,double **trans,double *shift,
			double *ref_pixdim,double *ref_dim,double *src_pixdim,double *src_dim)
{
  int i,j;
  for (i=0; i<3; i++) {
    for (j=0; j<3; j++) {
      trans[i][j] = VGetPixel(transimage,0,i,j);
    }
  }
  for (i=0; i<3; i++)
    shift[i] = VGetPixel(transimage,0,i,3);

  for (i=0; i<3; i++)
    ref_pixdim[i] = VGetPixel(transimage,0,i,4);

  for (i=0; i<3; i++)
    ref_dim[i] = VGetPixel(transimage,0,i,5);

  for (i=0; i<3; i++)
    src_pixdim[i] = VGetPixel(transimage,0,i,6);

  for (i=0; i<3; i++)
    src_dim[i] = VGetPixel(transimage,0,i,7);
}

void VSetResolution(VImage transimage,double **trans,double newres,double *shift,double *scale)
{
  /* resolution of reference image */
  double resx0 = VGetPixel(transimage,0,0,6);
  double resy0 = VGetPixel(transimage,0,1,6);
  double resz0 = VGetPixel(transimage,0,2,6);

  /* change resolution */
  scale[0] = resx0/newres;
  scale[1] = resy0/newres;
  scale[2] = resz0/newres;

  int i;
  MatrixScale3x3(trans,scale[0],scale[1],scale[2]);
  for (i=0; i<3; i++) shift[i] *= scale[i];
}


#pragma once

#include <list>
#include <map>
#include <memory>
#include <string>
#include <gsl/gsl_math.h>

#include "Expressions/Expression.h"
#include "Scanner/scanner.h"
#include "Parser/parser.h"
#include "Utils/Exception.h"

class MathEngine {

    std::unique_ptr<Parser> parser;
    Variables variables;

    public:
        MathEngine();

        std::list<Scanner::Token> scan(const std::string& input);

        expression parse(const std::string& input);
        expression operator()(const std::string& input);

        expression eval(const std::string& input);
        expression eval(expression inputExpr);

        std::string formatInput(const std::string& input, int& cursorPosition);
        std::string evaluateOutput(const std::string& input, const std::string& output);

};


#include <assert.h>
#include <inttypes.h>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include <gsl/gsl_math.h>

#include "imshift.h"
#include "conv1.h"

void showMatrix(double * I, size_t M, size_t N)
{
  for(size_t kk = 0; kk<N; kk++) {
    for(size_t ll = 0; ll<M; ll++)
    {
      printf("%5.2f ", I[ll*M+kk]);
    }
    printf("\n");
  }
}

int imshift3(double * V, size_t M, size_t N, size_t P,
    double dx, double dy, double dz, int method)
{

  int status = 0;

  double * Kx = NULL; size_t nKx;
  double * Ky = NULL; size_t nKy;
  double * Kz = NULL; size_t nKz;

  status = generateShift(&Kx, dx, &nKx, method);
  if(status != 0)
    return -1;

  status = generateShift(&Ky, dy, &nKy, method);
  if(status != 0) {
    free(Kx);
    return -2;
  }

  status = generateShift(&Kz, dz, &nKz, method);
  if(status != 0) {
    free(Kx);
    free(Ky);
    return -3;
  }

  status = conv1_3(V, M, N, P,
      Kx, nKx,
      Ky, nKy,
      Kz, nKz);

      free(Kz);
      free(Ky);
      free(Kx);

      return status;
}

int generateShift(double ** K, double delta, size_t * size, int order)
  /** Allocate space for a 1D shifting kernel, K,
   * shifted by delta in [-1,1]
   * returns the size of the kernel in size
   * order sets the polynomial order, 1 = linear, 2 = quadratic
   *
   * Errors:
   *  -1, K != NULL
   *  -2, no valid interpolation method selected
   *  -3, unable to allocate memory for the kernel
   *
   * */
{

  if(K[0]!=NULL)
    return -1;

  if(order == 1)
  {
    size_t N = 3;
    size[0] = N;
    K[0] = malloc(N*sizeof(double));
    if(K[0] == NULL)
      return(-3);

    for(size_t kk = 0; kk<N; kk++)
      K[0][kk] = GSL_MAX(0, 1-fabs(kk-(1-delta)));
    return 0;
  }

  if(order == 3)
  {
    size_t N = 5;
    size[0] = N;
    K[0] = malloc(N*sizeof(double));
    if(K[0] == NULL)
      return -3;

    // set up and solve polynomial equation here
    double a = -.5;

    for(size_t kk = 0; kk < N; kk++)
    {
      double x = kk-(2-delta);
      double ax = fabs(x);
      if(ax<=1)
        K[0][kk] = (a+2)*ax*ax*ax - (a+3)*ax*ax + 1;
      if(ax>1)
        K[0][kk] = a*ax*ax*ax -5*a*ax*ax  +8*a*ax -4*a;
      if(ax>2)
        K[0][kk] = 0;
    }
    return 0;
  }

  return -2;
}

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

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

  if(argc>1)
    printf("Warning: unused command line arguments\n");

  {
    printf("--> Testing kernels\n");
    double * K;
    double sum;

    for(double delta = -.5; delta < .6; delta+=.1) {
    for(int method = 1; method < 4; method = method +2)
    {
      K = NULL;
      size_t nK = 0;
      assert(generateShift(&K, delta, &nK, 3)==0);

      sum = 0;
      for(int kk = 0; kk<(int) nK; kk++)
        sum += K[kk];

      printf("delta: %f order: %d sum: %f\n", delta, method, sum);

     free(K);
    }
    }
  }

  {
    printf("--> Testing 1D\n");
    double delta = .3;
    size_t N = 10;
    double * V = malloc(N*sizeof(double));

    for(size_t kk=0; kk<N; kk++)
      V[kk] = kk%2;

    double * K = NULL;
    size_t nK = 0;
    assert(generateShift(&K, delta, &nK, 3)==0);

    printf("Kernel:\n");
    for(size_t kk = 0; kk<nK; kk++)
      printf("%.1f ", K[kk]);
    printf("\n");
    printf("Input:\n");
    for(size_t kk = 0; kk<N; kk++)
      printf("%.1f ", V[kk]);
    printf("\n");

    double * buff = calloc(N,sizeof(double));

    conv1(V, buff, N, K, nK, 1);
    printf("Output\n");
    for(size_t kk = 0; kk<N; kk++)
      printf("%.1f ", V[kk]);
    printf("\n");

    free(buff);
    free(K);
    free(V);
  }

  {
    printf("Testing 2D\n");
    size_t M = 11;
    size_t N = 11;

    double * Kx = NULL;
    double * Ky = NULL;
    size_t nKx;
    size_t nKy;

    double deltax = .1;
    double deltay = .6;

    int method = 1;
    assert(generateShift(&Kx, deltax, &nKx, method)==0);
    assert(generateShift(&Ky, deltay, &nKy, method)==0);

    printf("Kernels:\n");
double sum = 0;
    printf(" deltax: %f\n", deltax);
    for(size_t kk = 0; kk<nKx; kk++) {
      printf("%5.1f ", Kx[kk]);
      sum += Kx[kk];
    }
    printf("s: %.2f\n", sum);
    sum = 0;
    printf(" deltay: %f\n", deltay);
    for(size_t kk = 0; kk<nKy; kk++) {
      printf("%5.1f ", Ky[kk]);
      sum += Ky[kk];
    }
    printf("s: %.2f\n", sum);

    double * V = calloc(M*N, sizeof(double));
    V[(M*N-1)/2] = 1;

    printf("V:\n");
    showMatrix(V, M, N);
    double * C1 = malloc(M*N*sizeof(double));
    double * C2 = malloc(M*N*sizeof(double));
    memcpy(C1,V, M*N*sizeof(double));
    memcpy(C2,V, M*N*sizeof(double));

    conv1_3(C1,  M, N, 1, Kx, nKx, Ky, nKy, NULL, 0);
    conv1_3(C2,  M, N, 1, Ky, nKy, Kx, nKx, NULL, 0);

    printf("C1\n");
    showMatrix(C1, M, N);

    printf("C2\n");
    showMatrix(C2, M, N);

    printf("Testing symmetry, C1=C2' ... ");
    for(size_t kk = 0; kk<M; kk++)
      for(size_t ll = 0; ll<N; ll++)
        assert(C1[kk+M*ll] == C2[ll+M*kk]);
    printf("ok!\n");

    free(V);
    free(Ky);
    free(Kx);
    free(C1);
    free(C2);

  }

  {
    printf("Testing 3D\n");
    double delta = .3;
    size_t M = 11;
    size_t N = 11;
    size_t P = 11;
    double * V3 = calloc(M*N*P,sizeof(double));
    V3[(M*N*P-1)/2] = 1;
    double * K = NULL;
    size_t nK = 0;
    assert(generateShift(&K, delta, &nK, 1)==0);

    clock_t t1 = clock();
    conv1_3(V3, M, N,P, K,nK,K,nK,K,nK);
    clock_t t2 = clock();

    printf("V3(:,:,6):\n");
    showMatrix(V3 + M*N*6, M, N);

    printf("Time: %f ms\n", (double) 1000*(t2-t1)/CLOCKS_PER_SEC);

    free(V3);
    free(K);
  }

  return 0;
}


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

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

double f_sin (double x, void * p) {
  p = 0;
  return sin(x);
}

int
main(void)
{
  double tol = 100.0 * GSL_DBL_EPSILON;
  double x;

  gsl_cheb_series * cs = gsl_cheb_alloc(40);
  gsl_cheb_series * csd = gsl_cheb_alloc(40);
  gsl_cheb_series * csi = gsl_cheb_alloc(40);

  gsl_function F_sin;

  F_sin.function = f_sin;
  F_sin.params = 0;

  gsl_ieee_env_setup();

  gsl_cheb_init(cs, &F_sin, -M_PI, M_PI);

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval(cs, x);
    gsl_test_abs(r, sin(x), tol, "gsl_cheb_eval, sin(%.3g)", x);
  }

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_err(cs, x, &r, &e);
    gsl_test_abs(r, sin(x), tol, "gsl_cheb_eval_err, sin(%.3g)", x);
    gsl_test_factor(fabs(r-sin(x)) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_err, error sin(%.3g)", x);
  }

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval_n(cs, 25, x);
    gsl_test_abs(r, sin(x), tol, "gsl_cheb_eval_n, sin(%.3g)", x);
  }

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_n_err(cs, 25, x, &r, &e);
    gsl_test_abs(r, sin(x), 100.0 * tol, "gsl_cheb_eval_n_err, deriv sin(%.3g)", x);
    gsl_test_factor(fabs(r-sin(x)) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_n_err, error sin(%.3g)", x);
  }

  /* Test derivative */

  gsl_cheb_calc_deriv(csd, cs);

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval(csd, x);
    gsl_test_abs(r, cos(x), 1600 * tol, "gsl_cheb_eval, deriv sin(%.3g)", x);
  }

#ifdef TEST_DERIVATIVE_ERR
  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_err(csd, x, &r, &e);
    gsl_test_abs(r, cos(x), tol, "gsl_cheb_eval_err, deriv sin(%.3g)", x);
    gsl_test_factor(fabs(r-cos(x)) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_err, deriv error sin(%.3g)", x);
  }
#endif

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval_n(csd, 25, x);
    gsl_test_abs(r, cos(x), 1600 * tol, "gsl_cheb_eval_n, deriv sin(%.3g)", x);
  }

#ifdef TEST_DERIVATIVE_ERR
  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_n_err(csd, 25, x, &r, &e);
    gsl_test_abs(r, cos(x), 100.0 * tol, "gsl_cheb_eval_n_err, deriv sin(%.3g)", x);
    gsl_test_factor(fabs(r-cos(x)) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_n_err, deriv error sin(%.3g)", x);
  }
#endif

  /* Test integral */

  gsl_cheb_calc_integ(csi, cs);

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval(csi, x);
    gsl_test_abs(r, -(1+cos(x)), tol, "gsl_cheb_eval, integ sin(%.3g)", x);
  }

#ifdef TEST_INTEGRAL_ERR
  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_err(csi, x, &r, &e);
    gsl_test_abs(r, -(1+cos(x)), tol, "gsl_cheb_eval_err, integ sin(%.3g)", x);
    gsl_test_factor(fabs(r-(-1-cos(x))) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_err, integ error sin(%.3g)", x);
  }
#endif

  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r = gsl_cheb_eval_n(csi, 25, x);
    gsl_test_abs(r, -(1+cos(x)), tol, "gsl_cheb_eval_n, integ sin(%.3g)", x);
  }

#ifdef TEST_INTEGRAL_ERR
  for(x=-M_PI; x<M_PI; x += M_PI/100.0) {
    double r, e;
    gsl_cheb_eval_n_err(csi, 25, x, &r, &e);
    gsl_test_abs(r, -(1+cos(x)), 100.0 * tol, "gsl_cheb_eval_n_err, integ sin(%.3g)", x);
    gsl_test_factor(fabs(r-(-1-cos(x))) + GSL_DBL_EPSILON, e, 10.0,
                    "gsl_cheb_eval_n_err, integ error sin(%.3g)", x);
  }
#endif

  gsl_cheb_free(csi);
  gsl_cheb_free(csd);
  gsl_cheb_free(cs);

  exit (gsl_test_summary());
}


/*
    This code derives from the R wavelets package and it is modified by Davide
    Albanese <albanese@fbk.it>.

    The Python interface is written by  Davide Albanese <albanese@fbk.it>.
    (C) 2009 mlpy Developers.

    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, see <http://www.gnu.org/licenses/>.
*/

#include <Python.h>
#include <numpy/arrayobject.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_wavelet.h>
#include <gsl/gsl_math.h>

#define SQRT_2 1.4142135623730951

void
uwt_forward (double *V, int n, int j,
	      double *h, double *g, int l,
	      double *Wj, double *Vj)
{
  int t, k, z;
  double k_div;

  for(t = 0; t < n; t++)
    {
      k = t;
      Wj[t] = h[0] * V[k];
      Vj[t] = g[0] * V[k];

      for(z = 1; z < l; z++)
	{
	  k -= (int) pow(2, (j - 1));

	  k_div = -k / (double) n;

	  if(k < 0)
	    k += (int) ceil(k_div) * n;

	  Wj[t] += h[z] * V[k];
	  Vj[t] += g[z] * V[k];
	}
    }
}

void
uwt_backward (double *W, double *V, int j,
	       int n, double *h, double *g,
	       int l, double *Vj)
{
  int t, k, z;
  double k_div;

  for(t = 0; t < n; t++)
    {
      k = t;
      Vj[t] = h[0] * W[k] + g[0] * V[k];

      for(z = 1; z < l; z++)
	{
	  k += (int) pow(2, (j - 1));
	  k_div = (double) k / (double) n;

	  if(k >= n)
	    k -= (int) floor(k_div) * n;

	  Vj[t] += h[z] * W[k] + g[z] * V[k];
	}
    }
}

static PyObject *uwt_uwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *x = NULL; PyObject *xa = NULL;
  PyObject *Xa = NULL;
  int levels = 0;
  char wf;

  int i, k, j, n, J;

  double *_x;
  double *_X;

  double *v;
  double *wj, *vj;
  double *h, *g;

  npy_intp Xa_dims[2];

  gsl_wavelet *wave;

  /* Parse Tuple*/
  static char *kwlist[] = {"x", "wf", "k", "levels", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci|i", kwlist, &x, &wf, &k, &levels))
    return NULL;

  xa = PyArray_FROM_OTF(x, NPY_DOUBLE, NPY_IN_ARRAY);
  if (xa == NULL) return NULL;

  n = (int) PyArray_DIM(xa, 0);
  _x = (double *) PyArray_DATA(xa);

  switch (wf)
    {
    case 'd':
      wave = gsl_wavelet_alloc (gsl_wavelet_daubechies_centered, k);
      break;

    case 'h':
      wave = gsl_wavelet_alloc (gsl_wavelet_haar_centered, k);
      break;

    case 'b':
      wave = gsl_wavelet_alloc (gsl_wavelet_bspline_centered, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  h = (double *) malloc(wave->nc * sizeof(double));
  g = (double *) malloc(wave->nc * sizeof(double));

  for(i=0; i<wave->nc; i++)
    {
      h[i] = wave->h1[i] / SQRT_2;
      g[i] = wave->g1[i] / SQRT_2;
    }

  if (levels == 0)
    J = (int) floor(log(((n-1) / (wave->nc-1)) + 1) / log(2));
  else
    J = levels;

  Xa_dims[0] = (npy_intp) (2 * J);
  Xa_dims[1] = PyArray_DIM(xa, 0);
  Xa = PyArray_SimpleNew(2, Xa_dims, NPY_DOUBLE);
  _X = (double *) PyArray_DATA(Xa);

  v = _x;
  for(j=0; j<J; j++)
    {
      wj = _X +(j * n);
      vj = _X +((j + J) * n);
      uwt_forward(v, n, j+1, g, h, wave->nc, wj,  vj);
      v = vj;
    }

  gsl_wavelet_free(wave);
  free(h);
  free(g);
  Py_DECREF(xa);

  return Py_BuildValue("N", Xa);
}

static PyObject *uwt_iuwt(PyObject *self, PyObject *args, PyObject *keywds)
{
  PyObject *X = NULL; PyObject *Xa = NULL;

  PyObject *xa = NULL;

  char wf;
  int i, k, n, J;
  double *w1, *v1;

  double *_X;
  double *_x;
  double *h, *g;

  npy_intp xa_dims[1];

  gsl_wavelet *wave;

  /* Parse Tuple*/
  static char *kwlist[] = {"X", "wf", "k", NULL};
  if (!PyArg_ParseTupleAndKeywords(args, keywds, "Oci", kwlist, &X, &wf, &k))
    return NULL;

  Xa = PyArray_FROM_OTF(X, NPY_DOUBLE, NPY_IN_ARRAY);
  if (Xa == NULL) return NULL;

  n = (int) PyArray_DIM(Xa, 1);
  J = ((int) PyArray_DIM(Xa, 0)) / 2;

  _X = (double *) PyArray_DATA(Xa);

  switch (wf)
    {
    case 'd':
      wave = gsl_wavelet_alloc (gsl_wavelet_daubechies, k);
      break;

    case 'h':
      wave = gsl_wavelet_alloc (gsl_wavelet_haar, k);
      break;

    case 'b':
      wave = gsl_wavelet_alloc (gsl_wavelet_bspline, k);
      break;

    default:
      PyErr_SetString(PyExc_ValueError, "wavelet family is not valid");
      return NULL;
    }

  h = (double *) malloc(wave->nc * sizeof(double));
  g = (double *) malloc(wave->nc * sizeof(double));

  for(i=0; i<wave->nc; i++)
    {
      h[i] = wave->h2[i] / SQRT_2;
      g[i] = wave->g2[i] / SQRT_2;
    }

  w1 = _X;
  v1 = _X + (J * n);

  xa_dims[0] = (npy_intp) n;
  xa = PyArray_SimpleNew(1, xa_dims, NPY_DOUBLE);
  _x = (double *) PyArray_DATA(xa);

  uwt_backward (w1, v1, 1, n, g, h, wave->nc, _x);

  gsl_wavelet_free(wave);
  free(h);
  free(g);
  Py_DECREF(Xa);

  return Py_BuildValue("N", xa);
}

/* Doc strings: */
static char module_doc[]  = "Undecimated Wavelet Transform Module";

static char uwt_uwt_doc[] =
  "Undecimated Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   x : 1d array_like object (the length is restricted to powers of two)\n"
  "      data\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet family\n"
  "   k : int\n"
  "      member of the wavelet family\n\n"
  "         * daubechies: k = 4, 6, ..., 20 with k even\n"
  "         * haar: the only valid choice of k is k = 2\n"
  "         * bspline: k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n\n"
  "   levels : int\n"
  "      level of the decomposition (J).\n"
  "      If levels = 0 this is the value J such that the length of X\n"
  "      is at least as great as the length of the level J wavelet filter,\n"
  "      but less than the length of the level J+1 wavelet filter.\n"
  "      Thus, j <= log_2((n-1)/(l-1)+1), where n is the length of x\n\n"
  ":Returns:\n"
  "   X : 2d numpy array (2J * len(x))\n"
  "      misaligned scaling and wavelet coefficients::\n\n"
  "         [[wavelet coefficients W_1]\n"
  "          [wavelet coefficients W_2]\n"
  "                        :\n"
  "          [wavelet coefficients W_J]\n"
  "          [scaling coefficients V_1]\n"
  "          [scaling coefficients V_2]\n"
  "                       :\n"
  "          [scaling coefficients V_J]]"
  ;

static char uwt_iuwt_doc[] =
  "Inverse Undecimated Wavelet Tranform\n\n"
  ":Parameters:\n"
  "   X : 2d array_like object (the length is restricted to powers of two)\n"
  "      misaligned scaling and wavelet coefficients\n"
  "   wf : string ('d': daubechies, 'h': haar, 'b': bspline)\n"
  "      wavelet family\n"
  "   k : int\n"
  "      member of the wavelet family\n\n"
  "         * daubechies: k = 4, 6, ..., 20 with k even\n"
  "         * haar: the only valid choice of k is k = 2\n"
  "         * bspline: k = 103, 105, 202, 204, 206, 208, 301, 303, 305 307, 309\n\n"
  ":Returns:\n"
  "   x : 1d numpy array\n"
  "      data"
  ;

/* Method table */
static PyMethodDef uwt_methods[] = {
  {"uwt",
   (PyCFunction)uwt_uwt,
   METH_VARARGS | METH_KEYWORDS,
   uwt_uwt_doc},
  {"iuwt",
   (PyCFunction)uwt_iuwt,
   METH_VARARGS | METH_KEYWORDS,
   uwt_iuwt_doc},
  {NULL, NULL, 0, NULL}
};

#if PY_MAJOR_VERSION >= 3

static struct PyModuleDef moduledef = {
  PyModuleDef_HEAD_INIT,
  "_uwt",
  module_doc,
  -1,
  uwt_methods,
  NULL, NULL, NULL, NULL
};

PyObject *PyInit__uwt(void)
{
  PyObject *m;
  m = PyModule_Create(&moduledef);
  if (!m) {
    return NULL;
  }

  import_array();

  return m;
}

#else

PyMODINIT_FUNC init_uwt(void)
{
  PyObject *m;

  m = Py_InitModule3("_uwt", uwt_methods, module_doc);
  if (m == NULL) {
    return;
  }

  import_array();
}

#endif


#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_math.h>

#include <math.h>
#include <options/options.h>

#include "../csm/csm_all.h"

struct ld_noise_params {
	int seed;

	double sigma_xy;
	double sigma_theta_deg;

	const char* file_input;
	const char* file_output;

	int debug;
};

const char * banner =
	"A simple program for adding slip to odometry \n\n"
		"The 'odometry' field is set to 'true_pose' + noise.\n"
		"If 'true_pose' is not available, then the 'odometry' \n"
		"field is set to 'odometry' + noise.\n\n"
		"Note: this program does *not* simulate the effect of \n"
		"slip or odometry error in a realistic way; each scan \n"
		"in the file is considered separately, the error does \n"
		"not depend on the traveled distance, etc.\n\n";

int main(int argc, const char ** argv) {
	sm_set_program_name(argv[0]);

	struct ld_noise_params p;

	options_banner(banner);

	struct option* ops = options_allocate(10);
	options_double(ops, "sigma_theta_deg", &p.sigma_theta_deg, 0.0,
		"Std deviation of gaussian noise for theta (deg) (disabled if 0)");
	options_double(ops, "sigma_xy", &p.sigma_xy, 0.0,
		"Std deviation of gaussian noise for x,y (disabled if 0)");
	options_int(ops, "seed", &p.seed, 0,
		"Seed for random number generator (if 0, use GSL_RNG_SEED env. variable).");
	options_string(ops, "in", &p.file_input, "stdin", "Input file ");
	options_string(ops, "out", &p.file_output, "stdout", "Output file ");

	options_int(ops, "debug", &p.debug, 0, "Shows debug information");

	if(!options_parse_args(ops, argc, argv)) {
		options_print_help(ops, stderr);
		return -1;
	}

	sm_debug_write(p.debug);

	gsl_rng_env_setup();
	gsl_rng * rng = gsl_rng_alloc (gsl_rng_ranlxs0);
	if(p.seed != 0)
	gsl_rng_set(rng, (unsigned int) p.seed);

	FILE * in = open_file_for_reading(p.file_input);
	if(!in) return -3;

	FILE * out = open_file_for_writing(p.file_output);
	if(!out) return -2;

	LDP ld; int count=0;
	while( (ld = ld_read_smart(in))) {
		count++;
		if(!ld_valid_fields(ld))  {
			sm_error("Invalid laser data (#%d in file)\n", count);
			continue;
		}

		if(!any_nan(ld->true_pose, 3))
			copy_d( (const double*) ld->true_pose, 3, ld->odometry);

		double e[3] = {0,0,0};

		if(p.sigma_xy > 0) {
			e[0]  = gsl_ran_gaussian(rng, p.sigma_xy);
			e[1]  = gsl_ran_gaussian(rng, p.sigma_xy);
		}

		if(p.sigma_theta_deg > 0) {
			e[2] = gsl_ran_gaussian(rng, deg2rad(p.sigma_theta_deg));
		}

		ld->odometry[0] += e[0];
		ld->odometry[1] += e[1];
		ld->odometry[2] += e[2];

		sm_debug("Adding noise %s.\n", friendly_pose(e));

		ld_write_as_json(ld, out);

		ld_free(ld);
	}

	return 0;
}