#include <assert.h>
#include <complex.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <math.h>

#include "include/linear.h"

#if defined(__cplusplus)
extern "C" {
#endif  // __cplusplus

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

#if defined(DEBUG)
#include <stdio.h>
#define debug(fmt, ...) fprintf(stderr, "ccall: " fmt "\n", ##__VA_ARGS__)
#else
#define debug(...)
#endif  // DEBUG

#define MAKE_API(V0, V1, V2, V3, V4, V5)                                        \
    switch (type) {                                                             \
        case 1:                                                                 \
            MAKE_PROG(float, s##V0, s##V1, s##V2, s##V3, s##V4, s##V5)          \
            break;                                                              \
        case 2:                                                                 \
            MAKE_PROG(double, d##V0, d##V1, d##V2, d##V3, d##V4, d##V5)         \
            break;                                                              \
        case 3:                                                                 \
            MAKE_PROG(float complex, c##V0, c##V1, c##V2, c##V3, c##V4, c##V5)  \
            break;                                                              \
        case 4:                                                                 \
            MAKE_PROG(double complex, z##V0, z##V1, z##V2, z##V3, z##V4, z##V5) \
            break;                                                              \
        default:                                                                \
            break;                                                              \
    }

#define MAKE_API_REAL(V0, V1, V2, V3, V4, V5)                            \
    switch (type) {                                                      \
        case 1:                                                          \
            MAKE_PROG(float, s##V0, s##V1, s##V2, s##V3, s##V4, s##V5);  \
            break;                                                       \
        case 2:                                                          \
            MAKE_PROG(double, d##V0, d##V1, d##V2, d##V3, d##V4, d##V5); \
        default:                                                         \
            break;                                                       \
    }

#define MAKE_API_COMPLEX(V0, V1, V2, V3, V4, V5)                                 \
    switch (type) {                                                              \
        case 3:                                                                  \
            MAKE_PROG(float complex, c##V0, c##V1, c##V2, c##V3, c##V4, c##V5);  \
            break;                                                               \
        case 4:                                                                  \
            MAKE_PROG(double complex, z##V0, z##V1, z##V2, z##V3, z##V4, z##V5); \
        default:                                                                 \
            break;                                                               \
    }

/* max and min */
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))

/* zero test */
static inline __attribute__((always_inline)) bool s_is_zero(float x) {
    return x > -1e-6 && x < 1e-6;
}

/* zero test */
static inline __attribute__((always_inline)) bool d_is_zero(double x) {
    return x > -1e-6 && x < 1e-6;
}

/* zero test */
static inline __attribute__((always_inline)) bool c_is_zero(float complex x) {
    return crealf(x) > -1e-6 && crealf(x) < 1e-6 && cimagf(x) > -1e-6 && cimagf(x) < 1e-6;
}

/* zero test */
static inline __attribute__((always_inline)) bool z_is_zero(double complex x) {
    return creal(x) > -1e-6 && creal(x) < 1e-6 && cimag(x) > -1e-6 && cimag(x) < 1e-6;
}

/* constant zero */
#define s_zero (0.)
#define d_zero (0.)
#define c_zero (0. + 0. * I)
#define z_zero (0. + 0. * I)

/* constant one */
#define s_one (1.)
#define d_one (1.)
#define c_one (1. + 0. * I)
#define z_one (1. + 0. * I)

int identity(int type, void *r, int m, int n) {
    int i, loop = m > n ? n : m;
    debug("identity is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5) \
    for (i = 0; i < loop; ++i) {             \
        ((T *)r)[i * n + i] = 1.;            \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int random_(int type, void *r, int m, int n) {
    int i, loop = m * n;
    srand(time(NULL));
    debug("random is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)    \
    for (i = 0; i < loop; ++i) {                \
        ((T *)r)[i] = (float)rand() / RAND_MAX; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int diag(int type, void *r, int row, int column, const void *src) {
    int i;
    debug("diag is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)      \
    memset(r, 0x00, row *column * sizeof(T));     \
    for (i = 0; i < row; ++i) {                   \
        ((T *)r)[i * column + i] = ((T *)src)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int diagonal(int type, void *r, const void *src, int row, int column) {
    int i, loop = row > column ? column : row;
    debug("diagonal is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)      \
    for (i = 0; i < loop; ++i) {                  \
        ((T *)r)[i] = ((T *)src)[i * column + i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int sum(int type, void *r, const void *src, int row, int column) {
    int i;
    float srv = 0.;
    double drv = 0.;
    float complex crv = 0. + 0. * I;
    double complex zrv = 0. + 0. * I;
    debug("sum is called.");
#define MAKE_PROG(T, ACC, V1, V2, V3, V4, V5) \
    for (i = 0; i < row * column; ++i) {      \
        ACC += ((T *)src)[i];                 \
    }                                         \
    *((T *)r) = ACC;
    MAKE_API(rv, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int product(int type, void *r, const void *src, int row, int column) {
    int i;
    float srv = 1.;
    double drv = 1.;
    float complex crv = 1. + 0. * I;
    double complex zrv = 1. + 0. * I;
    debug("product is called.");
#define MAKE_PROG(T, ACC, V1, V2, V3, V4, V5) \
    for (i = 0; i < row * column; ++i) {      \
        ACC *= ((T *)src)[i];                 \
    }                                         \
    *((T *)r) = ACC;
    MAKE_API(rv, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int mean(int type, void *r, const void *src, int row, int column) {
    int i;
    float srv = 0.;
    double drv = 0.;
    float complex crv = 0. + 0. * I;
    double complex zrv = 0. + 0. * I;
    debug("mean is called.");
#define MAKE_PROG(T, ACC, V1, V2, V3, V4, V5) \
    for (i = 0; i < row * column; ++i) {      \
        ACC += ((T *)src)[i];                 \
    }                                         \
    *((T *)r) = ACC / (row * column);
    MAKE_API(rv, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int transpose(int type, void *r, const void *src, int row, int column) {
    int i, j;
    debug("transpose is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)                    \
    for (i = 0; i < row; ++i) {                                 \
        for (j = 0; j < column; ++j) {                          \
            ((T *)r)[j * row + i] = ((T *)src)[i * column + j]; \
        }                                                       \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int conjugate(int type, void *r, const void *src, int row, int column) {
    int i, j;
    debug("conjugate is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)                       \
    for (i = 0; i < row; ++i) {                                    \
        for (j = 0; j < column; ++j) {                              \
            ((T *)r)[j * row + i] = creal(((T *)src)[i * column + j]) \
                                  - cimag(((T *)src)[i * column + j]) * I; \
        }                                                         \
    }
    MAKE_API_COMPLEX(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int hermite(int type, void *r, const void *src, int row, int column) {
    debug("hermite is called.");
    conjugate(type, r, src, row, column);
    transpose(type, r, src, row, column);
    return 0;
}

int lower(int type, void *r, const void *src, int row, int column) {
    int i, j, loop = row > column ? column : row;
    debug("lower triangularize is called");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5) \
    memcpy(r, src, row *column * sizeof(T)); \
    for (i = 0; i < loop; ++i) {             \
        for (j = i; j < column; ++j) {       \
            ((T *)r)[i * column + j] = 0.;   \
        }                                    \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int upper(int type, void *r, const void *src, int row, int column) {
    int i, j, loop = row > column ? column : row;
    debug("upper triangularize is called");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)                       \
    for (i = 0; i < loop; ++i) {                                   \
        for (j = i; j < column; ++j) {                             \
            ((T *)r)[i * column + j] = ((T *)src)[i * column + j]; \
        }                                                          \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int shift(int type, void *r, void *x, const void *src, int row, int column) {
    int i;
    debug("shift is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)   \
    for (i = 0; i < row * column; ++i) {       \
        ((T *)r)[i] = ((T *)src)[i] + *(T *)x; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int times(int type, void *r, void *x, const void *src, int row, int column) {
    float sscalar = *((float *)x);
    double dscalar = *((double *)x);
    float complex *cscalar = (float complex *)x;
    double complex *zscalar = (double complex *)x;

    debug("times is called.");
#define MAKE_PROG(T, AXPY, SCALAR, V2, V3, V4, V5)   \
    memset(r, 0x00, row * column * sizeof(T));  \
    cblas_##AXPY(row * column, SCALAR, (T *)src, 1, (T *)r, 1);
    MAKE_API(axpy, scalar, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int negative(int type, void *r, const void *src, int row, int column) {
    int i;
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5) \
    for (i = 0; i < row * column; ++i) { \
        ((T *)r)[i] = - ((T *)src)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int add(int type, void *r, int m, int n, int k, const void *A, const void *B) {
    int i, loop = m * n;
    debug("add is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)     \
    for (i = 0; i < loop; ++i) {                 \
        ((T *)r)[i] = ((T *)A)[i] + ((T *)B)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int minus(int type, void *r, int m, int n, int k, const void *A, const void *B) {
    int i, loop = m * n;
    debug("minus is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)     \
    for (i = 0; i < loop; ++i) {                 \
        ((T *)r)[i] = ((T *)A)[i] - ((T *)B)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int mult(int type, void *r, int m, int n, int k, const void *A, const void *B) {
    int i, loop = m * n;
    debug("mult is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)     \
    for (i = 0; i < loop; ++i) {                 \
        ((T *)r)[i] = ((T *)A)[i] * ((T *)B)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int division(int type, void *r, int m, int n, int k, const void *A, const void *B) {
    int i, loop = m * n;
    debug("division is called.");
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5)     \
    for (i = 0; i < loop; ++i) {                 \
        ((T *)r)[i] = ((T *)A)[i] / ((T *)B)[i]; \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int dot(int type, void *r, int m, int n, int k, const void *A, const void *B) {
    debug("dot is called: (%d, %d) x (%d, %d) -> (%d, %d).", m, k, k, n, m, n);
    float salpha = 1., sbeta = 0.;
    double dalpha = 1., dbeta = 0.;
    float complex _calpha = 1. + 0. * I, _cbeta = 0. + 0. * I;
    double complex _zalpha = 1. + 0. * I, _zbeta = 0. + 0. * I;
    float complex *calpha = &_calpha, *cbeta = &_cbeta;
    double complex *zalpha = &_zalpha, *zbeta = &_zbeta;
#define MAKE_PROG(T, GEMM, ALPHA, BETA, V3, V4, V5)                                          \
    cblas_##GEMM(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, ALPHA, (const T *)A, k, \
                 (const T *)B, n, BETA, (T *)r, n);
    MAKE_API(gemm, alpha, beta, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int inner(int type, void *r, int n, const void *A, int stepa, int offa, const void *B, int stepb,
          int offb) {
    debug("inner is called: length of the vectors is %d.", n);
#define MAKE_PROG(T, DOT, V1, V2, V3, V4, V5) \
    *((T *)r) = cblas_##DOT(n, ((const T *)A) + offa, stepa, ((const T *)B) + offb, stepb);
    MAKE_API_REAL(dot, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
#define MAKE_PROG(T, DOTC_SUB, V1, V2, V3, V4, V5) \
    cblas_##DOTC_SUB(n, ((const T *)A) + offa, stepa, ((const T *)B) + offb, stepb, r);
    MAKE_API_COMPLEX(dotc_sub, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int det(int type, void *r, const void *src, int row, int column) {
    float srv = 1.;
    double drv = 1.;
    float complex crv = 1. + 0. * I;
    double complex zrv = 1. + 0. * I;
    int i, singular = 0;
    int *ipiv = (int *)malloc(column * sizeof(int));
    memset(ipiv, 0x00, column * sizeof(int));
    float *sp = NULL;
    double *dp = NULL;
    float complex *cp = NULL;
    double complex *zp = NULL;
    debug("det is called.");

// step 1: copy
//
// step 2: LU factorization by getrf, if singular > 0, then the input matrix is singular.
//
// step 3: multiply all pivots
//
// step 4: clear intermediate memory

#define MAKE_PROG(T, COPY, GETRF, MA, ACC, V4, V5)                            \
    MA = (T *)malloc(row * column * sizeof(T));                               \
    cblas_##COPY(row *column, (const T *)src, 1, MA, 1);                      \
    singular = LAPACKE_##GETRF(CblasRowMajor, row, column, MA, column, ipiv); \
    if (singular == 0) {                                                      \
        for (i = 0; i < column; ++i) {                                        \
            if (ipiv[i] != i) {                                               \
                ACC *= -1.0;                                                  \
            }                                                                 \
            ACC *= MA[i * column + i];                                        \
        }                                                                     \
    } else {                                                                  \
        ACC = 0.;                                                             \
    }                                                                         \
    *((T *)r) = ACC;                                                          \
    if (MA != NULL) {                                                         \
        free(MA);                                                             \
    }
    MAKE_API(copy, getrf, p, rv, NULL, NULL);
#undef MAKE_PROG

    free(ipiv);

    return 0;
}

int trace(int type, void *r, const void *src, int row, int column) {
    int i, loop = row > column ? column : row;
    float srv = 0.;
    double drv = 0.;
    float complex crv = 0. + 0. * I;
    double complex zrv = 0. + 0. * I;
    debug("trace is called.");
#define MAKE_PROG(T, F, ACC, V2, V3, V4, V5) \
    for (i = 0; i < loop; ++i) {             \
        ACC += ((T *)src)[i * column + i];   \
    }                                        \
    *((T *)r) = ACC;
    MAKE_API(NULL, rv, NULL, NULL, NULL, NULL);
#undef MAKE_PROG
    return 0;
}

int rank(int type, int *r, const void *src, int row, int column) {
    int i;
    int *ipiv = (int *)malloc(column * sizeof(int));
    memset(ipiv, 0x00, column * sizeof(int));

    float *sp = NULL;
    double *dp = NULL;
    float complex *cp = NULL;
    double complex *zp = NULL;

    float *stau = NULL;
    double *dtau = NULL;
    float complex *ctau = NULL;
    double complex *ztau = NULL;

    debug("rank is called, original matrix: %d x %d.", row, column);
    *r = 0;

// ref: http://people.sc.fsu.edu/~jburkardt/f_src/geqp3/geqp3.html
//
// step 1: copy
//
// step 2: LU factorization by getrf, if singular > 0, then the input matrix is singular.

#define MAKE_PROG(T, COPY, GEQP3, _IS_ZERO, MA, TAU, V5)                \
    MA = (T *)malloc(row * column * sizeof(T));                         \
    cblas_##COPY(row *column, (T *)src, 1, MA, 1);                      \
    TAU = (T *)malloc(min(row, column) * sizeof(T));                    \
    LAPACKE_##GEQP3(CblasRowMajor, row, column, MA, column, ipiv, TAU); \
    for (i = 0; i < row && i < column; ++i) {                           \
        if (!_IS_ZERO(MA[i * column + i])) {                            \
            *r += 1;                                                    \
        }                                                               \
    }                                                                   \
    if (MA != NULL) {                                                   \
        free(MA);                                                       \
    }                                                                   \
    if (TAU != NULL) {                                                  \
        free(TAU);                                                      \
    }
    MAKE_API(copy, geqp3, _is_zero, p, tau, NULL)
#undef MAKE_PROG

    free(ipiv);

    return 0;
}

// TODO
int norm(int type, void *r, const void *src, int row, int column) {
    debug("norm is called.");
    return 0;
}

int inverse(int type, void *r, const void *src, int row, int column) {
    int singular = 0;
    int *ipiv = (int *)malloc(column * sizeof(int));
    debug("inverse is called.");

// step 1: do LU decomposition via getrf
//
// step 2: do inversion via getri

#define MAKE_PROG(T, COPY, GETRF, GETRI, V3, V4, V5)                              \
    cblas_##COPY(row *column, (T *)src, 1, (T *)r, 1);                            \
    singular = LAPACKE_##GETRF(CblasRowMajor, row, column, (T *)r, column, ipiv); \
    if (singular == 0) {                                                          \
        LAPACKE_##GETRI(CblasRowMajor, row, (T *)r, column, ipiv);                \
    } else {                                                                      \
        memset(r, 0xff, row *column * sizeof(T));                                 \
    }
    MAKE_API(copy, getrf, getri, NULL, NULL, NULL);
#undef MAKE_PROG

    free(ipiv);
    return 0;
}

// // Compute the (Moore-Penrose) pseudo-inverse of a matrix.
// int pinv(int type, void *r, const void *A, int row, int column, void *rcond) {
//     float scutoff = *((float *)rcond), ccutoff = *((float *)rcond);
//     double dcutoff = *((double *)rcond), zcutoff = *((double *)rcond);

//     void *u = NULL, *v = NULL, *ut = NULL, *vt = NULL, *S = NULL;
//     float *ssigma = NULL, *csigma = NULL;
//     double *dsigma = NULL, *zsigma = NULL;

//     int i = 0;

//     conjugate(type, *(void **)&A, A, row, column); // remove `const` qualifier.

// #define MAKE_PROG(GESDD, CUTOFF, SIGMA)
//     u = malloc(row * row * sizeof(T));
//     vt = malloc(column * column * sizeof(T));
//     S = malloc(min(row, column) * sizeof(T));
//     ssigma = csigma = (float *)S;
//     dsigma = zsigma = (double *)S;
//     LAPACKE_##GESDD(CblasRowMajor, 'A', r0, c0, (T *)A, c0, SIGMA, (T *)u, c1, (T *)vt, c3);
//     CUTOFF *= SIGMA[0];
//     for ()
// }

// Eigen system of ordinary matrix.
//
//      + For every column vector x in right eigenvectors V, we have Ax = \lambda x.
//      + For every column vector x in left eigenvectors U, we have x^H * A = \lambda x^H.
//
// Noticing that eigenvectors are stored as column vectors in U^T and V.
//
// ref: https://software.intel.com/en-us/mkl-developer-reference-c-geev
int eigen(int type, void *A, int r0, int c0, void *Lambda, int r1, int c1, void *UT, int r2, int c2,
          void *V, int r3, int c3) {
    int i, j;

    float *sp = NULL;
    double *dp = NULL;
    float complex *cp = NULL;
    double complex *zp = NULL;

    float complex *s_ut = (float complex *)UT, *s_v = (float complex *)V;
    double complex *d_ut = (double complex *)UT, *d_v = (double complex *)V;

    float *swr = NULL, *swi = NULL, *svl = NULL, *svr = NULL;
    double *dwr = NULL, *dwi = NULL, *dvl = NULL, *dvr = NULL;

    assert(r0 == c0);             // square matrix.
    assert(r1 == 1 && c1 == c0);  // Lambda is a vector.
    assert(r2 == 0 || (r0 == r2 && r0 == r3));
    assert(r3 == 0 || (c0 == c2 && r0 == c3));

    char jobvl = r2 == 0 ? 'N' : 'V';
    char jobvr = r3 == 0 ? 'N' : 'V';

    debug("eigen is called, jobvl: %c, jobvr: %c.", jobvl, jobvr);

#define MAKE_PROG(T, COPY, MA, V2, V3, V4, V5) \
    MA = (T *)malloc(r0 * c0 * sizeof(T));     \
    cblas_##COPY(r0 *c0, (T *)A, 1, MA, 1);
    MAKE_API(copy, p, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

#define MAKE_PROG(T, GEEV, MA, WR, WI, VL, VR)                                       \
    WR = (T *)malloc(r0 * sizeof(T));                                                \
    WI = (T *)malloc(r0 * sizeof(T));                                                \
    VL = (T *)malloc(r0 * c0 * sizeof(T));                                           \
    VR = (T *)malloc(r0 * c0 * sizeof(T));                                           \
    LAPACKE_##GEEV(CblasRowMajor, jobvl, jobvr, r0, MA, c0, WR, WI, VL, c2, VR, c3); \
    /* Copy eigenvalues (Lambda) */                                                  \
    for (i = 0; i < r0; ++i) {                                                       \
        ((T *)Lambda)[2 * i] = WR[i];                                                \
        ((T *)Lambda)[2 * i + 1] = WI[i];                                            \
    }
    MAKE_API_REAL(geev, p, wr, wi, vl, vr);
#undef MAKE_PROG

#define MAKE_PROG(T, _IS_ZERO, WI, VL, VR, _UT, _V)                            \
    /* Copy left and right eigenvectors (UT and V) */                          \
    for (j = 0; j < r0; ++j) {                                                 \
        if (_IS_ZERO(WI[j])) { /* eigenvector only has real value */           \
            for (i = 0; i < r0; ++i) {                                         \
                _UT[i * c2 + j] = VL[i * c2 + j] + 0. * I;                     \
                _V[i * c3 + j] = VR[i * c3 + j] + 0. * I;                      \
            }                                                                  \
        } else { /* eigenvector has complex value */                           \
            for (i = 0; i < r0; ++i) {                                         \
                _UT[i * c2 + j] = VL[i * c2 + j] + VL[i * c2 + j + 1] * I;     \
                _V[i * c3 + j] = VR[i * c3 + j] + VR[i * c3 + j + 1] * I;      \
                _UT[i * c2 + j + 1] = VL[i * c2 + j] - VL[i * c2 + j + 1] * I; \
                _V[i * c3 + j + 1] = VR[i * c3 + j] - VR[i * c3 + j + 1] * I;  \
            }                                                                  \
            j += 1; /* IMPORTANT: skip the conjugate eigenvalue */             \
        }                                                                      \
    }
    MAKE_API_REAL(_is_zero, wi, vl, vr, _ut, _v);
#undef MAKE_PROG

#define MAKE_PROG(T, GEEV, MA, V2, V3, V4, V5) \
    LAPACKE_##GEEV(CblasRowMajor, jobvl, jobvr, r0, MA, c0, (T *)Lambda, (T *)UT, c2, (T *)V, c3);
    MAKE_API_COMPLEX(geev, p, NULL, NULL, NULL, NULL);
#undef MAKE_PROG

#define MAKE_PROG(T, MA, V1, V2, V3, V4, V5) \
    if (MA != NULL) {                        \
        free(MA);                            \
    }
    MAKE_API(p, NULL, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

#define MAKE_PROG(T, WR, WI, VL, VR, V4, V5) \
    if (WR != NULL) {                        \
        free(WR);                            \
    }                                        \
    if (WI != NULL) {                        \
        free(WI);                            \
    }                                        \
    if (VL != NULL) {                        \
        free(VL);                            \
    }                                        \
    if (VR != NULL) {                        \
        free(VR);                            \
    }
    MAKE_API_REAL(wr, wi, vl, vr, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

// Eigen system of symmetric matrix: A = Z \Lambda Z^T (or Z^H)
//
// Here \Lambda is a diagonal matrix whose diagonal elements are the eigenvalues \lambda_i, and Z is
// the
// orthogonal matrix whose columns are the eigenvectors.
int eigenh(int type, void *A, int r0, int c0, void *Lambda, int r1, int c1, void *Z, int r2,
           int c2) {
    int i, j;

    float *sp = NULL;
    double *dp = NULL;
    float complex *cp = NULL;
    double complex *zp = NULL;

    assert(r0 == c0);             // square matrix.
    assert(r1 == 1 && c1 == c0);  // Lambda is a vector.
    assert(r2 == 0 || (r0 == r2 && c0 == c2));

    char jobz = r2 == 0 ? 'N' : 'V';

    debug("eigenh is called, jobz: %c.", jobz);

#define MAKE_PROG(T, COPY, SYEVD, V2, V3, V4, V5) \
    cblas_##COPY(r0 *c0, (T *)A, 1, (T *)Z, 1);   \
    LAPACKE_##SYEVD(CblasRowMajor, jobz, 'U', r0, (T *)Z, c0, Lambda);
    MAKE_API_REAL(copy, syevd, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

#define MAKE_PROG(T, COPY, HEEVD, V2, V3, V4, V5) \
    cblas_##COPY(r0 *c0, (T *)A, 1, (T *)Z, 1);   \
    LAPACKE_##HEEVD(CblasRowMajor, jobz, 'U', r0, (T *)Z, c0, Lambda);
    MAKE_API_COMPLEX(copy, heevd, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

// AP = LU, where rm1 is L, rm2 is U, rm3 is P and m is A.
int lu(int type, void *rm1, int r1, int c1, void *rm2, int r2, int c2, void *rm3, int r3, int c3,
       const void *m) {
    int i, j, t;
    float *sp;
    double *dp;
    //     int *ipiv = (int *)malloc(c2 * sizeof(int));
    //     int *permutation = (int *)malloc(c2 * sizeof(int));
    //     assert(c1 == r2);
    //     assert(c2 == c3);
    //     assert(r3 == c3);
    //     memset(ipiv, 0xff, sizeof(int) * c2);
    //     memset(permutation, 0x00, sizeof(int) * c2);
    //     debug("lu decompose is called.");
    // #define MAKE_PROG(T, V, F) \
//     if (r1 > c1) {         \
//         V = (T *)rm1;      \
//     } else {               \
//         V = (T *)rm2;      \
//     }                      \
//     cblas_##F(r1 *c2, (T *)m, 1, V, 1);
    //     MAKE_API(p, copy);
    // #undef MAKE_PROG

    // #define MAKE_PROG(T, V, F)                         \
// /* do LU decomposition, compute matrix L and U. */ \
// // if (r1 > c1) {                                       \
//     //     clapack_##F(CblasRowMajor, r1, c1, V, c1, ipiv); \
//     //     for (i = 0; i < r1 && i < c2; ++i) {             \
//     //         j = i;                                       \
//     //         while (++j < c1) {                           \
//     //             ((T *)rm2)[i * c2 + j] = V[i * c1 + j];  \
//     //             V[i * c1 + j] = 0.0;                     \
//     //         }                                            \
//     //         ((T *)rm2)[i * c2 + i] = 1.0;                \
//     //     }                                                \
//     // } else {                                             \
//     //     clapack_##F(CblasRowMajor, r2, c2, V, c2, ipiv); \
//     //     for (i = 0; i < r1 && i < c2; ++i) {             \
//     //         for (j = 0; j <= i; ++j) {                   \
//     //             ((T *)rm1)[i * c1 + j] = V[i * c2 + j];  \
//     //             V[i * c2 + j] = 0.0;                     \
//     //         }                                            \
//     //         ((T *)rm2)[i * c2 + i] = 1.0;                \
//     //     }                                                \
//     // }
    // // MAKE_API(p, getrf);
    // #undef MAKE_PROG

    //     /* Compute permutation matrix P according to ipiv. */
    //     for (i = 0; i < c2; ++i) {
    //         permutation[i] = i;
    //     }
    //     for (i = 0; i < c2; ++i) {
    //         if (ipiv[i] >= 0 && ipiv[i] != i) {
    //             t = permutation[i];
    //             permutation[i] = permutation[ipiv[i]];
    //             permutation[ipiv[i]] = t;
    //         }
    //     }

    // #define MAKE_PROG(T, V, F)                         \
//     for (i = 0; i < r3; ++i) {                     \
//         ((T *)rm3)[permutation[i] * c3 + i] = 1.0; \
//     }
    //     MAKE_API(NULL, NULL);
    // #undef MAKE_PROG
    //     free(ipiv);
    //     free(permutation);
    return 0;
}

// A = QR
//
// The shape of result Q and R decided by the argument (r1, c2) and (r2, c2).
//
// + The reduced mode: (equals to numpy's numpy.linalg.qr(src, mode='reduced'))
//
//      A: m x n
//      Q: m x min(m, n) orthogonal matrix, the min(m, n) leading columns forms an orthonormal basis
//      in the
//          space spanned by the columns of A.
//      R: min(m, n) x n upper trapezoidal matrix, when m >= n, R is upper triangular.
//
// + The complete mode: (equals to numpy's numpy.linalg.qr(src, mode='complete'))
//
//      A: m x n
//      Q: m x m orthogonal matrix, the min(m, n) leading columns forms an orthonormal basis in the
//          space spanned by the columns of A.
//      R: m x n upper trapezoidal matrix, when m >= n, R is upper triangular.
//
int qr(int type, void *A, int r0, int c0, void *Q, int r1, int c1, void *R, int r2, int c2) {
    float *stau = NULL;
    double *dtau = NULL;
    float complex *ctau = NULL;
    double complex *ztau = NULL;
    float *sp = NULL;
    double *dp = NULL;
    float complex *cp = NULL;
    double complex *zp = NULL;

    int i, j;

    debug("QR decomposition is called.");

// reduced mode:
//
//      assert(r0 == r1);
//      assert(min(r0, c0) == c1);
//      assert(min(r0, c0) == r2);
//      assert(c0 == c2);

// complete mode:
//
//      assert(r0 == r1);
//      assert(r0 == c1);
//      assert(r0 == r2);
//      assert(c0 == c2);

#define MAKE_PROG(T, COPY, GEQRF, MA, TAU, _ZERO, V5)              \
    MA = (T *)malloc(r0 * c0 * sizeof(T));                         \
    cblas_##COPY(r0 *c0, (T *)A, 1, (T *)MA, 1);                   \
    TAU = (T *)malloc(c0 * sizeof(T));                             \
    LAPACKE_##GEQRF(CblasRowMajor, r0, c0, (T *)MA, c0, (T *)TAU); \
    /* initialize as zero */                                       \
    memset(Q, 0x00, r1 *c1 * sizeof(T));                           \
    memset(R, 0x00, r2 *c2 * sizeof(T));                           \
    /* copy Q and R */                                             \
    for (i = 0; i < r0; ++i) {                                     \
        j = 0;                                                     \
        while (j < i) {                                            \
            ((T *)Q)[i * c1 + j] = ((T *)MA)[i * c0 + j];          \
            j++;                                                   \
        }                                                          \
        while (j < c0) {                                           \
            ((T *)R)[i * c2 + j] = ((T *)MA)[i * c0 + j];          \
            j++;                                                   \
        }                                                          \
    }
    MAKE_API(copy, geqrf, p, tau, _zero, NULL)
#undef MAKE_PROG

/* make Q */

#define MAKE_PROG(T, ORGQR, TAU, V2, V3, V4, V5) \
    LAPACKE_##ORGQR(CblasRowMajor, r1, c1, min(r0, c0), Q, c1, (T *)TAU);
    MAKE_API_REAL(orgqr, tau, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

#define MAKE_PROG(T, UNGQR, TAU, V2, V3, V4, V5) \
    LAPACKE_##UNGQR(CblasRowMajor, r1, c1, min(r0, c0), Q, c1, (T *)TAU);
    MAKE_API_COMPLEX(ungqr, tau, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

#define MAKE_PROG(T, MA, TAU, V2, V3, V4, V5) \
    if (MA != NULL) {                         \
        free(MA);                             \
    }                                         \
    if (TAU != NULL) {                        \
        free(TAU);                            \
    }
    MAKE_API(p, tau, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

// Singular value decomposition: A = U \Sigma V^T (or V^H)
int svd(int type, void *A, int r0, int c0, void *U, int r1, int c1, void *Sigma, int r2, int c2,
        void *VT, int r3, int c3) {
    char job = (r1 == 0 && r3 == 0) ? 'N' : (r0 != c0 && r1 == c1) ? 'A' : 'S';

    debug("svd is called, job is %c.", job);

#define MAKE_PROG(T, GESDD, V1, V2, V3, V4, V5) \
    LAPACKE_##GESDD(CblasRowMajor, job, r0, c0, (T *)A, c0, Sigma, (T *)U, c1, (T *)VT, c3);
    MAKE_API(gesdd, NULL, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

int jordan(int type, void *rm1, int r1, int c1, void *rm2, int r2, int c2, void *rm3, int r3,
           int c3, const void *m) {
    return 0;
}

int cholesky(int type, char uplo, void *A, int r0, int c0, void *U, int r1, int c1) {
    assert(r0 == c0);  // square matrix.
    assert(r0 == r1);
    assert(c0 == c1);

    debug("cholesky is called.");

#define MAKE_PROG(T, COPY, POTRF, V2, V3, V4, V5)     \
    cblas_##COPY(r0 *c0, (const T *)A, 1, (T *)U, 1); \
    LAPACKE_##POTRF(CblasRowMajor, uplo, r0, (T *)U, c0);
    MAKE_API(copy, potrf, NULL, NULL, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

int schur(int type, void *rm1, int r1, int c1, void *rm2, int r2, int c2, void *rm3, int r3, int c3,
          const void *m) {
    return 0;
}

// Linear transformation: r = Av.
int transform(int type, void *r, int row, int column, const void *A, const void *v) {
    float salpha = 1., sbeta = 0.;
    double dalpha = 1., dbeta = 0.;
    float complex _calpha = 1. + 0. * I, _cbeta = 0. + 0. * I;
    double complex _zalpha = 1. + 0. * I, _zbeta = 0. + 0. * I;
    float complex *calpha = &_calpha, *cbeta = &_cbeta;
    double complex *zalpha = &_zalpha, *zbeta = &_zbeta;

    debug("transform is called: matrix has size (%d, %d).", row, column);

#define MAKE_PROG(T, GEMV, ALPHA, BETA, V3, V4, V5)                                     \
    cblas_##GEMV(CblasRowMajor, CblasNoTrans, row, column, ALPHA, (const T *)A, column, \
                 (const T *)v, 1, BETA, (T *)r, 1);
    MAKE_API(gemv, alpha, beta, NULL, NULL, NULL)
#undef MAKE_PROG

    return 0;
}

// Linear array generation.
int linspace(int type, void *r, float const start, float const end, int size) {
    float sep = (end - start) / (size - 1);

    float ssep = sep;
    double dsep = (double)sep;
    float complex csep = sep + 0. * I;
    double complex zsep = (double)sep + 0. * I;

    float sval = start;
    double dval = (double)start;
    float complex cval = start + 0. * I;
    double complex zval = (double)start + 0. * I;

    int i;

#define MAKE_PROG(T, SEP, VAL, V2, V3, v4, V5) \
    for (i = 0; i < size; ++i, VAL += SEP) { \
        ((T *)r)[i] = VAL; \
    }
    MAKE_API(sep, val, NULL, NULL, NULL, NULL);
#undef MAKE_PROG

    return 0;
}

int replicate(int type, void *r, void const *value, int size) {
    int i;
#define MAKE_PROG(T, V0, V1, V2, V3, V4, V5) \
    for (i = 0; i < size; ++i) {               \
        ((T *)r)[i] = *((T *)value);               \
    }
    MAKE_API(NULL, NULL, NULL, NULL, NULL, NULL)
#undef MAKE_PROG
    return 0;
}

// element-wise functions.

int logistic(int type, void *r, const void *A, int row, int column) {
    int i;

    const float sunit = 1.;
    const double dunit = 1.;
    const float complex cunit = 1. + 0. * I;
    const double complex zunit = 1. + 0. * I;

    switch (type) {
        case 1:
            for (int i = 0; i < row * column; ++i) {
                ((float *)r)[i] = sunit / (sunit + exp(- ((float *)A)[i]));
            }
            break;
        case 2:
            for (int i = 0; i < row * column; ++i) {
                ((double *)r)[i] = dunit / (dunit + exp(- ((double *)A)[i]));
            }
            break;
        case 3:
            for (int i = 0; i < row * column; ++i) {
                ((float complex *)r)[i] = cunit / (cunit + cexp(- ((float complex *)A)[i]));
            }
            break;
        case 4:
            for (int i = 0; i < row * column; ++i) {
                ((double complex *)r)[i] = zunit / (zunit + cexp(- ((double complex *)A)[i]));
            }
            break;
        default: break;
    }
    return 0;
}

int logisticd(int type, void *r, const void *A, int row, int column) {
    int i;

    const float sunit = 1.;
    const double dunit = 1.;
    const float complex cunit = 1. + 0. * I;
    const double complex zunit = 1. + 0. * I;

    logistic(type, r, A, row, column);

#define MAKE_PROG(T, UNIT, V1, V2, V3, V4, V5) \
    for (i = 0; i < row * column; ++i) { \
        ((T *)r)[i] = ((T *)r)[i] * (UNIT - ((T *)r)[i]); \
    }
    MAKE_API(unit, NULL, NULL, NULL, NULL, NULL);
#undef MAKE_PROG

    return 0;
}

#if defined(__cplusplus)
}
#endif  // __cplusplus


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gbpInterpolate.h>

double interpolate_derivative(interp_info *interp, double x) {
    double r_val;
    r_val = gsl_interp_eval_deriv(
        ((interp_info *)interp)->interp, ((interp_info *)interp)->x, ((interp_info *)interp)->y, x, ((interp_info *)interp)->accel);

    return (r_val);
}


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

/* Author:  G. Jungman */

/* Calculate the matrix exponential, following
 * Moler + Van Loan, SIAM Rev. 20, 801 (1978).
 */

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

#include "gsl_linalg.h"

/* store one of the suggested choices for the
 * Taylor series / square  method from Moler + VanLoan
 */
struct moler_vanloan_optimal_suggestion
{
  int k;
  int j;
};
typedef  struct moler_vanloan_optimal_suggestion  mvl_suggestion_t;

/* table from Moler and Van Loan
 * mvl_tab[gsl_mode_t][matrix_norm_group]
 */
static mvl_suggestion_t mvl_tab[3][6] =
{
  /* double precision */
  {
    { 5, 1 }, { 5, 4 }, { 7, 5 }, { 9, 7 }, { 10, 10 }, { 8, 14 }
  },

  /* single precision */
  {
    { 2, 1 }, { 4, 0 }, { 7, 1 }, { 6, 5 }, { 5, 9 }, { 7, 11 }
  },

  /* approx precision */
  {
    { 1, 0 }, { 3, 0 }, { 5, 1 }, { 4, 5 }, { 4, 8 }, { 2, 11 }
  }
};

inline
static double
sup_norm(const gsl_matrix * A)
{
  double min, max;
  gsl_matrix_minmax(A, &min, &max);
  return GSL_MAX_DBL(fabs(min), fabs(max));
}

static
mvl_suggestion_t
obtain_suggestion(const gsl_matrix * A, gsl_mode_t mode)
{
  const unsigned int mode_prec = GSL_MODE_PREC(mode);
  const double norm_A = sup_norm(A);
  if(norm_A < 0.01) return mvl_tab[mode_prec][0];
  else if(norm_A < 0.1) return mvl_tab[mode_prec][1];
  else if(norm_A < 1.0) return mvl_tab[mode_prec][2];
  else if(norm_A < 10.0) return mvl_tab[mode_prec][3];
  else if(norm_A < 100.0) return mvl_tab[mode_prec][4];
  else if(norm_A < 1000.0) return mvl_tab[mode_prec][5];
  else
  {
    /* outside the table we simply increase the number
     * of squarings, bringing the reduced matrix into
     * the range of the table; this is obviously suboptimal,
     * but that is the price paid for not having those extra
     * table entries
     */
    const double extra = log(1.01*norm_A/1000.0) / M_LN2;
    const int extra_i = (unsigned int) ceil(extra);
    mvl_suggestion_t s = mvl_tab[mode][5];
    s.j += extra_i;
    return s;
  }
}

/* use series representation to calculate matrix exponential;
 * this is used for small matrices; we use the sup_norm
 * to measure the size of the terms in the expansion
 */
static void
matrix_exp_series(
  const gsl_matrix * B,
  gsl_matrix * eB,
  int number_of_terms
  )
{
  int count;
  gsl_matrix * temp = gsl_matrix_calloc(B->size1, B->size2);

  /* init the Horner polynomial evaluation,
   * eB = 1 + B/number_of_terms; we use
   * eB to collect the partial results
   */
  gsl_matrix_memcpy(eB, B);
  gsl_matrix_scale(eB, 1.0/number_of_terms);
  gsl_matrix_add_diagonal(eB, 1.0);
  for(count = number_of_terms-1; count >= 1; --count)
  {
    /*  mult_temp = 1 + B eB / count  */
    gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, B, eB, 0.0, temp);
    gsl_matrix_scale(temp, 1.0/count);
    gsl_matrix_add_diagonal(temp, 1.0);

    /*  transfer partial result out of temp */
    gsl_matrix_memcpy(eB, temp);
  }

  /* now eB holds the full result; we're done */
  gsl_matrix_free(temp);
}

int
gsl_linalg_exponential_ss(
  const gsl_matrix * A,
  gsl_matrix * eA,
  gsl_mode_t mode
  )
{
  if(A->size1 != A->size2)
  {
    GSL_ERROR("cannot exponentiate a non-square matrix", GSL_ENOTSQR);
  }
  else if(A->size1 != eA->size1 || A->size2 != eA->size2)
  {
    GSL_ERROR("exponential of matrix must have same dimension as matrix", GSL_EBADLEN);
  }
  else
  {
    int i;
    const mvl_suggestion_t sugg = obtain_suggestion(A, mode);
    const double divisor = exp(M_LN2 * sugg.j);

    gsl_matrix * reduced_A = gsl_matrix_alloc(A->size1, A->size2);

    /*  decrease A by the calculated divisor  */
    gsl_matrix_memcpy(reduced_A, A);
    gsl_matrix_scale(reduced_A, 1.0/divisor);

    /*  calculate exp of reduced matrix; store in eA as temp  */
    matrix_exp_series(reduced_A, eA, sugg.k);

    /*  square repeatedly; use reduced_A for scratch */
    for(i = 0; i < sugg.j; ++i)
    {
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, eA, eA, 0.0, reduced_A);
      gsl_matrix_memcpy(eA, reduced_A);
    }

    gsl_matrix_free(reduced_A);

    return GSL_SUCCESS;
  }
}


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

// Compute comoving mass function
double mass_function(double M_interp, double z, cosmo_info **cosmo, int mode, ...) {
    va_list vargs;
    va_start(vargs, mode);

    // Get passed parameters (if mode specifies they are there).
    double *P = NULL;
    if(SID_CHECK_BITFIELD_SWITCH(mode, MF_PASS_PARAMS))
        P = (double *)va_arg(vargs, double *);

    // Initialize/compute some misc. cosmology things
    double sigma_interp = sqrt(power_spectrum_variance(k_of_M(M_interp, *cosmo), z, cosmo, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER));
    double Omega_M      = ((double *)ADaPS_fetch(*cosmo, "Omega_M"))[0];
    double rho_o        = Omega_M * rho_crit_z(0, *cosmo);

    // Compute mass function (eqn 41 of Lukic et al, 2007, for example)
    double dlnInvs_dlogM = dln_Inv_sigma_dlogM(cosmo, M_interp, PSPEC_LINEAR_TF, PSPEC_ALL_MATTER);
    double rval          = rho_o * scaled_mass_function(sigma_interp, mode, P) * dlnInvs_dlogM / M_interp;
    va_end(vargs);
    return (rval);
}


/* addmatrix_gsl.c */
/*
  Program to add two variable dimension matrices input by user
*/
// https://www.gnu.org/software/gsl/manual/html_node/Example-programs-for-matrices.html

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

int main (void)
{
  int i, j;
  gsl_matrix * m = gsl_matrix_alloc(10, 3);

  for (i = 0; i < 10; i++)
    for (j = 0; j < 3; j++)
      gsl_matrix_set(m, i, j, 0.23 + 100*i + j);

  for (i = 0; i < 100; i++)  /* OUT OF RANGE ERROR */
    for (j = 0; j < 3; j++)
      printf("m(%d, %d) = %g\n", i, j,
	     gsl_matrix_get(m, i, j));

  gsl_matrix_free(m);

  return 0;
}


/*
VEGAS_TABLE Version 1.  5/27/16
report issues to Tim Waters,
waters@lanl.gov

**** Basic code description:
Input: 	An ascii table with entries
		on a uniform grid of (x,y) values
		and a (x,y) value for which an
    interpolated value is sought
Output: The interpolated value
     corresponding to that (x,y) pair

**** Detailed description:

>> Step 1. Instantiation:
The class constructor calls a method
to parse an ascii text file which contains
an (N+1)x(M+1) table:
The first row contains the M values of x.
The first column contains the N values of y.
The remainder of the table contains the NxM
values of z.

Here is a simple example of an input file:

4	1e0     1e1     1e2     1e3
1e4	1e-20	2e-20	3e-20	4e-20
1e5	5e-20	6e-20	7e-20	8e-20
1e6	1e-19	2e-19	3e-19	4e-19
1e7	5e-19	6e-19	7e-19	8e-19
1e8	1e-18	2e-18	3e-18	4e-18

In this example, N = 5 and M = 4.
M must be specified as the first element.
The remaining elements in row 1 are values
of the photoionization parameter, xi.
The left column below M are the temperature
values.  The remaining elements are net heating
rates that are functions of T and xi.

By default, the input file is limited to size
1000 x 1000.  To increase this size limit,
increase Nmax in the default constructor.

>> Step 2. Table initialization:
The file parsed above is read into a
table, but this is not the actual table
used.  The actual table is made upon calling
an initialization function, and optionally
array arguments can be passed specifying the
min/max of both T and xi. This may be useful
when combining multiple overlapping tables
and will allow for flexibility if tables
generated from non-uniform grids are needed,
although a table-search algorithm has not
been implemented.  The input table is then
reduced to a size that contains only the
values between these limits.

>> Step 3. Invocation:
The code returns the interpolated value
of whatever rate was in the input file.
The code uses the bicubic and bilinear
interpolation algorithms from the GSL
library.  By default, bicubic is used.

Importantly, the code assumes that a
nearly uniform grid was produced via
x = j*(xmax-xmin)/(M-1) + xmin
y = i*(ymax-ymin)/(N-1) + ymin
This allows avoiding an expensive
table search for the lookup operation.
For a uniform grid generated from the
above formulas, the table indices
for a desired value of (x,y) are
i_x = (M-1)*(x - xmin)/(xmax - xmin)
i_y = (N-1)*(y - ymin)/(ymax - ymin)
Since xstar may slightly modify the
grid locations (x,y) originally
specified, our lookup operation
consists of first guessing that the
lookup value indices are (i_x,i_y)
and then allowing for some wiggle
room by searching 1 index left, right,
above, and below upon finding that our
guess was incorrect.  The code will
terminate if this search fails.

**** Example usages:
1. Bicubic interpolation on a table that was
generated from a log-log spaced grid
VEGAS_LUT hc_xstar("input_table.tab","bicubic");
hc_xstar.initialize_table();
hc_rate = hc_xstar.get_rate(T,xi);

1a. If lin-lin spacing was used instead, use
VEGAS_LUT hc_xstar("input_table.tab","bicubic",false,false);

1c. To apply a bounding box, use instead
const double T_bbox[2] = {1e5,1e7};
const double xi_bbox[2] = {1e1,5e2};
hc_xstar.initialize_table(T_bbox,xi_bbox);
*/

#ifndef H_VEGAS_TABLE
#define H_VEGAS_TABLE

#include <iostream>		// std::cout, std::endl
#include <iomanip>      // std::setprecision
#include <fstream>		// std::ifstream
#include <string>
#include <vector>
#include <time.h>
#include <cmath>

#include <gsl/gsl_interp2d.h>
#include <gsl/gsl_spline2d.h>

class VEGAS_LUT
{
public:
    //default constructor will parse the input file
    //log-log spacing for (T,xi) is assumed by default
	VEGAS_LUT(	const std::string a="VEGAS_table.tab",
				const std::string b="bicubic",
				bool c=true, bool d=true):
	filename(a),gsl_routine(b),logT(c),logxi(d)
	{
	 // allocate a (Nmax x Nmax) table for reading in the file
	  int Nmax = 1000;
	  input_table = new double*[Nmax];
	  for (int i = 0; i < Nmax; i++)
    	input_table[i] = new double[Nmax];
      xivals = new double[Nmax];
      Tvals = new double[Nmax];

      gsl_init();
      parse_file();
	}
	~VEGAS_LUT()  { free_memory(); }

	//user must call the initialize_table method
	void initialize_table(const double*, const double*);

	//user function that returns an interpolated value
	double get_rate(double, double);

	// actual table (T,xi) bounds:
	// hydro code needs to know Temp bounds when doing implicit root finding
	double y_min,y_max,x_min,x_max;

private:
	bool logT,logxi;					// determines log gridding
	double* xlim;						// input table xi bounds
	double* ylim;						// input table T bounds
	const double* T_bounds;				// user specified T bounds
	const double* xi_bounds;			// user specified xi bounds
	double** 	input_table; 			// pointer to input table
	double** 	table;					// pointer to actual table
	double* 	xivals; 				// xi-vals from input table
	double*		Tvals; 					// T-vals from input table
	double* 	xvals; 					// xi-vals for actual table
	double*		yvals; 					// T-vals for actual table
	unsigned int 	NT,	  // # rows of input table (# of Ts)
					Nxi,  // # cols of input table (# of xis)
					N,	  // # rows of actual table (# of Ts)
					M,	  // # cols of actual table (# of xis)
					i_T,  // T-offset between the input and actual tables
					i_xi; // xi-offset between the input and actual tables
	typedef std::vector<double> array_t;	//dynamic array data type
	std::string filename, gsl_routine;		//input strings

	//member functions for handling the input file
	void parse_file();
	bool read_value(std::ifstream&, const int, array_t&);
	void free_input_table();
	void free_memory();

	//other member functions
	void reduce_input_table();
	void make_table(unsigned int, unsigned int);
	void determine_box_indices(double, double, unsigned int&, unsigned int&);

	//GSL variables and member functions
	bool use_gsl_bicubic;
	const gsl_interp2d_type *gsl_scheme;
	typedef gsl_spline2d* spline_t;
	spline_t **spline_table;
    gsl_interp_accel *xacc, *yacc;
    size_t nx,ny;
    void gsl_init();
    void make_spline_table();
};

void VEGAS_LUT::gsl_init()
{
    if (gsl_routine == "bilinear")
    {
      gsl_scheme = gsl_interp2d_bilinear;
      use_gsl_bicubic = false;
      nx = 2;
      ny = 2;
    }
    else if (gsl_routine == "bicubic")
    {
      gsl_scheme = gsl_interp2d_bicubic;
      use_gsl_bicubic = true;
      nx = 4;
      ny = 4;
    }
    else
    {
      std::cout << "ERROR: GSL interpolation scheme " << gsl_routine
      << " not recognized. Choose from:\n"
      << "bilinear\n"
      << "bicubic\n"
      << std::endl;
      exit(0);
    }
}

bool VEGAS_LUT::read_value(std::ifstream& fin, const int j, array_t& input_data)
{
  double value;
  if (fin >> value)
  {
    input_data[j]=value;
    return true;
  }
  else return false;
}

void VEGAS_LUT::parse_file()
{
  std::ifstream fin(filename.c_str());
  if (!fin)
  {
    std::cout << "File " << filename << " does not exist!" << std::endl;
	exit(0);
  }
  bool file_is_open = false;
  bool end_of_line = false;

  // read in first value: Nxi
  array_t val1(1);
  file_is_open = read_value(fin,0,val1);
  Nxi = (unsigned int) val1[0];

  // read in remainder of first row: all xi-values
  array_t this_row(Nxi+1);
  int i=1,j;
  while(!end_of_line)
  {
    file_is_open = read_value(fin,i,this_row);
    xivals[i-1] = this_row[i];
    if( i!=0 && i%Nxi == 0 )
        end_of_line = true;
    else i++; //next row element
  }

  // read in 1st element of 2nd row: the 1st T value
  file_is_open = read_value(fin,0,this_row);
  Tvals[0] = this_row[0];

  // read in the rest of the file
  end_of_line = false;
  i = 1; j=0;
  while(file_is_open)
  {
    file_is_open = read_value(fin,i,this_row);

    if( i!=0 && i%Nxi == 0 )
    {
        end_of_line = true;
        i=0; // j++; //reset column, increment row
    }
    else i++; //next row element

    if(end_of_line)
    {
      // 1st element of this_row is T
      Tvals[j] = this_row[0];

      // remaining elements are rates
      for (int ii=0; ii<Nxi; ii++)
        //std::cout << "this_row[" << ii << "] = " << this_row[ii] << std::endl;
        input_table[j][ii] = this_row[ii+1];

      j++;
    }
    end_of_line = false;
  }

  NT = j; //number of temperature values
  fin.close();

  if (Nxi < 2 || NT < 2)
    {
    std::cout << "ERROR: Table dimensions must be at least 2x2!" << std::endl;
    exit(0);
    }
}

void VEGAS_LUT::make_table(unsigned int i0, unsigned int j0)
{

    // allocate memory
	table = new double*[N];
	for (int j = 0; j < N; j++)
    	table[j] = new double[M];

    // assign values
	for (int j = 0; j < N; j++)
	  for (int i = 0; i < M; i++)
	    table[j][i] = input_table[j0+j][i0+i];
}

void VEGAS_LUT::make_spline_table()
{

    // allocate memory for spline_table
	spline_table = new spline_t*[N];
	for (int j = 0; j < N; j++)
    	spline_table[j] = new spline_t[M];

    /* Each location in our table is to be thought of as a 4-corner box.
     * The corners have locations (xb[], yb[]) and function values (zb[])
     * and this data is collected ahead of time so that GSL only needs to
     * access it to carry out the interpolation.  For the case of bicubic
     * interpolation, GSL will also take numerical derivatives and store
     * all that data, so a lot of expense is saved by collecting this at
     * initialization. */
    double *xb = new double[nx];
	double *yb = new double[ny];
	double *zb = new double[nx*ny];

	/* Associations:
	T  = y(j), with j from (0,N-1)
	xi = x(i), with i from (0,M-1)
	*/
	for (int j=1; j < (N-2); j++)
      for (int i=1; i < (M-2); i++)
      {
      	// allocate storage for interpolation data
        gsl_spline2d *spline = gsl_spline2d_alloc(gsl_scheme, nx, ny);

        // evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
	    for (int jb=0; jb < nx; jb++)
		  for (int ib=0; ib < ny; ib++)
		  {
		    if (use_gsl_bicubic)
		    {
		      xb[ib] = xvals[i+ib-1];
		      yb[jb] = yvals[j+jb-1];
			  gsl_spline2d_set(spline, zb, ib, jb, table[j+jb-1][i+ib-1]);
		    }
		    else
		    {
		      xb[ib] = xvals[i+ib];
		      yb[jb] = yvals[j+jb];
		      gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
		    }
		  }

          /* add position data
          * In the case of bicubic, this also internally calculates
          * derivative data and stores that as well. */
          gsl_spline2d_init(spline, xb, yb, zb, nx, ny);

          // store this interpolation data in our spline table
          spline_table[j][i] = spline;
      } //end loop

    /* The above loop excludes all boxes on table edges, which always require
     * doing bilinear interpolation, so add that data now */
    int j_edges[2] = {0, N-2};
    int i_edges[2] = {0, M-2};
    int i,j;

    for (int jj=0; jj < 2; jj++)
      for (int i=0; i < (M-1); i++)
      {
        j = j_edges[jj];

		gsl_spline2d *spline = gsl_spline2d_alloc(gsl_interp2d_bilinear, 2, 2);

		// evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
		  for (int jb=0; jb < 2; jb++)
			for (int ib=0; ib < 2; ib++)
			{
				xb[ib] = xvals[i+ib];
				yb[jb] = yvals[j+jb];
				gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
			}

		/* add position data
        * In the case of bicubic, this also internally calculates
        * derivative data and stores that as well. */
        gsl_spline2d_init(spline, xb, yb, zb, 2, 2);

        // store this interpolation data in our spline table
        //std::cout<<  "(i,j) = (" << i << ", " << j << ")" << std::endl;
        spline_table[j][i] = spline;
      }

    for (int j=0; j < (N-1); j++)
      for (int ii=0; ii < 2; ii++)
      {
        i = i_edges[ii];

		gsl_spline2d *spline = gsl_spline2d_alloc(gsl_interp2d_bilinear, 2, 2);

		// evaluate position data (xb[],yb[]) and store function values (zb[]) to spline
		  for (int jb=0; jb < 2; jb++)
			for (int ib=0; ib < 2; ib++)
			{
				xb[ib] = xvals[i+ib];
				yb[jb] = yvals[j+jb];
				gsl_spline2d_set(spline, zb, ib, jb, table[j+jb][i+ib]);
			}

		/* add position data
        * In the case of bicubic, this also internally calculates
        * derivative data and stores that as well. */
        gsl_spline2d_init(spline, xb, yb, zb, 2, 2);

        // store this interpolation data in our spline table
        //std::cout<<  "(i,j) = (" << i << ", " << j << ")" << std::endl;
        spline_table[j][i] = spline;
      }

    // These I think are used when a lookup table must be searched
    // but since we know the lookup locations there is no point
    xacc = NULL; //gsl_interp_accel_alloc();
    yacc = NULL; //gsl_interp_accel_alloc();

}

void VEGAS_LUT::free_memory()
{
  for (int i = 0; i < N; i++)
  {
    delete[] table[i];
    delete[] spline_table[i];
  }

  //gsl_interp_accel_free(xacc);
  //gsl_interp_accel_free(yacc);

}

void VEGAS_LUT::free_input_table()
{
  for (int i = 0; i < N; i++)
    delete[] input_table[i];
}

void VEGAS_LUT::reduce_input_table()
{

    double Tmin,Tmax,ximin,ximax;
    Tmin = T_bounds[0];
    Tmax = T_bounds[1];
    ximin = xi_bounds[0];
    ximax = xi_bounds[1];

    if (Tmin < Tvals[0] || Tmax > Tvals[NT-1])
    {
    std::cout << "ERROR: Tmin or Tmax is not contained within file "
    << filename << "!"
    << std::endl;
    exit(0);
    }

    if (ximin < xivals[0] || ximax > xivals[Nxi-1])
    {
    std::cout << "ERROR: x_min or x_max is not contained within file "
    << filename << "!"
    << std::endl;
    exit(0);
    }

    // find the indices of the table corresponding to input min/max values
    int i = 0;
    while (Tvals[i] < Tmax && i < NT)
      i++;
    y_max = Tvals[i];  //so y_max is slightly greater than Tmax

    int ii = 0;
    while (Tvals[ii] <= Tmin && ii < i)
      ii++;
    y_min = Tvals[ii-1]; //so y_min is slightly less than Tmin

    // assign private variables
    i_T = ii-1; // start position in input table
    N = i - ii + 2; // # of T-vals in desired table

    i = 0;
    while (xivals[i] < ximax && i < Nxi)
      i++;
    x_max = xivals[i]; //so x_max is slightly greater than ximax

    ii = 0;
    while (xivals[ii] <= ximin && ii < i)
      ii++;
    x_min = xivals[ii-1]; //so x_min is slightly less than ximin

    // assign private variables
    i_xi = ii-1; // start position in input table
    M = i - ii + 2; // # of xi-vals in desired table
}

void VEGAS_LUT::initialize_table(const double * T_bbox = NULL, const double * xi_bbox = NULL)
{
	int i0,j0;

	if (T_bbox != NULL || xi_bbox != NULL)
	{
	  T_bounds = T_bbox;
	  xi_bounds = xi_bbox;
	  reduce_input_table(); // this sets i_xi, i_T, N, M, y_min, Tmax, x_min, x_max
	  i0 = i_xi;
	  j0 = i_T;
	}
	else // the actual table will be the same size as the input table
	{
	  y_min = Tvals[0];
	  y_max = Tvals[NT-1];
	  x_min = xivals[0];
	  x_max = xivals[Nxi-1];
	  N = NT;
	  M = Nxi;
	  i0 = 0;
	  j0 = 0;
	}

	// store the values of xi and T that will be used
    // they way they will be used (i.e. log or lin)
    xvals = new double[M];
    yvals = new double[N];
    double x,y;
    for (int i=0; i<M; i++)
    {
      if(logxi)	x = log10(xivals[i0+i]);
      else 		x = xivals[i0+i];
      xvals[i] = x;
    }
    for (int j=0; j<N; j++)
    {
      if (logT)	y = log10(Tvals[j0+j]);
      else 		y = Tvals[j0+j];
      yvals[j] = y;
    }

    // generate the actual table of H/C rates
	make_table(i0,j0);

	// alloc storage for max/min vals of T,xi
	xlim = new double[2]; // xi limits
  ylim = new double[2]; // T limits

    // set any log gridding: use limits of input table
	 if (logT) {
	   ylim[0] = log10(Tvals[0]);
	   ylim[1] = log10(Tvals[NT-1]); }
	  else {
	   ylim[0] = Tvals[0];
	   ylim[1] = Tvals[NT-1]; }

     if (logxi) {
       xlim[0] = log10(xivals[0]);
       xlim[1] = log10(xivals[Nxi-1]); }
     else {
       xlim[0] = xivals[0];
       xlim[1] = xivals[Nxi-1]; }

    // generate the spline table for GSL
	make_spline_table();

	// delete input_table
	free_input_table();

}

void VEGAS_LUT::determine_box_indices(double x, double y, unsigned int &i_x, unsigned int &i_y)
{
  unsigned int i,j;
  bool correct_guess = false;

  /* First guess the indices assuming the input table was
   * generated from a uniform grid: the actual xi and T values
   * that xstar produces data for may be slightly different
   * as xstar's stopping criteria algorithms may slightly adjust
   * the input values.  On rare occasions this guess can be off
   * by 1 index, so here we allow for that. */
  i = (Nxi-1) * (x - xlim[0]) / (xlim[1] - xlim[0]) - i_xi;
  j = (NT-1) * (y - ylim[0]) / (ylim[1] - ylim[0]) - i_T ;

  /* now we check our guess in x and if needed, adjust one box left/right */
  if (x > xvals[i] && x < xvals[i+1])  //then we guessed right
  {
    i_x = i;
    correct_guess = true;
  }

  if (!correct_guess)
  {
    if (i==0 || i==(M-1))
    {
      if (i==0 && x < xvals[2])
        i_x = 1;
      else if (i==(M-1) && x > xvals[M-2])
        i_x = M-2;
    }
    else if (x < xvals[i]) //then move to the left 1
      i_x = i-1;
    else if (x > xvals[i+1]) //then move to the right 1
      i_x = i+1;
    else // we give up
    {
      std::cout << "ERROR: search for the table xi-index failed!"
      			<< "\nDouble check inputs or try debugging "
      			<< "function determine_box_indices()"
      			<< std::endl;
      exit(0);
    }
  }

  /* now we check our guess in y and if needed, adjust one box up/down */
  correct_guess = false; // reset

  if (y > yvals[j] && y < yvals[j+1])  //then we guessed right
  {
    i_y = j;
    correct_guess = true;
  }

  if (!correct_guess)
  {
    if (j==0 || j==(N-1))
    {
      if (j==0 && y < yvals[2])
        i_y = 1;
      else if (j==(N-1) && y > yvals[N-2])
        i_y = N-2;
    }
    else if (y < yvals[j]) //then move down 1
      i_y = j-1;
    else if (y > yvals[j+1]) //then move up 1
      i_y = j+1;
    else // we give up
    {
      std::cout << "ERROR: search for the table T-index failed!"
      			<< "\nDouble check inputs or try debugging "
      			<< "function determine_box_indices()"
      			<< std::endl;
      exit(0);
    }
  }

}

double VEGAS_LUT::get_rate(double y_val, double x_val)
{
	unsigned int i_x,i_y;
	double x,y;

	/* terminate program if values exceed bounding box
    if (y_val < y_min || y_val > y_max || x_val < x_min || x_val > x_max)
    {
      std::cout
      << "\nFATAL ERROR: (y_val,x_val) = (" << y_val << ", " << x_val << ") "
      << "lies outside of table's bounding box!\n"
      << ">> Bounding Box:\n"
      << "(y_min,y_max) = (" << y_min << ", " << y_max << ")\n"
      << "(x_min,x_max) = (" << x_min << ", " << x_max << ")\n"
      << std::endl;
	  exit(0);
    }
    */

    /* use boundary rates for values outside bounding box */
    double eps = 1e-10;
    if (y_val < y_min) y_val = y_min*(1. + eps);
    if (y_val > y_max) y_val = y_max*(1. - eps);
    if (x_val < x_min) x_val = x_min*(1. + eps);
    if (x_val > x_max) x_val = x_max*(1. - eps);

	// apply any log scaling
	if (logT)  y = log10(y_val);
	else       y = y_val;
  if (logxi) x = log10(x_val);
  else       x = x_val;

	 determine_box_indices(x,y,i_x,i_y);

	return gsl_spline2d_eval(spline_table[i_y][i_x], x, y, xacc, yacc);
}

#endif //H_VEGAS_TABLE


/*
 Copyright (C) 2002 M. Marques, A. Castro, A. Rubio, G. Bertsch, M. Oliveira

 This program is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2, 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.

 $Id: minimizer_low.c 11822 2014-02-25 21:18:45Z dstrubbe $
*/

#include <config.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_multimin.h>

#include "string_f.h"

/* A. First, the interface to the gsl function that calculates the minimum
   of an N-dimensional function, with the knowledge of the function itself
   and its gradient. */

/* This is a type used to communicate with Fortran; func_d is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*func_d)(const int*, const double*, double*, const int*, double*);
typedef struct{
  func_d func;
} param_fdf_t;

/* Compute both f and df together. */
static void
my_fdf (const gsl_vector *v, void *params, double *f, gsl_vector *df)
{
  double *x, *gradient, ff2;
  int i, dim, getgrad;
  param_fdf_t * p;

  p = (param_fdf_t *) params;

  dim = v->size;
  x = (double *)malloc(dim*sizeof(double));
  gradient = (double *)malloc(dim*sizeof(double));

  for(i=0; i<dim; i++) x[i] = gsl_vector_get(v, i);
  getgrad = (df == NULL) ? 0 : 1;

  p->func(&dim, x, &ff2, &getgrad, gradient);

  if(f != NULL)
    *f = ff2;

  if(df != NULL){
    for(i=0; i<dim; i++)
      gsl_vector_set(df, i, gradient[i]);
  }

  free(x); free(gradient);
}

static double
my_f (const gsl_vector *v, void *params)
{
  double val;

  my_fdf(v, params, &val, NULL);
  return val;
}

/* The gradient of f, df = (df/dx, df/dy). */
static void
my_df (const gsl_vector *v, void *params, gsl_vector *df)
{
  my_fdf(v, params, NULL, df);
}

typedef void (*print_f_ptr)(const int*, const int*, const double*, const double*, const double*, const double*);

int FC_FUNC_(oct_minimize, OCT_MINIMIZE)
     (const int *method, const int *dim, double *point, const double *step, const double *line_tol,
      const double *tolgrad, const double *toldr, const int *maxiter, func_d f,
      const print_f_ptr write_info, double *minimum)
{
  int iter = 0;
  int status;
  double maxgrad, maxdr;
  int i;
  double * oldpoint;
  double * grad;

  const gsl_multimin_fdfminimizer_type *T = NULL;
  gsl_multimin_fdfminimizer *s;
  gsl_vector *x;
  gsl_vector *absgrad, *absdr;
  gsl_multimin_function_fdf my_func;

  param_fdf_t p;

  p.func = f;

  oldpoint = (double *) malloc(*dim * sizeof(double));
  grad     = (double *) malloc(*dim * sizeof(double));

  my_func.f = &my_f;
  my_func.df = &my_df;
  my_func.fdf = &my_fdf;
  my_func.n = *dim;
  my_func.params = (void *) &p;

  /* Starting point */
  x = gsl_vector_alloc (*dim);
  for(i=0; i<*dim; i++) gsl_vector_set (x, i, point[i]);

  /* Allocate space for the gradient */
  absgrad = gsl_vector_alloc (*dim);
  absdr = gsl_vector_alloc (*dim);

  //GSL recommends line_tol = 0.1;
  switch(*method){
  case 1:
    T = gsl_multimin_fdfminimizer_steepest_descent;
    break;
  case 2:
    T = gsl_multimin_fdfminimizer_conjugate_fr;
    break;
  case 3:
    T = gsl_multimin_fdfminimizer_conjugate_pr;
    break;
  case 4:
    T = gsl_multimin_fdfminimizer_vector_bfgs;
    break;
  case 5:
    T = gsl_multimin_fdfminimizer_vector_bfgs2;
    break;
  }

  s = gsl_multimin_fdfminimizer_alloc (T, *dim);

  gsl_multimin_fdfminimizer_set (s, &my_func, x, *step, *line_tol);
  do
    {
      iter++;
      for(i=0; i<*dim; i++) oldpoint[i] = point[i];

      /* Iterate */
      status = gsl_multimin_fdfminimizer_iterate (s);

      /* Get current minimum, point and gradient */
      *minimum = gsl_multimin_fdfminimizer_minimum(s);
      for(i=0; i<*dim; i++) point[i] = gsl_vector_get(gsl_multimin_fdfminimizer_x(s), i);
      for(i=0; i<*dim; i++) grad[i] = gsl_vector_get(gsl_multimin_fdfminimizer_gradient(s), i);

      /* Compute convergence criteria */
      for(i=0; i<*dim; i++) gsl_vector_set(absdr, i, fabs(point[i]-oldpoint[i]));
      maxdr = gsl_vector_max(absdr);
      for(i=0; i<*dim; i++) gsl_vector_set(absgrad, i, fabs(grad[i]));
      maxgrad = gsl_vector_max(absgrad);

      /* Print information */
      write_info(&iter, dim, minimum, &maxdr, &maxgrad, point);

      /* Store infomation for next iteration */
      for(i=0; i<*dim; i++) oldpoint[i] = point[i];

      if (status)
        break;

      if ( (maxgrad <= *tolgrad) || (maxdr <= *toldr) ) status = GSL_SUCCESS;
      else status = GSL_CONTINUE;
    }
  while (status == GSL_CONTINUE && iter <= *maxiter);

  if(status == GSL_CONTINUE) status = 1025;

  gsl_multimin_fdfminimizer_free (s);
  gsl_vector_free (x); gsl_vector_free(absgrad); gsl_vector_free(absdr);

  free(oldpoint);
  free(grad);

  return status;
}

/* B. Second, the interface to the gsl function that calculates the minimum
   of a one-dimensional function, with the knowledge of the function itself,
   but not its gradient. */

/* This is a type used to communicate with Fortran; func1 is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*func1)(const double*, double*);
typedef struct{
  func1 func;
} param_f1_t;

double fn1(double x, void * params)
{
  param_f1_t * p = (param_f1_t* ) params;
  double fx;
  p->func(&x, &fx);
  return fx;
}

void FC_FUNC_(oct_1dminimize, OCT_1DMINIMIZE)(double *a, double *b, double *m, func1 f, int *status)
{
  int iter = 0;
  int max_iter = 100;
  const gsl_min_fminimizer_type *T;
  gsl_min_fminimizer *s;
  gsl_function F;
  param_f1_t p;

  p.func = f;

  F.function = &fn1;
  F.params = (void *) &p;

  T = gsl_min_fminimizer_brent;
  s = gsl_min_fminimizer_alloc (T);

  *status = gsl_min_fminimizer_set (s, &F, *m, *a, *b);

  gsl_set_error_handler_off();

  do
    {
      iter++;
      *status = gsl_min_fminimizer_iterate (s);

      *m = gsl_min_fminimizer_x_minimum (s);
      *a = gsl_min_fminimizer_x_lower (s);
      *b = gsl_min_fminimizer_x_upper (s);

      *status = gsl_min_test_interval (*a, *b, 0.00001, 0.0);

      /*if (*status == GSL_SUCCESS) printf ("Converged:\n");*/
      /*printf ("%5d [%.7f, %.7f] %.7f \n", iter, *a, *b,*m);*/
    }
  while (*status == GSL_CONTINUE && iter < max_iter);
  gsl_min_fminimizer_free(s);

}

/* C. Third, the interface to the gsl function that calculates the minimum
   of an N-dimensional function, with the knowledge of the function itself,
   but not its gradient. */

/* This is a type used to communicate with Fortran; funcn is the type of the
   interface to a Fortran subroutine that calculates the function and its
   gradient. */
typedef void (*funcn)(int*, double*, double*);
typedef struct{
  funcn func;
} param_fn_t;

double fn(const gsl_vector *v, void * params)
{
  double val;
  double *x;
  int i, dim;
  param_fn_t * p;

  p = (param_fn_t *) params;
  dim = v->size;
  x = (double *)malloc(dim*sizeof(double));

  for(i=0; i<dim; i++) x[i] = gsl_vector_get(v, i);
  p->func(&dim, x, &val);

  free(x);
  return val;
}

typedef void (*print_f_fn_ptr)(const int*, const int*, const double*, const double*, const double*);

int FC_FUNC_(oct_minimize_direct, OCT_MINIMIZE_DIRECT)
     (const int *method, const int *dim, double *point, const double *step,
      const double *toldr, const int *maxiter, funcn f,
      const print_f_fn_ptr write_info, double *minimum)
{
  int iter = 0, status, i;
  double size;

  const gsl_multimin_fminimizer_type *T = NULL;
  gsl_multimin_fminimizer *s = NULL;
  gsl_vector *x, *ss;
  gsl_multimin_function my_func;

  param_fn_t p;
  p.func = f;

  my_func.f = &fn;
  my_func.n = *dim;
  my_func.params = (void *) &p;

  /* Set the initial vertex size vector */
  ss = gsl_vector_alloc (*dim);
  gsl_vector_set_all (ss, *step);

  /* Starting point */
  x = gsl_vector_alloc (*dim);
  for(i=0; i<*dim; i++) gsl_vector_set (x, i, point[i]);

  switch(*method){
  case 6:
    T = gsl_multimin_fminimizer_nmsimplex;
    break;
  }

  s = gsl_multimin_fminimizer_alloc (T, *dim);
  gsl_multimin_fminimizer_set (s, &my_func, x, ss);

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

      if(status) break;

      *minimum = gsl_multimin_fminimizer_minimum(s);
      for(i=0; i<*dim; i++) point[i] = gsl_vector_get(gsl_multimin_fminimizer_x(s), i);

      size = gsl_multimin_fminimizer_size (s);
      status = gsl_multimin_test_size (size, *toldr);

      write_info(&iter, dim, minimum, &size, point);

    }
  while (status == GSL_CONTINUE && iter < *maxiter);

  if(status == GSL_CONTINUE) status = 1025;

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free(s);
  return status;
}


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

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

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

  for (i = 0; i < n; ++i)
    {
      size_t wsize = gsl_movstat_fill(etype, x, i, H, J, window);
      double quant1, quant2;

      gsl_sort(window, 1, wsize);
      quant1 = gsl_stats_quantile_from_sorted_data(window, 1, wsize, q);
      quant2 = gsl_stats_quantile_from_sorted_data(window, 1, wsize, 1.0 - q);

      gsl_vector_set(y, i, quant2 - quant1);
    }

  free(window);

  return GSL_SUCCESS;
}

static double
func_qqr(const size_t n, double x[], void * params)
{
  double q = *(double *) params;
  double quant1, quant2;

  (void) params;

  gsl_sort(x, 1, n);
  quant1 = gsl_stats_quantile_from_sorted_data(x, 1, n, q);
  quant2 = gsl_stats_quantile_from_sorted_data(x, 1, n, 1.0 - q);

  return (quant2 - quant1);
}

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

  F.function = func_qqr;
  F.params = (void *) &q;

  random_vector(x, rng_p);

  /* y = QQR(x) with slow brute force method */
  slow_movqqr(etype, q, x, y, H, J);

  /* y = QQR(x) with fast method */
  gsl_movstat_qqr(etype, x, q, z, w);

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

  /* z = QQR(x) in-place */
  gsl_vector_memcpy(z, x);
  gsl_movstat_qqr(etype, z, q, z, w);

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

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

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

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

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

  test_qqr_proc(eps, 0.1, 100, 0, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.1, 1000, 3, 3, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.2, 1000, 0, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.3, 1000, 5, 0, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.25, 2000, 10, 5, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.4, 2000, 5, 10, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.25, 20, 10, 50, GSL_MOVSTAT_END_PADZERO, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 10, GSL_MOVSTAT_END_PADZERO, rng_p);

  test_qqr_proc(eps, 0.1, 100, 0, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.1, 1000, 3, 3, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.2, 1000, 0, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.3, 1000, 5, 0, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.25, 2000, 10, 5, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.4, 2000, 5, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 10, 50, GSL_MOVSTAT_END_PADVALUE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 10, GSL_MOVSTAT_END_PADVALUE, rng_p);

  test_qqr_proc(eps, 0.1, 100, 0, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.1, 1000, 3, 3, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.2, 1000, 0, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.3, 1000, 5, 0, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.25, 2000, 10, 5, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.4, 2000, 5, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 10, 50, GSL_MOVSTAT_END_TRUNCATE, rng_p);
  test_qqr_proc(eps, 0.25, 20, 50, 10, GSL_MOVSTAT_END_TRUNCATE, rng_p);
}


#include <gsl/gsl_spline.h>
#include <galpy_potentials.h>
// interpSphericalPotential: 6 arguments: amp (not used here), rmin, rmax,
// M(<rmax), Phi0, Phimax
double interpSphericalPotentialrevaluate(double r,double t,
					 struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  double Phi0= *(args+4);
  double Phimax= *(args+5);
  if ( r >= rmax ) {
    return -Mmax/r+Phimax;
  }
  else {
    return r < rmin ? 0. : \
      -gsl_spline_eval_integ(*potentialArgs->spline1d,
			     rmin,r,*potentialArgs->acc1d) + Phi0;
  }
}
double interpSphericalPotentialrforce(double r,double t,
				      struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  if ( r >= rmax ) {
    return -Mmax/r/r;
  }
  else {
    return r < rmin ? 0. : gsl_spline_eval(*potentialArgs->spline1d,
					   r,*potentialArgs->acc1d);
  }
}
double interpSphericalPotentialr2deriv(double r,double t,
				       struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  double Mmax= *(args+3);
  if ( r >= rmax ) {
    return -2. * Mmax / r / r / r;
  }
  else {
    return r < rmin ? 0. : -gsl_spline_eval_deriv(*potentialArgs->spline1d,
						  r,*potentialArgs->acc1d);
  }
}
double interpSphericalPotentialrdens(double r,double t,
				     struct potentialArg * potentialArgs){
  double * args= potentialArgs->args;
  //Get args
  double rmin= *(args+1);
  double rmax= *(args+2);
  if ( r >= rmax ) {
    return 0.;
  }
  else {
    return r < rmin ? 0. : M_1_PI / 4. \
      * ( interpSphericalPotentialr2deriv(r,t,potentialArgs)
	  - 2. * interpSphericalPotentialrforce(r,t,potentialArgs)/r);
  }
}


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

void init_Vmax_to_Mvir_NFW(cosmo_info **cosmo, int mode, double z) {
    interp_info *interp;
    if(!ADaPS_exist((*cosmo), "lVmax_to_lMvir_%.5f_interp", z)) {
        SID_log("Initializing Vmax->M_vir interpolation...", SID_LOG_OPEN);
        int     n_k;
        double *lk_P;
        double *lM;
        double *lVmax;
        int     n_M   = 201;
        double  lM_lo = 0.;
        double  lM_hi = 20.;
        double  dlM   = (lM_hi - lM_lo) / (double)(n_M - 1);
        lM            = (double *)SID_malloc(sizeof(double) * n_M);
        lVmax         = (double *)SID_malloc(sizeof(double) * n_M);
        int i_M;
        for(i_M = 0; i_M < n_M; i_M++) {
            if(i_M == 0)
                lM[i_M] = lM_lo;
            else if(i_M == (n_M - 1))
                lM[i_M] = lM_hi;
            else
                lM[i_M] = lM[i_M - 1] + dlM;
        }
        for(i_M = 0; i_M < n_M; i_M++) {
            lM[i_M] += take_log10(M_SOL);
            lVmax[i_M] = take_log10(V_max_NFW(take_alog10(lM[i_M]), z, mode, cosmo));
        }
        init_interpolate(lVmax, lM, (size_t)n_M, gsl_interp_cspline, &interp);
        ADaPS_store_interp(cosmo, (void *)(interp), "lVmax_to_lMvir_%.5f_interp", z);
        SID_free(SID_FARG lM);
        SID_free(SID_FARG lVmax);
        SID_log("Done.", SID_LOG_CLOSE);
    }
}
double Vmax_to_Mvir_NFW(double V_max, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double V2_vir;
    double g_c;
    double r_val = 0.;
    if(V_max > 0.) {
        interp_info *interp;
        init_Vmax_to_Mvir_NFW(cosmo, mode, z);
        interp = (interp_info *)ADaPS_fetch(*cosmo, "lVmax_to_lMvir_%.5f_interp", z);
        r_val  = take_alog10(interpolate(interp, take_log10(V_max)));
    }
    return (r_val);
}


#include <stdio.h>
#include <math.h>
#include <sys/types.h>
#include <time.h>
#include <complex.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_cdf.h>
#include <gsl/gsl_sf_gamma.h>

#define FALSE 0
#define TRUE 1

#ifndef M_PI
#define M_PI            3.141592653589793238462643383280        /* pi */
#endif

#define LN2PI 1.83787706640934548356065947

extern char parameterfilename[50];
extern char parameterlogfilename[50];

#define MBIG 1000000000
#define MSEED 161803398
#define MZ 0
#define FAC (1.0/MBIG)
#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define MAX(A,B) ((A) > (B) ? (A) : (B))
enum {NOT_AVAILABLE=0, ABOVE_DETECTION=1, BELOW_DETECTION=2, MEASURED=3};

typedef struct {
  int n;
  int m;
  int **a;
} Matrix;

void ReadParam( char*, void*, char* ) ;
void ExperimentNumber( char*, char* );
void Error( char* );
void FileError( char*, char* );
double ***doublematrix3( int, int, int );
double **doublematrix( int, int );
int **integermatrix( int, int );
uint **uintegermatrix( int, int );
uint *uintegervector( int );
char **charmatrix( int, int );
double *doublevector( int );
int *integervector( int );
char *charvector( int );

void Int_HeapSort(int*, int);
void HeapSort( double*, int );
void circmeanvar( double*, int, double*, double* );
void meanvar( double*, int, double*, double* );
void meanvarl( long double*, int, long double*, long double* );
void wmeanvar( long double *x, int n, long double *m, long double *s2 );
double mean( double*, int, int );
int choldc_block(double**, long long);

uint   binomialRNG(uint, double, const gsl_rng*);
void   multinomialRNG(uint, const double*, size_t, unsigned int*,
		      const gsl_rng*);
int    poissonRNG(double, const gsl_rng*);
int    uniform_intRNG(int, int, const gsl_rng*);
double normalRNG(double, double, const gsl_rng*);
double betaRNG(double, double, const gsl_rng*);
double chisqRNG(double, const gsl_rng*);
double scaleinvchisqRNG(double, double, const gsl_rng*);
double invgammaRNG(double, double, const gsl_rng*);
double gammaRNG(double, double, const gsl_rng*);
double uniformRNG(double, double, const gsl_rng*);
double paretoRNG(double, double, const gsl_rng*);
void   dirichlet_multinomialRNG(uint, const double*, size_t, uint*,
				const gsl_rng*);
void   dirichletRNG(const double*, size_t, double*, const gsl_rng*);
uint   beta_binomialRNG(uint, double, double, const gsl_rng*);
uint   negative_binomialRNG(double, double, const gsl_rng*);
uint   beta_negative_binomialRNG(uint, double, double, const gsl_rng*);
/* Matrix *wishartRNG(int, int, Matrix*, Matrix*, const gsl_rng*); */
/* void   MVNRNG(int, Matrix*, Matrix*, Matrix*, const gsl_rng*); */

double poissonCMF(const double, const double, int);
double gamma_scalePDF(const double, const double, const double);
double betaPDF(const double, const double, const double);
double exponentialPDF(const double, const double);
double poissonPMF(const double, const double);
double geometricPDF(const double, const double);
double trnormalPDF(const double, const double, const double);
double normalCDF(const double, const double, const double, int);
double normalPDF(const double, const double, const double);
double binomialPMF(const double, const double, const double);
double multinomialPMF(const unsigned int, const double*, const double*);
double uniformPDF(const double, const double, const double);
double logisticPDF(const double, const double, const double);
double betabinomialPMF(const double, const double, const double, const double);
/* double MVNPDF(Matrix*, Matrix*, Matrix*, const double, int); */

double identity(double);
double logit(double);
double invlogit(double);

Matrix *MatrixInit(Matrix*, int, int, int*);
Matrix *MatrixDestroy(Matrix*);
/* Matrix *Matrix_Init(Matrix*, char*, int, int); */
/* Matrix *Matrix_Fill(Matrix*, int, int, ...); */
/* Matrix *Matrix_Destroy(Matrix*); */
/* Matrix *Matrix_Copy(Matrix*, Matrix*); */
/* Matrix *Matrix_Sub_Copy(Matrix*, Matrix*, int); */
/* Matrix *Matrix_Scalar_Multiply(Matrix*, double, Matrix*); */
/* Matrix *Matrix_Multiply(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Multiply(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Cholesky(Matrix*, Matrix*); */
/* Matrix *Matrix_Invert(Matrix*, Matrix*); */
/* Matrix *Matrix_Add(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Add_Diagonal(Matrix*, Matrix*); */
/* Matrix *Matrix_Subtract(Matrix*, Matrix*, Matrix*); */
/* Matrix *Matrix_Transpose(Matrix*, Matrix*); */
/* double Matrix_Log_Determinant(Matrix*, int); */
/* void   Matrix_Print(Matrix*); */

double rtnewt(void(*)(double, double*, double*, double*),
	      double, double, double, double*);
double rtbis( double (*)(double, double*), double, double, double, int*,
	      double* );

size_t strlen(const char*);
char *strncpy(char*, const char*, size_t);
char *strcpy(char*, const char*);
char *strcat(char*, const char*);
char *strchr ( const char*, int );
int  strcmp (const char*, const char*);
void *calloc(size_t, size_t);
void *malloc(size_t);
void free(void*);
void exit(int);

double von_mises_cdf ( double x, double a, double b );
double von_mises_cdf_inv ( double cdf, double a, double b );


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

#include <gsl/gsl_matrix_double.h>
#include <gsl/gsl_vector_double.h>

#include "compat.h"
#include "sip-utils.h"
#include "gslutils.h"
#include "mathutil.h"
#include "sip.h"

double wcs_pixel_center_for_size(double size) {
    return 0.5 + 0.5 * size;
}

int sip_compute_inverse_polynomials(sip_t* sip, int NX, int NY,
                                    double xlo, double xhi,
                                    double ylo, double yhi) {
    int inv_sip_order;
    int M, N;
    int i, j, p, q, gu, gv;
    double maxu, maxv, minu, minv;
    double u, v, U, V;
    gsl_matrix *mA;
    gsl_vector *b1, *b2, *x1, *x2;
    tan_t* tan;

    assert(sip->a_order == sip->b_order);
    assert(sip->ap_order == sip->bp_order);
    tan = &(sip->wcstan);

    logverb("sip_compute-inverse_polynomials: A %i, AP %i\n",
            sip->a_order, sip->ap_order);

    /*
     basic idea: lay down a grid in image, for each gridpoint, push
     through the polynomial to get yourself into warped image
     coordinate (but not yet lifted onto the sky).  Then, using the
     set of warped gridpoints as inputs, fit back to their original
     grid locations as targets.
     */
    inv_sip_order = sip->ap_order;

    // Number of grid points to use:
    if (NX == 0)
        NX = 10 * (inv_sip_order + 1);
    if (NY == 0)
        NY = 10 * (inv_sip_order + 1);
    if (xhi == 0)
        xhi = tan->imagew;
    if (yhi == 0)
        yhi = tan->imageh;

    logverb("NX,NY %i,%i, x range [%f, %f], y range [%f, %f]\n",
            NX,NY, xlo, xhi, ylo, yhi);

    // Number of coefficients to solve for:
    // We only compute the upper triangle polynomial terms
    N = (inv_sip_order + 1) * (inv_sip_order + 2) / 2;

    // Number of samples to fit.
    M = NX * NY;

    mA = gsl_matrix_alloc(M, N);
    b1 = gsl_vector_alloc(M);
    b2 = gsl_vector_alloc(M);
    assert(mA);
    assert(b1);
    assert(b2);

    /*
     *  Rearranging formula (4), (5), and (6) from the SIP paper gives the
     *  following equations:
     *
     *    +----------------------- Linear pixel coordinates in PIXELS
     *    |                        before SIP correction
     *    |                   +--- Intermediate world coordinates in DEGREES
     *    |                   |
     *    v                   v
     *                   -1
     *    U = [CD11 CD12]   * x
     *    V   [CD21 CD22]     y
     *
     *    +---------------- PIXEL distortion delta from telescope to
     *    |                 linear coordinates
     *    |    +----------- Linear PIXEL coordinates before SIP correction
     *    |    |       +--- Polynomial U,V terms in powers of PIXELS
     *    v    v       v
     *
     *    -f(u1,v1) =  p11 p12 p13 p14 p15 ... * ap1
     *    -f(u2,v2) =  p21 p22 p23 p24 p25 ...   ap2
     *    ...
     *
     *    -g(u1,v1) =  p11 p12 p13 p14 p15 ... * bp1
     *    -g(u2,v2) =  p21 p22 p23 p24 p25 ...   bp2
     *    ...
     *
     *  which recovers the A and B's.
     */

    minu = xlo - tan->crpix[0];
    maxu = xhi - tan->crpix[0];
    minv = ylo - tan->crpix[1];
    maxv = yhi - tan->crpix[1];

    // Sample grid locations.
    i = 0;
    for (gu=0; gu<NX; gu++) {
        for (gv=0; gv<NY; gv++) {
            double fuv, guv;
            // Calculate grid position in original image pixels
            u = (gu * (maxu - minu) / (NX-1)) + minu;
            v = (gv * (maxv - minv) / (NY-1)) + minv;
            // compute U=u+f(u,v) and V=v+g(u,v)
            sip_calc_distortion(sip, u, v, &U, &V);
            fuv = U - u;
            guv = V - v;
            // Polynomial terms...
            j = 0;
            for (p = 0; p <= inv_sip_order; p++)
                for (q = 0; q <= inv_sip_order; q++) {
                    if (p + q > inv_sip_order)
                        continue;
                    assert(j < N);
                    gsl_matrix_set(mA, i, j,
                                   pow(U, (double)p) * pow(V, (double)q));
                    j++;
                }
            assert(j == N);
            gsl_vector_set(b1, i, -fuv);
            gsl_vector_set(b2, i, -guv);
            i++;
        }
    }
    assert(i == M);

    // Solve the linear equation.
    if (gslutils_solve_leastsquares_v(mA, 2, b1, &x1, NULL, b2, &x2, NULL)) {
        ANERROR("Failed to solve SIP inverse matrix equation!");
        return -1;
    }

    // Extract the coefficients
    j = 0;
    for (p = 0; p <= inv_sip_order; p++)
        for (q = 0; q <= inv_sip_order; q++) {
            if ((p + q > inv_sip_order))
                continue;
            assert(j < N);
            sip->ap[p][q] = gsl_vector_get(x1, j);
            sip->bp[p][q] = gsl_vector_get(x2, j);
            j++;
        }
    assert(j == N);

    // Check that we found values that actually invert the polynomial.
    // The error should be particularly small at the grid points.
    if (true) {
        // rms error accumulators:
        double sumdu = 0;
        double sumdv = 0;
        int Z;
        for (gu = 0; gu < NX; gu++) {
            for (gv = 0; gv < NY; gv++) {
                double newu, newv;
                // Calculate grid position in original image pixels
                u = (gu * (maxu - minu) / (NX-1)) + minu;
                v = (gv * (maxv - minv) / (NY-1)) + minv;
                sip_calc_distortion(sip, u, v, &U, &V);
                sip_calc_inv_distortion(sip, U, V, &newu, &newv);
                sumdu += square(u - newu);
                sumdv += square(v - newv);
            }
        }
        sumdu /= (NX*NY);
        sumdv /= (NX*NY);
        debug("RMS error of inverting a distortion (at the grid points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));

        sumdu = 0;
        sumdv = 0;
        Z = 1000;
        for (i=0; i<Z; i++) {
            double newu, newv;
            u = uniform_sample(minu, maxu);
            v = uniform_sample(minv, maxv);
            sip_calc_distortion(sip, u, v, &U, &V);
            sip_calc_inv_distortion(sip, U, V, &newu, &newv);
            sumdu += square(u - newu);
            sumdv += square(v - newv);
        }
        sumdu /= Z;
        sumdv /= Z;
        debug("RMS error of inverting a distortion (at random points, in pixels):\n");
        debug("  du: %g\n", sqrt(sumdu));
        debug("  dv: %g\n", sqrt(sumdu));
        debug("  dist: %g\n", sqrt(sumdu + sumdv));
    }

    gsl_matrix_free(mA);
    gsl_vector_free(b1);
    gsl_vector_free(b2);
    gsl_vector_free(x1);
    gsl_vector_free(x2);

    return 0;
}

bool tan_pixel_is_inside_image(const tan_t* wcs, double x, double y) {
    return (x >= 1 && x <= wcs->imagew && y >= 1 && y <= wcs->imageh);
}

bool sip_pixel_is_inside_image(const sip_t* wcs, double x, double y) {
    return tan_pixel_is_inside_image(&(wcs->wcstan), x, y);
}

int* sip_filter_stars_in_field(const sip_t* sip, const tan_t* tan,
                               const double* xyz, const double* radec,
                               int N, double** p_xy, int* inds, int* p_Ngood) {
    int i, Ngood;
    int W, H;
    double* xy = NULL;
    bool allocd = false;

    assert(sip || tan);
    assert(xyz || radec);
    assert(p_Ngood);

    Ngood = 0;
    if (!inds) {
        inds = malloc(N * sizeof(int));
        allocd = true;
    }

    if (p_xy)
        xy = malloc(N * 2 * sizeof(double));

    if (sip) {
        W = (int)sip->wcstan.imagew;
        H = (int)sip->wcstan.imageh;
    } else {
        W = (int)tan->imagew;
        H = (int)tan->imageh;
    }

    for (i=0; i<N; i++) {
        double x, y;
        if (xyz) {
            if (sip) {
                if (!sip_xyzarr2pixelxy(sip, xyz + i*3, &x, &y))
                    continue;
            } else {
                if (!tan_xyzarr2pixelxy(tan, xyz + i*3, &x, &y))
                    continue;
            }
        } else {
            if (sip) {
                if (!sip_radec2pixelxy(sip, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            } else {
                if (!tan_radec2pixelxy(tan, radec[i*2], radec[i*2+1], &x, &y))
                    continue;
            }
        }
        // FIXME -- check half- and one-pixel FITS issues.
        if ((x < 0) || (y < 0) || (x >= W) || (y >= H))
            continue;

        inds[Ngood] = i;
        if (xy) {
            xy[Ngood * 2 + 0] = x;
            xy[Ngood * 2 + 1] = y;
        }
        Ngood++;
    }

    if (allocd)
        inds = realloc(inds, Ngood * sizeof(int));

    if (xy)
        xy = realloc(xy, Ngood * 2 * sizeof(double));
    if (p_xy)
        *p_xy = xy;

    *p_Ngood = Ngood;

    return inds;
}


// Copyright (c) Microsoft Corporation.
// Licensed under the MIT License.

#pragma once

#include <Eigen/Geometry>
#include <gsl/gsl>

namespace mage
{
    inline void ToEuler(const Eigen::Quaternionf& quat, float& xRad, float& yRad, float& zRad)
    {
        Eigen::Matrix3f eigenMat = quat.toRotationMatrix();
        Eigen::Vector3f eulerAngles = eigenMat.eulerAngles(0, 1, 2);
        xRad = eulerAngles.x();
        yRad = eulerAngles.y();
        zRad = eulerAngles.z();
    }

    inline Eigen::Quaternionf FromEuler(float xRad, float yRad, float zRad)
    {
        return Eigen::AngleAxisf(xRad, Eigen::Vector3f::UnitX())
            * Eigen::AngleAxisf(yRad, Eigen::Vector3f::UnitY())
            * Eigen::AngleAxisf(zRad, Eigen::Vector3f::UnitZ());
    }

    template<int N>
    inline Eigen::Map<const Eigen::Matrix<float, N, 1>> ToCMap(gsl::span<const float, N> values)
    {
        return Eigen::Map<const Eigen::Matrix<float, N, 1>>{ values.data() };
    }

    template<int N>
    inline Eigen::Map<Eigen::Matrix<float, N, 1>> ToMap(gsl::span<float, N> values)
    {
        return Eigen::Map<const Eigen::Matrix<float, N, 1>>{ values.data() };
    }

    template<typename T>
    inline typename T::ConstMapType ToCMap(const T& element)
    {
        return T::ConstMapType{ element.data() };
    }

    template<typename T>
    inline typename T::MapType ToMap(T& element)
    {
        return T::MapType{ element.data() };
    }

}