/** Fit a set of points to a model that is subject to rotation and translation.
 *
 * The model is defined by a set of primitive faces such as plane, cylinder, sphere, etc.
 * The measured points are first translated by (xt, yt, zt),
 * then rotated about x-y-z axes by g, b, a angles in sequence,
 *
 * These 6 parameters are the output of the fit.
 *
 * Author: Yuan Mei
 */
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_multifit_nlinear.h>

#ifdef LINE_MAX
#undef LINE_MAX
#define LINE_MAX 4096
#endif

#ifndef LINE_MAX
#define LINE_MAX 4096
#endif

/** Check if the character is a field separator. */
#ifndef sepq
#define sepq(a) ((a)==' ' || (a)=='\t')
#endif

#define NDIM   3    /**< number of dimensions */

enum {
    FACE_PLANE=1,
    FACE_CYLINDER=2,
    FACE_SPHERE=3,
    FACE_LINE=12,
    FACE_POINT=13
};

struct face {
    int ftype; /**< type of face: plane, cylinder etc... */
    double x0; /**< origin */
    double y0;
    double z0;
    double nx; /**< normal vector */
    double ny;
    double nz;
    double r;  /**< radius */
};

struct data {
    double pr;   /**< probe radius */
    const struct face *faces;
    size_t n;    /**< number of measured data points */
    size_t *fid; /**< idx of face in the registered faces */
    double *x;   /**< measured point */
    double *y;
    double *z;
    double *w;   /**< weights */
    double *x1;  /**< after transformation */
    double *y1;
    double *z1;
};

/** Distance from measured point to face. */
int dist_f(const gsl_vector *p, void *data, gsl_vector *f)
{
    double pr = ((struct data *)data)->pr;
    const struct face *faces = ((struct data *)data)->faces;
    size_t n = ((struct data *)data)->n;
    size_t *fid = ((struct data *)data)->fid;
    double *x = ((struct data *)data)->x;
    double *y = ((struct data *)data)->y;
    double *z = ((struct data *)data)->z;

    double a  = gsl_vector_get(p, 0), Ca = cos(a), Sa = sin(a);
    double b  = gsl_vector_get(p, 1), Cb = cos(b), Sb = sin(b);
    double g  = gsl_vector_get(p, 2), Cg = cos(g), Sg = sin(g);
    double xt = gsl_vector_get(p, 3);
    double yt = gsl_vector_get(p, 4);
    double zt = gsl_vector_get(p, 5);

    size_t i;
    for (i = 0; i < n; i++) {
        int ftype = faces[fid[i]].ftype;
        double x0 = faces[fid[i]].x0;
        double y0 = faces[fid[i]].y0;
        double z0 = faces[fid[i]].z0;
        double nx = faces[fid[i]].nx;
        double ny = faces[fid[i]].ny;
        double nz = faces[fid[i]].nz;
        double r  = faces[fid[i]].r;

        double x1, y1, z1;
        x1 = Ca*Cb * (x[i]+xt) + (Ca*Sb*Sg-Sa*Cg) * (y[i]+yt) + (Ca*Sb*Cg+Sa*Sg) * (z[i]+zt);
        y1 = Sa*Cb * (x[i]+xt) + (Sa*Sb*Sg+Ca*Cg) * (y[i]+yt) + (Sa*Sb*Cg-Ca*Sg) * (z[i]+zt);
        z1 =  - Sb * (x[i]+xt) +            Cb*Sg * (y[i]+yt) +            Cb*Cg * (z[i]+zt);

        ((struct data *)data)->x1[i] = x1;
        ((struct data *)data)->y1[i] = y1;
        ((struct data *)data)->z1[i] = z1;

        double d, dist=0.0;
        switch (ftype) {
        case FACE_PLANE:
            d = -(nx*x0 + ny*y0 + nz*z0);
            dist = fabs(nx*x1 + ny*y1 + nz*z1 + d)
                / sqrt(nx*nx + ny*ny + nz*nz);
            dist -= pr;
            break;
        case FACE_CYLINDER:
        case FACE_LINE:
        {
            double dx = x1 - x0, dy = y1 - y0, dz = z1 - z0;
            double s1 = dy * nz - dz * ny, s2 = dz*nx - dx*nz, s3 = dx*ny - dy*nx;
            dist = fabs(sqrt((s1*s1 + s2*s2 + s3*s3)/(nx*nx + ny*ny + nz*nz)) - r);
            if (ftype == FACE_CYLINDER)
                dist -= pr;
        }
            break;
        case FACE_SPHERE:
        case FACE_POINT:
            dist = fabs(sqrt((x1-x0)*(x1-x0) + (y1-y0)*(y1-y0) + (z1-z0)*(z1-z0)) - r);
            if (ftype == FACE_SPHERE)
                dist -= pr;
            break;
        default:
            dist = 0.0;
            break;
        }
        gsl_vector_set(f, i, dist);
    }

    return GSL_SUCCESS;
}

void callback(const size_t iter, void *params,
              const gsl_multifit_nlinear_workspace *w)
{
    gsl_vector *f = gsl_multifit_nlinear_residual(w);
    gsl_vector *x = gsl_multifit_nlinear_position(w);
    double rcond=0;

    /* compute reciprocal condition number of J(x) */
    /* cond(J) = %8.4f, 1.0 / rcond */
    gsl_multifit_nlinear_rcond(&rcond, w);

    fprintf(stderr, "iter %2zu: a = %7.4f, b = %7.4f, g = %7.4f, xt = %7.4f, yt = %7.4f, zt = %7.4f, |f(x)| = %.4f\n",
            iter,
            gsl_vector_get(x, 0),
            gsl_vector_get(x, 1),
            gsl_vector_get(x, 2),
            gsl_vector_get(x, 3),
            gsl_vector_get(x, 4),
            gsl_vector_get(x, 5),
            gsl_blas_dnrm2(f));
}

/** Read a long line from file.
 * @param[inout] s string of the line, is allocated when s==NULL and n==0 and grown as needed.
 * @param[inout] n current size of s
 * @return s
 */
static char *file_read_long_line(char **s, size_t *n, FILE *fp)
{
    const int bufsz = LINE_MAX;
    char *p;
    size_t cnt, sz;

    if ( *s == NULL && *n == 0 ) {
        *n = bufsz;
        if ( (*s = calloc(*n, sizeof(char))) == NULL ) exit(-1);
    }
    p = *s;
    sz = *n;
    while ( 1 ) {
        if ( fgets(p, sz, fp) == NULL ) return NULL;
        cnt = strlen(*s);
        if ( (*s)[cnt-1] == '\n' ) {
            break;
        } else { /* line too long, expand the buffer */
            *n += bufsz;
            if ( (*s = realloc(*s, (*n)*sizeof(char))) == NULL ) exit(-1);
            p = *s + cnt;
            sz = bufsz;
        }
    }

    return *s;
}

/** Read face definition file.
 *
 * @param[inout] n number of faces read from file.  If the given value
 * *n > 0, *n is interpreted as the number of elements in the faces
 * array and only up to *n faces will be read from file.  However, if
 * the file contains less than *n faces, *n will be updated to reflect
 * the available number of elements.
 *
 * @param[inout] faces array of faces.  If *faces != NULL, *faces will
 * be used rather than allocated.
 */
int read_faces(const char *fname, size_t *n, struct face **faces)
{
    char *linebuf = NULL;
    size_t linen = 0;
    FILE *fp;
    if ((fp = fopen(fname, "r"))==NULL) {
        perror(fname);
        return -1;
    }
    struct face fc;
    int fid;
    size_t lid = 0;
    ssize_t nelem = -1;
    if (*faces == NULL) {
        /* get number of elements in the file */
        while (file_read_long_line(&linebuf, &linen, fp)) {
            lid++;
            if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
            int ret = sscanf(linebuf, "%d %d %lf %lf %lf %lf %lf %lf %lf", &fid, &fc.ftype,
                             &fc.x0, &fc.y0, &fc.z0, &fc.nx, &fc.ny, &fc.nz, &fc.r);
            if (ret < 9 || fid < 0 || fc.ftype <= 0) {
                fprintf(stderr, "Malformatted face at line %zd\n", lid);
            } else {
                if (fid > nelem) nelem = fid;
            }
        }
        nelem++;
        if (nelem == 0) {
            fprintf(stderr, "No valid face in file.\n");
            return -1;
        } else {
            fprintf(stderr, "%zd faces available in file.\n", nelem);
        }
        *n = nelem;
        if ((*faces = calloc(nelem, sizeof(struct face))) == NULL) {
            perror("calloc *faces");
            return -1;
        }
    }
    rewind(fp);
    nelem = 0;
    lid = 0;
    while (file_read_long_line(&linebuf, &linen, fp) && (nelem < *n)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int ret = sscanf(linebuf, "%d %d %lf %lf %lf %lf %lf %lf %lf", &fid, &fc.ftype,
                         &fc.x0, &fc.y0, &fc.z0, &fc.nx, &fc.ny, &fc.nz, &fc.r);
        if (ret < 9 || fid < 0 || fc.ftype <= 0) {
            fprintf(stderr, "Malformatted face at line %zd, skipped.\n", lid);
        } else {
            if (fid >= *n) break;
            struct face *fc1 = *faces;
            memcpy(&fc1[fid], &fc, sizeof(fc));
            nelem++;
        }
    }

    fprintf(stderr, "%zd faces constructed.\n", nelem);
    free(linebuf);
    fclose(fp);
    return 0;
}

int read_points(const char *fname, size_t *n, struct data *data)
{
    char *linebuf = NULL;
    size_t linen = 0;
    FILE *fp;
    if ((fp = fopen(fname, "r"))==NULL) {
        perror(fname);
        return -1;
    }

    int fidmax = -1;
    *n = 0;
    size_t lid = 0;
    while (file_read_long_line(&linebuf, &linen, fp)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int ret = 0;
        for (int i=0; i<linen; i++) { /* count number of ';' */
            if (linebuf[i] == ';') ret++;
        }
        int fid;
        sscanf(linebuf, "%*d;%d", &fid);
        if (ret < 8 || fid < 0) {
            fprintf(stderr, "Malformatted point at line %zd\n", lid);
        } else {
            (*n)++;
            if (fid > fidmax) fidmax = fid;
        }
    }
    fprintf(stderr, "%zd points in file.  fidmax = %d\n", *n, fidmax);
    data->n = *n;
    data->fid = calloc(data->n, sizeof(size_t));
    data->x   = calloc(data->n, sizeof(double));
    data->y   = calloc(data->n, sizeof(double));
    data->z   = calloc(data->n, sizeof(double));
    data->w   = calloc(data->n, sizeof(double));
    data->x1  = calloc(data->n, sizeof(double));
    data->y1  = calloc(data->n, sizeof(double));
    data->z1  = calloc(data->n, sizeof(double));

    rewind(fp);

    lid = 0;
    size_t idx = 0;
    while (file_read_long_line(&linebuf, &linen, fp)) {
        lid++;
        if (linebuf[0] == '#' || linebuf[0] == '\n') continue;
        int fid = 0, ret = 0;
        double x, y, z, s;
        // ret = sscanf(linebuf, "%*d;%d;%*s ;%*d;%lf;%lf;%lf;;%lf", &fid, &x, &y, &z, &s);
        char *buf = linebuf, *endptr;
        for (; *buf != ';' ; buf++){;} buf++; /* find the next character past a ';' */
        fid = strtol(buf, &endptr, 0); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        x = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        y = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        z = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;
        for (; *buf != ';' ; buf++){;} buf++;
        for (; *buf != ';' ; buf++){;} buf++;
        s = strtod(buf, &endptr); if (endptr > buf) ret++; buf = endptr++;

        if (ret < 5 || fid < 0) {
            fprintf(stderr, "Malformatted point at line %zd, skipped.\n", lid);
        } else {
            data->fid[idx] = fid;
            data->x[idx] = x;
            data->y[idx] = y;
            data->z[idx] = z;
            data->w[idx] = (fabs(s)>1e-8) ? (1.0/(s*s)) : 1.0;
            idx++;
        }
    }

    fprintf(stderr, "%zd points read.\n", idx);
    free(linebuf);
    fclose(fp);
    return 0;
}

