static char help[] =
"Solve the linear biharmonic equation in 2D.  Equation is\n"
"  Lap^2 u = f\n"
"where Lap = - grad^2 is the positive Laplacian, equivalently\n"
"  u_xxxx + 2 u_xxyy + u_yyyy = f(x,y)\n"
"Domain is unit square  S = (0,1)^2.   Boundary conditions are homogeneous\n"
"simply-supported:  u = 0,  Lap u = 0.  The equation is rewritten as a\n"
"2x2 block system with SPD Laplacian blocks on the diagonal:\n"
"   | Lap |  0  |  | v |   | f | \n"
"   |-----|-----|  |---| = |---| \n"
"   | -I  | Lap |  | u |   | 0 | \n"
"Includes manufactured, polynomial exact solution.  The discretization is\n"
"structured-grid (DMDA) finite differences.  Includes analytical Jacobian.\n"
"Recommended preconditioning combines fieldsplit:\n"
"   -pc_type fieldsplit -pc_fieldsplit_type multiplicative|additive \n"
"with multigrid as the preconditioner for the diagonal blocks:\n"
"   -fieldsplit_v_pc_type mg|gamg -fieldsplit_u_pc_type mg|gamg\n"
"(GMG requires setting levels and Galerkin coarsening.)  One can also do\n"
"monolithic multigrid (-pc_type mg|gamg).\n\n";

#include <petsc.h>

typedef struct {
    PetscReal  v, u;
} Field;

typedef struct {
    PetscReal  (*f)(PetscReal x, PetscReal y);  // right-hand side
} BiharmCtx;

static PetscReal c(PetscReal x) {
    return x*x*x * (1.0-x)*(1.0-x)*(1.0-x);
}

static PetscReal ddc(PetscReal x) {
    return 6.0 * x * (1.0-x) * (1.0 - 5.0 * x + 5.0 * x*x);
}

static PetscReal d4c(PetscReal x) {
    return - 72.0 * (1.0 - 5.0 * x + 5.0 * x*x);
}

static PetscReal u_exact_fcn(PetscReal x, PetscReal y) {
    return c(x) * c(y);
}

static PetscReal lap_u_exact_fcn(PetscReal x, PetscReal y) {
    return - ddc(x) * c(y) - c(x) * ddc(y);  // Lap u = - grad^2 u
}

static PetscReal f_fcn(PetscReal x, PetscReal y) {
    return d4c(x) * c(y) + 2.0 * ddc(x) * ddc(y) + c(x) * d4c(y);  // Lap^2 u = grad^4 u
}

extern PetscErrorCode FormExactWLocal(DMDALocalInfo*, Field**, BiharmCtx*);
extern PetscErrorCode FormFunctionLocal(DMDALocalInfo*, Field**, Field **FF, BiharmCtx*);
extern PetscErrorCode FormJacobianLocal(DMDALocalInfo*, Field**, Mat, Mat, BiharmCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    DM             da;
    SNES           snes;
    Vec            w, w_initial, w_exact;
    BiharmCtx      user;
    Field          **aW;
    PetscReal      normv, normu, errv, erru;
    DMDALocalInfo  info;

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

    user.f = &f_fcn;
    ierr = DMDACreate2d(PETSC_COMM_WORLD,
                        DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_STAR,
                        3,3,PETSC_DECIDE,PETSC_DECIDE,
                        2,1,              // degrees of freedom, stencil width
                        NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);  // this must be called BEFORE SetUniformCoordinates
    ierr = DMDASetUniformCoordinates(da,0.0,1.0,0.0,1.0,-1.0,-1.0); CHKERRQ(ierr);
    ierr = DMDASetFieldName(da,0,"v"); CHKERRQ(ierr);
    ierr = DMDASetFieldName(da,1,"u"); 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 = SNESSetType(snes,SNESKSPONLY); CHKERRQ(ierr);
    ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

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

    ierr = SNESGetSolution(snes,&w); CHKERRQ(ierr);
    ierr = SNESGetDM(snes,&da); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);

    ierr = DMCreateGlobalVector(da,&w_exact); CHKERRQ(ierr);
    ierr = DMDAVecGetArray(da,w_exact,&aW); CHKERRQ(ierr);
    ierr = FormExactWLocal(&info,aW,&user); CHKERRQ(ierr);
    ierr = DMDAVecRestoreArray(da,w_exact,&aW); CHKERRQ(ierr);
    ierr = VecStrideNorm(w_exact,0,NORM_INFINITY,&normv); CHKERRQ(ierr);
    ierr = VecStrideNorm(w_exact,1,NORM_INFINITY,&normu); CHKERRQ(ierr);
    ierr = VecAXPY(w,-1.0,w_exact); CHKERRQ(ierr);
    ierr = VecStrideNorm(w,0,NORM_INFINITY,&errv); CHKERRQ(ierr);
    ierr = VecStrideNorm(w,1,NORM_INFINITY,&erru); CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD,
        "done on %d x %d grid ...\n"
        "  errors |v-vex|_inf/|vex|_inf = %.5e, |u-uex|_inf/|uex|_inf = %.5e\n",
        info.mx,info.my,errv/normv,erru/normu); CHKERRQ(ierr);

    ierr = VecDestroy(&w_exact); CHKERRQ(ierr);
    ierr = SNESDestroy(&snes); CHKERRQ(ierr);
    return PetscFinalize();
}

