#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <assert.h>
#include <gsl/gsl_vector.h>

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

void reoshift(gsl_vector *v, int h)
{
  if ( h > 0 ) {
    gsl_vector *temp = gsl_vector_alloc(v->size);
    gsl_vector_view p = gsl_vector_subvector(v, 0, v->size - h);
    gsl_vector_view p1 = gsl_vector_subvector(temp, h, v->size - h);
    gsl_vector_memcpy(&p1.vector, &p.vector);
    p = gsl_vector_subvector(temp, 0, h);
    gsl_vector_set_zero(&p.vector);
    gsl_vector_memcpy(v, temp);
    gsl_vector_free(temp);
  }
}

gsl_vector *poly_long_div(gsl_vector *n, gsl_vector *d, gsl_vector **r)
{
  gsl_vector *nt = NULL, *dt = NULL, *rt = NULL, *d2 = NULL, *q = NULL;
  int gn, gt, gd;

  if ( (n->size >= d->size) && (d->size > 0) && (n->size > 0) ) {
    nt = gsl_vector_alloc(n->size); assert(nt != NULL);
    dt = gsl_vector_alloc(n->size); assert(dt != NULL);
    rt = gsl_vector_alloc(n->size); assert(rt != NULL);
    d2 = gsl_vector_alloc(n->size); assert(d2 != NULL);
    gsl_vector_memcpy(nt, n);
    gsl_vector_set_zero(dt); gsl_vector_set_zero(rt);
    gsl_vector_view p = gsl_vector_subvector(dt, 0, d->size);
    gsl_vector_memcpy(&p.vector, d);
    gsl_vector_memcpy(d2, dt);
    gn = n->size - 1;
    gd = d->size - 1;
    gt = 0;

    while( gsl_vector_get(d, gd) == 0 ) gd--;

    while ( gn >= gd ) {
      reoshift(dt, gn-gd);
      double v = gsl_vector_get(nt, gn)/gsl_vector_get(dt, gn);
      gsl_vector_set(rt, gn-gd, v);
      gsl_vector_scale(dt, v);
      gsl_vector_sub(nt, dt);
      gt = MAX(gt, gn-gd);
      while( (gn>=0) && (gsl_vector_get(nt, gn) == 0.0) ) gn--;
      gsl_vector_memcpy(dt, d2);
    }

    q = gsl_vector_alloc(gt+1); assert(q != NULL);
    p = gsl_vector_subvector(rt, 0, gt+1);
    gsl_vector_memcpy(q, &p.vector);
    if ( r != NULL ) {
      if ( (gn+1) > 0 ) {
	*r = gsl_vector_alloc(gn+1); assert( *r != NULL );
	p = gsl_vector_subvector(nt, 0, gn+1);
	gsl_vector_memcpy(*r, &p.vector);
      } else {
	*r = gsl_vector_alloc(1); assert( *r != NULL );
	gsl_vector_set_zero(*r);
      }
    }
    gsl_vector_free(nt); gsl_vector_free(dt);
    gsl_vector_free(rt); gsl_vector_free(d2);
    return q;
  } else {
    q = gsl_vector_alloc(1); assert( q != NULL );
    gsl_vector_set_zero(q);
    if ( r != NULL ) {
      *r = gsl_vector_alloc(n->size); assert( *r != NULL );
      gsl_vector_memcpy(*r, n);
    }
    return q;
  }
}

void poly_print(gsl_vector *p)
{
  int i;
  for(i=p->size-1; i >= 0; i--) {
    if ( i > 0 )
      printf("%lfx^%d + ",
	     gsl_vector_get(p, i), i);
    else
      printf("%lf\n", gsl_vector_get(p, i));
  }
}

gsl_vector *create_poly(int d, ...)
{
  va_list al;
  int i;
  gsl_vector *r = NULL;

  va_start(al, d);
  r = gsl_vector_alloc(d); assert( r != NULL );

  for(i=0; i < d; i++)
    gsl_vector_set(r, i, va_arg(al, double));

  return r;
}


#include <math.h>

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

#include "regional.h"

void init_weights(t_stochasticity_grid *g) {
    /* The weights will be normalized s.t. the sum of squared weights is one */
    g->weights[0] = 1.0;
    double sum = 1.0;
    for (int i = 1; i<g->depth; i++) {
        double val = g->weights[i-1] / g->weight_factor;
        g->weights[i] = val;
        sum += val*val;
    }
    double N = sqrt(sum);
    for (int i = 0; i < g->depth; i++) {
        g->weights[i] /= N;
    }
}

/*
 'arr' should point to a double array of size at least height*width.
*/
t_stochasticity_grid *create_stochasticity_grid(double *arr, int height, int width, double weight_factor, double mean, double stddev, long int seed) {
    t_stochasticity_grid *g = malloc( sizeof(t_stochasticity_grid) );
    g->arr = arr;
    g->height = height;
    g->width = width;

    /* It is best to use array dimensions of power of 2 */

    int d = height > width ? height : width; // Take the max
    g->depth = ceil(log2(d)) + 1;
    g->stddev = stddev;
    g->mean = mean;
    g->weight_factor = weight_factor;

    g->rng = gsl_rng_alloc(gsl_rng_mt19937);
    gsl_rng_set(g->rng, seed);

    g->weights = malloc( sizeof(double) * g->depth );
    init_weights(g);

    return g;
}

void free_stochasticity_grid(t_stochasticity_grid *g) {
    gsl_rng_free(g->rng);
    free(g->weights);
    g->weights = NULL;
    g->arr = NULL;
    free(g);
}

static inline double gaussian(const gsl_rng *rng, double stddev) {
    return gsl_ran_gaussian_ziggurat(rng, stddev);
}

void fill_quadrant(const t_stochasticity_grid *g, int y0, int x0, int y1, int x1, int level) {
    double weight = g->weights[level];
    double rval = gaussian(g->rng, g->stddev);
    for(int i = y0; i < y1; i++) {
        for(int j = x0; j < x1; j++) {
            int offset = i * g->width + j;
            double val = rval*weight;
            g->arr[offset] += val;
        }
    }
}

void recursive_division(const t_stochasticity_grid *g, int level, int y0, int x0, int y1, int x1) {
    if (level >= g->depth) return;
    if (x1-x0 == 1 && y1-y0 == 1) return; // Skip 1x1 grids

    if (y0 >= y1) return;
    if (x0 >= x1) return;
    //printf("Depth: %i. (%i, %i, %i, %i)\n", level, y0, x0, y1, x1);

    fill_quadrant(g, y0, x0, y1, x1, level);

    if (level + 1 == g->depth) return; // Avoid unnecessary recursion; skip level with 1x1 grids

    int ydist = (y1-y0)/2.0 + 0.5;
    int xdist = (x1-x0)/2.0 + 0.5;
    int ymid = y0+ydist;
    int xmid = x0+xdist;

    /* Upper left */
    recursive_division(g, level+1, y0, x0, ymid, xmid);

    /* Upper right */
    recursive_division(g, level+1, ymid, x0, y1, xmid);

    /* Bottom left */
    recursive_division(g, level+1, y0, xmid, ymid, x1);

    /* Bottom right */
    recursive_division(g, level+1, ymid, xmid, y1, x1);
}

/**
 Generate spatially autocorrelated coefficients using a recursive quadtree subdivision.
 The pointer 'arr' is a 2-dimensional output array of size 'height' * 'width'.
 The variable 'variance' gives the variance of the log-norm distribution with mean and cut-off at 1.0.
*/
void generate_stochasticity(t_stochasticity_grid *g) {
    /* Initialize the array by generating the last level (i.e., 1x1 grids) */
    for(int i = 0; i < g->height; i++) {
        for(int j = 0; j < g->width; j++) {
            double weight = g->weights[g->depth-1];
            g->arr[i*g->width + j] = weight*gaussian(g->rng, g->stddev);
        }
    }

    recursive_division(g, 0, 0, 0, g->height, g->width);

    for(int i = 0; i < g->height; i++) {
        for(int j = 0; j < g->width; j++) {
            double y = g->arr[i*g->width + j];
            y = exp(y + g->mean);
            if (y > 1.0) y = 1.0;
            g->arr[i*g->width + j] = y;
        }
    }

}


/* randist/nbinomial.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 James Theiler, Brian Gough
 *
 * 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 <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

/* The negative binomial distribution has the form,

   prob(k) =  Gamma(n + k)/(Gamma(n) Gamma(k + 1))  p^n (1-p)^k

   for k = 0, 1, ... . Note that n does not have to be an integer.

   This is the Leger's algorithm (given in the answers in Knuth) */

unsigned int
gsl_ran_negative_binomial (const gsl_rng * r, double p, double n)
{
  double X = gsl_ran_gamma (r, n, 1.0) ;
  unsigned int k = gsl_ran_poisson (r, X*(1-p)/p) ;
  return k ;
}

double
gsl_ran_negative_binomial_pdf (const unsigned int k, const double p, double n)
{
  double P;

  double f = gsl_sf_lngamma (k + n) ;
  double a = gsl_sf_lngamma (n) ;
  double b = gsl_sf_lngamma (k + 1.0) ;

  P = exp(f-a-b) * pow (p, n) * pow (1 - p, (double)k);

  return P;
}


/* interpolation/interp_poly.c
 *
 * Copyright (C) 2001 DAN, HO-JIN
 *
 * 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.
 */