int main(int argc, char **argv)
{
    if (argc != 10) {
        fprintf(stderr,
                "Usage: %s faces_file points_file pr a b g xt yt zt\n\n"
                "    pr is the probe radius.\n"
                "    The last 6 parameters are initial guesses.\n"
                "    Points are first translated, then rotated about x(g), y(b), z(a).\n"
                "    The 2nd column in points_file shall be faceid,\n"
                "        which is the first column of faces_file.\n"
                "    The last column in points_file shall be measurement sigma.\n", argv[0]);
        return EXIT_FAILURE;
    }
    size_t nfaces=0;
    struct face *faces=NULL;
    read_faces(argv[1], &nfaces, &faces);

    size_t nd=0;
    struct data data;
    read_points(argv[2], &nd, &data);
    data.faces = faces;

    data.pr = atof(argv[3]);

    #define np 6
    double p_init[np] = {0}; /* starting values */
    for (int i=0; i<np; i++) {
        p_init[i] = atof(argv[4+i]);
    }

    const gsl_multifit_nlinear_type *T = gsl_multifit_nlinear_trust;
    gsl_multifit_nlinear_workspace *w;
    gsl_multifit_nlinear_fdf fdf;
    gsl_multifit_nlinear_parameters fdf_params =
        gsl_multifit_nlinear_default_parameters();

    gsl_vector *f;
    gsl_matrix *J;
    gsl_matrix *covar = gsl_matrix_alloc(np, np);

    gsl_vector_view params = gsl_vector_view_array(p_init, np);
    gsl_vector_view wts = gsl_vector_view_array(data.w, nd);
    gsl_rng *r;
    double chisq, chisq0;
    int status, info;

    const double xtol = 1e-8;
    const double gtol = 1e-8;
    const double ftol = 1e-8;

    gsl_rng_env_setup();
    r = gsl_rng_alloc(gsl_rng_default);

    /* define the function to be minimized */
    fdf.f = dist_f;
    fdf.df = NULL;   /* set to NULL for finite-difference Jacobian */
    fdf.fvv = NULL;  /* not using geodesic acceleration */
    fdf.n = nd;
    fdf.p = np;
    fdf.params = &data; /* `data' supplied to the penalty function */

    /* allocate workspace with default parameters */
    w = gsl_multifit_nlinear_alloc(T, &fdf_params, nd, np);

    /* initialize solver with starting point and weights */
    gsl_multifit_nlinear_winit(&params.vector, &wts.vector, &fdf, w);

    /* compute initial cost function */
    f = gsl_multifit_nlinear_residual(w);
    gsl_blas_ddot(f, f, &chisq0);

    /* solve the system with a maximum of 100 iterations */
    status = gsl_multifit_nlinear_driver(100, xtol, gtol, ftol,
                                         callback, NULL, &info, w);

    /* compute covariance of best fit parameters */
    J = gsl_multifit_nlinear_jac(w);
    gsl_multifit_nlinear_covar(J, 0.0, covar);

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

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

    fprintf(stderr, "summary from method '%s/%s'\n",
            gsl_multifit_nlinear_name(w),
            gsl_multifit_nlinear_trs_name(w));
    fprintf(stderr, "number of iterations: %zu\n",
            gsl_multifit_nlinear_niter(w));
    fprintf(stderr, "function evaluations: %zu\n", fdf.nevalf);
    fprintf(stderr, "Jacobian evaluations: %zu\n", fdf.nevaldf);
    fprintf(stderr, "reason for stopping: %s\n",
            (info == 1) ? "small step size" : "small gradient");
    fprintf(stderr, "initial |f(x)| = %f\n", sqrt(chisq0));
    fprintf(stderr, "final   |f(x)| = %f\n", sqrt(chisq));

    {
        double dof = nd - np;
        double c = GSL_MAX_DBL(1, sqrt(chisq / dof));

        fprintf(stderr, "chisq/dof = %g\n", chisq / dof);

        fprintf(stderr, "a  = %16g +/- %g\n", FIT(0), c*ERR(0));
        fprintf(stderr, "b  = %16g +/- %g\n", FIT(1), c*ERR(1));
        fprintf(stderr, "g  = %16g +/- %g\n", FIT(2), c*ERR(2));
        fprintf(stderr, "xt = %16g +/- %g\n", FIT(3), c*ERR(3));
        fprintf(stderr, "yt = %16g +/- %g\n", FIT(4), c*ERR(4));
        fprintf(stderr, "zt = %16g +/- %g\n", FIT(5), c*ERR(5));
    }

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

    /* compute residual of every point */
    dist_f(w->x, &data, f); // pure distance, no weights.
    printf("#    fid   ftype  distance          x1          y1          z1\n");
    for (int i=0; i<nd; i++) {
        printf("%8zd %7d %9.6f %11.5f %11.5f %11.5f\n", data.fid[i], faces[data.fid[i]].ftype,
               gsl_vector_get(f, i), data.x1[i], data.y1[i], data.z1[i]);
    }

    gsl_multifit_nlinear_free(w);
    gsl_matrix_free(covar);
    gsl_rng_free(r);

    free(faces);
    free(data.fid);
    free(data.x);
    free(data.y);
    free(data.z);
    free(data.w);
    free(data.x1);
    free(data.y1);
    free(data.z1);
    return EXIT_SUCCESS;
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_cblas.h>
#include "cblas.h"

void
cblas_dger (const enum CBLAS_ORDER order, const int M, const int N,
            const double alpha, const double *X, const int incX,
            const double *Y, const int incY, double *A, const int lda)
{
#define BASE double
#include "source_ger.h"
#undef BASE
}


#ifndef H_GPC_UTILS
#define H_GPC_UTILS

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_poly.h>

/* The GSL is a pain to work with. The library DOES NOT HAVE a determinant() function
  or an inv() function: you have to write your own routines. */

#define gmg gsl_matrix_get
#define gms gsl_matrix_set

void m_trans(const gsl_matrix*A, gsl_matrix*A_t);
void m_mult(const gsl_matrix*A, const gsl_matrix*B, gsl_matrix*AB);
void m_add(const gsl_matrix*A, const gsl_matrix*B, gsl_matrix*ApB);
void m_add_to(const gsl_matrix*A, gsl_matrix*B);
void m_scale(double m, gsl_matrix*A);
double m_dot(const gsl_matrix*A,const gsl_matrix*B);
void m_inv(const gsl_matrix*A, gsl_matrix*invA);
double m_det(const gsl_matrix*A);

/* Returns the real part of the roots in roots */
int poly_real_roots(unsigned int n, const double*a, double *roots);

int poly_greatest_real_root(unsigned int n, const double*a, double *root);

void m_display(const char*str, gsl_matrix*m);

#endif


//
// Created by daeyun on 7/3/17.
//

#pragma once

#include <random>
#include <algorithm>
#include <gsl/gsl_assert>
#include <cstring>

namespace scene3d {
namespace Random {

static std::mt19937 &Engine() {
  thread_local static std::mt19937 engine{std::random_device{}()};
  return engine;
}

template<class Iter>
static void Shuffle(Iter begin, Iter end) {
  std::shuffle(begin, end, Engine());
}

static int UniformInt(int n) {
  std::uniform_int_distribution<decltype(n)> dist{0, n - 1};
  return dist(Engine());
}

static double Rand() {
  std::uniform_real_distribution<double> dist{0, 1};
  return dist(Engine());
}

// If n is greater than pool size, some items will be chosen more than once.
template<class T>
static void ChooseN(size_t n, std::vector<T> *mutable_pool, std::vector<T> *choices) {
  Expects(!mutable_pool->empty());
  auto left_size = mutable_pool->size();
  for (int i = 0; i < n; ++i) {
    std::string RandomString(size_t length);
    if (left_size <= 0) {
      left_size = mutable_pool->size();
    }
    auto &chosen = mutable_pool->at(UniformInt(left_size));
    choices->push_back(chosen);
    std::swap(chosen, mutable_pool->at(left_size - 1));
    --left_size;
  }
}

static std::string RandomString(size_t length) {
  auto randchar = []() -> char {
    constexpr char charset[] =
        "0123456789"
            "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
            "abcdefghijklmnopqrstuvwxyz";
    auto i = UniformInt(static_cast<int>(std::strlen(charset)));
    return charset[i];
  };
  std::string str(length, 0);
  std::generate_n(str.begin(), length, randchar);
  return str;
}

}
}


//////////////////////////////////////////////////////////////////////////////////
// Stefano Allesina Dec 2009
// sallesina@uchicago.edu
// Given a matrix generated by the niche model and a vector containing
// the positions of the species used to generate the matrix, finds the likelihood.
//////////////////////////////////////////////////////////////////////////////////
// Compile
// gcc NewNicheLikelihood.c -o LNICHE -lgsl -lgslcblas -Wall -DHAVE_INLINE -O3
//////////////////////////////////////////////////////////////////////////////////
// Run
// ./LNICHE 4 Matrix4.txt Vec4.txt
// Where:
// * 4 is the number of species
// * Matrix4.txt is a text file containing a space separated adjacency matrix
//               where the rows are the resources and the cols are the consumers
// * Vec4.txt is a space separated text file containing the positions (in [0,1.0])
//            of the species
//////////////////////////////////////////////////////////////////////////////////
// Output
// NONE
//////////////////////////////////////////////////////////////////////////////////

#include<stdio.h>
#include<math.h>
#include <stdlib.h>
#include<gsl/gsl_math.h>
#include<gsl/gsl_integration.h>
#include<gsl/gsl_vector.h>
#include<gsl/gsl_matrix.h>
#include<gsl/gsl_rng.h>
#include<gsl/gsl_randist.h>
#include<gsl/gsl_cdf.h>
#include<gsl/gsl_blas.h>
#include<gsl/gsl_linalg.h>
#include<gsl/gsl_permutation.h>
#include<gsl/gsl_sort_vector_double.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_plain.h>
#include <gsl/gsl_monte_miser.h>
#include <gsl/gsl_monte_vegas.h>

// GLOBAL FILE FOR OUTPUT
FILE * FOUT;

// Structure needed for numerical integration
struct myP { double nlm1; double nl; double nr; double nrp1; double n; double beta;};

// UTILITIES
int PrintAdjMat(gsl_matrix * X, int a){
  int i,j;
  for (i=0;i<a;i++){
      for (j=0;j<a;j++){
	fprintf(FOUT,"%1.0f ",gsl_matrix_get(X,i,j));
      }
      fprintf(FOUT,"\n");
  }
  return 0;
}
int PrintVector(gsl_vector * X, int a){
  int i;
  for (i=0;i<a;i++){
    fprintf(FOUT,"%1.3f ",gsl_vector_get(X,i));
  }
  fprintf(FOUT,"\n");
  return 0;
}
// END UTILITIES

// INTEGRATION FOR CONSUMERS
double Inte(double x, void *p){
  struct myP * params  = (struct myP *)p;
  double nlm1=(params->nlm1);
  double nl=(params->nl);
  double nr=(params->nr);
  double nrp1=(params->nrp1);
  double n=(params->n);
  double beta=(params->beta);
  double res=0;
  res=gsl_ran_beta_pdf(x,1.0,beta);
  //fprintf(stderr,"Pdf %f\n",res);
  // Difference
  double c0,c1;
  double r2=(double )x*n/2.;
  if (x*n<=GSL_MIN_DBL(nr-nlm1,nrp1-nl)){
    // case '
    c0=GSL_MIN_DBL(n,nr-r2);
    c1=GSL_MIN_DBL(n,nl+r2);
  }
  else{
    if (x*n<=GSL_MAX_DBL(nr-nlm1,nrp1-nl)){
      // case ''
      if ((nr-nlm1)<=(nrp1-nl)){
	// a<=b
	c0=GSL_MIN_DBL(n,nlm1+r2);
	c1=GSL_MIN_DBL(n,nl+r2);
      }
      else{
	// b<a
	c0=GSL_MIN_DBL(n,nr-r2);
	c1=GSL_MIN_DBL(n,nrp1-r2);
      }
    }
    else{
      // case '''
      c0=GSL_MIN_DBL(n,nlm1+r2);
      c1=GSL_MIN_DBL(n,nrp1-r2);
    }
  }
  res*=(c1-c0);
  //fprintf(stderr,"Center %f (%f)\n",res,(c1-c0));
  // Normalization
  if ((n+r2)<1.0){
    res/=(n-r2);
  }
  else{
    res/=(1.0-2.0*r2);
  }
  //fprintf(stderr,"Rescale %f\n",res);
  return (res);
}

double Integrate (double nlm1, double nl, double n, double nr, double nrp1, double Beta){
  double error;
  double result;
  double low,upp;
  gsl_function F;
  struct myP params = { 0., 0., 0., 0., 0., 0.};
  // setup the environment
  gsl_integration_workspace * w= gsl_integration_workspace_alloc (10000);
  params.beta=Beta;
  params.nlm1=nlm1;
  params.nl=nl;
  params.n=n;
  params.nr=nr;
  params.nrp1=nrp1;

  F.function = &Inte;
  F.params = &params;
  low=GSL_MIN_DBL((nr-nl)/n,1.0);
  upp=GSL_MIN_DBL((nrp1-nlm1)/n,1.0);
  gsl_integration_qags(&F,low,upp,0,1e-4,10000,w,&result, &error);
  //fprintf(stderr,"Prob %f\n",result);
  if (result==0.0) {
    fprintf(FOUT,"The diet is incompatible with the niche model!!\n");
    return -10000.0; // The diet is impossible!
  }
  return log(result);
}

// INTEGRATION FOR BASAL SPECIES
double InteBasal(double x, void *p){
  struct myP * params  = (struct myP *)p;
  double nlm1=(params->nlm1);
  double nrp1=(params->nrp1);
  double n=(params->n);
  double beta=(params->beta);
  double res=0;
  res=gsl_ran_beta_pdf(x,1.0,beta);
  double r2=(double )x*n/2.;
  double c1=nrp1-r2;
  double c0=nlm1+r2;
  res*=(c1-c0);
  //fprintf(stderr,"Center %f (%f)\n",res,(c1-c0));
  // Normalization
  if ((n+r2)<1.0){
    res/=(n-r2);
  }
  else{
    res/=(1.0-2.0*r2);
  }
  return (res);
}

double IntegrateBasal(double GapLeft, double GapRight, double n, double Beta){
  double error;
  double result;
  double low,upp;
  gsl_function F;
  struct myP params = { 0., 0., 0., 0., 0., 0.};
  // setup the environment
  gsl_integration_workspace * w= gsl_integration_workspace_alloc (10000);
  params.beta=Beta;
  params.nlm1=GapLeft;
  params.nl=0.0;
  params.n=n;
  params.nr=0.0;
  params.nrp1=GapRight;

  F.function = &InteBasal;
  F.params = &params;
  low=GSL_MIN_DBL(0.0,1.0);
  upp=GSL_MIN_DBL((GapRight-GapLeft)/n,1.0);
  gsl_integration_qags(&F,low,upp,0,1e-4,10000,w,&result, &error);
  //fprintf(stderr,"Prob %f\n",result);
  if (result==0.0) {
    fprintf(FOUT,"The diet is incompatible with the niche model!!\n");
    return -10000000.0; // The diet is impossible!
  }
  return (result);
}

// COMPUTE THE LIKELIHOOD OF A MATRIX PERFECTLY COMPATIBLE WITH THE NICHE MODEL
double ComputeLikelihood(gsl_matrix *A, gsl_vector *Pos, int N, double Beta){
  double Prob=0.0;
  int i,j;
  double nlm1, nl, n, nr, nrp1;
  double TmpProb;
  for (i=0;i<N;i++){
    if (gsl_matrix_get(A,i,0)==1){
      fprintf(FOUT,"The first species is not basal!!\n");
      Prob-=100000000.0;
    }
  }
  if (Prob>-1.0) fprintf(FOUT,"First    Sp 0, LogLikelihood Diet 0.0 (1.0)\n");
  for (i=1;i<N;i++){
    n=gsl_vector_get(Pos,i);
    nlm1=nl=0.0;
    nr=nrp1=1.0;
    // Find first prey
    for (j=0;j<N;j++){
      if (gsl_matrix_get(A,j,i)==1){
	nl=gsl_vector_get(Pos,j);
	if (j>0){
	  nlm1=gsl_vector_get(Pos,j-1);
	}
	break;
      }
    }
    if (nl>0.0){
      // i is a predator
      // Find the last prey
      for (j=N-1;j>=0;j--){
	if (gsl_matrix_get(A,j,i)==1){
	  nr=gsl_vector_get(Pos,j);
	  if (j<(N-1)){
	    nrp1=gsl_vector_get(Pos,j+1);
	  }
	  break;
	}
      }
      // Now compute the probability
      TmpProb=0.0;
      //fprintf(stderr,"Computing probability of %d (%f %f %f %f %f) Beta: %f\n", i, nlm1, nl, n, nr, nrp1, Beta);
      TmpProb=Integrate(nlm1, nl, n, nr, nrp1, Beta);
      fprintf(FOUT,"Consumer Sp %d, LogLikelihood Diet %f (%f)\n",i,TmpProb,exp(TmpProb));
      Prob+=TmpProb;
    }
    else{
      TmpProb=0.0;
      // for each species compute the probability that the species is not eating any one in between
      double GapLeft, GapRight;
      for (j=0;j<=i;j++){
	if (j==0){
	  GapLeft=0.0;
	}
	else{
	  GapLeft=gsl_vector_get(Pos,j-1);
	}
	GapRight=gsl_vector_get(Pos,j);
	//fprintf(stderr,"Left: %f Right:%f Cumulative: %f\n",GapLeft,GapRight,TmpProb);
	TmpProb+=IntegrateBasal(GapLeft,GapRight, n,Beta);
      }
      fprintf(FOUT,"Basal    Sp %d, LogLikelihood Diet %f (%f)\n",i,log(TmpProb),TmpProb);
      // now that you have summed across all possibilities take the log and sum it
      Prob+=log(TmpProb);
    }
  }
  return Prob;
}

// READ THE MATRIX AND RUN THE PROBABILITY
// Main
int main(int argc, char *argv[]){
  // First argument: number of species
  int Sp=atoi(argv[1]);
  char * FileMat=(argv[2]);
  char * FilePos=(argv[3]);
  // Read the matrix
  FILE * F;
  // matrix
  gsl_matrix * A =gsl_matrix_calloc(Sp,Sp);
  F=fopen(FileMat,"rb");
  gsl_matrix_fscanf(F,A);
  fclose(F);
  int i,j;
  double C,Beta;
  C=Beta=0.0;
  // compute the C and Beta
  for (i=0;i<Sp;i++){
    for (j=0;j<Sp;j++){
      if(gsl_matrix_get(A,i,j)==1) C+=1.0;
    }
  }
  C=(double)C/(double)Sp/(double)Sp;
  Beta=((double) Sp -1.0)/(2.0*C*(double) Sp) -1.0;
  char OutFile[200];
  sprintf(OutFile,"Output-%s",FileMat);
  if ((FOUT = fopen(OutFile, "w")) == NULL){
    puts("Unable to open the file");
    return 0;
  }
  // read the vector
  gsl_vector * Eta=gsl_vector_calloc(Sp);
  F=fopen(FilePos,"rb");
  gsl_vector_fscanf(F,Eta);
  fclose(F);
  fprintf(FOUT,"Compatible Adjacency Matrix\n");
  PrintAdjMat(A,Sp);
  fprintf(FOUT,"Species Positions\n");
  PrintVector(Eta,Sp);
  fprintf(FOUT,"Beta: %f\n",Beta);
  fprintf(FOUT,"LogLikelihood: %f\n",ComputeLikelihood(A,Eta,Sp,Beta));
  gsl_matrix_free(A);
  gsl_vector_free(Eta);
  fclose(FOUT);
  return 0;
}


//This computes the IDFT (inverse discrete Fourier transformation) along dim of matrix X.
//This uses a CBLAS matrix multiplication by the IDFT matrix.

//The real-valued case is complicated (i.e., where only real part is output).
//This will only exactly invert with DFT if the DFT output all F=ndft/2+1 positive freqs.
//That is, use of this function to invert a specific DFT requires knowledge of how that DFT was run.

//Profile note: I tried splitting the matrix multiply into two halves,
//for positive and negative freqs, so that X would not need to be copied into Xc,
//but it was definitely slower.

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <cblas.h>

#ifndef M_PI
    #define M_PI 3.14159265358979323846
#endif

#ifdef __cplusplus
namespace codee {
extern "C" {
#endif

int idft_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);
int idft_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc);

int idft_cblas_s (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idft_cblas_s: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndft<Lx) { fprintf(stderr,"error in idft_cblas_s: ndft must be >= Lx (length of vecs in X)\n"); return 1; }

    //Scaling
    const float s = sc ? 2.0f*sqrtf(0.5f*(float)ndft)/(float)ndft : 1.0f/(float)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Init IDFT matrix
        const size_t NN = ndft * ndft;
        const float P2_N = (float)(2.0*M_PI/(double)ndft);
        float *IDFT;
        IDFT = (float *)aligned_alloc(sizeof(float),2u*NN*sizeof(float));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<ndft; ++l) { *IDFT++ = s; *IDFT++ = 0.0f; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<ndft; ++l)
            {
                *IDFT++ = s * cosf(P2_N*(float)n*(float)l);
                *IDFT++ = s * sinf(P2_N*(float)n*(float)l);
            }
        }
        IDFT -= 2u*NN;

        //Init complex matrix multiply
        const float z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};
        float *Xc, *Yc;
        Xc = (float *)aligned_alloc(sizeof(float),2u*ndft*sizeof(float));
        Yc = (float *)aligned_alloc(sizeof(float),2u*ndft*sizeof(float));
        if (!Xc) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        if (!Yc) { fprintf(stderr,"error in idft_cblas_s: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<2u*ndft; ++n, ++Xc) { *Xc = 0.0f; }
        Xc -= 2u*ndft;

        if (Lx==N)
        {
            //Copy into Xc
            for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
            if (ndft%2u==0u) { X -= 2; }
            for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
            Xc -= 2u*ndft;

            //Matrix multiply
            cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

            //Output real part only
            cblas_scopy((int)ndft,Yc,2,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=ndft)
                {
                    //Copy into Xc
                    for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
                    if (ndft%2u==0u) { X -= 2; }
                    for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
                    Xc -= 2u*ndft;
                    X += 2u*(ndft-ndft/2u-ndft%2u);

                    //Matrix multiply
                    cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                    //Output real part only
                    cblas_scopy((int)ndft,Yc,2,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u*K*(ndft-ndft/2u-ndft%2u-Lx)+2u, ++Y)
                    {
                        //Copy into Xc
                        for (size_t nf=Lx; nf>0u; --nf, X+=2u*K) { *Xc++ = *X; *Xc++ = *(X+1); }
                        if (ndft%2u==0u) { X -= 2u*K; }
                        for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2u*K; *Xc++ = *X; *Xc++ = -*(X+1); }
                        Xc -= 2u*ndft;

                        //Matrix multiply
                        cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                        //Output real part only
                        cblas_scopy((int)ndft,Yc,2,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT); free(Xc); free(Yc);
    }

    return 0;
}

int idft_cblas_d (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idft_cblas_d: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndft<Lx) { fprintf(stderr,"error in idft_cblas_d: ndft must be >= Lx (length of vecs in X)\n"); return 1; }

    //Scaling
    const double s = sc ? 2.0*sqrt(0.5*(double)ndft)/(double)ndft : 1.0/(double)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = *X; }
    }
    else
    {
        //Init IDFT matrix
        const size_t NN = ndft * ndft;
        const double P2_N = 2.0*M_PI/(double)ndft;
        double *IDFT;
        IDFT = (double *)aligned_alloc(sizeof(double),2u*NN*sizeof(double));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<ndft; ++l) { *IDFT++ = s; *IDFT++ = 0.0; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<ndft; ++l)
            {
                *IDFT++ = s * cos(P2_N*(double)n*(double)l);
                *IDFT++ = s * sin(P2_N*(double)n*(double)l);
            }
        }
        IDFT -= 2u*NN;

        //Init complex matrix multiply
        const double z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};
        double *Xc, *Yc;
        Xc = (double *)aligned_alloc(sizeof(double),2u*ndft*sizeof(double));
        Yc = (double *)aligned_alloc(sizeof(double),2u*ndft*sizeof(double));
        if (!Xc) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        if (!Yc) { fprintf(stderr,"error in idft_cblas_d: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t n=0u; n<2u*ndft; ++n, ++Xc) { *Xc = 0.0; }
        Xc -= 2u*ndft;

        if (Lx==N)
        {
            //Copy into Xc
            for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
            if (ndft%2u==0u) { X -= 2; }
            for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
            Xc -= 2u*ndft;

            //Matrix multiply
            cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

            //Output real part only
            cblas_dcopy((int)ndft,Yc,2,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=ndft)
                {
                    //Copy into Xc
                    for (size_t nf=Lx; nf>0u; --nf) { *Xc++ = *X++; *Xc++ = *X++; }
                    if (ndft%2u==0u) { X -= 2; }
                    for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2; *Xc++ = *X; *Xc++ = -*(X+1); }
                    Xc -= 2u*ndft;
                    X += 2u*(ndft-ndft/2u-ndft%2u);

                    //Matrix multiply
                    cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                    //Output real part only
                    cblas_dcopy((int)ndft,Yc,2,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2u*K*(ndft-ndft/2u-ndft%2u-Lx)+2u, ++Y)
                    {
                        //Copy into Xc
                        for (size_t nf=Lx; nf>0u; --nf, X+=2u*K) { *Xc++ = *X; *Xc++ = *(X+1); }
                        if (ndft%2u==0u) { X -= 2u*K; }
                        for (size_t nf=ndft-ndft/2u; nf>1u; --nf) { X-=2u*K; *Xc++ = *X; *Xc++ = -*(X+1); }
                        Xc -= 2u*ndft;

                        //Matrix multiply
                        cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)ndft,o,IDFT,(int)ndft,Xc,1,z,Yc,1);

                        //Output real part only
                        cblas_dcopy((int)ndft,Yc,2,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT); free(Xc); free(Yc);
    }

    return 0;
}

