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

#ifndef __GSL_EIGEN_H__
#define __GSL_EIGEN_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

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

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef struct {
  size_t size;
  double * d;
  double * sd;
} gsl_eigen_symm_workspace;

GSL_FUN gsl_eigen_symm_workspace * gsl_eigen_symm_alloc (const size_t n);
GSL_FUN void gsl_eigen_symm_free (gsl_eigen_symm_workspace * w);
GSL_FUN int gsl_eigen_symm (gsl_matrix * A, gsl_vector * eval, gsl_eigen_symm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * gc;
  double * gs;
} gsl_eigen_symmv_workspace;

GSL_FUN gsl_eigen_symmv_workspace * gsl_eigen_symmv_alloc (const size_t n);
GSL_FUN void gsl_eigen_symmv_free (gsl_eigen_symmv_workspace * w);
GSL_FUN int gsl_eigen_symmv (gsl_matrix * A, gsl_vector * eval, gsl_matrix * evec, gsl_eigen_symmv_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
} gsl_eigen_herm_workspace;

GSL_FUN gsl_eigen_herm_workspace * gsl_eigen_herm_alloc (const size_t n);
GSL_FUN void gsl_eigen_herm_free (gsl_eigen_herm_workspace * w);
GSL_FUN int gsl_eigen_herm (gsl_matrix_complex * A, gsl_vector * eval,
                         gsl_eigen_herm_workspace * w);

typedef struct {
  size_t size;
  double * d;
  double * sd;
  double * tau;
  double * gc;
  double * gs;
} gsl_eigen_hermv_workspace;

GSL_FUN gsl_eigen_hermv_workspace * gsl_eigen_hermv_alloc (const size_t n);
GSL_FUN void gsl_eigen_hermv_free (gsl_eigen_hermv_workspace * w);
GSL_FUN int gsl_eigen_hermv (gsl_matrix_complex * A, gsl_vector * eval,
                           gsl_matrix_complex * evec,
                           gsl_eigen_hermv_workspace * w);

typedef struct {
  size_t size;           /* matrix size */
  size_t max_iterations; /* max iterations since last eigenvalue found */
  size_t n_iter;         /* number of iterations since last eigenvalue found */
  size_t n_evals;        /* number of eigenvalues found so far */

  int compute_t;         /* compute Schur form T = Z^t A Z */

  gsl_matrix *H;         /* pointer to Hessenberg matrix */
  gsl_matrix *Z;         /* pointer to Schur vector matrix */
} gsl_eigen_francis_workspace;

GSL_FUN gsl_eigen_francis_workspace * gsl_eigen_francis_alloc (void);
GSL_FUN void gsl_eigen_francis_free (gsl_eigen_francis_workspace * w);
GSL_FUN void gsl_eigen_francis_T (const int compute_t,
                          gsl_eigen_francis_workspace * w);
GSL_FUN int gsl_eigen_francis (gsl_matrix * H, gsl_vector_complex * eval,
                       gsl_eigen_francis_workspace * w);
GSL_FUN int gsl_eigen_francis_Z (gsl_matrix * H, gsl_vector_complex * eval,
                         gsl_matrix * Z,
                         gsl_eigen_francis_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *diag;            /* diagonal matrix elements from balancing */
  gsl_vector *tau;             /* Householder coefficients */
  gsl_matrix *Z;               /* pointer to Z matrix */
  int do_balance;              /* perform balancing transformation? */
  size_t n_evals;              /* number of eigenvalues found */

  gsl_eigen_francis_workspace *francis_workspace_p;
} gsl_eigen_nonsymm_workspace;

GSL_FUN gsl_eigen_nonsymm_workspace * gsl_eigen_nonsymm_alloc (const size_t n);
GSL_FUN void gsl_eigen_nonsymm_free (gsl_eigen_nonsymm_workspace * w);
GSL_FUN void gsl_eigen_nonsymm_params (const int compute_t, const int balance,
                               gsl_eigen_nonsymm_workspace *w);
GSL_FUN int gsl_eigen_nonsymm (gsl_matrix * A, gsl_vector_complex * eval,
                       gsl_eigen_nonsymm_workspace * w);
GSL_FUN int gsl_eigen_nonsymm_Z (gsl_matrix * A, gsl_vector_complex * eval,
                         gsl_matrix * Z, gsl_eigen_nonsymm_workspace * w);

typedef struct {
  size_t size;                 /* size of matrices */
  gsl_vector *work;            /* scratch workspace */
  gsl_vector *work2;           /* scratch workspace */
  gsl_vector *work3;           /* scratch workspace */

  gsl_matrix *Z;               /* pointer to Schur vectors */

  gsl_eigen_nonsymm_workspace *nonsymm_workspace_p;
} gsl_eigen_nonsymmv_workspace;

GSL_FUN gsl_eigen_nonsymmv_workspace * gsl_eigen_nonsymmv_alloc (const size_t n);
GSL_FUN void gsl_eigen_nonsymmv_free (gsl_eigen_nonsymmv_workspace * w);
GSL_FUN void gsl_eigen_nonsymmv_params (const int balance,
                                gsl_eigen_nonsymmv_workspace *w);
GSL_FUN int gsl_eigen_nonsymmv (gsl_matrix * A, gsl_vector_complex * eval,
                        gsl_matrix_complex * evec,
                        gsl_eigen_nonsymmv_workspace * w);
GSL_FUN int gsl_eigen_nonsymmv_Z (gsl_matrix * A, gsl_vector_complex * eval,
                          gsl_matrix_complex * evec, gsl_matrix * Z,
                          gsl_eigen_nonsymmv_workspace * w);

typedef struct {
  size_t size;            /* size of matrices */
  gsl_eigen_symm_workspace *symm_workspace_p;
} gsl_eigen_gensymm_workspace;

GSL_FUN gsl_eigen_gensymm_workspace * gsl_eigen_gensymm_alloc (const size_t n);
GSL_FUN void gsl_eigen_gensymm_free (gsl_eigen_gensymm_workspace * w);
GSL_FUN int gsl_eigen_gensymm (gsl_matrix * A, gsl_matrix * B,
                       gsl_vector * eval, gsl_eigen_gensymm_workspace * w);
GSL_FUN int gsl_eigen_gensymm_standardize (gsl_matrix * A, const gsl_matrix * B);

typedef struct {
  size_t size;            /* size of matrices */
  gsl_eigen_symmv_workspace *symmv_workspace_p;
} gsl_eigen_gensymmv_workspace;

GSL_FUN gsl_eigen_gensymmv_workspace * gsl_eigen_gensymmv_alloc (const size_t n);
GSL_FUN void gsl_eigen_gensymmv_free (gsl_eigen_gensymmv_workspace * w);
GSL_FUN int gsl_eigen_gensymmv (gsl_matrix * A, gsl_matrix * B,
                        gsl_vector * eval, gsl_matrix * evec,
                        gsl_eigen_gensymmv_workspace * w);

typedef struct {
  size_t size;            /* size of matrices */
  gsl_eigen_herm_workspace *herm_workspace_p;
} gsl_eigen_genherm_workspace;

GSL_FUN gsl_eigen_genherm_workspace * gsl_eigen_genherm_alloc (const size_t n);
GSL_FUN void gsl_eigen_genherm_free (gsl_eigen_genherm_workspace * w);
GSL_FUN int gsl_eigen_genherm (gsl_matrix_complex * A, gsl_matrix_complex * B,
                       gsl_vector * eval, gsl_eigen_genherm_workspace * w);
GSL_FUN int gsl_eigen_genherm_standardize (gsl_matrix_complex * A,
                                   const gsl_matrix_complex * B);

typedef struct {
  size_t size;            /* size of matrices */
  gsl_eigen_hermv_workspace *hermv_workspace_p;
} gsl_eigen_genhermv_workspace;

GSL_FUN gsl_eigen_genhermv_workspace * gsl_eigen_genhermv_alloc (const size_t n);
GSL_FUN void gsl_eigen_genhermv_free (gsl_eigen_genhermv_workspace * w);
GSL_FUN int gsl_eigen_genhermv (gsl_matrix_complex * A, gsl_matrix_complex * B,
                        gsl_vector * eval, gsl_matrix_complex * evec,
                        gsl_eigen_genhermv_workspace * w);

typedef struct {
  size_t size;            /* size of matrices */
  gsl_vector *work;       /* scratch workspace */

  size_t n_evals;         /* number of eigenvalues found */
  size_t max_iterations;  /* maximum QZ iterations allowed */
  size_t n_iter;          /* number of iterations since last eigenvalue found */
  double eshift;          /* exceptional shift counter */

  int needtop;            /* need to compute top index? */

  double atol;            /* tolerance for splitting A matrix */
  double btol;            /* tolerance for splitting B matrix */

  double ascale;          /* scaling factor for shifts */
  double bscale;          /* scaling factor for shifts */

  gsl_matrix *H;          /* pointer to hessenberg matrix */
  gsl_matrix *R;          /* pointer to upper triangular matrix */

  int compute_s;          /* compute generalized Schur form S */
  int compute_t;          /* compute generalized Schur form T */

  gsl_matrix *Q;          /* pointer to left Schur vectors */
  gsl_matrix *Z;          /* pointer to right Schur vectors */
} gsl_eigen_gen_workspace;

GSL_FUN gsl_eigen_gen_workspace * gsl_eigen_gen_alloc (const size_t n);
GSL_FUN void gsl_eigen_gen_free (gsl_eigen_gen_workspace * w);
GSL_FUN void gsl_eigen_gen_params (const int compute_s, const int compute_t,
                           const int balance, gsl_eigen_gen_workspace * w);
GSL_FUN int gsl_eigen_gen (gsl_matrix * A, gsl_matrix * B,
                   gsl_vector_complex * alpha, gsl_vector * beta,
                   gsl_eigen_gen_workspace * w);
GSL_FUN int gsl_eigen_gen_QZ (gsl_matrix * A, gsl_matrix * B,
                      gsl_vector_complex * alpha, gsl_vector * beta,
                      gsl_matrix * Q, gsl_matrix * Z,
                      gsl_eigen_gen_workspace * w);

typedef struct {
  size_t size;            /* size of matrices */

  gsl_vector *work1;      /* 1-norm of columns of A */
  gsl_vector *work2;      /* 1-norm of columns of B */
  gsl_vector *work3;      /* real part of eigenvector */
  gsl_vector *work4;      /* imag part of eigenvector */
  gsl_vector *work5;      /* real part of back-transformed eigenvector */
  gsl_vector *work6;      /* imag part of back-transformed eigenvector */

  gsl_matrix *Q;          /* pointer to left Schur vectors */
  gsl_matrix *Z;          /* pointer to right Schur vectors */

  gsl_eigen_gen_workspace *gen_workspace_p;
} gsl_eigen_genv_workspace;

GSL_FUN gsl_eigen_genv_workspace * gsl_eigen_genv_alloc (const size_t n);
GSL_FUN void gsl_eigen_genv_free (gsl_eigen_genv_workspace * w);
GSL_FUN int gsl_eigen_genv (gsl_matrix * A, gsl_matrix * B,
                    gsl_vector_complex * alpha, gsl_vector * beta,
                    gsl_matrix_complex * evec,
                    gsl_eigen_genv_workspace * w);
GSL_FUN int gsl_eigen_genv_QZ (gsl_matrix * A, gsl_matrix * B,
                       gsl_vector_complex * alpha, gsl_vector * beta,
                       gsl_matrix_complex * evec,
                       gsl_matrix * Q, gsl_matrix * Z,
                       gsl_eigen_genv_workspace * w);

typedef enum {
  GSL_EIGEN_SORT_VAL_ASC,
  GSL_EIGEN_SORT_VAL_DESC,
  GSL_EIGEN_SORT_ABS_ASC,
  GSL_EIGEN_SORT_ABS_DESC
}
gsl_eigen_sort_t;

/* Sort eigensystem results based on eigenvalues.
 * Sorts in order of increasing value or increasing
 * absolute value.
 *
 * exceptions: GSL_EBADLEN
 */

GSL_FUN int gsl_eigen_symmv_sort(gsl_vector * eval, gsl_matrix * evec,
                         gsl_eigen_sort_t sort_type);

