/*

  simulate NMR peakshape for two-site chemical exchange

  written by Sung-Hun Bae
  Nov. 17, 2008

*/

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <cmath>
#include <complex>
#include <gsl/gsl_fft_complex.h>

#define TINY  1.0e-12

using namespace std;

enum SITE_LABEL { A, B, N_SITE };
enum QUADRATURE { REAL, IMAG };
typedef struct {
  double Omegaa;// frequency of A site (rad/s)
  double Omegab;// frequency of B site (rad/s)
  double kab;	// reaction rate constant, A -> B (1/s)
  double kba;	// reaction rate constant, B -> A (1/s)
  double R20a;	// transverse relaxation rate of A site (1/s)
  double R20b;	// transverse relaxation rate of B site	(1/s)
  double M0a;	// initial magnetization
  double M0b;	// initial magnetization
  SITE_LABEL site;	// A or B
  QUADRATURE quad;	// REAL or IMAG
  } two_site_exchange;

//
// Magnetization (two-site exchange)
//

void N (double t, double &R, double &I, void * params) {
  two_site_exchange * x = (two_site_exchange *) params;
  complex<double> c1,c2,c3,lp,lm,a11,a12,a21,a22;
  complex<double> Omegaa = sqrt(complex<double>(-1.))*x->Omegaa;
  complex<double> Omegab = sqrt(complex<double>(-1.))*x->Omegab;
  double mag;
  c1 = (-Omegaa - Omegab + x->R20a + x->R20b + x->kab + x->kba);
  c2 = (-Omegaa + Omegab + x->R20a - x->R20b + x->kab - x->kba);
  c3 = sqrt(c2*c2+4.*x->kab*x->kba);
  lp = 0.5*(c1+c3);
  lm = 0.5*(c1-c3);
  if (x->site == A) {
    a11 = 0.5*((1.-c2/c3)*exp(-t*lm)+(1.+c2/c3)*exp(-t*lp));
    a12 = x->kba/c3*(exp(-t*lm)-exp(-t*lp));
    R += real(a11*x->M0a+a12*x->M0b);
    I += imag(a11*x->M0a+a12*x->M0b);
    }
  if (x->site == B) {
    a22 = 0.5*((1.+c2/c3)*exp(-t*lm)+(1.-c2/c3)*exp(-t*lp));
    a21 = x->kab/c3*(exp(-t*lm)-exp(-t*lp));
    R += real(a21*x->M0a+a22*x->M0b);
    I += imag(a21*x->M0a+a22*x->M0b);
    }
}

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

  if (argc < 7 || argc > 17) {
    printf("\t\t\t\t\t\t\tSung-Hun Bae 2008.11\n");
    printf("   ");
    printf("Usage: xpshape [-model AB|ACB]\n");
    printf("   ");
    printf("               [-dw #(Hz)] [-kab #(1/s)] [-kba #(1/s)]\n");
    printf("   ");
    printf("               [-R20a #(1/s)] [-R20b #(1/s)]\n");
    printf("   ");
    printf("               [-a0 conc(M)] [-c0 conc(M)]\n");
    printf("   ");
    printf("               [-swh Hz(600)] [-fid pts(8192)]\n\n");
    printf("   ");
    printf("simulate NMR peakshape for two-site chemical exchange\n\n");
    printf("   ");
    printf("-model,AB  : A = B     (dw,kab,kba,R20a,R20b required)\n");
    printf("   ");
    printf("       ACB : A + C = B (dw,kab,kba,R20a,R20b,a0,c0 required)\n");
    printf("\n");
    printf("   ");
    printf("-dw,  (A.frequency= -dw/2, B.frequency= +dw/2)\n");
    printf("   ");
    printf("-R20a,(exchange-free R2 of A state)\n");
    printf("   ");
    printf("-R20b,(exchange-free R2 of B state)\n");
    printf("   ");
    printf("-kab, (rate constant of A->B)\n");
    printf("   ");
    printf("-kba, (rate constant of B->A)\n");
    printf("   ");
    printf("-a0,  (initial concentration of A)\n");
    printf("   ");
    printf("-c0,  (initial concentration of C)\n");
    printf("   ");
    printf("-swh, (spectrum = -swh/2 ... +swh/2)\n");
    printf("   ");
    printf("-fid, (must be ...,1024,2048,4096,8192,16384,32768,65536,..)\n");
    printf("\n");

    exit (1);
    }

  // default settings

  double SWH = 600;		  // (Hz)
  unsigned int FID_SIZE = 8192;	  // (complex data points)

  // receiving parameters

  unsigned int i = 1;

  string model;
  double dw,R20a,R20b,kab,kba,concA0,concC0;
  bool dw_,R20a_,R20b_,kab_,kba_,concA0_,concC0_;
  double pb,Kd,concA,concB,concC,C,u,v;

  dw_ = R20a_ = R20b_ = kab_ = kba_ = concA0_ = concC0_ = false;

  while ((argc - i) > 0) {
    if ((argc-i) > 0 && !strcmp(argv[i],"-model")) {
      model = string(argv[++i]);
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-dw")) {
      dw = 2.*M_PI*atof(argv[++i]); // (Hz) -> (rad/s)
      dw_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-kab")) {
      kab = atof(argv[++i]);
      kab_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-kba")) {
      kba = atof(argv[++i]);
      kba_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-R20a")) {
      R20a = atof(argv[++i]);
      R20a_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-R20b")) {
      R20b = atof(argv[++i]);
      R20b_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-a0")) {
      concA0 = atof(argv[++i]);
      concA0_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-c0")) {
      concC0 = atof(argv[++i]);
      concC0_ = true;
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-swh")) {
      SWH = atof(argv[++i]);
      i++;
      }
    if ((argc-i) > 0 && !strcmp(argv[i],"-fid")) {
      FID_SIZE = atoi(argv[++i]);
      i++;
      }
    }

  double fid[2*FID_SIZE];	    // complex points
  double dwell = 1./(SWH);	    // (sec)
  double norm = 1./sqrt(FID_SIZE);  // scaling factor

  if (model == "AB" && dw_ && R20a_ && R20b_ && kab_ && kba_) {
    // dw, R20a, R20b, kab, kba
    Kd = kba/kab;
    pb = 1./(1.+Kd);
    C = 1.0;
    }
  else if (model == "ACB" && dw_ && R20a_ && R20b_ && kab_ && kba_
    && concA0_ && concC0_) {
    // dw, R20a, R20b, kab(=kon), kba(=koff), concA0, concC0
    kab = (concC0 > TINY ? kab : 0.0);
    kba = (concC0 > TINY ? kba : 0.0);
    Kd = ((kab > TINY) && (kba > TINY) ? kba/kab : 0.0);
    u = 0.5*(concA0+concC0+Kd);
    v = (concC0 > TINY ? (u-sqrt(u*u-concA0*concC0)) : 0.0);
    concA = (v > TINY ? concA0-v : concA0);
    concB = (v > TINY ? v : 0.0);
    concC = (v > TINY ? concC0 - v : concC0);
    pb = (concC0 > TINY ? kab*concC/(kab*concC+kba) : 0.0);
    C = concC;
    }
  else {
    printf("error: missing parameter(s)\n");
    exit(1);
    }

  two_site_exchange x = {-dw/2.,dw/2.,kab*C,kba,R20a,R20b,(1.-pb),pb,A,REAL};

  // header information
  printf("# peak shape simulation ");
  if (model == "AB")  printf("[A = B]\n");
  if (model == "ACB") printf("[A + C = B]\n");

  printf("# dw     = %g (rad/s)\n", dw);
  printf("# kab    = %g (1/s)\n",kab);
  printf("# kba    = %g (1/s)\n",kba);
  printf("# R20a   = %g (1/s)\n",R20a);
  printf("# R20b   = %g (1/s)\n",R20b);
  if (model == "AB") {
  printf("# pb     = %g\n", pb);
  }
  if (model == "ACB") {
  printf("# kon    = %g (1/s)\n",kab);
  printf("# koff   = %g (1/s)\n",kba);
  printf("# Kd     = %g (M)\n",Kd);
  printf("# pb     = %g\n",pb);
  printf("# [A]ini = %g (M)\n",concA0);
  printf("# [C]ini = %g (M)\n",concC0);
  printf("# [A]    = %g (M)\n",concA);
  printf("# [C]    = %g (M)\n",concC);
  printf("# [B]    = %g (M)\n",concB);
  }
  printf("# FID    = %d (complex points)\n",FID_SIZE);
  printf("# SWH    = %g (Hz)\n",SWH);
  printf("# X-axis unit: (Hz)\n");

  // creating complex data points
  for (i = 0; i < FID_SIZE; i++) {
    fid[2*i] = fid[2*i+1] = 0.0;
    x.site = A;
    N (i*dwell, fid[2*i], fid[2*i+1], &x);  // A
    x.site = B;
    N (i*dwell, fid[2*i], fid[2*i+1], &x);  // B
    }

  // Fast Fourier Transform (FFT)
  gsl_fft_complex_radix2_forward (fid, 1, FID_SIZE);

  // spectrum
  for (i = FID_SIZE/2; i < FID_SIZE; i++)
    printf("%g %g\n",(-0.5*SWH + SWH*(i-FID_SIZE/2)/FID_SIZE),norm*fid[2*i]);
  for (i = 0; i <= FID_SIZE/2; i++)
    printf("%g %g\n",(-0.5*SWH + SWH*(i+FID_SIZE/2)/FID_SIZE),norm*fid[2*i]);
  printf("&\n");

  return 0;
}