int idft_cblas_c (float *Y, const float *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idft_cblas_c: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndft<Lx) { fprintf(stderr,"error in idft_cblas_c: ndft must be >= Lx (length of vecs in X)\n"); return 1; }

    //Scaling
    const float s = sc ? 2.0f*sqrtf(0.5f*(float)ndft)/(float)ndft : 1.0f/(float)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X * s; }
    }
    else
    {
        //Init IDFT matrix
        const size_t LN = Lx * ndft;
        const float P2_N = (float)(2.0*M_PI/(double)ndft);
        float *IDFT;
        IDFT = (float *)aligned_alloc(sizeof(float),2u*LN*sizeof(float));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_c: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l) { *IDFT++ = s; *IDFT++ = 0.0f; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<Lx; ++l)
            {
                *IDFT++ = s * cosf(P2_N*(float)n*(float)l);
                *IDFT++ = s * sinf(P2_N*(float)n*(float)l);
            }
        }
        IDFT -= 2u*LN;

        //Init complex matrix multiply
        const float z[2] = {0.0f,0.0f}, o[2] = {1.0f,0.0f};

        if (Lx==N)
        {
            //Matrix multiply
            cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=2u*ndft)
                {
                    //Matrix multiply
                    cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2, Y+=2)
                    {
                        //Matrix multiply
                        cblas_cgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,(int)K,z,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT);
    }

    return 0;
}