/* Modified for standalone use in polynomial directory, B.Gough 2001 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>

int
gsl_poly_dd_init (double dd[], const double xa[], const double ya[],
                  size_t size)
{
  size_t i, j;

  /* Newton's divided differences */

  dd[0] = ya[0];

  for (j = size - 1; j >= 1; j--)
    {
      dd[j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
    }

  for (i = 2; i < size; i++)
    {
      for (j = size - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (xa[j] - xa[j - i]);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_poly_dd_taylor (double c[], double xp,
                    const double dd[], const double xa[], size_t size,
                    double w[])
{
  size_t i, j;

  for (i = 0; i < size; i++)
    {
      c[i] = 0.0;
      w[i] = 0.0;
    }

  w[size - 1] = 1.0;

  c[0] = dd[0];

  for (i = size - 1; i-- > 0;)
    {
      w[i] = -w[i + 1] * (xa[size - 2 - i] - xp);

      for (j = i + 1; j < size - 1; j++)
        {
          w[j] = w[j] - w[j + 1] * (xa[size - 2 - i] - xp);
        }

      for (j = i; j < size; j++)
        {
          c[j - i] += w[j] * dd[size - i - 1];
        }
    }

  return GSL_SUCCESS;
}


#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>

int
main (void)
{
  double data[] = { -1.0, 1.0, -1.0, 1.0,
                    -8.0, 4.0, -2.0, 1.0,
                    27.0, 9.0, 3.0, 1.0,
                    64.0, 16.0, 4.0, 1.0 };

  gsl_matrix_view m
    = gsl_matrix_view_array (data, 4, 4);

  gsl_vector_complex *eval = gsl_vector_complex_alloc (4);
  gsl_matrix_complex *evec = gsl_matrix_complex_alloc (4, 4);

  gsl_eigen_nonsymmv_workspace * w =
    gsl_eigen_nonsymmv_alloc (4);

  gsl_eigen_nonsymmv (&m.matrix, eval, evec, w);

  gsl_eigen_nonsymmv_free (w);

  gsl_eigen_nonsymmv_sort (eval, evec,
                           GSL_EIGEN_SORT_ABS_DESC);

  {
    int i, j;

    for (i = 0; i < 4; i++)
      {
        gsl_complex eval_i
           = gsl_vector_complex_get (eval, i);
        gsl_vector_complex_view evec_i
           = gsl_matrix_complex_column (evec, i);

        printf ("eigenvalue = %g + %gi\n",
                GSL_REAL(eval_i), GSL_IMAG(eval_i));
        printf ("eigenvector = \n");
        for (j = 0; j < 4; ++j)
          {
            gsl_complex z =
              gsl_vector_complex_get(&evec_i.vector, j);
            printf("%g + %gi\n", GSL_REAL(z), GSL_IMAG(z));
          }
      }
  }

  gsl_vector_complex_free(eval);
  gsl_matrix_complex_free(evec);

  return 0;
}


/**
 *
 * @generated d Tue Jan  7 11:45:26 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "auxiliary.h"

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int d_check_orthogonality(int M, int N, int LDQ, double *Q)
{
    double alpha, beta;
    double normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    double eps;
    double *work = (double *)malloc(minMN*sizeof(double));

    eps = LAPACKE_dlamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    double *Id = (double *) malloc(minMN*minMN*sizeof(double));
    memset((void*)Id, 0, minMN*minMN*sizeof(double));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (double)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_dsyrk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_dlansy_work(LAPACK_COL_MAJOR, 'i', 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int d_check_QRfactorization(int M, int N, double *A1, double *A2, int LDA, double *Q)
{
    double Anorm, Rnorm;
    double alpha, beta;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Ql       = (double *)malloc(M*N*sizeof(double));
    double *Residual = (double *)malloc(M*N*sizeof(double));
    double *work              = (double *)malloc(max(M,N)*sizeof(double));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        double *R = (double *)malloc(N*N*sizeof(double));
        memset((void*)R, 0, N*N*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, (alpha), Q, LDA, R, N, (beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        double *L = (double *)malloc(M*M*sizeof(double));
        memset((void*)L, 0, M*M*sizeof(double));
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(double));
        cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, (alpha), L, M, Q, LDA, (beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Residual, M, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */

int d_check_LLTfactorization(int N, double *A1, double *A2, int LDA, int uplo)
{
    double Anorm, Rnorm;
    double alpha;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    double *Residual = (double *)malloc(N*N*sizeof(double));
    double *L1       = (double *)malloc(N*N*sizeof(double));
    double *L2       = (double *)malloc(N*N*sizeof(double));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(double));
    memset((void*)L2, 0, N*N*sizeof(double));

    alpha= 1.0;

    LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_dlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_dtrmm(CblasColMajor, CblasRight, CblasLower, CblasTrans, CblasNonUnit, N, N, (alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, N, Residual, N, work);
    Anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the gemm
 */
double d_check_gemm(PLASMA_enum transA, PLASMA_enum transB, int M, int N, int K,
                   double alpha, double *A, int LDA,
                   double *B, int LDB,
                   double beta, double *Cplasma,
                   double *Cref, int LDC,
                   double *Cinitnorm, double *Cplasmanorm, double *Clapacknorm )
{
    double beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(K,max(M, N))* sizeof(double));

    *Cinitnorm   = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref,    LDC, work);
    *Cplasmanorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cplasma, LDC, work);

    cblas_dgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, M, N, K,
                (alpha), A, LDA, B, LDB, (beta), Cref, LDC);

    *Clapacknorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    cblas_daxpy(LDC * N, (beta_const), Cplasma, 1, Cref, 1);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the trsm
 */
double d_check_trsm(PLASMA_enum side, PLASMA_enum uplo, PLASMA_enum trans, PLASMA_enum diag,
                   int M, int NRHS, double alpha,
                   double *A, int LDA,
                   double *Bplasma, double *Bref, int LDB,
                   double *Binitnorm, double *Bplasmanorm, double *Blapacknorm )
{
    double beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(M, NRHS)* sizeof(double));
    /*double eps = LAPACKE_dlamch_work('e');*/

    *Binitnorm   = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, Bref,    LDB, work);
    *Bplasmanorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bplasma, LDB, work);

    cblas_dtrsm(CblasColMajor, (CBLAS_SIDE)side, (CBLAS_UPLO)uplo,
                (CBLAS_TRANSPOSE)trans, (CBLAS_DIAG)diag, M, NRHS,
                (alpha), A, LDA, Bref, LDB);

    *Blapacknorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);

    cblas_daxpy(LDB * NRHS, (beta_const), Bplasma, 1, Bref, 1);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);
    Rnorm = Rnorm / *Blapacknorm;
    /* max(M,NRHS) * eps);*/

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the solution
 */

double d_check_solution(int M, int N, int NRHS, double *A, int LDA,
                      double *B,  double *X, int LDB,
                      double *anorm, double *bnorm, double *xnorm )
{
/*     int info_solution; */
    double Rnorm = -1.00;
    double zone  =  1.0;
    double mzone = -1.0;
    double *work = (double *)malloc(max(M, N)* sizeof(double));

    *anorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, N,    A, LDA, work);
    *xnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, X, LDB, work);
    *bnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    cblas_dgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, (zone), A, LDA, X, LDB, (mzone), B, LDB);

    Rnorm = LAPACKE_dlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    free(work);

    return Rnorm;
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <nubspline.h>
#include "SimulationSteps.h"
#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>

void eval_NUBspline_1d_d(NUBspline_1d_d * restrict spline, double x,
	double* restrict val);

void eval_NUBspline_1d_d_vg(NUBspline_1d_d * restrict spline, double x,
	double* restrict val, double* restrict grad);

void eval_NUBspline_1d_d_vgl(NUBspline_1d_d * restrict spline, double x,
	double* restrict val, double* restrict grad, double* restrict lapl);

void InterpolateWithSplines(int NumPointsToInterpolate, int NumPointsToEvaluate, double* x, double* NodalX, double *func, double *NodalFunc, double* NodalDeriv)
{

	NUgrid* myGrid = create_general_grid(x, NumPointsToInterpolate);
	BCtype_d myBC;
	myBC.lCode = NATURAL;
	myBC.rCode = NATURAL;
	NUBspline_1d_d *mySpline = create_NUBspline_1d_d(myGrid, myBC, func);
	for(int i =0; i < NumPointsToEvaluate; i++)
	{
		if (NodalDeriv != NULL)
			eval_NUBspline_1d_d_vg(mySpline, NodalX[i], &NodalFunc[i], &NodalDeriv[i]);
		else
			eval_NUBspline_1d_d(mySpline, NodalX[i], &NodalFunc[i]);
	}

}

void InterpolateWithGSLSplines(int NumPointsToInterpolate, int NumPointsToEvaluate, double* x, double* NodalX, double* func, double* NodalFunc, double* NodalDeriv)
{
	gsl_interp_accel *acc = gsl_interp_accel_alloc();
	gsl_spline* spline = gsl_spline_alloc(gsl_interp_cspline, NumPointsToInterpolate);
	gsl_spline_init(spline, x, func, NumPointsToInterpolate);

	for(int j = 0; j < NumPointsToEvaluate; j++)
	{
		NodalFunc[j] = gsl_spline_eval(spline, NodalX[j], acc);
		if (NodalDeriv != NULL)
			NodalDeriv[j] = gsl_spline_eval_deriv(spline, NodalX[j], acc);
	}

}


#pragma once

#include "shapes3D.h"

#include <gsl/gsl>

namespace ad {

template <int N_baseEdges>
std::vector<vertex3D::Data> make_pyramid(float aBaseRadius, float aHeight)
{
    std::vector<vertex3D::Data> result;
    // There are twice the number of edges in the pyramid, each edge being two vertices
    constexpr std::size_t ElementsCount{N_baseEdges*2*2};
    result.reserve(ElementsCount);

    Vec<3> topPosition = {0.0f, 0.0f, aHeight};

    auto getBasePosition = [](int aId, float aRadius) -> Vec<3>
    {
        return {
            std::cos(2*pi<>*aId/N_baseEdges) * aRadius,
            std::sin(2*pi<>*aId/N_baseEdges) * aRadius,
            0.0f,
        };
    };

    Vec<3> position_prev = getBasePosition(-1, aBaseRadius);
    for(int i=0; i<N_baseEdges; ++i)
    {
        Vec<3> position_a = getBasePosition(i, aBaseRadius);
        Vec<3> position_b = getBasePosition(i+1, aBaseRadius);

        // The base segment
        {
            // The normal of BA vector (not AB)
            Vec<3> ba = (position_a - position_b).normalize();
            Vec<3> base_normal = {-ba.y(), ba.x(), 0.0f};

            result.emplace_back(vertex3D::Data{{position_a}, {base_normal}});
            result.emplace_back(vertex3D::Data{{position_b}, {base_normal}});
        }

        // The segment connecting base vertex "i" to the top
        {
            float teta = std::atan(aBaseRadius/aHeight); // angle between Z and a side edge
            Vec<3> b_prev = (position_prev - position_b).normalize();
            Vec<3> side_normal = {
                -b_prev.y() * std::cos(teta),
                b_prev.x() * std::cos(teta),
                std::sin(teta),
            };
            result.emplace_back(vertex3D::Data{{position_a}, {side_normal}});
            result.emplace_back(vertex3D::Data{{topPosition}, {side_normal}});
        }

        position_prev = position_a;
    }

    Expects(result.size() == ElementsCount);
    return result;
}

} // namespace ad


/* fit/linear.c
 *
 * Copyright (C) 2000 Brian Gough
 *
 * 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 <gsl/gsl_errno.h>
#include <gsl/gsl_fit.h>

/* Fit the data (x_i, y_i) to the linear relationship

   Y = c0 + c1 x

   returning,

   c0, c1  --  coefficients
   cov00, cov01, cov11  --  variance-covariance matrix of c0 and c1,
   sumsq   --   sum of squares of residuals

   This fit can be used in the case where the errors for the data are
   uknown, but assumed equal for all points. The resulting
   variance-covariance matrix estimates the error in the coefficients
   from the observed variance of the points around the best fit line.
*/

int
gsl_fit_linear (const double *x, const size_t xstride,
                const double *y, const size_t ystride,
                const size_t n,
                double *c0, double *c1,
                double *cov_00, double *cov_01, double *cov_11, double *sumsq)
{
  double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      m_x += (x[i * xstride] - m_x) / (i + 1.0);
      m_y += (y[i * ystride] - m_y) / (i + 1.0);
    }

  for (i = 0; i < n; i++)
    {
      const double dx = x[i * xstride] - m_x;
      const double dy = y[i * ystride] - m_y;

      m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
      m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
    }

  /* In terms of y = a + b x */

  {
    double s2 = 0, d2 = 0;
    double b = m_dxdy / m_dx2;
    double a = m_y - m_x * b;

    *c0 = a;
    *c1 = b;

    /* Compute chi^2 = \sum (y_i - (a + b * x_i))^2 */

    for (i = 0; i < n; i++)
      {
        const double dx = x[i * xstride] - m_x;
        const double dy = y[i * ystride] - m_y;
        const double d = dy - b * dx;
        d2 += d * d;
      }

    s2 = d2 / (n - 2.0);        /* chisq per degree of freedom */

    *cov_00 = s2 * (1.0 / n) * (1 + m_x * m_x / m_dx2);
    *cov_11 = s2 * 1.0 / (n * m_dx2);

    *cov_01 = s2 * (-m_x) / (n * m_dx2);

    *sumsq = d2;
  }

  return GSL_SUCCESS;
}

/* Fit the weighted data (x_i, w_i, y_i) to the linear relationship

   Y = c0 + c1 x

   returning,

   c0, c1  --  coefficients
   s0, s1  --  the standard deviations of c0 and c1,
   r       --  the correlation coefficient between c0 and c1,
   chisq   --  weighted sum of squares of residuals */