/*

  For physical applications it is important to remember that the index
  appearing in the DFT does not correspond directly to a physical frequency.
  If the time-step of the DFT is \Delta then the frequency-domain includes
  both positive and negative frequencies, ranging from -1/(2\Delta) through
  0 to +1/(2\Delta).
  The positive frequencies are stored from the beginning of the array up to
  the middle, and the negative frequencies are stored backwards from the
  end of the array.

     index    z               x = FFT(z)

     0        z(t = 0)        x(f = 0)
     1        z(t = 1)        x(f = 1/(N Delta))
     2        z(t = 2)        x(f = 2/(N Delta))
     .        ........        ..................
     N/2      z(t = N/2)      x(f = +1/(2 Delta),
                                    -1/(2 Delta))
     .        ........        ..................
     N-3      z(t = N-3)      x(f = -3/(N Delta))
     N-2      z(t = N-2)      x(f = -2/(N Delta))
     N-1      z(t = N-1)      x(f = -1/(N Delta))

*/


/* 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__ */


/* 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);
}


// 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


#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


#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;
}


/* -*- 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;
}


/*
 * 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);
}


/* linalg/cod.c
 *
 * Copyright (C) 2016, 2017 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.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permute_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>

/*
 * This module contains routines for factoring an M-by-N matrix A as:
 *
 * A P = Q R Z^T
 *
 * known as the Complete Orthogonal Decomposition, where:
 *
 * P is a N-by-N permutation matrix
 * Q is M-by-M orthogonal
 * R has an r-by-r upper triangular block
 * Z is N-by-N orthogonal
 *
 * When A is full rank, Z = I and this becomes the QR decomposition
 * with column pivoting. When A is rank deficient, then
 *
 * R = [ R11 0 ] where R11 is r-by-r and r = rank(A)
 *     [  0  0 ]
 */