int idft_cblas_z (double *Y, const double *X, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t dim, const size_t ndft, const int sc)
{
    if (dim>3u) { fprintf(stderr,"error in idft_cblas_z: dim must be in [0 3]\n"); return 1; }

    const size_t N = R*C*S*H;
    const size_t Lx = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    if (ndft<Lx) { fprintf(stderr,"error in idft_cblas_z: ndft must be >= Lx (length of vecs in X)\n"); return 1; }

    //Scaling
    const double s = sc ? 2.0*sqrt(0.5*(double)ndft)/(double)ndft : 1.0/(double)ndft;

    if (N==0u || ndft==0u) {}
    else if (ndft==1u)
    {
        for (size_t n=2u*N; n>0u; --n, ++X, ++Y) { *Y = *X * s; }
    }
    else
    {
        //Init IDFT matrix
        const size_t LN = Lx * ndft;
        const double P2_N = 2.0*M_PI/(double)ndft;
        double *IDFT;
        IDFT = (double *)aligned_alloc(sizeof(double),2u*LN*sizeof(double));
        if (!IDFT) { fprintf(stderr,"error in idft_cblas_z: problem with aligned_alloc. "); perror("aligned_alloc"); return 1; }
        for (size_t l=0u; l<Lx; ++l) { *IDFT++ = s; *IDFT++ = 0.0; }
        for (size_t n=1u; n<ndft; ++n)
        {
            for (size_t l=0u; l<Lx; ++l)
            {
                *IDFT++ = s * cos(P2_N*(double)n*(double)l);
                *IDFT++ = s * sin(P2_N*(double)n*(double)l);
            }
        }
        IDFT -= 2u*LN;

        //Init complex matrix multiply
        const double z[2] = {0.0,0.0}, o[2] = {1.0,0.0};

        if (Lx==N)
        {
            //Matrix multiply
            cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
        }
        else
        {
            const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
            const size_t B = (iscolmajor && dim==0u) ? C*S*H : K;
            const size_t V = N/Lx, G = V/B;

            if (K==1u && (G==1u || B==1u))
            {
                for (size_t v=0u; v<V; ++v, Y+=2u*ndft)
                {
                    //Matrix multiply
                    cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,1,z,Y,1);
                }
            }
            else
            {
                for (size_t g=G; g>0u; --g, X+=2u*B*(Lx-1u), Y+=2u*B*(ndft-1u))
                {
                    for (size_t b=B; b>0u; --b, X+=2, Y+=2)
                    {
                        //Matrix multiply
                        cblas_zgemv(CblasRowMajor,CblasNoTrans,(int)ndft,(int)Lx,o,IDFT,(int)Lx,X,(int)K,z,Y,(int)K);
                    }
                }
            }
        }
        free(IDFT);
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <stdio.h>
#include <gsl/gsl_monte.h>
#include <gsl/gsl_monte_vegas.h>
#include <gsl/gsl_math.h>

#include "timer.c"
#include "timer.h"

extern double g (double *t, size_t dim, void *params);

double dipole_approx (double r);

double gaussian (double *x, int dim);

int main (void)
{
    double res, err;

    size_t dim = 6;
    double x1[] = { 0., 0., 0., 0., 0., 0., };
    double xu[] = { 1., 1., 1., 1., 1., 1., };
    double distmin = 1.001;
    double distmax = 4.;
    double dist;
    int np = 20;
    double nt = (distmax - distmin) / (np - 1);
    double vegas[20], dipole[20], distance[20];

    gsl_rng *r = gsl_rng_alloc (gsl_rng_taus2);
    unsigned long seed = 1UL;

    gsl_rng_set (r, seed);

    size_t calls = 1000000;

    dist = distmin;

    gsl_monte_function G = { &g, dim, &dist };

    gsl_monte_vegas_state *sv = gsl_monte_vegas_alloc (dim);

    gsl_monte_vegas_init (sv);

 // Vegas Integration
 /*commented so does not output in order to create a proper res
   printf ("#  Stat      Dist       E(r)        ErrEst      Dipolapprox\n");      */

  timer_start ();

  for (int i = 0; i < np; i++) {
     gsl_monte_vegas_integrate (&G, x1, xu, dim, calls / 5, r, sv, &res, &err);
  do
  {
  gsl_monte_vegas_integrate (&G, x1, xu, dim, calls, r, sv, &res, &err);          }
  while (fabs (gsl_monte_vegas_chisq (sv) - 1.0) > 0.2);

    fflush (stdout);
    dist += nt;
    vegas[i] = res;
    distance[i] = dist;
    dipole[i] = -2. / pow (dist, 3.);
    }

    timer_stop();

    gsl_monte_vegas_free (sv);

    double sum;
    double x[6];

    long i, j, nn;

    nn = 1000000;

    timer_start ();
    double home[20];

    dist = distmin;
    for (j = 0; j < np; j++)
    {
        sum = 0.;
        for (i = 0; i < nn; i++)
        {

        for (int k = 0; k < (int) dim; k++)
            {
                x[k] = gsl_rng_uniform (r);
            }
            sum += g (x, dim, &dist);
        }
        res = sum/nn;
        dist += nt;
        home[j] = res;
    }

     timer_stop ();

     gsl_rng_free (r);

     double homeerr = 0.0;

    for (int e = 0; e < np; e++)
    {
    homeerr += fabs(home[e] - vegas[e]);
    }
    printf("#    Dist             Vegas          Home         Dipolapprox\n");
    for( int l = 0; l < np; l++)
    {
    double dd = distance[l];
    double vv = fabs(vegas[l]);
    double hh = fabs(home[l]);
    double di = fabs(dipole[l]);
    printf("   %.6f         %.6f       %.6f      %.6f\n", dd, vv, hh, di);
    }

    return 0;
}


#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 set_NFW_params(double M, double z, int mode, cosmo_info **cosmo, double *c_vir, double *R_vir) {
    if(mode != NFW_MODE_DEFAULT)
        SID_exit_error("Unknown mode (%d) in set_NFW_params()", SID_ERROR_LOGIC, mode);

    switch(ADaPS_exist(*cosmo, "M_WDM")) {
        case GBP_FALSE: {
            double Omega_M  = ((double *)ADaPS_fetch(*cosmo, "Omega_M"))[0];
            double h_Hubble = ((double *)ADaPS_fetch(*cosmo, "h_Hubble"))[0];

            // Mass-concentration from Munoz-Cuartas et al 2010
            double w     = 0.029;
            double m     = 0.097;
            double alpha = -110.001;
            double beta  = 2469.720;
            double gamma = 16.885;
            double a_z   = w * z - m;
            double b_z   = alpha / (z + gamma) + beta / pow(z + gamma, 2.);
            double Delta = Delta_vir(z, *cosmo);
            Delta        = 200.;

            (*c_vir) = take_alog10(a_z * take_log10(M / (M_SOL / h_Hubble)) + b_z);
            (*R_vir) = R_Delta_z(M, Delta, z, *cosmo); // Bullock et al '01
        } break;
        case GBP_TRUE:
            SID_exit_error("ENS not working.", SID_ERROR_LOGIC);
            //(*c_vir)=c_ENS(M,z,*cosmo);          // Eke, Navarro and Steinmetz
            //(*R_vir)=R_Delta_z(M,200.,z,*cosmo); // R_200
            break;
    }
}


#ifndef SIGNAL_H_
#define SIGNAL_H_

#include <gsl/gsl_statistics_double.h>
#include <complex>
#include <fftw3.h>
#include <math.h>
#include <iostream>
#include "fourier_transform.h"
#include "array.h"
#include "rng.h"
#include <assert.h>