int
gsl_fit_wlinear (const double *x, const size_t xstride,
                 const double *w, const size_t wstride,
                 const double *y, const size_t ystride,
                 const size_t n,
                 double *c0, double *c1,
                 double *cov_00, double *cov_01, double *cov_11,
                 double *chisq)
{

  /* compute the weighted means and weighted deviations from the means */

  /* wm denotes a "weighted mean", wm(f) = (sum_i w_i f_i) / (sum_i w_i) */

  double W = 0, wm_x = 0, wm_y = 0, wm_dx2 = 0, wm_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          W += wi;
          wm_x += (x[i * xstride] - wm_x) * (wi / W);
          wm_y += (y[i * ystride] - wm_y) * (wi / W);
        }
    }

  W = 0;                        /* reset the total weight */

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          const double dx = x[i * xstride] - wm_x;
          const double dy = y[i * ystride] - wm_y;

          W += wi;
          wm_dx2 += (dx * dx - wm_dx2) * (wi / W);
          wm_dxdy += (dx * dy - wm_dxdy) * (wi / W);
        }
    }

  /* In terms of y = a + b x */

  {
    double d2 = 0;
    double b = wm_dxdy / wm_dx2;
    double a = wm_y - wm_x * b;

    *c0 = a;
    *c1 = b;

    *cov_00 = (1 / W) * (1 + wm_x * wm_x / wm_dx2);
    *cov_11 = 1 / (W * wm_dx2);

    *cov_01 = -wm_x / (W * wm_dx2);

    /* Compute chi^2 = \sum w_i (y_i - (a + b * x_i))^2 */

    for (i = 0; i < n; i++)
      {
        const double wi = w[i * wstride];

        if (wi > 0)
          {
            const double dx = x[i * xstride] - wm_x;
            const double dy = y[i * ystride] - wm_y;
            const double d = dy - b * dx;
            d2 += wi * d * d;
          }
      }

    *chisq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_linear_est (const double x,
                    const double c0, const double c1,
                    const double c00, const double c01, const double c11,
                    double *y, double *y_err)
{
  *y = c0 + c1 * x;
  *y_err = sqrt (c00 + x * (2 * c01 + c11 * x));
  return GSL_SUCCESS;
}

int
gsl_fit_mul (const double *x, const size_t xstride,
             const double *y, const size_t ystride,
             const size_t n,
             double *c1, double *cov_11, double *sumsq)
{
  double m_x = 0, m_y = 0, m_dx2 = 0, m_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      m_x += (x[i * xstride] - m_x) / (i + 1.0);
      m_y += (y[i * ystride] - m_y) / (i + 1.0);
    }

  for (i = 0; i < n; i++)
    {
      const double dx = x[i * xstride] - m_x;
      const double dy = y[i * ystride] - m_y;

      m_dx2 += (dx * dx - m_dx2) / (i + 1.0);
      m_dxdy += (dx * dy - m_dxdy) / (i + 1.0);
    }

  /* In terms of y =  b x */

  {
    double s2 = 0, d2 = 0;
    double b = (m_x * m_y + m_dxdy) / (m_x * m_x + m_dx2);

    *c1 = b;

    /* Compute chi^2 = \sum (y_i -  b * x_i)^2 */

    for (i = 0; i < n; i++)
      {
        const double dx = x[i * xstride] - m_x;
        const double dy = y[i * ystride] - m_y;
        const double d = (m_y - b * m_x) + dy - b * dx;
        d2 += d * d;
      }

    s2 = d2 / (n - 1.0);        /* chisq per degree of freedom */

    *cov_11 = s2 * 1.0 / (n * (m_x * m_x + m_dx2));

    *sumsq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_wmul (const double *x, const size_t xstride,
              const double *w, const size_t wstride,
              const double *y, const size_t ystride,
              const size_t n,
              double *c1, double *cov_11, double *chisq)
{

  /* compute the weighted means and weighted deviations from the means */

  /* wm denotes a "weighted mean", wm(f) = (sum_i w_i f_i) / (sum_i w_i) */

  double W = 0, wm_x = 0, wm_y = 0, wm_dx2 = 0, wm_dxdy = 0;

  size_t i;

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          W += wi;
          wm_x += (x[i * xstride] - wm_x) * (wi / W);
          wm_y += (y[i * ystride] - wm_y) * (wi / W);
        }
    }

  W = 0;                        /* reset the total weight */

  for (i = 0; i < n; i++)
    {
      const double wi = w[i * wstride];

      if (wi > 0)
        {
          const double dx = x[i * xstride] - wm_x;
          const double dy = y[i * ystride] - wm_y;

          W += wi;
          wm_dx2 += (dx * dx - wm_dx2) * (wi / W);
          wm_dxdy += (dx * dy - wm_dxdy) * (wi / W);
        }
    }

  /* In terms of y = b x */

  {
    double d2 = 0;
    double b = (wm_x * wm_y + wm_dxdy) / (wm_x * wm_x + wm_dx2);

    *c1 = b;

    *cov_11 = 1 / (W * (wm_x * wm_x + wm_dx2));

    /* Compute chi^2 = \sum w_i (y_i - b * x_i)^2 */

    for (i = 0; i < n; i++)
      {
        const double wi = w[i * wstride];

        if (wi > 0)
          {
            const double dx = x[i * xstride] - wm_x;
            const double dy = y[i * ystride] - wm_y;
            const double d = (wm_y - b * wm_x) + (dy - b * dx);
            d2 += wi * d * d;
          }
      }

    *chisq = d2;
  }

  return GSL_SUCCESS;
}

int
gsl_fit_mul_est (const double x,
                 const double c1, const double c11,
                 double *y, double *y_err)
{
  *y = c1 * x;
  *y_err = sqrt (c11) * fabs (x);
  return GSL_SUCCESS;
}


/* interpolation/interp_poly.c
 *
 * Copyright (C) 2001 DAN, HO-JIN
 * Copyright (C) 2013 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.
 */

/* Modified for standalone use in polynomial directory, B.Gough 2001 */

#include <config.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_poly.h>

