#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlin.h>

struct data {
  size_t n;
  double * y;
  double * sigma;
};

int
expb_f (const gsl_vector * x, void *params,
        gsl_vector * f)
{
  size_t n = ((struct data *)params)->n;
  double *y = ((struct data *)params)->y;
  double *sigma = ((struct data *) params)->sigma;

  double A = gsl_vector_get (x, 0);
  double lambda = gsl_vector_get (x, 1);
  double b = gsl_vector_get (x, 2);

  size_t i;

  for (i = 0; i < n; i++)
    {
      /* Model Yi = A * exp(-lambda * i) + b */
      double t = i;
      double Yi = A * exp (-lambda * t) + b;
      gsl_vector_set (f, i, (Yi - y[i])/sigma[i]);
    }

  return GSL_SUCCESS;
}

int
expb_df (const gsl_vector * x, void *params,
         gsl_matrix * J)
{
  size_t n = ((struct data *)params)->n;
  double *sigma = ((struct data *) params)->sigma;

  double A = gsl_vector_get (x, 0);
  double lambda = gsl_vector_get (x, 1);

  size_t i;

  for (i = 0; i < n; i++)
    {
      /* Jacobian matrix J(i,j) = dfi / dxj, */
      /* where fi = (Yi - yi)/sigma[i],      */
      /*       Yi = A * exp(-lambda * i) + b  */
      /* and the xj are the parameters (A,lambda,b) */
      double t = i;
      double s = sigma[i];
      double e = exp(-lambda * t);
      gsl_matrix_set (J, i, 0, e/s);
      gsl_matrix_set (J, i, 1, -t * A * e/s);
      gsl_matrix_set (J, i, 2, 1/s);

    }
  return GSL_SUCCESS;
}

int
expb_fdf (const gsl_vector * x, void *params,
          gsl_vector * f, gsl_matrix * J)
{
  expb_f (x, params, f);
  expb_df (x, params, J);

  return GSL_SUCCESS;
}

#define N 40

int
main (void)
{
  const gsl_multifit_fdfsolver_type *T;
  gsl_multifit_fdfsolver *s;

  int status;
  size_t i, iter = 0;

  const size_t n = N;
  const size_t p = 3;

  gsl_matrix *covar = gsl_matrix_alloc (p, p);

  double y[N], sigma[N];

  struct data d = { n, y, sigma};

  gsl_multifit_function_fdf f;

  double x_init[3] = { 1.0, 0.0, 0.0 };

  gsl_vector_view x = gsl_vector_view_array (x_init, p);

  const gsl_rng_type * type;
  gsl_rng * r;

  gsl_rng_env_setup();

  type = gsl_rng_default;
  r = gsl_rng_alloc (type);

  f.f = &expb_f;
  f.df = &expb_df;
  f.fdf = &expb_fdf;
  f.n = n;
  f.p = p;
  f.params = &d;

  /* This is the data to be fitted */

  for (i = 0; i < n; i++)
    {
      double t = i;
      y[i] = 1.0 + 5 * exp (-0.1 * t)
                 + gsl_ran_gaussian (r, 0.1);
      sigma[i] = 0.1;
      printf ("data: %d %g %g\n", i, y[i], sigma[i]);
    };

  T = gsl_multifit_fdfsolver_lmsder;
  s = gsl_multifit_fdfsolver_alloc (T, n, p);
  gsl_multifit_fdfsolver_set (s, &f, &x.vector);

  print_state (iter, s);

  do
    {
      iter++;
      status = gsl_multifit_fdfsolver_iterate (s);

      printf ("status = %s\n", gsl_strerror (status));

      print_state (iter, s);

      if (status)
        break;

      status = gsl_multifit_test_delta (s->dx, s->x,
                                        1e-4, 1e-4);
    }
  while (status == GSL_CONTINUE && iter < 500);

  gsl_multifit_covar (s->J, 0.0, covar);

  gsl_matrix_fprintf (stdout, covar, "%g");

#define FIT(i) gsl_vector_get(s->x, i)
#define ERR(i) sqrt(gsl_matrix_get(covar,i,i))

  printf ("A      = %.5f +/- %.5f\n", FIT(0), ERR(0));
  printf ("lambda = %.5f +/- %.5f\n", FIT(1), ERR(1));
  printf ("b      = %.5f +/- %.5f\n", FIT(2), ERR(2));

  {
    double chi = gsl_blas_dnrm2(s->f);
    printf("chisq/dof = %g\n",  pow(chi, 2.0)/ (n - p));
  }

  printf ("status = %s\n", gsl_strerror (status));

  gsl_multifit_fdfsolver_free (s);
  return 0;
}

int
print_state (size_t iter, gsl_multifit_fdfsolver * s)
{
  printf ("iter: %3u x = % 15.8f % 15.8f % 15.8f "
          "|f(x)| = %g\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->x, 2),
          gsl_blas_dnrm2 (s->f));
}


/* randist/bernoulli.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>

/* The bernoulli distribution has the form,

   prob(0) = 1-p, prob(1) = p

   */

unsigned int
gsl_ran_bernoulli (const gsl_rng * r, double p)
{
  double u = gsl_rng_uniform (r) ;

  if (u < p)
    {
      return 1 ;
    }
  else
    {
      return 0 ;
    }
}

double
gsl_ran_bernoulli_pdf (const unsigned int k, double p)
{
  if (k == 0)
    {
      return 1 - p ;
    }
  else if (k == 1)
    {
      return p ;
    }
  else
    {
      return 0 ;
    }
}


#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <stdarg.h>
#include <gbpLib.h>
#include <gbpMath.h>
#include <gbpCosmo_mass_functions.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>

// Scaled mass functions
double scaled_mass_function(double sigma, int mode, double *P) {
    double rval;
    if(SID_CHECK_BITFIELD_SWITCH(mode, MF_WATSON)) { // Watson et al. (2013) universal FoF mass function
        double A     = 0.282;
        double alpha = 2.163;
        double beta  = 1.406;
        double gamma = 1.210;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A     = P[0];
            alpha = P[1];
            beta  = P[2];
            gamma = P[3];
        }
        rval = A * (pow(beta / sigma, alpha) + 1.) * exp(-gamma / (sigma * sigma));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_TIAMAT)) { // Poole et al. (2013) universal FoF mass function for Tiamat
        double A     = 0.03331;
        double alpha = 1.153;
        double beta  = 12.33;
        double gamma = 1.009;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A     = P[0];
            alpha = P[1];
            beta  = P[2];
            gamma = P[3];
        }
        rval = A * (pow(beta / sigma, alpha) + 1.) * exp(-gamma / (sigma * sigma));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_JENKINS)) { // Jenkins et al. (2001)
        double A = 0.315;
        double B = 0.61;
        double C = 3.8;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A = P[0];
            B = P[1];
            C = P[2];
        }
        rval = A * exp(-pow((double)fabs((float)(take_ln(1. / sigma) + B)), C));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_ST)) { // Sheth-Torman (1999)
        double delta_k = 1.686;
        double A       = 0.3222;
        double B       = 0.707;
        double C       = 0.3;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            A = P[0];
            B = P[1];
            C = P[2];
        }
        rval = A * sqrt(2. * B / PI) * (delta_k / sigma) * exp(-B * delta_k * delta_k / (2. * sigma * sigma)) *
               (1. + pow(sigma * sigma / (B * delta_k * delta_k), C));
    } else if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PS)) { // Press-Schechter (1974)
        double delta_k = 1.686;
        if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS)) {
            delta_k = P[0];
        }
        rval = sqrt(2. / PI) * (delta_k / sigma) * exp(-delta_k * delta_k / (2. * sigma * sigma));
    } else
        SID_exit_error("A valid mass function was not specified with mode (%d) in scaled_mass_function().\n",
                       SID_ERROR_LOGIC, mode);
    return (rval);
}


static char help[] =
"Solves a 1D advection plus diffusion problem using FD discretization\n"
"and a structured-grid (DMDA).  Option prefix -b1_.  Equation is\n"
"  - eps u'' + (a(x) u)' = 0\n"
"with a(x)=1, on domain [-1,1], and with Dirichlet boundary conditions\n"
"u(-1) = 1, u(1) = 0.  Default eps=0.01.  The diffusion discretized by\n"
"centered, as usual, but advection is by first-order upwinding, centered,\n"
"or van Leer limiter scheme.  An analytic Jacobian is implemented, except\n"
"for the van Leer limiter.  The limiters in the residual and Jacobian\n"
"evaluations are separately controllable.\n\n";

#include <petsc.h>

typedef enum {NONE, CENTERED, VANLEER} LimiterType;
static const char *LimiterTypes[] = {"none","centered","vanleer",
                                     "LimiterType", "", NULL};

static PetscReal centered(PetscReal theta) {
    return 0.5;
}

static PetscReal vanleer(PetscReal theta) {
    const PetscReal abstheta = PetscAbsReal(theta);
    return 0.5 * (theta + abstheta) / (1.0 + abstheta);
}

static void* limiterptr[] = {NULL, &centered, &vanleer};

typedef struct {
    PetscReal   eps;          // amount of diffusion; require: eps > 0
    PetscReal   (*limiter_fcn)(PetscReal),
                (*jac_limiter_fcn)(PetscReal);
} AdCtx;

static PetscReal u_exact(PetscReal x, AdCtx *usr) {
    return (1.0 - exp((x-1) / usr->eps)) / (1.0 - exp(- 2.0 / usr->eps));
}

static PetscReal wind_a(PetscReal x) {
    return 1.0;
}