// this is slowly turning into sth that should maybe rather be called
// "stochastic processes" rather than signal

namespace neurophys {

class SpectrumDescriptor
{
public:
    virtual double get(const double f) const = 0;
};

class WhiteSpectrum: public SpectrumDescriptor
{
public:
    WhiteSpectrum(const double s0): s0_(s0) {}
    virtual double get(const double f) const { return s0_; }
private:
    const double s0_;
};

class BandLimitedFlatSpectrum: public SpectrumDescriptor
{
public:
    BandLimitedFlatSpectrum(const double s0, const double f0, const double f1):
        s0_(s0), f0_(f0), f1_(f1)
        {}
    virtual double get(const double f) const
    {
        return (f >= f0_ && f < f1_) ? s0_ : 0.;
    }
private:
    const double s0_;
    const double f0_;
    const double f1_;
};

class PowerLawSpectrum: public SpectrumDescriptor
{
public:
    PowerLawSpectrum(const double expo, const double f0, const double f1):
        expo_(expo), f0_(f0), f1_(f1), A_(0)
    {
        // normalization, so that variance = 1
        A_ = 1./2 * (1.-expo) / (pow(f1,1.-expo) - expo*pow(f0,1.-expo));
    }
    virtual double get(const double f) const
    {
        if (f < f0_) return A_ * pow(f0_, -expo_);
        else if (f < f1_) return A_ * pow(f, -expo_);
        else return 0.;
    }
private:
    const double expo_;
    const double f0_;
    const double f1_;
    double A_;
};

namespace signal {

Array<double> generate_gaussian(RNG& rng,
        const C2RFourierTransform& c2rft, const SpectrumDescriptor& spec,
        const size_t N, const double T);

};

}

#endif /* SIGNAL_H */


#include <math.h>
#include <gmp.h>
#if BLAS_IMPL == openblas
#include <blas/cblas.h>
#else
#include <cblas.h>
#endif
#include "attrib.h"
#include "math.h"

#define CLEANUP \
    return err; \
    error: \
    if (ctx->err == 0) \
    	strcpy(ctx->err_msg, "Unexpected math."); \
    ctx->err = err; \
    return err;

#define CHECK(expr) \
    if ((err = (expr))) goto error;

VIO_CONST
uint32_t vmaxui(uint32_t a, uint32_t b) {
    return a > b ? a : b;
}

#define OP_IMPL(name) \
    vio_err_t generic_##name(vio_ctx *ctx, vio_val *a, vio_val *b) { \
        vio_err_t err = 0; \
        vio_val *va, *vb; \
        if (a->what == vv_str || b->what == vv_str || a->what == vv_quot || b->what == vv_quot || \
            a->what == vv_tagword || b->what == vv_tagword) { \
            err = vio_raise(ctx, VE_WRONG_TYPE, \
                            "Function '" #name "' expects numeric types or vectors; received '%s' and '%s' operands.", \
                            vio_val_type_name(a->what), vio_val_type_name(b->what));\
            goto error; \
        }

#define END_OP(_name) \
    CLEANUP \
}