int
gsl_poly_dd_init (double dd[], const double xa[], const double ya[],
                  size_t size)
{
  size_t i, j;

  /* Newton's divided differences */

  dd[0] = ya[0];

  for (j = size - 1; j >= 1; j--)
    {
      dd[j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
    }

  for (i = 2; i < size; i++)
    {
      for (j = size - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (xa[j] - xa[j - i]);
        }
    }

  return GSL_SUCCESS;
}

int
gsl_poly_dd_taylor (double c[], double xp,
                    const double dd[], const double xa[], size_t size,
                    double w[])
{
  size_t i, j;

  for (i = 0; i < size; i++)
    {
      c[i] = 0.0;
      w[i] = 0.0;
    }

  w[size - 1] = 1.0;

  c[0] = dd[0];

  for (i = size - 1; i-- > 0;)
    {
      w[i] = -w[i + 1] * (xa[size - 2 - i] - xp);

      for (j = i + 1; j < size - 1; j++)
        {
          w[j] = w[j] - w[j + 1] * (xa[size - 2 - i] - xp);
        }

      for (j = i; j < size; j++)
        {
          c[j - i] += w[j] * dd[size - i - 1];
        }
    }

  return GSL_SUCCESS;
}

/*
gsl_poly_dd_hermite_init()
  Compute divided difference representation of data
for Hermite polynomial interpolation

Inputs: dd   - (output) array of size 2*size containing
               divided differences, dd[k] = f[z_0,z_1,...,z_k]
        za   - (output) array of size 2*size containing
               z values
        xa   - x data
        ya   - y data
        dya  - dy/dx data
        size - size of xa,ya,dya arrays

Return: success
*/

int
gsl_poly_dd_hermite_init (double dd[], double za[], const double xa[], const double ya[],
                          const double dya[], const size_t size)
{
  const size_t N = 2 * size;
  size_t i, j;

  /* Hermite divided differences */

  dd[0] = ya[0];

  /* compute: dd[j] = f[z_{j-1},z_j] for j \in [1,N-1] */
  for (j = 0; j < size; ++j)
    {
      za[2*j] = xa[j];
      za[2*j + 1] = xa[j];

      if (j != 0)
        {
          dd[2*j] = (ya[j] - ya[j - 1]) / (xa[j] - xa[j - 1]);
          dd[2*j - 1] = dya[j - 1];
        }
    }

  dd[N - 1] = dya[size - 1];

  for (i = 2; i < N; i++)
    {
      for (j = N - 1; j >= i; j--)
        {
          dd[j] = (dd[j] - dd[j - 1]) / (za[j] - za[j - i]);
        }
    }

  return GSL_SUCCESS;
} /* gsl_poly_dd_hermite_init() */


/* integration/integration.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * 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.
 */

/* Created: [GJ] Tue Apr 23 21:26:53 EDT 1996
 */
#include <config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_errno.h>
/* #include "interpolation.h" */
#include <gsl/gsl_integration.h>
#if 0

static char * info_string = 0;

void set_integ_info(const char * mess)
{
  if(info_string != 0) free(info_string);
  info_string = 0;
  if(mess != 0) {
    info_string = (char *)malloc((strlen(mess)+1) * sizeof(char));
    strcpy(info_string, mess);
  }
}

/* Integration function for use with open_romberg(). */
#define FUNC(x) ((*func)(x))
static double midpnt(double (*func)(double), double a, double b, int n)
{
  double x,tnm,sum,del,ddel;
  static double s;
  static int it;
  int j;

  if (n == 1) {
    it=1;
    return (s=(b-a)*FUNC(0.5*(a+b)));
  }
  else {
    tnm=it;
    del=(b-a)/(3.0*tnm);
    ddel=del+del;
    x=a+0.5*del;
    sum=0.0;
    for (j=1;j<=it;j++) {
      sum += FUNC(x);
      x += ddel;
      sum += FUNC(x);
      x += del;
    }
    it *= 3;
    s=(s+(b-a)*sum/tnm)/3.0;
    return s;
  }
}
#undef FUNC

#define JMAX 10
#define JMAXP JMAX+1
#define K 5
double open_romberg(double(*func)(double), double a, double b, double eps)
{
  int j;
  double ss,dss,h[JMAXP+1],s[JMAXP+1];

  h[1]=1.0;
  for (j=1;j<=JMAX;j++) {
    s[j]=midpnt(func,a,b,j);
    if (j >= K) {
      /* local_polint(&h[j-K],&s[j-K],K,0.0,&ss,&dss); */
      interp_poly(&h[j-K]+1,&s[j-K]+1,K,0.0,&ss,&dss);
      if (fabs(dss) < eps * fabs(ss)) return ss;
    }
    s[j+1]=s[j];
    h[j+1]=h[j]/9.0;
  }

  push_error("open_romberg: too many steps", Error_ConvFail_);
  push_generic_error("open_romberg:", info_string);

  return 0.;
}
#undef JMAX
#undef JMAXP
#undef K

double gauss_legendre_10(double (*func)(double), double a, double b)
{
  int j;
  static double x[] = {0.,
                         0.1488743389,
                         0.4333953941,
                         0.6794095682,
                         0.8650633666,
                         0.9739065285};
  static double w[] = {0.,
                         0.2955242247,
                         0.2692667193,
                         0.2190863625,
                         0.1494513491,
                         0.0666713443};

  double xm = 0.5 * (b + a);
  double xr = 0.5 * (b - a);

  double s = 0.;
  double dx;
  double result;

  for(j=1; j<=5; j++){
    dx = xr * x[j];
    s += w[j] * ((*func)(xm+dx) + (*func)(xm-dx));
  }
  result = s * xr;

  return result;
}

/************************************************************************
 *                                                                      *
 * Trapezoid rule.                                                      *
 *                                                                      *
 * The original trapzd() function from the Numerical Recipes worked     *
 * by side-effects; it tried to remember the value of the integral      *
 * at the current level of refinement. This is REAL BAD, because if     *
 * you try to use it to do a double integral you will get a surprise.   *
 * You cannot tell which "integral" it is remembering. This stems from  *
 * the stupid fact that there was only one, essentially global,         *
 * variable that was doing this memory job.                             *
 *                                                                      *
 * The solution is simple: pass the current refinement to the function  *
 * so that it can be remembered by an external environment, making it   *
 * easy to avoid confusion.                                             *
 *                                                                      *
 * So the new-method code-fragment for doing an integral looks like:    *
 *                                                                      *
 *       double answer;                                                 *
 *       for(j=1; j<=M+1; j++)                                          *
 *         trapezoid_rule(func, a, b, j, &answer);                      *
 *                                                                      *
 ************************************************************************/
void trapezoid_rule(double(*f)(double), double a, double b, int n, double *s)
{
  double x, tnm, sum, del;
  int it, j;

  if(n==1){
    *s = 0.5 * (b-a) * (f(b) + f(a));
  }
  else {
    for(it=1, j=1; j < n-1; j++)  it <<= 1;
    tnm = (double) it;
    del = (b-a) / tnm;
    x = a + 0.5 * del;

    for(sum=0., j=1; j<=it; j++, x+=del) { sum += f(x); }

    *s = 0.5 * (*s + del * sum);
  }
}

/************************************************************************
 *                                                                      *
 * Trapezoid rule.                                                      *
 * This version produces a tracing output.                              *
 *                                                                      *
 ************************************************************************/
#define FUNC(x) ((*func)(x))

void test_trapezoid_rule(double(*func)(double), double a, double b, int n,
                         double *s)
{
  double x, tnm, sum, del;
  int it, j;

  if(n==1){
    printf("t:  a= %g  b= %g   f(a)= %g  f(b)= %g\n",
           a, b, FUNC(a), FUNC(b));
  }

  if(n==1){
    *s = 0.5 * (b-a) * (FUNC(b) + FUNC(a));
    printf("s= %g\n", *s);
  }
  else {
    for(it=1, j=1; j < n-1; j++)  it <<= 1;
    tnm = (double) it;
    del = (b-a) / tnm;
    x = a + 0.5 * del;

    for(sum=0., j=1; j<=it; j++, x+=del) sum += FUNC(x);

    *s = 0.5 * (*s + del * sum);

    printf("sum= %g   tnm= %g  del= %g  s= %g\n", sum, tnm, del, *s);
  }
}
#undef FUNC

/* This is fixed to use the non-side-effecting version of the
 * trapezoidal rule, as implemented in trapezoid_rule() above.
 * See the discussion there for explanation of the original problem.
 */
#define JMAX 20
double gsl_integ_simpson(double (*func)(double), double a, double b, double eps)
{
  int j;
  double s, st, ost, os;

  ost = os = -1.e50;

  for(j=1; j<=JMAX; j++){

    trapezoid_rule(func, a, b, j, &st);
    s = (4.*st - ost) / 3.;

    if(fabs(s-os) < eps * fabs(os))
      return s;

    os = s;
    ost = st;
  }

  GSL_MESSAGE("simpson: too many steps");

  return 0.;
}
#undef JMAX

double gsl_integ_simpson_table(const double * x, const double * y, int n)
{
  int i;
  double result = 0.;
  for(i=0; i<n-1; i++) {
    result += 0.5 * (y[i+1] + y[i]) * (x[i+1] - x[i]);
  }
  return result;
}

/* apparatus for lorentzian variable change to remove pole */
static double (*dummy_f)(double);
static double dummy_x0;
static double dummy_w;
static inline double f_y(double y)
{ return dummy_f(dummy_x0 + dummy_w * tan(dummy_w * y)); }

double gsl_integ_lorenz(double (*f)(double),
                        double x0, double w,
                        double a, double b,
                        double eps)
{
  dummy_f = f;
  dummy_x0 = x0;
  dummy_w = fabs(w);

  if(dummy_w < 10.*sqrt(min_double)) {
    char buff[100];
    sprintf(buff,"lorenz_integ: width w= %g  too small", dummy_w);
    GSL_MESSAGE(buff);
    return 0.;
  }
  else {
    double lower_y = atan((a-x0)/dummy_w) / dummy_w;
    double upper_y = atan((b-x0)/dummy_w) / dummy_w;
    return gsl_integ_simpson(f_y, lower_y, upper_y, eps);
  }
}

#endif /* 0 */


/**
 * nn.c
 *
 * Created by mizu-bai
 *
 * This is a implementation of Tensors Warm-up: numpy in LEARNING PYTORCH WITH EXAMPLES
 * (https://pytorch.org/tutorials/beginner/pytorch_with_examples.html#) using C with GSL.
 */

#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_rng.h>

int main(int argc, const char *argv[]) {
    // create random input and output data
    int size = 2000;
    gsl_vector *x = gsl_vector_alloc(size);
    gsl_vector *y = gsl_vector_alloc(size);
    for (int i = 0; i < size; i++) {
        double x_tmp = 2 * M_PI / (double)(size - 1) * (double)i - M_PI;
        gsl_vector_set(x, i, x_tmp);
        gsl_vector_set(y, i, sin(x_tmp));
    }

    // randomly init weights
    int w_size = 4;
    gsl_vector *w = gsl_vector_alloc(w_size);
    gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
    for (int i = 0; i < w_size; i++) {
        gsl_vector_set(w, i, gsl_ran_gaussian(r, 1.0));
    }
    gsl_rng_free(r);

    double learning_rate = 1e-6;
    for(int t = 0; t < 2000; t++) {
        // forward pass, compute predicted y & compute loss
        gsl_vector *y_pred = gsl_vector_alloc(size);
        double loss = 0;
        for (int i = 0; i < size; i++) {
            double w_tmp = gsl_poly_eval(w->data, w_size, gsl_vector_get(x, i));
            gsl_vector_set(y_pred, i, w_tmp);
            loss += gsl_pow_2(gsl_vector_get(y_pred, i) - gsl_vector_get(y, i));
        }
        if (t % 100 == 99) {
            printf("t: %d, loss: %.4f\n", t, loss);
        }

        // compute gradients of w with respect to loss
        gsl_vector *grad_y_pred = gsl_vector_alloc(size);
        gsl_vector_memcpy(grad_y_pred, y_pred);
        gsl_vector_sub(grad_y_pred, y);
        gsl_vector_scale(grad_y_pred, 2);
        gsl_vector *grad_w = gsl_vector_alloc(w_size);
        gsl_vector_set_all(grad_w, 0.0);
        for(int i = 0; i < size; i++) {
            gsl_vector *grad_y_pred_tmp = gsl_vector_alloc(size);
            gsl_vector *x_tmp = gsl_vector_alloc(size);

            gsl_vector_set(grad_w, 0, gsl_vector_get(grad_w, 0) + gsl_vector_get(grad_y_pred, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_mul(grad_y_pred_tmp, x);
            gsl_vector_set(grad_w, 1, gsl_vector_get(grad_w, 1) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_memcpy(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(grad_y_pred_tmp, x_tmp);
            gsl_vector_set(grad_w, 2, gsl_vector_get(grad_w, 2) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_memcpy(grad_y_pred_tmp, grad_y_pred);
            gsl_vector_memcpy(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(x_tmp, x);
            gsl_vector_mul(grad_y_pred_tmp, x_tmp);
            gsl_vector_set(grad_w, 3, gsl_vector_get(grad_w, 3) + gsl_vector_get(grad_y_pred_tmp, i));

            gsl_vector_free(grad_y_pred_tmp);
            gsl_vector_free(x_tmp);
        }

        gsl_vector_scale(grad_w, learning_rate);
        gsl_vector_sub(w, grad_w);

        // free
        gsl_vector_free(y_pred);
        gsl_vector_free(grad_y_pred);
        gsl_vector_free(grad_w);
    }

    printf("Result: y = %.4f + %.4f * x + %.4f * x^2 + %.4f * x^3\n", gsl_vector_get(w, 0), gsl_vector_get(w, 1), gsl_vector_get(w, 2), gsl_vector_get(w, 3));

    // free
    gsl_vector_free(x);
    gsl_vector_free(y);
    gsl_vector_free(w);

    return 0;
}


/* cdf/exponential.c
 *
 * Copyright (C) 2003, 2007 Brian Gough
 *
 * 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 <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_cdf.h>

/* The exponential distribution has the form

   p(x) dx = exp(-x/mu) dx/mu

   for x = 0 ... +infty */

double
gsl_cdf_exponential_P (const double x, const double mu)
{
  if (x < 0)
    {
      return 0;
    }
  else
    {
      double P = -expm1 (-x / mu);

      return P;
    }
}

double
gsl_cdf_exponential_Q (const double x, const double mu)
{
  if (x < 0)
    {
      return 1;
    }
  else
    {
      double Q = exp (-x / mu);

      return Q;
    }
}


/* specfunc/gsl_sf_legendre.h
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2001, 2002, 2004 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 */

#ifndef __GSL_SF_LEGENDRE_H__
#define __GSL_SF_LEGENDRE_H__

#include <gsl/gsl_sf_result.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

/* P_l(x)   l >= 0; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int     gsl_sf_legendre_Pl_e(const int l, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_Pl(const int l, const double x);

/* P_l(x) for l=0,...,lmax; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Pl_array(
  const int lmax, const double x,
  double * result_array
  );

/* P_l(x) and P_l'(x) for l=0,...,lmax; |x| <= 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Pl_deriv_array(
  const int lmax, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* P_l(x), l=1,2,3
 *
 * exceptions: none
 */
int gsl_sf_legendre_P1_e(double x, gsl_sf_result * result);
int gsl_sf_legendre_P2_e(double x, gsl_sf_result * result);
int gsl_sf_legendre_P3_e(double x, gsl_sf_result * result);
double gsl_sf_legendre_P1(const double x);
double gsl_sf_legendre_P2(const double x);
double gsl_sf_legendre_P3(const double x);

/* Q_0(x), x > -1, x != 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Q0_e(const double x, gsl_sf_result * result);
double gsl_sf_legendre_Q0(const double x);

/* Q_1(x), x > -1, x != 1
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Q1_e(const double x, gsl_sf_result * result);
double gsl_sf_legendre_Q1(const double x);

/* Q_l(x), x > -1, x != 1, l >= 0
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_Ql_e(const int l, const double x, gsl_sf_result * result);
double gsl_sf_legendre_Ql(const int l, const double x);

/* P_l^m(x)  m >= 0; l >= m; |x| <= 1.0
 *
 * Note that this function grows combinatorially with l.
 * Therefore we can easily generate an overflow for l larger
 * than about 150.
 *
 * There is no trouble for small m, but when m and l are both large,
 * then there will be trouble. Rather than allow overflows, these
 * functions refuse to calculate when they can sense that l and m are
 * too big.
 *
 * If you really want to calculate a spherical harmonic, then DO NOT
 * use this. Instead use legendre_sphPlm() below, which  uses a similar
 * recursion, but with the normalized functions.
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int     gsl_sf_legendre_Plm_e(const int l, const int m, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_Plm(const int l, const int m, const double x);

/* P_l^m(x)  m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_legendre_Plm_array(
  const int lmax, const int m, const double x,
  double * result_array
  );

/* P_l^m(x)  and d(P_l^m(x))/dx;  m >= 0; lmax >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM, GSL_EOVRFLW
 */
int gsl_sf_legendre_Plm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* P_l^m(x), normalized properly for use in spherical harmonics
 * m >= 0; l >= m; |x| <= 1.0
 *
 * There is no overflow problem, as there is for the
 * standard normalization of P_l^m(x).
 *
 * Specifically, it returns:
 *
 *        sqrt((2l+1)/(4pi)) sqrt((l-m)!/(l+m)!) P_l^m(x)
 *
 * exceptions: GSL_EDOM
 */
int     gsl_sf_legendre_sphPlm_e(const int l, int m, const double x, gsl_sf_result * result);
double  gsl_sf_legendre_sphPlm(const int l, const int m, const double x);

/* sphPlm(l,m,x) values
 * m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_sphPlm_array(
  const int lmax, int m, const double x,
  double * result_array
  );

/* sphPlm(l,m,x) and d(sphPlm(l,m,x))/dx values
 * m >= 0; l >= m; |x| <= 1.0
 * l=|m|,...,lmax
 *
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_sphPlm_deriv_array(
  const int lmax, const int m, const double x,
  double * result_array,
  double * result_deriv_array
  );

/* size of result_array[] needed for the array versions of Plm
 * (lmax - m + 1)
 */
int gsl_sf_legendre_array_size(const int lmax, const int m);

/* Irregular Spherical Conical Function
 * P^{1/2}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_half_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_half(const double lambda, const double x);

/* Regular Spherical Conical Function
 * P^{-1/2}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_mhalf_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_mhalf(const double lambda, const double x);

/* Conical Function
 * P^{0}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_0_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_0(const double lambda, const double x);

/* Conical Function
 * P^{1}_{-1/2 + I lambda}(x)
 *
 * x > -1.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_1_e(const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_1(const double lambda, const double x);

/* Regular Spherical Conical Function
 * P^{-1/2-l}_{-1/2 + I lambda}(x)
 *
 * x > -1.0, l >= -1
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_sph_reg_e(const int l, const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_sph_reg(const int l, const double lambda, const double x);

/* Regular Cylindrical Conical Function
 * P^{-m}_{-1/2 + I lambda}(x)
 *
 * x > -1.0, m >= -1
 * exceptions: GSL_EDOM
 */
int gsl_sf_conicalP_cyl_reg_e(const int m, const double lambda, const double x, gsl_sf_result * result);
double gsl_sf_conicalP_cyl_reg(const int m, const double lambda, const double x);

/* The following spherical functions are specializations
 * of Legendre functions which give the regular eigenfunctions
 * of the Laplacian on a 3-dimensional hyperbolic space.
 * Of particular interest is the flat limit, which is
 * Flat-Lim := {lambda->Inf, eta->0, lambda*eta fixed}.
 */

/* Zeroth radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * legendre_H3d_0(lambda,eta) := sin(lambda*eta)/(lambda*sinh(eta))
 *
 * Normalization:
 * Flat-Lim legendre_H3d_0(lambda,eta) = j_0(lambda*eta)
 *
 * eta >= 0.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_0_e(const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d_0(const double lambda, const double eta);

/* First radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * legendre_H3d_1(lambda,eta) :=
 *    1/sqrt(lambda^2 + 1) sin(lam eta)/(lam sinh(eta))
 *    (coth(eta) - lambda cot(lambda*eta))
 *
 * Normalization:
 * Flat-Lim legendre_H3d_1(lambda,eta) = j_1(lambda*eta)
 *
 * eta >= 0.0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_1_e(const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d_1(const double lambda, const double eta);

/* l'th radial eigenfunction of the Laplacian on the
 * 3-dimensional hyperbolic space.
 *
 * Normalization:
 * Flat-Lim legendre_H3d_l(l,lambda,eta) = j_l(lambda*eta)
 *
 * eta >= 0.0, l >= 0
 * exceptions: GSL_EDOM
 */
int gsl_sf_legendre_H3d_e(const int l, const double lambda, const double eta, gsl_sf_result * result);
double gsl_sf_legendre_H3d(const int l, const double lambda, const double eta);

/* Array of H3d(ell),  0 <= ell <= lmax
 */
int gsl_sf_legendre_H3d_array(const int lmax, const double lambda, const double eta, double * result_array);

/* associated legendre P_{lm} routines */

typedef enum
{
  GSL_SF_LEGENDRE_SCHMIDT,
  GSL_SF_LEGENDRE_SPHARM,
  GSL_SF_LEGENDRE_FULL,
  GSL_SF_LEGENDRE_NONE
} gsl_sf_legendre_t;

int gsl_sf_legendre_array(const gsl_sf_legendre_t norm,
                          const size_t lmax, const double x,
                          double result_array[]);
int gsl_sf_legendre_array_e(const gsl_sf_legendre_t norm,
                            const size_t lmax, const double x,
                            const double csphase,
                            double result_array[]);
int gsl_sf_legendre_deriv_array(const gsl_sf_legendre_t norm,
                                const size_t lmax, const double x,
                                double result_array[],
                                double result_deriv_array[]);
int gsl_sf_legendre_deriv_array_e(const gsl_sf_legendre_t norm,
                                  const size_t lmax, const double x,
                                  const double csphase,
                                  double result_array[],
                                  double result_deriv_array[]);
int gsl_sf_legendre_deriv_alt_array(const gsl_sf_legendre_t norm,
                                    const size_t lmax, const double x,
                                    double result_array[],
                                    double result_deriv_array[]);
int gsl_sf_legendre_deriv_alt_array_e(const gsl_sf_legendre_t norm,
                                      const size_t lmax, const double x,
                                      const double csphase,
                                      double result_array[],
                                      double result_deriv_array[]);
int gsl_sf_legendre_deriv2_array(const gsl_sf_legendre_t norm,
                                 const size_t lmax, const double x,
                                 double result_array[],
                                 double result_deriv_array[],
                                 double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_array_e(const gsl_sf_legendre_t norm,
                                   const size_t lmax, const double x,
                                   const double csphase,
                                   double result_array[],
                                   double result_deriv_array[],
                                   double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_alt_array(const gsl_sf_legendre_t norm,
                                     const size_t lmax, const double x,
                                     double result_array[],
                                     double result_deriv_array[],
                                     double result_deriv2_array[]);
int gsl_sf_legendre_deriv2_alt_array_e(const gsl_sf_legendre_t norm,
                                       const size_t lmax, const double x,
                                       const double csphase,
                                       double result_array[],
                                       double result_deriv_array[],
                                       double result_deriv2_array[]);
size_t gsl_sf_legendre_array_n(const size_t lmax);
size_t gsl_sf_legendre_array_index(const size_t l, const size_t m);
size_t gsl_sf_legendre_nlm(const size_t lmax);

__END_DECLS

#endif /* __GSL_SF_LEGENDRE_H__ */


/* randist/dirichlet.c
 *
 * Copyright (C) 2007 Brian Gough
 * Copyright (C) 2002 Gavin E. Crooks <gec@compbio.berkeley.edu>
 *
 * 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 <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sf_gamma.h>

/* The Dirichlet probability distribution of order K-1 is

     p(\theta_1,...,\theta_K) d\theta_1 ... d\theta_K =
        (1/Z) \prod_i=1,K \theta_i^{alpha_i - 1} \delta(1 -\sum_i=1,K \theta_i)

   The normalization factor Z can be expressed in terms of gamma functions:

      Z = {\prod_i=1,K \Gamma(\alpha_i)} / {\Gamma( \sum_i=1,K \alpha_i)}

   The K constants, \alpha_1,...,\alpha_K, must be positive. The K parameters,
   \theta_1,...,\theta_K are nonnegative and sum to 1.

   The random variates are generated by sampling K values from gamma
   distributions with parameters a=\alpha_i, b=1, and renormalizing.
   See A.M. Law, W.D. Kelton, Simulation Modeling and Analysis (1991).

   Gavin E. Crooks <gec@compbio.berkeley.edu> (2002)
*/

static void ran_dirichlet_small (const gsl_rng * r, const size_t K, const double alpha[], double theta[]);

void
gsl_ran_dirichlet (const gsl_rng * r, const size_t K,
                   const double alpha[], double theta[])
{
  size_t i;
  double norm = 0.0;

  for (i = 0; i < K; i++)
    {
      theta[i] = gsl_ran_gamma (r, alpha[i], 1.0);
    }

  for (i = 0; i < K; i++)
    {
      norm += theta[i];
    }

  if (norm < GSL_SQRT_DBL_MIN)  /* Handle underflow */
    {
      ran_dirichlet_small (r, K, alpha, theta);
      return;
    }

  for (i = 0; i < K; i++)
    {
      theta[i] /= norm;
    }
}

/* When the values of alpha[] are small, scale the variates to avoid
   underflow so that the result is not 0/0.  Note that the Dirichlet
   distribution is defined by a ratio of gamma functions so we can
   take out an arbitrary factor to keep the values in the range of
   double precision. */

static void
ran_dirichlet_small (const gsl_rng * r, const size_t K,
                     const double alpha[], double theta[])
{
  size_t i;
  double norm = 0.0, umax = 0;

  for (i = 0; i < K; i++)
    {
      double u = log(gsl_rng_uniform_pos (r)) / alpha[i];

      theta[i] = u;

      if (u > umax || i == 0) {
        umax = u;
      }
    }

  for (i = 0; i < K; i++)
    {
      theta[i] = exp(theta[i] - umax);
    }

  for (i = 0; i < K; i++)
    {
      theta[i] = theta[i] * gsl_ran_gamma (r, alpha[i] + 1.0, 1.0);
    }

  for (i = 0; i < K; i++)
    {
      norm += theta[i];
    }

  for (i = 0; i < K; i++)
    {
      theta[i] /= norm;
    }
}

double
gsl_ran_dirichlet_pdf (const size_t K,
                       const double alpha[], const double theta[])
{
  return exp (gsl_ran_dirichlet_lnpdf (K, alpha, theta));
}

double
gsl_ran_dirichlet_lnpdf (const size_t K,
                         const double alpha[], const double theta[])
{
  /*We calculate the log of the pdf to minimize the possibility of overflow */
  size_t i;
  double log_p = 0.0;
  double sum_alpha = 0.0;

  for (i = 0; i < K; i++)
    {
      log_p += (alpha[i] - 1.0) * log (theta[i]);
    }

  for (i = 0; i < K; i++)
    {
      sum_alpha += alpha[i];
    }

  log_p += gsl_sf_lngamma (sum_alpha);

  for (i = 0; i < K; i++)
    {
      log_p -= gsl_sf_lngamma (alpha[i]);
    }

  return log_p;
}


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

double w[] = {	52.21, 53.12, 54.48, 55.84, 57.20,
		58.57, 59.93, 61.29, 63.11, 64.47,
		66.28, 68.10, 69.92, 72.19, 74.46 };
double h[] = {	1.47, 1.50, 1.52, 1.55, 1.57,
		1.60, 1.63, 1.65, 1.68, 1.70,
		1.73, 1.75, 1.78, 1.80, 1.83	};

int main()
{
	int n = sizeof(h)/sizeof(double);
	gsl_matrix *X = gsl_matrix_calloc(n, 3);
	gsl_vector *Y = gsl_vector_alloc(n);
	gsl_vector *beta = gsl_vector_alloc(3);

	for (int i = 0; i < n; i++) {
		gsl_vector_set(Y, i, w[i]);

		gsl_matrix_set(X, i, 0, 1);
		gsl_matrix_set(X, i, 1, h[i]);
		gsl_matrix_set(X, i, 2, h[i] * h[i]);
	}

	double chisq;
	gsl_matrix *cov = gsl_matrix_alloc(3, 3);
	gsl_multifit_linear_workspace * wspc = gsl_multifit_linear_alloc(n, 3);
	gsl_multifit_linear(X, Y, beta, cov, &chisq, wspc);

	printf("Beta:");
	for (int i = 0; i < 3; i++)
		printf("  %g", gsl_vector_get(beta, i));
	printf("\n");

	gsl_matrix_free(X);
	gsl_matrix_free(cov);
	gsl_vector_free(Y);
	gsl_vector_free(beta);
	gsl_multifit_linear_free(wspc);

}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf_expint.h>

#include "cosmocalc.h"
#include "haloprofs.h"

double fourierTransformNFW(double k, double m, double rvir, double c)
{
  double rs,rhos;
  double ukm;

  rs = rvir/c;
  rhos = m/4.0/M_PI/rs/rs/rs/(log(1.0+c) - c/(1.0+c));

  if(k > 0)
    ukm = 4.0*M_PI*rhos*rs*rs*rs/m
      *(sin(k*rs)*(gsl_sf_Si((1.0+c)*k*rs) - gsl_sf_Si(k*rs)) - sin(c*k*rs)/(1.0+c)/k/rs + cos(k*rs)*(gsl_sf_Ci((1.0+c)*k*rs) - gsl_sf_Ci(k*rs)));
  else
    ukm = 1.0;

  return ukm;
}

double NFWprof(double r, double m, double rvir, double c)
{
  double rs,rhos;

  rs = rvir/c;
  rhos = m/4.0/M_PI/rs/rs/rs/(log(1.0+c) - c/(1.0+c));

  return rhos/(r/rs)/(1+r/rs)/(1+r/rs);
}

double NFWprof_menc(double r, double m, double rvir, double c)
{
  double mc = log(1+c) - c/(1.0+c);
  double mr = c*r/rvir;
  mr = log(1.0 + mr) - mr/(1.0+mr);

  return mr/mc*m;
}

inline double _duffy2008_concNFW(double m, double a)
{
  return 6.71/pow(1.0/a,0.44)*pow(m/2e12,-0.091);
}

double concNFW(double m, double a)
{
  return _duffy2008_concNFW(m,a);
}

double duffy2008_concNFW(double m, double a)
{
  return _duffy2008_concNFW(m,a);
}

/*
double bh2012_concNFW(double m, double a)
{
  double nu = DELTAC/sigmaMtophat(m,a);

}

double prada2011_concNFW(double m, double a)
{

}
*/


/* roots/secant.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Reid Priedhorsky, Brian Gough
 *
 * 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.
 */

/* secant.c -- secant root finding algorithm

   The secant algorithm is a variant of the Newton algorithm with the
   derivative term replaced by a numerical estimate from the last two
   function evaluations.

   x[i+1] = x[i] - f(x[i]) / f'_est

   where f'_est = (f(x[i]) - f(x[i-1])) / (x[i] - x[i-1])

   The exact derivative is used for the initial value of f'_est.

*/

#include <config.h>

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double f;
    double df;
  }
secant_state_t;

static int secant_init (void * vstate, gsl_function_fdf * fdf, double * root);
static int secant_iterate (void * vstate, gsl_function_fdf * fdf, double * root);

static int
secant_init (void * vstate, gsl_function_fdf * fdf, double * root)
{
  secant_state_t * state = (secant_state_t *) vstate;

  const double x = *root;

  GSL_FN_FDF_EVAL_F_DF (fdf, x, &(state->f), &(state->df));

  return GSL_SUCCESS;

}

static int
secant_iterate (void * vstate, gsl_function_fdf * fdf, double * root)
{
  secant_state_t * state = (secant_state_t *) vstate;

  const double x = *root ;
  const double f = state->f;
  const double df = state->df;

  double x_new, f_new, df_new;

  if (state->df == 0.0)
    {
      GSL_ERROR("derivative is zero", GSL_EZERODIV);
    }

  x_new = x - (f / df);

  f_new = GSL_FN_FDF_EVAL_F(fdf, x_new) ;
  df_new = (f_new - f) / (x_new - x) ;

  *root = x_new ;

  state->f = f_new ;
  state->df = df_new ;

  if (!finite (f_new))
    {
      GSL_ERROR ("function value is not finite", GSL_EBADFUNC);
    }

  if (!finite (df_new))
    {
      GSL_ERROR ("derivative value is not finite", GSL_EBADFUNC);
    }

  return GSL_SUCCESS;
}

static const gsl_root_fdfsolver_type secant_type =
{"secant",                              /* name */
 sizeof (secant_state_t),
 &secant_init,
 &secant_iterate};

const gsl_root_fdfsolver_type  * gsl_root_fdfsolver_secant = &secant_type;


#include "linearwaves.h"
#include <gsl/gsl_integration.h>
typedef struct GSL_params {
    double x;
    int m;
} GSL_params;

void init_params(void) {

    params.mp = 1e-5;
    params.softening = .6*.05;
    params.softening2 = params.softening*params.softening;
    params.a = 1;
    params.indirect = TRUE;
    return;

}

double potential(double phi, double x) {
    double res = -pow(x*x + params.a*params.a + params.softening2 - 2*params.a*x*cos(phi),-.5);
    if (params.indirect) {
        res += cos(phi)*params.a/(x*x);
    }
    return res;
}

double dr_potential(double phi, double x) {
    double res = ( x - params.a*cos(phi)) * pow(x*x + params.a*params.a + params.softening2 - 2*params.a*x*cos(phi),-1.5);

    if (params.indirect) {
        res -= 2* cos(phi)*params.a/(x*x*x);
    }
    return res;
}

double dp_potential(double phi, double x) {
    double res = params.a*x*sin(phi)* pow(x*x + params.a*params.a + params.softening2 - 2*params.a*x*cos(phi),-1.5);

    if (params.indirect) {
        res +=  sin(phi)*params.a/(x*x);
    }
    return res;
}

double pfunc(double phi, void *params) {
    GSL_params p = *(GSL_params *)params;
    double x  = p.x;
    int m = p.m;
    return cos(m*phi) * potential(phi,x);
}
double dpfunc(double phi, void *params) {
    GSL_params p = *(GSL_params *)params;
    double x  = p.x;
    int m = p.m;
    return cos(m*phi) * dr_potential(phi,x);
}

void force(double x, int m, double complex *res) {
    int nspace = 1000;
    int gsl_order = 5;
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(nspace);

    gsl_function F1, F2;
    F1.function = &pfunc;
    F2.function = &dpfunc;
    GSL_params p;
    p.x = x;
    p.m = m;
    F1.params = &p;
    F2.params = &p;

    double tol = params.tol;
    double error;

    double res_r, dr_res_r;
    gsl_integration_qag(&F1,0,M_PI, tol, tol, nspace, gsl_order , w, &res_r, &error);
    gsl_integration_qag(&F2,0,M_PI, tol, tol, nspace, gsl_order , w, &dr_res_r, &error);
    res_r *= params.mp/M_PI;
    dr_res_r *= params.mp/M_PI;

    res[0] = dr_res_r;// *-1;
    res[1] = res_r ;//* -I * m/x;
    res[2] = 0;
    gsl_integration_workspace_free(w);
    return;
}


//
// Created by Harold on 2020/9/16.
//

#ifndef M_MATH_M_INTERP_H
#define M_MATH_M_INTERP_H

#include <gsl/gsl_interp.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>
#include <vector>
#include <algorithm>

namespace M_MATH {
    class Interpolation1D {
    public:
        enum InterpType {
            LINEAR,
            POLYNOMIAL,
            CSPLINE,
            CSPLINE_PERIODIC,
            AKIMA,
            AKIMA_PERIODIC,
            STEFFEN
        };

        static bool interpolate(double const* x,
                                double const* y,
                                size_t length,
                                InterpType type,
                                double const* n_x,
                                size_t n_length,
                                double* n_y);

        // this will create tmp var inside, better to use former one if possible
        template<typename IT_X, typename IT_Y, typename IT_NX, typename IT_NY>
        static bool interpolate(IT_X x_begin, IT_X x_end,
                                IT_Y y_begin, IT_Y y_end,
                                InterpType type,
                                IT_NX nx_begin, IT_NX nx_end,
                                IT_NY ny_begin);

    private:
        static const gsl_interp_type* InterpType2GSL(InterpType type);
    };

    class Interpolation2D {
    public:
        enum InterpType {
            BI_LINEAR,
            BI_CUBIC
        };

        static bool interpolate(double const* x,
                                double const* y,
                                double const* z,
                                size_t x_length,
                                size_t y_length,
                                InterpType type,
                                double const* n_x,
                                double const* n_y,
                                size_t n_x_length,
                                size_t n_y_length,
                                double* n_z);

        // this will create tmp var inside, better to use former one if possible
        template<typename IT_X, typename IT_Y, typename IT_Z, typename IT_NX, typename IT_NY, typename IT_NZ>
        static bool interpolate(IT_X x_begin, IT_X x_end,
                                IT_Y y_begin, IT_Y y_end,
                                IT_Z z_begin, IT_Z z_end,
                                InterpType type,
                                IT_NX nx_begin, IT_NX nx_end,
                                IT_NY ny_begin, IT_NY ny_end,
                                IT_NZ nz_begin);

    private:
        static const gsl_interp2d_type* InterpType2GSL(InterpType type);
    };

    const gsl_interp_type *Interpolation1D::InterpType2GSL(Interpolation1D::InterpType type) {
        switch (type) {
            case LINEAR:
                return gsl_interp_linear;
            case POLYNOMIAL:
                return gsl_interp_polynomial;
            case CSPLINE:
                return gsl_interp_cspline;
            case CSPLINE_PERIODIC:
                return gsl_interp_cspline_periodic;
            case AKIMA:
                return gsl_interp_akima;
            case AKIMA_PERIODIC:
                return gsl_interp_akima_periodic;
            case STEFFEN:
                return gsl_interp_steffen;
        }
        return nullptr;
    }

    bool Interpolation1D::interpolate(const double *x,
                                      const double *y,
                                      size_t length,
                                      Interpolation1D::InterpType type,
                                      const double *n_x,
                                      size_t n_length,
                                      double* n_y) {
        gsl_spline* spl = gsl_spline_alloc(InterpType2GSL(type), length);
        if (spl == nullptr)
            return false;
        gsl_interp_accel* acc = gsl_interp_accel_alloc();
        if (acc == nullptr) {
            return false;
        }
        gsl_spline_init(spl, x, y, length);
        for (auto i = 0; i < n_length; ++i)
            n_y[i] = gsl_spline_eval(spl, n_x[i], acc);
        gsl_spline_free(spl);
        gsl_interp_accel_free(acc);
        return true;
    }

    template<typename IT_X, typename IT_Y, typename IT_NX, typename IT_NY>
    bool Interpolation1D::interpolate(const IT_X x_begin, const IT_X x_end,
                                      const IT_Y y_begin, const IT_Y y_end,
                                      Interpolation1D::InterpType type,
                                      const IT_NX nx_begin, const IT_NX nx_end,
                                      IT_NY ny_begin) {
        auto len = std::min(std::distance(x_begin, x_end), std::distance(y_begin, y_end));
        auto n_len = std::distance(nx_begin, nx_end);
        std::vector<double> x_tmp(len), y_tmp(len);
        std::copy(x_begin, x_begin+len, x_tmp.begin());
        std::copy(y_begin, y_begin+len, y_tmp.begin());

        gsl_spline* spl = gsl_spline_alloc(InterpType2GSL(type), len);
        if (spl == nullptr)
            return false;
        gsl_interp_accel* acc = gsl_interp_accel_alloc();
        if (acc == nullptr) {
            return false;
        }
        gsl_spline_init(spl, x_tmp.data(), y_tmp.data(), len);
        for (auto i = 0; i < n_len; ++i)
            *(ny_begin+i) = gsl_spline_eval(spl, *(nx_begin+i), acc);
        gsl_spline_free(spl);
        gsl_interp_accel_free(acc);
        return true;
    }

    const gsl_interp2d_type *Interpolation2D::InterpType2GSL(Interpolation2D::InterpType type) {
        switch (type) {
            case BI_LINEAR:
                return gsl_interp2d_bilinear;
            case BI_CUBIC:
                return gsl_interp2d_bicubic;
        }
        return nullptr;
    }

    bool Interpolation2D::interpolate(const double *x,
                                      const double *y,
                                      const double *z,
                                      size_t x_length,
                                      size_t y_length,
                                      Interpolation2D::InterpType type,
                                      const double *n_x,
                                      const double *n_y,
                                      size_t n_x_length,
                                      size_t n_y_length,
                                      double *n_z) {
        gsl_spline2d* spl = gsl_spline2d_alloc(InterpType2GSL(type), x_length, y_length);
        if (spl == nullptr)
            return false;
        gsl_interp_accel *x_acc = gsl_interp_accel_alloc();
        if (x_acc == nullptr)
            return false;
        gsl_interp_accel *y_acc = gsl_interp_accel_alloc();
        if (y_acc == nullptr)
            return false;
        // zij = z[j * x_length + i]
        gsl_spline2d_init(spl, x, y, z, x_length, y_length);
        for (auto i = 0; i < n_x_length; ++i)
            for (auto j = 0; j < n_y_length; ++j)
                n_z[j * n_x_length + i] = gsl_spline2d_eval(spl, n_x[i], n_y[j], x_acc, y_acc);
        gsl_spline2d_free(spl);
        gsl_interp_accel_free(x_acc);
        gsl_interp_accel_free(y_acc);
        return true;
    }

    template<typename IT_X, typename IT_Y, typename IT_Z, typename IT_NX, typename IT_NY, typename IT_NZ>
    bool Interpolation2D::interpolate(const IT_X x_begin, const IT_X x_end,
                                      const IT_Y y_begin, const IT_Y y_end,
                                      const IT_Z z_begin, const IT_Z z_end,
                                      Interpolation2D::InterpType type,
                                      const IT_NX nx_begin, const IT_NX nx_end,
                                      const IT_NY ny_begin, const IT_NY ny_end,
                                      const IT_NZ nz_begin) {
        auto x_length = std::distance(x_begin, x_end);
        auto y_length = std::distance(y_begin, y_end);
        auto z_length = std::distance(z_begin, z_end);
        auto n_x_length = std::distance(nx_begin, nx_end);
        auto n_y_length = std::distance(ny_begin, ny_end);
        std::vector<double> x_tmp(x_length), y_tmp(y_length), z_tmp(z_length);
        std::copy(x_begin, x_end, x_tmp.begin());
        std::copy(y_begin, y_end, y_tmp.begin());
        std::copy(z_begin, z_end, z_tmp.begin());

        gsl_spline2d* spl = gsl_spline2d_alloc(InterpType2GSL(type), x_length, y_length);
        if (spl == nullptr)
            return false;
        gsl_interp_accel *x_acc = gsl_interp_accel_alloc();
        if (x_acc == nullptr)
            return false;
        gsl_interp_accel *y_acc = gsl_interp_accel_alloc();
        if (y_acc == nullptr)
            return false;
        // zij = z[j * x_length + i]
        gsl_spline2d_init(spl, x_tmp.data(), y_tmp.data(), z_tmp.data(), x_length, y_length);
        for (auto i = 0; i < n_x_length; ++i)
            for (auto j = 0; j < n_y_length; ++j)
                *(nz_begin + j * n_x_length + i) = gsl_spline2d_eval(spl, *(nx_begin + i), *(ny_begin + j), x_acc, y_acc);
        gsl_spline2d_free(spl);
        gsl_interp_accel_free(x_acc);
        gsl_interp_accel_free(y_acc);
        return true;
    }
}

#endif //M_MATH_M_INTERP_H


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

#include "../ellipsoid/ellipsoid.h"

#undef __FUNCT__
#define __FUNCT__ "TestNormalizationMPFR"
PetscErrorCode TestNormalizationMPFR()
{
  PetscErrorCode ierr;
  EllipsoidalSystem e;
  const PetscInt prec = 128;
  PetscReal xA = 3.0;
  PetscReal yB = 2.0;
  PetscReal zC = 1.0;
  mpfr_t a, b, c;
  mpfr_t a2, b2, c2;
  mpfr_t a3, b3, c3;
  mpfr_t temp1, temp2, temp3;
  mpfr_t pi;

  PetscFunctionBegin;
  mpfr_set_default_prec(4*prec);

  mpfr_inits(a, b, c, NULL);
  mpfr_inits(a2, b2, c2, NULL);
  mpfr_inits(a3, b3, c3, NULL);
  mpfr_inits(temp1, temp2, temp3, pi, NULL);

  mpfr_set_d(a, xA, MPFR_RNDN);
  mpfr_set_d(b, yB, MPFR_RNDN);
  mpfr_set_d(c, zC, MPFR_RNDN);

  //init ellipsoidal system
  printf("initEllipsoidalSystem...\n");
  ierr = initEllipsoidalSystem(&e, xA, yB, zC, prec);CHKERRQ(ierr);
  printf("done.\n");

  mpfr_mul(a2, a, a, MPFR_RNDN);
  mpfr_mul(a3, a2, a, MPFR_RNDN);
  mpfr_mul(b2, b, b, MPFR_RNDN);
  mpfr_mul(b3, b2, b, MPFR_RNDN);
  mpfr_mul(c2, c, c, MPFR_RNDN);
  mpfr_mul(c3, c2, c, MPFR_RNDN);

  mpfr_t firstTerm, secondTerm, LambdaD, LambdaDprime;
  mpfr_inits(firstTerm, secondTerm, LambdaD, LambdaDprime, NULL);

  //Dassios (B14)
  //firstTerm = (a*a + b*b + c*c)/3.0;
  mpfr_add(firstTerm, a2, b2, MPFR_RNDN);
  mpfr_add(firstTerm, firstTerm, c2, MPFR_RNDN);
  mpfr_div_d(firstTerm, firstTerm, 3.0, MPFR_RNDN);

  //double secondTerm = sqrt((a*a*a*a - b*b*c*c) + (b*b*b*b - a*a*c*c) + (c*c*c*c - a*a*b*b))/3.0;
  mpfr_mul(temp1, a3, a, MPFR_RNDN);
  mpfr_mul(temp2, b2, c2, MPFR_RNDN);
  mpfr_sub(temp1, temp1, temp2, MPFR_RNDN);
  mpfr_mul(temp2, b3, b, MPFR_RNDN);
  mpfr_mul(temp3, a2, c2, MPFR_RNDN);
  mpfr_sub(temp2, temp2, temp3, MPFR_RNDN);
  mpfr_add(temp1, temp1, temp2, MPFR_RNDN);
  mpfr_mul(temp2, c3, c, MPFR_RNDN);
  mpfr_mul(temp3, a2, b2, MPFR_RNDN);
  mpfr_sub(temp2, temp2, temp3, MPFR_RNDN);
  mpfr_add(temp1, temp1, temp2, MPFR_RNDN);
  mpfr_sqrt(secondTerm, temp1, MPFR_RNDN);

  //double LambdaD = firstTerm + secondTerm;
  mpfr_add(LambdaD, firstTerm, secondTerm, MPFR_RNDN);

  //double LambdaDprime = firstTerm - secondTerm;
  mpfr_sub(LambdaDprime, firstTerm, secondTerm, MPFR_RNDN);

  mpfr_t hx, hy, hz;
  mpfr_inits(hx, hy, hz, NULL);

  //Dassios (B16)-(B20)
  //double hx = sqrt(b*b - c*c);
  mpfr_sub(temp1, b2, c2, MPFR_RNDN);
  mpfr_sqrt(hx, temp1, MPFR_RNDN);

  //double hy = e.k;
  mpfr_set(hy, e.hp_k, MPFR_RNDN);
  //double hz = e.h;
  mpfr_set(hz, e.hp_h, MPFR_RNDN);

  //set pi
  mpfr_const_pi(pi, MPFR_RNDN);

  mpfr_t analytic[9];
  mpfr_t approx[9], doubleApprox[9];
  for(PetscInt k=0; k < 9; ++k)
    mpfr_inits(analytic[k], approx[k], doubleApprox[k], NULL);

  //analytic[0] = 4*PETSC_PI
  mpfr_mul_d(analytic[0], pi, 4.0, MPFR_RNDN);
  printf("analytic[0] = %4.4e\n", mpfr_get_d(analytic[0], MPFR_RNDN));

  //analytic[1] = 4*PETSC_PI/3 * hy*hy*hz*hz
  mpfr_div_d(temp1, pi, 3.0, MPFR_RNDN);
  mpfr_mul_d(temp1, temp1, 4.0, MPFR_RNDN);
  mpfr_mul(temp2, hy, hy, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hz, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hz, MPFR_RNDN);
  mpfr_mul(analytic[1], temp1, temp2, MPFR_RNDN);

  //analytic[2] = 4*PETSC_PI/3 * hx*hx*hz*hz
  mpfr_div_d(temp1, pi, 3.0, MPFR_RNDN);
  mpfr_mul_d(temp1, temp1, 4.0, MPFR_RNDN);
  mpfr_mul(temp2, hx, hx, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hz, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hz, MPFR_RNDN);
  mpfr_mul(analytic[2], temp1, temp2, MPFR_RNDN);

  //analytic[3] = 4*PETSC_PI/3 * hx*hx*hy*hy
  mpfr_div_d(temp1, pi, 3.0, MPFR_RNDN);
  mpfr_mul_d(temp1, temp1, 4.0, MPFR_RNDN);
  mpfr_mul(temp2, hx, hx, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hy, MPFR_RNDN);
  mpfr_mul(temp2, temp2, hy, MPFR_RNDN);
  mpfr_mul(analytic[3], temp1, temp2, MPFR_RNDN);

  //analytic[4] = -8*PETSC_PI/5 * (LambdaD - LambdaDprime)*(LambdaD - a*a)*(LambdaD - b*b)*(LambdaD - c*c)

  //analytic[5] = 8*PETSC_PI/5 * (LambdaD - LambdaDprime)*(LambdaDprime - a*a)*(LambdaDprime - b*b)*(LambdaDprime - c*c),

  ierr = calcNormalizationMPFR(&e, 0, 0, approx+0);CHKERRQ(ierr);
  ierr = calcNormalization    (&e, 0, 0, doubleApprox+0);CHKERRQ(ierr);
  ierr = calcNormalizationMPFR(&e, 1, 0, approx+1);CHKERRQ(ierr);
  ierr = calcNormalizationMPFR(&e, 1, 1, approx+2);CHKERRQ(ierr);
  mpfr_t err[9];
  for(PetscInt k=0; k < 4; ++k) {
    mpfr_init(err[k]);
    mpfr_sub(err[k], approx[k], analytic[k], MPFR_RNDN);
    mpfr_div(err[k], err[k], analytic[k], MPFR_RNDN);
    mpfr_abs(err[k], err[k], MPFR_RNDN);
    mpfr_log10(err[k], err[k], MPFR_RNDN);
    printf("the error is %2.2f\n", mpfr_get_d(err[k], MPFR_RNDN));
  }

  for(PetscInt k=0; k < 9; ++k)
    mpfr_clears(analytic[k], approx[k], err[k], doubleApprox[k], NULL);
  mpfr_clears(hx, hy, hz, NULL);
  mpfr_clears(temp1, temp2, temp3, pi, NULL);
  mpfr_clears(a3, b3, c3, NULL);
  mpfr_clears(a2, b2, c2, NULL);
  mpfr_clears(a, b, c, NULL);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT __ "main"
PetscErrorCode main(int argc, char **argv)
{
  PetscErrorCode ierr;

  PetscFunctionBeginUser;

  ierr = PetscInitialize(&argc, &argv, NULL, NULL);CHKERRQ(ierr);

  ierr = TestNormalizationMPFR();

  ierr = PetscFinalize();

}


static char help[] = "A structured-grid Poisson solver using DMDA+KSP.\n\n";

#include <petsc.h>

extern PetscErrorCode formMatrix(DM, Mat);
extern PetscErrorCode formExact(DM, Vec);
extern PetscErrorCode formRHS(DM, Vec);

//STARTMAIN
int main(int argc,char **args) {
    PetscErrorCode ierr;
    DM            da;
    Mat           A;
    Vec           b,u,uexact;
    KSP           ksp;
    PetscReal     errnorm;
    DMDALocalInfo info;

    ierr = PetscInitialize(&argc,&args,NULL,help); if (ierr) return ierr;

    // change default 9x9 size using -da_grid_x M -da_grid_y N
    ierr = DMDACreate2d(PETSC_COMM_WORLD,
                 DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
                 9,9,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);

    // create linear system matrix A
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMCreateMatrix(da,&A); CHKERRQ(ierr);
    ierr = MatSetFromOptions(A); CHKERRQ(ierr);

    // create RHS b, approx solution u, exact solution uexact
    ierr = DMCreateGlobalVector(da,&b); CHKERRQ(ierr);
    ierr = VecDuplicate(b,&u); CHKERRQ(ierr);
    ierr = VecDuplicate(b,&uexact); CHKERRQ(ierr);

    // fill vectors and assemble linear system
    ierr = formExact(da,uexact); CHKERRQ(ierr);
    ierr = formRHS(da,b); CHKERRQ(ierr);
    ierr = formMatrix(da,A); CHKERRQ(ierr);

    // create and solve the linear system
    ierr = KSPCreate(PETSC_COMM_WORLD,&ksp); CHKERRQ(ierr);
    ierr = KSPSetOperators(ksp,A,A); CHKERRQ(ierr);
    ierr = KSPSetFromOptions(ksp); CHKERRQ(ierr);
    ierr = KSPSolve(ksp,b,u); CHKERRQ(ierr);

    // report on grid and numerical error
    ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr);    // u <- u + (-1.0) uxact
    ierr = VecNorm(u,NORM_INFINITY,&errnorm); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info);CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
                "on %d x %d grid:  error |u-uexact|_inf = %g\n",
                info.mx,info.my,errnorm); CHKERRQ(ierr);

    VecDestroy(&u);  VecDestroy(&uexact);  VecDestroy(&b);
    MatDestroy(&A);  KSPDestroy(&ksp);  DMDestroy(&da);
    return PetscFinalize();
}
//ENDMAIN

//STARTMATRIX
PetscErrorCode formMatrix(DM da, Mat A) {
    PetscErrorCode ierr;
    DMDALocalInfo  info;
    MatStencil     row, col[5];
    PetscReal      hx, hy, v[5];
    PetscInt       i, j, ncols;

    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    hx = 1.0/(info.mx-1);  hy = 1.0/(info.my-1);
    for (j = info.ys; j < info.ys+info.ym; j++) {
        for (i = info.xs; i < info.xs+info.xm; i++) {
            row.j = j;           // row of A corresponding to (x_i,y_j)
            row.i = i;
            col[0].j = j;        // diagonal entry
            col[0].i = i;
            ncols = 1;
            if (i==0 || i==info.mx-1 || j==0 || j==info.my-1) {
                v[0] = 1.0;      // on boundary: trivial equation
            } else {
                v[0] = 2*(hy/hx + hx/hy); // interior: build a row
                if (i-1 > 0) {
                    col[ncols].j = j;    col[ncols].i = i-1;
                    v[ncols++] = -hy/hx;
                }
                if (i+1 < info.mx-1) {
                    col[ncols].j = j;    col[ncols].i = i+1;
                    v[ncols++] = -hy/hx;
                }
                if (j-1 > 0) {
                    col[ncols].j = j-1;  col[ncols].i = i;
                    v[ncols++] = -hx/hy;
                }
                if (j+1 < info.my-1) {
                    col[ncols].j = j+1;  col[ncols].i = i;
                    v[ncols++] = -hx/hy;
                }
            }
            ierr = MatSetValuesStencil(A,1,&row,ncols,col,v,INSERT_VALUES); CHKERRQ(ierr);
        }
    }
    ierr = MatAssemblyBegin(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(A,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    return 0;
}
//ENDMATRIX

//STARTEXACT
PetscErrorCode formExact(DM da, Vec uexact) {
    PetscErrorCode ierr;
    PetscInt       i, j;
    PetscReal      hx, hy, x, y, **auexact;
    DMDALocalInfo  info;

    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    hx = 1.0/(info.mx-1);  hy = 1.0/(info.my-1);
    ierr = DMDAVecGetArray(da, uexact, &auexact);CHKERRQ(ierr);
    for (j = info.ys; j < info.ys+info.ym; j++) {
        y = j * hy;
        for (i = info.xs; i < info.xs+info.xm; i++) {
            x = i * hx;
            auexact[j][i] = x*x * (1.0 - x*x) * y*y * (y*y - 1.0);
        }
    }
    ierr = DMDAVecRestoreArray(da, uexact, &auexact);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode formRHS(DM da, Vec b) {
    PetscErrorCode ierr;
    PetscInt       i, j;
    PetscReal      hx, hy, x, y, f, **ab;
    DMDALocalInfo  info;

    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    hx = 1.0/(info.mx-1);  hy = 1.0/(info.my-1);
    ierr = DMDAVecGetArray(da, b, &ab);CHKERRQ(ierr);
    for (j=info.ys; j<info.ys+info.ym; j++) {
        y = j * hy;
        for (i=info.xs; i<info.xs+info.xm; i++) {
            x = i * hx;
            if (i==0 || i==info.mx-1 || j==0 || j==info.my-1) {
                ab[j][i] = 0.0;  // on boundary: 1*u = 0
            } else {
                f = 2.0 * ( (1.0 - 6.0*x*x) * y*y * (1.0 - y*y)
                    + (1.0 - 6.0*y*y) * x*x * (1.0 - x*x) );
                ab[j][i] = hx * hy * f;
            }
        }
    }
    ierr = DMDAVecRestoreArray(da, b, &ab); CHKERRQ(ierr);
    return 0;
}
//ENDEXACT


/* deriv/deriv.c
 *
 * Copyright (C) 2004, 2007 Brian Gough
 *
 * 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 <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_deriv.h>

static void
central_deriv (const gsl_function * f, double x, double h,
               double *result, double *abserr_round, double *abserr_trunc)
{
  /* Compute the derivative using the 5-point rule (x-h, x-h/2, x,
     x+h/2, x+h). Note that the central point is not used.

     Compute the error using the difference between the 5-point and
     the 3-point rule (x-h,x,x+h). Again the central point is not
     used. */

  double fm1 = GSL_FN_EVAL (f, x - h);
  double fp1 = GSL_FN_EVAL (f, x + h);

  double fmh = GSL_FN_EVAL (f, x - h / 2);
  double fph = GSL_FN_EVAL (f, x + h / 2);

  double r3 = 0.5 * (fp1 - fm1);
  double r5 = (4.0 / 3.0) * (fph - fmh) - (1.0 / 3.0) * r3;

  double e3 = (fabs (fp1) + fabs (fm1)) * GSL_DBL_EPSILON;
  double e5 = 2.0 * (fabs (fph) + fabs (fmh)) * GSL_DBL_EPSILON + e3;

  /* The next term is due to finite precision in x+h = O (eps * x) */

  double dy = GSL_MAX (fabs (r3 / h), fabs (r5 / h)) *(fabs (x) / h) * GSL_DBL_EPSILON;

  /* The truncation error in the r5 approximation itself is O(h^4).
     However, for safety, we estimate the error from r5-r3, which is
     O(h^2).  By scaling h we will minimise this estimated error, not
     the actual truncation error in r5. */

  *result = r5 / h;
  *abserr_trunc = fabs ((r5 - r3) / h); /* Estimated truncation error O(h^2) */
  *abserr_round = fabs (e5 / h) + dy;   /* Rounding error (cancellations) */
}

int
gsl_deriv_central (const gsl_function * f, double x, double h,
                   double *result, double *abserr)
{
  double r_0, round, trunc, error;
  central_deriv (f, x, h, &r_0, &round, &trunc);
  error = round + trunc;

  if (round < trunc && (round > 0 && trunc > 0))
    {
      double r_opt, round_opt, trunc_opt, error_opt;

      /* Compute an optimised stepsize to minimize the total error,
         using the scaling of the truncation error (O(h^2)) and
         rounding error (O(1/h)). */

      double h_opt = h * pow (round / (2.0 * trunc), 1.0 / 3.0);
      central_deriv (f, x, h_opt, &r_opt, &round_opt, &trunc_opt);
      error_opt = round_opt + trunc_opt;

      /* Check that the new error is smaller, and that the new derivative
         is consistent with the error bounds of the original estimate. */

      if (error_opt < error && fabs (r_opt - r_0) < 4.0 * error)
        {
          r_0 = r_opt;
          error = error_opt;
        }
    }

  *result = r_0;
  *abserr = error;

  return GSL_SUCCESS;
}

static void
forward_deriv (const gsl_function * f, double x, double h,
               double *result, double *abserr_round, double *abserr_trunc)
{
  /* Compute the derivative using the 4-point rule (x+h/4, x+h/2,
     x+3h/4, x+h).

     Compute the error using the difference between the 4-point and
     the 2-point rule (x+h/2,x+h).  */

  double f1 = GSL_FN_EVAL (f, x + h / 4.0);
  double f2 = GSL_FN_EVAL (f, x + h / 2.0);
  double f3 = GSL_FN_EVAL (f, x + (3.0 / 4.0) * h);
  double f4 = GSL_FN_EVAL (f, x + h);

  double r2 = 2.0*(f4 - f2);
  double r4 = (22.0 / 3.0) * (f4 - f3) - (62.0 / 3.0) * (f3 - f2) +
    (52.0 / 3.0) * (f2 - f1);

  /* Estimate the rounding error for r4 */

  double e4 = 2 * 20.67 * (fabs (f4) + fabs (f3) + fabs (f2) + fabs (f1)) * GSL_DBL_EPSILON;

  /* The next term is due to finite precision in x+h = O (eps * x) */

  double dy = GSL_MAX (fabs (r2 / h), fabs (r4 / h)) * fabs (x / h) * GSL_DBL_EPSILON;

  /* The truncation error in the r4 approximation itself is O(h^3).
     However, for safety, we estimate the error from r4-r2, which is
     O(h).  By scaling h we will minimise this estimated error, not
     the actual truncation error in r4. */

  *result = r4 / h;
  *abserr_trunc = fabs ((r4 - r2) / h); /* Estimated truncation error O(h) */
  *abserr_round = fabs (e4 / h) + dy;
}

int
gsl_deriv_forward (const gsl_function * f, double x, double h,
                   double *result, double *abserr)
{
  double r_0, round, trunc, error;
  forward_deriv (f, x, h, &r_0, &round, &trunc);
  error = round + trunc;

  if (round < trunc && (round > 0 && trunc > 0))
    {
      double r_opt, round_opt, trunc_opt, error_opt;

      /* Compute an optimised stepsize to minimize the total error,
         using the scaling of the estimated truncation error (O(h)) and
         rounding error (O(1/h)). */

      double h_opt = h * pow (round / (trunc), 1.0 / 2.0);
      forward_deriv (f, x, h_opt, &r_opt, &round_opt, &trunc_opt);
      error_opt = round_opt + trunc_opt;

      /* Check that the new error is smaller, and that the new derivative
         is consistent with the error bounds of the original estimate. */

      if (error_opt < error && fabs (r_opt - r_0) < 4.0 * error)
        {
          r_0 = r_opt;
          error = error_opt;
        }
    }

  *result = r_0;
  *abserr = error;

  return GSL_SUCCESS;
}

int
gsl_deriv_backward (const gsl_function * f, double x, double h,
                    double *result, double *abserr)
{
  return gsl_deriv_forward (f, x, -h, result, abserr);
}


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

int
main (void)
{
  double c  = GSL_CONST_MKSA_SPEED_OF_LIGHT;
  double au = GSL_CONST_MKSA_ASTRONOMICAL_UNIT;
  double minutes = GSL_CONST_MKSA_MINUTE;

  /* distance stored in meters */
  double r_earth = 1.00 * au;
  double r_mars  = 1.52 * au;

  double t_min, t_max;

  t_min = (r_mars - r_earth) / c;
  t_max = (r_mars + r_earth) / c;

  printf ("light travel time from Earth to Mars:\n");
  printf ("minimum = %.1f minutes\n", t_min / minutes);
  printf ("maximum = %.1f minutes\n", t_max / minutes);

  return 0;
}