extern PetscErrorCode FormUExact(DMDALocalInfo*, AdCtx*, Vec);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, PetscReal*,PetscReal*, AdCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, PetscReal*, Mat, Mat, AdCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da, da_after;
    SNES           snes;
    Vec            u_initial, u, u_exact;
    PetscReal      hx, err2, errinf;
    DMDALocalInfo  info;
    LimiterType    limiter = NONE, jac_limiter;
    PetscBool      snesfdset, snesfdcolorset;
    AdCtx          user;

    PetscInitialize(&argc,&argv,(char*)0,help);

    user.eps = 0.01;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,"b1_",
               "both1d (1D advection-diffusion solver) options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-eps","positive diffusion coefficient",
               "both1d.c",user.eps,&(user.eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-limiter","flux-limiter type",
               "both1d.c",LimiterTypes,
               (PetscEnum)limiter,(PetscEnum*)&limiter,NULL); CHKERRQ(ierr);
    jac_limiter = limiter;
    ierr = PetscOptionsEnum("-jac_limiter","flux-limiter type used in Jacobian evaluation",
               "both1d.c",LimiterTypes,
               (PetscEnum)jac_limiter,(PetscEnum*)&jac_limiter,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    if (user.eps <= 0.0) {
        SETERRQ1(PETSC_COMM_SELF,2,"eps=%.3f invalid ... eps > 0 required",user.eps);
    }
    user.limiter_fcn = limiterptr[limiter];
    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd",&snesfdset); CHKERRQ(ierr);
    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd_color",&snesfdcolorset); CHKERRQ(ierr);
    if (snesfdset || snesfdcolorset) {
        user.jac_limiter_fcn = NULL;
        jac_limiter = 4;   // corresponds to empty string
    } else
        user.jac_limiter_fcn = limiterptr[jac_limiter];

    ierr = DMDACreate1d(PETSC_COMM_WORLD,DM_BOUNDARY_NONE,
                 3, // default to hx=1 grid
                 1,2, // d.o.f., stencil width
                 NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMDASetUniformCoordinates(da,-1.0,1.0,-1.0,1.0,-1.0,1.0); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);

    ierr = SNESCreate(PETSC_COMM_WORLD,&snes);CHKERRQ(ierr);
    ierr = SNESSetDM(snes,da);CHKERRQ(ierr);
    ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
            (DMDASNESFunction)FormFunctionLocal,&user);CHKERRQ(ierr);
    ierr = DMDASNESSetJacobianLocal(da,
            (DMDASNESJacobian)FormJacobianLocal,&user); CHKERRQ(ierr);
    ierr = SNESSetApplicationContext(snes,&user); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

    ierr = DMGetGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = VecSet(u_initial,0.0); CHKERRQ(ierr);
    ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
    ierr = DMRestoreGlobalVector(da,&u_initial); CHKERRQ(ierr);
    ierr = DMDestroy(&da); CHKERRQ(ierr);

    ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
         "done on %d point grid (eps = %g, limiter = %s, jac_limiter = %s)\n",
         info.mx,user.eps,LimiterTypes[limiter],LimiterTypes[jac_limiter]); CHKERRQ(ierr);

    ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
    ierr = FormUExact(&info,&user,u_exact); CHKERRQ(ierr);
    ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) u_exact
    ierr = VecNorm(u,NORM_INFINITY,&errinf); CHKERRQ(ierr);
    ierr = VecNorm(u,NORM_2,&err2); CHKERRQ(ierr);
    hx = 2.0 / (info.mx - 1);
    err2 *= PetscSqrtReal(hx);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
         "numerical error:  |u-uexact|_inf = %.4e,  |u-uexact|_2 = %.4e\n",
         errinf,err2); CHKERRQ(ierr);

    VecDestroy(&u_exact);  SNESDestroy(&snes);
    return PetscFinalize();
}

PetscErrorCode FormUExact(DMDALocalInfo *info, AdCtx *usr, Vec uex) {
    PetscErrorCode  ierr;
    PetscInt   i;
    PetscReal  hx, x, *auex;

    hx = 2.0 / (info->mx - 1);
    ierr = DMDAVecGetArray(info->da, uex, &auex);CHKERRQ(ierr);
    for (i=info->xs; i<info->xs+info->xm; i++) {
        x = -1.0 + i * hx;
        auex[i] = u_exact(x,usr);
    }
    ierr = DMDAVecRestoreArray(info->da, uex, &auex);CHKERRQ(ierr);
    return 0;
}

/* compute residuals:
     F_i = (- eps u'' + (a(x) u)') * hx   at interior points
     F_i = c (u - (b.c.))          at boundary points         */
PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, PetscReal *au,
                                 PetscReal *aF, AdCtx *usr) {
    const PetscReal  eps = usr->eps,
                     hx = 2.0 / (info->mx - 1),
                     halfx = hx / 2.0,
                     hx2 = hx * hx,
                     scdiag = (2.0 * eps) / hx + 1.0;
    PetscReal        x, uE, uW, uxx, a, u_up, flux, u_dn, u_far, theta;
    PetscInt         i;

    // for each owned cell, non-advective part of residual at cell center
    for (i=info->xs; i<info->xs+info->xm; i++) {
        if (i == 0) {
            aF[i] = scdiag * (au[i] - 1.0);
        } else if (i == info->mx-1) {
            aF[i] = scdiag * (au[i] - 0.0);
        } else {
            uW = (i == 1)          ? 1.0 : au[i-1];
            uE = (i == info->mx-2) ? 0.0 : au[i+1];
            uxx = (uW - 2.0 * au[i] + uE) / hx2;
            aF[i] = - eps * uxx * hx;
        }
    }
    // for each E face of an owned cell, compute flux at the face center
    //     and then add that to the correct residual
    // note -1 start to get W faces of owned cells living on ownership
    //     boundaries
    for (i=info->xs-1; i<info->xs+info->xm; i++) {
        // if cell center is outside [-1,1], or on x=1 boundary, then no need
        // to compute a flux
        if (i < 0 || i == info->mx-1)
            continue;
        // traverse E cell face center points x_{i+1/2} for flux contributions
        // get pth component of wind and first-order upwind flux
        x = -1.0 + i * hx;
        a = wind_a(x + halfx);
        if (a >= 0.0) {
            if (i == 0) {
                u_up = 1.0;
            } else {
                u_up = au[i];
            }
        } else {
            if (i+1 == info->mx-1) {
                u_up = 0.0;
            } else {
                u_up = au[i+1];
            }
        }
        flux = a * u_up;
        if (usr->limiter_fcn != NULL) {
            // flux correction from high-order formula with psi(theta)
            if (a >= 0)
                u_dn = (i+1 < info->mx-1) ? au[i+1] : 0.0;
            else
                u_dn = au[i];
            if (u_dn != u_up) {
                if (a >= 0)
                   u_far = (i-1 > 0) ? au[i-1] : 1.0;
                else
                   u_far = (i+2 < info->mx-1) ? au[i+2] : 0.0;
                theta = (u_up - u_far) / (u_dn - u_up);
                flux += a * (*(usr->limiter_fcn))(theta) * (u_dn - u_up);
            }
        }
        // update non-boundary and owned F_i on both sides of computed flux
        // note aF[] does not have stencil width
        if (i > 0 && i >= info->xs)
            aF[i] += flux;  // flux out of i at E
        if (i+1 < info->mx-1 && i+1 < info->xs + info->xm)
            aF[i+1] -= flux;  // flux into i+1 at W
    }
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, PetscReal *u,
                                 Mat J, Mat P, AdCtx *usr) {
    PetscErrorCode ierr;
    const PetscReal eps = usr->eps,
                    hx = 2.0 / (info->mx - 1),
                    halfx = hx / 2.0,
                    scdiag = (2.0 * eps) / hx + 1.0;
    PetscInt        i, col[3];
    PetscReal       x, aE, aW, v[3];

    if (usr->jac_limiter_fcn == &vanleer) {
        SETERRQ(PETSC_COMM_SELF,1,"Jacobian for vanleer limiter is not implemented");
    }

    ierr = MatZeroEntries(P); CHKERRQ(ierr);

    for (i=info->xs; i<info->xs+info->xm; i++) {
        if (i == 0 || i == info->mx-1) {
            v[0] = scdiag;
            ierr = MatSetValues(P,1,&i,1,&i,v,ADD_VALUES); CHKERRQ(ierr);
        } else {
            // diffusive part
            col[0] = i;
            v[0] = (2.0 * eps) / hx;
            col[1] = i-1;
            v[1] = (i-1 > 0) ? - eps / hx : 0.0;
            col[2] = i+1;
            v[2] = (i+1 < info->mx-1) ? - eps / hx : 0.0;
            ierr = MatSetValues(P,1,&i,3,col,v,ADD_VALUES); CHKERRQ(ierr);
            // advective part: from each adjacent face
            x = -1.0 + i * hx;
            aE = wind_a(x + halfx);
            aW = wind_a(x - halfx);
            if (aE >= 0.0) {
                col[0] = i;
                v[0] = aE;
            } else {
                col[0] = i+1;
                v[0] = (i+1 < info->mx-1) ? aE : 0.0;  // check if i+1 is boundary
            }
            if (aW >= 0.0) {
                col[1] = i-1;
                v[1] = (i-1 > 0) ? - aW : 0.0;  // check if i-1 is boundary
            } else {
                col[1] = i;
                v[1] = - aW;
            }
            ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
            if (usr->jac_limiter_fcn == &centered) {
                col[0] = i+1;
                col[1] = i;
                if (aE >= 0.0) {
                    v[0] = (i+1 < info->mx-1) ? aE/2.0 : 0.0;  // check if i+1 is boundary
                    v[1] = - aE/2.0;
                } else {
                    v[0] = (i+1 < info->mx-1) ? - aE/2.0 : 0.0;  // check if i+1 is boundary
                    v[1] = aE/2.0;
                }
                ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
                col[0] = i;
                col[1] = i-1;
                if (aW >= 0.0) {
                    v[0] = - aW/2.0;
                    v[1] = (i-1 > 0) ? aW/2.0 : 0.0;  // check if i-1 is boundary
                } else {
                    v[0] = aW/2.0;
                    v[1] = (i-1 > 0) ? - aW/2.0 : 0.0;  // check if i-1 is boundary
                }
                ierr = MatSetValues(P,1,&i,2,col,v,ADD_VALUES); CHKERRQ(ierr);
            }
        }
    }
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY);CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_movstat.h>

/* compute moving variance by explicitely constructing window and computing variance */
int
slow_movvar(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
            const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double variance = (wsize > 1) ? gsl_stats_variance(window, 1, wsize) : 0.0;

      gsl_vector_set(y, i, variance);
    }

  free(window);

  return GSL_SUCCESS;
}

/* compute moving variance by explicitely constructing window and computing variance */
int
slow_movsd(const gsl_movstat_end_t etype, const gsl_vector * x, gsl_vector * y,
           const int H, const int J)
{
  const size_t n = x->size;
  const int K = H + J + 1;
  double *window = malloc(K * sizeof(double));
  size_t i;

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double sd = (wsize > 1) ? gsl_stats_sd(window, 1, wsize) : 0.0;

      gsl_vector_set(y, i, sd);
    }

  free(window);

  return GSL_SUCCESS;
}

static double
func_var(const size_t n, double x[], void * params)
{
  (void) params;
  if (n > 1)
    return gsl_stats_variance(x, 1, n);
  else
    return 0.0;
}

static double
func_sd(const size_t n, double x[], void * params)
{
  (void) params;
  if (n > 1)
    return gsl_stats_sd(x, 1, n);
  else
    return 0.0;
}