#define GIVEN_VECF \
    if (a->what == vv_vecf && b->what == vv_vecf && a->vlen == b->vlen) {

#define DONE_VECF }

#define GIVEN_NUMERIC \
    else if (vio_is_numeric(a) && vio_is_numeric(b)) { \
        CHECK(vio_coerce(ctx, b, &vb, a->what)) \
        if (vb == NULL) { \
            CHECK(vio_coerce(ctx, a, &va, b->what)) \
            if (va == NULL) { \
                err = VE_NUMERIC_CONVERSION_FAIL; \
                goto error; \
            } \
            a = b; \
            b = va; \
        } \
        else \
            b = vb; \
        switch (a->what) {

#define DONE_NUMERIC \
        default: err = VE_WRONG_TYPE; goto error; \
        } \
    }

#define STANDARD_NUMERIC(op, fop) \
    GIVEN_NUMERIC \
        case vv_int: CHECK(vio_push_int(ctx, b->i32 op a->i32)) break; \
        case vv_float: CHECK(vio_push_float(ctx, b->f32 op a->f32)) break; \
        case vv_num: { mpf_t c; mpf_init(c); mpf_##fop(c, b->n, a->n); CHECK(vio_push_num(ctx, c)) break; } \
    DONE_NUMERIC

#define GIVEN_VEC \
    else CHECK(vio_coerce(ctx, a, &va, vv_vec)) \
    else CHECK(vio_coerce(ctx, b, &vb, vv_vec)) \
    else if (va && vb) { \

#define DONE_VEC }

#define AUTO_VECTORIZE(name) \
    GIVEN_VEC \
        uint32_t maxl = vmaxui(va->vlen, vb->vlen); \
        for (uint32_t i = 0; i < maxl; ++i) \
            CHECK(generic_##name(ctx, va->vv[i % va->vlen], vb->vv[i % vb->vlen])) \
        CHECK(vio_push_vec(ctx, maxl)) \
    DONE_VEC

OP_IMPL(add)
    GIVEN_VECF
        cblas_daxpy(b->vlen, 1, a->vf32, 1, b->vf32, 1);
        CHECK(vio_push_vecf32(ctx, b->vlen, b->vf32))
    DONE_VECF
    STANDARD_NUMERIC(+, add)
    AUTO_VECTORIZE(add)
END_OP(add)

OP_IMPL(sub)
    GIVEN_VECF
        cblas_daxpy(b->vlen, -1.0, a->vf32, 1, b->vf32, 1);
        CHECK(vio_push_vecf32(ctx, b->vlen, b->vf32))
    DONE_VECF
    STANDARD_NUMERIC(-, sub)
    AUTO_VECTORIZE(sub)
END_OP(sub)

OP_IMPL(mul)
    if (b->what == vv_vecf && a->what == vv_float) {
        va = a;
        a = b;
        b = va;
    }
    if (a->what == vv_vecf && b->what == vv_float) {
        cblas_dscal(a->vlen, b->f32, a->vf32, 1);
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    }
    else GIVEN_VECF
        for (uint32_t i = 0; i < a->vlen; ++i)
            a->vf32[i] *= b->vf32[i];
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    DONE_VECF
    STANDARD_NUMERIC(*, mul)
    AUTO_VECTORIZE(mul)
END_OP(mul)

OP_IMPL(div)
    if (b->what == vv_vecf && a->what == vv_float) {
        va = a;
        a = b;
        b = va;
    }
    if (a->what == vv_vecf && b->what == vv_float) {
        cblas_dscal(a->vlen, 1.0 / b->f32, a->vf32, 1);
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    }
    else GIVEN_VECF
        for (uint32_t i = 0; i < a->vlen; ++i)
            a->vf32[i] /= b->vf32[i];
        CHECK(vio_push_vecf32(ctx, a->vlen, a->vf32))
    DONE_VECF
    STANDARD_NUMERIC(/, div)
    AUTO_VECTORIZE(div)
END_OP(div)

#define BIN_OP(name) \
    vio_err_t vio_##name(vio_ctx *ctx) { \
        vio_err_t err = 0; \
        VIO__ENSURE_ATLEAST(2) \
\
        vio_val *a = ctx->stack[--ctx->sp], *b; \
        b = ctx->stack[--ctx->sp]; \
        err = generic_##name(ctx, a, b); \
\
        CLEANUP \
    }

BIN_OP(add)
BIN_OP(sub)
BIN_OP(mul)
BIN_OP(div)

#define UN_IMPL(f) \
    vio_err_t vio_##f(vio_ctx *ctx) { \
        vio_err_t err = 0; \
        vio_float x, y; \
        VIO__CHECK(vio_pop_float(ctx, &x)); \
        y = f(x); \
        VIO__CHECK(vio_push_float(ctx, y)); \
        return 0; \
        error: \
        return err; \
    }

LIST_MATH_UNARY(UN_IMPL)


#ifndef QCQP_B_H
#define QCQP_B_H

#include "Matrix.h"
#include "ALM_L_Katyusha.h"
#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <math.h>

/*This class solves problem of the form
min 1/2 x^\top Q_0 x+b_0^\top x
s.t. 1/2 x^\top Q_i x+b_i^\top x -1\leq 0,\enspace \forall i=1,\dots,m
-b\leq x\leq b

using IPALM with loopless Katyusha as inner solver.
*/

template<typename L, typename D>
class QCQP_b: public ALM_L_Katyusha<L, D>
{
private:

  vector<D> Lambda; // maximal eigenvalues of Q0, Q1,...Qm.

  vector<D> norm_c; //||[b0;b1;...;bm] ||_2

  vector<D> norm_1_c; //||[b0;b1;...;bm] ||_1

  D b;  // absolute bound on each variable

  D nb_q_c;  //number of quadratic constraints

  D nb_v; //number of variables

  Matrix<L,D> data_Q; // Q=[Q0;Q1;...;Qm]; b=[b0;b1;...;bm].

protected:

public:

  QCQP_b(const char* Matrix_file,D val_b)
  :ALM_L_Katyusha<L,D>(), data_Q(Matrix_file)
  {
    b= val_b;
    nb_v=data_Q.nfeatures;
    nb_q_c=data_Q.nsamples/data_Q.nfeatures- 1;
    this->mu_g=0;
  }

  inline D get_n(){
    return data_Q.nfeatures;
  }

  inline D get_m(){
    return data_Q.nsamples/data_Q.nfeatures- 1;
  }

 void set_dimension(){
    this->d= nb_v;
    this->m= nb_q_c;
  }

  void set_is_feasibility_constraint(){
    for(L i=0;i<nb_q_c;i++)
    this->is_feasiblity_constraint[i]=1;
  }

  inline D value_of_P(vector<D> & x){
    D res=0;
    for(L i=0;i<this->d;i++)
    {
      if (fabs(x[i])> b){
        cout<< "error: the variable x_"<< i<<"="<<x[i]<<". It should be in [-"<<b<<", "<<b<<" ]"<< endl;
        system("pause");
        return std::numeric_limits<double>::max();
      }
    }
    return res;
  }

  inline void prox_of_P(D alpha,vector<D> & x, vector<D> &  y){
    for(L i=0;i<this->d;i++)
    {
      D x2=x[i];
      if (x2>= b){
        y[i]=b;
      }
      else if(x2<=-b){
        y[i]=-b;
      }
      else{
        y[i]=x2;
      }
    }
  }

  D prox_of_h_star_j(D x,D beta, L j){
    if(x>=0) return x;
    else return 0;
  }

  D value_of_h_star_j(D x,L j){
    if(x>=0) return 0;
    else {
      cout<< "Error! x="<<x<<" should be nonnegative."<<endl;
      system("pause");
      return std::numeric_limits<double>::max();
    }
  }

  void compute_gradient_f0(vector<D> & x, vector<D>& g){
    for(L j=0;j<nb_v;j++){
      g[j]=0;
      for(L k = data_Q.ptr[j]; k < data_Q.ptr[j + 1];k++){
        L kj=data_Q.row_idx[k];
        g[j]+=x[kj]*data_Q.A[k];
      }
      g[j]+=data_Q.b[j];
    }
  }

  D compute_gradient_and_value_f_i(vector<D> & x ,vector<D> & g, L i){
    D fi=0;
    for(L j=0;j<nb_v;j++){
      g[j]=0;
      L j2=i*nb_v+j;
      for(L k = data_Q.ptr[j2]; k < data_Q.ptr[j2 + 1];k++){
        L kj=data_Q.row_idx[k];
        g[j]+=x[kj]*data_Q.A[k];
      }
      D bj=data_Q.b[j2];
      fi+=x[j]*g[j]/2+x[j]*bj;
      g[j]+=bj;
    }
    return fi-1;
  }

  D value_of_f0(vector<D> & x){
    return my_value_of_f_i( x, 0);
  }

  D value_of_f_i(vector<D> & x, L i){
    return my_value_of_f_i( x, i)-1;
  }

  D my_value_of_f_i(vector<D> & x, L i){
    D fi=0;
    for(L j=0;j<nb_v;j++){
      D Qjx=0;
      L j2=i*nb_v+j;
      for(L k = data_Q.ptr[j2]; k < data_Q.ptr[j2 + 1];k++){
        L kj=data_Q.row_idx[k];
        Qjx+=x[kj]*data_Q.A[k];
      }
      fi+=x[j]*Qjx/2+x[j]*data_Q.b[j2];
    }
    return fi;
  }

  D value_of_h_j(D x, L i){
    cout<< "Error! The "<<i<<"th function should be a feasibility constraint"<< endl;
    system("pause");
    return std::numeric_limits<double>::max();
  }

  D distance_to_domain_of_h_j(D x, L i){
    if(x<=0)
    return 0;
    else
    return x;
  }

  D compute_Qi_x(vector<D> & x ,vector<D> & g, L i){
    D normg=0;
    for(L j=0;j<nb_v;j++){
      g[j]=0;
      L j2=i*nb_v+j;
      for(L k = data_Q.ptr[j2]; k < data_Q.ptr[j2 + 1];k++){
        L kj=data_Q.row_idx[k];
        g[j]+=x[kj]*data_Q.A[k];
      }
      normg+=g[j]*g[j];
    }
    return sqrt(normg);
  }

  void compute_lambda(){
    D maxv=0;
    D minv=std::numeric_limits<double>::max();
    Lambda.resize(nb_q_c+1);
    norm_c.resize(nb_q_c+1,0);
    norm_1_c.resize(nb_q_c+1,0);
    std::vector<D> xk(nb_v,1);
    std::vector<D> yk(nb_v);
    D normk;
    D tmp;
    for (L i= 0; i<nb_q_c+1 ; i++){
      for(L kk=0;kk<20;kk++){
        normk= compute_Qi_x(xk , yk ,i);
        for (L j=0;j<nb_v;j++)
        xk[j]=yk[j]/normk;
      }
      D res=0;
      normk= compute_Qi_x(xk, yk ,i);
      for (L j=0;j<nb_v;j++)
      res+=xk[j]*yk[j];
      Lambda[i]= res;
      if (res> maxv){
        maxv= res;
      }
      if (res< minv){
        minv= res;
      }
      D res2= 0;
      D res3= 0;
      for (L j=0; j< nb_v; j++){
        res2+= data_Q.b[i*nb_v+j]*data_Q.b[i*nb_v+j];
        res3+= fabs(data_Q.b[i*nb_v+j]);
      }
      norm_c[i]= sqrt(res2);
      norm_1_c[i]=res3;
    }
    cout<< "max Lambda= "<< maxv<< " min Lambda= "<< minv<< endl;
  }

  inline void set_Li_Lf(){
    this->Li.clear();
    this->Li.resize(this->nsamples,0);
    this->Li[0]=this->nsamples*Lambda[0];
    this->Lf=Lambda[0];
    D maxLi=this->Li[0];
    D minLi=this->Li[0];
    for (L i=0; i< nb_q_c; i++){
      D M_p_j= nb_v*Lambda[i+ 1]*b*b+ b*norm_1_c[i+1]+ 1;
      D M_dp_j= sqrt(nb_v)*Lambda[i+ 1]*b+ norm_c[i+1];
      D L_dp_j= Lambda[i+1];
      D d_s_j= M_p_j+ this->beta_s*this->lambda_s[i];
      D tmp=L_dp_j*d_s_j/this->beta_s+ M_dp_j*M_dp_j/this->beta_s;
      this->Li[i+1]= this->nsamples*tmp;
      this->Lf+=tmp;
      if (this->Li[i+1]> maxLi){
        maxLi= this->Li[i+ 1];
      }
      if (this->Li[i+ 1]< minLi){
        minLi= this->Li[i+ 1];
      }
    }
    this->sumLi=this->Lf*this->nsamples;
    cout<<"  max of Li: "<<maxLi<<"  min of Li: "<<minLi<<" sumLi="<<this->sumLi<<" Lf= "<<this->Lf<<endl;
  }

  void ALM_QCQP_solver(D beta_0, D epsilon_0,  D eta, D rho,vector<D> & x0,vector<D> & y0,L val_tau, L max_nb_outer, L p_N_1, L p_N_2,string filename1, string filename2, D time){
    compute_lambda();
    this->ALM_solve_with_L_Katyusha(beta_0, epsilon_0,  eta, rho,x0,y0, val_tau, max_nb_outer,  p_N_1,  p_N_2,  filename1,  filename2, time);
  }

};

#endif


#ifndef __MTPOWERSPECTRUM_ENGINE_H__
#define  __MTPOWERSPECTRUM_ENGINE_H__

#include <memory>
#include <vector>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_fft_complex.h>
#include "mspass/seismic/TimeSeries.h"
#include "mspass/utility/dmatrix.h"
#include "mspass/seismic/PowerSpectrum.h"

namespace mspass::algorithms::deconvolution{
/*! \brief Multittaper power spectral estimator.

The multitaper method uses averages of spectra windowed by Slepian functions.
This class can be used to compute power spectra.  For efficiency the design
has constructors that build the Slepian functions and cache them in a
private area.  Use of the pply method that returns vector of
power spectral estimates.  Get methods can be called to get frequency
properties of the data returned.  This could have been done with a class, but
that was judged better left to a python wrapper using this class.
*/
class MTPowerSpectrumEngine
{
public:
  MTPowerSpectrumEngine();
  MTPowerSpectrumEngine(const int winsize, const double tbp, const int ntapers);
  MTPowerSpectrumEngine(const MTPowerSpectrumEngine& parent);
  ~MTPowerSpectrumEngine();
  //~MTPowerSpectrumEngine();
  MTPowerSpectrumEngine& operator=(const MTPowerSpectrumEngine& parent);
  /*! \process a TimeSeries.

  This is one of two methods for applying the multiaper algorithm to data.
  This one uses dt and data length to set the Rayleigh bin size (df).   If
  the input data vector length is not the same as the operator length an
  elog complaint is posted to parent.   Short data are processed but should
  be considered suspect unless the sizes differ by only a tiny fraction
  (e.g. and off by one error from rounding).  Long data will be truncated
  on the right (i.e. sample 0 will be the start of the window used).

  \param parent is the data to process
  \return vector containing estimated power spwecrum
  */
  mspass::seismic::PowerSpectrum apply(const mspass::seismic::TimeSeries& d);
  /*! \brief Low level processing of vector of data.

  This is lower level function that processes a raw vector of data.   Since
  it does not know the sample interval it cannot compute the rayleigh bin
  size so if callers need that feature they must do that (simple) calculation
  themselves.   Unlike the TimeSeries method this one will throw an
  exception if the input data size does not match the operator size.

  \param d is the vector of data to process.  d.size() must this->taperlen() value.
  \return vector containing estimated power spectrum (usual convention with
    0 containing 0 frequency value)
  \exception throw a MsPASSError if the size of d does not match operator length
  */
  std::vector<double> apply(const std::vector<double>& d);
  double df() const {return deltaf;};

  std::vector<double> frequencies();
  /*! Retrieve the taper length.*/
  int taper_length() const
  {
    return taperlen;
  };
  /*! Retrieve time-bandwidth product.*/
  double time_bandwidth_product()  const
  {
    return tbp;
  };
  /*! Return number of tapers used by this engine. */
  int number_tapers() const
  {
    return ntapers;
  };
  /*! \brief PUtter equivalent of df.

  The computation of the Rayleigh bin size (dt) is actually quote trivial but
  this convenience functon allows users of the  vector<double> method to
  handle the comutation easily.  It uses the internal oeprator size, however,
  to compute the df size it returns because the operator is dogmatic about
  using that size.  Users wishing to call the frequency method and the apply
  method on raw vector data need to call this function before calling frequency.

  \param dt is the data sample interval (time domain)

  \return computed df
  */
  double set_df(double dt)
  {
    deltaf=1.0/(dt*static_cast<double>(taperlen));
    return deltaf;
  };
private:
  int taperlen;
  int ntapers;
  double tbp;
  mspass::utility::dmatrix tapers;
  /* Frequency bin interval of last data processed.*/
  double deltaf;
  gsl_fft_complex_wavetable *wavetable;
  gsl_fft_complex_workspace *workspace;
};
} //namespace ed
#endif


#ifndef SMSVM_C_H
#define SMSVM_C_H

#include "Matrix.h"
#include "PDCD_SMSVM.h"
#include <string>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdio.h>      /* printf */
#include <time.h>
#include <fstream>
#include <algorithm>
#include <iomanip>
#include <ctime>
#include <math.h>

//This class solves problem: min_{x,omega} sum_i max(1- b_i^T(A_ix+ omega), 0)+ P(x) by SMART_CD
// where P(x)=frac{lambda2}{2}|x|_2^2 +lambda1|x|_1.

// Let X= (x; omega), M= (b.*A b), b_i^T(A_ix+ omega)= M_iX
// phi_i(x)= 0, h_i(x)= max(1- x,0), g(X)= P(x).

template<typename L, typename D>
class SMSVM_c: public PDCD_SMSVM<L, D>
{
private:

  D lambda1;

  D lambda2;

  Matrix<L,D> my_M;

  D val_lambda_f;

protected:

public:

  SMSVM_c(const char* Matrix_file,D val_lambda1, D val_lambda2)
  :PDCD_SMSVM<L,D>(),my_M(Matrix_file)
  {
    lambda1=val_lambda1;
    lambda2=val_lambda2;
    val_lambda_f=0;
    this->mu_f=0;
    this->mu_g=lambda2;
    my_M.matrix_modified_to_svm(my_M);
  }

  L get_n(){return my_M.nfeatures;}
  L get_m(){return my_M.nsamples;}

  inline D gradient_of_phi_j(D x1, L i){
    return 0;
  }

  inline D value_of_g_i(D x, L i){
    if (i== this->n- 1){
      return 0;
    }
    else{
      return lambda2*x*x/2+lambda1*fabs(x);
    }
  }

  inline D value_of_phi_j(D x1, L i){return 0;}

  inline D prox_of_g_i(D x1,D x2,D x3, L i){
    if(i== this->n - 1){
      return compute_one_step2(1.0/x2, x1, x3)-x3;
    }
    else{
      return compute_one_step(1.0/x2, x1, x3)-x3;
    }
  }

  D compute_one_step(D tau, D u, D x){
    D new_x;
    if(x>tau*(lambda1+u))
    new_x=(x-tau*(lambda1+u))/(1+lambda2*tau);
    else if(x<tau*(u-lambda1))
    new_x=(x-tau*(u-lambda1))/(1+lambda2*tau);
    else
    new_x=0;
    return new_x;
  }

  D compute_one_step2(D tau, D u, D x){
    return (x-tau*u)/(1+lambda2*tau);
  }

  inline D value_of_phistar_i(D x,L i) {return 0;}

  inline void set_matrix_M(){
    this->data_M=my_M;
  }

  inline void set_matrix_A(){
    this->data_A.nsamples=0;
    this->data_A.nfeatures=this->data_M.nfeatures;
    this->data_A.nnz=0;
    this->data_A.ptr.resize(1,0);
    this->data_A.ptr_t.resize(this->data_M.nfeatures+1,0);
  }

  inline D distance_to_subgradient_of_g2(vector<D> & gra, vector<D> & x){
    D res=0;
    D tmp;
    D xv;
    for(L i=0;i<this->n;i++){
      tmp=-gra[i];
      xv=x[i];
      if(xv>0) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(xv<0) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else if(tmp-lambda2*xv>lambda1) res+=(tmp-lambda2*xv-lambda1)*(tmp-lambda2*xv-lambda1);
      else if(tmp-lambda2*xv<-lambda1) res+=(tmp-lambda2*xv+lambda1)*(tmp-lambda2*xv+lambda1);
      else res+=0;
    }
    return sqrt(res);
  }

  void SMART_CD_solver(D beta_0, vector<D> & x0,vector<D> & y0,L val_tau, L max_nb_outer, L p_N, string filename1, D time){
    this->PDCD_SMSVM_solver(beta_0, x0,y0, val_tau, max_nb_outer,  p_N, val_lambda_f, filename1, time);
  }

};

#endif


/* ode-initval2/rk1imp.c
 *
 * Copyright (C) 2009, 2010 Tuomo Keskitalo
 *
 * 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.
 */

/* Implicit Euler a.k.a backward Euler method. */

/* Reference: Ascher, U.M., Petzold, L.R., Computer methods for
   ordinary differential and differential-algebraic equations, SIAM,
   Philadelphia, 1998.
*/

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

#include "odeiv_util.h"
#include "rksubs.c"
#include "modnewton1.c"

/* Stage of method */
#define RK1IMP_STAGE 1

typedef struct
{
  gsl_matrix *A;                /* Runge-Kutta coefficients */
  double *y_onestep;            /* Result with one step */
  double *y_twostep;            /* Result with two half steps */
  double *ytmp;                 /* Temporary work space */
  double *y_save;               /* Backup space */
  double *YZ;                   /* Runge-Kutta points */
  double *fYZ;                  /* Derivatives at YZ */
  gsl_matrix *dfdy;             /* Jacobian matrix */
  double *dfdt;                 /* time derivative of f */
  modnewton1_state_t *esol;     /* nonlinear equation solver */
  double *errlev;               /* desired error level of y */
  const gsl_odeiv2_driver *driver;      /* pointer to driver object */
}
rk1imp_state_t;

static void *
rk1imp_alloc (size_t dim)
{
  rk1imp_state_t *state = (rk1imp_state_t *) malloc (sizeof (rk1imp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk1imp_state",
                      GSL_ENOMEM);
    }

  state->A = gsl_matrix_alloc (RK1IMP_STAGE, RK1IMP_STAGE);

  if (state->A == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for A", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->y_twostep = (double *) malloc (dim * sizeof (double));

  if (state->y_twostep == 0)
    {
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->y_save = (double *) malloc (dim * sizeof (double));

  if (state->y_save == 0)
    {
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_save", GSL_ENOMEM);
    }

  state->YZ = (double *) malloc (dim * RK1IMP_STAGE * sizeof (double));

  if (state->YZ == 0)
    {
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for YZ", GSL_ENOMEM);
    }

  state->fYZ = (double *) malloc (dim * RK1IMP_STAGE * sizeof (double));

  if (state->fYZ == 0)
    {
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for fYZ", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->dfdy = gsl_matrix_alloc (dim, dim);

  if (state->dfdy == 0)
    {
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->esol = modnewton1_alloc (dim, RK1IMP_STAGE);

  if (state->esol == 0)
    {
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for esol", GSL_ENOMEM);
    }

  state->errlev = (double *) malloc (dim * sizeof (double));

  if (state->errlev == 0)
    {
      modnewton1_free (state->esol);
      gsl_matrix_free (state->dfdy);
      free (state->dfdt);
      free (state->fYZ);
      free (state->YZ);
      free (state->y_save);
      free (state->ytmp);
      free (state->y_twostep);
      free (state->y_onestep);
      gsl_matrix_free (state->A);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for errlev", GSL_ENOMEM);
    }

  state->driver = NULL;

  return state;
}

static int
rk1imp_apply (void *vstate, size_t dim, double t, double h,
              double y[], double yerr[],
              const double dydt_in[], double dydt_out[],
              const gsl_odeiv2_system * sys)
{
  /* Makes an implicit Euler step with size h and estimates the local
     error of the step by step doubling.
   */

  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  double *const y_onestep = state->y_onestep;
  double *const y_twostep = state->y_twostep;
  double *const ytmp = state->ytmp;
  double *const y_save = state->y_save;
  double *const YZ = state->YZ;
  double *const fYZ = state->fYZ;
  gsl_matrix *const dfdy = state->dfdy;
  double *const dfdt = state->dfdt;
  double *const errlev = state->errlev;

  const modnewton1_state_t *esol = state->esol;

  /* Runge-Kutta coefficients */

  gsl_matrix *A = state->A;
  const double b[] = { 1.0 };
  const double c[] = { 1.0 };
  gsl_matrix_set (A, 0, 0, 1.0);

  if (esol == NULL)
    {
      GSL_ERROR ("no non-linear equation solver speficied", GSL_EINVAL);
    }

  /* Get desired error levels via gsl_odeiv2_control object through driver
     object, which is a requirement for this stepper.
   */

  if (state->driver == NULL)
    {
      return GSL_EFAULT;
    }
  else
    {
      size_t i;

      for (i = 0; i < dim; i++)
        {
          if (dydt_in != NULL)
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           dydt_in[i], h, i, &errlev[i]);
            }
          else
            {
              gsl_odeiv2_control_errlevel (state->driver->c, y[i],
                                           0.0, h, i, &errlev[i]);
            }
        }
    }

  /* Evaluate Jacobian for modnewton1 */

  {
    int s = GSL_ODEIV_JA_EVAL (sys, t, y, dfdy->data, dfdt);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Calculate a single step with size h */

  {
    int s = modnewton1_init ((void *) esol, A, h, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h, YZ, fYZ);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (y_onestep, h, y, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      return s;
  }

  /* Error estimation by step doubling */

  {
    int s = modnewton1_init ((void *) esol, A, h / 2.0, dfdy, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* 1st half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t, h / 2.0, y,
                              sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + c[0] * h / 2.0, YZ, fYZ);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = rksubs (ytmp, h / 2.0, y, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      return s;
  }

  /* Save original y values in case of error */

  DBL_MEMCPY (y_save, y, dim);

  /* 2nd half step */

  {
    int s = modnewton1_solve ((void *) esol, A, c, t + h / 2.0, h / 2.0,
                              ytmp, sys, YZ, errlev);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    int s = GSL_ODEIV_FN_EVAL (sys, t + h / 2.0 + c[0] * h / 2.0, YZ, fYZ);
    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  {
    /* Note: rk1imp returns y using the results from two half steps
       instead of the single step since the results are freely
       available and more precise.
     */

    int s = rksubs (y_twostep, h / 2.0, ytmp, fYZ, b, RK1IMP_STAGE, dim);

    if (s != GSL_SUCCESS)
      {
        DBL_MEMCPY (y, y_save, dim);
        return s;
      }
  }

  DBL_MEMCPY (y, y_twostep, dim);

  /* Error estimation */

  {
    size_t i;
    for (i = 0; i < dim; i++)
      {
        yerr[i] = ODEIV_ERR_SAFETY * 0.5 * fabs (y_twostep[i] - y_onestep[i]);
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          /* Restore original values */
          DBL_MEMCPY (y, y_save, dim);

          return s;
        }
    }

  return GSL_SUCCESS;
}

static int
rk1imp_set_driver (void *vstate, const gsl_odeiv2_driver * d)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  state->driver = d;

  return GSL_SUCCESS;
}

static int
rk1imp_reset (void *vstate, size_t dim)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->y_onestep, dim);
  DBL_ZERO_MEMSET (state->y_twostep, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->y_save, dim);
  DBL_ZERO_MEMSET (state->YZ, dim);
  DBL_ZERO_MEMSET (state->fYZ, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk1imp_order (void *vstate)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;
  state = 0;                    /* prevent warnings about unused parameters */
  return 1;
}

static void
rk1imp_free (void *vstate)
{
  rk1imp_state_t *state = (rk1imp_state_t *) vstate;

  free (state->errlev);
  modnewton1_free (state->esol);
  gsl_matrix_free (state->dfdy);
  free (state->dfdt);
  free (state->fYZ);
  free (state->YZ);
  free (state->y_save);
  free (state->ytmp);
  free (state->y_twostep);
  free (state->y_onestep);
  gsl_matrix_free (state->A);
  free (state);
}

static const gsl_odeiv2_step_type rk1imp_type = {
  "rk1imp",                     /* name */
  1,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &rk1imp_alloc,
  &rk1imp_apply,
  &rk1imp_set_driver,
  &rk1imp_reset,
  &rk1imp_order,
  &rk1imp_free
};

const gsl_odeiv2_step_type *gsl_odeiv2_step_rk1imp = &rk1imp_type;


/** @file matrix.h
 *  @author T J Atherton
 *
 *  @brief Veneer class over the objectmatrix type that interfaces with blas and lapack
 */

#ifndef matrix_h
#define matrix_h

#include <stdio.h>
#include "veneer.h"
/** Use Apple's Accelerate library for LAPACK and BLAS */
#ifdef __APPLE__
#ifdef MORPHO_LINALG_USE_ACCELERATE
#include <Accelerate/Accelerate.h>
#define MATRIX_LAPACK_PRESENT
#endif
#endif

/** Otherwise, use LAPACKE */
#ifndef MATRIX_LAPACK_PRESENT
#include <cblas.h>
#include <lapacke.h>
#define MORPHO_LINALG_USE_LAPACKE
#define MATRIX_LAPACK_PRESENT
#endif

/* -------------------------------------------------------
 * Matrix objects
 * ------------------------------------------------------- */

extern objecttype objectmatrixtype;
#define OBJECT_MATRIX objectmatrixtype

/** Matrices are a purely numerical collection type oriented toward linear algebra.
    Elements are stored in column-major format, i.e.
        [ 1 2 ]
        [ 3 4 ]
    is stored ( 1, 3, 2, 4 ) in memory. This is for compatibility with standard linear algebra packages */

typedef struct {
    object obj;
    unsigned int nrows;
    unsigned int ncols;
    double *elements;
    double matrixdata[];
} objectmatrix;

/** Tests whether an object is a matrix */
#define MORPHO_ISMATRIX(val) object_istype(val, OBJECT_MATRIX)

/** Gets the object as an matrix */
#define MORPHO_GETMATRIX(val)   ((objectmatrix *) MORPHO_GETOBJECT(val))

/** Creates a matrix object */
objectmatrix *object_newmatrix(unsigned int nrows, unsigned int ncols, bool zero);

/** Creates a new matrix from an array */
objectmatrix *object_matrixfromarray(objectarray *array);

/** Creates a new matrix from an existing matrix */
objectmatrix *object_clonematrix(objectmatrix *array);

/** @brief Use to create static matrices on the C stack
    @details Intended for small matrices; Caller needs to supply a double array of size nr*nc. */
#define MORPHO_STATICMATRIX(darray, nr, nc)      { .obj.type=OBJECT_MATRIX, .obj.status=OBJECT_ISUNMANAGED, .obj.next=NULL, .elements=darray, .nrows=nr, .ncols=nc }

/** Macro to decide if a matrix is 'small' or 'large' and hence static or dynamic allocation should be used. */
#define MATRIX_ISSMALL(m) (m->nrows*m->ncols<MORPHO_MAXIMUMSTACKALLOC)

/* -------------------------------------------------------
 * Matrix class
 * ------------------------------------------------------- */

#define MATRIX_CLASSNAME "Matrix"

#define MATRIX_TRANSPOSE_METHOD "transpose"
#define MATRIX_TRACE_METHOD "trace"
#define MATRIX_INNER_METHOD "inner"
#define MATRIX_DET_METHOD "det"
#define MATRIX_EIGENVALUES_METHOD "eigenvalues"
#define MATRIX_EIGENSYSTEM_METHOD "eigensystem"
#define MATRIX_NORM_METHOD "norm"
#define MATRIX_GETCOLUMN_METHOD "column"
#define MATRIX_SETCOLUMN_METHOD "setcolumn"

#define MATRIX_DIMENSIONS_METHOD "dimensions"

#define MATRIX_INDICESOUTSIDEBOUNDS       "MtrxBnds"
#define MATRIX_INDICESOUTSIDEBOUNDS_MSG   "Matrix index out of bounds."

#define MATRIX_INVLDINDICES               "MtrxInvldIndx"
#define MATRIX_INVLDINDICES_MSG           "Matrix indices must be integers."

#define MATRIX_INVLDNUMINDICES            "MtrxInvldNumIndx"
#define MATRIX_INVLDNUMINDICES_MSG        "Matrix expects two arguments for indexing."

#define MATRIX_CONSTRUCTOR                "MtrxCns"
#define MATRIX_CONSTRUCTOR_MSG            "Matrix() constructor should be called either with dimensions or an array, list or matrix initializer."

#define MATRIX_INVLDARRAYINIT             "MtrxInvldInit"
#define MATRIX_INVLDARRAYINIT_MSG         "Invalid initializer passed to Matrix()."

#define MATRIX_ARITHARGS                  "MtrxInvldArg"
#define MATRIX_ARITHARGS_MSG              "Matrix arithmetic methods expect a matrix or number as their argument."

#define MATRIX_INCOMPATIBLEMATRICES       "MtrxIncmptbl"
#define MATRIX_INCOMPATIBLEMATRICES_MSG   "Matrices have incompatible shape."

#define MATRIX_SINGULAR                   "MtrxSnglr"
#define MATRIX_SINGULAR_MSG               "Matrix is singular."

#define MATRIX_NOTSQ                      "MtrxNtSq"
#define MATRIX_NOTSQ_MSG                  "Matrix is not square."

#define MATRIX_SETCOLARGS                 "MtrxStClArgs"
#define MATRIX_SETCOLARGS_MSG             "Method setcolumn expects an integer column index and a column matrix as arguments."

/* -------------------------------------------------------
 * Matrix errors
 * ------------------------------------------------------- */

typedef enum { MATRIX_OK, MATRIX_INCMPTBLDIM, MATRIX_SING, MATRIX_INVLD, MATRIX_BNDS, MATRIX_NSQ, MATRIX_ALLOC } objectmatrixerror;

/* -------------------------------------------------------
 * Matrix interface
 * ------------------------------------------------------- */

bool matrix_getarraydimensions(objectarray *array, unsigned int dim[], unsigned int maxdim, unsigned int *ndim);
value matrix_getarrayelement(objectarray *array, unsigned int ndim, unsigned int *indx);

bool matrix_getlistdimensions(objectlist *list, unsigned int dim[], unsigned int maxdim, unsigned int *ndim);
bool matrix_getlistelement(objectlist *list, unsigned int ndim, unsigned int *indx, value *val);

bool matrix_setelement(objectmatrix *matrix, unsigned int row, unsigned int col, double value);
bool matrix_getelement(objectmatrix *matrix, unsigned int row, unsigned int col, double *value);

bool matrix_getcolumn(objectmatrix *matrix, unsigned int col, double **v);
bool matrix_setcolumn(objectmatrix *matrix, unsigned int col, double *v);
bool matrix_addtocolumn(objectmatrix *m, unsigned int col, double alpha, double *v);

objectmatrixerror matrix_copy(objectmatrix *a, objectmatrix *out);
objectmatrixerror matrix_add(objectmatrix *a, objectmatrix *b, objectmatrix *out);
objectmatrixerror matrix_accumulate(objectmatrix *a, double lambda, objectmatrix *b);
objectmatrixerror matrix_sub(objectmatrix *a, objectmatrix *b, objectmatrix *out);
objectmatrixerror matrix_mul(objectmatrix *a, objectmatrix *b, objectmatrix *out);
objectmatrixerror matrix_inner(objectmatrix *a, objectmatrix *b, double *out);
objectmatrixerror matrix_divs(objectmatrix *a, objectmatrix *b, objectmatrix *out);
objectmatrixerror matrix_divl(objectmatrix *a, objectmatrix *b, objectmatrix *out);
objectmatrixerror matrix_inverse(objectmatrix *a, objectmatrix *out);
objectmatrixerror matrix_transpose(objectmatrix *a, objectmatrix *out);
objectmatrixerror matrix_trace(objectmatrix *a, double *out);
objectmatrixerror matrix_scale(objectmatrix *a, double scale);
objectmatrixerror matrix_identity(objectmatrix *a);
double matrix_sum(objectmatrix *a);
//objectmatrixerror matrix_det(objectmatrix *a, double *out);
//objectmatrixerror matrix_eigensystem(objectmatrix *a, double *val, objectmatrix *vec);

void matrix_print(objectmatrix *m);

void matrix_initialize(void);

#endif /* matrix_h */


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

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

/*
 * This module contains routines for estimating the condition number
 * of matrices in the 1-norm. The algorithm is based on the paper,
 *
 * [1] N. J. Higham, "FORTRAN codes for estimating the one-norm of
 * a real or complex matrix, with applications to condition estimation",
 * ACM Trans. Math. Soft., vol. 14, no. 4, pp. 381-396, December 1988.
 */

static double condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A);
static int condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A,
                             double * rcond, gsl_vector * work);
static int condest_same_sign(const gsl_vector * x, const gsl_vector * y);
static int condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);
static int condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params);

/*
gsl_linalg_tri_upper_rcond()
  Estimate reciprocal condition number of upper triangular matrix

Inputs: A     - upper triangular matrix, N-by-N
        rcond - (output) reciprocal condition number estimate
        work  - workspace, length 3*N

Return: success/error
*/

int
gsl_linalg_tri_upper_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  int status = condest_tri_rcond(CblasUpper, A, rcond, work);
  return status;
}

/*
gsl_linalg_tri_lower_rcond()
  Estimate reciprocal condition number of lower triangular matrix

Inputs: A     - lower triangular matrix, N-by-N
        rcond - (output) reciprocal condition number estimate
        work  - workspace, length 3*N

Return: success/error
*/

int
gsl_linalg_tri_lower_rcond(const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  int status = condest_tri_rcond(CblasLower, A, rcond, work);
  return status;
}

/*
gsl_linalg_invnorm1()
  Estimate the 1-norm of ||A^{-1}||, where A is a square
N-by-N matrix

Inputs: N        - size of matrix
        Ainvx    - pointer to function which calculates:
                   x := A^{-1} x or x := A^{-t} x
        params   - parameters to pass to Ainvx
        Ainvnorm - (output) estimate of ||A^{-1}||_1
        work     - workspace, length 3*N
*/

int
gsl_linalg_invnorm1(const size_t N,
                    int (* Ainvx)(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params),
                    void * params, double * Ainvnorm, gsl_vector * work)
{
  if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      const size_t maxit = 5;
      gsl_vector_view x = gsl_vector_subvector(work, 0, N);
      gsl_vector_view v = gsl_vector_subvector(work, N, N);
      gsl_vector_view xi = gsl_vector_subvector(work, 2*N, N);
      double gamma, gamma_old, temp;
      size_t i, k;

      for (i = 0; i < N; ++i)
        gsl_vector_set(&x.vector, i, 1.0 / (double) N);

      /* compute v = A^{-1} x */
      gsl_vector_memcpy(&v.vector, &x.vector);
      (*Ainvx)(CblasNoTrans, &v.vector, params);

      /* gamma = ||v||_1 */
      gamma = gsl_blas_dasum(&v.vector);

      /* xi = sign(v) */
      for (i = 0; i < N; ++i)
        {
          double vi = gsl_vector_get(&v.vector, i);
          gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));
        }

      /* x = A^{-t} xi */
      gsl_vector_memcpy(&x.vector, &xi.vector);
      (*Ainvx)(CblasTrans, &x.vector, params);

      for (k = 0; k < maxit; ++k)
        {
          size_t j = (size_t) gsl_blas_idamax(&x.vector);

          /* v := A^{-1} e_j */
          gsl_vector_set_zero(&v.vector);
          gsl_vector_set(&v.vector, j, 1.0);
          (*Ainvx)(CblasNoTrans, &v.vector, params);

          gamma_old = gamma;
          gamma = gsl_blas_dasum(&v.vector);

          /* check for repeated sign vector (algorithm has converged) */
          if (condest_same_sign(&v.vector, &xi.vector) || (gamma < gamma_old))
            break;

          /* xi = sign(v) */
          for (i = 0; i < N; ++i)
            {
              double vi = gsl_vector_get(&v.vector, i);
              gsl_vector_set(&xi.vector, i, GSL_SIGN(vi));
            }

          /* x = A^{-t} sign(v) */
          gsl_vector_memcpy(&x.vector, &xi.vector);
          (*Ainvx)(CblasTrans, &x.vector, params);
        }

      temp = 1.0; /* (-1)^i */
      for (i = 0; i < N; ++i)
        {
          double term = 1.0 + (double) i / (N - 1.0);
          gsl_vector_set(&x.vector, i, temp * term);
          temp = -temp;
        }

      /* x := A^{-1} x */
      (*Ainvx)(CblasNoTrans, &x.vector, params);

      temp = 2.0 * gsl_blas_dasum(&x.vector) / (3.0 * N);
      if (temp > gamma)
        {
          gsl_vector_memcpy(&v.vector, &x.vector);
          gamma = temp;
        }

      *Ainvnorm = gamma;

      return GSL_SUCCESS;
    }
}