PetscErrorCode FormExactWLocal(DMDALocalInfo *info, Field **aW, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, j;
    PetscReal  xymin[2], xymax[2], hx, hy, x, y;
    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = i * hx;
            aW[j][i].u = u_exact_fcn(x,y);
            aW[j][i].v = lap_u_exact_fcn(x,y);
        }
    }
    return 0;
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, Field **aW,
                                 Field **FF, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt   i, j;
    PetscReal  xymin[2], xymax[2], hx, hy, darea, scx, scy, scdiag, x, y,
               ve, vw, vn, vs, ue, uw, un, us;
    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    darea = hx * hy;               // multiply FD equations by this
    scx = hy / hx;
    scy = hx / hy;
    scdiag = 2.0 * (scx + scy);    // diagonal scaling
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = xymin[1] + j * hy;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = xymin[0] + i * hx;
            if (i==0 || i==info->mx-1 || j==0 || j==info->my-1) {
                FF[j][i].v = scdiag * aW[j][i].v;
                FF[j][i].u = scdiag * aW[j][i].u;
            } else {
                ve = (i+1 == info->mx-1) ? 0.0 : aW[j][i+1].v;
                vw = (i-1 == 0)          ? 0.0 : aW[j][i-1].v;
                vn = (j+1 == info->my-1) ? 0.0 : aW[j+1][i].v;
                vs = (j-1 == 0)          ? 0.0 : aW[j-1][i].v;
                FF[j][i].v = scdiag * aW[j][i].v - scx * (vw + ve) - scy * (vs + vn)
                             - darea * (*(user->f))(x,y);
                ue = (i+1 == info->mx-1) ? 0.0 : aW[j][i+1].u;
                uw = (i-1 == 0)          ? 0.0 : aW[j][i-1].u;
                un = (j+1 == info->my-1) ? 0.0 : aW[j+1][i].u;
                us = (j-1 == 0)          ? 0.0 : aW[j-1][i].u;
                FF[j][i].u = - darea * aW[j][i].v
                             + scdiag * aW[j][i].u - scx * (uw + ue) - scy * (us + un);
            }
        }
    }
    ierr = PetscLogFlops(18.0*info->xm*info->ym);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormJacobianLocal(DMDALocalInfo *info, Field **aW,
                                 Mat J, Mat Jpre, BiharmCtx *user) {
    PetscErrorCode ierr;
    PetscInt     i, j, c, ncol;
    PetscReal    xymin[2], xymax[2], hx, hy, darea, scx, scy, scdiag, val[6];
    MatStencil   col[6], row;

    ierr = DMGetBoundingBox(info->da,xymin,xymax); CHKERRQ(ierr);
    hx = (xymax[0] - xymin[0]) / (info->mx - 1);
    hy = (xymax[1] - xymin[1]) / (info->my - 1);
    darea = hx * hy;               // multiply FD equations by this
    scx = hy / hx;
    scy = hx / hy;
    scdiag = 2.0 * (scx + scy);    // diagonal scaling
    for (j = info->ys; j < info->ys + info->ym; j++) {
        row.j = j;
        for (i = info->xs; i < info->xs + info->xm; i++) {
            row.i = i;
            for (c = 0; c < 2; c++) { // v,u equations are c=0,1
                row.c = c;
                col[0].c = c;     col[0].i = i;       col[0].j = j;
                val[0] = scdiag;
                if (i==0 || i==info->mx-1 || j==0 || j==info->my-1) {
                    ierr = MatSetValuesStencil(Jpre,1,&row,1,col,val,INSERT_VALUES);
                        CHKERRQ(ierr);
                } else {
                    ncol = 1;
                    if (i+1 < info->mx-1) {
                        col[ncol].c = c;  col[ncol].i = i+1;  col[ncol].j = j;
                        val[ncol++] = -scx;
                    }
                    if (i-1 > 0) {
                        col[ncol].c = c;  col[ncol].i = i-1;  col[ncol].j = j;
                        val[ncol++] = -scx;
                    }
                    if (j+1 < info->my-1) {
                        col[ncol].c = c;  col[ncol].i = i;    col[ncol].j = j+1;
                        val[ncol++] = -scy;
                    }
                    if (j-1 > 0) {
                        col[ncol].c = c;  col[ncol].i = i;    col[ncol].j = j-1;
                        val[ncol++] = -scy;
                    }
                    if (c == 1) {  // u equation;  has off-diagonal block entry
                        col[ncol].c = 0;
                        col[ncol].i = i;  col[ncol].j = j;
                        val[ncol++] = - darea;
                    }
                    ierr = MatSetValuesStencil(Jpre,1,&row,ncol,col,val,INSERT_VALUES);
                        CHKERRQ(ierr);
                }
            }
        }
    }

    ierr = MatAssemblyBegin(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(Jpre,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != Jpre) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


//
// Created by karl_ on 2020-5-3.
//

#include <stdlib.h>
#include <math.h>
#include <cblas.h>
#include <core/color-space.h>
#include <imgproc/histogram-equalization.h>
#include "color-convert.h"

// ref: https://www.math.uci.edu/icamp/courses/math77c/demos/hist_eq.pdf
static void
histeq(double const *src, unsigned src_stride, double *dst, unsigned dst_stride, size_t area, unsigned range) {
    unsigned *cdf;
    size_t i;
    cdf = calloc(range, sizeof(unsigned));
    for (i = 0; i < area; ++i) {
        cdf[(int) (src[i * src_stride] * range)] += 1;
    }
    for (i = 1; i < range; ++i) {
        cdf[i] += cdf[i - 1];
    }
    for (i = 0; i < area; ++i) {
        dst[i * dst_stride] = floor((double) (range - 1) * cdf[(int) (src[i * src_stride] * range)] / area) / range;
    }
    free(cdf);
}

static gboolean histeq_grayscale(CoreImage *src, CoreImage **dst) {
    gdouble *src_data_cast = NULL;
    gdouble *dst_data_cast = NULL;
    gpointer src_data, dst_data;
    CoreSize *size = NULL;
    guint8 channel;
    gsize i, area;
    CoreColorSpace color_space;
    CorePixelType pixel_type;
    gdouble range, inv_range;
    g_return_val_if_fail(src != NULL, FALSE);
    g_return_val_if_fail(dst != NULL, FALSE);

    channel = core_image_get_channel(src);
    g_return_val_if_fail(channel == 1, FALSE);

    src = g_object_ref(src);
    src_data = core_image_get_data(src);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    color_space = core_image_get_color_space(src);
    pixel_type = core_image_get_pixel_type(src);
    range = core_pixel_get_range(pixel_type);
    inv_range = 1.0 / range;

    /* cast to double and normalize pixel data to [0, 1] */
    if (core_pixel_is_double(pixel_type)) {
        src_data_cast = src_data;
    } else if (core_pixel_is_uint8(pixel_type)) {
        src_data_cast = g_malloc(sizeof(gdouble) * area);
        for (i = 0; i < area; ++i) {
            src_data_cast[i] = ((guint8 *) src_data)[i] * inv_range;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data_cast = g_malloc(sizeof(gdouble) * area);

    /* run the algorithm */
    histeq(src_data_cast, 1, dst_data_cast, 1, area, 256);

    /* cast back to uchar */
    if (core_pixel_is_double(pixel_type)) {
        /* exactly do nothing */
    } else if (core_pixel_is_uint8(pixel_type)) {
        /* reuse allocated memory */
        for (i = 0; i < area; ++i) {
            ((guint8 *) dst_data_cast)[i] = dst_data_cast[i] * 255.0;
        }
    } else {
        g_return_val_if_fail(FALSE, FALSE);
    }
    dst_data = dst_data_cast;
    if (core_pixel_is_uint8(pixel_type)) {
        /* release useless memory, can be removed for speed */
        dst_data = g_realloc(dst_data, sizeof(guint8) * area);
    }

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, color_space, pixel_type, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, color_space, pixel_type, size, FALSE);
    }

    /* free allocated additional memory */
    if (core_pixel_is_uint8(pixel_type)) {
        g_free(src_data_cast);
    }
    g_object_unref(size);
    g_object_unref(src);
    return TRUE;
}

static gboolean histeq_hsl(CoreImage *src, CoreImage **dst) {
    gdouble *src_data, *dst_data;
    CoreSize *size;
    gsize area, block_size;

    src_data = core_image_get_data(src);
    size = core_image_get_size(src);
    area = core_size_get_area(size);
    block_size = area * 3 * sizeof(gdouble);
    dst_data = g_malloc(block_size);
    memcpy(dst_data, src_data, block_size);
    histeq(src_data + 2, 3, dst_data + 2, 3, area, 255);

    if (*dst == NULL) {
        *dst = core_image_new_with_data(dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    } else {
        core_image_assign_data(*dst, dst_data, CORE_COLOR_SPACE_HSL, CORE_PIXEL_D3, size, FALSE);
    }

    g_object_unref(size);
}

gboolean imgproc_histogram_equalization(CoreImage *src, CoreImage **dst) {
    CoreColorSpace src_color_space = core_image_get_color_space(src);
    CoreImage *dummy = NULL;
    gboolean ret;

    if (src_color_space == CORE_COLOR_SPACE_GRAY_SCALE) {
        ret = histeq_grayscale(src, dst);
    } else {
        if (src_color_space == CORE_COLOR_SPACE_RGB) {
            imgproc_to_HSL(src, &dummy);
        } else {
            dummy = g_object_ref(src);
        }
        ret = histeq_hsl(dummy, dst);
        g_object_unref(dummy);
        if (src_color_space == CORE_COLOR_SPACE_RGB) {
            imgproc_to_RGB(*dst, dst);
        }
    }
    return ret;
}


/* Cholesky Decomposition
 *
 * Copyright (C) 2000  Thomas Walter
 *
 * 03 May 2000: Modified for GSL by Brian Gough
 * 29 Jul 2005: Additions by Gerard Jungman
 * Copyright (C) 2000,2001, 2002, 2003, 2005, 2007 Brian Gough, Gerard Jungman
 *
 * This is free software; you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation; either version 3, or (at your option) any
 * later version.
 *
 * This source 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.
 */

/*
 * Cholesky decomposition of a symmetrix positive definite matrix.
 * This is useful to solve the matrix arising in
 *    periodic cubic splines
 *    approximating splines
 *
 * This algorithm does:
 *   A = L * L'
 * with
 *   L  := lower left triangle matrix
 *   L' := the transposed form of L.
 *
 */

#include <config.h>

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

static inline
double
quiet_sqrt (double x)
     /* avoids runtime error, for checking matrix for positive definiteness */
{
  return (x >= 0) ? sqrt(x) : GSL_NAN;
}

int
gsl_linalg_cholesky_decomp (gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR("cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t i,j,k;
      int status = 0;

      /* Do the first 2 rows explicitly.  It is simple, and faster.  And
       * one can return if the matrix has only 1 or 2 rows.
       */

      double A_00 = gsl_matrix_get (A, 0, 0);

      double L_00 = quiet_sqrt(A_00);

      if (A_00 <= 0)
        {
          status = GSL_EDOM ;
        }

      gsl_matrix_set (A, 0, 0, L_00);

      if (M > 1)
        {
          double A_10 = gsl_matrix_get (A, 1, 0);
          double A_11 = gsl_matrix_get (A, 1, 1);

          double L_10 = A_10 / L_00;
          double diag = A_11 - L_10 * L_10;
          double L_11 = quiet_sqrt(diag);

          if (diag <= 0)
            {
              status = GSL_EDOM;
            }

          gsl_matrix_set (A, 1, 0, L_10);
          gsl_matrix_set (A, 1, 1, L_11);
        }

      for (k = 2; k < M; k++)
        {
          double A_kk = gsl_matrix_get (A, k, k);

          for (i = 0; i < k; i++)
            {
              double sum = 0;

              double A_ki = gsl_matrix_get (A, k, i);
              double A_ii = gsl_matrix_get (A, i, i);

              gsl_vector_view ci = gsl_matrix_row (A, i);
              gsl_vector_view ck = gsl_matrix_row (A, k);

              if (i > 0) {
                gsl_vector_view di = gsl_vector_subvector(&ci.vector, 0, i);
                gsl_vector_view dk = gsl_vector_subvector(&ck.vector, 0, i);

                gsl_blas_ddot (&di.vector, &dk.vector, &sum);
              }

              A_ki = (A_ki - sum) / A_ii;
              gsl_matrix_set (A, k, i, A_ki);
            }

          {
            gsl_vector_view ck = gsl_matrix_row (A, k);
            gsl_vector_view dk = gsl_vector_subvector (&ck.vector, 0, k);

            double sum = gsl_blas_dnrm2 (&dk.vector);
            double diag = A_kk - sum * sum;

            double L_kk = quiet_sqrt(diag);

            if (diag <= 0)
              {
                status = GSL_EDOM;
              }

            gsl_matrix_set (A, k, k, L_kk);
          }
        }

      /* Now copy the transposed lower triangle to the upper triangle,
       * the diagonal is common.
       */

      for (i = 1; i < M; i++)
        {
          for (j = 0; j < i; j++)
            {
              double A_ij = gsl_matrix_get (A, i, j);
              gsl_matrix_set (A, j, i, A_ij);
            }
        }

      if (status == GSL_EDOM)
        {
          GSL_ERROR ("matrix must be positive definite", GSL_EDOM);
        }

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_solve (const gsl_matrix * LLT,
                           const gsl_vector * b,
                           gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Copy x <- b */

      gsl_vector_memcpy (x, b);

      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}

int
gsl_linalg_cholesky_svx (const gsl_matrix * LLT,
                         gsl_vector * x)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (LLT->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* Solve for c using forward-substitution, L c = b */

      gsl_blas_dtrsv (CblasLower, CblasNoTrans, CblasNonUnit, LLT, x);

      /* Perform back-substitution, U x = c */

      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, LLT, x);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_cholesky_invert()
  Compute the inverse of a symmetric positive definite matrix in
Cholesky form.

Inputs: LLT - matrix in cholesky form on input
              A^{-1} = L^{-t} L^{-1} on output

Return: success or error
*/

int
gsl_linalg_cholesky_invert(gsl_matrix * LLT)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      size_t N = LLT->size1;
      size_t i, j;
      double sum;
      gsl_vector_view v1, v2;

      /* invert the lower triangle of LLT */
      for (i = 0; i < N; ++i)
        {
          double ajj;

          j = N - i - 1;

          gsl_matrix_set(LLT, j, j, 1.0 / gsl_matrix_get(LLT, j, j));
          ajj = -gsl_matrix_get(LLT, j, j);

          if (j < N - 1)
            {
              gsl_matrix_view m;

              m = gsl_matrix_submatrix(LLT, j + 1, j + 1,
                                       N - j - 1, N - j - 1);
              v1 = gsl_matrix_subcolumn(LLT, j, j + 1, N - j - 1);

              gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasNonUnit,
                             &m.matrix, &v1.vector);

              gsl_blas_dscal(ajj, &v1.vector);
            }
        } /* for (i = 0; i < N; ++i) */

      /*
       * The lower triangle of LLT now contains L^{-1}. Now compute
       * A^{-1} = L^{-t} L^{-1}
       *
       * The (ij) element of A^{-1} is column i of L^{-1} dotted into
       * column j of L^{-1}
       */

      for (i = 0; i < N; ++i)
        {
          for (j = i + 1; j < N; ++j)
            {
              v1 = gsl_matrix_subcolumn(LLT, i, j, N - j);
              v2 = gsl_matrix_subcolumn(LLT, j, j, N - j);

              /* compute Ainv_{ij} = sum_k Linv_{ki} Linv_{kj} */
              gsl_blas_ddot(&v1.vector, &v2.vector, &sum);

              /* store in upper triangle */
              gsl_matrix_set(LLT, i, j, sum);
            }

          /* now compute the diagonal element */
          v1 = gsl_matrix_subcolumn(LLT, i, i, N - i);
          gsl_blas_ddot(&v1.vector, &v1.vector, &sum);
          gsl_matrix_set(LLT, i, i, sum);
        }

      /* copy the transposed upper triangle to the lower triangle */

      for (j = 1; j < N; j++)
        {
          for (i = 0; i < j; i++)
            {
              double A_ij = gsl_matrix_get (LLT, i, j);
              gsl_matrix_set (LLT, j, i, A_ij);
            }
        }

      return GSL_SUCCESS;
    }
} /* gsl_linalg_cholesky_invert() */

int
gsl_linalg_cholesky_decomp_unit(gsl_matrix * A, gsl_vector * D)
{
  const size_t N = A->size1;
  size_t i, j;

  /* initial Cholesky */
  int stat_chol = gsl_linalg_cholesky_decomp(A);

  if(stat_chol == GSL_SUCCESS)
  {
    /* calculate D from diagonal part of initial Cholesky */
    for(i = 0; i < N; ++i)
    {
      const double C_ii = gsl_matrix_get(A, i, i);
      gsl_vector_set(D, i, C_ii*C_ii);
    }

    /* multiply initial Cholesky by 1/sqrt(D) on the right */
    for(i = 0; i < N; ++i)
    {
      for(j = 0; j < N; ++j)
      {
        gsl_matrix_set(A, i, j, gsl_matrix_get(A, i, j) / sqrt(gsl_vector_get(D, j)));
      }
    }

    /* Because the initial Cholesky contained both L and transpose(L),
       the result of the multiplication is not symmetric anymore;
       but the lower triangle _is_ correct. Therefore we reflect
       it to the upper triangle and declare victory.
       */
    for(i = 0; i < N; ++i)
      for(j = i + 1; j < N; ++j)
        gsl_matrix_set(A, i, j, gsl_matrix_get(A, j, i));
  }

  return stat_chol;
}


#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <gsl/gsl_blas.h>
#include "my_timer.h"

#define N 5

int cal_index_offset(int ndim, int *darr, int *position) {
    int size = 1;
    int offset = 0;
    for(int i = ndim-1; i >= 0; i--) {
        offset += position[i] * size;
        size *= darr[i];
    }
    return offset;
}

void print_mat_recursive(double *mat, int ndim, int *darr, int pdim, int *position) {
    if(pdim >= ndim) {
        int index_offset = cal_index_offset(ndim, darr, position);
        printf("%lf ", mat[index_offset]);
        return;
    }

    printf("[ ");
    for(int i = 0; i < darr[pdim]; i++) {
        position[pdim] = i;
        print_mat_recursive(mat, ndim, darr, pdim+1, position);
    }
    printf("]");
}

void print_mat(double *mat, int ndim, ...) {
    int position[ndim];
    int darr[ndim];

    va_list args;
    va_start(args, ndim);
    for(int i = 0; i < ndim; i++) {
        darr[i] = va_arg(args, int);
    }
    va_end(args);

    print_mat_recursive(mat, ndim, darr, 0, position);
    printf("\n");
}

void mm_brute_force(double *a, double *b, double *c, int p, int q, int r) {
    for(int i = 0; i < p; i++) {
        for(int k = 0; k < q; k++) {
            for(int j = 0; j < r; j++) {
                c[i * r + j] += a[i * q + k] * b[k * r + j];
            }
        }
    }
}

void mm_dgemm(double *a, double *b, double *c, int p, int q, int r) {
    int l = p;
    int m = q;
    int n = r;

    int lda = m;
    int ldb = n;
    int ldc = n;

    double alpha = 1.0;
    double beta = 0.0;

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
            l, n, m, alpha, a, lda, b, ldb, beta, c, ldc);
}

void example_dgemm() {
    int l, m, n;
    l = m = n = N;

    double *mat_a = (double*)malloc(sizeof(double) * l * m);
    double *mat_b = (double*)malloc(sizeof(double) * m * n);
    double *mat_c = (double*)malloc(sizeof(double) * l * n);

    for(int i = 0; i < (l*m); i++) {
        mat_a[i] = i + 1;
    }

    for(int i = 0; i < (m*n); i++) {
        mat_b[i] = i + 1;
    }

    for(int i = 0; i < (l*n); i++) {
        mat_c[i] = 0;
    }

    my_timer_t mt;
    my_timer_start(&mt);
    mm_dgemm(mat_a, mat_b, mat_c, l, m, n);
    my_timer_end(&mt);
    my_timer_print(&mt);
    printf("\n");

    print_mat(mat_a, 2, l, m);
    printf("\n");
    print_mat(mat_b, 2, m, n);
    printf("\n");
    print_mat(mat_c, 2, l, n);
    printf("\n");

    free(mat_a);
    free(mat_b);
    free(mat_c);
}

int main(void) {
    example_dgemm();
    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <float.h>
#define COMPEARTH_PRIVATE_DET3X3 1
#define COMPEARTH_PRIVATE_CROSS3 1
#define COMPEARTH_PRIVATE_NORM3 1
#define COMPEARTH_PRIVATE_GEM3 1
#define COMPEARTH_PRIVATE_GEMT3 1
#include "compearth.h"
#ifdef COMPEARTH_USE_MKL
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreserved-id-macro"
#pragma clang diagnostic ignored "-Wstrict-prototypes"
#endif
#include <mkl_lapacke.h>
//#include <mkl_cblas.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#else
#include <lapacke.h>
//#include <cblas.h>
#endif

#define LWORK 15

/*!
 * @brief Converts a nearly orthogonal matrix into a numerically orthogonal
 *        matrix.
 *
 * @param[in] n        Number of bases.
 * @param[in] itype    Orthogonalization strategy. \n
 *                       CE_ORTH_SVD orthgonalizes with the SVD. \n
 *                       CE_ORTH_TAPE2012 orthogalizes with Tape 2012c
 *                       Appendix E. \n
 *                       CE_ORTH_QUAT orthogonalizes with quaternions.
 * @param[in] Uin      [3 x 3 x n] set of bases where each [3 x 3] basis is
 *                     in column major order.
 *
 * @param[out] Uout    [3 x 3 x n] set of re-orthgonalized bases where
 *                     each [3 x 3] bais is in column major order.
 * @param[out] dtUin   If NULL then this will be ignored.  \n
 *                     Otherwise, this is an array of dimension [n] holding
 *                     the determinants of the input basis.
 * @param[out] dtUout  If NULL then this will be ignored.  \n
 *                     Otherwise, this is an array of dimension [n] holding
 *                     the determinants of the output basis.
 *
 * @result 0 indicates success.
 *
 * @author Carl Tape and converted to C by Ben Baker.
 *
 */
int compearth_Uorth(const int n,
                    const enum ceOrthoType_enum itype,
                    const double *__restrict__ Uin,
                    double *__restrict__ Uout,
                    double *__restrict__ dtUin,
                    double *__restrict__ dtUout)
{
    double Ut[9] __attribute__((aligned(64)));
    double U[9] __attribute__((aligned(64)));
    double Vt[9] __attribute__((aligned(64)));
    double work[LWORK], s[3], p[3], det, normb, normp;
    int i, ierr;
    bool lwantDetIn;
    const double tol = DBL_EPSILON*100.0;
    // Check inputs
    if (n < 1 || Uin == NULL || Uout == NULL)
    {
        if (n < 1){fprintf(stderr, "%s: No matrices\n", __func__);}
        if (Uin == NULL){fprintf(stderr, "%s: Uin is NULL\n", __func__);}
        if (Uout == NULL){fprintf(stderr, "%s: Uout is NULL\n", __func__);}
        return -1;
    }
    lwantDetIn = false;
    if (dtUin != NULL){lwantDetIn = true;}
    // Orthogonalize basis with SVD
    if (itype == CE_ORTH_SVD)
    {
        for (i=0; i<n; i++)
        {
            // This will frequently be called with matrices that may already
            // be sufficiently orthonormal.
            det = det3x3ColumnMajor(&Uin[9*i]);
            if (fabs(det - 1.0) > 1.e-12)
            {
                // Compute SVD
                memcpy(Ut, &Uin[9*i], 9*sizeof(double));
                ierr = LAPACKE_dgesvd_work(LAPACK_COL_MAJOR, 'A', 'A',
                                           3, 3, Ut, 3, s, U, 3,
                                           Vt, 3, work, LWORK);
                if (ierr != 0)
                {
                    fprintf(stderr, "%s: Error computing Ut\n", __func__);
                    return -1;
                }
                // Compute U*V' - note Vt is already computed by SVD
                gemm3_colMajorNoTransNoTrans(U, Vt, &Uout[9*i]);
                // N.B. I could get the determinant here from the singular
                // values but i'll wait
            }
            else
            {
                memcpy(&Uout[9*i], &Uin[9*i], 9*sizeof(double));
            }
            if (lwantDetIn){dtUin[i] = det;}
        }
    }
    // Orthgonalize with suggestion in TapeTape2012c Appendix E
    else if (itype == CE_ORTH_TAPE2012)
    {
        for (i=0; i<n; i++)
        {
            // This will frequently be called with matrices that may already
            // be sufficiently orthonormal.
            det = det3x3ColumnMajor(&Uin[9*i]);
            if (fabs(det - 1.0) > tol)
            {
                cross3(&Uin[9*i], &Uin[9*i+3], p);
                normb = norm3(&Uin[9*i+3]);
                normp = norm3(p);
                Uout[9*i]   = Uin[9*i];
                Uout[9*i+1] = Uin[9*i+1];
                Uout[9*i+2] = Uin[9*i+2];
                Uout[9*i+3] = Uin[9*i+3]/normb;
                Uout[9*i+4] = Uin[9*i+4]/normb;
                Uout[9*i+5] = Uin[9*i+5]/normb;
                Uout[9*i+6] = Uin[9*i+6]/normp;
                Uout[9*i+7] = Uin[9*i+7]/normp;
                Uout[9*i+8] = Uin[9*i+8]/normp;
            }
            else
            {
                memcpy(&Uout[9*i], &Uin[9*i], 9*sizeof(double));
            }
            if (lwantDetIn){dtUin[i] = det;}
        }
    }
    else if (itype == CE_ORTH_QUAT)
    {
        fprintf(stderr, "%s: Error quaternions not yet programmed\n",
                 __func__);
        return -1;
    }
    else if (itype == CE_NO_ORTH)
    {
        memcpy(Uout, Uin, 9*(size_t) n*sizeof(double));
        //cblas_dcopy(9*n, Uin, 1, Uout, 1);
    }
    else
    {
        fprintf(stderr, "%s: Only itype=1 is programmed\n", __func__);
        return -1;
    }
    //if (dtUin != NULL)
    //{
    //    for (i=0; i<n; i++)
    //    {
    //        dtUin[i] = det3x3ColumnMajor(&Uin[9*i]);
    //    }
    //}
    if (dtUout != NULL)
    {
        for (i=0; i<n; i++)
        {
            dtUout[i] = det3x3ColumnMajor(&Uout[9*i]);
        }
    }
    return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>

int
func_f (const gsl_vector * x, void *params, gsl_vector * f)
{
  double x1 = gsl_vector_get(x, 0);
  double x2 = gsl_vector_get(x, 1);

  gsl_vector_set(f, 0, 100.0 * (x2 - x1*x1));
  gsl_vector_set(f, 1, 1.0 - x1);

  return GSL_SUCCESS;
}

int
func_df (const gsl_vector * x, void *params, gsl_matrix * J)
{
  double x1 = gsl_vector_get(x, 0);

  gsl_matrix_set(J, 0, 0, -200.0*x1);
  gsl_matrix_set(J, 0, 1, 100.0);
  gsl_matrix_set(J, 1, 0, -1.0);
  gsl_matrix_set(J, 1, 1, 0.0);

  return GSL_SUCCESS;
}

int
func_fvv (const gsl_vector * x, const gsl_vector * v,
          void *params, gsl_vector * fvv)
{
  double v1 = gsl_vector_get(v, 0);

  gsl_vector_set(fvv, 0, -200.0 * v1 * v1);
  gsl_vector_set(fvv, 1, 0.0);

  return GSL_SUCCESS;
}

void
callback(const size_t iter, void *params,
         const gsl_multifit_nlinear_workspace *w)
{
  gsl_vector * x = gsl_multifit_nlinear_position(w);

  /* print out current location */
  printf("%f %f\n",
         gsl_vector_get(x, 0),
         gsl_vector_get(x, 1));
}

void
solve_system(gsl_vector *x0, gsl_multifit_nlinear_fdf *fdf,
             gsl_multifit_nlinear_parameters *params)
{
  const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
  const size_t max_iter = 200;
  const double xtol = 1.0e-8;
  const double gtol = 1.0e-8;
  const double ftol = 1.0e-8;
  const size_t n = fdf->n;
  const size_t p = fdf->p;
  gsl_multifit_nlinear_workspace *work =
    gsl_multifit_nlinear_alloc(T, params, n, p);
  gsl_vector * f = gsl_multifit_nlinear_residual(work);
  gsl_vector * x = gsl_multifit_nlinear_position(work);
  int info;
  double chisq0, chisq, rcond;

  /* initialize solver */
  gsl_multifit_nlinear_init(x0, fdf, work);

  /* store initial cost */
  gsl_blas_ddot(f, f, &chisq0);

  /* iterate until convergence */
  gsl_multifit_nlinear_driver(max_iter, xtol, gtol, ftol,
                              callback, NULL, &info, work);

  /* store final cost */
  gsl_blas_ddot(f, f, &chisq);

  /* store cond(J(x)) */
  gsl_multifit_nlinear_rcond(&rcond, work);

  /* print summary */

  fprintf(stderr, "NITER         = %zu\n", gsl_multifit_nlinear_niter(work));
  fprintf(stderr, "NFEV          = %zu\n", fdf->nevalf);
  fprintf(stderr, "NJEV          = %zu\n", fdf->nevaldf);
  fprintf(stderr, "NAEV          = %zu\n", fdf->nevalfvv);
  fprintf(stderr, "initial cost  = %.12e\n", chisq0);
  fprintf(stderr, "final cost    = %.12e\n", chisq);
  fprintf(stderr, "final x       = (%.12e, %.12e)\n",
          gsl_vector_get(x, 0), gsl_vector_get(x, 1));
  fprintf(stderr, "final cond(J) = %.12e\n", 1.0 / rcond);

  printf("\n\n");

  gsl_multifit_nlinear_free(work);
}

int
main (void)
{
  const size_t n = 2;
  const size_t p = 2;
  gsl_vector *f = gsl_vector_alloc(n);
  gsl_vector *x = gsl_vector_alloc(p);
  gsl_multifit_nlinear_fdf fdf;
  gsl_multifit_nlinear_parameters fdf_params =
    gsl_multifit_nlinear_default_parameters();

  /* print map of Phi(x1, x2) */
  {
    double x1, x2, chisq;
    double *f1 = gsl_vector_ptr(f, 0);
    double *f2 = gsl_vector_ptr(f, 1);

    for (x1 = -1.2; x1 < 1.3; x1 += 0.1)
      {
        for (x2 = -0.5; x2 < 2.1; x2 += 0.1)
          {
            gsl_vector_set(x, 0, x1);
            gsl_vector_set(x, 1, x2);
            func_f(x, NULL, f);

            chisq = (*f1) * (*f1) + (*f2) * (*f2);
            printf("%f %f %f\n", x1, x2, chisq);
          }
        printf("\n");
      }
    printf("\n\n");
  }

  /* define function to be minimized */
  fdf.f = func_f;
  fdf.df = func_df;
  fdf.fvv = func_fvv;
  fdf.n = n;
  fdf.p = p;
  fdf.params = NULL;

  /* starting point */
  gsl_vector_set(x, 0, -0.5);
  gsl_vector_set(x, 1, 1.75);

  fprintf(stderr, "=== Solving system without acceleration ===\n");
  fdf_params.trs = gsl_multifit_nlinear_trs_lm;
  solve_system(x, &fdf, &fdf_params);

  fprintf(stderr, "=== Solving system with acceleration ===\n");
  fdf_params.trs = gsl_multifit_nlinear_trs_lmaccel;
  solve_system(x, &fdf, &fdf_params);

  gsl_vector_free(f);
  gsl_vector_free(x);

  return 0;
}


#include "cones.h"
#ifdef LAPACK_LIB_FOUND
#include <cblas.h>
#include <lapacke.h>
#endif
void projectsdc(double * X, int n, Work * w);

/* in place projection (with branches) */
void projCone(double *x, Cone * k, Work * w)
{
	int i;
	int count;

	/* project onto positive orthant */
	for(i = k->f; i < k->f+k->l; ++i)
	{
		if(x[i] < 0.0) x[i] = 0.0;
		//x[i] = (x[i] < 0.0) ? 0.0 : x[i];
	}
	count = k->l+k->f;
	/* project onto SOC */
	for(i = 0; i < k->qsize; ++i)
	{
		double v1 = x[count];
		double s = calcNorm(&(x[count+1]),k->q[i]-1);
		double alpha = (s + v1)/2.0;

		if(s <= v1) { /* do nothing */ }
		else if (s <= - v1) {
			memset(&(x[count]), 0, k->q[i]*sizeof(double));
		} else {
			x[count] = alpha;
			scaleArray(&(x[count+1]), alpha/s, k->q[i]-1);
      //cblas_dscal(k->q[i]-1, alpha/s, &(x[count+1]),1);
		}
		count += k->q[i];
	}
#ifdef LAPACK_LIB_FOUND
	/* project onto PSD cone */
	for (i=0; i < k->ssize; ++i){
		projectsdc(&(x[count]),k->s[i],w);
		count += (k->s[i])*(k->s[i]);
	}
#else
  if(k->ssize > 0){
    coneOS_printf("WARNING: solving SDP, no lapack library specified in makefile!\n");
    coneOS_printf("ConeOS will return a wrong answer!\n");
}
#endif
	/* project onto OTHER cones */
}

#ifdef LAPACK_LIB_FOUND
void projectsdc(double *X, int n, Work * w)
{ /* project onto the positive semi-definite cone */
  if (n == 1) {
    if(X[0] < 0.0) X[0] = 0.0;
    return;
  }

  int i, j, m=0;
  double * Xs = w->Xs;
  double * Z = w->Z;
  double * e = w->e;
  memcpy(Xs,X,n*n*sizeof(double));

  // Xs = X + X', save div by 2 for eigen-recomp
  for (i = 0; i < n; ++i){
    cblas_daxpy(n, 1, &(X[i]), n, &(Xs[i*n]), 1);
    //b_daxpy(n, 1, &(X[i]), n, &(Xs[i*n]), 1);
  }

  double EIG_TOL = 1e-8;
  double vupper = calcNorm(Xs,n*n);
  LAPACKE_dsyevr( LAPACK_COL_MAJOR, 'V', 'V', 'U', n, Xs, n, 0.0, vupper, -1, -1, EIG_TOL, &m, e, Z, n , NULL);

  memset(X, 0, n*n*sizeof(double));
  for (i = 0; i < m; ++i) {
    cblas_dsyr(CblasColMajor, CblasLower, n, e[i]/2, &(Z[i*n]), 1, X, n);
    //b_dsyr('L', n, -e[i]/2, &(Z[i*n]), 1, Xs, n);
  }
  // fill in upper half
  for (i = 0; i < n; ++i){
    for (j = i+1; j < n; ++j){
      X[i + j*n] = X[j + i*n];
    }
  }
}
#endif


#include <viaio/Vlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_combination.h>

/* binomial coefficient, n choose k */
double binom (int n,int k)
{
  double prod,ix,nx,kx;
  int i;
  nx = (double)n;
  kx = (double)k;

  prod = ix = 1;
  for (i=1; i<=k; i++) {
    prod *= (nx-kx+ix)/ix;
    ix++;
  }
  return prod;
}

char **randomCombinations(long maxiter,int n,unsigned long int seed)
{
  size_t iter,i,j,k;
  size_t znum,maxnum=90000;

  /* ini random */
  fprintf(stderr," random shuffle\n");
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  /* ini data structs */
  int  *src = (int *) calloc(n,sizeof(int));
  char *tmp = (char *)calloc(n,sizeof(char));
  for (i=0; i<n; i++) src[i] = i;

  char  **table = (char **)calloc(maxiter,sizeof(char *));
  for (i=0; i<maxiter; i++)
    table[i] = (char *)calloc(n,sizeof(char));

  /* generate permutation table */
  for (iter=0; iter<maxiter; iter++) {
    znum = 0;

  skip:
    if (znum >= maxnum) VError(" znum %d",znum);
    k = n/2;
    if (n%2 == 1 && iter%2 == 0) k++;  /* for odd number of images */
    gsl_ran_shuffle (rx,src,n,sizeof(int));

    memset(tmp,'0',n*sizeof(char));
    for (j=0; j<k; j++) tmp[src[j]] = '1';

    /* check if already there */
    for (j=0; j<iter; j++) {
      if (memcmp(tmp,table[j],(size_t)n) == 0 && znum < maxnum) {
	znum++;
	goto skip;
      }
    }
    memcpy(table[iter],tmp,(size_t)n);
  }
  return table;
}

char **XXrandomCombinations(long maxiter,int n,unsigned long int seed)
{
  size_t iter,i,j;
  double p=0.5;

  /* ini random */
  gsl_rng_env_setup();
  const gsl_rng_type *T = gsl_rng_default;
  gsl_rng *rx = gsl_rng_alloc(T);
  gsl_rng_set(rx,(unsigned long int)seed);

  /* ini data structs */
  char  *tmp = (char *)calloc(n,sizeof(char));
  char  **table = (char **)calloc(maxiter,sizeof(char *));
  for (i=0; i<maxiter; i++)
    table[i] = (char *)calloc(n,sizeof(char));

  /* generate permutation table */
  iter = 0;
  while (iter < maxiter) {
    memset(tmp,'0',n*sizeof(char));

    int ic=0;
    for (j=0; j<n; j++) {
      if (gsl_ran_bernoulli(rx,p) > 0) {
	tmp[j] = '1';
	ic++;
      }
    }
    for (j=0; j<iter; j++) {  /* check if already there */
      if (memcmp(tmp,table[j],n) == 0) goto skip;
    }
    memcpy(table[iter],tmp,(size_t)n);
    iter++;
  skip: ;
  }
  return table;
}

char **allCombinations(long maxiter,int n,int iskip)
{
  size_t iter,i,j,num,b;

  /* ini data structs */
  char  *tmp = (char *)calloc(n,sizeof(char));
  char  **table = (char **)calloc(maxiter,sizeof(char *));
  for (i=0; i<maxiter; i++)
    table[i] = (char *)calloc(n,sizeof(char));

  size_t xmax = (size_t) gsl_pow_int(2.0,(int)n);

  iter = 0;
  for (i=0; i<xmax; i+=iskip) {
    /* decimal to binary */
    memset(tmp,'0',n*sizeof(char));
    j=0;
    num = i;
    while(num > 0) {
      b=num%2;
      if (b == 1) tmp[j] = '1';
      num=num/2;
      j++;
    }
    /* fprintf(stderr," iter: %5d  %s\n",iter,tmp); */
    memcpy(table[iter],tmp,(size_t)n);
    iter++;
    if (iter >= maxiter) return table;
  }
  return table;
}


#pragma once

#include <gsl/gsl>
#include <type_traits>

#include "Exponentials.h"
#include "HyperionUtils/Concepts.h"
#include "HyperionUtils/Macros.h"
#include "HyperionUtils/OptionAndResult.h"

namespace hyperion::math {
	using utils::None;
	using utils::Option;
	using utils::Some;
	using utils::concepts::FloatingPoint;
	/// @brief The possible kinds of value interpolation possible by `Interpolator`
	enum class InterpolationType
	{
		Linear = 0,
		Logarithmic,
		Exponential
	};

	IGNORE_PADDING_START
	/// @brief Interpolates from a starting value to a final value in the way prescribed by `Type`
	/// @note `T` must be a floating point type
	/// @note The start value must be in [0, target value] and the target value must be > 0
	///
	/// @tparam T - The floating point type to perform operations in
	/// @tparam Type - The type of interpolation
	template<FloatingPoint T, InterpolationType Type = InterpolationType::Linear>
	class Interpolator {
	  public:
		static constexpr T DEFAULT_TARGET_VALUE = gsl::narrow_cast<T>(1.0);
		static constexpr T DEFAULT_INITIAL_VALUE = gsl::narrow_cast<T>(0.0);
		static constexpr T DEFAULT_TRANSITION_LENGTH = gsl::narrow_cast<T>(0.1);
		static constexpr size_t DEFAULT_SAMPLE_RATE = 44100ULL;

		/// @brief Creates a default `Interpolator`
		constexpr Interpolator() noexcept = default;

		/// @brief Creates an `Interpolator` with the given target value
		///
		/// @param targetValue - The target final value to interpolate to
		constexpr explicit Interpolator(T targetValue) noexcept
			: m_target_value(targetValue),
			  mLinearTransitionStep(
				  (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds)),
			  m_samples_to_transition(get_samples_to_transition()) {
		}

		/// @brief Creates an `Interpolator` with the given target value and sample rate
		///
		/// @param targetValue - The target final value to interpolate to
		/// @param sampleRate - The sample rate to process at
		constexpr explicit Interpolator(T targetValue, size_t sampleRate) noexcept
			: m_sample_rate(sampleRate), m_target_value(targetValue),
			  mLinearTransitionStep(
				  (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds)),
			  m_samples_to_transition(get_samples_to_transition()) {
		}

		/// @brief Creates an `Interpolator` with the given target and initial values, and sample
		/// rate
		///
		/// @param targetValue - The target final value to interpolate to
		/// @param initial_value - The initial value to start interpolation from
		/// @param sampleRate - The sample rate to process at
		constexpr Interpolator(T targetValue, T initial_value, size_t sampleRate) noexcept
			: m_sample_rate(sampleRate), m_target_value(targetValue),
			  m_current_value(initial_value), m_initial_value(m_current_value),
			  mLinearTransitionStep(
				  (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds)),
			  m_samples_to_transition(get_samples_to_transition()) {
		}

		/// @brief Creates an `Interpolator` with the given target and initial values, time to
		/// interpolate over, and sample rate
		///
		/// @param targetValue - The target final value to interpolate to
		/// @param initial_value - The initial value to start interpolation from
		/// @param transitionLengthSeconds - The transition time to interpolate over
		/// @param sampleRate - The sample rate to process at
		constexpr Interpolator(T targetValue,
							   T initial_value,
							   T transitionLengthSeconds,
							   size_t sampleRate) noexcept
			: m_sample_rate(sampleRate), m_target_value(targetValue),
			  m_current_value(initial_value), m_initial_value(m_current_value),
			  m_transition_lengthSeconds(transitionLengthSeconds),
			  mLinearTransitionStep(
				  (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds)),
			  m_samples_to_transition(get_samples_to_transition()) {
		}
		constexpr Interpolator(const Interpolator& interpolator) noexcept = default;
		constexpr Interpolator(Interpolator&& interpolator) noexcept = default;
		~Interpolator() noexcept = default;

		/// @brief Gets the next value in the interpolation sequence
		/// If `currentSample` is given, gets the value in the sequence for the given sample index
		/// @note This is only available for `Interpolator`s of type
		/// `InterpolationType::Logarithmic` or `InterpolationType::Exponential`
		/// @param currentSample - The (optional) sample in the sequence to get the value for
		///
		/// @return - The interpolation value
		inline auto get_next_value(Option<size_t> currentSample = None()) noexcept -> T {
			if(currentSample.is_some()) {
				m_current_transition_sample = currentSample.unwrap();
			}

			if(m_current_transition_sample <= m_samples_to_transition) {
				m_current_value = interpolate(m_current_transition_sample);
				m_current_transition_sample++;
			}

			return m_current_value;
		}

		/// @brief Resets the `Interpolator`.
		/// - If `initial_value` is given, the `Interpolator` will start at the given one instead of
		/// `DEFAULT_INITIAL_VALUE`
		/// - If `transitionLengthSeconds` is also given, the `Interpolator` will interpolate over
		/// that length of time instead of the current one
		///
		/// @param initial_value - The initial value to start interpolation from
		/// @param transitionLengthSeconds - The transition time to interpolate over
		inline auto
		reset(Option<T> initial_value, Option<T> transitionLengthSeconds) noexcept -> void {
			if(initial_value.is_some()) {
				m_current_value = initial_value.unwrap();
				m_initial_value = m_current_value;
			}
			else {
				m_current_value = DEFAULT_INITIAL_VALUE;
				m_initial_value = m_current_value;
			}

			if(transitionLengthSeconds.is_some()) {
				m_transition_lengthSeconds = transitionLengthSeconds.unwrap();
			}

			mLinearTransitionStep
				= (m_target_value - m_current_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds);
			m_samples_to_transition = get_samples_to_transition();
			m_current_transition_sample = 0ULL;
		}

		/// @brief Sets the target value to interpolate to, to the given one
		///
		/// @param targetValue - The value to interpolate to
		constexpr inline auto set_target(T targetValue) noexcept -> void {
			m_target_value = targetValue;
			m_initial_value = m_current_value;
			mLinearTransitionStep
				= (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds);
			m_samples_to_transition = get_samples_to_transition();
		}

		/// @brief Sets the sample rate to use for interpolation
		///
		/// @param sampleRate - The sample rate to use for interpolation
		inline auto set_sample_rate(size_t sampleRate) noexcept -> void {
			m_sample_rate = sampleRate;
			mLinearTransitionStep
				= (m_target_value - m_initial_value)
				  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds);
			m_samples_to_transition = get_samples_to_transition();
			m_current_transition_sample = 0ULL;
		}

		constexpr auto
		operator=(const Interpolator& interpolator) noexcept -> Interpolator& = default;
		constexpr auto operator=(Interpolator&& interpolator) noexcept -> Interpolator& = default;

	  private:
		static constexpr T LOG_TIME_FACTOR = gsl::narrow_cast<T>(5.0);
		static constexpr T EXP_TIME_FACTOR = gsl::narrow_cast<T>(0.693);

		size_t m_sample_rate = DEFAULT_SAMPLE_RATE;
		T m_target_value = DEFAULT_TARGET_VALUE;
		T m_current_value = DEFAULT_INITIAL_VALUE;
		T m_initial_value = DEFAULT_INITIAL_VALUE;
		T m_transition_lengthSeconds = DEFAULT_TRANSITION_LENGTH;
		T mLinearTransitionStep
			= (m_target_value - m_initial_value)
			  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds);
		size_t m_samples_to_transition = get_samples_to_transition();
		size_t m_current_transition_sample = 0;

		/// @brief Gets the interpolated value for the given sample
		///
		/// @param sample - The sample to get the interpolated value for
		///
		/// @return The interpolated value
		inline auto interpolate(size_t sample) noexcept -> T {
			if constexpr(Type == InterpolationType::Linear) {
				return linear_interpolation(sample);
			}
			else if constexpr(Type == InterpolationType::Logarithmic) {
				return log_interpolation(sample);
			}
			else {
				return exp_interpolation(sample);
			}
		}

		inline auto linear_interpolation(size_t sample) noexcept -> T {
			return m_initial_value + (mLinearTransitionStep * gsl::narrow_cast<T>(sample));
		}

		/// @brief Performs "logarithmic" interpolation.
		/// In reality, this is a pseudo logarithmic shape:
		///
		/// y = transition_factor *
		/// [1 + (peak_factor - 1) * (-e^{1 - t / (transtionLength / time_factor) } ) ]
		///
		/// where t = sample / sampleRate
		///
		/// which has a  horizontal asymptote at  ~ the target value and more pronouced slope
		///
		/// @param sample - The sample to get the interpolation value for
		///
		/// @return - The interpolated value
		inline auto log_interpolation(size_t sample) noexcept -> T {
			return (m_initial_value - m_target_value)
					   * math::Exponentials::exp(-gsl::narrow_cast<T>(sample)
												 / (gsl::narrow_cast<T>(m_sample_rate)
													* m_transition_lengthSeconds / LOG_TIME_FACTOR))
				   + m_target_value;
		}

		/// @brief Performs exponential interpolation:
		///
		/// y = e^{t / (transitionLength / time_factor) } - 1
		///
		/// where t = sample / sampleRate
		///
		/// @param sample The sample to get the interpolation value for
		///
		/// @return - The interpolated value
		inline auto exp_interpolation(size_t sample) noexcept -> T {
			return (m_target_value - m_initial_value)
					   * (math::Exponentials::exp(
							  gsl::narrow_cast<T>(sample)
							  / (gsl::narrow_cast<T>(m_sample_rate) * m_transition_lengthSeconds
								 / EXP_TIME_FACTOR))
						  - gsl::narrow_cast<T>(1.0))
				   + m_initial_value;
		}

		/// @brief Gets the number of samples necessary to fully complete the interpolation sequence
		///
		/// @return The number of samples in the interpolation sequence
		inline auto get_samples_to_transition() noexcept -> size_t {
			return gsl::narrow_cast<size_t>(gsl::narrow_cast<T>(m_sample_rate)
											* m_transition_lengthSeconds);
		}
	};
	IGNORE_PADDING_STOP

} // namespace hyperion::math


/**
Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
    in the documentation and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
    from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/

#ifndef GEOMETRIC_TOOLS_MATH_VECTOR_H
#define GEOMETRIC_TOOLS_MATH_VECTOR_H

/**
* Includes
**/
#include <iostream>
#include <limits>
#include <cmath>
#include <cstring>
#include <geometric_tools/Misc/Helper.h>
#include <cblas.h>

namespace GeometricTools { namespace Math {

template<unsigned int ROWS, unsigned int COLS>
class Matrix;

/**
* General Vector Class
* Supports N-dimension vectors
**/
template<unsigned int N>
class Vector
{
protected:
    // values of vector
    double values_[N];
    // helper variable for initialization
    unsigned int index_;

    /**
    * Initialize Vector from double values
    * C++11 feature - variadic templates for multiple arguments
    **/
    template<typename... Args>
    void initialize(double h, Args&&... args)
    {
        if(index_>=N)
            index_=0;
        // add first argument to the vector data
        values_[index_] = h;
        index_++;
        // Initialize with rest of arguments
        initialize(std::forward<Args>(args)...);
    }

    /**
    * Initialize Vector from double values
    * C++11 feature - variadic templates for multiple arguments
    * Terminating Case - end of arguments
    **/
    template<typename... Args>
    void initialize()
    {
        index_ = 0;
    }

public:

    /**
    * Default Constructor
    * initialize all data to zero
    **/
    Vector()
    {
        memset(values_,0,N*sizeof(double));
    }

    /**
    * Copy Constructor
    * @param other - Vector to copy from
    **/
    Vector(const Vector& other)
    {
        memcpy(values_,other.values_,N*sizeof(double));
    }

    /**
    * Constructor
    * @param h - list of double values
    **/
    template<typename... Args>
    Vector(double h, Args&&... args)
    {
        index_ = 0;
        memset(values_,0,N*sizeof(double));
        // use recursive initialization
        initialize(h, args...);
    }

    /**
    * Overloading += operator
    * Addition of 2 Vectors
    * @param other - vector to perform addition with
    * @return Vector - self (as the result is saved there)
    **/
    Vector operator+=(const Vector& other)
    {
        cblas_daxpy(N, 1.0, &other.values_[0], 1, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading -= operator
    * Subtraction of 2 Vectors
    * @param other - vector to perform subtraction with
    * @return Vector - self (as the result is saved there)
    **/
    Vector operator-=(const Vector& other)
    {
        cblas_daxpy(N, -1.0, &other.values_[0], 1, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading += operator
    * Addition with scalar (double)
    * @param other - double to add with
    * @return Vector - self (result)
    **/
    Vector operator+=(const double& other)
    {
        Vector tmp;
        tmp.ones();
        cblas_daxpy(N, other, &tmp.values_[0], 1, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading -= operator
    * Subtraction of scalar (double)
    * @param other - double to substract
    * @return Vector - self (result)
    **/
    Vector operator-=(const double& other)
    {
        Vector tmp;
        tmp.ones();
        cblas_daxpy(N, -other, &tmp.values_[0], 1, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading *= operator
    * Multiplication with scalar (double)
    * @param other - double to multiply with
    * @return Vector - self (result)
    **/
    Vector operator*=(const double& other)
    {
        cblas_dscal(N, other, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading /= operator
    * Division with scalar (double) - if zero ignores division (returns self)
    * @param other - double to divide with
    * @return Vector - self (result)
    **/
    Vector operator/=(const double& other)
    {
        if(std::abs(other) < std::numeric_limits<double>::epsilon())
            return (*this);
        cblas_dscal(N, 1.0/other, &values_[0], 1);
        return *this;
    }

    /**
    * Overloading + operator
    * Addition of 2 Vectors
    * @return Vector - the result of the addition
    **/
    Vector operator+(const Vector& lh) const
    {
        Vector t = (*this);
        t += lh;
        return t;
    }

    /**
    * Overloading - operator
    * Subtraction of 2 Vectors
    * @return Vector - the result of the subtraction
    **/
    Vector operator-(const Vector& lh) const
    {
        Vector t = (*this);
        t -= lh;
        return t;
    }

    /**
    * Overloading * operator
    * Multiplication of 2 Vectors (Dot product)
    * @return double - the result of the multiplication
    **/
    double operator*(const Vector& lh) const
    {
        double s = 0.0;
        for(unsigned int i=0;i<N;i++)
            s += (*this)[i]*lh[i];
        return s;
    }

    /**
    * Overloading + operator
    * Addition with scalar (double)
    * @param other - double to add with
    * @return Vector - the result
    **/
    Vector operator+(const double& other) const
    {
        Vector tmp = (*this);
        tmp += other;
        return tmp;
    }

    /**
    * Overloading - operator
    * Subtraction of scalar (double)
    * @param other - double to subtract
    * @return Vector - the result
    **/
    Vector operator-(const double& other) const
    {
        Vector tmp = (*this);
        tmp -= other;
        return tmp;
    }

    /**
    * Overloading * operator
    * Multiplication with scalar (double)
    * @param other - double to multiply with
    * @return Vector - the result
    **/
    Vector operator*(const double& other) const
    {
        Vector tmp = (*this);
        tmp *= other;
        return tmp;
    }

    /**
    * Overloading / operator
    * Division with scalar (double) - if zero ignores division (returns self)
    * @param other - double to divide with
    * @return Vector - the result
    **/
    Vector operator/(const double& other) const
    {
        Vector tmp = (*this);
        tmp /= other;
        return tmp;
    }

    /**
    * Overloading minus (-) operator
    **/
    Vector operator-()
    {
        return -1*(*this);
    }

    /**
    * Overloading == operator
    * @param other - Vector to compare
    * @return bool
    **/
    bool operator==(const Vector& other) const
    {
        Vector tmp = (*this)-other;
        return (tmp.lengthSq()<std::numeric_limits<double>::epsilon());
    }

    /**
    * Overloading != operator
    * @param other - Vector to compare
    * @return bool
    **/
    bool operator!=(const Vector& other) const
    {
        Vector tmp = (*this)-other;
        return (tmp.lengthSq()>=std::numeric_limits<double>::epsilon());
    }

    // !-- THIS NEEDS TO BE REMOVED --! //
    /**
    * Get pointer to values array
    * @return pointer to array
    **/
    double* data()
    {
        return values_;
    }

    /**
    * Get Length of Vector
    * @return double - length
    **/
    double length() const
    {
        return cblas_dnrm2(N, &values_[0], 1);
    }

    /**
    * Set vector to 1s
    **/
    void ones()
    {
        memset(&values_[0], 1, N*sizeof(double));
    }

    /**
    * Get LengthSq of Vector
    * @return double - length squared
    **/
    double lengthSq() const
    {
        double s = cblas_dnrm2(N, &values_[0], 1);
        return s*s;
    }

    /**
    * Normalize Vector
    **/
    void normalize()
    {
        double l = length();
        if(l>std::numeric_limits<double>::epsilon())
            *this /= l;
    }

    /**
    * Normalized Vector
    **/
    Vector normalized()
    {
        Vector tmp = Vector(*this);
        tmp.normalize();
        return tmp;
    }

    /**
    * Overloading () operator
    * Access Vector Matlab-like
    * @param i - index to return
    * @return double - value of i-th element
    **/
    double& operator()(int i) //needs assert legal index
    {
        return values_[i];
    }

    /**
    * Overloading () operator - const version
    * Access Vector Matlab-like
    * @param i - index to return
    * @return double - value of i-th element
    **/
    double operator()(int i) const //needs assert legal index
    {
        return values_[i];
    }

    /**
    * Overloading [] operator
    * Access Vector Array-like
    * @param i - index to return
    * @return double - value of i-th element
    **/
    double& operator[](int i) //assert legal index
    {
        return values_[i];
    }

    /**
    * Overloading [] operator - const version
    * Access Vector Array-like
    * @param i - index to return
    * @return double - value of i-th element
    **/
    double operator[](int i) const //assert legal index
    {
        return values_[i];
    }

    /**
    * Get ith unit vector with size N
    * @param i
    * @return Vector - unit vector
    **/
    static Vector e(const unsigned int& i)
    {
        Vector k;
        k[i] = 1;
        return k;
    }

    /**
    * Destructor
    **/
    ~Vector(void)
    {
    }

    template<unsigned int ROWS, unsigned int COLS>
    friend class Matrix;
};

/**
* Cross Product
* Applies only to 3D Vectors
* @return Vector - the result of the cross product
**/
inline Vector<3> cross(const Vector<3>& rh, const Vector<3>& lh)
{
    return Vector<3>(rh[1]*lh[2]-rh[2]*lh[1], rh[2]*lh[0]-rh[0]*lh[2], rh[0]*lh[1]-rh[1]*lh[0]);
}

/**
* Projection of a to e
* @return Vector - the result of the cross product
**/
template <unsigned int N>
Vector<N> projection(const Vector<N>& a, const Vector<N>& e)
{
    return ((e*a)*e/(e*e));
}

/**
* Overloading << operator
* "print" vector to stream
* @param ostream - stream to print the vector to
* @param Vector - vector to print
**/
template <unsigned int N>
std::ostream& operator<<(std::ostream& os, const Vector<N>& obj)
{
    for(unsigned int i=0;i<N-1;i++)
        os<<obj[i]<<" ";
    os<<obj[N-1];
    return os;
}

/**
* Overloading >> operator
* read vector from stream
* @param istream - stream to read the vector from
* @param Vector - vector to read
**/
template <unsigned int N>
std::istream& operator>>(std::istream& in, Vector<N>& obj)
{
    for(unsigned int i=0;i<N;i++)
        in>>obj[i];
    return in;
}

/**
* Overloading / operator
* Division with scalar (double)
* Perform Vector / scalar (as opposed to Vector / scalar inside Class)
**/
template <unsigned int N>
Vector<N> operator/(const double& a, const Vector<N>& b)
{
    if(std::abs(a) < std::numeric_limits<double>::epsilon())
        return Vector<N>();
    Vector<N> tmp = Vector<N>(b);
    for(unsigned int i=0;i<N;i++) {
        if(std::abs(tmp[i]) < std::numeric_limits<double>::epsilon())
            return b;
        tmp[i] = a/tmp[i];
    }
    return tmp;
}

/**
* Overloading * operator
* Multiplication with scalar (double)
* Perform scalar * Vector (as opposed to Vector * scalar inside Class)
**/
template <unsigned int N>
Vector<N> operator*(const double& a, const Vector<N>& b)
{
    return b*a;
}

/**
* Typedefs for frequently used types
**/
typedef Vector<2> Vector2;
typedef Vector<3> Vector3;
typedef Vector<4> Vector4;

} }

#endif


/* **************************************** *
 *		ODE integration functions			*
 *	Provides GSL ode integrator and			*
 *	a simple euler integrator				*
 * **************************************** */

#include "canonical.h"
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#define DIM 2
#define NPTS 100.0 // number of points to print out, best as double notation

/* Many good algorithms don't need the Jacobian, so we'll just pass a null pointer */
double *jac = NULL;

void
gslode(void *params, double MAX_TIME, FILE *theory)
{
	PAR pars = *(PAR *)params;

	/* Create our ODE system */
	gsl_odeiv_system sys = {func, jac, DIM, params};

	/* Range of time to simulate*/
	double t = 0.0, t1 = MAX_TIME;
	/* Initial step size, will be modified as needed by adaptive alogorithm */
	double h = 1e-6;
	/* initial conditions */
	double y[DIM] = { pars.Xo, 0.0 };

	/* Define method as Embedded Runge-Kutta Prince-Dormand (8,9) method */
	const gsl_odeiv_step_type * T
	 = gsl_odeiv_step_rk4;
//	 = gsl_odeiv_step_rk8pd;
	/* allocate stepper for our method of correct dimension*/
	gsl_odeiv_step * s
	 = gsl_odeiv_step_alloc (T, DIM);
	/* control will maintain absolute and relative error */
	gsl_odeiv_control * c
	 = gsl_odeiv_control_y_new (1e-6, 0.0);
	/* allocate the evolver */
	gsl_odeiv_evolve * e
	 = gsl_odeiv_evolve_alloc (DIM);

	/*dummy to make easy to switch to regular printing */
	double ti = t1;
	int i;

	/* Uncomment the outer for loop to print *
	 * 100 sample pts at regular intervals   */
	for (i = 1; i <= NPTS; i++){   ti = i * t1 / NPTS;
		while (t < ti)
		{
			int status = gsl_odeiv_evolve_apply (e, c, s,
												&sys,
												&t, t1,
												&h, y);
			if (status != GSL_SUCCESS)
			   break;
//			fprintf(theory,"%.6e %.6e %.6e\n",t,y[0],y[1]); //adaptive mesh
		}
		fprintf(theory,"%.6e %.6e %.6e\n",t,y[0],y[1]);  }

	gsl_odeiv_evolve_free (e);
	gsl_odeiv_control_free (c);
	gsl_odeiv_step_free (s);
}

void euler(void *params, double MAX_TIME, FILE *theory){
	PAR pars = *(PAR *)params;

	double dt = .01;
	int sample;

	double y[DIM] = { pars.Xo, 0.0 };
	double f[DIM];
	double t=0.0;

	for(sample = 0; sample < MAX_TIME/dt; sample++){

		if( sample % (int) (1/dt) == 0)
		{
			fprintf
			(
				theory, "%.6e %.6e %.6e\n",
				dt*sample,
				y[0],
				y[1]
			);
		}
		int status = func(t, y, f, &pars);
		if(status != GSL_SUCCESS)
			break;
		y[0] = y[0] + dt*f[0];
		y[1] = y[1] + dt*f[1];
	}

}


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

int
main (void)
{
  int i;
  /* coefficients of P(x) =  -1 + x^5  */
  double a[6] = { -1, 0, 0, 0, 0, 1 };
  double z[10];

  gsl_poly_complex_workspace * w
      = gsl_poly_complex_workspace_alloc (6);

  gsl_poly_complex_solve (a, 6, w, z);

  gsl_poly_complex_workspace_free (w);

  for (i = 0; i < 5; i++)
    {
      printf ("z%d = %+.18f %+.18f\n",
              i, z[2*i], z[2*i+1]);
    }

  return 0;
}


#include <math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>

#define max(A, B) ((A) > (B) ? (A) : (B))
#define min(A, B) ((A) < (B) ? (A) : (B))

double R0(double flock_size, double housing_period, double prob_dam_to_lamb, int maxage, double *params)
{
    int n_age_classes = (maxage+1)*12;
    int c, a, i, y, t;
    int infectious_period, years_infectious, period_infectious_to_lambs;

    int latent_period = lrint(params[5]);
    double beta_field = params[0];
    double beta_housed = params[1];

    double p_mort, p_cull, sum, lambs_per_dam;
    double removal_rate[n_age_classes], infection_rate;
    double cohort_size_age_0;
    double cohort_size[maxage+1][n_age_classes];
    double infecteds[maxage+1][n_age_classes];
    double R0[n_age_classes][n_age_classes];

    // age-specific mortality and cull probabilities from Andrew's data
    double prob_mortality[] = {
        0.01,
        0.012,
        0.027,
        0.041,
        0.019,
        0.021,
        0.034,
    };

    double prob_culled[] = {
        0,
        0.046,
        0.065,
        0.093,
        0.134,
        0.196,
        0.295,
    };

    // create vector of removal rates of a cohort from ages 0 to n_age_classes
    for (a = 0; a < n_age_classes; a++) {
        p_mort = prob_mortality[a/12];
        p_cull = prob_culled[a/12];
        removal_rate[a] = -(log(1.-p_mort)+log(1.-p_cull))/12.;
    }

    // these two ways are equivalent
    // create vector of infection rates within a year starting in April
    infection_rate = ((12-housing_period)*beta_field+housing_period*beta_housed)/12.;
    // for (a = 0; a < 12; a++)
        // if (a < 12-housing_period)
        //     infection_rate[a] = beta_field;
        // else
        //     infection_rate[a] = beta_housed;

    // find intial cohort size in a non-infected flock
    // take initial cohort size as 1
    // find size of cohort as it ages
    cohort_size[0][0] = 1;
    for (a = 1; a < n_age_classes; a++)
        cohort_size[0][a] = cohort_size[0][a-1]*exp(-removal_rate[a]);
    // sum size of all existent cohorts in April
    sum = 0;
    for (y = 0; y <= maxage; y++)
        sum += cohort_size[0][12*y];
    cohort_size_age_0 = flock_size/sum;

    // for cohorts of y years old at first exposure
    // assuming that the infectious ewe is starts to be infectious in April - but
    // this is not necessary, just convenient
    for (y = 0; y <= maxage; y++) {
        // cohort size at age 0
        cohort_size[y][0] = cohort_size_age_0;

        // just removal for first y years before exposure
        for (a = 1; a < 12*y; a++)
            cohort_size[y][a] = cohort_size[y][a-1]*exp(-removal_rate[a]);
        // then removal and infection for rest of lifespan
        for (; a < n_age_classes; a++)
            cohort_size[y][a] = cohort_size[y][a-1]*exp(-removal_rate[a]-infection_rate);

        // calculate number of infecteds
        infecteds[y][0] = 0;
        for (a = 1; a < n_age_classes; a++)
            infecteds[y][a] = infection_rate/(removal_rate[a]+infection_rate)*(cohort_size[y][a-1]-cohort_size[y][a]);
    }

    // set matrix elements to zero
    for (a = 0; a < n_age_classes; a++)
        for (i = 0; i < n_age_classes; i++)
            R0[a][i] = 0;

    // for a ewe infected at age a, sum the number of ewes it infects of age i
    for (a = 0; a < n_age_classes; a++) {
        infectious_period = max(0, n_age_classes-a-latent_period);
        for (t = 0; t < infectious_period; t++) {
            // for all cohorts from past to future
            for (y = maxage; y >= -maxage; y--) {
                // get cohort
                // +ve y are cohorts in the past
                // -ve y are future cohorts (identical to cohort 0)
                c = max(0, y);
                // age of cohort y at time t
                i = 12*y+t;
                if (0 <= i && i < n_age_classes)
                    R0[a][i] += infecteds[c][i];
            }
        }
    }

    // contribution of vertical transmission to an infectious ewe's lambs
    lambs_per_dam = cohort_size_age_0/(flock_size-cohort_size_age_0);

    for (a = 0; a < n_age_classes; a++) {
        // a ewe infected at age a is infectious for max_age-a-1-latent_period months
        infectious_period = max(0, n_age_classes-a-latent_period);
        if (a < 24)
            // only 2 year olds and older give birth
            // so ewes infectious before 24 months cannot infect their lambs
            // until they are 2 years old
            period_infectious_to_lambs = max(0, n_age_classes-24-latent_period);
        else
            period_infectious_to_lambs = infectious_period;
        years_infectious = period_infectious_to_lambs/12;
        R0[a][0] += lambs_per_dam*prob_dam_to_lamb*years_infectious;
    }

    gsl_matrix *A = gsl_matrix_alloc(n_age_classes, n_age_classes);
    gsl_vector_complex *v = gsl_vector_complex_alloc(n_age_classes);
    gsl_eigen_nonsymm_workspace *w = gsl_eigen_nonsymm_alloc(n_age_classes);
    for (a = 0; a < n_age_classes; a++)
        for (i = 0; i < n_age_classes; i++)
            gsl_matrix_set(A, a, i, R0[a][i]);
    gsl_eigen_nonsymm(A, v, w);
    double max_eigenvalue = 0, e;
    for (a = 0; a < n_age_classes; a++) {
        e = GSL_REAL(gsl_vector_complex_get(v, a));
        if (e > max_eigenvalue)
            max_eigenvalue = e;
    }
    gsl_eigen_nonsymm_free(w);
    gsl_vector_complex_free(v);
    gsl_matrix_free(A);

    return max_eigenvalue;
}


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

#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_multifit_nlin.h>

/* Compute the covariance matrix

   cov = inv (J^T J)

   by QRP^T decomposition of J
*/

int
gsl_multifit_covar (const gsl_matrix * J, double epsrel, gsl_matrix * covar)
{
  double tolr;

  size_t i, j, k;
  size_t kmax = 0;

  gsl_matrix * r;
  gsl_vector * tau;
  gsl_vector * norm;
  gsl_permutation * perm;

  size_t m = J->size1, n = J->size2 ;

  if (m < n)
    {
      GSL_ERROR ("Jacobian be rectangular M x N with M >= N", GSL_EBADLEN);
    }

  if (covar->size1 != covar->size2 || covar->size1 != n)
    {
      GSL_ERROR ("covariance matrix must be square and match second dimension of jacobian", GSL_EBADLEN);
    }

  r = gsl_matrix_alloc (m, n);
  tau = gsl_vector_alloc (n);
  perm = gsl_permutation_alloc (n) ;
  norm = gsl_vector_alloc (n) ;

  {
    int signum = 0;
    gsl_matrix_memcpy (r, J);
    gsl_linalg_QRPT_decomp (r, tau, perm, &signum, norm);
  }

  /* Form the inverse of R in the full upper triangle of R */

  tolr = epsrel * fabs(gsl_matrix_get(r, 0, 0));

  for (k = 0 ; k < n ; k++)
    {
      double rkk = gsl_matrix_get(r, k, k);

      if (fabs(rkk) <= tolr)
        {
          break;
        }

      gsl_matrix_set(r, k, k, 1.0/rkk);

      for (j = 0; j < k ; j++)
        {
          double t = gsl_matrix_get(r, j, k) / rkk;
          gsl_matrix_set (r, j, k, 0.0);

          for (i = 0; i <= j; i++)
            {
              double rik = gsl_matrix_get (r, i, k);
              double rij = gsl_matrix_get (r, i, j);

              gsl_matrix_set (r, i, k, rik - t * rij);
            }
        }
      kmax = k;
    }

  /* Form the full upper triangle of the inverse of R^T R in the full
     upper triangle of R */

  for (k = 0; k <= kmax ; k++)
    {
      for (j = 0; j < k; j++)
        {
          double rjk = gsl_matrix_get (r, j, k);

          for (i = 0; i <= j ; i++)
            {
              double rij = gsl_matrix_get (r, i, j);
              double rik = gsl_matrix_get (r, i, k);

              gsl_matrix_set (r, i, j, rij + rjk * rik);
            }
        }

      {
        double t = gsl_matrix_get (r, k, k);

        for (i = 0; i <= k; i++)
          {
            double rik = gsl_matrix_get (r, i, k);

            gsl_matrix_set (r, i, k, t * rik);
          };
      }
    }

  /* Form the full lower triangle of the covariance matrix in the
     strict lower triangle of R and in w */

  for (j = 0 ; j < n ; j++)
    {
      size_t pj = gsl_permutation_get (perm, j);

      for (i = 0; i <= j; i++)
        {
          size_t pi = gsl_permutation_get (perm, i);

          double rij;

          if (j > kmax)
            {
              gsl_matrix_set (r, i, j, 0.0);
              rij = 0.0 ;
            }
          else
            {
              rij = gsl_matrix_get (r, i, j);
            }

          if (pi > pj)
            {
              gsl_matrix_set (r, pi, pj, rij);
            }
          else if (pi < pj)
            {
              gsl_matrix_set (r, pj, pi, rij);
            }

        }

      {
        double rjj = gsl_matrix_get (r, j, j);
        gsl_matrix_set (covar, pj, pj, rjj);
      }
    }

  /* symmetrize the covariance matrix */

  for (j = 0 ; j < n ; j++)
    {
      for (i = 0; i < j ; i++)
        {
          double rji = gsl_matrix_get (r, j, i);

          gsl_matrix_set (covar, j, i, rji);
          gsl_matrix_set (covar, i, j, rji);
        }
    }

  gsl_matrix_free (r);
  gsl_permutation_free (perm);
  gsl_vector_free (tau);
  gsl_vector_free (norm);

  return GSL_SUCCESS;
}


#include <Python.h>
#include <arrayobject.h>
#include <time.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_erf.h>

static PyObject* g_engineError;
static gsl_rng* g_rng;

/** low level helpers **/

/** numpy helpers **/

/**
 *  Check that PyArrayObject is a double (Float) type and a vector
 * 	return 1 if an error and raise exception.
 */
static int not_doublevector(PyArrayObject *vec)  {
	if (vec->descr->type_num != NPY_DOUBLE || vec->nd != 1)  {
		PyErr_SetString(PyExc_ValueError,
			"In not_doublevector: array must be of type Float and 1 dimensional (n).");
		return 1;
	}
	return 0;
}

static double lognormal(double m, double stddev) {
/*
m: mean of x
stddev: std of x
*/
	return gsl_ran_lognormal(g_rng,
			log(m) - 0.5 * log(1.0 + (stddev*stddev)/(m*m)),
			sqrt(log((stddev*stddev)/(m*m) + 1)) );
}

/** python callables **/

static PyObject* engine_bivariate_gaussian(PyObject *self, PyObject *args) {
	float sx, sy;
	if (!PyArg_ParseTuple(args, "ff", &sx, &sy))
        return NULL;
	double x, y;
	gsl_ran_bivariate_gaussian(g_rng, sx, sy, 0.0, &x, &y);
	return Py_BuildValue("(ff)", (float)x, (float)y);
}

static PyObject* engine_beta(PyObject *self, PyObject *args) {
	float a = 0;
	float b = 0;
	if (!PyArg_ParseTuple(args, "ff", &a, &b))
        return NULL;
	double sample = (float)gsl_ran_beta(g_rng, a, b);
	return Py_BuildValue("f", (float)sample);
}

static PyObject* engine_lognormal(PyObject *self, PyObject *args) {
	float m = 0;
	float stddev = 0;
	if (!PyArg_ParseTuple(args, "ff", &m, &stddev))
        return NULL;
	return Py_BuildValue("f", (float)lognormal(m, stddev));
}

static PyObject* engine_lognormal_cdf(PyObject *self, PyObject *args) {
	float m = 0;
	float stddev = 0;
	float x = 0;
	if (!PyArg_ParseTuple(args, "fff", &x, &m, &stddev))
        return NULL;
	double m2 = log(m) - 0.5 * log(1.0 + (stddev*stddev)/(m*m));
	double s2 = sqrt(log((stddev*stddev)/(m*m) + 1));
	return Py_BuildValue("f", (float)gsl_cdf_lognormal_P(x, m2, s2));
}

static PyObject* engine_lognormal_cdf_inv(PyObject *self, PyObject *args) {
	float m = 0;
	float stddev = 0;
	float p = 0;
	if (!PyArg_ParseTuple(args, "fff", &p, &m, &stddev))
		return NULL;
	double m2 = log(m) - 0.5 * log(1.0 + (stddev*stddev)/(m*m));
	double s2 = sqrt(log((stddev*stddev)/(m*m) + 1));
	return Py_BuildValue("f", (float)gsl_cdf_lognormal_Pinv(p, m2, s2));
}

static PyObject* engine_percentileofscore(PyObject *self, PyObject *args) {
	// python: engine.percentileofscore([scores], score)
	double score = 0;
	PyObject* listObj;
	if (!PyArg_ParseTuple(args, "O!d", &PyList_Type, &listObj, &score))
        return NULL;

	// loop through python list of doubles, counting scores less than or equal to score
	int arrsz = PyList_Size(listObj);
	if (arrsz == 0)
		return NULL;
	int i;
	int count=0;
	for (i=0; i<arrsz; i++) {
		double aScore = PyFloat_AsDouble(PyList_GetItem(listObj, i));
		if (aScore <= score)
			count++;
	}
	float percentile = count / (float)arrsz;
	return Py_BuildValue("f", percentile);
}

static PyObject* engine_percentileofscore_array(PyObject *self, PyObject *args) {
	// python: engine.percentileofscore(numpy.array[scores], score)
	double score = 0;
	PyArrayObject* arrayObj;
	if (!PyArg_ParseTuple(args, "O!d", &PyArray_Type, &arrayObj, &score))
        return NULL;
	if (not_doublevector(arrayObj)) {
		printf("invalid type!\n");
		return NULL;
	}

	// loop through python list of doubles, counting scores less than or equal to score
	double* cin = (double *)arrayObj->data;
	int arrsz = arrayObj->dimensions[0];
	if (arrsz == 0)
		return NULL;
	int i;
	int count=0;
	for (i=0; i<arrsz; i++) {
		double aScore = cin[i];
		printf("score: %lf\n", aScore);
		if (aScore <= score)
			count++;
	}
	float percentile = count / (float)arrsz;
	return Py_BuildValue("f", percentile);
}

static PyObject* engine_erf(PyObject *self, PyObject *args) {
	float x = 0;
	if (!PyArg_ParseTuple(args, "f", &x))
        return NULL;
	double sample = gsl_sf_erf(x);
	return Py_BuildValue("f", (float)sample);
}

static PyObject* engine_poisson(PyObject *self, PyObject *args) {
	float mean = 0;
	if (!PyArg_ParseTuple(args, "f", &mean))
        return NULL;
	unsigned int sample = gsl_ran_poisson(g_rng, mean);
	return Py_BuildValue("i", (int)sample);
}

static PyObject* engine_seed(PyObject *self, PyObject *args) {
	long seed = 0;
	if (!PyArg_ParseTuple(args, "l", &seed))
        return NULL;
	gsl_rng_set(g_rng, seed);
	Py_INCREF(Py_None);
	return Py_None;
}

/** debris module **/

static PyObject* debris_generate_item(PyObject* self, PyObject* args) {
	int typeid;
	float V, xord, yord, flighttime_mean, flighttime_stddev, cdav, mass_mean, mass_stddev, fa_mean, fa_stddev;
	if (!PyArg_ParseTuple(args, "fffffifffff",
			&V,
			&xord, &yord,
			&flighttime_mean,
			&flighttime_stddev,
			&typeid,
			&cdav,
			&mass_mean, &mass_stddev,
			&fa_mean, &fa_stddev))
		return NULL;

	double mass = lognormal(mass_mean, mass_stddev);
	double fa = lognormal(fa_mean, fa_stddev);
	double flight_time = lognormal(flighttime_mean, flighttime_stddev);

	double t = 9.81 * flight_time / V;
	double k = (1.2 * V * V) / (2 * 9.81 * (mass/fa) );
	double flight_distance=0;

	switch (typeid) {
	case 0:
		flight_distance = (V*V/9.81) * (1.0/k) * (0.2060*pow(k*t, 2) + 0.011*k*t);
		break;
	case 1:
		flight_distance = (V*V/9.81) * (1.0/k) * (0.072*pow(k*t, 2) + 0.3456*k*t);
		break;
	case 2:
		flight_distance = (V*V/9.81) * (1.0/k) * (0.0723*pow(k*t, 2) + 0.2376*k*t);
		break;
	}

	// Sample Impact Location
	double sigma_x = flight_distance / 3.0;
	double sigma_y = flight_distance / 12.0;
	double x, y;
	gsl_ran_bivariate_gaussian(g_rng, sigma_x, sigma_y, 1.0, &x, &y);

	// translate to footprint coords
	double item_X = xord - flight_distance + x;
	double item_Y = yord + y;

	// calculate momentum
	double pa = 1.2;
	double b = sqrt( (pa * cdav * fa) / (mass) );
	double beta_mean = 1 - exp(-b * sqrt(flight_distance));

    double dispersion = 0;
    if (beta_mean == 1.0) {
    	dispersion = 1/beta_mean + 3.0;
    	beta_mean -= 0.001;
    }
    else if (beta_mean == 0) {
    	dispersion = 4.0;
    }
    else {
        dispersion = fmax(1.0/beta_mean, 1.0/(1.0 - beta_mean)) + 3.0;
    }

    double beta_a = beta_mean * dispersion;
    double beta_b = dispersion * (1 - beta_mean);

    double sampled = gsl_ran_beta(g_rng, beta_a, beta_b);
    float item_momentum = (float)(sampled * V * mass);

	/* output: (item_momentum, item_X, item_Y)
	 */
	return Py_BuildValue("(fff)", item_momentum, item_X, item_Y);
}

/** Module setup **/

static PyMethodDef EngineMethods[] = {
	{"beta", engine_beta, METH_VARARGS, "Draw from Beta using given a,b"},
	{"lognormal", engine_lognormal, METH_VARARGS, "Draw from Lognormal using given mean, cov"},
	{"lognormal_cdf", engine_lognormal_cdf, METH_VARARGS, "lognormal cdf"},
	{"lognormal_cdf_inv", engine_lognormal_cdf_inv, METH_VARARGS, "lognormal inv cdf"},
	{"erf", engine_erf, METH_VARARGS, "erf"},
	{"bivariate_gaussian", engine_bivariate_gaussian, METH_VARARGS, "Bivariate normal distribution"},
	{"poisson", engine_poisson, METH_VARARGS, "Draw from Poisson using given mean"},
	{"percentileofscore", engine_percentileofscore, METH_VARARGS, "Return a quantile value of sorted_data"},
	{"percentileofscore_array", engine_percentileofscore_array, METH_VARARGS, "Return a quantile value of sorted_data"},
	{"seed", engine_seed, METH_VARARGS, "Set random number seed"},
	{"debris_generate_item", debris_generate_item, METH_VARARGS, "Generate debris item for source"},
	{NULL, NULL, 0, NULL}
};

PyMODINIT_FUNC initengine(void) {
	PyObject* m = Py_InitModule("engine", EngineMethods);
	if (m == NULL)
		return;
	import_array();

	g_engineError = PyErr_NewException("engine.error", NULL, NULL);
	Py_INCREF(g_engineError);
	PyModule_AddObject(m, "error", g_engineError);

	// setup GSL random number generators
	const gsl_rng_type* T;
	gsl_rng_env_setup();
	T = gsl_rng_default;
	g_rng = gsl_rng_alloc(T);
	gsl_rng_set(g_rng, (long)time(NULL));
}


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

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

/*-*-*-*-*-*-*-*-*-*-*-* Allocators *-*-*-*-*-*-*-*-*-*-*-*/

gsl_cheb_series *
gsl_cheb_alloc(const size_t order)
{
  gsl_cheb_series * cs = (gsl_cheb_series *) malloc(sizeof(gsl_cheb_series));

  if(cs == 0) {
    GSL_ERROR_VAL("failed to allocate gsl_cheb_series struct", GSL_ENOMEM, 0);
  }

  cs->order    = order;
  cs->order_sp = order;

  cs->c = (double *) malloc((order+1) * sizeof(double));

  if(cs->c == 0) {
    GSL_ERROR_VAL("failed to allocate cheb coefficients", GSL_ENOMEM, 0);
  }

  cs->f = (double *) malloc((order+1) * sizeof(double));

  if(cs->f == 0) {
    GSL_ERROR_VAL("failed to allocate cheb function space", GSL_ENOMEM, 0);
  }

  return cs;
}

void gsl_cheb_free(gsl_cheb_series * cs)
{
  RETURN_IF_NULL (cs);
  free(cs->f);
  free(cs->c);
  free(cs);
}

/*-*-*-*-*-*-*-*-*-*-*-* Initializer *-*-*-*-*-*-*-*-*-*-*-*/

int gsl_cheb_init(gsl_cheb_series * cs, const gsl_function *func,
                  const double a, const double b)
{
  size_t k, j;

  if(a >= b) {
    GSL_ERROR_VAL("null function interval [a,b]", GSL_EDOM, 0);
  }
  cs->a = a;
  cs->b = b;
  /* cs->err = 0.0; */

  {
    double bma = 0.5 * (cs->b - cs->a);
    double bpa = 0.5 * (cs->b + cs->a);
    double fac = 2.0/(cs->order +1.0);

    for(k = 0; k<=cs->order; k++) {
      double y = cos(M_PI * (k+0.5)/(cs->order+1));
      cs->f[k] = GSL_FN_EVAL(func, (y*bma + bpa));
    }

    for(j = 0; j<=cs->order; j++) {
      double sum = 0.0;
      for(k = 0; k<=cs->order; k++)
        sum += cs->f[k]*cos(M_PI * j*(k+0.5)/(cs->order+1));
      cs->c[j] = fac * sum;
    }

  }
  return GSL_SUCCESS;
}

size_t
gsl_cheb_order (const gsl_cheb_series * cs)
{
  return cs->order;
}

size_t
gsl_cheb_size (const gsl_cheb_series * cs)
{
  return (cs->order + 1);
}

double *
gsl_cheb_coeffs (const gsl_cheb_series * cs)
{
  return cs->c;
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv2.h>

#include "cosmocalc.h"

#define ODEGROWTH //the integral formulation is onyl correct for LCDM, any wCDM cosmology will not work

#ifdef ODEGROWTH

#define LNAINITGROWTH (log(AEXPN_MIN_GROWTH))
#define ABSERR 1e-8
#define RELERR 1e-8
#define HSTART 1e-4
#define ODE_TYPE gsl_odeiv2_step_rk8pd

static int growth_ode_sys_w0wa(double t, const double y[], double dydt[], void *params);

//ODE for growth from Komatsu et al. 2009, ApJS, 180, 330 (WMAP5 paper)
static int growth_ode_sys_w0wa(double t, const double y[], double dydt[], void *params)
{
  double a = exp(t); //t = log(a)
  double weffa = weff(a);
  double ha = hubble_noscale(a);
  double omegaDEa = cosmoData.OmegaL/ha/ha/pow(a,3.0*(1.0 + weffa));
  double omegaKa = cosmoData.OmegaK/a/a/ha/ha;

  dydt[0] = y[1];
  dydt[1] = (1.5*weffa*omegaDEa - 0.5*omegaKa - 2.5)*y[1] - (2.0*omegaKa + 1.5*(1.0 - weffa)*omegaDEa)*y[0];

  return GSL_SUCCESS;
}

double growth_function_exact(double a)
{
  gsl_odeiv2_system odesys = {growth_ode_sys_w0wa, NULL, 2, NULL};
  gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(&odesys,ODE_TYPE,HSTART,ABSERR,RELERR);

  double lna_init;
  double lna_final,y[2];
  double ga,gnorm;
  int status;

  //do g(a) unormalized
  y[0] = 1.0;
  y[1] = 0.0;
  lna_init = LNAINITGROWTH;
  lna_final = log(a);

  status = gsl_odeiv2_driver_apply(d,&lna_init,lna_final,y);
  ga = y[0]*a;
  if(status != GSL_SUCCESS)
    {
      fprintf(stderr,"error in integrating growth function! a = %lf\n",a);
      assert(status == GSL_SUCCESS);
    }

  //do g(a = 1.0) to get normalization
  y[0] = 1.0;
  y[1] = 0.0;
  lna_init = LNAINITGROWTH;
  lna_final = 0.0;

  status = gsl_odeiv2_driver_apply(d,&lna_init,lna_final,y);
  gnorm = y[0];
  if(status != GSL_SUCCESS)
    {
      fprintf(stderr,"error in integrating growth function! a = %lf\n",a);
      assert(status == GSL_SUCCESS);
    }

  gsl_odeiv2_driver_free(d);

  return ga/gnorm;
}

double growth_function_exact_nonorm(double a)
{
  gsl_odeiv2_system odesys = {growth_ode_sys_w0wa, NULL, 2, NULL};
  gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(&odesys,ODE_TYPE,HSTART,ABSERR,RELERR);

  double lna_init;
  double lna_final,y[2];
  double ga;
  int status;

  //do g(a) unormalized
  y[0] = 1.0;
  y[1] = 0.0;
  lna_init = LNAINITGROWTH;
  lna_final = log(a);

  status = gsl_odeiv2_driver_apply(d,&lna_init,lna_final,y);
  ga = y[0]*a;
  if(status != GSL_SUCCESS)
    {
      fprintf(stderr,"error in integrating growth function! a = %lf\n",a);
      assert(status == GSL_SUCCESS);
    }

  return ga;
}

double growth_function(double a)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static double growth_function_norm;
  static gsl_spline *cosmocalc_growth_function_spline = NULL;
  static gsl_interp_accel *cosmocalc_growth_function_acc = NULL;

  double growth_function_table[COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH];
  double a_table[COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH];
  long i;

  gsl_odeiv2_system odesys = {growth_ode_sys_w0wa, NULL, 2, NULL};
  gsl_odeiv2_driver *d;
  double afact,lna_init;
  double lna_final,y[2];
  int status;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      //init ODE integrator
      d = gsl_odeiv2_driver_alloc_y_new(&odesys,ODE_TYPE,HSTART,ABSERR,RELERR);

      for(i=0;i<COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH;++i)
        {
	  //get scale factor
	  afact = log((1e-3 + AEXPN_MAX)/AEXPN_MIN_GROWTH)/(COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH-1.0)*((double) i) + log(AEXPN_MIN_GROWTH);
	  a_table[i] = afact;

	  //do growth function integration
	  //init ODE
	  y[0] = 1.0;
	  y[1] = 0.0;
	  lna_init = LNAINITGROWTH;
	  lna_final = afact;
  	  status = gsl_odeiv2_driver_apply(d,&lna_init,lna_final,y);
	  growth_function_table[i] = y[0]*exp(afact);
	  if(status != GSL_SUCCESS)
	    {
	      fprintf(stderr,"error in integrating growth function! a = %lf\n",a);
	      assert(status == GSL_SUCCESS);
	    }
	}

      //get normalization
      y[0] = 1.0;
      y[1] = 0.0;
      lna_init = LNAINITGROWTH;
      lna_final = 0.0;

      status = gsl_odeiv2_driver_apply(d,&lna_init,lna_final,y);
      growth_function_norm = y[0];
      if(status != GSL_SUCCESS)
	{
	  fprintf(stderr,"error in integrating growth function! a = %lf\n",1.0);
	  assert(status == GSL_SUCCESS);
	}

      //free ODE stuff
      gsl_odeiv2_driver_free(d);

      //init the spline and accelerators
      if(cosmocalc_growth_function_spline != NULL)
        gsl_spline_free(cosmocalc_growth_function_spline);
      cosmocalc_growth_function_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_growth_function_spline,a_table,growth_function_table,(size_t) (COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH));
      if(cosmocalc_growth_function_acc != NULL)
        gsl_interp_accel_reset(cosmocalc_growth_function_acc);
      else
        cosmocalc_growth_function_acc = gsl_interp_accel_alloc();
    }

  return gsl_spline_eval(cosmocalc_growth_function_spline,log(a),cosmocalc_growth_function_acc)/growth_function_norm;
}

#else

static double growth_function_integ_funct(double a, void *p);

/* function for integration using gsl integration */
static double growth_function_integ_funct(double a, void *p)
{
  double hubblefac = hubble_noscale(a);
  double adot = a*hubblefac;
  double alim = (*((double*)p));

  return 1.0/adot/adot/adot*hubble_noscale(alim);
}

double growth_function_exact(double a)
{
#define WORKSPACE_NUM 100000
#define ABSERR 0.0
#define RELERR 1e-8

  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr,afact,norm;

  workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);

  afact = a;
  F.function = &growth_function_integ_funct;
  F.params = &(afact);
  gsl_integration_qag(&F,0.0,afact,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);

  afact = 1.0;
  F.params = &(afact);
  gsl_integration_qag(&F,0.0,afact,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&norm,&abserr);

  gsl_integration_workspace_free(workspace);

#undef ABSERR
#undef RELERR
#undef WORKSPACE_NUM

  return result/norm;
}

double growth_function(double a)
{
#define WORKSPACE_NUM 100000
#define ABSERR 1e-8
#define RELERR 1e-8

  static int initFlag = 1;
  static int currCosmoNum;
  static double growth_function_norm;
  static gsl_spline *cosmocalc_growth_function_spline = NULL;
  static gsl_interp_accel *cosmocalc_growth_function_acc = NULL;

  double growth_function_table[COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH];
  double a_table[COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH];
  long i;
  gsl_integration_workspace *workspace;
  gsl_function F;
  double result,abserr,afact;

  if(initFlag == 1 || currCosmoNum != cosmoData.cosmoNum)
    {
      initFlag = 0;
      currCosmoNum = cosmoData.cosmoNum;

      workspace = gsl_integration_workspace_alloc((size_t) WORKSPACE_NUM);
      F.function = &growth_function_integ_funct;

      for(i=0;i<COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH;++i)
	{
	  afact = (1e-3 + AEXPN_MAX - AEXPN_MIN)/(COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH-1.0)*((double) i) + AEXPN_MIN;
	  a_table[i] = afact;
	  F.params = &(afact);
	  gsl_integration_qag(&F,0.0,afact,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&result,&abserr);
	  growth_function_table[i] = result;
	}

      afact = 1.0;
      F.params = &(afact);
      gsl_integration_qag(&F,0.0,1.0,ABSERR,RELERR,(size_t) WORKSPACE_NUM,GSL_INTEG_GAUSS51,workspace,&growth_function_norm,&abserr);

      gsl_integration_workspace_free(workspace);

      //init the spline and accelerators
      if(cosmocalc_growth_function_spline != NULL)
	gsl_spline_free(cosmocalc_growth_function_spline);
      cosmocalc_growth_function_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_growth_function_spline,a_table,growth_function_table,(size_t) (COSMOCALC_GROWTH_FUNCTION_TABLE_LENGTH));
      if(cosmocalc_growth_function_acc != NULL)
	gsl_interp_accel_reset(cosmocalc_growth_function_acc);
      else
	cosmocalc_growth_function_acc = gsl_interp_accel_alloc();
    }

  return gsl_spline_eval(cosmocalc_growth_function_spline,a,cosmocalc_growth_function_acc)/growth_function_norm;

#undef WORKSPACE_NUM
#undef ABSERR
#undef RELERR
}
#endif

#undef ODEGROWTH