static void
test_variance_proc(const double tol, const size_t n, const size_t H, const size_t J,
                   const gsl_movstat_end_t etype, gsl_rng * rng_p)
{
  gsl_movstat_workspace * w = gsl_movstat_alloc2(H, J);
  gsl_vector * x = gsl_vector_alloc(n);
  gsl_vector * y = gsl_vector_alloc(n);
  gsl_vector * z = gsl_vector_alloc(n);
  gsl_movstat_function F1, F2;
  char buf[2048];

  F1.function = func_var;
  F1.params = NULL;

  F2.function = func_sd;
  F2.params = NULL;

  random_vector(x, rng_p);

  /* test variance */

  /* y = variance(x) with slow brute force method */
  slow_movvar(etype, x, y, H, J);

  /* y = variance(x) with fast method */
  gsl_movstat_variance(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u variance random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = variance(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_variance(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u variance random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = variance(x) with user-defined function */
  gsl_movstat_apply(etype, &F1, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u variance user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* test standard deviation */

  /* y = stddev(x) with slow brute force method */
  slow_movsd(etype, x, y, H, J);

  /* y = stddev(x) with fast method */
  gsl_movstat_sd(etype, x, z, w);

  /* test y = z */
  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u stddev random", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = stddev(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_sd(etype, z, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u stddev random in-place", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  /* z = stddev(x) with user-defined function */
  gsl_movstat_apply(etype, &F2, x, z, w);

  sprintf(buf, "n=%zu H=%zu J=%zu endtype=%u stddev user", n, H, J, etype);
  compare_vectors(tol, z, y, buf);

  gsl_movstat_free(w);
  gsl_vector_free(x);
  gsl_vector_free(y);
  gsl_vector_free(z);
}

static void
test_variance(gsl_rng * rng_p)
{
  const double eps = 1.0e-10;

  test_variance_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 1000, 0, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 1000, 5, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_variance_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_variance_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 1000, 1, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 1000, 5, 1, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_variance_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_variance_proc(eps, 1000, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 1000, 3, 3, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 1000, 0, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 1000, 5, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 2000, 10, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 2000, 5, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 20, 10, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_variance_proc(eps, 20, 50, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


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

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

/* Factorise a general M x N matrix A into
 *
 *   A = Q L
 *
 * where Q is orthogonal (M x M) and L is lower triangular (M x N).
 *
 * Q is stored as a packed set of Householder transformations in the
 * strict upper triangular part of the input matrix.
 *
 * L is stored in the diagonal and lower triangle of the input matrix.
 *
 * The full matrix for Q can be obtained as the product
 *
 *       Q = Q_k .. Q_2 Q_1
 *
 * where k = MIN(M,N) and
 *
 *       Q_i = (I - tau_i * v_i * v_i')
 *
 * and where v_i is a Householder vector
 *
 *       v_i = [A(1,N-k+i), A(2,N-k+i), ... , A(M-k+i,N-k+i), 1, 0, ..., 0]
 *
 * This storage scheme is the same as in LAPACK.  */

/*
gsl_linalg_QL_decomp()
  Perform QL decomposition of a matrix A

Inputs: A   - M-by-N matrix
        tau - (output) Householder coefficients, length N

Notes:
1) The K = MIN(M, N) Householder scalars are stored in tau(N-K+1:N)
on output; the rest of tau is used as temporary workspace
*/

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

  if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else
    {
      const size_t K = GSL_MIN(M, N);
      size_t i;

      for (i = 0; i < K; i++)
        {
          /* compute the Householder transformation to annihilate the (N-K+i)-th
             column of the matrix */

          gsl_vector_view c = gsl_matrix_subcolumn (A, N - i - 1, 0, M - i);
          double * alpha = gsl_matrix_ptr(A, M - i - 1, N - i - 1);
          double tau_j = gsl_linalg_householder_transform2 (alpha, &(c.vector));

          /* apply the transformation to A(1:M-i,1:N-i-2) from the left */
          if (i + 1 < N)
            {
              gsl_vector_view work = gsl_vector_subvector(tau, 0, N - i - 1);
              gsl_matrix_view m = gsl_matrix_submatrix (A, 0, 0, M - i, N - i - 1);
              double tmp = *alpha;

              *alpha = 1.0;
              gsl_linalg_householder_left (tau_j, &(c.vector), &(m.matrix), &work.vector);
              *alpha = tmp;
            }

          gsl_vector_set (tau, N - i - 1, tau_j);
        }

      return GSL_SUCCESS;
    }
}

/* form the orthogonal matrix Q from the packed QL matrix */
int
gsl_linalg_QL_unpack (const gsl_matrix * QL, const gsl_vector * tau, gsl_matrix * Q, gsl_matrix * L)
{
  const size_t M = QL->size1;
  const size_t N = QL->size2;

  if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be M x M", GSL_ENOTSQR);
    }
  else if (L->size1 != M || L->size2 != N)
    {
      GSL_ERROR ("L matrix must be M x N", GSL_ENOTSQR);
    }
  else if (tau->size != N)
    {
      GSL_ERROR ("size of tau must be N", GSL_EBADLEN);
    }
  else
    {
      const size_t K = GSL_MIN(M, N);
      size_t i;

      /* initialize Q to the identity */
      gsl_matrix_set_identity (Q);

      for (i = 0; i < K; ++i)
        {
          gsl_vector_const_view h = gsl_matrix_const_subcolumn (QL, N - K + i, 0, M - K + i + 1);
          gsl_matrix_view m = gsl_matrix_submatrix (Q, 0, 0, M - K + i + 1, M - K + i + 1);
          gsl_vector_view work = gsl_matrix_subcolumn(L, 0, 0, M - K + i + 1);
          double ti = gsl_vector_get (tau, N - K + i);
          double * ptr = gsl_matrix_ptr((gsl_matrix *) QL, M - K + i, N - K + i);
          double tmp = *ptr;

          *ptr = 1.0;
          gsl_linalg_householder_left (ti, &h.vector, &m.matrix, &work.vector);
          *ptr = tmp;
        }

      /* form the left triangular matrix L from a packed QL matrix */
      gsl_matrix_set_zero(L);

      if (M >= N)
        {
          gsl_matrix_const_view src = gsl_matrix_const_submatrix(QL, M - N, 0, N, N);
          gsl_matrix_view dest = gsl_matrix_submatrix(L, M - N, 0, N, N);
          gsl_matrix_tricpy(CblasLower, CblasNonUnit, &dest.matrix, &src.matrix);
        }
      else
        {
          gsl_matrix_const_view src1 = gsl_matrix_const_submatrix(QL, 0, 0, M, N - M);
          gsl_matrix_view dest1 = gsl_matrix_submatrix(L, 0, 0, M, N - M);

          gsl_matrix_const_view src2 = gsl_matrix_const_submatrix(QL, 0, N - M, M, M);
          gsl_matrix_view dest2 = gsl_matrix_submatrix(L, 0, N - M, M, M);

          gsl_matrix_memcpy(&dest1.matrix, &src1.matrix);
          gsl_matrix_tricpy(CblasLower, CblasNonUnit, &dest2.matrix, &src2.matrix);
        }

      return GSL_SUCCESS;
    }
}


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

#include <config.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_cblas.h>

static double cholesky_band_norm1(const gsl_matrix * A);
static int cholesky_band_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*
gsl_linalg_cholesky_band_decomp()
  Cholesky decomposition of a square symmetric positive definite banded
matrix

Inputs: A - matrix in banded format, N-by-ndiag where N is the size of
            the matrix and ndiag is the number of nonzero diagonals.

Notes:
1) The main diagonal of the Cholesky factor is stored in the first column
of A; the first subdiagonal in the second column and so on.

2) If ndiag > 1, the 1-norm of A is stored in A(N,ndiag) on output

3) At each diagonal element, the matrix is factored as

A(j:end,j:end) = [ A11 A21^T ] = [ alpha  0 ] [ alpha v^T ]
                 [ A21 A22   ]   [   v    L ] [   0   L^T ]

where:

alpha = sqrt(A(j,j))
v = A(j+1:end, j) / alpha
A22 = L L^T + v v^T

So we start at A(1,1) and work right. Pseudo-code is:

loop j = 1, ..., N
  alpha = sqrt(A(j,j))
  A(j+1:end, j) := A(j+1:end, j) / alpha     (DSCAL)
  A(j+1:end, j+1:end) -= v v^T               (DSYR)

Due to the banded structure, v has at most p non-zero elements, where
p is the lower bandwidth
*/

int
gsl_linalg_cholesky_band_decomp(gsl_matrix * A)
{
  const size_t N = A->size1;     /* size of matrix */
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */

  if (ndiag > N)
    {
      GSL_ERROR ("invalid matrix dimensions", GSL_EBADLEN);
    }
  else
    {
      const size_t p = ndiag - 1; /* lower bandwidth */
      const int kld = (int) GSL_MAX(1, p);
      size_t j;

      if (ndiag > 1)
        {
          /*
           * calculate 1-norm of A and store in lower right of matrix, which is not accessed
           * by rest of routine. gsl_linalg_cholesky_band_rcond() will use this later. If
           * A is diagonal, there is no empty slot to store the 1-norm, so the rcond routine
           * will have to reconstruct it from the Cholesky factor.
           */
          double Anorm = cholesky_band_norm1(A);
          gsl_matrix_set(A, N - 1, p, Anorm);
        }

      for (j = 0; j < N; ++j)
        {
          double ajj = gsl_matrix_get(A, j, 0);
          size_t lenv;

          if (ajj <= 0.0)
            {
              GSL_ERROR("matrix is not positive definite", GSL_EDOM);
            }

          ajj = sqrt(ajj);
          gsl_matrix_set(A, j, 0, ajj);

          /* number of elements in v, which will normally be p, unless we
           * are in lower right corner of matrix */
          lenv = GSL_MIN(p, N - j - 1);

          if (lenv > 0)
            {
              gsl_vector_view v = gsl_matrix_subrow(A, j, 1, lenv);
              gsl_matrix_view m = gsl_matrix_submatrix(A, j + 1, 0, lenv, lenv);

              gsl_blas_dscal(1.0 / ajj, &v.vector);

              m.matrix.tda = kld;
              gsl_blas_dsyr(CblasUpper, -1.0, &v.vector, &m.matrix);
            }
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_band_solve (const gsl_matrix * LLT,
                                const gsl_vector * b,
                                gsl_vector * x)
{
  if (LLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LLT->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* copy x <- b */
      gsl_vector_memcpy (x, b);

      status = gsl_linalg_cholesky_band_svx(LLT, x);

      return status;
    }
}

int
gsl_linalg_cholesky_band_svx (const gsl_matrix * LLT, gsl_vector * x)
{
  if (LLT->size1 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* solve for c using forward-substitution, L c = b */
      cblas_dtbsv(CblasColMajor, CblasLower, CblasNoTrans, CblasNonUnit,
                  (int) LLT->size1, (int) (LLT->size2 - 1), LLT->data, LLT->tda,
                  x->data, x->stride);

      /* perform back-substitution, L^T x = c */
      cblas_dtbsv(CblasColMajor, CblasLower, CblasTrans, CblasNonUnit,
                  (int) LLT->size1, (int) (LLT->size2 - 1), LLT->data, LLT->tda,
                  x->data, x->stride);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_band_solvem (const gsl_matrix * LLT,
                                 const gsl_matrix * B,
                                 gsl_matrix * X)
{
  if (LLT->size1 != B->size1)
    {
      GSL_ERROR ("LLT size1 must match B size1", GSL_EBADLEN);
    }
  else if (LLT->size1 != X->size1)
    {
      GSL_ERROR ("LLT size1 must match solution size1", GSL_EBADLEN);
    }
  else if (B->size2 != X->size2)
    {
      GSL_ERROR ("B size2 must match X size2", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* copy X <- B */
      gsl_matrix_memcpy (X, B);

      status = gsl_linalg_cholesky_band_svxm(LLT, X);

      return status;
    }
}

int
gsl_linalg_cholesky_band_svxm (const gsl_matrix * LLT, gsl_matrix * X)
{
  if (LLT->size1 != X->size1)
    {
      GSL_ERROR ("LLT size1 must match solution size1", GSL_EBADLEN);
    }
  else
    {
      int status;
      const size_t nrhs = X->size2;
      size_t j;

      for (j = 0; j < nrhs; ++j)
        {
          gsl_vector_view xj = gsl_matrix_column(X, j);

          status = gsl_linalg_cholesky_band_svx (LLT, &xj.vector);
          if (status)
            return status;
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_band_invert (const gsl_matrix * LLT, gsl_matrix * Ainv)
{
  if (Ainv->size1 != Ainv->size2)
    {
      GSL_ERROR("Ainv must be square", GSL_ENOTSQR);
    }
  else if (LLT->size1 != Ainv->size1)
    {
      GSL_ERROR("cholesky matrix has different dimensions from Ainv", GSL_EBADLEN);
    }
  else
    {
      int status;

      /* unpack Cholesky factor into lower triangle of Ainv */
      status = gsl_linalg_cholesky_band_unpack(LLT, Ainv);
      if (status)
        return status;

      /* call the standard Cholesky inversion routine */
      status = gsl_linalg_cholesky_invert(Ainv);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_band_unpack (const gsl_matrix * LLT, gsl_matrix * L)
{
  const size_t N = LLT->size1;

  if (N != L->size1)
    {
      GSL_ERROR("L matrix does not match LLT dimensions", GSL_EBADLEN);
    }
  else if (L->size1 != L->size2)
    {
      GSL_ERROR("L matrix is not square", GSL_ENOTSQR);
    }
  else
    {
      const size_t p = LLT->size2 - 1; /* lower bandwidth */
      size_t i;

      for (i = 0; i < p + 1; ++i)
        {
          gsl_vector_const_view v = gsl_matrix_const_subcolumn(LLT, i, 0, N - i);
          gsl_vector_view w = gsl_matrix_subdiagonal(L, i);

          gsl_vector_memcpy(&w.vector, &v.vector);
        }

      /* zero out remaining subdiagonals */
      for (i = p + 1; i < N; ++i)
        {
          gsl_vector_view w = gsl_matrix_subdiagonal(L, i);
          gsl_vector_set_zero(&w.vector);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_band_rcond (const gsl_matrix * LLT, double * rcond, gsl_vector * work)
{
  const size_t N = LLT->size1;

  if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      int status;
      const size_t ndiag = LLT->size2;
      double Anorm;    /* ||A||_1 */
      double Ainvnorm; /* ||A^{-1}||_1 */

      if (ndiag == 1)
        {
          /* diagonal matrix, compute 1-norm since it has not been stored */
          gsl_vector_const_view v = gsl_matrix_const_column(LLT, 0);
          Anorm = gsl_vector_max(&v.vector);
          Anorm = Anorm * Anorm;
        }
      else
        {
          /* 1-norm is stored in A(N, ndiag) by gsl_linalg_cholesky_band_decomp() */
          Anorm = gsl_matrix_get(LLT, N - 1, ndiag - 1);
        }

      *rcond = 0.0;

      /* return if matrix is singular */
      if (Anorm == 0.0)
        return GSL_SUCCESS;

      status = gsl_linalg_invnorm1(N, cholesky_band_Ainv, (void *) LLT, &Ainvnorm, work);
      if (status)
        return status;

      if (Ainvnorm != 0.0)
        *rcond = (1.0 / Anorm) / Ainvnorm;

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_cholesky_band_scale()
  This function computes scale factors diag(S), such that

diag(S) A diag(S)

has a condition number within a factor N of the matrix
with the smallest condition number over all possible
diagonal scalings. See Corollary 7.6 of:

N. J. Higham, Accuracy and Stability of Numerical Algorithms (2nd Edition),
SIAM, 2002.

Inputs: A - symmetric positive definite matrix
        S - (output) scale factors, S_i = 1 / sqrt(A_ii)
*/

int
gsl_linalg_cholesky_band_scale(const gsl_matrix * A, gsl_vector * S)
{
  const size_t N = A->size1;     /* size of matrix */
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */

  if (ndiag > N)
    {
      GSL_ERROR ("invalid matrix dimensions", GSL_EBADLEN);
    }
  else if (N != S->size)
    {
      GSL_ERROR("S must have length N", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* compute S_i = 1/sqrt(A_{ii}) */
      for (i = 0; i < N; ++i)
        {
          double Aii = gsl_matrix_get(A, i, 0);

          if (Aii <= 0.0)
            gsl_vector_set(S, i, 1.0); /* matrix not positive definite */
          else
            gsl_vector_set(S, i, 1.0 / sqrt(Aii));
        }

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_cholesky_band_scale_apply()
  This function applies scale transformation to A:

A <- diag(S) A diag(S)

Inputs: A - (input/output)
            on input, symmetric positive definite matrix in banded format
            on output, diag(S) * A * diag(S) in banded format
        S - (input) scale factors
*/

int
gsl_linalg_cholesky_band_scale_apply(gsl_matrix * A, const gsl_vector * S)
{
  const size_t N = A->size1;     /* size of matrix */
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */

  if (ndiag > N)
    {
      GSL_ERROR ("invalid matrix dimensions", GSL_EBADLEN);
    }
  else if (N != S->size)
    {
      GSL_ERROR("S must have length N", GSL_EBADLEN);
    }
  else
    {
      size_t i, j;

      for (j = 0; j < N; ++j)
        {
          double sj = gsl_vector_get(S, j);

          for (i = j; i < GSL_MIN(N, j + ndiag); ++i)
            {
              double si = gsl_vector_get(S, i);
              double * ptr = gsl_matrix_ptr(A, j, i - j);
              *ptr *= sj * si;
            }
        }

      return GSL_SUCCESS;
    }
}

/* compute 1-norm of symmetric banded matrix */
static double
cholesky_band_norm1(const gsl_matrix * A)
{
  const size_t N = A->size1;
  const size_t ndiag = A->size2; /* number of diagonals in band, including main diagonal */
  double value;

  if (ndiag == 1)
    {
      /* diagonal matrix */
      gsl_vector_const_view v = gsl_matrix_const_column(A, 0);
      CBLAS_INDEX_t idx = gsl_blas_idamax(&v.vector);
      value = gsl_vector_get(&v.vector, idx);
    }
  else
    {
      size_t j;

      value = 0.0;
      for (j = 0; j < N; ++j)
        {
          size_t ncol = GSL_MIN(ndiag, N - j); /* number of elements in column j below and including main diagonal */
          gsl_vector_const_view v = gsl_matrix_const_subrow(A, j, 0, ncol);
          double sum = gsl_blas_dasum(&v.vector);
          size_t k, l;

          /* sum now contains the absolute sum of elements below and including main diagonal for column j; we
           * have to add the symmetric elements above the diagonal */
          k = j;
          l = 1;
          while (k > 0 && l < ndiag)
            {
              double Akl = gsl_matrix_get(A, --k, l++);
              sum += fabs(Akl);
            }

          value = GSL_MAX(value, sum);
        }
    }

  return value;
}

/* x := A^{-1} x = A^{-t} x, A = L L^T */
static int
cholesky_band_Ainv(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * LLT = (gsl_matrix * ) params;

  (void) TransA; /* unused parameter warning */

  /* compute x := L^{-1} x */
  cblas_dtbsv(CblasColMajor, CblasLower, CblasNoTrans, CblasNonUnit,
              (int) LLT->size1, (int) (LLT->size2 - 1), LLT->data, LLT->tda,
              x->data, x->stride);

  /* compute x := L^{-T} x */
  cblas_dtbsv(CblasColMajor, CblasLower, CblasTrans, CblasNonUnit,
              (int) LLT->size1, (int) (LLT->size2 - 1), LLT->data, LLT->tda,
              x->data, x->stride);

  return GSL_SUCCESS;
}


/*
 # This file is part of the Astrometry.net suite.
 # Licensed under a 3-clause BSD style license - see LICENSE
 */

#include <assert.h>
#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>
#include <stdarg.h>

#include "os-features.h"
#include "gslutils.h"
#include "errors.h"

static void errhandler(const char * reason,
                       const char * file,
                       int line,
                       int gsl_errno) {
    ERROR("GSL error: \"%s\" in %s:%i (gsl errno %i = %s)",
          reason, file, line,
          gsl_errno,
          gsl_strerror(gsl_errno));
}

void gslutils_use_error_system() {
    gsl_set_error_handler(errhandler);
}

int gslutils_invert_3x3(const double* A, double* B) {
    gsl_matrix* LU;
    gsl_permutation *p;
    gsl_matrix_view mB;
    int rtn = 0;
    int signum;

    p = gsl_permutation_alloc(3);
    gsl_matrix_const_view mA = gsl_matrix_const_view_array(A, 3, 3);
    mB = gsl_matrix_view_array(B, 3, 3);
    LU = gsl_matrix_alloc(3, 3);

    gsl_matrix_memcpy(LU, &mA.matrix);
    if (gsl_linalg_LU_decomp(LU, p, &signum) ||
        gsl_linalg_LU_invert(LU, p, &mB.matrix)) {
        ERROR("gsl_linalg_LU_decomp() or _invert() failed.");
        rtn = -1;
    }

    gsl_permutation_free(p);
    gsl_matrix_free(LU);
    return rtn;
}

void gslutils_matrix_multiply(gsl_matrix* C,
                              const gsl_matrix* A, const gsl_matrix* B) {
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, B, 0.0, C);
}

int gslutils_solve_leastsquares_v(gsl_matrix* A, int NB, ...) {
    int i, res;
    gsl_vector**  B = malloc(NB * sizeof(gsl_vector*));
    // Whoa, three-star programming!
    gsl_vector*** X = malloc(NB * sizeof(gsl_vector**));
    gsl_vector*** R = malloc(NB * sizeof(gsl_vector**));

    gsl_vector** Xtmp = malloc(NB * sizeof(gsl_vector*));
    gsl_vector** Rtmp = malloc(NB * sizeof(gsl_vector*));

    va_list va;
    va_start(va, NB);
    for (i=0; i<NB; i++) {
        B[i] = va_arg(va, gsl_vector*);
        X[i] = va_arg(va, gsl_vector**);
        R[i] = va_arg(va, gsl_vector**);
    }
    va_end(va);

    res = gslutils_solve_leastsquares(A, B, Xtmp, Rtmp, NB);
    for (i=0; i<NB; i++) {
        if (X[i])
            *(X[i]) = Xtmp[i];
        else
            gsl_vector_free(Xtmp[i]);
        if (R[i])
            *(R[i]) = Rtmp[i];
        else
            gsl_vector_free(Rtmp[i]);
    }
    free(Xtmp);
    free(Rtmp);
    free(X);
    free(R);
    free(B);
    return res;
}

int gslutils_solve_leastsquares(gsl_matrix* A, gsl_vector** B,
                                gsl_vector** X, gsl_vector** resids,
                                int NB) {
    int i;
    gsl_vector *tau, *resid = NULL;
    Unused int ret;
    int M, N;

    M = A->size1;
    N = A->size2;

    for (i=0; i<NB; i++) {
        assert(B[i]);
        assert(B[i]->size == M);
    }

    tau = gsl_vector_alloc(MIN(M, N));
    assert(tau);

    ret = gsl_linalg_QR_decomp(A, tau);
    assert(ret == 0);
    // A,tau now contains a packed version of Q,R.

    for (i=0; i<NB; i++) {
        if (!resid) {
            resid = gsl_vector_alloc(M);
            assert(resid);
        }
        X[i] = gsl_vector_alloc(N);
        assert(X[i]);
        ret = gsl_linalg_QR_lssolve(A, tau, B[i], X[i], resid);
        assert(ret == 0);
        if (resids) {
            resids[i] = resid;
            resid = NULL;
        }
    }

    gsl_vector_free(tau);
    if (resid)
        gsl_vector_free(resid);

    return 0;
}


/**
  * Copyright 2019 José Manuel Abuín Mosquera <josemanuel.abuin@usc.es>
  *
  * This file is part of Matrix Market Suite.
  *
  * Matrix Market Suite 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.
  *
  * Matrix Market Suite 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 Matrix Market Suite. If not, see <http://www.gnu.org/licenses/>.
  */

#include <cblas.h>
#include "ConjugateGradientSolver.h"

int ConjugateGradientSolver(unsigned long *II, unsigned long *J, double *A, unsigned long M, unsigned long N, unsigned long long nz, double *b, unsigned long M_Vector, unsigned long N_Vector, unsigned long long nz_vector, double *x, int iterationNumber) {

	//A*x=b

        double *Ap=(double *) malloc(nz_vector * sizeof(double));
        double *r=(double *) malloc(nz_vector * sizeof(double));
        double *p=(double *) malloc(nz_vector * sizeof(double));
	//double *x=(double *) calloc(nz_vector,sizeof(double));

	//r = b-A*x
	//If we take x=0 the init multiplication is avoided and r=b

	memcpy(r, b, N*sizeof(double));

	//p=r

	memcpy(p, r, N*sizeof(double));

	//rsold = r*r
	double rsold = cblas_ddot(N,r,1,r,1);

	int stop = 0;

	double alphaCG = 0.0;

	double rsnew = 0.0;
	unsigned long k = 0;

	unsigned long maxIterations = M*2;

	if(iterationNumber != 0 ){
		maxIterations = iterationNumber;
	}

	//int i  = 0;

	while(!stop){

		//Ap=A*p
		// for(i=0; i<M; i++){
		//	Ap[i] = 0.0;
		// }
		cblas_dgemv(CblasRowMajor, CblasNoTrans, M,N , 1.0, A, N, p, 1, 0.0, Ap, 1);

		//alphaCG=rsold/(p'*Ap)
		alphaCG = rsold/cblas_ddot(N,p,1,Ap,1);

		//x=x+alphaCG*p
		cblas_daxpy(N,alphaCG,p,1,x,1);

		//r=r-alphaCG*Ap
		cblas_daxpy(N,-alphaCG,Ap,1,r,1);

		//rsnew = r'*r
		rsnew = cblas_ddot(N,r,1,r,1);

		if((sqrt(rsnew)<=EPSILON)||(k == maxIterations)){
			stop = 1;
		}

		//p=r+rsnew/rsold*p
		cblas_dscal(N, rsnew/rsold, p, 1);
		cblas_daxpy(N,1.0,r,1,p,1);

		rsold = rsnew;

		k++;
	}

	//memcpy(b, x, N*sizeof(double));

        free(Ap);
        free(r);
        free(p);
	//free(x);

	fprintf(stderr, "[%s] Number of iterations %lu\n",__func__,k);

	return 1;
}


/*
 * Gsl_blas_dgemv: test gsl_blas_dgemv (matrix . vector)
 *
 * Copyright (c) 2012 Jérémie Decock
 *
 * Required: GSL library (libgsl0-dev)
 * Usage: gcc gsl_blas_dgemv.c -lgsl -lgslcblas -lm
 *    or: gcc gsl_blas_dgemv.c $(pkg-config --libs gsl)
 */

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

void main() {

    gsl_vector * v = gsl_vector_alloc(3);
    gsl_vector * r = gsl_vector_alloc(3);
    gsl_matrix * m = gsl_matrix_alloc(3, 3);

    gsl_vector_set_all(v, 2.0);
    gsl_matrix_set_all(m, 3.0);

    gsl_blas_dgemv(CblasNoTrans,
                   1.0, m, v,
                   0.0, r);

    gsl_vector_fprintf(stdout, r, "%f");

    gsl_vector_free(v);
    gsl_vector_free(r);
    gsl_matrix_free(m);

}


/* SO3.c */

// SO(3) Lie group implemented with GNU GSL
// cf. wikipedia, "Rotation_group_SO(3)#Infinitesimal_rotations"

// EY note : 20160519 Bizarrely, matrix multiplication isn't implemented in a straightforward manner with the gsl_matrix class; see blas (which I don't know what it is yet) and dgemm for the libraries, and SO3blas.c in this repo for that implementation

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

// Function prototype
// gsl_matrix commute(gsl_matrix, gsl_matrix);

int main(void)
{
  gsl_matrix *L_x = gsl_matrix_calloc(3,3);
  gsl_matrix *L_y = gsl_matrix_calloc(3,3);
  gsl_matrix *L_z = gsl_matrix_calloc(3,3);

  gsl_matrix_set(L_x,1,2,-1);
  gsl_matrix_set(L_x,2,1,1);

  gsl_matrix_set(L_y,0,2,1);
  gsl_matrix_set(L_y,2,0,-1);

  gsl_matrix_set(L_z,0,1,-1);
  gsl_matrix_set(L_z,1,0,1);

  for (int i=0;i<3;i++)
    for (int j=0;j<3;j++)
      printf("m(%d,%d) = %g\n", i, j,
	     gsl_matrix_get(L_x,i,j));

  for (int i=0;i<3;i++)
    for (int j=0;j<3;j++)
      printf("m(%d,%d) = %g\n", i, j,
	     gsl_matrix_get(L_y,i,j));

 for (int i=0;i<3;i++)
   for (int j=0;j<3;j++)
      printf("m(%d,%d) = %g\n", i, j,
	     gsl_matrix_get(L_z,i,j));

  gsl_matrix_free(L_x);
  gsl_matrix_free(L_y);
  gsl_matrix_free(L_z);

  return 0;
}

//gsl_matrix commute(gsl_matrix x, gsl_matrix y)
//{

//}


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include "matrix.h"

void Matrix_Multiply(Matrix a, Matrix b, Matrix c) {
    /* for dimentions:
     * a[m*k],b[k*n],c[m*n]
     * We use:
     * RowMajor, NoTrans, NoTrans, m, n, k, 1, A, k, B, n, 0, C, n);
     */

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, a.rows, b.cols, a.cols,
                1, a.values, a.cols, b.values, b.cols, 0, c.values, b.cols);
}

void Matrix_Print(Matrix m) {
    printf("=====\n");
    printf("rows: %lld, columns: %lld\n", m.rows, m.cols);
    for (int i = 0; i < m.rows; i++) {
        for (int j = 0; j < m.cols; j++) {
            printf("%lf ", m.values[i * m.cols + j]);
        }
        printf("\n");
    }
}

void Matrix_Add(Matrix a, Matrix b, Matrix c) {
    for (int i = 0; i < a.rows * a.cols; i++) {
        c.values[i] = a.values[i] + b.values[i];
    }
}

void Matrix_Scale(Matrix a, double n) {
    cblas_dscal(a.rows * a.cols, n, a.values, 1);
}

void Matrix_Test(void) {
    double *A, *B, *C;
    int m, n, k, i;
    double alpha, beta;
    m = 2000, k = 200, n = 1000;
    printf(
            " Initializing data for matrix multiplication C=A*B for matrix \n"
                    " A(%ix%i) and matrix B(%ix%i)\n",
            m, k, k, n);
    alpha = 1.0;
    beta = 0.0;

    A = (double *) malloc(m * k * sizeof(double));
    B = (double *) malloc(k * n * sizeof(double));
    C = (double *) malloc(m * n * sizeof(double));
    if (A == NULL || B == NULL || C == NULL) {
        printf("\n ERROR: Can't allocate memory for matrices. Aborting... \n\n");
        free(A);
        free(B);
        free(C);
        return;
    }

    printf(" Intializing matrix data \n");
    for (i = 0; i < (m * k); i++) {
        A[i] = (double) (i + 1);
    }

    for (i = 0; i < (k * n); i++) {
        B[i] = (double) (-i - 1);
    }

    for (i = 0; i < (m * n); i++) {
        C[i] = 0.0;
    }

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, alpha, A, k,
                B, n, beta, C, n);
    printf("\n Computations completed.\n");
    free(A);
    free(B);
    free(C);
}


/* fft/hc_main.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 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 <stdlib.h>
#include <math.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_fft_halfcomplex.h>

#include "hc_pass.h"

int
FUNCTION(gsl_fft_halfcomplex,backward) (BASE data[], const size_t stride,
                                        const size_t n,
                                        const TYPE(gsl_fft_halfcomplex_wavetable) * wavetable,
                                        TYPE(gsl_fft_real_workspace) * work)
{
  int status = FUNCTION(gsl_fft_halfcomplex,transform) (data, stride, n, wavetable, work) ;
  return status ;
}

int
FUNCTION(gsl_fft_halfcomplex,inverse) (BASE data[], const size_t stride,
                                       const size_t n,
                                       const TYPE(gsl_fft_halfcomplex_wavetable) * wavetable,
                                       TYPE(gsl_fft_real_workspace) * work)
{
  int status = FUNCTION(gsl_fft_halfcomplex,transform) (data, stride, n, wavetable, work);

  if (status)
    {
      return status;
    }

  /* normalize inverse fft with 1/n */

  {
    const double norm = 1.0 / n;
    size_t i;
    for (i = 0; i < n; i++)
      {
        data[stride*i] *= norm;
      }
  }
  return status;
}

int
FUNCTION(gsl_fft_halfcomplex,transform) (BASE data[], const size_t stride, const size_t n,
                                         const TYPE(gsl_fft_halfcomplex_wavetable) * wavetable,
                                         TYPE(gsl_fft_real_workspace) * work)
{
  BASE * const scratch = work->scratch;

  BASE * in;
  BASE * out;
  size_t istride, ostride ;

  size_t factor, product, q;
  size_t i;
  size_t nf;
  int state;
  int product_1;
  int tskip;
  TYPE(gsl_complex) *twiddle1, *twiddle2, *twiddle3, *twiddle4;

  if (n == 0)
    {
      GSL_ERROR ("length n must be positive integer", GSL_EDOM);
    }

  if (n == 1)
    {                           /* FFT of one data point is the identity */
      return 0;
    }

  if (n != wavetable->n)
    {
      GSL_ERROR ("wavetable does not match length of data", GSL_EINVAL);
    }

  if (n != work->n)
    {
      GSL_ERROR ("workspace does not match length of data", GSL_EINVAL);
    }

  nf = wavetable->nf;
  product = 1;
  state = 0;

  for (i = 0; i < nf; i++)
    {
      factor = wavetable->factor[i];
      product_1 = product;
      product *= factor;
      q = n / product;

      tskip = (q + 1) / 2 - 1;

      if (state == 0)
        {
          in = data;
          istride = stride;
          out = scratch;
          ostride = 1;
          state = 1;
        }
      else
        {
          in = scratch;
          istride = 1;
          out = data;
          ostride = stride;
          state = 0;
        }

      if (factor == 2)
        {
          twiddle1 = wavetable->twiddle[i];
          FUNCTION(fft_halfcomplex,pass_2) (in, istride, out, ostride,
                                            product, n, twiddle1);
        }
      else if (factor == 3)
        {
          twiddle1 = wavetable->twiddle[i];
          twiddle2 = twiddle1 + tskip;
          FUNCTION(fft_halfcomplex,pass_3) (in, istride, out, ostride,
                                            product, n, twiddle1, twiddle2);
        }
      else if (factor == 4)
        {
          twiddle1 = wavetable->twiddle[i];
          twiddle2 = twiddle1 + tskip;
          twiddle3 = twiddle2 + tskip;
          FUNCTION(fft_halfcomplex,pass_4) (in, istride, out, ostride,
                                            product, n, twiddle1, twiddle2,
                                            twiddle3);
        }
      else if (factor == 5)
        {
          twiddle1 = wavetable->twiddle[i];
          twiddle2 = twiddle1 + tskip;
          twiddle3 = twiddle2 + tskip;
          twiddle4 = twiddle3 + tskip;
          FUNCTION(fft_halfcomplex,pass_5) (in, istride, out, ostride,
                                            product, n, twiddle1, twiddle2,
                                            twiddle3, twiddle4);
        }
      else
        {
          twiddle1 = wavetable->twiddle[i];
          FUNCTION(fft_halfcomplex,pass_n) (in, istride, out, ostride,
                                            factor, product, n, twiddle1);
        }
    }

  if (state == 1)               /* copy results back from scratch to data */
    {
      for (i = 0; i < n; i++)
        {
          data[stride*i] = scratch[i] ;
        }
    }

  return 0;

}


#include <stdbool.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include "numstates.h"

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

    EnergyCache *Ecache = init_EnergyCache(na, nb, nc, num_bands, G_order, G_neg, Efn, use_cache);

    double E = 0.0;
    double count = NumStates(E, Ecache);
    double eps = 1e-9;
    double expected_E0 = 0.0;
    if (fabs(count - expected_E0) > eps) {
        printf("Incorrect occupation; got %f, expected %f\n", count, expected_E0);
        return 1;
    }

    E = 6.0;
    count = NumStates(E, Ecache);
    double expected_E6 = 0.5;
    if (fabs(count - expected_E6) > eps) {
        printf("Incorrect occupation; got %f, expected %f\n", count, expected_E6);
        return 1;
    }

    E = 12.0;
    count = NumStates(E, Ecache);
    double expected_E12 = 1.0;
    if (fabs(count - expected_E12) > eps) {
        printf("Incorrect occupation; got %f, expected %f\n", count, expected_E12);
        return 1;
    }

    E = 26.0;
    count = NumStates(E, Ecache);
    double expected_E24 = 2.0;
    if (fabs(count - expected_E24) > eps) {
        printf("Incorrect occupation; got %f, expected %f\n", count, expected_E24);
        return 1;
    }

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


/* Author:  G. Jungman */

#ifndef __GSL_SF_H__
#define __GSL_SF_H__

#include <gsl/gsl_sf_result.h>

#include <gsl/gsl_sf_airy.h>
#include <gsl/gsl_sf_bessel.h>
#include <gsl/gsl_sf_clausen.h>
#include <gsl/gsl_sf_coupling.h>
#include <gsl/gsl_sf_coulomb.h>
#include <gsl/gsl_sf_dawson.h>
#include <gsl/gsl_sf_debye.h>
#include <gsl/gsl_sf_dilog.h>
#include <gsl/gsl_sf_elementary.h>
#include <gsl/gsl_sf_ellint.h>
#include <gsl/gsl_sf_elljac.h>
#include <gsl/gsl_sf_erf.h>
#include <gsl/gsl_sf_exp.h>
#include <gsl/gsl_sf_expint.h>
#include <gsl/gsl_sf_fermi_dirac.h>
#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_sf_gegenbauer.h>
#include <gsl/gsl_sf_hyperg.h>
#include <gsl/gsl_sf_laguerre.h>
#include <gsl/gsl_sf_lambert.h>
#include <gsl/gsl_sf_legendre.h>
#include <gsl/gsl_sf_log.h>
#include <gsl/gsl_sf_pow_int.h>
#include <gsl/gsl_sf_psi.h>
#include <gsl/gsl_sf_synchrotron.h>
#include <gsl/gsl_sf_transport.h>
#include <gsl/gsl_sf_trig.h>
#include <gsl/gsl_sf_zeta.h>

#endif /* __GSL_SF_H__ */


#include <limits.h>

#include <asf.h>
#include <asf_export.h>
#include <asf_raster.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_statistics.h>

/* Get sample size in bytes of the data types represented by the
   meta_parameters_t.  */
size_t
get_sample_size (meta_parameters *metadata)
{
  size_t sample_size;
  switch ( metadata->general->data_type ) {
  case BYTE:
    sample_size = sizeof (int8_t);
    break;
  case INTEGER16:
    sample_size = sizeof (int16_t);
    break;
  case INTEGER32:
    sample_size = sizeof (int32_t);
    break;
  case REAL32:
    sample_size = sizeof (float);
    break;
  case REAL64:
    sample_size = sizeof (double);
    break;
  default:
    /* Other types aren't handled.  */
    asfPrintError ("This program can handle byte, int16, int32, real32, and real64 data types.\n");
    sample_size = 0; // not reached
  }

  return sample_size;
}

/* Get the image data in data file image_data_file_name, using
   metadata metadata.  A pointer to new memory containing the image
   data is returned.  */
void *
get_image_data (meta_parameters *metadata, const char *image_data_file)
{
  size_t sample_size = get_sample_size (metadata);
  size_t pixel_count;
  void *data;
  size_t read_count;
  int return_code;

  /* Read the image data itself.  */
  FILE *ifp = fopen (image_data_file, "r");
  if ( ifp == NULL )
    asfPrintError("Failed to open %s: %s", image_data_file, strerror(errno));

  /* Total number of samples in image.  */
  pixel_count = metadata->general->line_count * metadata->general->sample_count;
  data = MALLOC (pixel_count * sample_size);
  read_count = fread (data, sample_size, pixel_count, ifp);
  if ( read_count != pixel_count ) {
    if ( feof (ifp) ) {
      asfPrintError("Read wrong amount of data from %s", image_data_file);
    }
    else if ( ferror (ifp) ) {
      asfPrintError("Read of file %s failed: %s", image_data_file,
                    strerror(errno));
    }
    else {
      /* Shouldn't get here.  */
      asfPrintError("Unknown error reading %s\n", image_data_file);
    }
    exit (EXIT_FAILURE);
  }

  return_code = fclose (ifp);
  asfRequire ((return_code==0), "Error closing file %s\n", image_data_file);

  return data;
}

/* Return the average of all the kernel_size * kernel_size elements
   centered around element i, j.  The i and j arguments must be within
   the bounds of img, and the kernel_size must be odd.  The image is
   reflected at the edges and corners for the purpose of determining
   this average.  */
unsigned char
averaging_kernel (gsl_matrix_uchar *img, int kernel_size, size_t i, size_t j)
{
  int i_idx, j_idx;
  int i_min = i - kernel_size / 2;
  int i_max = i + kernel_size / 2;
  int j_min = j - kernel_size / 2;
  int j_max = j + kernel_size / 2;
  int sum = 0;
  int average; /* Truncated average.  */

  asfRequire(kernel_size%2 != 0, "Odd-sized kernels only.\n");

  for ( i_idx = i_min ; i_idx < i_max ; i_idx++ ) {
    /* The i index to use, adjusted in case we are off the edge of the
       image.  This choice (and the corresponding choice for j)
       implement a kernel that pretends that a mirror image of the
       image exists at the image edges (and corners). */
    int itu = i_idx;
    if ( itu < 0 )
      itu = -itu - 1;
    else if ( itu >= img->size1 )
      itu = img->size1 - (itu - img->size1) - 1;
    for ( j_idx = j_min ; j_idx < j_max ; j_idx++ ) {
      /* See the comment for variable itu above.  */
      int jtu = j_idx;
      if ( jtu < 0 )
        jtu = -jtu - 1;
      else if ( jtu >= img->size2 )
        jtu = img->size2 - (jtu - img->size2) - 1;
      sum += gsl_matrix_uchar_get (img, itu, jtu);
    }
  }

  average = sum / pow (kernel_size, 2); /* Truncated average.  */
  /* Since we are averaging unsigned char values, this should always
     be true.  */
  asfRequire(average<=UCHAR_MAX,
             "The average value of the image is above its maximum value.\n");

  return average;
}

/* Average together tiles of kernel_size pixels in the *width by
   *height image at pixels, reducing its size by a factor of about
   kernel size in each dimension.  The new image replaces the old in
   pixels (extra memory is freed) and a pointer to the possibly
   relocated pixel data is returned.  The new image width and height
   replace the input width and height arguments.  */
unsigned char *average_unsigned_char_pixels (unsigned char *pixels,
                                             unsigned long *width,
                                             unsigned long *height,
					     int kernel_size)
{
  /* Input width and height.  */
  size_t iwidth = *width, iheight = *height;
  gsl_matrix_uchar *iimg;
  size_t ii, jj;
  size_t owidth;
  size_t oheight;
  gsl_matrix_uchar *oimg;
  unsigned char *reallocated_pixels;

  asfRequire (kernel_size%2 != 0, "Odd-sized kernels only.\n");

  /* Make a matrix form of the input image for easy indexing.  */
  iimg = gsl_matrix_uchar_alloc (iheight, iwidth);
  for ( ii = 0 ; ii < iheight ; ii++ ) {
    for ( jj = 0 ; jj < iwidth ; jj++ ) {
      gsl_matrix_uchar_set (iimg, ii, jj, pixels[ii * iwidth + jj]);
    }
  }

  /* Dimensions of the averaged image.  */
  owidth = ceil (iwidth / kernel_size);
  oheight = ceil (iheight / kernel_size);

  /* Form the output image.  */
  oimg = gsl_matrix_uchar_alloc (oheight, owidth);
  for ( ii = 0 ; ii < oheight ; ii++ ) {
    for ( jj = 0 ; jj < owidth ; jj++ ) {
      gsl_matrix_uchar_set
        (oimg, ii, jj, averaging_kernel (iimg, kernel_size,
                                         ii * kernel_size + kernel_size / 2,
                                         jj * kernel_size + kernel_size / 2));
    }
  }

  /* Write output image back into pixel memory.  */
  for ( ii = 0 ; ii < oheight ; ii++ ) {
    for ( jj = 0 ; jj < owidth ; jj++ ) {
      pixels[ii * owidth + jj] = gsl_matrix_uchar_get (oimg, ii, jj);
    }
  }

  /* Done with matrix forms of the image.  */
  gsl_matrix_uchar_free (oimg);
  gsl_matrix_uchar_free (iimg);

  /* Modify the input/output arguments and resize the allocated space
     as promised.  */
  *width = owidth;
  *height = oheight;
  reallocated_pixels = realloc (pixels, owidth * oheight);

  return reallocated_pixels;
}

/* Given a block of pixel_count floats, map them linearly from range
   [max (minsample, mean - 3 * sigma), min (maxsample, mean + 3 * sigma)]
   into the unsigned char range, with input samples outside the above
   range clamped.  Return the data in new malloc()ed memory.  */
unsigned char *map_floats_to_unsigned_bytes (float *daf, size_t pixel_count)
{
  unsigned char *pixels = malloc (pixel_count * sizeof (unsigned char));

  /* Minimum and maximum values in the input data.  */
  float imin = gsl_stats_float_min (daf, 1, pixel_count);
  float imax = gsl_stats_float_max (daf, 1, pixel_count);
  /* Mean value of input data.  */
  float imean = gsl_stats_float_mean (daf, 1, pixel_count);
  /* Standard deviation of input data.  */
  float isdev = gsl_stats_float_sd (daf, 1, pixel_count);
  /* Minimum and maximum after clamping.  */
  float omin = GSL_MAX (imean - 3 * isdev, imin);
  float omax = GSL_MIN (imean + 3 * isdev, imax);

  /* Shift we need to apply to the data to get it to fall in the
     range of the unsigned chars.  */
  float bias = -omin + 0.25;

  /* Compute all the output pixels.  */
  size_t ii;
  for ( ii = 0 ; ii < pixel_count ; ii++ ) {
    if ( daf[ii] < omin ) {
      pixels[ii] = 0;           /* Clamp low.  */
    }
    else if ( daf[ii] > omax ) {
      pixels[ii] = UCHAR_MAX;   /* Clamp high.  */
    }
    else {
      pixels[ii] = (daf[ii] + bias) * (UCHAR_MAX / (omax - omin));
    }
  }

  return pixels;
}

/* Scale the *width x *height image at pixels st its large dimension
   is less than or equal to max_large_dimension.  The memory pointed
   to by pixels is resized and possible relocated, with the new
   location being returned.  The new image width and height are
   returned in *width and *height.  */
unsigned char *scale_unsigned_char_image_dimensions (unsigned char *pixels,
                                      unsigned long max_large_dimension,
                                      unsigned long *width,
                                      unsigned long *height)
{
  /* This assertion is pretty obvious, but since the algorithm needs
     it to work correctly, its included.  */
  asfRequire(max_large_dimension>1,
             "Image needs to be at least one pixel by one pixel.\n");
  if ( GSL_MAX (*width, *height) > max_large_dimension ) {
    int kernel_size = GSL_MAX (*width, *height) / max_large_dimension + 1;
    if ( kernel_size % 2 != 1 ) {
      kernel_size++;
    }
    pixels = average_unsigned_char_pixels (pixels, width, height, kernel_size);
  }

  return pixels;
}

/* Figure basic statistics for the image as needed for the different data
   scaling techniques available to the user */
void
get_statistics (FloatImage *si, scale_t sample_mapping, int sampling_stride,
                float *mean, float *standard_deviation,
                float *min_sample, float *max_sample,
                float *omin, float *omax, gsl_histogram **hist,
                float no_data)
{
  asfRequire(*max_sample >= *min_sample,
              "get_statistics() error ...max_sample parameter < min_sample parameter\n");

  if ( sample_mapping == SIGMA ) {
    float_image_approximate_statistics (si, sampling_stride, mean,
                                        standard_deviation, no_data);
    *omin = *mean - 2 * (*standard_deviation);
    *omax = *mean + 2 * (*standard_deviation);
  }

  else if ( sample_mapping == MINMAX ) {
    float_image_statistics (si, min_sample, max_sample, mean,
                            standard_deviation, no_data);
    *omin = *min_sample;
    *omax = *max_sample;
  }

  else if ( sample_mapping == HISTOGRAM_EQUALIZE ) {
    float_image_statistics (si, min_sample, max_sample, mean,
                            standard_deviation, no_data);
    // Add a little bit of tail padding on the histgram to avoid problems
    // when searching for image min or max in the histogram (gsl histogram
    // limitation)
    //*omin = *min_sample * 0.025 * (*min_sample);
    //*omax = *max_sample * 0.025 * (*max_sample);
    //
    // NOTE:  The above doesn't work properly for data with negative
    // values and also, I think the first multiply is a typo and that
    // a '+' was probably intended.  If so, then the affect of the
    // above code is for the GSL increment (bin counting) function to
    // ignore values outside the adjusted min/max range, e.g. ignore
    // boundary case values (black and white noise tends to sum in the
    // limits, so ignoring the boundaries produces a higher quality
    // image.)

    // NOTE: The following assumes that the intent of the above code was
    // to ignore boundary case values, but the new histogram limits are
    // now calculated based on the full data range.
    float bin_range;

    bin_range = (*max_sample - *min_sample) / NUM_HIST_BINS;
    *omin = *min_sample + (0.025 * bin_range);
    *omax = *max_sample - (0.025 * bin_range);
    *hist = float_image_gsl_histogram (si, *omin, *omax, NUM_HIST_BINS);
  }
}

#ifndef linux
#ifndef darwin
#ifndef win32
static double
round (double arg)
{
    return floor (arg + 0.5);
}
#endif // #ifndef win32
#endif // #ifndef darwin
#endif // #ifndef linux

/* Create a byte value based on the floating point value and whatever scaling
   method the user chose */
unsigned char
pixel_float2byte(float paf, scale_t sample_mapping,
                 float omin, float omax, gsl_histogram *hist,
                 gsl_histogram_pdf *hist_pdf, float no_data_value)
{
  const unsigned char min_brightness=0;          // Minimum brightess via byte
  const unsigned char max_brightness=UCHAR_MAX;  // Maximum brightess via byte
  unsigned char pab = 0;                         // Pixel As Byte.
  size_t hist_bin;                       // histogram bin for given float value
  double slope, offset;

  // This case is easy -- always map the "no data" value to 0.
  if (paf == no_data_value)
      return 0;

  switch ( sample_mapping ) {
  case TRUNCATE:
    if ( paf <= (float) min_brightness ) {
      pab = min_brightness;
    }
    else if ( paf >= (float) max_brightness ) {
      pab = max_brightness;
    }
    else {
      pab = (unsigned char) paf;
    }
    break;
  case MINMAX:
  case MINMAX_MEDIAN:
  case SIGMA:
    slope = max_brightness / (omax-omin);
    offset = -slope * omin;
    if ((slope * paf + offset) < (float)min_brightness)
      pab = min_brightness;
    else if ((slope * paf + offset) > (float)max_brightness)
      pab = max_brightness;
    else
      pab = slope * paf + offset;
    break;
  case HISTOGRAM_EQUALIZE:
    // Since we used a mask when we called float_image_statistics() earlier,
    // we've got to account for it when writing the image out. The only way we
    // could think to do that was casting the mask value to byte :(.
    //
    // FIXME: The following cast of FLOAT_IMAGE_DEFAULT_MASK (currently equal
    // to 0.0) is incorrect for images such as DEMs and db's that contain
    // negative data.  I'm voting for something similar, but force data values
    // sufficiently close to omin TO omin ...if we do any forcing at all.
    // SIDE EFFECTS: As-is, the following will result in black shot-noise and
    // 'topo line' type artifacts in images that contain negative data (in regions
    // where zero-crossings occur or in noise near 0.0)
    //
    if (0==gsl_fcmp(paf, FLOAT_IMAGE_DEFAULT_MASK, 0.00000000001)) {
      pab = (unsigned char)FLOAT_IMAGE_DEFAULT_MASK;
    }
    else {
      // Determine which bin the pixel-as-float is in
      if (paf <= gsl_histogram_min(hist)) {
	hist_bin = 0;
      }
      else if (paf >= gsl_histogram_max(hist)) {
        hist_bin = gsl_histogram_bins(hist) - 1;
      }
      else {
        // Pixel is within a valid range, so the following should not complain
	gsl_histogram_find (hist, paf, &hist_bin);
      }

      double pdf_at_index = gsl_histogram_get ((gsl_histogram*)hist_pdf,
					       hist_bin);
      pab = (unsigned char)(max_brightness * pdf_at_index);
    }
    break;
  default:
    asfPrintError("Impossible: sample_mapping=%d\n", sample_mapping);
    break;
  }
  return pab;
}


#ifndef SIR_H
#define SIR_H

#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_errno.h>
#include <iostream>
#include "DiffEq_Sim.h"

using namespace std;

class SIR : public DiffEq_Sim {

    private:
        const double beta;
        const double gamma;

    public:
        SIR() : beta(0.0), gamma(0.0) { nbins=3;}
        SIR(double b, double g): beta(b), gamma(g) { nbins=3; }
        ~SIR() {};

        void initialize( double S, double I, double R) {
            y = new double[nbins];
            y[0] = S; y[1] = I; y[2] = R;
        }

        void derivative(double const y[], double dydt[]) {
            dydt[0] = -beta*y[0]*y[1];
            dydt[1] = +beta*y[0]*y[1] - gamma*y[1];
            dydt[2] = +gamma*y[1];
        }

};

#endif


#pragma once

#include "rev/Utilities.h"
#include <algorithm>
#include <cstddef>
#include <glm/glm.hpp>
#include <gsl/gsl_assert>
#include <gsl/span>
#include <limits>
#include <vector>

namespace rev {

template <typename PointType>
struct WeightedControlPoint {
    PointType point;
    typename PointType::value_type weight;
};

template <typename PointType>
class NurbsCurve {
public:
    using ControlPointType = WeightedControlPoint<PointType>;
    using ValueType = typename PointType::value_type;

    NurbsCurve(size_t order, gsl::span<const ValueType> knots,
        gsl::span<const ControlPointType> controlPoints)
        : _order(order)
        , _knots(knots.begin(), knots.end())
        , _controlPoints(controlPoints.begin(), controlPoints.end())
    {
        Expects(order > 1);
        Expects(static_cast<size_t>(controlPoints.size()) >= order);
        Expects(knots.size() == (controlPoints.size() + order));
    }

    ValueType getStart() const { return _knots.front(); }

    ValueType getEnd() const { return _knots.back(); }

    PointType operator[](ValueType position) const
    {
        auto knotsBegin = _knots.begin();
        auto knotsEnd = _knots.end();
        auto spanStart = std::lower_bound(knotsBegin, knotsEnd, position);
        size_t controlPointIndex = std::distance(knotsBegin, spanStart);
        if (controlPointIndex == 0) {
            // Before the first knot. Just return the value of the first control
            // point.
            return _controlPoints.front().point;
        }

        if (spanStart == knotsEnd) {
            // We're past the last knot. Just return the value of the last control
            // point.
            return _controlPoints.back().point;
        }

        size_t controlPointsToProcess = _order;
        size_t controlPointCount = _controlPoints.size();
        if (controlPointIndex > controlPointCount) {
            controlPointsToProcess -= (controlPointIndex - controlPointCount);
        }

        PointType numerator{ 0 };
        ValueType denominator{ 0 };
        do {
            controlPointIndex--;
            auto& controlPoint = _controlPoints[controlPointIndex];
            ValueType weightedBasis
                = basisValue(position, controlPointIndex, _order) * controlPoint.weight;
            numerator += weightedBasis * _controlPoints[controlPointIndex].point;
            denominator += weightedBasis;
            controlPointsToProcess--;
        } while (controlPointIndex && controlPointsToProcess);
        return numerator / denominator;
    }

private:
    ValueType basisValue(ValueType position, size_t controlPointIndex, size_t order) const
    {
        auto& controlPoint = _controlPoints[controlPointIndex];
        ValueType firstKnot = _knots[controlPointIndex];
        ValueType lastKnot = _knots[controlPointIndex + order];
        if (order == 2) {
            // Triangle function
            ValueType middleKnot = _knots[controlPointIndex + 1];
            if (position < middleKnot) {
                return rampUp(position, firstKnot, middleKnot);
            } else {
                return rampDown(position, middleKnot, lastKnot);
            }
        } else {
            size_t lowerOrder = order - 1;
            ValueType secondKnot = _knots[controlPointIndex + 1];
            ValueType secondToLastKnot = _knots[controlPointIndex + lowerOrder];

            ValueType result{ 0 };
            if (position < secondToLastKnot) {
                result += rampUp(position, firstKnot, secondToLastKnot)
                    * basisValue(position, controlPointIndex, lowerOrder);
            }

            if (position > secondKnot) {
                result += rampDown(position, secondKnot, lastKnot)
                    * basisValue(position, controlPointIndex + 1, lowerOrder);
            }

            return result;
        }
    }

    ValueType rampUp(ValueType value, ValueType bottom, ValueType top) const
    {
        if (nearEqual(top, bottom)) {
            return 0.5;
        }
        return (value - bottom) / (top - bottom);
    }

    ValueType rampDown(ValueType value, ValueType bottom, ValueType top) const
    {
        if (nearEqual(top, bottom)) {
            return 0.5;
        }
        return (top - value) / (top - bottom);
    }

    size_t _order;
    std::vector<ValueType> _knots;
    std::vector<ControlPointType> _controlPoints;
};
} // namespace rev


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

/* Author:  G. Jungman */

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_gegenbauer.h"

#include "error.h"

/* See: [Thompson, Atlas for Computing Mathematical Functions] */

int
gsl_sf_gegenpoly_1_e(double lambda, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(lambda == 0.0) {
    result->val = 2.0*x;
    result->err = 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = 2.0*lambda*x;
    result->err = 4.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
}

int
gsl_sf_gegenpoly_2_e(double lambda, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(lambda == 0.0) {
    const double txx = 2.0*x*x;
    result->val  = -1.0 + txx;
    result->err  = 2.0 * GSL_DBL_EPSILON * fabs(txx);
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    result->val = lambda*(-1.0 + 2.0*(1.0+lambda)*x*x);
    result->err = GSL_DBL_EPSILON * (2.0 * fabs(result->val) + fabs(lambda));
    return GSL_SUCCESS;
  }
}

int
gsl_sf_gegenpoly_3_e(double lambda, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(lambda == 0.0) {
    result->val = x*(-2.0 + 4.0/3.0*x*x);
    result->err = GSL_DBL_EPSILON * (2.0 * fabs(result->val) + fabs(x));
    return GSL_SUCCESS;
  }
  else {
    double c = 4.0 + lambda*(6.0 + 2.0*lambda);
    result->val = 2.0*lambda * x * ( -1.0 - lambda + c*x*x/3.0 );
    result->err = GSL_DBL_EPSILON * (2.0 * fabs(result->val) + fabs(lambda * x));
    return GSL_SUCCESS;
  }
}

int
gsl_sf_gegenpoly_n_e(int n, double lambda, double x, gsl_sf_result * result)
{
  /* CHECK_POINTER(result) */

  if(lambda <= -0.5 || n < 0) {
    DOMAIN_ERROR(result);
  }
  else if(n == 0) {
    result->val = 1.0;
    result->err = 0.0;
    return GSL_SUCCESS;
  }
  else if(n == 1) {
    return gsl_sf_gegenpoly_1_e(lambda, x, result);
  }
  else if(n == 2) {
    return gsl_sf_gegenpoly_2_e(lambda, x, result);
  }
  else if(n == 3) {
    return gsl_sf_gegenpoly_3_e(lambda, x, result);
  }
  else {
    if(lambda == 0.0 && (x >= -1.0 || x <= 1.0)) {
      /* 2 T_n(x)/n */
      const double z = n * acos(x);
      result->val = 2.0 * cos(z) / n;
      result->err = 2.0 * GSL_DBL_EPSILON * fabs(z * result->val);
      return GSL_SUCCESS;
    }
    else {
      int k;
      gsl_sf_result g2;
      gsl_sf_result g3;
      int stat_g2 = gsl_sf_gegenpoly_2_e(lambda, x, &g2);
      int stat_g3 = gsl_sf_gegenpoly_3_e(lambda, x, &g3);
      int stat_g  = GSL_ERROR_SELECT_2(stat_g2, stat_g3);
      double gkm2 = g2.val;
      double gkm1 = g3.val;
      double gk = 0.0;
      for(k=4; k<=n; k++) {
        gk = (2.0*(k+lambda-1.0)*x*gkm1 - (k+2.0*lambda-2.0)*gkm2) / k;
	gkm2 = gkm1;
	gkm1 = gk;
      }
      result->val = gk;
      result->err = 2.0 * GSL_DBL_EPSILON * 0.5 * n * fabs(gk);
      return stat_g;
    }
  }
}

int
gsl_sf_gegenpoly_array(int nmax, double lambda, double x, double * result_array)
{
  int k;

  /* CHECK_POINTER(result_array) */

  if(lambda <= -0.5 || nmax < 0) {
    GSL_ERROR("domain error", GSL_EDOM);
  }

  /* n == 0 */
  result_array[0] = 1.0;
  if(nmax == 0) return GSL_SUCCESS;

  /* n == 1 */
  if(lambda == 0.0)
    result_array[1] = 2.0*x;
  else
    result_array[1] = 2.0*lambda*x;

  /* n <= nmax */
  for(k=2; k<=nmax; k++) {
    double term1 = 2.0*(k+lambda-1.0) * x * result_array[k-1];
    double term2 = (k+2.0*lambda-2.0)	  * result_array[k-2];
    result_array[k] = (term1 - term2) / k;
  }

  return GSL_SUCCESS;
}

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

#include "eval.h"

double gsl_sf_gegenpoly_1(double lambda, double x)
{
  EVAL_RESULT(gsl_sf_gegenpoly_1_e(lambda, x, &result));
}

double gsl_sf_gegenpoly_2(double lambda, double x)
{
  EVAL_RESULT(gsl_sf_gegenpoly_2_e(lambda, x, &result));
}

double gsl_sf_gegenpoly_3(double lambda, double x)
{
  EVAL_RESULT(gsl_sf_gegenpoly_3_e(lambda, x, &result));
}

double gsl_sf_gegenpoly_n(int n, double lambda, double x)
{
  EVAL_RESULT(gsl_sf_gegenpoly_n_e(n, lambda, x, &result));
}


/* $Header$ */

/* Purpose: GSL functions that handle missing values - stubs only
   Extra NCO code derived from GSL removed 2020-04-02 to avoid license conflict between GSL GPL and NCO BSD code */

/* Copyright (C) 1995--present Charlie Zender
   This file is part of NCO, the netCDF Operators. NCO is free software.
   You may redistribute and/or modify NCO under the terms of the
   3-Clause BSD License with exceptions described in the LICENSE file */

#include <gsl/gsl_statistics.h>
#include <gsl/gsl_fit.h>
#include "nco_gsl.h" /* Missing value-aware GSL functions */

/* 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
nco_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,
 const double *mss_val)
{

  return gsl_fit_linear(x, xstride, y, ystride, n, c0, c1, cov_00, cov_01, cov_11, sumsq);

}

static double
compute_covariance
(const double data1[],
 const size_t stride1,
 const double data2[],
 const size_t stride2,
 const size_t n,
 const double mean1,
 const double mean2,
 const double *mss_val) /* Missing value */
{
  /* takes a dataset and finds the covariance */

  long double covariance = 0 ;

  size_t i;

  /* find the sum of the squares */
  if (mss_val==NULL)
  {
    for (i = 0; i < n; i++)
    {
      const long double delta1 = (data1[i * stride1] - mean1);
      const long double delta2 = (data2[i * stride2] - mean2);
      covariance += (delta1 * delta2 - covariance) / (i + 1);
    }

  }
  else
  {
    for (i = 0; i < n; i++)
    {
      if (data2[i * stride2]!=*mss_val)
      {
        const long double delta1 = (data1[i * stride1] - mean1);
        const long double delta2 = (data2[i * stride2] - mean2);
        covariance += (delta1 * delta2 - covariance) / (i + 1);
      }
    }
  }

  return covariance ;
}

double
nco_gsl_stats_covariance_m
(const double data1[],
 const size_t stride1,
 const double data2[],
 const size_t stride2,
 const size_t n,
 const double mean1,
 const double mean2,
 const double *mss_val) /* Missing value */
{
  return gsl_stats_covariance_m(data1, stride1, data2,stride2,n,mean1,mean2);

}

double
nco_gsl_stats_covariance
(const double data1[],
 const size_t stride1,
 const double data2[],
 const size_t stride2,
 const size_t n,
 const double *mss_val) /* Missing value */
{
  return gsl_stats_covariance(data1, stride1, data2,stride2,n);

}

double
nco_gsl_stats_mean
(const double data[],
 const size_t stride,
 const size_t size,
 const double *mss_val) /* Missing value */
{

  return gsl_stats_mean(data, stride, size);

}


/* min/golden.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 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.
 */

/* goldensection.c -- goldensection minimum finding algorithm */

#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_min.h>

#include "min.h"

typedef struct
  {
    double dummy;
  }
goldensection_state_t;

static int goldensection_init (void * vstate, gsl_function * f, double x_minimum, double f_minimum, double x_lower, double f_lower, double x_upper, double f_upper);
static int goldensection_iterate (void * vstate, gsl_function * f, double * x_minimum, double * f_minimum, double * x_lower, double * f_lower, double * x_upper, double * f_upper);

static int
goldensection_init (void * vstate, gsl_function * f, double x_minimum, double f_minimum, double x_lower, double f_lower, double x_upper, double f_upper)
{
  goldensection_state_t * state = (goldensection_state_t *) vstate;

  /* no initialization required, prevent warnings about unused variables */

  state = 0;
  f = 0;
  x_minimum = 0;
  f_minimum = 0;
  x_lower = 0;
  f_lower = 0;
  x_upper = 0;
  f_upper = 0;

  return GSL_SUCCESS;
}

static int
goldensection_iterate (void * vstate, gsl_function * f, double * x_minimum, double * f_minimum, double * x_lower, double * f_lower, double * x_upper, double * f_upper)
{
  goldensection_state_t * state = (goldensection_state_t *) vstate;

  const double x_center = *x_minimum ;
  const double x_left = *x_lower ;
  const double x_right = *x_upper ;

  const double f_min = *f_minimum;

  const double golden = 0.3819660; /* golden = (3 - sqrt(5))/2 */

  const double w_lower = (x_center - x_left);
  const double w_upper = (x_right - x_center);

  double x_new, f_new;

  state = 0 ; /* avoid warning about unused parameters */

  x_new = x_center + golden * ((w_upper > w_lower) ? w_upper : -w_lower) ;

  SAFE_FUNC_CALL (f, x_new, &f_new);

  if (f_new < f_min)
    {
      *x_minimum = x_new ;
      *f_minimum = f_new ;
      return GSL_SUCCESS;
    }
  else if (x_new < x_center && f_new > f_min)
    {
      *x_lower = x_new ;
      *f_lower = f_new ;
      return GSL_SUCCESS;
    }
  else if (x_new > x_center && f_new > f_min)
    {
      *x_upper = x_new ;
      *f_upper = f_new ;
      return GSL_SUCCESS;
    }
  else
    {
      return GSL_FAILURE;
    }
}

static const gsl_min_fminimizer_type goldensection_type =
{"goldensection",                               /* name */
 sizeof (goldensection_state_t),
 &goldensection_init,
 &goldensection_iterate};

const gsl_min_fminimizer_type  * gsl_min_fminimizer_goldensection = &goldensection_type;