GSL_FUN int gsl_eigen_hermv_sort(gsl_vector * eval, gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

GSL_FUN int gsl_eigen_nonsymmv_sort(gsl_vector_complex * eval,
                            gsl_matrix_complex * evec,
                            gsl_eigen_sort_t sort_type);

GSL_FUN int gsl_eigen_gensymmv_sort (gsl_vector * eval, gsl_matrix * evec,
                             gsl_eigen_sort_t sort_type);

GSL_FUN int gsl_eigen_genhermv_sort (gsl_vector * eval, gsl_matrix_complex * evec,
                             gsl_eigen_sort_t sort_type);

GSL_FUN int gsl_eigen_genv_sort (gsl_vector_complex * alpha, gsl_vector * beta,
                         gsl_matrix_complex * evec,
                         gsl_eigen_sort_t sort_type);

/* Prototypes for the schur module */

GSL_FUN int gsl_schur_gen_eigvals(const gsl_matrix *A, const gsl_matrix *B,
                          double *wr1, double *wr2, double *wi,
                          double *scale1, double *scale2);

GSL_FUN int gsl_schur_solve_equation(double ca, const gsl_matrix *A, double z,
                             double d1, double d2, const gsl_vector *b,
                             gsl_vector *x, double *s, double *xnorm,
                             double smin);

GSL_FUN int gsl_schur_solve_equation_z(double ca, const gsl_matrix *A,
                               gsl_complex *z, double d1, double d2,
                               const gsl_vector_complex *b,
                               gsl_vector_complex *x, double *s,
                               double *xnorm, double smin);

/* The following functions are obsolete: */

/* Eigensolve by Jacobi Method
 *
 * The data in the matrix input is destroyed.
 *
 * exceptions:
 */
GSL_FUN int
gsl_eigen_jacobi(gsl_matrix * matrix,
                      gsl_vector * eval,
                      gsl_matrix * evec,
                      unsigned int max_rot,
                      unsigned int * nrot);

/* Invert by Jacobi Method
 *
 * exceptions:
 */
GSL_FUN int
gsl_eigen_invert_jacobi(const gsl_matrix * matrix,
                             gsl_matrix * ainv,
                             unsigned int max_rot);

__END_DECLS

#endif /* __GSL_EIGEN_H__ */


/*
* -----------------------------------------------------------------
*  ell_lib.h
*  Ellipsoid Library
*  Version: 2.0
*  Last Update: July 22, 2020
*
*  Programmer: Americo Barbosa da Cunha Junior
*              americo.cunhajr@gmail.com
* -----------------------------------------------------------------
*  Copyright (c) 2010-2019, Americo Barbosa da Cunha Junior
*  All rights reserved.
* -----------------------------------------------------------------
*  This is the header file for BST_LIB module, a computational
*  library to work with n-dimensional ellipsoids.
* -----------------------------------------------------------------
*/

#ifndef __ELL_LIB_H__
#define __ELL_LIB_H__

#include <stdlib.h>
#include <gsl/gsl_linalg.h>

#undef  ELL_TOL
#define ELL_TOL 1.0e-7

#undef  ELL_TRUE
#define ELL_TRUE 1

#undef  ELL_FALSE
#define ELL_FALSE 0

/*
*------------------------------------------------------------
* function prototypes
*------------------------------------------------------------
*/

void ell_psd2eig(gsl_matrix *B,
                 gsl_matrix *V,
                 gsl_vector *lam);

void ell_psd2chol(gsl_matrix *B,
                  gsl_matrix *L);

void ell_eig2chol(gsl_matrix *V,
                  gsl_vector *sig,
                  gsl_matrix *L);

unsigned int ell_pt_in(gsl_vector *x,
                       gsl_vector *c,
                       gsl_matrix *L);

void ell_map2uhs(gsl_vector *x,
                 gsl_vector *c,
                 gsl_matrix *L,
                 gsl_vector *y);

void ell_pt_modify(gsl_vector *p,
                   gsl_vector *c,
                   gsl_matrix *L);

#endif /* __ELL_LIB_H__ */


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

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_mode.h>
#include "bessel.h"
#include "bessel_amp_phase.h"
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_bessel.h>

#include "cheb_eval.c"

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

/* based on SLATEC besj0, 1977 version, w. fullerton */

/* chebyshev expansions for Bessel functions

 series for bj0        on the interval  0.          to  1.60000d+01
                                        with weighted error   7.47e-18
                                         log weighted error  17.13
                               significant figures required  16.98
                                    decimal places required  17.68

*/

static double bj0_data[13] = {
   0.100254161968939137,
  -0.665223007764405132,
   0.248983703498281314,
  -0.0332527231700357697,
   0.0023114179304694015,
  -0.0000991127741995080,
   0.0000028916708643998,
  -0.0000000612108586630,
   0.0000000009838650793,
  -0.0000000000124235515,
   0.0000000000001265433,
  -0.0000000000000010619,
   0.0000000000000000074,
};
static cheb_series bj0_cs = {
  bj0_data,
  12,
  -1, 1,
  9
};

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

int gsl_sf_bessel_J0_e(const double x, gsl_sf_result * result)
{
  double y = fabs(x);

  /* CHECK_POINTER(result) */

  if(y < 2.0*GSL_SQRT_DBL_EPSILON) {
    result->val = 1.0;
    result->err = y*y;
    return GSL_SUCCESS;
  }
  else if(y <= 4.0) {
    return cheb_eval_e(&bj0_cs, 0.125*y*y - 1.0, result);
  }
  else {
    const double z = 32.0/(y*y) - 1.0;
    gsl_sf_result ca;
    gsl_sf_result ct;
    gsl_sf_result cp;
    const int stat_ca = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bm0_cs,  z, &ca);
    const int stat_ct = cheb_eval_e(&_gsl_sf_bessel_amp_phase_bth0_cs, z, &ct);
    const int stat_cp = gsl_sf_bessel_cos_pi4_e(y, ct.val/y, &cp);
    const double sqrty = sqrt(y);
    const double ampl  = (0.75 + ca.val) / sqrty;
    result->val  = ampl * cp.val;
    result->err  = fabs(cp.val) * ca.err/sqrty + fabs(ampl) * cp.err;
    result->err += GSL_DBL_EPSILON * fabs(result->val);
    return GSL_ERROR_SELECT_3(stat_ca, stat_ct, stat_cp);
  }
}

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

#include "eval.h"

double gsl_sf_bessel_J0(const double x)
{
  EVAL_RESULT(gsl_sf_bessel_J0_e(x, &result));
}


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

#include "../inc/knnring.h"

knnresult kNN(double *X, double *Y, int n, int m, int d, int k){

	knnresult k_nearest;

	//initalize the result's variable
	k_nearest.nidx  = (int *)malloc(m*k*sizeof(int));
	k_nearest.ndist = (double *)malloc(m*k*sizeof(double));
	k_nearest.m = m;
	k_nearest.k = k;

  //compute the distance Matrix D
	double *D = compute_D(X,Y,n,m,d,k);

  //select k-nearest neighbors for every point in QUERY
	k_select(D,k_nearest.ndist, k_nearest.nidx,n,m,k);
	return k_nearest;
}

double *compute_D(double *X, double *Y, int n, int m, int d, int k){

	//compute distances using
	//D = (X*X)ee_t - 2XY_t + ee_t(Y*Y)_t
	double *tempD   = (double *)malloc(n*m*sizeof(double));
	double *e_1     = (double *)malloc(d*m*sizeof(double));
	double *XX      = (double *)malloc(n*d*sizeof(double));

	for(int i=0;i<d*m;i++)
		e_1[i]=1;

	for(int i=0;i<n*d;i++)
		XX[i] = X[i]*X[i];

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, n,m,d,1,XX,d,e_1,m,0,tempD,m);

	free(e_1);
	free(XX);

	double *e_2 = (double *)malloc(n*d*sizeof(double));
	double *YY = (double *)malloc(m*d*sizeof(double));

	for(int i=0;i<n*d;i++)
		e_2[i]=1;

	for(int i=0;i<m*d;i++)
		YY[i] = Y[i]*Y[i];

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n,m,d,-2,X,d,Y,d,1,tempD,m);
	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, n,m,d,1,e_2,d,YY,d,1,tempD,m);

	for(int i=0;i<n*m;i++)
		tempD[i] = sqrt(fabs(tempD[i]));

	free(e_2);
	free(YY);

	return tempD;
}

void k_select(double *D, double *ndist, int *nidx,int n, int m, int k){
	for(int j=0;j<m;j++){
		for(int i=0;i<k;i++){
			ndist[j*k+i] = D[j+i*m];
			nidx[j*k+i]  = i;
		}
		//sort the initial k neighbors
		quick_sort(ndist,nidx,j*k,k*(j+1)-1);
	}

	//search the rest points to find it there are closer neighbors
	for(int j=0;j<m;j++){
		for(int i=k;i<n;i++){
			if(D[j+i*m] < ndist[k*(j+1)-1]){
				ndist[k*(j+1)-1] = D[j+i*m];
				nidx[k*(j+1)-1] = i;
				quick_sort(ndist,nidx,j*k,(j+1)*k-1);
			}
		}
	}
}

void quick_sort(double *k_dist, int *k_neigh, int low, int high){
	if(low<high){

		//actuall sorting an element
		int pi = partition(k_dist,k_neigh,low,high);

		//recursivelly call quick_sort
		quick_sort(k_dist,k_neigh,low,pi-1);
		quick_sort(k_dist,k_neigh,pi+1,high);
	}
}

int partition(double *k_dist, int *k_neigh,int low, int high){

	double temp_dist;
	int temp_point;
	int checker = 0;

	double pivot = k_dist[high];
	//printf("the pivot is %lf \n",pivot);
	int i = (low-1);
	for(int j=low;j<=high-1;j++){
		if(k_dist[j]<=pivot){
			/*swap the two elements*/

			i=i+1;
			temp_dist  = k_dist[i];
			temp_point = k_neigh[i];

			k_dist[i]  = k_dist[j];
			k_neigh[i] = k_neigh[j];

			k_dist[j]  = temp_dist;
			k_neigh[j] = temp_point;
			checker = 1;
		}
	}

	/* swap the pivot */
	i=i+1;
	temp_dist  = k_dist[i];
	temp_point = k_neigh[i];

	k_dist[i]  = k_dist[high];
	k_neigh[i] = k_neigh[high];

	k_dist[high]  = temp_dist;
	k_neigh[high] = temp_point;

	return i;
}