static int cod_RZ(gsl_matrix * A, gsl_vector * tau);
static double cod_householder_transform(double *alpha, gsl_vector * v);
static int cod_householder_mh(const double tau, const gsl_vector * v,
                              gsl_matrix * A, gsl_vector * work);
static int cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w);
static int cod_householder_Zvec(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
                                gsl_vector * v);
static int cod_trireg_solve(const gsl_matrix * R, const double lambda, const gsl_vector * b,
                            gsl_matrix * S, gsl_vector * x, gsl_vector * work);

int
gsl_linalg_COD_decomp_e(gsl_matrix * A, gsl_vector * tau_Q, gsl_vector * tau_Z,
                        gsl_permutation * p, double tol, size_t * rank, gsl_vector * work)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau_Q->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (tau_Z->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (p->size != N)
    {
      GSL_ERROR ("permutation size must be N", GSL_EBADLEN);
    }
  else if (work->size != N)
    {
      GSL_ERROR ("work size must be N", GSL_EBADLEN);
    }
  else
    {
      int status, signum;
      size_t r;

      /* decompose: A P = Q R */
      status = gsl_linalg_QRPT_decomp(A, tau_Q, p, &signum, work);
      if (status)
        return status;

      /* estimate rank of A */
      r = gsl_linalg_QRPT_rank(A, tol);

      if (r < N)
        {
          /*
           * matrix is rank-deficient, so that the R factor is
           *
           * R = [ R11 R12 ] =~ [ R11 R12 ]
           *     [  0  R22 ]    [  0   0  ]
           *
           * compute RZ decomposition of upper trapezoidal matrix
           * [ R11 R12 ] = [ R11~ 0 ] Z
           */
          gsl_matrix_view R_upper = gsl_matrix_submatrix(A, 0, 0, r, N);
          gsl_vector_view t = gsl_vector_subvector(tau_Z, 0, r);

          cod_RZ(&R_upper.matrix, &t.vector);
        }

      *rank = r;

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_COD_decomp(gsl_matrix * A, gsl_vector * tau_Q, gsl_vector * tau_Z,
                      gsl_permutation * p, size_t * rank, gsl_vector * work)
{
  return gsl_linalg_COD_decomp_e(A, tau_Q, tau_Z, p, -1.0, rank, work);
}

/*
gsl_linalg_COD_lssolve()
  Find the least squares solution to the overdetermined system

min ||b - A x||^2

for M >= N using the COD factorization A P = Q R Z

Inputs: QRZT     - matrix A, in COD compressed format, M-by-N
        tau_Q    - Householder scalars for Q, length min(M,N)
        tau_Z    - Householder scalars for Z, length min(M,N)
        perm     - permutation matrix
        rank     - rank of A
        b        - rhs vector, length M
        x        - (output) solution vector, length N
        residual - (output) residual vector, b - A x, length M
*/

int
gsl_linalg_COD_lssolve (const gsl_matrix * QRZT, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
                        const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
                        gsl_vector * x, gsl_vector * residual)
{
  const size_t M = QRZT->size1;
  const size_t N = QRZT->size2;

  if (M < N)
    {
      GSL_ERROR ("QRZT matrix must have M>=N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (rank > GSL_MIN (M, N))
    {
      GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (M != residual->size)
    {
      GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZT, 0, 0, rank, rank);
      gsl_vector_view QTb1 = gsl_vector_subvector(residual, 0, rank);
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, rank);

      gsl_vector_set_zero(x);

      /* compute residual = Q^T b = [ c1 ; c2 ] */
      gsl_vector_memcpy(residual, b);
      gsl_linalg_QR_QTvec (QRZT, tau_Q, residual);

      /* solve x1 := R11^{-1} (Q^T b)(1:r) */
      gsl_vector_memcpy(&(x1.vector), &(QTb1.vector));
      gsl_blas_dtrsv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), &(x1.vector));

      /* compute Z ( R11^{-1} x1; 0 ) */
      cod_householder_Zvec(QRZT, tau_Z, rank, x);

      /* compute x = P Z^T ( R11^{-1} x1; 0 ) */
      gsl_permute_vector_inverse(perm, x);

      /* compute residual = b - A x = Q (Q^T b - R [ R11^{-1} x1; 0 ]) = Q [ 0 ; c2 ] */
      gsl_vector_set_zero(&(QTb1.vector));
      gsl_linalg_QR_Qvec(QRZT, tau_Q, residual);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_COD_lssolve2()
  Find the least squares solution to the Tikhonov regularized
system in standard form:

min ||b - A x||^2 + lambda^2 ||x||^2

for M >= N using the COD factorization A P = Q R Z

Inputs: lambda   - parameter
        QRZT     - matrix A, in COD compressed format, M-by-N
        tau_Q    - Householder scalars for Q, length min(M,N)
        tau_Z    - Householder scalars for Z, length min(M,N)
        perm     - permutation matrix
        rank     - rank of A
        b        - rhs vector, length M
        x        - (output) solution vector, length N
        residual - (output) residual vector, b - A x, length M
        S        - workspace, rank-by-rank
        work     - workspace, length rank
*/

int
gsl_linalg_COD_lssolve2 (const double lambda, const gsl_matrix * QRZT, const gsl_vector * tau_Q, const gsl_vector * tau_Z,
                         const gsl_permutation * perm, const size_t rank, const gsl_vector * b,
                         gsl_vector * x, gsl_vector * residual, gsl_matrix * S, gsl_vector * work)
{
  const size_t M = QRZT->size1;
  const size_t N = QRZT->size2;

  if (M < N)
    {
      GSL_ERROR ("QRZT matrix must have M>=N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (rank > GSL_MIN (M, N))
    {
      GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (M != residual->size)
    {
      GSL_ERROR ("matrix size must match residual size", GSL_EBADLEN);
    }
  else if (S->size1 != rank || S->size2 != rank)
    {
      GSL_ERROR ("S must be rank-by-rank", GSL_EBADLEN);
    }
  else if (work->size != rank)
    {
      GSL_ERROR ("work must be length rank", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view R11 = gsl_matrix_const_submatrix (QRZT, 0, 0, rank, rank);
      gsl_vector_view c1 = gsl_vector_subvector(residual, 0, rank);
      gsl_vector_view y1 = gsl_vector_subvector(x, 0, rank);

      gsl_vector_set_zero(x);

      /* compute residual = Q^T b = [ c1 ; c2 ]*/
      gsl_vector_memcpy(residual, b);
      gsl_linalg_QR_QTvec (QRZT, tau_Q, residual);

      /* solve [ R11 ; lambda*I ] y1 = [ (Q^T b)(1:r) ; 0 ] */
      cod_trireg_solve(&(R11.matrix), lambda, &(c1.vector), S, &(y1.vector), work);

      /* save y1 for later residual calculation */
      gsl_vector_memcpy(work, &(y1.vector));

      /* compute Z [ y1; 0 ] */
      cod_householder_Zvec(QRZT, tau_Z, rank, x);

      /* compute x = P Z^T ( y1; 0 ) */
      gsl_permute_vector_inverse(perm, x);

      /* compute residual = b - A x = Q (Q^T b - [ R11 y1; 0 ]) = Q [ c1 - R11*y1 ; c2 ] */

      /* work = R11*y1 */
      gsl_blas_dtrmv(CblasUpper, CblasNoTrans, CblasNonUnit, &(R11.matrix), work);

      gsl_vector_sub(&(c1.vector), work);
      gsl_linalg_QR_Qvec(QRZT, tau_Q, residual);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_COD_unpack()
  Unpack encoded COD decomposition into the matrices Q,R,Z,P

Inputs: QRZT  - encoded COD decomposition
        tau_Q - Householder scalars for Q
        tau_Z - Householder scalars for Z
        rank  - rank of matrix (as determined from gsl_linalg_COD_decomp)
        Q     - (output) M-by-M matrix Q
        R     - (output) M-by-N matrix R
        Z     - (output) N-by-N matrix Z
*/

int
gsl_linalg_COD_unpack(const gsl_matrix * QRZT, const gsl_vector * tau_Q,
                      const gsl_vector * tau_Z, const size_t rank, gsl_matrix * Q,
                      gsl_matrix * R, gsl_matrix * Z)
{
  const size_t M = QRZT->size1;
  const size_t N = QRZT->size2;

  if (tau_Q->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau_Q must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (tau_Z->size != GSL_MIN (M, N))
    {
      GSL_ERROR ("size of tau_Z must be MIN(M,N)", GSL_EBADLEN);
    }
  else if (rank > GSL_MIN (M, N))
    {
      GSL_ERROR ("rank must be <= MIN(M,N)", GSL_EBADLEN);
    }
  else if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q must by M-by-M", GSL_EBADLEN);
    }
  else if (R->size1 != M || R->size2 != N)
    {
      GSL_ERROR ("R must by M-by-N", GSL_EBADLEN);
    }
  else if (Z->size1 != N || Z->size2 != N)
    {
      GSL_ERROR ("Z must by N-by-N", GSL_EBADLEN);
    }
  else
    {
      size_t i;
      gsl_matrix_view R11 = gsl_matrix_submatrix(R, 0, 0, rank, rank);
      gsl_matrix_const_view QRZT11 = gsl_matrix_const_submatrix(QRZT, 0, 0, rank, rank);

      /* form Q matrix */

      gsl_matrix_set_identity(Q);

      for (i = GSL_MIN (M, N); i-- > 0;)
        {
          gsl_vector_const_view h = gsl_matrix_const_subcolumn (QRZT, i, i, M - i);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, i, i, M - i, M - i);
          double ti = gsl_vector_get (tau_Q, i);
          gsl_linalg_householder_hm (ti, &h.vector, &m.matrix);
        }

      /* form Z matrix */
      gsl_matrix_set_identity(Z);

      if (rank < N)
        {
          gsl_vector_view work = gsl_matrix_row(R, 0); /* temporary workspace, size N */

          /* multiply I by Z from the right */
          gsl_linalg_COD_matZ(QRZT, tau_Z, rank, Z, &work.vector);
        }

      /* copy rank-by-rank upper triangle of QRZT into R and zero the rest */
      gsl_matrix_set_zero(R);
      gsl_matrix_tricpy('U', 1, &R11.matrix, &QRZT11.matrix);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_COD_matZ
  Multiply an M-by-N matrix A on the right by Z (N-by-N)

Inputs: QRZT  - encoded COD matrix
        tau_Z - Householder scalars for Z
        rank  - matrix rank
        A     - on input, M-by-N matrix
                on output, A * Z
        work  - workspace of length M
*/

int
gsl_linalg_COD_matZ(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
                    gsl_matrix * A, gsl_vector * work)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau_Z->size != GSL_MIN (QRZT->size1, QRZT->size2))
    {
      GSL_ERROR("tau_Z must be GSL_MIN(M,N)", GSL_EBADLEN);
    }
  else if (QRZT->size2 != N)
    {
      GSL_ERROR("QRZT must have N columns", GSL_EBADLEN);
    }
  else if (work->size != M)
    {
      GSL_ERROR("workspace must be length M", GSL_EBADLEN);
    }
  else
    {
      /* if rank == N, then Z = I and there is nothing to do */
      if (rank < N)
        {
          size_t i;

          for (i = rank; i > 0 && i--; )
            {
              gsl_vector_const_view h = gsl_matrix_const_subrow (QRZT, i, rank, N - rank);
              gsl_matrix_view m = gsl_matrix_submatrix (A, 0, i, M, N - i);
              double ti = gsl_vector_get (tau_Z, i);
              cod_householder_mh (ti, &h.vector, &m.matrix, work);
            }
        }

      return GSL_SUCCESS;
    }
}

/*********************************************
 * INTERNAL ROUTINES                         *
 *********************************************/

/*
cod_RZ()
  Perform RZ decomposition of an upper trapezoidal matrix,

A = [ A11 A12 ] = [ R 0 ] Z

where A is M-by-N with N >= M, A11 is M-by-M upper triangular,
and A12 is M-by-(N-M). On output, Z is stored as Householder
reflectors in the A12 portion of A,

Z = Z(1) Z(2) ... Z(M)

Inputs: A   - M-by-N matrix with N >= M
              On input, upper trapezoidal matrix [ A11 A12 ]
              On output, A11 is overwritten by R (subdiagonal elements
              are not touched), and A12 is overwritten by Z in packed storage
        tau - (output) Householder scalars, size M
*/

static int
cod_RZ(gsl_matrix * A, gsl_vector * tau)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (tau->size != M)
    {
      GSL_ERROR("tau has wrong size", GSL_EBADLEN);
    }
  else if (N < M)
    {
      GSL_ERROR("N must be >= M", GSL_EINVAL);
    }
  else if (M == N)
    {
      /* quick return */
      gsl_vector_set_all(tau, 0.0);
      return GSL_SUCCESS;
    }
  else
    {
      size_t k;

      for (k = M; k > 0 && k--; )
        {
          double *alpha = gsl_matrix_ptr(A, k, k);
          gsl_vector_view z = gsl_matrix_subrow(A, k, M, N - M);
          double tauk;

          /* compute Householder reflection to zero [ A(k,k) A(k,M+1:N) ] */
          tauk = cod_householder_transform(alpha, &z.vector);
          gsl_vector_set(tau, k, tauk);

          if ((tauk != 0) && (k > 0))
            {
              gsl_vector_view w = gsl_vector_subvector(tau, 0, k);
              gsl_matrix_view B = gsl_matrix_submatrix(A, 0, k, k, N - k);

              cod_householder_mh(tauk, &z.vector, &B.matrix, &w.vector);
            }
        }

      return GSL_SUCCESS;
    }
}

static double
cod_householder_transform(double *alpha, gsl_vector * v)
{
  double beta, tau;
  double xnorm = gsl_blas_dnrm2(v);

  if (xnorm == 0)
    {
      return 0.0; /* tau = 0 */
    }

  beta = - (*alpha >= 0.0 ? +1.0 : -1.0) * gsl_hypot(*alpha, xnorm);
  tau = (beta - *alpha) / beta;

  {
    double s = (*alpha - beta);

    if (fabs(s) > GSL_DBL_MIN)
      {
        gsl_blas_dscal (1.0 / s, v);
      }
    else
      {
        gsl_blas_dscal (GSL_DBL_EPSILON / s, v);
        gsl_blas_dscal (1.0 / GSL_DBL_EPSILON, v);
      }

    *alpha = beta;
  }

  return tau;
}

/*
cod_householder_hv
  Apply Householder reflection H = (I - tau*v*v') to vector v from the left,

w' = H * w

Inputs: tau  - Householder scalar
        v    - Householder vector, size M
        w    - on input, w vector, size M
               on output, H * w

Notes:
1) Based on LAPACK routine DLARZ
*/

static int
cod_householder_hv(const double tau, const gsl_vector * v, gsl_vector * w)
{
  if (tau == 0)
    {
      return GSL_SUCCESS; /* H = I */
    }
  else
    {
      const size_t M = w->size;
      const size_t L = v->size;
      double w0 = gsl_vector_get(w, 0);
      gsl_vector_view w1 = gsl_vector_subvector(w, M - L, L);
      double d1, d;

      /* d1 := v . w(M-L:M) */
      gsl_blas_ddot(v, &w1.vector, &d1);

      /* d := w(1) + v . w(M-L:M) */
      d = w0 + d1;

      /* w(1) = w(1) - tau * d */
      gsl_vector_set(w, 0, w0 - tau * d);

      /* w(M-L:M) = w(M-L:M) - tau * d * v */
      gsl_blas_daxpy(-tau * d, v, &w1.vector);

      return GSL_SUCCESS;
    }
}

/*
cod_householder_mh
  Apply Householder reflection H = (I - tau*v*v') to matrix A from the right

Inputs: tau  - Householder scalar
        v    - Householder vector, size N-M
        A    - matrix, size M-by-N
        work - workspace, size M

Notes:
1) Based on LAPACK routine DLARZ
*/

static int
cod_householder_mh(const double tau, const gsl_vector * v, gsl_matrix * A,
                   gsl_vector * work)
{
  if (tau == 0)
    {
      return GSL_SUCCESS; /* H = I */
    }
  else
    {
      const size_t M = A->size1;
      const size_t N = A->size2;
      const size_t L = v->size;
      gsl_vector_view A1 = gsl_matrix_subcolumn(A, 0, 0, M);
      gsl_matrix_view C = gsl_matrix_submatrix(A, 0, N - L, M, L);

      /* work(1:M) = A(1:M,1) */
      gsl_vector_memcpy(work, &A1.vector);

      /* work(1:M) = work(1:M) + A(1:M,M+1:N) * v(1:N-M) */
      gsl_blas_dgemv(CblasNoTrans, 1.0, &C.matrix, v, 1.0, work);

      /* A(1:M,1) = A(1:M,1) - tau * work(1:M) */
      gsl_blas_daxpy(-tau, work, &A1.vector);

      /* A(1:M,M+1:N) = A(1:M,M+1:N) - tau * work(1:M) * v(1:N-M)' */
      gsl_blas_dger(-tau, work, v, &C.matrix);

      return GSL_SUCCESS;
    }
}

/*
cod_householder_Zvec
  Multiply a vector by Z

Inputs: QRZT  - encoded COD matrix
        tau_Z - Householder scalars for Z
        rank  - matrix rank
        v     - on input, vector of length N
                on output, Z^T * v
*/

static int
cod_householder_Zvec(const gsl_matrix * QRZT, const gsl_vector * tau_Z, const size_t rank,
                     gsl_vector * v)
{
  const size_t M = QRZT->size1;
  const size_t N = QRZT->size2;

  if (tau_Z->size != GSL_MIN (M, N))
    {
      GSL_ERROR("tau_Z must be GSL_MIN(M,N)", GSL_EBADLEN);
    }
  else if (v->size != N)
    {
      GSL_ERROR("v must be length N", GSL_EBADLEN);
    }
  else
    {
      if (rank < N)
        {
          size_t i;

          for (i = 0; i < rank; ++i)
            {
              gsl_vector_const_view h = gsl_matrix_const_subrow (QRZT, i, rank, N - rank);
              gsl_vector_view w = gsl_vector_subvector (v, i, N - i);
              double ti = gsl_vector_get (tau_Z, i);
              cod_householder_hv(ti, &h.vector, &w.vector);
            }
        }

      return GSL_SUCCESS;
    }
}

/*
cod_trireg_solve()

  This function computes the solution to the least squares system

  [    R     ] x = [ b ]
  [ lambda*I ]     [ 0 ]

where R is an N-by-N upper triangular matrix, lambda is a scalar parameter,
and b is a vector of length N. This is done by computing the QR factorization

[    R     ] = W S^T
[ lambda*I ]

where S^T is upper triangular, and solving

S^T x = W^T [ b ]
            [ 0 ]

Inputs: R      - full rank upper triangular matrix; the diagonal
                 elements are modified but restored on output
        lambda - scalar parameter lambda
        b      - right hand side vector b
        S      - workspace, N-by-N
        x      - (output) least squares solution of the system
        work   - workspace of length N
*/

static int
cod_trireg_solve (const gsl_matrix * R, const double lambda, const gsl_vector * b,
                  gsl_matrix * S, gsl_vector * x, gsl_vector * work)
{
  const size_t N = R->size2;
  gsl_vector_const_view diag = gsl_matrix_const_diagonal(R);
  size_t i, j, k;

  if (lambda <= 0.0)
    {
      GSL_ERROR("lambda must be positive", GSL_EINVAL);
    }

  /* copy R and b to preserve input and initialise S; store diag(R) in work */
  gsl_matrix_transpose_tricpy('U', 0, S, R);
  gsl_vector_memcpy(work, &diag.vector);
  gsl_vector_memcpy(x, b);

  /* eliminate the diagonal matrix lambda*I using Givens rotations */

  for (j = 0; j < N; j++)
    {
      double bj = 0.0;

      gsl_matrix_set (S, j, j, lambda);

      for (k = j + 1; k < N; k++)
        {
          gsl_matrix_set (S, k, k, 0.0);
        }

      /* the transformations to eliminate the row of lambda*I modify only a
         single element of b beyond the first n, which is initially
         zero */

      for (k = j; k < N; k++)
        {
          /* determine a Givens rotation which eliminates the
             appropriate element in the current row of lambda*I */

          double sine, cosine;

          double xk = gsl_vector_get (x, k);
          double rkk = gsl_vector_get (work, k);
          double skk = gsl_matrix_get (S, k, k);

          if (skk == 0)
            {
              continue;
            }

          if (fabs (rkk) < fabs (skk))
            {
              double cotangent = rkk / skk;
              sine = 0.5 / sqrt (0.25 + 0.25 * cotangent * cotangent);
              cosine = sine * cotangent;
            }
          else
            {
              double tangent = skk / rkk;
              cosine = 0.5 / sqrt (0.25 + 0.25 * tangent * tangent);
              sine = cosine * tangent;
            }

          /* Compute the modified diagonal element of r and the
             modified element of [b,0] */

          {
            double new_rkk = cosine * rkk + sine * skk;
            double new_xk = cosine * xk + sine * bj;

            bj = -sine * xk + cosine * bj;

            gsl_vector_set(work, k, new_rkk);
            gsl_matrix_set(S, k, k, new_rkk);
            gsl_vector_set(x, k, new_xk);
          }

          /* Accumulate the transformation in the row of s */

          for (i = k + 1; i < N; i++)
            {
              double sik = gsl_matrix_get (S, i, k);
              double sii = gsl_matrix_get (S, i, i);

              double new_sik = cosine * sik + sine * sii;
              double new_sii = -sine * sik + cosine * sii;

              gsl_matrix_set(S, i, k, new_sik);
              gsl_matrix_set(S, i, i, new_sii);
            }
        }
    }

  /* solve: S^T x = rhs in place */
  gsl_blas_dtrsv(CblasLower, CblasTrans, CblasNonUnit, S, x);

  return GSL_SUCCESS;
}


#ifdef HAVE_CONFIG_H
#include "config.h"
#endif

#include <php.h>
#include <cblas.h>
#include <lapacke.h>

#include "php_ext.h"
#include "kernel/operators.h"

void tensor_matmul(zval * return_value, zval * a, zval * b)
{
    unsigned int i, j;
    Bucket * row;
    zval rowC, c;

    zend_zephir_globals_def * zephir_globals = ZEPHIR_VGLOBAL;

    openblas_set_num_threads(zephir_globals->num_threads);

    zend_array * aHat = Z_ARR_P(a);
    zend_array * bHat = Z_ARR_P(b);

    Bucket * ba = aHat->arData;
    Bucket * bb = bHat->arData;

    unsigned int m = zend_array_count(aHat);
    unsigned int p = zend_array_count(bHat);
    unsigned int n = zend_array_count(Z_ARR(bb[0].val));

    double * va = emalloc(m * p * sizeof(double));
    double * vb = emalloc(n * p * sizeof(double));
    double * vc = emalloc(m * n * sizeof(double));

    for (i = 0; i < m; ++i) {
        row = Z_ARR(ba[i].val)->arData;

        for (j = 0; j < p; ++j) {
            va[i * p + j] = zephir_get_doubleval(&row[j].val);
        }
    }

    for (i = 0; i < p; ++i) {
        row = Z_ARR(bb[i].val)->arData;

        for (j = 0; j < n; ++j) {
            vb[i * n + j] = zephir_get_doubleval(&row[j].val);
        }
    }

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, p, 1.0, va, p, vb, n, 0.0, vc, n);

    array_init_size(&c, m);

    for (i = 0; i < m; ++i) {
        array_init_size(&rowC, n);

        for (j = 0; j < n; ++j) {
            add_next_index_double(&rowC, vc[i * n + j]);
        }

        add_next_index_zval(&c, &rowC);
    }

    RETVAL_ARR(Z_ARR(c));

    efree(va);
    efree(vb);
    efree(vc);
}

void tensor_dot(zval * return_value, zval * a, zval * b)
{
    unsigned int i;

    zend_array * aHat = Z_ARR_P(a);
    zend_array * bHat = Z_ARR_P(b);

    Bucket * ba = aHat->arData;
    Bucket * bb = bHat->arData;

    unsigned int n = zend_array_count(aHat);

    double sigma = 0.0;

    for (i = 0; i < n; ++i) {
        sigma += zephir_get_doubleval(&ba[i].val) * zephir_get_doubleval(&bb[i].val);
    }

    RETVAL_DOUBLE(sigma);
}

void tensor_inverse(zval * return_value, zval * a)
{
    unsigned int i, j;
    Bucket * row;
    zval rowB, b;

    zend_zephir_globals_def * zephir_globals = ZEPHIR_VGLOBAL;

    openblas_set_num_threads(zephir_globals->num_threads);

    zend_array * aHat = Z_ARR_P(a);

    Bucket * ba = aHat->arData;

    unsigned int n = zend_array_count(aHat);

    double * va = emalloc(n * n * sizeof(double));

    for (i = 0; i < n; ++i) {
        row = Z_ARR(ba[i].val)->arData;

        for (j = 0; j < n; ++j) {
            va[i * n + j] = zephir_get_doubleval(&row[j].val);
        }
    }

    lapack_int status;

    int pivots[n];

    status = LAPACKE_dgetrf(LAPACK_ROW_MAJOR, n, n, va, n, pivots);

    if (status != 0) {
        RETURN_NULL();
    }

    status = LAPACKE_dgetri(LAPACK_ROW_MAJOR, n, va, n, pivots);

    if (status != 0) {
        RETURN_NULL();
    }

    array_init_size(&b, n);

    for (i = 0; i < n; ++i) {
        array_init_size(&rowB, n);

        for (j = 0; j < n; ++j) {
            add_next_index_double(&rowB, va[i * n + j]);
        }

        add_next_index_zval(&b, &rowB);
    }

    RETVAL_ARR(Z_ARR(b));

    efree(va);
}