static int
condest_tri_rcond(CBLAS_UPLO_t Uplo, const gsl_matrix * A, double * rcond, gsl_vector * work)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (M != N)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (work->size != 3 * N)
    {
      GSL_ERROR ("work vector must have length 3*N", GSL_EBADLEN);
    }
  else
    {
      int status;
      double Anorm = condest_tri_norm1(Uplo, A); /* ||A||_1 */
      double Ainvnorm;                           /* ||A^{-1}||_1 */

      *rcond = 0.0;

      /* don't continue if matrix is singular */
      if (Anorm == 0.0)
        return GSL_SUCCESS;

      /* estimate ||A^{-1}||_1 */
      if (Uplo == CblasUpper)
        status = gsl_linalg_invnorm1(N, condest_invtriu, (void *) A, &Ainvnorm, work);
      else
        status = gsl_linalg_invnorm1(N, condest_invtril, (void *) A, &Ainvnorm, work);

      if (status)
        return status;

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

      return GSL_SUCCESS;
    }
}

/* calculate 1 norm of triangular matrix */
static double
condest_tri_norm1(CBLAS_UPLO_t Uplo, const gsl_matrix * A)
{
  const size_t N = A->size2;
  double max = 0.0;
  size_t i, j;

  if (Uplo == CblasUpper)
    {
      for (j = 0; j < N; ++j)
        {
          double sum = 0.0;
          for (i = 0; i <= j; ++i)
            {
              double Aij = gsl_matrix_get(A, i, j);
              sum += fabs(Aij);
            }

          max = GSL_MAX(max, sum);
        }
    }
  else
    {
      for (j = 0; j < N; ++j)
        {
          double sum = 0.0;
          for (i = j; i < N; ++i)
            {
              double Aij = gsl_matrix_get(A, i, j);
              sum += fabs(Aij);
            }

          max = GSL_MAX(max, sum);
        }
    }

  return max;
}

/* return 1 if sign(x) = sign(y), 0 otherwise */
static int
condest_same_sign(const gsl_vector * x, const gsl_vector * y)
{
  const size_t n = x->size;
  size_t i;

  for (i = 0; i < n; ++i)
    {
      double xi = gsl_vector_get(x, i);
      double yi = gsl_vector_get(y, i);
      if (GSL_SIGN(xi) != GSL_SIGN(yi))
        return 0;
    }

  return 1;
}

/* x := A^{-1} x, A upper triangular */
static int
condest_invtriu(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * A = (gsl_matrix *) params;
  return gsl_blas_dtrsv(CblasUpper, TransA, CblasNonUnit, A, x);
}

/* x := A^{-1} x, A lower triangular */
static int
condest_invtril(CBLAS_TRANSPOSE_t TransA, gsl_vector * x, void * params)
{
  gsl_matrix * A = (gsl_matrix *) params;
  return gsl_blas_dtrsv(CblasLower, TransA, CblasNonUnit, A, x);
}