void write_to_file(char *str1, int *nidx, double *ndist, int amount, int k){
  FILE *fp;
  //file for indexes
  fp = fopen(str1,"w+");

  int counter = 1;
  for(int i=0;i<amount;i++){
    fprintf(fp,"%d)%d -- %lf\n",counter,nidx[i],ndist[i]);
    counter ++;
		if((i+1)%k == 0){
      fprintf(fp,"k nearest neighbors for next point \n");
      counter = 1;
    }
  }
	fclose(fp);
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <string.h>

//** Init Multinomial
gsl_rng* init_gsl_rng(){
		gsl_rng* r;
 		const gsl_rng_type* T2;

    srand(time(NULL));
    unsigned int seed;
    seed= rand();

    gsl_rng_env_setup();

    T2 = gsl_rng_default;
    r = gsl_rng_alloc (T2);
    gsl_rng_set (r, seed);

		return r;
}

unsigned int substract(unsigned int val){
	if(val>0)
		return val-1;
	else
		return 0;
}

//** Data must be dictionated
//** Doc-Word-topic
void dt2b_inference(int** data , unsigned int** uKu, unsigned  int** pKp, unsigned  int** KuKp, const int it, const int Ku,const int Kp, const int nusers, const int nplaces, const int nrecs){

	float l,prev_l,prob=0;

 	//** Allocate Dynamic Tables
	unsigned int zu[Ku];
	memset( zu, 0, Ku*sizeof(unsigned int) );
	unsigned int zp[Kp];
	memset( zp, 0, Kp*sizeof(unsigned int) );
	double p_zuzp[Ku][Kp];

  //** Initialization
	gsl_rng * r;
	r=init_gsl_rng();

	//** Initialize Dynamic Tables
	unsigned int oneutopic,oneptopic;
	size_t rowid;
	for(rowid=0;rowid<nrecs;rowid++){
			oneutopic=data[rowid][2];
			oneptopic=data[rowid][3];
			zu[(size_t)oneutopic]++;
			zp[(size_t)oneptopic]++;
	}

	//** Iterate
	size_t i,s,kp,ku;
	int curr_u,curr_p,curr_ku,curr_kp;
	for(i=0;i<it;i++){
		l=0;
		//printf("iteration %zu\n",i);
		for(rowid=0;rowid<nrecs;rowid++){

			//** Discount
			curr_u=data[rowid][0];
			curr_p=data[rowid][1];
			curr_ku=data[rowid][2];
			curr_kp=data[rowid][3];

			uKu[curr_u][curr_ku]=substract(uKu[curr_u][curr_ku]);
			pKp[curr_p][curr_kp]=substract(pKp[curr_p][curr_kp]);
			KuKp[curr_ku][curr_kp]=substract(KuKp[curr_ku][curr_kp]);
			zu[curr_ku]=substract(zu[curr_ku]);
			zp[curr_kp]=substract(zp[curr_kp]);

			//** Obtain Posterior
			prob=0;
			for(ku=0;ku<Ku;ku++){
				for(kp=0;kp<Kp;kp++){
					p_zuzp[ku][kp]=((KuKp[ku][kp]+0.5)*(uKu[curr_u][ku]+0.5)*(pKp[curr_p][kp]+0.5))/((zu[ku]+0.5*nusers)*(zp[kp]+0.5*nplaces));
					prob+=p_zuzp[ku][kp];
				}
			}
			l+=log(prob);

			//** Obtain one sample
    	unsigned int samples[Ku*Kp];
    	double* p_z = (double*) p_zuzp;

   		gsl_ran_multinomial(r, Ku*Kp, 1, p_z, samples);

 			size_t chosen_ku,chosen_kp;
 			for(s=0;s<Ku*Kp;s++){

 				if (samples[s]>0){

 					//get two indexes from one
 					chosen_ku=(unsigned int) (s/Kp);
 					chosen_kp=(unsigned int) (s%Kp);

 					break;
 				}
 			}

 			//** Update
 			data[rowid][2]=chosen_ku;
 			data[rowid][3]=chosen_kp;
 			uKu[curr_u][chosen_ku]+=1;
 			pKp[curr_p][chosen_kp]+=1;
 			KuKp[chosen_ku][chosen_kp]+=1;
 			zu[chosen_ku]+=1;
 			zp[chosen_kp]+=1;

		}

	if (i==0)
		prev_l=l;

	//** Check convergence
		if (l<prev_l && i>50){
			printf("Stopped at iteration %zu",i);
			break;
		}

		printf("Likelihood at iteration %zu: %f \n",i,l);

	}

	gsl_rng_free (r);
}


#include<stdio.h>
#include<math.h>
#include"recur.c"
#include <gsl/gsl_integration.h>
#include"gsl_integration.c"
#define NMAX 100
#include"timer.h"
#include"timer.c"
#include"adjust.c"

/* The integral is evaluated for n=0 to n=100 using both a derived recursion formula
 * and the GSL general integrator. The values of both are stored in their own
 * respective arrays. The values and their absolute error are printed in order to
 * compare the accuracy of the two methods. Then, both functions are timed to
 * determine which one is faster at generating the results. Finally, the timings of
 * the two functions are divided to provide a ratio describing the difference in
 * efficiency of the two methods.
*/
int main (void)
{
    double vals1[NMAX + 1], vals2[NMAX + 1], time1a, time1b, time2a, time2b, tmin = 1., tmax = 2.;      // Initializing all the "double" variables for use in the program//
    int nmin = 0, nmax = NMAX, count1 = 1000, count2 = 800;

    integral_recur (nmin, nmax, vals1); // Recursion formula stores I(0) to I(100) in the array vals1//
    integral_gen (nmin, nmax, vals2);   // GSL integration algorithm stores I(0) to I(100) in the array vals2//

    printf
        ("      --n--       --Recur--        --GSL Integrator--   --Absolute Error*10^9--\n");
    int k;

    for (k = 0; k < 101; k++)
    {
        printf ("   %6d     %15.15f     %15.15f      %15.15f \n", k, vals1[k],
            vals2[k], fabs (1.e9 * (vals1[k] - vals2[k])));
    }                           // Prints the values for each term in the sequence as generated by both of the algorithms //
    int p;

    printf
        ("\n--Timing for recurrence function--\nTime per call (usec) Total Time (sec)      Count\n");
    do
    {
        timer_start ();         // The timer is started.
        for (p = 0; p < count1; p++)    // The function is called "count1" times.
        {
            integral_recur (nmin, nmax, vals1); // The recursion formula stores each of the sequence values in the array vals1//
        }
        time1a = timer_stop (); // The timer is stopped.
        time1b = time1a / count1;       // The time per function call is calculated as the total time divided by the number of function calls.
        printf (" %10.2f usec     %10.6f sec    %10d\n", time1b * 1.e6,
            time1a, count1);
        count1 = adjust_rep_count (count1, time1a, tmin, tmax); // The number of function calls "count1" is adjusted.
    }
    while ((time1a > tmax) || (time1a < tmin)); // This is repeated until the total time is within the desired range.
    printf ("Time per recur function call = %f usec \n", time1b * 1.e6);
    printf
        ("\n--Timing for GSL integration function--\nTime per call (usec) Total Time (sec)      Count\n");
    do
    {
        timer_start ();         // The timer is started.
        for (p = 0; p < count2; p++)    // The function is called "count2" times.
        {
            integral_gen (nmin, nmax, vals2);   // The GSL integration formula stores each of the sequence values in the array vals1//
        }
        time2a = timer_stop (); // The timer is stopped.
        time2b = time2a / count2;       // The time per function call is calculated as the total time divided by the number of function calls.
        printf (" %10.2f usec     %10.6f sec    %10d\n", time2b * 1.e6,
            time2a, count2);
        count2 = adjust_rep_count (count2, time2a, tmin, tmax); // The number of function calls "count2" is adjusted.
    }
    while ((time2a > tmax) || (time2a < tmin)); // This is repeated until the total time is within the desired range.
    printf ("Time per GSL function call = %f usec \n", time2b * 1.e6);

    printf ("Ratio of times Tgsl / Trecur = %f\n", time2b / time1b);    // The ratio of times per function call of each function is printed//
    return 0;
}


/**
 * @brief handle cosmological functions like power spectrum, growth, etc.
 *
 * @file core_power.h
 * @author Michal Vrastil
 * @date 2018-06-21
 */

#pragma once
#include "precision.hpp"
#include <gsl/gsl_spline.h>

/*******************//**
* FORWARD DECLARATIONS *
************************/

class Cosmo_Param; ///< declaration in params.hpp
class Sim_Param; ///< declaration in params.hpp
template <typename T, size_t N> class Data_Vec; ///< declaration in class_data_vec.hpp

/**
 * @brief Initialize CCL power spectrum
 *
 * @param cosmo cosmological parameters
 */
void norm_pwr(Cosmo_Param& cosmo);

/**
 * @brief when computing growth factor outside CCL range we need to normalize the growth factor; \f$D(a=1)\equiv1\f$
 *
 * @param cosmo cosmological parameters
 * @return FTYPE_t unnormalized growth factor at 'a = 1'
 */
FTYPE_t norm_growth_factor(const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t growth_factor(FTYPE_t a, const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t growth_rate(FTYPE_t a, const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t growth_change(FTYPE_t a, const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t Omega_lambda(FTYPE_t a, const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param k
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t lin_pow_spec(FTYPE_t a, FTYPE_t k, const Cosmo_Param& cosmo);

/**
 * @brief
 *
 * @param a
 * @param k
 * @param cosmo cosmological parameters
 * @return FTYPE_t
 */
FTYPE_t non_lin_pow_spec(FTYPE_t a, FTYPE_t k, const Cosmo_Param& cosmo);

/**
 * @class:	Interp_obj
 * @brief:	linear interpolation (Steffen) of data [x, y]
 */
class Interp_obj
{
public:
    Interp_obj(): is_init(false) {}
    ~Interp_obj();
    double operator()(double x) const;
    template <typename T, size_t N>
    void init(const Data_Vec<T, N>& data);
    double x_min, x_max;

private:
    bool is_init;
    gsl_spline* spline;
    gsl_interp_accel* acc;
};

/**
 * @class:	Extrap_Pk
 * @brief:	linear interpolation of data [k, P(k)] within 'useful' range
 *          fit to primordial P_i(k) below the 'useful' range
 *          fit to Padé approximant R [0/3] above the 'useful' range
 *
 *  Steffen interpolation of data [k, P(k)] within range k_min, k_max
 *  fit to primordial P_i(k) below this range, fit A*k^ns above
 */
template <typename T, size_t N>
class Extrap_Pk : public Interp_obj
{
public:
    Extrap_Pk(const Data_Vec<T, N>& data, const Sim_Param& sim);
    Extrap_Pk(const Data_Vec<T, N>& data, const Sim_Param& sim, const size_t m_l, const size_t n_u);
    Extrap_Pk(const Data_Vec<T, N>& data, const Sim_Param& sim, const size_t m_l, const size_t n_l,
              const size_t m_u, const size_t n_u);
    double operator()(double k) const;

    void fit_lin(const Data_Vec<T, N>& data, const size_t m, const size_t n, double& A);
    void fit_power_law(const Data_Vec<T, N>& data, const size_t m, const size_t n, double& A, double& n_s);

    double A_low; ///< amplitude of linear power in lower range
    const Cosmo_Param& cosmo;
    double A_up, n_s; ///< scale-free power spectrum in upper range
    T k_min, k_max; ///< interpolation range
};

/**
 * @class:	Extrap_Pk_Nl
 * @brief:	creates Extrapolate object (linear power spectrum) from data and store non-linear parameters
            call 'operator()(k)' based on k_split (upper range of the linear)
 */
template <typename T, size_t N>
class Extrap_Pk_Nl : public Extrap_Pk<T, N>
{
public:
    Extrap_Pk_Nl(const Data_Vec<T, N>& data, const Sim_Param &sim, T A_nl, T a_eff);
    const T A_nl, a_eff, k_split;
    double operator()(double k) const;
};

/**
 * @brief compute correlation function and store results
 *
 * @tparam P callable object with 'operator()(double)'
 * @param sim simulation parameters
 * @param P_k power spectrum (callable)
 * @param corr_func_binned object to store binned correlation function
 */
template<class P>
void gen_corr_func_binned_gsl_qawf(const Sim_Param &sim, const P& P_k, Data_Vec<FTYPE_t, 2>& corr_func_binned);

/**
 * @brief compute linear correlation function and store results
 *
 * @param sim simulation parameters
 * @param a scale factor
 * @param corr_func_binned object to store binned correlation function
 */
void gen_corr_func_binned_gsl_qawf_lin(const Sim_Param &sim, FTYPE_t a, Data_Vec<FTYPE_t, 2>& corr_func_binned);

/**
 * @brief compute non-linear correlation function and store results
 *
 * @param sim simulation parameters
 * @param a scale factor
 * @param corr_func_binned object to store binned correlation function
 */
void gen_corr_func_binned_gsl_qawf_nl(const Sim_Param &sim, FTYPE_t a, Data_Vec<FTYPE_t, 2>& corr_func_binned);

/**
 * @brief compute amplitude of density fluctuation and store results
 *
 * @tparam P callable object with 'operator()(double)'
 * @param sim simulation parameters
 * @param P_k power spectrum (callable)
 * @param sigma_binned object to store binned correlation function
 */
template<class P>
void gen_sigma_binned_gsl_qawf(const Sim_Param &sim, const P& P_k, Data_Vec<FTYPE_t, 2>& sigma_binned);

/**
 * @brief compute linear amplitude of density fluctuation and store results
 *
 * @param sim simulation parameters
 * @param a scale factor
 * @param sigma_binned object to store binned correlation function
 */
void gen_sigma_func_binned_gsl_qawf_lin(const Sim_Param &sim, FTYPE_t a, Data_Vec<FTYPE_t, 2>& sigma_binned);

/**
 * @brief compute non-linear amplitude of density fluctuation and store results
 *
 * @param sim simulation parameters
 * @param a scale factor
 * @param sigma_binned object to store binned correlation function
 */
void gen_sigma_func_binned_gsl_qawf_nl(const Sim_Param &sim, FTYPE_t a, Data_Vec<FTYPE_t, 2>& sigma_binned);


#include <mex.h>
#include <matrix.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include <blas.h>    // dgemv
#include <lapack.h>  // dstegr

#define  abs1(a)         ((a) < 0.0 ? -(a) : (a))
#define  sign1(a)        ((a)==0) ? 0 : (((a)>0.0)?1:(-1))
#define  max1(a,b)       ((a) > (b) ? (a) : (b))
#define  min1(a,b)       ((a) < (b) ? (a) : (b))

double dot1(const double*a,const double*b,const ptrdiff_t n)
{
    double ret = 0;
    ptrdiff_t i;
    for(i=0;i<n;i++)
        ret +=a[i]*b[i];
    return ret;
}
void AxSym(const double*A, const double*x, double*result, const ptrdiff_t n)
{
    /*
     * input:
     * A: n x n
     * x: n x 1
     * output:
     * result = A*x: n x 1
     * NOTE: A is a symmetric matrix, only the the lower triangular part of A is to be referenced
     *       x and the result can not be the same space
     */
    char *UPLO="L";ptrdiff_t N;double ALPHA; ptrdiff_t LDA; ptrdiff_t INCX; double BETA;ptrdiff_t INCY;
    N = n; ALPHA = 1; LDA = n; INCX=1; BETA=0; INCY=1;
    dsymv(UPLO,&N,&ALPHA,A,&LDA,x,&INCX,&BETA,result,&INCY);
}

double computeObj(double s, double u0, double u1, double u2,double d0,double d1,double d2)
{
    return (u2*s*s + u1*s + u0)  /  (d2*s*s + d1*s + d0);
}
double quadfrac2(double u0, double u1, double u2,double d0,double d1,double d2)
{
    double x1;double x2;
    double c2 = u2*d1 - d2*u1;
    double c1 = 2*u2*d0 - 2*d2*u0;
    double c0 = u1*d0 - d1*u0;
    double dd = sqrt(c1*c1 - 4*c2*c0);

    if(c2==0)
    {
        x1 = -c0/c1;
        x2 = x1;
    }
    else
    {
        x1 = (-c1+dd)/(2*c2);
        x2 = (-c1-dd)/(2*c2);
    }
    if(computeObj(x1,u0,u1,u2,d0,d1,d2)<computeObj(x2,u0,u1,u2,d0,d1,d2))
    {
        return x1;
    }
    else
    {
        return x2;
    }

}

ptrdiff_t pos_max(const double*a,const ptrdiff_t n)
{
    double val = abs1(a[0]);
    ptrdiff_t pos=0;
    ptrdiff_t i ;
    for (i=1;i<n;i++)
    {
        if(abs1(a[i]) > val)
        {
            val = abs1(a[i]);
            pos = i;
        }
    }
    return pos;
}
void pvec(const double*a,const ptrdiff_t n)
{
    // print the vector a
    ptrdiff_t i;
    printf("[ ");
    for (i=0;i<n;i++)
    {
        printf("%f ",a[i]);
    }
    printf("]\n");
}

void solve(double *x,const double *in_x, const double*A, const double*b, const double c, const double *Q, const double *r,const double s,const ptrdiff_t n,const ptrdiff_t max_iter)
{

    double *Ax = (double*)malloc(n*sizeof(double));
    double *Qx = (double*)malloc(n*sizeof(double));
    double *up_g =  (double*)malloc(n*sizeof(double));
    double *down_g =  (double*)malloc(n*sizeof(double));
    double *grad =  (double*)malloc(n*sizeof(double));
    double xAx = 0;double bx = 0; double xQx = 0; double rx = 0;
	ptrdiff_t iter, i,j;

    memcpy(x,in_x,n*sizeof(double));
    AxSym(A, x, Ax, n);
    xAx = dot1(x,Ax,n);
    bx = dot1(x,b,n);
    AxSym(Q,x,Qx,n);
    xQx = dot1(x,Qx,n);
    rx = dot1(r,x,n);

    for ( iter=1;iter<=max_iter;iter++)
    {
        double u0,u1,u2,d0,d1,d2,alpha;
        double up,down,fobj;

        up = 0.5*xAx + bx + c;
        down = 0.5*xQx + rx + s;
        fobj =  up / down;

        for (j=0;j<n;j++)
        {
            up_g[j] = Ax[j]+b[j];
            down_g[j] = Qx[j]+r[j];
            grad[j] = up_g[j]/down - fobj * down_g[j]/down ;
        }
        i = pos_max(grad,n);
        u0 = 0.5*xAx + bx + c;
        u1 = Ax[i] + b[i];
        u2 = 0.5*A[i*n+i];
        d0 = 0.5*xQx + rx + s;
        d1 = Qx[i] + r[i];
        d2 = 0.5*Q[i*n+i];

        alpha =  quadfrac2(u0,u1,u2,d0,d1,d2);
        x[i] = x[i] + alpha;

        xAx = xAx + 2*alpha*Ax[i] + alpha*alpha*A[i*n+i];
        xQx = xQx + 2*alpha*Qx[i] + alpha*alpha*Q[i*n+i];
        for (j=0;j<n;j++)
        {
            Ax[j] = Ax[j] + alpha*A[i*n+j];
            Qx[j] = Qx[j] + alpha*Q[i*n+j];
        }
        bx = bx + alpha*b[i];
        rx = rx + alpha*r[i];

    }

    for (ptrdiff_t i=0;i<n;i++)if(x[i]==0)x[i]=1e-100;
    free(Ax);
    free(Qx);
    free(up_g);
    free(down_g);
    free(grad);

}

void mexFunction (int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
    /*set up input arguments */
    double* x =            mxGetPr(prhs[0]);
    double* A =            mxGetPr(prhs[1]);
    double* b =            mxGetPr(prhs[2]);
    double c =            mxGetScalar(prhs[3]);
    double* D =            mxGetPr(prhs[4]);
    double* e =            mxGetPr(prhs[5]);
    double f =            mxGetScalar(prhs[6]);
    ptrdiff_t n =         mxGetScalar(prhs[7]);
    ptrdiff_t max_iter =         mxGetScalar(prhs[8]);
    double *out_x;
    plhs[0] = mxCreateDoubleMatrix(n,1,mxREAL);
    out_x = mxGetPr(plhs[0]);
    solve(out_x,x,A,b,c,D,e,f,n,max_iter);
}


//
// Created by jonathan on 1/5/18.
//

#ifndef CIMPLE_CIMPLE_MPC_COMPUTATION_H
#define CIMPLE_CIMPLE_MPC_COMPUTATION_H

#include <stddef.h>
#include <math.h>
#include "cimple_system.h"
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include "cimple_polytope_library.h"

/**
 * @brief Set up weight matrices for the quadratic problem
 * @param P
 * @param q
 * @param L
 * @param M
 * @param now
 * @param s_dyn
 * @param f_cost
 * @param time_horizon
 * @return
 */
polytope *set_cost_function(gsl_matrix *P,
                            gsl_vector *q,
                            gsl_matrix *L,
                            gsl_vector *M,
                            current_state *now,
                            system_dynamics *s_dyn,
                            cost_function *f_cost,
                            size_t time_horizon);

/**
 * @brief Set up GUROBI environment and solve qp
 * @param low_u
 * @param low_cost
 * @param P
 * @param q
 * @param opt_constraints
 * @param time_horizon
 * @param n
 */
void compute_optimal_control_qp(gsl_matrix *low_u,
                                double *low_cost,
                                gsl_matrix *P,
                                gsl_vector* q,
                                polytope *opt_constraints,
                                size_t time_horizon,
                                size_t n);

/**
 * @brief Calculate (optimal) input that will be applied to take plant from current state (now) to target_abs_state.
 *
 * Global function to compute continuous control input for discrete transition.
 *
 * Computes continuous control input sequence which takes the plant:
 *
 *      - from now
 *      - to (chebyshev center of) target_abs_state
 *      => partitions given by discrete dynamics (d_dyn)
 *
 * Control input is calculated such that it minimizes:
 *
 *      f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x + distance_error_weight * |xc - x(N)|_{ord}
 *      with xc == chebyshev center of target_abs_state
 *
 *    Notes
 *    =====
 *    1. The same horizon length as in reachability analysis
 *    should be used in order to guarantee feasibility.
 *
 *    2. If the closed loop algorithm has been used
 *    to compute reachability the input needs to be
 *    recalculated for each time step
 *    (with decreasing horizon length).
 *
 *    In this case only u(0) should be used as
 *    a control signal and u(1) ... u(N-1) discarded.
 *
 *    3. The "conservative" calculation makes sure that
 *    the plant remains inside the convex hull of the
 *    starting region during execution, i.e.::
 *
 *    x(1), x(2) ...  x(N-1) are in starting region.
 *
 *    If the original proposition preserving partition
 *    is not convex, then safety cannot be guaranteed.
 *
 * @param low_u row k contains the control input: u(k) dim[N x m]
 * @param now initial continuous state
 * @param d_dyn discrete abstraction of system
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param target_abs_state index of target region in discrete dynamics (d_dyn)
 * @param f_cost cost func matrices: f(x, u) = |Rx|_{ord} + |Qu|_{ord} + r'x + distance_error_weight *|xc - x(N)|_{ord}
 */
void get_input (gsl_matrix *u,
                current_state * now,
                discrete_dynamics *d_dyn,
                system_dynamics *s_dyn,
                int target_abs_state,
                cost_function * f_cost,
                size_t current_time_horizon,
                polytope **polytope_list_backup);

/**
 * @brief Calculates (optimal) input to reach desired state (P3) from current state (now) through convex optimization
 *
 * Calculate the sequence low_u such that:
 *
 *      - x(t+1) = A x(t) + B u(t) + K
 *      - x(k) in P1 for k = 0,...,N-1 (if closed loop == 'true')
 *      - x(N) in P3
 *      - [u(k); x(k)] always obey s_dyn.Uset
 *
 * Actual optimality is compared in get_input()
 *
 *
 * @param low_u currently optimal calculated input to target region (input to beat)
 * @param now current state
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param P1 current polytope (or hull of region) the system is in
 * @param P3 a polytope from the target region
 * @param ord ordinance of the norm that should be minimized ord in {1, 2, INFINITY} (currently only '2' is possible)
 * @param time_horizon
 * @param f_cost predefined cost functions |Rx|_{ord} + |Qu|_{ord} + r'x + mid_weight * |xc - x(N)|_{ord}
 * @param low_cost cost associate to low_u
 */
void search_better_path(gsl_matrix *low_u,
                        current_state *now,
                        system_dynamics *s_dyn,
                        polytope *P1,
                        polytope *P3,
                        int ord,
                        size_t time_horizon,
                        cost_function * f_cost,
                        double* low_cost,
                        polytope **polytope_list_backup,
                        size_t total_time);

/**
 * @brief Compute a polytope that constraints the system over the next N time steps to fullfill the GR(1) specifications
 *
 * @param L_full empty matrix when passed in, left side of constraint polytope at the end
 * @param M_full empty vector when passed in, right side of constraint polytope at the end
 * @param s_dyn system dynamics (including auxiliary matrices)
 * @param list_polytopes list of N+1 polytopes in which the systems needs to be in to reach new desired state at time N
 * @param N time horizon
 *
 * Compute the components of the polytope:
 *
 *      L [x(0)' u(0)' ... u(N-1)']' <= M
 *
 * which stacks the following constraints:
 *
 *      - x(t+1) = A x(t) + B u(t) + E d(t)
 *      - [u(k); x(k)] in s_dyn.Uset for all k => system obeys predefined constraints on how inputs may behave
 *
 * [L_full; M_full] polytope intersection of required and allowed polytopes
 *
 */
polytope * set_path_constraints(current_state * now,
                                system_dynamics * s_dyn,
                                polytope **list_polytopes,
                                size_t N);

#endif //CIMPLE_CIMPLE_MPC_COMPUTATION_H


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include "sum.h"

int main(int argc, char *argv[]) {
    int num_bands = 2;
    double t = 1.0;
    double E0 = 6.0;
    double deltaE = 14.0;
    void Efn(double k[3], gsl_vector *energies) {
        double cx = cos(2.0 * M_PI * k[0]);
        double cy = cos(2.0 * M_PI * k[1]);
        double cz = cos(2.0 * M_PI * k[2]);
        double tk = -2.0 * t * (cx + cy + cz);
        int i;
        double E0_band;
        for (i = 0; i < num_bands; i++) {
            E0_band = E0 + ((double)i) * deltaE;
            gsl_vector_set(energies, i, E0_band + tk);
        }
    }
    int na = 8;
    int nb = 8;
    int nc = 8;
    bool use_cache = true;

    gsl_matrix *R = gsl_matrix_calloc(3, 3); // R -> all zeros
    // Overall scale for R doesn't matter.
    gsl_matrix_set(R, 0, 0, 1.0);
    gsl_matrix_set(R, 1, 1, 1.0);
    gsl_matrix_set(R, 2, 2, 1.0);

    double num_electrons = 1.0;
    double E_Fermi = 0.0;
    double energy = SumEnergy(&E_Fermi, Efn, na, nb, nc, num_bands, num_electrons, R, use_cache);
    printf("Got E_Fermi = %f\n", E_Fermi);
    printf("Got energy = %f\n", energy);

    double tol = 1e-8;
    double expected = 6.0;
    if (fabs(energy - expected) > tol) {
        printf("Incorrect energy; got %f, expected %f\n", energy, expected);
        return 1;
    }

    printf("Sum test passed.\n");
    return 0;
}


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

//#define DEBUG
#ifndef DEBUG
#define IN 64
#define OUT 32
#define NUM_ITS 1000000
#else
#define IN 4
#define OUT 2
#define NUM_ITS 1
#endif
#define INIT_VAL 0.4711

#ifdef USE_BLAS
#include <cblas.h>
#endif

double *
mtrx(int rows, int cols) {
    int row, col;
    double *d, *it;
    d = (double *) malloc(rows * cols * sizeof(double));
    it = d;
    for (row = 0; row < rows; row++) {
        for (col = 0; col < cols; col++) {
            // Not very random, but makes debugging easier.
            *it = INIT_VAL;
            it++;
        }
    }
    return d;
}

void
forward(const int rows, const int cols, double *W, double *x,
        double *a) {
    #ifndef USE_BLAS
    int row, col;
    double *W_it, *x_it, *a_it;

    W_it = W;
    a_it = a;
    for (row = 0; row < rows; row++) {
        *a_it = 0.0;
        x_it = x;
        for (col = 0; col < cols; col++) {
            *a_it += *W_it * *x_it;
            W_it++;
            x_it++;
        }
        *a_it = tanh(*a_it);
        a_it++;
    }
    #else /* USE_BLAS */
    int row;

    cblas_dgemv(CblasColMajor, CblasNoTrans, rows, cols, 1.0, W, rows, x, 1,
            0.0, a, 1);
    for (row = 0; row < rows; row++) {
        a[row] = tanh(a[row]);
    }
    #endif /* USE_BLAS */
}

void
backward(const int rows, const int cols, double *x, double *a, double *d,
        double *b) {
    #ifndef USE_BLAS
    int row, col;
    double ad;
    double *x_it, *a_it, *d_it, *b_it;

    a_it = a;
    d_it = d;
    b_it = b;
    for (row = 0; row < rows; row++) {
        ad = (1.0 - pow(*a_it, 2)) * *d_it;

        x_it = x;
        for (col = 0; col < cols; col++) {
            *b_it = ad * *x_it;
            b_it++;
            x_it++;
        }
        a_it++;
        d_it++;
    }
    #else /* USE_BLAS */
    int row;
    double *tmp;

    tmp = (double *) malloc(rows * sizeof(double));
    for (row = 0; row < rows; row++) {
        tmp[row] = (1.0 - pow(a[row], 2)) * d[row];
    }
    cblas_dgemm(CblasColMajor, CblasTrans, CblasNoTrans, rows, cols, 1, 1.0,
            tmp, 1, x, 1, 0.0, b, rows);

    free(tmp);
    #endif /* USE_BLAS */
}

int
main() {
    int i;
    double *W, *x, *d, *a, *b;
    struct timespec tic, toc;

    W = mtrx(OUT, IN);
    x = mtrx(IN, 1);
    d = mtrx(OUT, 1);
    a = malloc(OUT * sizeof(double));
    b = malloc(OUT * IN * sizeof(double));

    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tic);
    for (i = 0; i < NUM_ITS; i++) {
        forward(OUT, IN, W, x, a);
        backward(OUT, IN, x, a, d, b);
    }
    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &toc);

    #ifdef DEBUG
    for (int j = 0; j < OUT; j++) {
        printf("%f ", a[j]);
    }
    printf("\n");

    double *b_it = b;
    for (int u = 0; u < OUT; u++) {
        for (int v = 0; v < IN; v++) {
            printf("%f ", *b_it);
            b_it++;
        }
        printf("\n");
    }
    #endif

    printf("%f\n", (toc.tv_nsec - tic.tv_nsec) / 100000000.0);

    free(b);
    free(a);
    free(d);
    free(x);
    free(W);

    return EXIT_SUCCESS;
}


// TRENTO: Reduced Thickness Event-by-event Nuclear Topology
// Copyright 2015 Jonah E. Bernhard, J. Scott Moreland
// TRENTO3D: Three-dimensional extension of TRENTO by Weiyao Ke
// MIT License

#ifndef ETA_H
#define ETA_H

#include <cmath>
#include <vector>
#include <gsl/gsl_fft_complex.h>

/// GSL fft of complex array, z_i = x_i + j*y_i
/// data are stored in one real array A that A[2i] = x_i and A[2i+1] = y_i
#define REAL(z,i) ((z)[2*(i)])
#define IMAG(z,i) ((z)[2*(i)+1])

double const sqrt2 = std::sqrt(2);
constexpr double TINY = 1e-12;
constexpr int relative_skew_switch = 1;
constexpr int absolute_skew_switch = 2;
/// the mean of the rapidity distribution as function of Ta(x,y),
/// Tb(x,y) and exp(y_beam).
/// For the case both Ta and Tb are tiny small, mean = 0.0 is returned.
/// For the case only Ta(Tb) = 0, it returns +(-)y_beam,
/// For the case y_beam >> 1, mean ~ 0.5*log(Ta/Tb)
double inline mean_function(double ta, double tb, double exp_ybeam){
  if (ta < TINY && tb < TINY) return 0.;
  return 0.5 * std::log((ta*exp_ybeam + tb/exp_ybeam) / std::max(ta/exp_ybeam + tb*exp_ybeam, TINY));
}

/// The coefficient of normalized width of the rapidity distribution
/// Currently, it is simply a constant independent of Ta(x,y) and Tb(x,y)
double inline std_function(double ta, double tb){
  (void)ta;
  (void)tb;
  return 1.;
}

/// The normalized skewness as function of Ta(x,y) and Tb(x,y)

double inline skew_function(double ta, double tb, int type_switch){
  if (type_switch == relative_skew_switch)
    return (ta - tb)/std::max(ta + tb, TINY);
  else if(type_switch == absolute_skew_switch)
      return ta-tb;
  else return 0.;
}

/// A fast pseudorapidity to rapidity transformer using pretabulated values
/// It returns the transfoamtion y(eta) and the jacobian dy/deta(eta)
class fast_eta2y {
private:
  double etamax_;
  double deta_;
  std::size_t neta_;
  std::vector<double> y_;
  std::vector<double> dydeta_;
public:
  fast_eta2y(double J, double etamax, double deta)
      : etamax_(etamax),
        deta_(deta),
        neta_(std::ceil(2.*etamax_/(deta_+1e-15))+1),
        y_(neta_, 0.),
        dydeta_(neta_, 0.) {

    for (std::size_t ieta = 0; ieta < neta_; ++ieta) {
      double eta = -etamax_ + ieta*deta_;
      double Jsh = J*std::sinh(eta);
      double sq = std::sqrt(1. + Jsh*Jsh);
      y_[ieta] = std::log(sq + Jsh);
      dydeta_[ieta] = J*std::cosh(eta)/sq;
    }
  }

  double rapidity(double eta){
    double steps = (eta + etamax_)/deta_;
    double xi = std::fmod(steps, 1.);
    std::size_t index = std::floor(steps);
    return y_[index]*(1. - xi) + y_[index+1]*xi;
  }

  double Jacobian(double eta){
    double steps = (eta + etamax_)/deta_;
    double xi = std::fmod(steps, 1.);
    std::size_t index = std::floor(steps);
    return dydeta_[index]*(1. - xi) + dydeta_[index+1]*xi;
  }

};

/// A class for cumulant generating function inversion
/// This class handles inverse fourier transform the cumulant generating function
/// A direct transformation F^{-1} exp(i*m*k-(std*k)^2/2-skew*(std*k)^3/6) results
/// in an ill-behaved function. Instead, skew is replaced by skew*exp(-(std*k)^2/2).
/// In this way higher order cumulant are included to regulated the function.
/// The reconstruction range is taken to be [-3.33, 3.33]*std, which does not
/// seem to be very large, however, by trail and error, this range elimiates
/// oscillations at large rapidity and leaves the details close to mid-rapidity
/// unchanged. We used GSL FFT library (more specialized library would be FFTW,
/// e.g.), with 256 points. The transformed results are stored and interpolated.
class cumulant_generating{
private:
  size_t const N;
  double * data, * dsdy;
  double eta_max;
  double deta;
  double center;

public:
  cumulant_generating(): N(256), data(new double[2*N]), dsdy(new double[2*N]){};

  /// This function set the mean, std and skew of the profile and use FFT to
  /// transform cumulant generating function at zero mean.
  void calculate_dsdy(double mean, double std, double skew){
    double k1, k2, k3, amp, arg;
    // adaptive eta_max = 3.33*std;
    center = mean;
    eta_max = std*3.33;
    deta = 2.*eta_max/(N-1.);
    double fftmean = eta_max/std;
      for(size_t i=0;i<N;i++){
          k1 = M_PI*(i-N/2.0)/eta_max*std;
      k2 = k1*k1;
      k3 = k2*k1;

          amp = std::exp(-k2/2.0);
          arg = fftmean*k1+skew/6.0*k3*amp;

      REAL(data,i) = amp*std::cos(arg);
          IMAG(data,i) = amp*std::sin(arg);
      }
       gsl_fft_complex_radix2_forward(data, 1, N);

      for(size_t i=0;i<N;i++){
          dsdy[i] = REAL(data,i)*(2.0*static_cast<double>(i%2 == 0)-1.0);
      }
  }

  /// When interpolating the funtion, the mean is put back by simply shifting
  /// the function by y = y - mean + dy/2, the last term is correcting for
  /// interpolating bin edge instead of bin center
  double interp_dsdy(double y){
    y = y-center+deta/2.;
    if (y < -eta_max || y >= eta_max) return 0.0;
    double xy = (y+eta_max)/deta;
    size_t iy = std::floor(xy);
    double ry = xy-iy;
    return dsdy[iy]*(1.-ry) + dsdy[iy+1]*ry;
  }
};
#endif


/********************************************************
  Stanford Driving Software
  Copyright (c) 2011 Stanford University
  All rights reserved.

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

* Redistributions of source code must retain the above
  copyright notice, this list of conditions and the
  following disclaimer.
* 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.
* The names of the contributors may not be used to endorse
  or promote products derived from this software
  without specific prior written permission.

  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 OWNER 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.
 ********************************************************/

#ifndef KALMAN_FILTER_H_
#define KALMAN_FILTER_H_

#include <vector>
#include <stdint.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit.h>
#include "gsl.h"

namespace vlr {

class KalmanFilter {

public:
  KalmanFilter();
  KalmanFilter(int state_dim);
  KalmanFilter(const KalmanFilter& kf);
  virtual ~KalmanFilter();

  void transitionUpdate();
  void observationUpdate(gsl_vector* innovation_vector);

  virtual gsl_vector* predictTransition(gsl_vector* orig);
  virtual void predictTransitions(std::vector<gsl_vector*>& orig_pts, std::vector<gsl_vector*>& next_pts);
  virtual gsl_vector* predictObservation(gsl_vector* pt);
  virtual void predictObservations(std::vector<gsl_vector*>& pts, std::vector<gsl_vector*>& obs);

protected:
  void calculateKalmanGain();

public:
  int getStateDim() const { return state_dim_; }
  void setStateDim(int state_dim);

  int getObservationDim() const { return observation_mat_->size1; }

  gsl_vector* getMean() const { return mean_; }
  gsl_matrix* getCovariance() const { return covariance_; }
  gsl_matrix* getKalmanGain() const { return kalman_gain_; }

  gsl_matrix* getTransitionMatrix() const { return transition_mat_; }
  void setTransitionMatrix(gsl_matrix* transition_mat) { transition_mat_ = transition_mat; }

  gsl_matrix* getTransitionCovariance() const { return transition_cov_; }
  void setTransitionCovariance(gsl_matrix* transition_cov) { transition_cov_ = transition_cov; }

  gsl_matrix* getObservationMatrix() const { return observation_mat_; }
  void setObservationMatrix(gsl_matrix* observation_mat) { observation_mat_ = observation_mat; }

  gsl_matrix* getObservationCovariance() const { return observation_cov_; }
  void setObservationCovariance(gsl_matrix* observation_cov) { observation_cov_ = observation_cov; }

  double getYaw() const { return yaw_; }
  void setYaw(double yaw) { yaw_ = yaw; }

  void print() const;

 protected:
  int state_dim_;

  gsl_vector* mean_;
  gsl_matrix* covariance_;

  // temp vars
  gsl_matrix* kalman_gain_;
  gsl_matrix* observation_mat_;
  gsl_matrix* observation_cov_;

  /*
   * TRANSITION MATRIX
   *
   * (for linear KF
   *  transition_mat_ is A)
   *
   *  A = [ 1 0 dt 0 ]
   *      [ 0 1 0 dt ]
   *      [ 0 0 1  0 ]
   *      [ 0 0 0  1 ]
   */
  gsl_matrix* transition_mat_;
  // do we own the pointer?
  uint32_t my_trans_mat_;
  /*
   * TRANSITION COVARIANCE
   *
   *  Q = [ 0 0  0  0  ]
   *      [ 0 0  0  0  ]
   *      [ 0 0 q^2 0  ]
   *      [ 0 0  0 q^2 ]
   */
  gsl_matrix* transition_cov_;
  // do we own the pointer?
  uint32_t my_trans_cov_;

  double yaw_;
};

} // namespace vlr

#endif /*KALMAN_FILTER_H_*/


#ifndef __GSLEXTRA_H__
#define __GSLEXTRA_H__

#include <iostream>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>

typedef struct gsl_quad_tensor
{
    int i;
    int j;
    int k;
    int l;

    gsl_matrix *** element;
}gsl_quad_tensor;

gsl_quad_tensor * gsl_quad_tensor_alloc(int i, int j, int k, int l);
gsl_quad_tensor * gsl_quad_tensor_calloc(int i,int j, int k, int l);
void gsl_quad_tensor_free(gsl_quad_tensor * q);

double gsl_quad_tensor_get(gsl_quad_tensor * q, int i, int j, int k, int l);
void gsl_quad_tensor_get_matrix(gsl_matrix * m, gsl_quad_tensor * q, int i, int j);

void gsl_quad_tensor_set(gsl_quad_tensor * q, int i, int j, int k, int l, double x);
void gsl_quad_tensor_set_matrix(gsl_quad_tensor * q, int i, int j, const gsl_matrix * m);

void gsl_quad_tensor_add(gsl_quad_tensor * A, gsl_quad_tensor * b);
void gsl_quad_tensor_sub(gsl_quad_tensor * A, gsl_quad_tensor * b);
void gsl_quad_tensor_scale(gsl_quad_tensor * q, double x);

void gsl_vector_complex_convert(gsl_vector * source, gsl_vector_complex * target, int length);
void gsl_matrix_complex_convert(gsl_matrix * source, gsl_matrix_complex * target, int rows, int columns);

void gsl_vector_complex_extract(gsl_vector_complex * source, gsl_vector * real, gsl_vector * imag, int length);
void gsl_matrix_complex_extract(gsl_vector_complex * source, gsl_matrix * real,gsl_matrix * imag, int rows, int columns);

void gsl_vector_complex_combine(gsl_vector * real, gsl_vector * imag, gsl_vector_complex * target);
void gsl_matrix_complex_combine(gsl_matrix * real,gsl_matrix * imag, gsl_matrix_complex * target);

void gsl_matrix_diag(gsl_matrix * target, gsl_vector * diag, int length);
void gsl_matrix_complex_diag(gsl_matrix_complex * target, gsl_vector_complex * diag, int length);

void gsl_matrix_mul(gsl_matrix * A, gsl_matrix *B, gsl_matrix * Result,int Acolumn,int Arow,int Bcolumn);
void gsl_matrix_complex_mul(gsl_matrix_complex * A, gsl_matrix_complex *B, gsl_matrix_complex * Result,int Acolumn,int Arow,int Bcolumn);

double gsl_vector_inner_product(gsl_vector * A, gsl_vector * B,int length);
gsl_complex gsl_vector_complex_product(gsl_vector_complex * A, gsl_vector_complex * B, int length);
gsl_complex gsl_vector_complex_inner_product(gsl_vector_complex * A, gsl_vector_complex * B, int length);
void gsl_vector_transform(gsl_vector * vec,gsl_matrix * trf,int length);
void gsl_vector_complex_transform(gsl_vector_complex * vec, gsl_matrix_complex * trf,int length);
void gsl_matrix_unitmatrix(gsl_matrix * m,int length);
void gsl_matrix_complex_unitmatrix(gsl_matrix_complex * m,int length);
void gsl_vector_complex_conjugate(gsl_vector_complex * v, int length);
void gsl_matrix_complex_conjugate(gsl_matrix_complex * m, int rows, int columns);

void gsl_matrix_square_root(gsl_matrix * dest, gsl_matrix * src, int length);
void gsl_matrix_inverse_square_root(gsl_matrix* dest, gsl_matrix * src, int length);
void gsl_eigen_Lowdin_diag(gsl_matrix * m, gsl_matrix * S, gsl_vector * eigen, gsl_matrix * eigenvec, int length);
void gsl_matrix_normalize(gsl_matrix * coef,int length, int columns);
void gsl_matrix_flip(gsl_matrix * dest,int height,int width);

#endif


// rand.h
#ifndef RAND_H
#define RAND_H
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#define URNG generator

class generator{
    const gsl_rng_type * T;
    public:
    gsl_rng * r;
    generator();
    ~generator();
    void seed(unsigned long int s);
};

#include "types.h"

class r_lognorm{
    double zeta;
    double sigma;
    public:
    r_lognorm(REAL mean,REAL sigma);
    ~r_lognorm();
    REAL operator()(URNG& g);
};

REAL lognorm_func(URNG& g,REAL mean,REAL sig);

class r_hypgeo{
    unsigned int n1;
    unsigned int n2;
    unsigned int t;
    public:
    r_hypgeo(unsigned int population1,unsigned int population2,unsigned int samples);
    ~r_hypgeo();
    unsigned int  operator()(URNG& g);
};

unsigned int hypgeo_func(URNG& g,unsigned int population1,unsigned int population2,unsigned int samples);

class r_uni{
    double a;
    double b;
    public:
    r_uni(REAL min,REAL max);
    ~r_uni();
    REAL operator()(URNG& g);
};

REAL unif_func(URNG& g,REAL min,REAL max);

REAL exp_func(URNG& g,REAL mu);

int ber_func(URNG& g ,REAL p);

#endif


#include <gsl/gsl_errno.h>
#include <math.h>
#include "acceleration.h"

int func(double t, const double y[], double f[], void *params) {
    /*
        y[0], y[1], y[2] are x, y, z
        y[3], y[4], y[5] are v_x, v_y, v_z
    */

    (void)(t); /* avoid unused parameter warning */
    struct HernquistParams pars = *(struct HernquistParams *)params;

    double r;
    double dPhi_dr;

    r = sqrt(y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
    dPhi_dr = pars.G * pars.m / pow(r + pars.c, 2);

    // Derivative of position is just velocity
    f[0] = y[3];
    f[1] = y[4];
    f[2] = y[5];

    // Derivative of velocity is acceleration from potential
    f[3] = -dPhi_dr * y[0] / r;
    f[4] = -dPhi_dr * y[1] / r;
    f[5] = -dPhi_dr * y[2] / r;

    return GSL_SUCCESS;
}


#include <stdio.h>
#include <string.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>

#include "chmath.h"
#include "err.h"

static char* me = "chmath";

static double get(const gsl_matrix* m, int i, int j) {
  return gsl_matrix_get(m, i, j);
}

int chmath_eig_values(const double A[6], /**/ double VAL[3]) {
  enum { XX, XY, XZ, YY, YZ, ZZ };
  enum { YX = XY, ZX = XZ, ZY = YZ };
  enum { X, Y, Z };

  double B[3 * 3];
  gsl_matrix_view m;
  gsl_vector* val;
  gsl_matrix* vec;
  gsl_eigen_symmv_workspace* w;
  int i, status;

  val = gsl_vector_alloc(3);
  vec = gsl_matrix_alloc(3, 3);
  w = gsl_eigen_symmv_alloc(3);

  i = 0;
  B[i++] = A[XX];
  B[i++] = A[XY];
  B[i++] = A[XZ];
  B[i++] = A[YX];
  B[i++] = A[YY];
  B[i++] = A[YZ];
  B[i++] = A[ZX];
  B[i++] = A[ZY];
  B[i++] = A[ZZ];

  m = gsl_matrix_view_array(B, 3, 3);
  status = gsl_eigen_symmv(&m.matrix, val, vec, w);
  if (status != GSL_SUCCESS) ERR(("gsl_eigen_symmv failed"));
  gsl_eigen_symmv_sort(val, vec, GSL_EIGEN_SORT_ABS_ASC);

  i = 0;
  VAL[i] = gsl_vector_get(val, i);
  i++;
  VAL[i] = gsl_vector_get(val, i);
  i++;
  VAL[i] = gsl_vector_get(val, i);

  gsl_vector_free(val);
  gsl_matrix_free(vec);
  gsl_eigen_symmv_free(w);
  return 0;
}

int chmath_eig_vectors(
    const double A[6], /**/ double a[3], double b[3], double c[3]) {
  enum { XX, XY, XZ, YY, YZ, ZZ };
  enum { YX = XY, ZX = XZ, ZY = YZ };
  enum { X, Y, Z };

  double B[3 * 3];
  gsl_matrix_view m;
  gsl_vector* val;
  gsl_matrix* vec;
  gsl_eigen_symmv_workspace* w;
  int i, status;

  val = gsl_vector_alloc(3);
  vec = gsl_matrix_alloc(3, 3);
  w = gsl_eigen_symmv_alloc(3);

  i = 0;
  B[i++] = A[XX];
  B[i++] = A[XY];
  B[i++] = A[XZ];
  B[i++] = A[YX];
  B[i++] = A[YY];
  B[i++] = A[YZ];
  B[i++] = A[ZX];
  B[i++] = A[ZY];
  B[i++] = A[ZZ];

  m = gsl_matrix_view_array(B, 3, 3);
  status = gsl_eigen_symmv(&m.matrix, val, vec, w);
  if (status != GSL_SUCCESS) ERR(("gsl_eigen_symmv failed"));
  gsl_eigen_symmv_sort(val, vec, GSL_EIGEN_SORT_ABS_ASC);

  i = 0;
  a[X] = get(vec, X, X);
  a[Y] = get(vec, Y, X);
  a[Z] = get(vec, Z, X);
  b[X] = get(vec, X, Y);
  b[Y] = get(vec, Y, Y);
  b[Z] = get(vec, Z, Y);
  c[X] = get(vec, X, Z);
  c[Y] = get(vec, Y, Z);
  c[Z] = get(vec, Z, Z);
  gsl_vector_free(val);
  gsl_matrix_free(vec);
  gsl_eigen_symmv_free(w);
  return 0;
}


#ifndef DISTANCE_H_MUJPGVJL
#define DISTANCE_H_MUJPGVJL

#include <algorithm>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/median.hpp>
#include <boost/accumulators/statistics/skewness.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/histogram.hpp>
#include <cstddef>
#include <gsl/gsl>
#include <opencv2/core/persistence.hpp>

namespace sens_loc {

/// This namespace contains useful function for statistical analysis of
/// keypoints, descriptors and other relevant quantities.
namespace analysis {

/// Wrapper-struct to hold the common set of statistical values.
struct statistic {
    using accumulator_t = boost::accumulators::accumulator_set<
        float,
        boost::accumulators::stats<boost::accumulators::tag::count,
                                   boost::accumulators::tag::min,
                                   boost::accumulators::tag::max,
                                   boost::accumulators::tag::median,
                                   boost::accumulators::tag::mean,
                                   boost::accumulators::tag::variance(
                                       boost::accumulators::lazy),
                                   boost::accumulators::tag::skewness>>;

    std::size_t        count    = 0UL;
    float              min      = 0.0F;
    float              max      = 0.0F;
    float              mean     = 0.0F;
    float              variance = 0.0F;
    float              stddev   = 0.0F;
    float              skewness = 0.0F;
    float              median   = 0.0F;
    std::vector<float> decentils{0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
                                 0.0F, 0.0F, 0.0F, 0.0F};

    /// Extract the statistical values from the original data.
    /// \pre the data must be sorted!
    static statistic make(gsl::span<const float> data) {
        using namespace std;
        Expects(is_sorted(begin(data), end(data)));

        statistic s;

        if (data.empty())
            return s;

        if (data.size() >= 10L) {
            const int      number_quantils = 10;
            std::ptrdiff_t delta           = data.size() / number_quantils;

            Expects(s.decentils.size() == 9UL);
            for (int i = 1; i < number_quantils; ++i) {
                s.decentils[i - 1] = data[i * delta];
            }
            Ensures(s.decentils.size() == 9UL);
        }
        s.median = data[data.size() / 2];

        accumulator_t accu;
        std::for_each(std::begin(data), std::end(data),
                      [&accu](float e) { accu(e); });
        using namespace boost::accumulators;
        s.count    = boost::accumulators::count(accu);
        s.min      = boost::accumulators::min(accu);
        s.max      = boost::accumulators::max(accu);
        s.mean     = boost::accumulators::mean(accu);
        s.variance = boost::accumulators::variance(accu);
        s.stddev   = std::sqrt(s.variance);
        s.skewness = boost::accumulators::skewness(accu);
        return s;
    }

    void reset() noexcept {
        count     = 0UL;
        min       = 0.0F;
        max       = 0.0F;
        mean      = 0.0F;
        variance  = 0.0F;
        stddev    = 0.0F;
        skewness  = 0.0F;
        median    = 0.0F;
        decentils = {0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F};
    }
};

/// Write-Functionality for OpenCVs-Filestorage API.
void write(cv::FileStorage& fs, const std::string& name, const statistic& stat);

/// This class processes 'float' datapoints and calculates both histograms
/// and basic statistical quantities, like the 'min', 'max', 'mean' and
/// others.
/// Its main use-case is to analyze distance information gathered while
/// processing image and matching data. It can be used for any scalar
/// quantity though.
class distance {
  public:
    using axis_t = boost::histogram::axis::regular<
        // 'float' values are tracked with the histogram.
        float,
        // do no transformation before insertion
        boost::histogram::axis::transform::id,
        // a string is the metadata for the axis (=title)
        std::string,
        // no overflow/underflow by construction.
        boost::histogram::axis::option::none_t>;
    using histo_t = decltype(boost::histogram::make_histogram(axis_t{}));

    distance() = default;
    explicit distance(gsl::span<const float> distances,
                      unsigned int           bin_count = 50U,
                      std::string            title     = "distance") noexcept
        : _bin_count{bin_count}
        , _axis_title{std::move(title)} {
        analyze(distances);
    }

    /// Analyses the provided distances. This will overwrite a previous
    /// analysis and dataset! Use this method to provide the data if the
    /// class was default constructed.
    /// \pre is_sorted(distances)
    void analyze(gsl::span<const float> distances, bool histo = true) noexcept;

    /// Return the reference to the potentially created histogram in \c analyze.
    /// If the histogram is not created, it will just be default constructed.
    /// \sa analyze
    [[nodiscard]] const histo_t& histogram() const noexcept { return _histo; }

    [[nodiscard]] std::size_t count() const noexcept { return _s.count; }
    [[nodiscard]] float       min() const noexcept { return _s.min; }
    [[nodiscard]] float       max() const noexcept { return _s.max; }
    [[nodiscard]] float       median() const noexcept { return _s.median; }
    [[nodiscard]] float       mean() const noexcept { return _s.mean; }
    [[nodiscard]] float       variance() const noexcept { return _s.variance; }
    [[nodiscard]] float       stddev() const noexcept { return _s.stddev; }
    [[nodiscard]] float       skewness() const noexcept { return _s.skewness; }

    /// Read-Only access to underlying statistical data.
    [[nodiscard]] const statistic& get_statistic() const noexcept { return _s; }

  private:
    unsigned int _bin_count  = 50UL;
    std::string  _axis_title = "distance";
    histo_t      _histo;
    statistic    _s;
};
}  // namespace analysis
}  // namespace sens_loc

#endif /* end of include guard: DISTANCE_H_MUJPGVJL */


/* -*- c -*- */
/*
 * Copyright 2002-2018 Free Software Foundation, Inc.
 *
 * This file is part of GNU Radio
 *
 * GNU Radio 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, or (at your option)
 * any later version.
 *
 * GNU Radio 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 GNU Radio; see the file COPYING.  If not, write to
 * the Free Software Foundation, Inc., 51 Franklin Street,
 * Boston, MA 02110-1301, USA.
 */

/*
 * generate MMSE FIR interpolation table values
 */

#include <math.h>
#include <assert.h>
#include <gsl/gsl_integration.h>

#define	MU	0.5			/* the MU for which we're computing coeffs */

#define	Ts	(1.0)			/* sampling period 		*/
#define	B	(1.0/(4*Ts))		/* one-sided signal bandwidth 	*/
//#define	B	(1.0/(8./3*Ts))		/* one-sided signal bandwidth 	*/

static unsigned global_n;
static double  *global_h;
double		global_mu = MU;
double		global_B  = B;

gsl_integration_workspace *global_gsl_int_workspace = NULL;

/*
 * This function computes the difference squared between the ideal
 * interpolator frequency response at frequency OMEGA and the
 * approximation defined by the FIR coefficients in global_h[]
 *
 * See eqn (9-7), "Digital Communication Receivers", Meyr, Moeneclaey
 * and Fechtel, Wiley, 1998.
 */

static double
integrand (double omega, void *params)
{
  double real_ideal;
  double real_approx;
  double real_diff;
  double imag_ideal;
  double imag_approx;
  double imag_diff;

  int	 i, n;
  int	 I1;
  double *h;

  real_ideal = cos (omega * Ts * global_mu);
  imag_ideal = sin (omega * Ts * global_mu);

  n = global_n;
  h = global_h;
  I1 = -(n / 2);

  real_approx = 0;
  imag_approx = 0;

  for (i = 0; i < n; i++){
    real_approx += h[i] * cos (-omega * Ts * (i + I1));
    imag_approx += h[i] * sin (-omega * Ts * (i + I1));
  }

  real_diff = real_ideal - real_approx;
  imag_diff = imag_ideal - imag_approx;

  return real_diff * real_diff + imag_diff * imag_diff;
}

/*
 * Integrate the difference squared over all frequencies of interest.
 */
double
c_fcn (double *x, int n)
{
  gsl_function F;
  double result, error;

  F.function = integrand;
  F.params = NULL;

  assert ((n & 1) == 0);	/* assert n is even */
  global_n = n;
  global_h = x;
  gsl_integration_qag(&F, -2 * M_PI * global_B, 2 * M_PI * global_B,
                      0.0, 1e-12, 1000, GSL_INTEG_GAUSS61,
                      global_gsl_int_workspace, &result, &error);
  return result;
}

/* this is the interface expected by the calling fortran code */

double
objective (double x[], int *ndim)
{
  return c_fcn (x, *ndim);
}

static double
si (double x)
{
  if (fabs (x) < 1e-9)
    return 1.0;

  return sin(x) / x;
}

/*
 * starting guess for optimization
 */
void initpt (double x[], int ndim)
{
  int i;
  for (i = 0; i < ndim; i++){
    x[i] = si (M_PI * ((double) (i - ndim/2) + global_mu));
  }
}


/* diff/diff.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 David Morrison
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_diff.h>

int
gsl_diff_backward (const gsl_function * f,
                   double x, double *result, double *abserr)
{
  /* Construct a divided difference table with a fairly large step
     size to get a very rough estimate of f''.  Use this to estimate
     the step size which will minimize the error in calculating f'. */

  int i, k;
  double h = GSL_SQRT_DBL_EPSILON;
  double a[3], d[3], a2;

  /* Algorithm based on description on pg. 204 of Conte and de Boor
     (CdB) - coefficients of Newton form of polynomial of degree 2. */

  for (i = 0; i < 3; i++)
    {
      a[i] = x + (i - 2.0) * h;
      d[i] = GSL_FN_EVAL (f, a[i]);
    }

  for (k = 1; k < 4; k++)
    {
      for (i = 0; i < 3 - k; i++)
        {
          d[i] = (d[i + 1] - d[i]) / (a[i + k] - a[i]);
        }
    }

  /* Adapt procedure described on pg. 282 of CdB to find best value of
     step size. */

  a2 = fabs (d[0] + d[1] + d[2]);

  if (a2 < 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      a2 = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  h = sqrt (GSL_SQRT_DBL_EPSILON / (2.0 * a2));

  if (h > 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      h = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  *result = (GSL_FN_EVAL (f, x) - GSL_FN_EVAL (f, x - h)) / h;
  *abserr = fabs (10.0 * a2 * h);

  return GSL_SUCCESS;
}

int
gsl_diff_forward (const gsl_function * f,
                  double x, double *result, double *abserr)
{
  /* Construct a divided difference table with a fairly large step
     size to get a very rough estimate of f''.  Use this to estimate
     the step size which will minimize the error in calculating f'. */

  int i, k;
  double h = GSL_SQRT_DBL_EPSILON;
  double a[3], d[3], a2;

  /* Algorithm based on description on pg. 204 of Conte and de Boor
     (CdB) - coefficients of Newton form of polynomial of degree 2. */

  for (i = 0; i < 3; i++)
    {
      a[i] = x + i * h;
      d[i] = GSL_FN_EVAL (f, a[i]);
    }

  for (k = 1; k < 4; k++)
    {
      for (i = 0; i < 3 - k; i++)
        {
          d[i] = (d[i + 1] - d[i]) / (a[i + k] - a[i]);
        }
    }

  /* Adapt procedure described on pg. 282 of CdB to find best value of
     step size. */

  a2 = fabs (d[0] + d[1] + d[2]);

  if (a2 < 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      a2 = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  h = sqrt (GSL_SQRT_DBL_EPSILON / (2.0 * a2));

  if (h > 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      h = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  *result = (GSL_FN_EVAL (f, x + h) - GSL_FN_EVAL (f, x)) / h;
  *abserr = fabs (10.0 * a2 * h);

  return GSL_SUCCESS;
}

int
gsl_diff_central (const gsl_function * f,
                  double x, double *result, double *abserr)
{
  /* Construct a divided difference table with a fairly large step
     size to get a very rough estimate of f'''.  Use this to estimate
     the step size which will minimize the error in calculating f'. */

  int i, k;
  double h = GSL_SQRT_DBL_EPSILON;
  double a[4], d[4], a3;

  /* Algorithm based on description on pg. 204 of Conte and de Boor
     (CdB) - coefficients of Newton form of polynomial of degree 3. */

  for (i = 0; i < 4; i++)
    {
      a[i] = x + (i - 2.0) * h;
      d[i] = GSL_FN_EVAL (f, a[i]);
    }

  for (k = 1; k < 5; k++)
    {
      for (i = 0; i < 4 - k; i++)
        {
          d[i] = (d[i + 1] - d[i]) / (a[i + k] - a[i]);
        }
    }

  /* Adapt procedure described on pg. 282 of CdB to find best
     value of step size. */

  a3 = fabs (d[0] + d[1] + d[2] + d[3]);

  if (a3 < 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      a3 = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  h = pow (GSL_SQRT_DBL_EPSILON / (2.0 * a3), 1.0 / 3.0);

  if (h > 100.0 * GSL_SQRT_DBL_EPSILON)
    {
      h = 100.0 * GSL_SQRT_DBL_EPSILON;
    }

  *result = (GSL_FN_EVAL (f, x + h) - GSL_FN_EVAL (f, x - h)) / (2.0 * h);
  *abserr = fabs (100.0 * a3 * h * h);

  return GSL_SUCCESS;
}


#include <viaio/VImage.h>
#include <string.h>
#include <gsl/gsl_matrix.h>
#include <viaio/Vlib.h>
#include "gsl_utils.h"

void prewhite(gsl_matrix_float** pinvX,gsl_matrix_float** invM,gsl_matrix_float* X,int numlags)
{
  int n = X->size2;

  /* preparation for whitening */

  /* X^T */
  gsl_matrix_float* transX = gsl_matrix_float_alloc(X->size2, X->size1);
  gsl_matrix_float_transpose_memcpy(transX,X);

  /* identity */
  gsl_matrix_float *R = gsl_matrix_float_alloc(n,n);
  gsl_matrix_float_set_identity(R);

  /* pseudo inverse */
  *pinvX = fmat_PseudoInv(transX,NULL);

  /* temporary matrix buffer for binary matrix operations */
  gsl_matrix_float *result;
  gsl_matrix_float *result2;

  result = fmat_x_mat(transX,*pinvX,NULL);
  gsl_matrix_float_sub(R,result);
  gsl_matrix_float_free(result);

  /* result buffer */
  gsl_matrix_float *M;

  if(numlags == 1){
    /* create a n-x-n matrix Diag1 where the k=1 sub- and suberdiagonals
     * are 1 and the remaining entries are 0.
     * Equivalent matlab code:
     *
     * keep = 1:n;
     * indk1=((keep(2:n)-keep(1:n-1))==1);
     * Diag1=diag(indk1,1)+diag(indk1,-1);
     */
    gsl_matrix_float *Diag1 = gsl_matrix_float_alloc(n,n);
    gsl_matrix_float_set_zero(Diag1);
    /* set superdiagonal to 1 */
    gsl_vector_float_view superview = gsl_matrix_float_superdiagonal(Diag1,1);
    gsl_vector_float_set_all(&superview.vector,1);
    /* set subdiagonal to 1 */
    gsl_vector_float_view subview = gsl_matrix_float_subdiagonal(Diag1,1);
    gsl_vector_float_set_all(&subview.vector,1);

    float M11 = trace(R);
    result = fmat_x_mat(R, Diag1, NULL);
    float M12 = trace(result);
    float M21 = M12/2.0;
    fmat_x_mat(R,Diag1,result);
    result2 = fmat_x_mat(result,R,NULL);
    fmat_x_mat(result2,Diag1,result);
    float M22 = trace(result)/2;
    gsl_matrix_float_free(result);
    gsl_matrix_float_free(result2);

    M = gsl_matrix_float_alloc(2,2);

    /* M=[M11 M12; M21 M22] */
    gsl_matrix_float_set(M,0,0,M11);
    gsl_matrix_float_set(M,0,1,M12);
    gsl_matrix_float_set(M,1,0,M21);
    gsl_matrix_float_set(M,1,1,M22);

    gsl_matrix_float_free(Diag1);

  }
  else {

    int i,j;
    M = gsl_matrix_float_alloc(numlags+1,numlags+1);
    gsl_matrix_float_set_zero(M);
    gsl_matrix_float* Di = gsl_matrix_float_alloc(n,n);
    gsl_matrix_float* Dj = gsl_matrix_float_alloc(n,n);

    /* This double loop can be supported by a look-up-table for
     * the diagonal matrixes. */
    for(i=0;i<numlags+1;i++){
      gsl_matrix_float_set_zero(Di);
      /* set maindiagonal to 1 */
      if(i==0){
	gsl_vector_float_view mdiag = gsl_matrix_float_diagonal(Di);
	gsl_vector_float_set_all(&mdiag.vector,1);
      }
      /* set i. super- and subdiagonal to 1 */
      else {
	/* set superdiagonal to 1 */
	gsl_vector_float_view superview = gsl_matrix_float_superdiagonal(Di,i);
	gsl_vector_float_set_all(&superview.vector,1);
	/* set subdiagonal to 1 */
	gsl_vector_float_view subview = gsl_matrix_float_subdiagonal(Di,i);
	gsl_vector_float_set_all(&subview.vector,1);
      }
      for(j=0;j<numlags+1;j++){
	gsl_matrix_float_set_zero(Dj);
	/* set maindiagonal to 1 */
	if(j==0){
	  gsl_vector_float_view mdiag = gsl_matrix_float_diagonal(Dj);
	  gsl_vector_float_set_all(&mdiag.vector,1);
	}
	/* set j. super- and subdiagonal to 1 */
	else {
	  /* set superdiagonal to 1 */
	  gsl_vector_float_view superview = gsl_matrix_float_superdiagonal(Dj,j);
	  gsl_vector_float_set_all(&superview.vector,1);
	  /* set subdiagonal to 1 */
	  gsl_vector_float_view subview = gsl_matrix_float_subdiagonal(Dj,j);
	  gsl_vector_float_set_all(&subview.vector,1);
	}

	result = fmat_x_mat(R,Di,NULL);
	result2 = fmat_x_mat(result,R,NULL);
	fmat_x_mat(result2,Dj,result);
	gsl_matrix_float_free(result2);

	gsl_matrix_float_set(M,i,j,trace(result)/(1+(i>0)));
	gsl_matrix_float_free(result);

      }
    }

    gsl_matrix_float_free(Di);
    gsl_matrix_float_free(Dj);
  }

  /* calculation of the return values */
  *invM = fInv(M,NULL);
  /* pinvX has already been initialized */

  /* free memory */
  gsl_matrix_float_free(transX);
  gsl_matrix_float_free(R);
  gsl_matrix_float_free(M);
}


/*
 * test_suite.h
 *
 *  Created on: Oct 16, 2013
 *      Author: vbonnici
 */

#ifndef TEST_SUITE_H_
#define TEST_SUITE_H_

/*
 * a set of functions for binomial tests, or other distribution
 */

//#define DEBUG_TEST_SUITE_H_

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

#include <vector>
#include <limits>

#include "data_ts.h"

#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>

namespace dolierlib{

/*
 * Pietro's binomial test
 */
inline
double dolier_binomial(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total){
	double  p = (double)b_dcount / (double)b_total;
	double result = 0;
	for(usize_t i = f_dcount; i<=f_total; i++){
		result += gsl_ran_binomial_pdf(static_cast<unsigned int>(i), p, static_cast<unsigned int>(f_total));
	}
	return result;
}

/*
 * a more efficient binomial test, thanks to gsl
 */
inline
double dolier_binomial_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total){
	double  p = (double)b_dcount / (double)b_total;
	//return gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total));
	return gsl_ran_binomial_pdf(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
			+ gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total));
}

/*
 * Pietro's binomial test with Bon Ferroni correction
 */
inline
double dolier_binomial_bonf(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total, const usize_t nof_kmers){
	double p = (double)b_dcount / (double)b_total;
	double result = 0;
	for(usize_t i = f_dcount; i<=f_total; i++){
		result += gsl_ran_binomial_pdf(static_cast<unsigned int>(i), p, static_cast<unsigned int>(f_total));
	}
	return double(nof_kmers) * result;
}

/*
 * a more efficient binomial test with Bon Ferroni correction, thanks to gsl
 */
inline
double dolier_binomial_bonf_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total, const usize_t nof_kmers){
	double  p = (double)b_dcount / (double)b_total;
	//return gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total)) * nof_kmers;
	return (
			gsl_ran_binomial_pdf(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
			+
			gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
			)
			* nof_kmers;
}

/*
 * other distributions tried during tests
 */

inline
double dolier_poisson_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total){
	double  p = (double)b_dcount / (double)b_total;
	double mu = p *  static_cast<double>(f_total);
	return gsl_ran_poisson_pdf(static_cast<unsigned int>(f_dcount),mu)
			+ gsl_cdf_poisson_Q(static_cast<unsigned int>(f_dcount), mu);
//	return gsl_ran_binomial_pdf(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
//			+ gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total));
}

inline
double dolier_poisson_bonf_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total, const usize_t nof_kmers){
	double  p = (double)b_dcount / (double)b_total;
	double mu = p *  static_cast<double>(f_total);
	if(mu==0) std::cout<<"#mu=0#\t"<<f_dcount<<"\t"<<f_total<<"\t"<<b_dcount<<"\t"<<b_total<<"\t"<<p<<"\n";
	return 		(gsl_ran_poisson_pdf(static_cast<unsigned int>(f_dcount),mu)
				+ gsl_cdf_poisson_Q(static_cast<unsigned int>(f_dcount), mu))
			* nof_kmers;
//	return (
//			gsl_ran_binomial_pdf(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
//			+
//			gsl_cdf_binomial_Q(static_cast<unsigned int>(f_dcount), p, static_cast<unsigned int>(f_total))
//			)
//			* nof_kmers;
}

inline
double dolier_poisson_mu1_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total){
	return gsl_ran_poisson_pdf(static_cast<unsigned int>(f_dcount),1)
			+ gsl_cdf_poisson_Q(static_cast<unsigned int>(f_dcount), 1);
}
inline
double dolier_poisson_mu1_bonf_Q(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total, const usize_t nof_kmers){
	return 		(gsl_ran_poisson_pdf(static_cast<unsigned int>(f_dcount),1)
				+ gsl_cdf_poisson_Q(static_cast<unsigned int>(f_dcount), 1))
			* nof_kmers;
}

const double SQRT_PI  = sqrt(3.141592653589793238462);

inline
double dolier_normal(double x, double sigma, double mu){
	return (1/(sigma * SQRT_PI))*(exp(-(((x-mu)*(x-mu)) / (2*sigma*sigma))));
}

inline
double dolier_approx_binomial(const usize_t f_dcount, const usize_t f_total, const usize_t b_dcount, const usize_t b_total){
	double  p = (double)b_dcount / (double)b_total;
	double result = 0;

	double np = static_cast<double>(f_total) * p;
	double npp = np * (1-p);

	for(usize_t i = f_dcount; i<=f_total; i++){
		result += dolier_normal(static_cast<double>(i), np, npp);
	}
	return result;
}

inline
double dolier_binomial(unsigned int x, double p, unsigned int total){
	return gsl_ran_binomial_pdf(x, p, total);
}

inline
double dolier_normall(unsigned int x, double p, unsigned int total){
	double np = static_cast<double>(total) * p;
	double npp = np * (1-p);
	return dolier_normal(static_cast<double>(x), np, npp);
}

}

#endif /* TEST_SUITE_H_ */


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_core.h>
#include <gbpCosmo_NFW_etc.h>
#include <gsl/gsl_sf_expint.h>

// FFT of NFW profile from White '01
double rho_NFW_fft(double k, double M_vir, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double r_s;
    double z_k;
    double g_c;
    double rho_o;
    double x1;
    double x2;
    double Ci1;
    double Ci2;
    double Si1;
    double Si2;
    double r_val;
    set_NFW_params(M_vir, z, mode, cosmo, &c_vir, &R_vir);
    r_s   = R_vir / c_vir;
    z_k   = k * r_s;
    g_c   = 1. / (log(1. + c_vir) - c_vir / (1. + c_vir));
    rho_o = M_vir * g_c / (FOUR_PI * r_s * r_s * r_s);
    x1    = 1. + c_vir;
    x2    = c_vir * z_k;
    Ci1   = gsl_sf_Ci((1. + c_vir) * z_k);
    Si1   = gsl_sf_Si((1. + c_vir) * z_k);
    Ci2   = gsl_sf_Ci(z_k);
    Si2   = gsl_sf_Si(z_k);
    r_val = (FOUR_PI * rho_o * r_s * r_s * r_s / M_vir) * (cos(z_k) * (Ci1 - Ci2) + sin(z_k) * (Si1 - Si2) - sin(c_vir * z_k) / (z_k * (1. + c_vir)));
    return (r_val);
}