/*
 *
 * ML fitting of dots using a 2D Gaussian model provided by gaussianInt2.
 * I.e., only x and y are fitted.
 *
 * Compilation:
 * For debugging:
 *  gcc  3.c -Wall -std=c99 -I/usr/local/include -L/usr/local/lib -lgsl -lgslcblas -lm -g -o 3
 *  valgrind ./3
 * For speed:
 *  gcc  -Wall -std=c99 3.c -I/usr/local/include -L/usr/local/lib -lgsl -lgslcblas -lm -O3 -o 3
 *  ./3
 * Memory accesses can also be checked with Valgrind or the gcc -fmudflap memory protection option.
 *
 * MATLAB interface in df_mlfit1.c
 * 2017.04.12. Valgrind ok.
 *
 * TODO:
 *  - What are good step sizes?
 *  - Set the initial positions with higher than integer precision if possible.
 *
 */

/*
   From the GSL documentation:

   If necessary you can turn off range checking completely without
   modifying any source files by recompiling your program with the
   preprocessor definition GSL_RANGE_CHECK_OFF. Provided your compiler
   supports inline functions the effect of turning off range checking is
   to replace calls to gsl_vector_get(v,i) by v->data[i*v->stride] and
   calls to gsl_vector_set(v,i,x) by v->data[i*v->stride]=x. Thus there
   should be no performance penalty for using the range checking
   functions when range checking is turned off.

*/

#include <assert.h>
#include <inttypes.h>
#include <stdint.h>
#include <stdio.h>

#include <gsl/gsl_multimin.h>
#include <gsl/gsl_statistics.h>

#include "gaussianInt2.h"
#include "mlfit.h"
#include "blit3.h"
#include "mlfit1.h"

// use the compile flag -D verbose=0 etc
// 0 = no information
// 1 = per dot information
// 2 = per iteration information
#ifndef verbose
#define verbose 1
#endif

// Globals
const size_t maxIterations = 10000; // for z-fitting
const double convCriteria = 1e-7; // for z-fitting

// Possibly, add the window size, denoted Ws in most places

// Headers
double lxy (const gsl_vector *, void *);
double lz (const gsl_vector *, void *);
int localizeDotXY(double *, size_t, double *,  double *,
    double); // sigma
int localizeDotZ(double *, size_t, double *,  double *,
    double); // sigma
int localize(double *, size_t, size_t, size_t, double *, size_t, double *,
    double, double); // sigma_xy, sigma_z -- size of dot
int unit_tests(void);

void lxy_df (const gsl_vector *v, void *params, gsl_vector * df);
void lxy_fdf (const gsl_vector *v, void *params, double * f, gsl_vector *df);

// Optimization constants
typedef struct {
  double * R; // local region
  double * G; // A already allocated temporary space for the Gaussian kernel
  size_t Rw; // R and G has size Rw*Rw
  double sigma; // For constant sigma fitting
  double bg;
} optParams;

double lz (const gsl_vector *v, void *params)
{
  double x;

  optParams *p = (optParams *) params;

  size_t Rw = p->Rw;
  double * R = p->R;
  double * GI = p->G;
  double sigma = p->sigma;
  double bg = p->bg;

#if verbose > 2
  printf("Rw: %lu\n", Rw);
  printf("Nphot: %lu\n", Nphot);
  printf("sigma: %f\n", sigma);
  printf("bg   : %f\n", bg);
#endif

  // Get the other parameters ...
  x = gsl_vector_get(v, 0);
  double Nphot = gsl_vector_get(v,1);

  //  printf("bg: %f, Nphot: %f, sigma: %f z: %f\n", bg, Nphot, sigma, x);

  // Create Gaussian ...
  double mu[] = {x};
  gaussianInt1(GI, mu, &sigma, Rw);
  for(size_t kk = 0; kk<Rw; kk++)
    GI[kk] = bg+Nphot*GI[kk];
#if verbose>2
  printf("GI:\n");
  showRegion(GI, Rw);
#endif

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum(mask.*(-(patch-model).^2./model - .5*log(model))));
   */

  double E = 0;
  for (size_t kk=0; kk<Rw; kk++)
  {
    E+= (GI[kk]-R[kk])*(GI[kk]-R[kk])/GI[kk] - .5*log(GI[kk]);
  }
  //  E = -E;

  if(0){
    printf("R=[");
    for (size_t kk=0; kk<Rw; kk++)
    {
      printf("%f ", R[kk]);
    }
    printf("]\n");
    printf("G = [");
    for (size_t kk=0; kk<Rw; kk++)
    {
      printf("%f ", GI[kk]);
    }
    printf("]\n");
    printf("E: %f, mu: %f\n", E, mu[0]);
  }

  /* Quadratic
   * for (size_t kk=0; kk<Rw*Rw; kk++)
   * E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]);
   */

  if(!isfinite(E))
    E = 10e99;

  return E;
}

double lxy (const gsl_vector *v, void *params)
  // likelihood in xy, function to optimize
{

  double x, y;

  optParams *p = (optParams *) params;

  size_t Rw = p->Rw;
  double * R = p->R;
  double * GI = p->G;
  double sigma = p->sigma;
  double bg = p->bg;

#if verbose > 2
  printf("Rw: %lu\n", Rw);
  printf("Nphot: %lu\n", Nphot);
  printf("sigma: %f\n", sigma);
  printf("bg   : %f\n", bg);
#endif

  // Get the other parameters ...
  x = gsl_vector_get(v, 0);
  y = gsl_vector_get(v, 1);
  double Nphot = gsl_vector_get(v,2);

  // Create Gaussian ...
  double mu[] = {x,y};
  gaussianInt2(GI, mu, &sigma, Rw);
  for(size_t kk = 0; kk<Rw*Rw; kk++)
    GI[kk] = bg+Nphot*GI[kk];
#if verbose>2
  printf("GI:\n");
  showRegion(GI, Rw);
#endif

  /*
   * from LL2PG.m
   *   model = x(3)+x(4)*gaussianInt2([x(1), x(2)], x(5), (size(patch, 1)-1)/2);
   *   mask = disk2d((size(patch,1)-1)/2);
   *   %L = -sum(sum(-(patch-model).^2./model - .5*log(model)));
   *   L = -sum(sum(mask.*(-(patch-model).^2./model - .5*log(model))));
   */

  double E = 0;
  for (size_t kk=0; kk<Rw*Rw; kk++)
    E+= (GI[kk]-R[kk])*(GI[kk]-R[kk])/GI[kk] - .5*log(GI[kk]);
  //  E = -E;

  /* Quadratic
   * for (size_t kk=0; kk<Rw*Rw; kk++)
   * E+= (GI[kk]-R[kk])*(GI[kk]-R[kk]);
   */
//  printf("E = %f\n", E);
  return E;
}

void lxy_df (const gsl_vector *v, void *params, gsl_vector * df)
{
  double h = 1.49e-8; // step size
  gsl_vector * dv = gsl_vector_alloc(3);
  gsl_vector_memcpy(dv, v);

  for(int kk = 0; kk<3; kk++)
  {
    gsl_vector_set(dv, kk, gsl_vector_get(v, kk) + h); // Set
    gsl_vector_set(df, kk, (lxy(dv, params)-lxy(v, params))/h);
   // printf("%f ", gsl_vector_get(df, kk));
    gsl_vector_set(dv, kk, gsl_vector_get(v, kk)); // Reset
  }
//  printf("\n");

  gsl_vector_free(dv);

}

void lxy_fdf (const gsl_vector *v, void *params, double * f, gsl_vector *df)
{
  *f = lxy(v, params);
  lxy_df(v, params, df);
}

int localizeDotXY(double * V, size_t Vm,
    double * D,  double * F, double sigma)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{

  /* Parameters that will not be optimized */
  optParams par;
  par.R = V;
  par.Rw = Vm;
  par.sigma = sigma;
  par.bg = estimateBG(V, Vm);
  par.G = (double *) malloc(Vm*Vm*sizeof(double));

  const gsl_multimin_fdfminimizer_type *T =
    gsl_multimin_fdfminimizer_vector_bfgs;
  //const gsl_multimin_fminimizer_type *T =
    //  gsl_multimin_fminimizer_nmsimplex2;

  gsl_multimin_fdfminimizer *s = NULL;
  gsl_vector *ss, *x;
  gsl_multimin_function_fdf minex_func;

  size_t iter = 0;
  int status;
  double size;

  /* Starting point for parameters to be optimized */
  x = gsl_vector_alloc (3);
  gsl_vector_set(x, 0, 0); // x position
  gsl_vector_set(x, 1, 0); // y position
  gsl_vector_set(x, 2, estimateNphot(V, Vm)); // Nphot

  /* Grid search to find a reasonable starting point
   * for (x,y) to avoid local minima */

  if(1) {
  double xmin = 0;
  double ymin = 0;
  double emin = 10e99;
  int ng = 5; // ng*ng grid points
  for(double dx = -.4; dx<.4; dx=dx+0.8/ng)
    for(double dy = -.4; dy<.4; dy=dy+0.8/ng)
    {
      gsl_vector_set(x, 0, dx); // x position
      gsl_vector_set(x, 1, dy); // y position

      double err = lxy(x, (void *) &par);
//    printf("%f %f :  %f\n", dx, dy, err);
      if( err < emin)
      {
        emin = err;
        xmin = dx;
        ymin = dy;
      }
    }
   gsl_vector_set(x, 0, xmin); // x position
   gsl_vector_set(x, 1, ymin); // y position
 //  printf("Initial position: %f %f\n", xmin, ymin);
  }

  /* Set initial step sizes */
  ss = gsl_vector_alloc(3);
  gsl_vector_set_all(ss, 0.1);

  /* Initialize method and iterate */
  minex_func.n = 3;
  minex_func.f = lxy; // Error
  minex_func.df = lxy_df; // Derivative
  minex_func.fdf = lxy_fdf; // Error and Derivative
  minex_func.params = ∥

  s = gsl_multimin_fdfminimizer_alloc (T, 3);
  gsl_multimin_fdfminimizer_set (s, // Optimizer
      &minex_func, // Function to optimize
      x, // Start point
      0.1, // Default step size of the line search
      0.1); // tol. 0 = exact line search 0.1 = recommended from GSL manual

  /* tol=0.1 was about twice as fast as tol=0 for this problem with no noticeable
   * regression in accuracy */

size_t maxIterationsQN = 500; // abort if more iterations

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

    if (status)
      break;

    size = 0; //gsl_multimin_fdfminimizer_size(s);
    status = gsl_multimin_test_gradient (s->gradient, 1e-4);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu x:%10.3e y:%10.3e NP:%6.1f f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          gsl_vector_get (s->x, 2),
          0.0, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxIterationsQN);

  // F is always written even if the convergence failed
  F[0] = gsl_vector_get (s->x, 0)+nearbyint(D[0]);
  F[1] = gsl_vector_get (s->x, 1)+nearbyint(D[1]);
  F[2] = D[2]; // Not optimized

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fdfminimizer_free (s);

  free(par.G);

  return status;
}

int localizeDotZ(double * V, size_t Vm,
    double * D,  double * F, double sigma)
  // Localization for a dot roughly centered in V of size Vm x Vm
  // D[0], D[1], D[3] are the global coordinates of the dot
  // F are the fitted coordinates
{

  // Non-optimized parameters
  optParams par;
  par.R = V;
  par.Rw = Vm;
  par.sigma = sigma;
  par.bg = estimateBG(V, Vm);
  par.G = (double *) malloc(Vm*sizeof(double));

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

  size_t iter = 0;
  int status;
  double size;

  /* Starting point */
  x = gsl_vector_alloc (2);
  gsl_vector_set(x, 0, 0); // z position
  gsl_vector_set(x, 1, estimateNphot(V, Vm)); // Nphot

  /* Set initial step sizes */
  ss = gsl_vector_alloc(2);
  gsl_vector_set_all(ss, 0.1);
  gsl_vector_set(ss, 1, 10); // Number of photons

  /* Initialize method and iterate */
  minex_func.n = 2;
  minex_func.f = lz;
  minex_func.params = ∥

  //printf(".\n"); fflush(stdout);

  s = gsl_multimin_fminimizer_alloc (T, 2);

  //  printf("..\n"); fflush(stdout);
  int error = gsl_multimin_fminimizer_set(s,
      &minex_func,
      x, // starting point
      ss); // Step sizes
  if(error>0)
    printf("error: %d\n", error);
  // printf("...\n"); fflush(stdout);

  do
  {
    iter++;
    status = gsl_multimin_fminimizer_iterate(s);
    //  printf("....\n"); fflush(stdout);

    if (status)
      break;

    size = gsl_multimin_fminimizer_size(s);
    status = gsl_multimin_test_size(size, convCriteria);
    //  printf(".....\n"); fflush(stdout);

    if (status == GSL_SUCCESS)
    {
#if verbose > 0
      printf ("converged to minimum at\n");
      printf ("%5lu z:%10.3e NP:%6.1f f() = %7.3f size = %10.3e\n",
          iter,
          gsl_vector_get (s->x, 0),
          gsl_vector_get (s->x, 1),
          s->fval, size);
#endif
    }
  }
  while (status == GSL_CONTINUE && iter < maxIterations);

  // Always Update Z position -- even if convergence failed
  F[2] = gsl_vector_get (s->x, 0)+nearbyint(D[2]);

  gsl_vector_free(x);
  gsl_vector_free(ss);
  gsl_multimin_fminimizer_free (s);

  free(par.G);

  return status;
}
int localize(double * V, size_t Vm, size_t Vn, size_t Vp,
    double * D, size_t Dm, double * F, double sigma_xy, double sigma_z)
  // run the localization routine for a list of dots
{

  /* The window size is a very important parameter. Small windows can not capture the full signals and that results in
   * bad fittings.
   * Ws = 7 is good until sigma = 1.1
   * Ws = 9 is good at least until sigma = 1.6
   */
  int Ws = 9; // Window size
  double * W = malloc(Ws*Ws*sizeof(double));

  for(size_t kk =0; kk<Dm; kk++)
  {
    // Copy the neighbourhood around each D into W
    if(getRegion(W, Ws,
          V, Vm, Vn, Vp, D+kk*3) == 0)
    {
#if verbose > 1
      printf("W:\n");
      showRegion(W, Ws);
#endif
      // Local fitting in W
#if verbose > 0
      int status = localizeDotXY(W,  Ws, D+kk*3, F+kk*3, sigma_xy);
      printf("Status: %d\n", status);
#else
      localizeDotXY(W,  Ws, D+kk*3, F+kk*3, sigma_xy);
#endif

      if(getZLine(W,Ws,
            V, Vm, Vn, Vp, D+kk*3) == 0)
      {
        localizeDotZ(W, Ws, D+kk*3, F+kk*3, sigma_z);
      }

    }
    else
    {
      // TODO: add also Nphot and status to the output
      F[kk*3] = -1;
      F[kk*3+1] = -1;
      F[kk*3+2] = -1;
    }
  }
  free(W);
  return 0;
}

int unit_tests(){
  double * V; // image
  int Vm = 1024; int Vn = 1024; int Vp = 60;
  double * D; // list of dots
  size_t Dm = 5; // number of dots
  double * F; // fitted dots

  printf("Image size: %dx%dx%d\n", Vm, Vn, Vp);
  printf("Localizing %lu dots\n", Dm);

  V = (double *) malloc(Vm*Vn*Vp*sizeof(double));
  D = (double *) malloc(3*Dm*sizeof(double));
  F = (double *) malloc(3*Dm*sizeof(double));

  // Initialize the data
  for(int kk=0; kk<Vm*Vn*Vp; kk++)
    V[kk] = 1000+rand_range(0,0);

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    D[pos] =   rand_range(6, Vm-7);
    D[pos+1] = rand_range(6, Vn-7);
    D[pos+2] = rand_range(6, Vp-7);
    if(D[pos] < 6)
      D[pos] = 6;
    if(D[pos+1] < 6)
      D[pos+1] = 6;
    if(D[pos+2] < 6)
      D[pos+2] = 6;
#if verbose > 0
    printf("D %03d %f %f %f\n", kk, D[pos], D[pos+1], D[pos+2]);
#endif
  }

  for(uint32_t kk=0; kk<Dm; kk++)
  {
    size_t pos = D[kk*3] + D[kk*3+1]*Vm + D[kk*3+2]*Vm*Vn;
    V[pos] = V[pos]+5;
  }

  D[0] = 100; D[1] = 100; D[2] = 30;
  V[(int) D[0]+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 7;
  V[(int) D[0]+1+(int) D[1]*Vm+(int) D[2]*Vm*Vn] = 6;

  // Run the optimization
  localize(V, Vm, Vn, Vp, D, Dm, F, 1.1, 1.1);

  if(1)
  {
    for(double sigma = 0.1; sigma<2; sigma=sigma+0.1)
      localize(V, Vm, Vn, Vp, D, Dm, F, sigma, sigma);
  }

  // In next version, also supply clustering information

#if verbose >0
  for(size_t kk = 0; kk<Dm; kk++)
  {
    size_t pos = kk*3;
    printf("%6lu (%f, %f, %f) -> (%f, %f, %f)\n", kk,
        D[pos], D[pos+1], D[pos+2],
        F[pos], F[pos+1], F[pos+2]);
  }
#endif
  free(F);
  free(D);
  free(V);
  return 0;
}

int main(int argc, char ** argv)
  // For testing, not used any more, see the MATLAB interface in
  // df_mlfit.c
{

  printf("%s\n", argv[0]);

  if(argc == 1)
    return unit_tests();

  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/time.h>
#include <math.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_blas.h>

#include <omp.h>

#include "util.h"

int main(int argc, char *argv[]) {
    char* input_fileName1 = argv[1];
    char* input_fileName2 = argv[2];
    int N_doc_bg = atoi(argv[3]);
    int N_kw = atoi(argv[4]);
    int N_obs = atoi(argv[5]);
    int N_iter = atoi(argv[6]);
    double sec_budget = atoi(argv[7]) / 100.0;
    char* output_fileName = argv[8];
    int N_doc = 480000 / 2;
    int fixed_padding = 2 * log(2) * (64 + log(N_doc) / log(2)) / sec_budget;
    printf("Padding: %d\n", fixed_padding);

    int* matrix = (int*) malloc(sizeof(int) * N_obs * N_obs);
    int* matrix_bg = (int*) malloc(sizeof(int) * N_kw * N_kw);
    int* matrix_padded = (int*) malloc(sizeof(int) * N_obs * N_obs);
    int* true_index = (int*) malloc(sizeof(int) * N_kw);
    int* permutation = (int*) malloc(sizeof(int) * N_obs);
    gsl_matrix* matrix_obs;

    for (int round = 0; round < 10; round++)
    {
        char input_fileName1_extend[40];
        char input_fileName2_extend[40];
        sprintf(input_fileName1_extend, "%s%d", input_fileName1, round);
        sprintf(input_fileName2_extend, "%s%d", input_fileName2, round);

        // Setup
        struct timeval tv1,tv2;
        gettimeofday(&tv1, NULL);
        read_matrix(&true_index, &matrix_bg, 1.0*N_doc/N_doc_bg, N_kw, input_fileName2_extend);
        read_matrix(&true_index, &matrix, 1.0, N_obs, input_fileName1_extend);
        gettimeofday(&tv2, NULL);
        printf("Reading done: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
        fflush(stdout);

        for (int iter = 0; iter < 10; iter++)
        {
            printf("Run %d\n", iter);
            matrix_obs = gsl_matrix_alloc(N_obs, N_obs);

            gettimeofday(&tv1, NULL);
            pad_matrix(&matrix_padded, &matrix, N_obs, N_doc, sec_budget, fixed_padding);
            gettimeofday(&tv2, NULL);
            printf("Padding done: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            gettimeofday(&tv1, NULL);
            observe_matrix(matrix_obs, &matrix_padded, N_obs);
            gettimeofday(&tv2, NULL);
            printf("Observed matrix generated: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            // Permute observed matrix randomly and attack
            gettimeofday(&tv1, NULL);
            attack(matrix_obs, &matrix_bg, &permutation, N_kw, N_obs, N_doc, sec_budget, fixed_padding, N_iter);
            gettimeofday(&tv2, NULL);
            printf("Main attack done: %f.\n",  (tv2.tv_usec - tv1.tv_usec) / 1000000.0 + (tv2.tv_sec - tv1.tv_sec));
            fflush(stdout);

            char output_fileName_full[40];
            sprintf(output_fileName_full, "%s%d-%d", output_fileName, round, iter);
            print_result(output_fileName_full, &permutation, &true_index, N_obs);
            //sprintf(output_fileName_full, "%s%d-%d-full", output_fileName, round, iter);
            //print_full_result(output_fileName_full, &permutation, &true_index, N_obs);
        }
    }

    free(matrix);
    free(matrix_padded);
    gsl_matrix_free(matrix_obs);
    return(0);
}

double log_score(int idx1, int idx2, gsl_matrix* matrix_obs, int** matrix, int** permutation, int N_kw, int N_doc, double sec_budget, int fixed_padding)
{
    int idx1_m = (*permutation)[idx1];
    int idx2_m = (*permutation)[idx2];

    if (idx1 == idx2)
    {
        int N_upper = 3 * sqrt((*matrix)[idx1_m*N_kw + idx2_m] * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]) / (double) N_doc);
        int N_lower = (*matrix)[idx1_m*N_kw + idx2_m] - N_upper;
        N_upper = (*matrix)[idx1_m*N_kw + idx2_m] + N_upper;
        int count_total = (int) gsl_matrix_get(matrix_obs, idx1, idx2);

        double score = 0;
        double prob = (*matrix)[idx1_m*N_kw + idx2_m] / (double) N_doc;
        for (int kk = N_lower; kk < N_upper+1; kk+=20)
            score += gsl_ran_binomial_pdf(kk, prob, N_doc) * gsl_ran_laplace_pdf(count_total - fixed_padding - 2*kk, 2.0/sec_budget);
        if (score == 0)
            return(-500.0);
    }

    int n1 = (int) gsl_matrix_get(matrix_obs, idx1, idx1) - (*matrix)[idx1_m*N_kw + idx1_m];
    int n2 = (int) gsl_matrix_get(matrix_obs, idx2, idx2) - (*matrix)[idx2_m*N_kw + idx2_m];

    double mean = 0.5 * gsl_matrix_get(matrix_obs, idx2, idx2) / N_doc * n1 + 0.5 * gsl_matrix_get(matrix_obs, idx1, idx1) / N_doc * n2;
    double var = 1.0 * (*matrix)[idx1_m*N_kw + idx2_m] / N_doc * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]);
    var += 0.25 * n1 / N_doc * gsl_matrix_get(matrix_obs, idx2, idx2) * (2 * N_doc - gsl_matrix_get(matrix_obs, idx2, idx2)) / N_doc;
    var += 0.25 * n2 / N_doc * gsl_matrix_get(matrix_obs, idx1, idx1) * (2 * N_doc - gsl_matrix_get(matrix_obs, idx1, idx1)) / N_doc;
    int count_total = (int) gsl_matrix_get(matrix_obs, idx1, idx2);

    int N_upper = 3 * sqrt((*matrix)[idx1_m*N_kw + idx2_m] * (N_doc - (*matrix)[idx1_m*N_kw + idx2_m]) / (double) N_doc);
    int N_lower = (*matrix)[idx1_m*N_kw + idx2_m] - N_upper;
    N_upper = (*matrix)[idx1_m*N_kw + idx2_m] + N_upper;

    double score = 0;
    double prob = (*matrix)[idx1_m*N_kw + idx2_m] / (double) N_doc;
    for (int kk = N_lower; kk < N_upper+1; kk+=20)
        score +=  gsl_ran_binomial_pdf(kk, prob, N_doc) * gsl_ran_gaussian_pdf(count_total - mean - kk, sqrt(var));

    if (score == 0)
        return(-500.0);
    return(log(20*score));
}

void initial_solution(int** permutation, gsl_matrix* matrix_obs, int** matrix, int fixed_padding, int N_kw, int N_obs)
{
    int* diff = (int*) malloc(sizeof(int) * N_kw);
    int* index = (int*) malloc(sizeof(int) * N_kw);
    int* occupied = (int*) malloc(sizeof(int) * N_kw);

    for (int ii = 0; ii < N_kw; ii++)
        occupied[ii] = -1;

    for (int ii = 0; ii < N_obs; ii++)
    {
        // reset index
        for (int jj = 0; jj < N_kw; jj++)
            index[jj] = jj;

        // compute differences
        for (int jj = 0; jj < N_kw; jj++)
            diff[jj] = abs(gsl_matrix_get(matrix_obs, ii, ii) - fixed_padding - 2*(*matrix)[jj*N_kw+jj]);

        // sorting
        for (int jj = 0; jj < N_kw-1; jj++)
        {
            for (int kk = 0; kk < N_kw-jj-1; kk++)
            {
                if (diff[kk] > diff[kk+1])
                {
                    int temp = diff[kk+1];
                    diff[kk+1] = diff[kk];
                    diff[kk] = temp;

                    temp = index[kk+1];
                    index[kk+1] = index[kk];
                    index[kk] = temp;
                }
            }
        }

        // use the smallest index available
        int idx = 0;
        while (occupied[idx] > 0)
            idx++;

        // update the data structures
        (*permutation)[ii] = index[idx];
        occupied[idx] = 1;
    }

}

void attack(gsl_matrix* matrix_obs, int** matrix, int** permutation, int N_kw, int N_obs, int N_doc, double sec_budget, int fixed_padding, int N_iter)
{
    // Initialise data structures
    double* score_matrix = (double*) malloc(sizeof(double) * N_obs * N_obs);
    double* score_row1 = (double*) malloc(sizeof(double) * N_obs);
    double* score_row2 = (double*) malloc(sizeof(double) * N_obs);

    int* permutation_tmp = (int*) malloc(sizeof(int) * N_obs);
    int* permutation_inv = (int*) malloc(sizeof(int) * N_kw);

    // Initialise permutations
    // Initial solution
    initial_solution(permutation, matrix_obs, matrix, fixed_padding, N_kw, N_obs);
    for (int ii = 0; ii < N_obs; ii++)
        permutation_tmp[ii] = (*permutation)[ii];
    for (int ii = 0; ii < N_kw; ii++)
        permutation_inv[ii] = -1;
    for (int ii = 0; ii < N_obs; ii++)
        permutation_inv[permutation_tmp[ii]] = ii;

    // Initialising RNG
    const gsl_rng_type * T;
    gsl_rng * r;
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);

    struct timeval tv1,tv2;
    gettimeofday(&tv1, NULL);

    // Compute initial score
    #pragma omp parallel for shared(score_matrix, matrix_obs, matrix)
    for (int ii = 0; ii < N_obs * N_obs; ii++)
        score_matrix[ii] = log_score((int) (ii / N_obs), ii % N_obs, matrix_obs, matrix, permutation, N_kw, N_doc, sec_budget, fixed_padding);
    gettimeofday(&tv2, NULL);
    printf("Initial score computed: %f.\n", (double) (tv2.tv_usec - tv1.tv_usec) / 1000000 + (double) (tv2.tv_sec - tv1.tv_sec));

    // Iterations of simulated annealing
    double temp = (double) N_kw;
    int N_stuck = 0;
    for (int iter = 0; iter < N_iter; iter++)
    {
        /* Status code */
        if (iter % (N_iter / 10) == 0)
        {
            gettimeofday(&tv1, NULL);
            printf("Iteration: %d, %d, %d.\n", iter, N_stuck, (int) (tv1.tv_sec - tv2.tv_sec));
            fflush(stdout);
            gettimeofday(&tv2, NULL);
        }

	    // used to be 10k
        if (N_stuck >= N_kw * 20)
            iter = N_iter;

        /* Main code */
        int idx1, idx2;
        permutation_generation(&idx1, &idx2, &permutation_tmp, permutation, &permutation_inv, matrix_obs, matrix, fixed_padding, N_kw, N_obs, N_doc, sec_budget);
        if (idx1 == idx2)
        {
            N_stuck += 1;
                continue;
        }

        #pragma omp parallel for shared(score_row1)
        for (int ii = 0; ii < N_obs; ii++)
            score_row1[ii] = log_score(idx1, ii, matrix_obs, matrix, &permutation_tmp, N_kw, N_doc, sec_budget, fixed_padding);

        if (idx2 >= 0)
            #pragma omp parallel for shared(score_row2)
            for (int ii = 0; ii < N_obs; ii++)
                score_row2[ii] = log_score(idx2, ii, matrix_obs, matrix, &permutation_tmp, N_kw, N_doc, sec_budget, fixed_padding);

        double score_diff = 0;
        for (int ii = 0; ii < N_obs; ii++)
            score_diff += score_row1[ii];
        for (int ii = 0; ii < N_obs; ii++)
            score_diff -= score_matrix[idx1*N_obs + ii];
        if (idx2 >= 0)
        {
            for (int ii = 0; ii < N_obs; ii++)
                score_diff += score_row2[ii];
            for (int ii = 0; ii < N_obs; ii++)
                score_diff -= score_matrix[idx2*N_obs + ii];
        }

        // compute difference in score, with exponentiation
        score_diff = score_diff / temp;

        if (score_diff < -40)
            score_diff = 0;
        else if (score_diff > 0)
            score_diff = 1.01;
        else
            score_diff = exp(score_diff);

        if (gsl_ran_flat(r, 0, 1) < score_diff)
        {
            // Update the scores
            for (int ii = 0; ii < N_obs; ii++)
                score_matrix[idx1*N_obs + ii] = score_row1[ii];
            for (int ii = 0; ii < N_obs; ii++)
                score_matrix[ii*N_obs + idx1] = score_row1[ii];
            if (idx2 >= 0)
            {
                for (int ii = 0; ii < N_obs; ii++)
                    score_matrix[idx2*N_obs + ii] = score_row2[ii];
                for (int ii = 0; ii < N_obs; ii++)
                    score_matrix[ii*N_obs + idx2] = score_row2[ii];
            }

            // Update the permutation
            permutation_inv[(*permutation)[idx1]] = -1;
            (*permutation)[idx1] = permutation_tmp[idx1];
            permutation_inv[permutation_tmp[idx1]] = idx1;
            if (idx2 >= 0)
            {
                (*permutation)[idx2] = permutation_tmp[idx2];
                permutation_inv[permutation_tmp[idx2]] = idx2;
            }
            N_stuck = 0;
        }
        else
        {
            // Update the permutation
            permutation_tmp[idx1] = (*permutation)[idx1];
            if (idx2 >= 0)
                permutation_tmp[idx2] = (*permutation)[idx2];
            N_stuck += 1;
        }

        temp *= 0.995;
    }

    free(score_matrix);
    free(score_row1);
    free(score_row2);
    gsl_rng_free(r);
}

void print_result(char* output_fileName, int** permutation, int** true_index, int N_obs)
{
    FILE* fp = fopen(output_fileName, "w");
    int count = 0;
    for (int ii = 0; ii < N_obs; ii++)
    {
        //if (ii < 50)
        //    printf("%d, %d\n", (*permutation)[ii], (*true_index)[ii]);
        if ((*permutation)[ii] == (*true_index)[ii])
            count++;
    }

    fprintf(fp, "%d\n", count);
    fclose(fp);
    printf("Success: %d/%d.\n", count, N_obs);
}

void print_full_result(char* output_fileName, int** permutation, int** true_index, int N_obs)
{
    FILE* fp = fopen(output_fileName, "w");
    int count = 0;
    for (int ii = 0; ii < N_obs; ii++)
        if ((*permutation)[ii] == (*true_index)[ii])
            count++;

    fprintf(fp, "%d\n", count);
    for (int ii = 0; ii < N_obs; ii++)
        fprintf(fp, "%d,%d\n", (*permutation)[ii], (*true_index)[ii]);

    fclose(fp);
}


#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>

// From Alam et al '02
double R_half_V_max_NFW(double M_vir, double z, int mode, cosmo_info **cosmo) {
    double c_vir;
    double R_vir;
    double r_o;
    double r_val = 0.;

    set_NFW_params(M_vir, z, mode, cosmo, &c_vir, &R_vir);

    r_o   = R_vir / c_vir;
    r_val = 0.13 * r_o;

    return (r_val);
}


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

#include <config.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_ieee_utils.h>

void rng_test (const gsl_rng_type * T, unsigned long int seed, unsigned int n,
               unsigned long int result);
void rng_float_test (const gsl_rng_type * T);
void generic_rng_test (const gsl_rng_type * T);
void rng_state_test (const gsl_rng_type * T);
void rng_parallel_state_test (const gsl_rng_type * T);
void rng_read_write_test (const gsl_rng_type * T);
int rng_max_test (gsl_rng * r, unsigned long int *kmax, unsigned long int ran_max) ;
int rng_min_test (gsl_rng * r, unsigned long int *kmin, unsigned long int ran_min, unsigned long int ran_max) ;
int rng_sum_test (gsl_rng * r, double *sigma);
int rng_bin_test (gsl_rng * r, double *sigma);

#define N  10000
#define N2 200000

int
main (void)
{
  const gsl_rng_type ** rngs = gsl_rng_types_setup();  /* get all rng types */

  const gsl_rng_type ** r ;

  gsl_ieee_env_setup ();

  gsl_rng_env_setup ();

  /* specific tests of known results for 10000 iterations with seed = 1 */

  rng_test (gsl_rng_rand, 1, 10000, 1910041713);
  rng_test (gsl_rng_randu, 1, 10000, 1623524161);
  rng_test (gsl_rng_cmrg, 1, 10000, 719452880);
  rng_test (gsl_rng_minstd, 1, 10000, 1043618065);
  rng_test (gsl_rng_mrg, 1, 10000, 2064828650);
  rng_test (gsl_rng_taus, 1, 10000, 2733957125UL);
  rng_test (gsl_rng_taus2, 1, 10000, 2733957125UL);
  rng_test (gsl_rng_taus113, 1, 1000, 1925420673UL);
  rng_test (gsl_rng_transputer, 1, 10000, 1244127297UL);
  rng_test (gsl_rng_vax, 1, 10000, 3051034865UL);

  /* Borosh13 test value from PARI: (1812433253^10000)%(2^32) */
  rng_test (gsl_rng_borosh13, 1, 10000, 2513433025UL);

  /* Fishman18 test value from PARI: (62089911^10000)%(2^31-1) */
  rng_test (gsl_rng_fishman18, 1, 10000, 330402013UL);

  /* Fishman2x test value from PARI:
     ((48271^10000)%(2^31-1) - (40692^10000)%(2^31-249))%(2^31-1) */
  rng_test (gsl_rng_fishman2x, 1, 10000, 540133597UL);

  /* Knuthran2 test value from PARI:
     { xn1=1; xn2=1; for (n=1,10000,
            xn = (271828183*xn1 - 314159269*xn2)%(2^31-1);
            xn2=xn1; xn1=xn; print(xn); ) } */
  rng_test (gsl_rng_knuthran2, 1, 10000, 1084477620UL);

  /* Knuthran test value taken from p188 in Knuth Vol 2. 3rd Ed */
  rng_test (gsl_rng_knuthran, 310952, 1009 * 2009 + 1, 461390032);

  /* Knuthran improved test value from Knuth's source */
  rng_test (gsl_rng_knuthran2002, 310952, 1, 708622036);
  rng_test (gsl_rng_knuthran2002, 310952, 2, 1005450560);
  rng_test (gsl_rng_knuthran2002, 310952, 100 * 2009 + 1, 995235265);
  rng_test (gsl_rng_knuthran2002, 310952, 1009 * 2009 + 1, 704987132);

  /* Lecuyer21 test value from PARI: (40692^10000)%(2^31-249) */
  rng_test (gsl_rng_lecuyer21, 1, 10000, 2006618587UL);

  /* Waterman14 test value from PARI: (1566083941^10000)%(2^32) */
  rng_test (gsl_rng_waterman14, 1, 10000, 3776680385UL);

  /* specific tests of known results for 10000 iterations with seed = 6 */

  /* Coveyou test value from PARI:
     x=6; for(n=1,10000,x=(x*(x+1))%(2^32);print(x);) */

  rng_test (gsl_rng_coveyou, 6, 10000, 1416754246UL);

  /* Fishman20 test value from PARI: (6*48271^10000)%(2^31-1) */
  rng_test (gsl_rng_fishman20, 6, 10000, 248127575UL);

  /* FIXME: the ranlux tests below were made by running the fortran code and
     getting the expected value from that. An analytic calculation
     would be preferable. */

  rng_test (gsl_rng_ranlux, 314159265, 10000, 12077992);
  rng_test (gsl_rng_ranlux389, 314159265, 10000, 165942);

  rng_test (gsl_rng_ranlxs0, 1, 10000, 11904320);
  /* 0.709552764892578125 * ldexp(1.0,24) */

  rng_test (gsl_rng_ranlxs1, 1, 10000, 8734328);
  /* 0.520606517791748047 * ldexp(1.0,24) */

  rng_test (gsl_rng_ranlxs2, 1, 10000, 6843140);
  /* 0.407882928848266602 * ldexp(1.0,24) */

  rng_test (gsl_rng_ranlxd1, 1, 10000, 1998227290UL);
  /* 0.465248546261094020 * ldexp(1.0,32) */

  rng_test (gsl_rng_ranlxd2, 1, 10000, 3949287736UL);
  /* 0.919515205581550532 * ldexp(1.0,32) */

  /* FIXME: the tests below were made by running the original code in
     the ../random directory and getting the expected value from
     that. An analytic calculation would be preferable. */

  rng_test (gsl_rng_slatec, 1, 10000, 45776);
  rng_test (gsl_rng_uni, 1, 10000, 9214);
  rng_test (gsl_rng_uni32, 1, 10000, 1155229825);
  rng_test (gsl_rng_zuf, 1, 10000, 3970);

  /* The tests below were made by running the original code and
     getting the expected value from that. An analytic calculation
     would be preferable. */

  rng_test (gsl_rng_r250, 1, 10000, 1100653588);
  rng_test (gsl_rng_mt19937, 4357, 1000, 1186927261);
  rng_test (gsl_rng_mt19937_1999, 4357, 1000, 1030650439);
  rng_test (gsl_rng_mt19937_1998, 4357, 1000, 1309179303);
  rng_test (gsl_rng_tt800, 0, 10000, 2856609219UL);

  rng_test (gsl_rng_ran0, 0, 10000, 1115320064);
  rng_test (gsl_rng_ran1, 0, 10000, 1491066076);
  rng_test (gsl_rng_ran2, 0, 10000, 1701364455);
  rng_test (gsl_rng_ran3, 0, 10000, 186340785);

  rng_test (gsl_rng_ranmar, 1, 10000, 14428370);

  rng_test (gsl_rng_rand48, 0, 10000, 0xDE095043UL);
  rng_test (gsl_rng_rand48, 1, 10000, 0xEDA54977UL);

  rng_test (gsl_rng_random_glibc2, 0, 10000, 1908609430);
  rng_test (gsl_rng_random8_glibc2, 0, 10000, 1910041713);
  rng_test (gsl_rng_random32_glibc2, 0, 10000, 1587395585);
  rng_test (gsl_rng_random64_glibc2, 0, 10000, 52848624);
  rng_test (gsl_rng_random128_glibc2, 0, 10000, 1908609430);
  rng_test (gsl_rng_random256_glibc2, 0, 10000, 179943260);

  rng_test (gsl_rng_random_bsd, 0, 10000, 1457025928);
  rng_test (gsl_rng_random8_bsd, 0, 10000, 1910041713);
  rng_test (gsl_rng_random32_bsd, 0, 10000, 1663114331);
  rng_test (gsl_rng_random64_bsd, 0, 10000, 864469165);
  rng_test (gsl_rng_random128_bsd, 0, 10000, 1457025928);
  rng_test (gsl_rng_random256_bsd, 0, 10000, 1216357476);

  rng_test (gsl_rng_random_libc5, 0, 10000, 428084942);
  rng_test (gsl_rng_random8_libc5, 0, 10000, 1910041713);
  rng_test (gsl_rng_random32_libc5, 0, 10000, 1967452027);
  rng_test (gsl_rng_random64_libc5, 0, 10000, 2106639801);
  rng_test (gsl_rng_random128_libc5, 0, 10000, 428084942);
  rng_test (gsl_rng_random256_libc5, 0, 10000, 116367984);

  rng_test (gsl_rng_ranf, 0, 10000, 2152890433UL);
  rng_test (gsl_rng_ranf, 2, 10000, 339327233);

  /* Test constant relationship between int and double functions */

  for (r = rngs ; *r != 0; r++)
    rng_float_test (*r);

  /* Test save/restore functions */

  for (r = rngs ; *r != 0; r++)
    rng_state_test (*r);

  for (r = rngs ; *r != 0; r++)
    rng_parallel_state_test (*r);

  for (r = rngs ; *r != 0; r++)
    rng_read_write_test (*r);

  /* generic statistical tests (these are just to make sure that we
     don't get any crazy results back from the generator, i.e. they
     aren't a test of the algorithm, just the implementation) */

  for (r = rngs ; *r != 0; r++)
    generic_rng_test (*r);

  exit (gsl_test_summary ());
}

void
rng_test (const gsl_rng_type * T, unsigned long int seed, unsigned int n,
          unsigned long int result)
{
  gsl_rng *r = gsl_rng_alloc (T);
  unsigned int i;
  unsigned long int k = 0;
  int status;

  if (seed != 0)
    {
      gsl_rng_set (r, seed);
    }

  for (i = 0; i < n; i++)
    {
      k = gsl_rng_get (r);
    }

  status = (k != result);
  gsl_test (status, "%s, %u steps (%u observed vs %u expected)",
            gsl_rng_name (r), n, k, result);

  gsl_rng_free (r);
}

void
rng_float_test (const gsl_rng_type * T)
{
  gsl_rng *ri = gsl_rng_alloc (T);
  gsl_rng *rf = gsl_rng_alloc (T);

  double u, c ;
  unsigned int i;
  unsigned long int k = 0;
  int status = 0 ;

  do
    {
      k = gsl_rng_get (ri);
      u = gsl_rng_get (rf);
    }
  while (k == 0) ;

  c = k / u ;
  for (i = 0; i < N2; i++)
    {
      k = gsl_rng_get (ri);
      u = gsl_rng_get (rf);
      if (c*k != u)
        {
          status = 1 ;
          break ;
        }
    }

  gsl_test (status, "%s, ratio of int to double (%g observed vs %g expected)",
            gsl_rng_name (ri), c, k/u);

  gsl_rng_free (ri);
  gsl_rng_free (rf);
}

void
rng_state_test (const gsl_rng_type * T)
{
  unsigned long int test_a[N], test_b[N];

  int i;

  gsl_rng *r = gsl_rng_alloc (T);
  gsl_rng *r_save = gsl_rng_alloc (T);

  for (i = 0; i < N; ++i)
    {
      gsl_rng_get (r);  /* throw away N iterations */
    }

  gsl_rng_memcpy (r_save, r);   /* save the intermediate state */

  for (i = 0; i < N; ++i)
    {
      test_a[i] = gsl_rng_get (r);
    }

  gsl_rng_memcpy (r, r_save);   /* restore the intermediate state */
  gsl_rng_free (r_save);

  for (i = 0; i < N; ++i)
    {
      test_b[i] = gsl_rng_get (r);
    }

  {
    int status = 0;
    for (i = 0; i < N; ++i)
      {
        status |= (test_b[i] != test_a[i]);
      }
    gsl_test (status, "%s, random number state consistency",
              gsl_rng_name (r));
  }

  gsl_rng_free (r);
}

void
rng_parallel_state_test (const gsl_rng_type * T)
{
  unsigned long int test_a[N], test_b[N];
  unsigned long int test_c[N], test_d[N];
  double test_e[N], test_f[N];

  int i;

  gsl_rng *r1 = gsl_rng_alloc (T);
  gsl_rng *r2 = gsl_rng_alloc (T);

  for (i = 0; i < N; ++i)
    {
      gsl_rng_get (r1);         /* throw away N iterations */
    }

  gsl_rng_memcpy (r2, r1);              /* save the intermediate state */

  for (i = 0; i < N; ++i)
    {
      /* check that there is no hidden state intermixed between r1 and r2 */
      test_a[i] = gsl_rng_get (r1);
      test_b[i] = gsl_rng_get (r2);
      test_c[i] = gsl_rng_uniform_int (r1, 1234);
      test_d[i] = gsl_rng_uniform_int (r2, 1234);
      test_e[i] = gsl_rng_uniform (r1);
      test_f[i] = gsl_rng_uniform (r2);
    }

  {
    int status = 0;
    for (i = 0; i < N; ++i)
      {
        status |= (test_b[i] != test_a[i]);
        status |= (test_c[i] != test_d[i]);
        status |= (test_e[i] != test_f[i]);
      }
    gsl_test (status, "%s, parallel random number state consistency",
              gsl_rng_name (r1));
  }

  gsl_rng_free (r1);
  gsl_rng_free (r2);

}

void
rng_read_write_test (const gsl_rng_type * T)
{
  unsigned long int test_a[N], test_b[N];

  int i;

  gsl_rng *r = gsl_rng_alloc (T);

  for (i = 0; i < N; ++i)
    {
      gsl_rng_get (r);  /* throw away N iterations */
    }

  { /* save the state to a binary file */
    FILE *f = fopen("test.dat", "wb");
    gsl_rng_fwrite(f, r);
    fclose(f);
  }

  for (i = 0; i < N; ++i)
    {
      test_a[i] = gsl_rng_get (r);
    }

  { /* read the state from a binary file */
    FILE *f = fopen("test.dat", "rb");
    gsl_rng_fread(f, r);
    fclose(f);
  }

  for (i = 0; i < N; ++i)
    {
      test_b[i] = gsl_rng_get (r);
    }

  {
    int status = 0;
    for (i = 0; i < N; ++i)
      {
        status |= (test_b[i] != test_a[i]);
      }
    gsl_test (status, "%s, random number generator read and write",
              gsl_rng_name (r));
  }

  gsl_rng_free (r);
}

void
generic_rng_test (const gsl_rng_type * T)
{
  gsl_rng *r = gsl_rng_alloc (T);
  const char *name = gsl_rng_name (r);
  unsigned long int kmax = 0, kmin = 1000;
  double sigma = 0;
  const unsigned long int ran_max = gsl_rng_max (r);
  const unsigned long int ran_min = gsl_rng_min (r);

  int status = rng_max_test (r, &kmax, ran_max);

  gsl_test (status,
            "%s, observed vs theoretical maximum (%lu vs %lu)",
            name, kmax, ran_max);

  status = rng_min_test (r, &kmin, ran_min, ran_max);

  gsl_test (status,
            "%s, observed vs theoretical minimum (%lu vs %lu)",
            name, kmin, ran_min);

  status = rng_sum_test (r, &sigma);

  gsl_test (status,
            "%s, sum test within acceptable sigma (observed %.2g sigma)",
            name, sigma);

  status = rng_bin_test (r, &sigma);

  gsl_test (status,
            "%s, bin test within acceptable chisq (observed %.2g sigma)",
            name, sigma);

  gsl_rng_set (r, 1);   /* set seed to 1 */
  status = rng_max_test (r, &kmax, ran_max);

  gsl_rng_set (r, 1);   /* set seed to 1 */
  status |= rng_min_test (r, &kmin, ran_min, ran_max);

  gsl_rng_set (r, 1);   /* set seed to 1 */
  status |= rng_sum_test (r, &sigma);

  gsl_test (status, "%s, maximum and sum tests for seed=1", name);

  gsl_rng_set (r, 12345);       /* set seed to a "typical" value */
  status = rng_max_test (r, &kmax, ran_max);

  gsl_rng_set (r, 12345);       /* set seed to a "typical" value */
  status |= rng_min_test (r, &kmin, ran_min, ran_max);

  gsl_rng_set (r, 12345);       /* set seed to a "typical" value */
  status |= rng_sum_test (r, &sigma);

  gsl_test (status, "%s, maximum and sum tests for non-default seeds", name);

  gsl_rng_free (r);
}

int
rng_max_test (gsl_rng * r, unsigned long int *kmax, unsigned long int ran_max)
{
  unsigned long int actual_uncovered;
  double expect_uncovered;
  int status;
  unsigned long int max = 0;
  int i;

  for (i = 0; i < N2; ++i)
    {
      unsigned long int k = gsl_rng_get (r);
      if (k > max)
        max = k;
    }

  *kmax = max;

  actual_uncovered = ran_max - max;
  expect_uncovered = (double) ran_max / (double) N2;

  status = (max > ran_max) || (actual_uncovered > 7 * expect_uncovered) ;

  return status;
}

int
rng_min_test (gsl_rng * r, unsigned long int *kmin,
              unsigned long int ran_min, unsigned long int ran_max)
{
  unsigned long int actual_uncovered;
  double expect_uncovered;
  int status;
  unsigned long int min = 1000000000UL;
  int i;

  for (i = 0; i < N2; ++i)
    {
      unsigned long int k = gsl_rng_get (r);
      if (k < min)
        min = k;
    }

  *kmin = min;

  actual_uncovered = min - ran_min;
  expect_uncovered = (double) ran_max / (double) N2;

  status = (min < ran_min) || (actual_uncovered > 7 * expect_uncovered);

  return status;
}

int
rng_sum_test (gsl_rng * r, double *sigma)
{
  double sum = 0;
  int i, status;

  for (i = 0; i < N2; ++i)
    {
      double x = gsl_rng_uniform (r) - 0.5;
      sum += x;
    }

  sum /= N2;

  /* expect the average to have a variance of 1/(12 n) */

  *sigma = sum * sqrt (12.0 * N2);

  /* more than 3 sigma is an error (increased to 3.1 to avoid false alarms) */

  status = (fabs (*sigma) > 3.1 || fabs(*sigma) < 0.003);

  if (status) {
      fprintf(stderr,"sum=%g, sigma=%g\n",sum,*sigma);
  }

  return status;
}

#define BINS 17
#define EXTRA 10
int
rng_bin_test (gsl_rng * r, double *sigma)
{
  int count[BINS+EXTRA];
  double chisq = 0;
  int i, status;

  for (i = 0; i < BINS+EXTRA; i++)
      count[i] = 0 ;

  for (i = 0; i < N2; i++)
    {
      int j = gsl_rng_uniform_int (r, BINS);
      count[j]++ ;
    }

  chisq = 0 ;
  for (i = 0; i < BINS; i++)
    {
      double x = (double)N2/(double)BINS ;
      double d = (count[i] - x) ;
      chisq += (d*d) / x;
    }

  *sigma = sqrt(chisq/BINS) ;

  /* more than 3 sigma is an error */

  status = (fabs (*sigma) > 3 || fabs(*sigma) < 0.003);

  for (i = BINS; i < BINS+EXTRA; i++)
    {
      if (count[i] != 0)
        {
          status = 1 ;
          gsl_test (status,
                    "%s, wrote outside range in bin test "
                    "(%d observed vs %d expected)",
                    gsl_rng_name(r), i, BINS - 1);
        }
    }

  return status;
}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifndef SCILIB_LINALG_BLAS2_MATRIX_VECTOR_PRODUCT_H
#define SCILIB_LINALG_BLAS2_MATRIX_VECTOR_PRODUCT_H

#ifdef USE_MKL
#include <mkl.h>
#else
#include <cblas.h>
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg_impl/lapack_types.h>
#include <experimental/mdspan>
#include <cassert>
#include <complex>
#include <type_traits>
#include <iostream>

namespace Sci {
namespace Linalg {

namespace stdex = std::experimental;

template <class T_a,
          stdex::extents<>::size_type nrows_a,
          stdex::extents<>::size_type ncols_a,
          class Layout_a,
          class Accessor_a,
          class T_x,
          stdex::extents<>::size_type ext_x,
          class Layout_x,
          class Accessor_x,
          class T_y,
          stdex::extents<>::size_type ext_y,
          class Layout_y,
          class Accessor_y>
    requires(!std::is_const_v<T_y>)
inline void matrix_vector_product(
    stdex::mdspan<T_a, stdex::extents<nrows_a, ncols_a>, Layout_a, Accessor_a>
        a,
    stdex::mdspan<T_x, stdex::extents<ext_x>, Layout_x, Accessor_x> x,
    stdex::mdspan<T_y, stdex::extents<ext_y>, Layout_y, Accessor_y> y)
{
    static_assert(x.static_extent(0) == a.static_extent(1));
    using size_type = stdex::extents<>::size_type;

    for (size_type i = 0; i < a.extent(0); ++i) {
        y(i) = T_y{0};
        for (size_type j = 0; j < a.extent(1); ++j) {
            y(i) += a(i, j) * x(j);
        }
    }
}

template <class Layout>
inline void matrix_vector_product(Sci::Matrix_view<double, Layout> a,
                                  Sci::Vector_view<double, Layout> x,
                                  Sci::Vector_view<double, Layout> y)
{
    static_assert(x.static_extent(0) == a.static_extent(1));

    constexpr double alpha = 1.0;
    constexpr double beta = 0.0;

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    const BLAS_INT incx = static_cast<BLAS_INT>(x.stride(0));
    const BLAS_INT incy = static_cast<BLAS_INT>(y.stride(0));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
    }
    cblas_dgemv(matrix_layout, CblasNoTrans, m, n, alpha, a.data(), lda,
                x.data(), incx, beta, y.data(), incy);
}

template <class Layout>
inline void matrix_vector_product(Sci::Matrix_view<const double, Layout> a,
                                  Sci::Vector_view<const double, Layout> x,
                                  Sci::Vector_view<double, Layout> y)
{
    static_assert(x.static_extent(0) == a.static_extent(1));

    constexpr double alpha = 1.0;
    constexpr double beta = 0.0;

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    const BLAS_INT incx = static_cast<BLAS_INT>(x.stride(0));
    const BLAS_INT incy = static_cast<BLAS_INT>(y.stride(0));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
    }
    cblas_dgemv(matrix_layout, CblasNoTrans, m, n, alpha, a.data(), lda,
                x.data(), incx, beta, y.data(), incy);
}

#ifdef USE_MKL
// Does not work with OpenBLAS version 0.2.14.1
template <class Layout>
inline void
matrix_vector_product(Sci::Matrix_view<std::complex<double>, Layout> a,
                      Sci::Vector_view<std::complex<double>, Layout> x,
                      Sci::Vector_view<std::complex<double>, Layout> y)
{
    static_assert(x.static_extent(0) == a.static_extent(1));

    constexpr std::complex<double> alpha = {1.0, 0.0};
    constexpr std::complex<double> beta = {0.0, 0.0};

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    const BLAS_INT incx = static_cast<BLAS_INT>(x.stride(0));
    const BLAS_INT incy = static_cast<BLAS_INT>(y.stride(0));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
    }
    cblas_zgemv(matrix_layout, CblasNoTrans, m, n, &alpha, a.data(), lda,
                x.data(), incx, &beta, y.data(), incy);
}

template <class Layout>
inline void
matrix_vector_product(Sci::Matrix_view<const std::complex<double>, Layout> a,
                      Sci::Vector_view<const std::complex<double>, Layout> x,
                      Sci::Vector_view<std::complex<double>, Layout> y)
{
    static_assert(x.static_extent(0) == a.static_extent(1));

    constexpr std::complex<double> alpha = {1.0, 0.0};
    constexpr std::complex<double> beta = {0.0, 0.0};

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));
    const BLAS_INT incx = static_cast<BLAS_INT>(x.stride(0));
    const BLAS_INT incy = static_cast<BLAS_INT>(y.stride(0));

    auto matrix_layout = CblasRowMajor;
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = CblasColMajor;
        lda = m;
    }
    cblas_zgemv(matrix_layout, CblasNoTrans, m, n, &alpha, a.data(), lda,
                x.data(), incx, &beta, y.data(), incy);
}
#endif

template <class T, class Layout, class Allocator>
inline Sci::Vector<T, Layout, Allocator>
matrix_vector_product(const Sci::Matrix<T, Layout, Allocator>& a,
                      const Sci::Vector<T, Layout, Allocator>& x)
{
    Sci::Vector<T, Layout, Allocator> res(a.extent(0));
    matrix_vector_product(a.view(), x.view(), res.view());
    return res;
}

template <class T, class Layout, class Allocator>
inline void matrix_vector_product(const Sci::Matrix<T, Layout, Allocator>& a,
                                  const Sci::Vector<T, Layout, Allocator>& x,
                                  Sci::Vector<T, Layout, Allocator>& res)
{
    assert(res.size() == a.extent(0));
    matrix_vector_product(a.view(), x.view(), res.view());
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_BLAS2_MATRIX_VECTOR_PRODUCT_H


#ifndef INCLUDED_TELEPATH_MATRIX_H
#define INCLUDED_TELEPATH_MATRIX_H

#include <cblas.h>
#include "telepath/blas/traits.h"

namespace telepath{

    enum ORDER {
        ROW_MAJOR = CblasRowMajor,
        COL_MAJOR = CblasColMajor
    };
    enum TRANSPOSE {
        NONE = CblasNoTrans,
        TRANS = CblasTrans,
        CONJ_TRANS = CblasConjTrans
    };
    enum UPLO{
        UPPER = CblasUpper,
        LOWER = CblasLower,
    };

    /*
    Struct for storing BLAS matrix data
    */
    template< typename T >
    struct matrix{
        ORDER layout;
        TRANSPOSE trans;
        T* array;
        std::size_t rows;
        std::size_t cols;
        std::size_t ld;
    };

    template< typename T >
    struct sym_matrix{
        ORDER layout;
        TRANSPOSE trans;
        UPLO uplo;
        T* array;
        std::size_t rows;
        std::size_t cols;
        std::size_t ld;
    };

} //namespace telepath

namespace blas{

    template< typename T >
    struct Traits< const telepath::matrix<T> >{
        using mat_t = typename telepath::matrix<T>;
        const mat_t& mat;
        constexpr Traits( const mat_t& m ) : mat( m ) { }
        constexpr telepath::ORDER layout() const{ return mat.layout; }
        constexpr telepath::TRANSPOSE trans() const{ return mat.trans; }
        constexpr const T* array() const{ return mat.array; }
        constexpr auto nrows() const{ return mat.rows; }
        constexpr auto ncols() const{ return mat.cols; }
        constexpr auto ld() const{ return mat.ld; }
    };

    template< typename T >
    struct Traits< telepath::matrix<T> >{
        using mat_t = typename telepath::matrix<T>;
        mat_t& mat;
        constexpr Traits( mat_t& m ) : mat( m ) { }
        constexpr telepath::ORDER layout() const{ return mat.layout; }
        constexpr telepath::TRANSPOSE trans() const{ return mat.trans; }
        inline T* array() { return mat.array; }
        constexpr const T* array() const{ return mat.array; }
        constexpr auto nrows() const{ return mat.rows; }
        constexpr auto ncols() const{ return mat.cols; }
        constexpr auto ld() const{ return mat.ld; }
    };

    template< typename T >
    struct Traits< const telepath::sym_matrix<T> >{
        using mat_t = telepath::sym_matrix<T>;
        const mat_t& mat;
        constexpr Traits( const mat_t& m ) : mat( m ) { }
        constexpr telepath::ORDER layout() const{ return mat.layout; }
        constexpr telepath::TRANSPOSE trans() const{ return mat.trans; }
        constexpr telepath::UPLO uplo() const{ return mat.uplo; }
        constexpr const T* array() const{ return mat.array; }
        constexpr auto nrows() const{ return mat.rows; }
        constexpr auto ncols() const{ return mat.cols; }
        constexpr auto ld() const{ return mat.ld; }
    };

    template< typename T >
    struct Traits< telepath::sym_matrix<T> >{
        using mat_t = typename telepath::sym_matrix<T>;
        mat_t& mat;
        constexpr Traits( mat_t& m ) : mat( m ) { }
        constexpr telepath::ORDER layout() const{ return mat.layout; }
        constexpr telepath::TRANSPOSE trans() const{ return mat.trans; }
        constexpr telepath::UPLO uplo() const{ return mat.uplo; }
        constexpr const T* array() const{ return mat.array; }
        inline T* array() { return mat.array; }
        constexpr auto nrows() const{ return mat.rows; }
        constexpr auto ncols() const{ return mat.cols; }
        constexpr auto ld() const{ return mat.ld; }
    };

    template< typename T >
    struct MatrixTraits< telepath::matrix<T> >{
        using mat = typename telepath::matrix<T>;
        using scalar_t = T;
        constexpr static CBLAS_ORDER layout( const mat& m ){
            return (CBLAS_ORDER) m.layout;
        }
        constexpr static CBLAS_TRANSPOSE trans( const mat& m ){
            return (CBLAS_TRANSPOSE) m.trans;
        }
        constexpr static auto nrows( const mat& m ){ return m.rows; }
        constexpr static auto ncols( const mat& m ){ return m.cols; }
        constexpr static T* array( mat& m ){ return m.array; }
        constexpr static const T* array( const mat& m ){ return m.array; }
        constexpr static auto ld( const mat& m ){ return m.ld; }
    };

    template< typename T >
    struct MatrixTraits< telepath::sym_matrix<T> >{
        using mat = typename telepath::sym_matrix<T>;
        using scalar_t = T;
        constexpr static CBLAS_ORDER layout( const mat& m ){
            return (CBLAS_ORDER) m.layout;
        }
        constexpr static CBLAS_TRANSPOSE trans( const mat& m ){
            return (CBLAS_TRANSPOSE) m.trans;
        }
        constexpr static CBLAS_UPLO uplo( const mat& m ){
            return (CBLAS_UPLO) m.uplo;
        }
        constexpr static auto nrows( const mat& m ){ return m.rows; }
        constexpr static auto ncols( const mat& m ){ return m.cols; }
        constexpr static T* array( mat& m ){ return m.array; }
        constexpr static const T* array( const mat& m ){ return m.array; }
        constexpr static auto ld( const mat& m ){ return m.ld; }
    };

} //namespace blas

#endif


/**
 *
 * @file core_zgessm.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include <cblas.h>
#include <lapacke.h>
#include "common.h"

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex64_t
 *
 *  CORE_zgessm applies the factors L computed by CORE_zgetrf_incpiv to
 *  a complex M-by-N tile A.
 *
 *******************************************************************************
 *
 * @param[in] M
 *          The number of rows of the tile A.  M >= 0.
 *
 * @param[in] N
 *         The number of columns of the tile A.  N >= 0.
 *
 * @param[in] K
 *         The number of columns of the tile L. K >= 0.
 *
 * @param[in] IB
 *         The inner-blocking size.  IB >= 0.
 *
 * @param[in] IPIV
 *         The pivot indices array of size K as returned by
 *         CORE_zgetrf_incpiv.
 *
 * @param[in] L
 *         The M-by-K lower triangular tile.
 *
 * @param[in] LDL
 *         The leading dimension of the array L.  LDL >= max(1,M).
 *
 * @param[in,out] A
 *         On entry, the M-by-N tile A.
 *         On exit, updated by the application of L.
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,M).
 *
 *******************************************************************************
 *
 * @return
 *         \retval PLASMA_SUCCESS successful exit
 *         \retval <0 if INFO = -k, the k-th argument had an illegal value
 *
 ******************************************************************************/
#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_zgessm = PCORE_zgessm
#define CORE_zgessm PCORE_zgessm
#endif
int CORE_zgessm(int M, int N, int K, int IB,
                const int *IPIV,
                const PLASMA_Complex64_t *L, int LDL,
                PLASMA_Complex64_t *A, int LDA)
{
    static PLASMA_Complex64_t zone  =  1.0;
    static PLASMA_Complex64_t mzone = -1.0;
    static int                ione  =  1;

    int i, sb;
    int tmp, tmp2;

    /* Check input arguments */
    if (M < 0) {
        coreblas_error(1, "Illegal value of M");
        return -1;
    }
    if (N < 0) {
        coreblas_error(2, "Illegal value of N");
        return -2;
    }
    if (K < 0) {
        coreblas_error(3, "Illegal value of K");
        return -3;
    }
    if (IB < 0) {
        coreblas_error(4, "Illegal value of IB");
        return -4;
    }
    if ((LDL < max(1,M)) && (M > 0)) {
        coreblas_error(7, "Illegal value of LDL");
        return -7;
    }
    if ((LDA < max(1,M)) && (M > 0)) {
        coreblas_error(9, "Illegal value of LDA");
        return -9;
    }

    /* Quick return */
    if ((M == 0) || (N == 0) || (K == 0) || (IB == 0))
        return PLASMA_SUCCESS;

    for(i = 0; i < K; i += IB) {
        sb = min(IB, K-i);
        /*
         * Apply interchanges to columns I*IB+1:IB*( I+1 )+1.
         */
        tmp  = i+1;
        tmp2 = i+sb;
        LAPACKE_zlaswp_work(LAPACK_COL_MAJOR, N, A, LDA, tmp, tmp2, IPIV, ione);
        /*
         * Compute block row of U.
         */
        cblas_ztrsm(
            CblasColMajor, CblasLeft, CblasLower, CblasNoTrans, CblasUnit,
            sb, N, CBLAS_SADDR(zone),
            &L[LDL*i+i], LDL,
            &A[i], LDA );

        if (i+sb < M) {
        /*
        * Update trailing submatrix.
        */
        cblas_zgemm(
            CblasColMajor, CblasNoTrans, CblasNoTrans,
            M-(i+sb), N, sb,
            CBLAS_SADDR(mzone), &L[LDL*i+(i+sb)], LDL,
            &A[i], LDA,
            CBLAS_SADDR(zone), &A[i+sb], LDA );
        }
    }
    return PLASMA_SUCCESS;
}


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/*
 Contains the PERDIF program that implements personalized diffusions for
 recommendations.

 Code by: Dimitris Berberidis and Athanasios N. Nikolakopoulos
 University of Minnesota 2018-2019
*/

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <inttypes.h>
#include <math.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
//#include <cblas.h>
#include <mkl.h>

#include "perdif_fit.h"
#include "rec_defs.h"

#if DEBUG
static double cost_func(double *, double *, double *, sz_med_t);
#endif

static void matrix_matrix_product(double *C, double *A, double *B, int, int,  int);
static double *unfold_matrix(d_mat_t);
static void constr_QP_with_PG(double *, double *, double *, sz_med_t);
static void grammian_matrix(double *, double *, int, int);
static double max_abs_dif(double *, double *, sz_long_t);
// static void project_to_simplex(double *, sz_med_t);
// static void project_to_simplex_thanos(double *, double *, sz_med_t length);
static void simplexproj_Duchi(double *y, double *x, const unsigned int length);
static void matvec(double *, double *, double *, int, int);
static double frob_norm(double *, sz_med_t);

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

void k_simplex(double *theta, d_mat_t P, int max_walk, double *dummy, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  // Unfold P for BLAS compatibility
  double *P_unfold = unfold_matrix(P);

  // Prepare parameters for constraned QP

  // first the Hessian matrix
  double A[max_walk * max_walk];
  grammian_matrix(A, P_unfold, (int)P.num_rows, (int)P.num_cols);

  // then the linear part
  double b[max_walk];

  if (rmse) {
    for (int i = 0; i < max_walk; i++)
      b[i] = -2.0 * (P_unfold[i]);
  } else {
    for (int i = 0; i < max_walk; i++) {
      b[i] = 0.0;
      for (int j = 0; j < P.num_rows; j++) {
        b[i] += P.val[i][j];
      }
      b[i] *= -2.0;
    }
  }

  constr_QP_with_PG(theta, A, b, (sz_med_t)max_walk);

  // free
  free(P_unfold);
}

// Simple search returns the single best step ( i.e. theta = [0 0 0 1 0 0 .... 0
// 0 ] )

void single_best(double *theta, d_mat_t P, int max_walk, double *dummy, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  for (int k = 0; k < max_walk; k++)
    theta[k] = 0.0;

  double loss[max_walk];

  // Compute loss of each k-step and find smallest
  int best_k = 0;
  double min_loss = 1.0e5;
  for (int k = 0; k < max_walk; k++) {
    loss[k] += pow(1.0 - P.val[0][k], 2);
    if (rmse) {
      for (int j = 1; j < P.num_rows; j++)
        loss[k] += P.val[j][k] * P.val[j][k];
    } else {
      for (int j = 1; j < P.num_rows; j++)
        loss[k] += pow(1.0 - P.val[j][k], 2);
    }
    if (loss[k] < min_loss) {
      min_loss = loss[k];
      best_k = k;
    }
  }

  theta[best_k] = 1.0;
}

void ppr_grid(double *theta, d_mat_t P, int max_walk, double *ppr_c, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  // Unfold P for BLAS compatibility
  double *P_unfold = unfold_matrix(P);

  // Candidate diffusions
  double *C = (double *)malloc(P.num_rows * GRID_POINTS * sizeof(double));
  matrix_matrix_product(C, P_unfold, ppr_c, P.num_rows, max_walk, GRID_POINTS);

  // find the best one
  double loss[GRID_POINTS];
  int best_point = 0;
  double min_loss = 1.0e5;
  for (int i = 0; i < GRID_POINTS; i++) {
    // first the regularizer
    loss[i] = 0.0;
    // then the ratings
    if (rmse) {
      loss[i] += pow(1.0 - C[i], 2);
      for (int j = 1; j < P.num_rows; j++)
        loss[i] += pow(C[j * GRID_POINTS + i], 2);
    } else {
      for (int j = 0; j < P.num_rows; j++)
        loss[i] += pow(1.0 - C[j * GRID_POINTS + i], 2);
    }

    if (loss[i] < min_loss) {
      min_loss = loss[i];
      best_point = i;
    }
  }

  // return the best one
  for (int k = 0; k < max_walk; k++)
    theta[k] = ppr_c[k * GRID_POINTS + best_point];

  // free
  free(P_unfold);
  free(C);
}

void hk_grid(double *theta, d_mat_t P, int max_walk, double *hk_c, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  // Unfold P for BLAS compatibility
  double *P_unfold = unfold_matrix(P);

  double *C = (double *)malloc(P.num_rows * GRID_POINTS * sizeof(double));
  matrix_matrix_product(C, P_unfold, hk_c, P.num_rows, max_walk, GRID_POINTS);

  // find the best one
  double loss[GRID_POINTS];
  int best_point = 0;
  double min_loss = 1.0e5;
  for (int i = 0; i < GRID_POINTS; i++) {
    // first the regularizer
    loss[i] = 0.0;

    // then the ratings
    if (rmse) {
      loss[i] += pow(1.0 - C[i], 2);
      for (int j = 1; j < P.num_rows; j++)
        loss[i] += pow(C[j * GRID_POINTS + i], 2);
    } else {
      for (int j = 0; j < P.num_rows; j++)
        loss[i] += pow(1.0 - C[j * GRID_POINTS + i], 2);
    }

    if (loss[i] < min_loss) {
      min_loss = loss[i];
      best_point = i;
    }
  }

  // return the best one
  for (int k = 0; k < max_walk; k++)
    theta[k] = hk_c[k * GRID_POINTS + best_point];

  // free
  free(P_unfold);
  free(C);
}

void dictionary_single(double *theta, d_mat_t P, int max_walk, double *dict, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  // Unfold P for BLAS compatibility
  double *P_unfold = unfold_matrix(P);

  // Prepare matrices
  double *PD = (double *)malloc(P.num_rows * GRID_POINTS * sizeof(double));
  matrix_matrix_product(PD, P_unfold, dict, (int)P.num_rows, (int)P.num_cols,
                        GRID_POINTS);
  double DD[GRID_POINTS * GRID_POINTS];
  grammian_matrix(DD, dict, max_walk, GRID_POINTS);
  double A[GRID_POINTS * GRID_POINTS];
  grammian_matrix(A, PD, (int)P.num_rows, GRID_POINTS);

  double g_D[GRID_POINTS];
  for (int i = 0; i < GRID_POINTS; i++) {
    g_D[i] = 0.0;
  }

  if (rmse) {
    for (int i = 0; i < GRID_POINTS; i++)
      g_D[i] += -2.0 * PD[i];
  } else {
    for (int i = 0; i < GRID_POINTS; i++) {
      for (int j = 0; j < P.num_rows; j++)
        g_D[i] += -2.0 * PD[j * GRID_POINTS + i];
    }
  }

  double theta_dict[GRID_POINTS];

  // call QP solver for dictionary coefficients
  constr_QP_with_PG(theta_dict, A, g_D, (sz_med_t)GRID_POINTS);

  // construct diffusion from dictionary coefficients
  matvec(theta, dict, theta_dict, max_walk, GRID_POINTS);

  // free
  free(PD);
  free(P_unfold);
}

void dictionary(double *theta, d_mat_t P, int max_walk, double *dict, bool rmse) {

  if (!rmse) {
    // adjust for BPR  metric
    for (int i = 1; i < P.num_rows; i++) {
      for (int j = 0; j < P.num_cols; j++)
        P.val[i][j] = P.val[0][j] - P.val[i][j];
    }
  }

  // Unfold P for BLAS compatibility
  double *P_unfold = unfold_matrix(P);

  // Prepare matrices
  double *PD = (double *)malloc(P.num_rows * 2 * GRID_POINTS * sizeof(double));
  matrix_matrix_product(PD, P_unfold, dict, (int)P.num_rows, (int)P.num_cols,
                        2 * GRID_POINTS);
  double DD[4 * GRID_POINTS * GRID_POINTS];
  grammian_matrix(DD, dict, max_walk, 2 * GRID_POINTS);
  double A[4 * GRID_POINTS * GRID_POINTS];
  grammian_matrix(A, PD, (int)P.num_rows, 2 * GRID_POINTS);

  double g_D[2 * GRID_POINTS];
  for (int i = 0; i < 2 * GRID_POINTS; i++) {
    g_D[i] = 0.0;
  }

  if (rmse) {
    for (int i = 0; i < 2 * GRID_POINTS; i++)
      g_D[i] += -2.0 * PD[i];
  } else {
    for (int i = 0; i < 2 * GRID_POINTS; i++) {
      for (int j = 0; j < P.num_rows; j++)
        g_D[i] += -2.0 * PD[j * 2 * GRID_POINTS + i];
    }
  }

  double theta_dict[2 * GRID_POINTS];

  // call QP solver for dictionary coefficients
  constr_QP_with_PG(theta_dict, A, g_D, (sz_med_t)2 * GRID_POINTS);

  // construct diffusion from dictionary coefficients
  matvec(theta, dict, theta_dict, max_walk, 2 * GRID_POINTS);

  // free
  free(PD);
  free(P_unfold);
}

// Returns PPR coefficients for given points on a grid of parameter a
double *ppr_coefficients(int max_walk) {

  int grid_points = GRID_POINTS;

  double *coef = (double *)malloc(max_walk * grid_points * sizeof(double));
  double width = 1.0 / (double)(grid_points + 2);

  double col_sum[grid_points];
  for (int j = 0; j < grid_points; j++)
    col_sum[j] = 0.0;

  for (int i = 0; i < max_walk; i++) {
    for (int j = 0; j < grid_points; j++) {
      // pagerank weights per step
      coef[i * grid_points + j] = (i == 0)
                                      ? (1 - (0.01 + width * ((double)j)))
                                      : (1 - (0.01 + width * ((double)j))) *
                                            pow(0.01 + width * ((double)j), i);
      col_sum[j] += coef[i * grid_points + j];
    }
  }

  // Normalize coefficients so tha they sum to 1
  for (int i = 0; i < max_walk; i++) {
    for (int j = 0; j < grid_points; j++)
      coef[i * grid_points + j] /= col_sum[j];
  }

  return coef;
}

// Returns HK coefficients for given points on a grid of parameter \tau
double *hk_coefficients(int max_walk) {

  int grid_points = GRID_POINTS;

  double *coef = (double *)malloc(max_walk * grid_points * sizeof(double));
  double width = (double)MAX_HK_COEF / (double)(grid_points + 2);

  // compute factorials from 1 to max_walk
  sz_long_t factorial[max_walk];
  factorial[0] = 1;
  for (int j = 1; j < max_walk; j++)
    factorial[j] = factorial[j - 1] * j;

  double col_sum[grid_points];
  for (int j = 0; j < grid_points; j++)
    col_sum[j] = 0.0;

  for (int i = 0; i < max_walk; i++) {
    for (int j = 0; j < grid_points; j++) {
      coef[i * grid_points + j] =
          pow(1.0 + width * ((double)j), i) / (double)factorial[i];
      col_sum[j] += coef[i * grid_points + j];
    }
  }
  // Normalize coefficients so that they sum to 1
  for (int i = 0; i < max_walk; i++) {
    for (int j = 0; j < grid_points; j++)
      coef[i * grid_points + j] /= col_sum[j];
  }

  return coef;
}

// Returns dictionary coefficients by concatenating PPR and HK
double *dict_coefficients(int max_walk) {
  int grid_points = GRID_POINTS;

  double *coef = (double *)malloc(2 * max_walk * grid_points * sizeof(double));

  double *hk = hk_coefficients(max_walk);
  double *ppr = ppr_coefficients(max_walk);

  for (int i = 0; i < max_walk; i++) {
    for (int j = 0; j < grid_points; j++) {
      coef[2 * i * grid_points + j] = ppr[i * grid_points + j];
    }
    for (int j = grid_points; j < 2 * grid_points; j++) {
      coef[2 * i * grid_points + j] = hk[i * grid_points + j - grid_points];
    }
  }

  // free temporary coefs
  free(ppr);
  free(hk);
  return coef;
}

///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////

// AUXILIARY FUNCTIONS

// Interface for CBLAS mutrix matrix product
// C =A*B
// A : m x k
// B : k x n
static void matrix_matrix_product(double *C, double *A, double *B, int m, int k,
                                  int n) {

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

  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1.0f, A, k, B,
              n, 0.0f, C, n);
}

// Unfold matrix from A[i[j] to A[i*M + j]
static double *unfold_matrix(d_mat_t A) {

  double *A_u = (double *)malloc(A.num_cols * A.num_rows * sizeof(double));

  for (sz_long_t i = 0; i < A.num_rows; i++)
    memcpy(A_u + i * A.num_cols, A.val[i], A.num_cols * sizeof(double));

  return A_u;
}

// Solving constrained quadratic minimization via projected gradient descent
// The following function returns x =arg min {x^T*A*x +x^T*B} s.t. x in Prob.
// Simplex
static void constr_QP_with_PG(double *x, double *A, double *b, sz_med_t K) {
  double inf_norm, step_size;
  double x_temp[K];
  double x_prev[K];

  step_size = STEPSIZE_2 / frob_norm(A, K);

  // Initialize to uniform
  for (sz_med_t i = 0; i < K; i++)
    x[i] = 1.0f / (double)K;

  sz_med_t iter = 0;
  memcpy(x_prev, x, K * sizeof(double));
  // int count=0;
  do {
    iter++;
    // step_size = 1 / ((double)iter) * step_size;
    // step_size = 1/(sqrt(iter));//*step_size;
    // step_size = 1/(double)iter;
    // Take gradient step
    matvec(x_temp, A, x, K, K);

    for (sz_med_t j = 0; j < K; j++)
      x[j] -= step_size * (2.0f * x_temp[j] + b[j]);

    // project to feasible set
    simplexproj_Duchi(x, x, K);

#if DEBUG
    printf("\n COST: ");
    printf(" %lf ", cost_func(A, b, x, K));
#endif

    inf_norm = max_abs_dif(x_prev, x, (sz_long_t)K);

    memcpy(x_prev, x, K * sizeof(double));

  } while (iter < MAXIT_GD && inf_norm > GD_TOL_2);

  // printf("\n Counter = %d", count);
  // printf("\n Optimization finished after: %"PRIu32" iterations\n", (uint32_t)
  // iter);
}

// Grammian matrix G =A'*A using CBLAS
// A : m x n
static void grammian_matrix(double *G, double *A, int m, int n) {

  for (int i = 0; i < n * n; i++)
    G[i] = 0.0f;

  double *A_copy = (double *)malloc(m * n * sizeof(double));

  memcpy(A_copy, A, m * n * sizeof(double));

  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans, n, n, m, 1.0f, A_copy, n,
              A, n, 0.0f, G, n);

  free(A_copy);
}

// Interface for CBLAS matrix vector product
// A : M x N
static void matvec(double *y, double *A, double *x, int M, int N) {

  // for (int i = 0; i < M; i++)
  // {
  // 	y[i] = 0.0f;
  // }

  cblas_dgemv(CblasRowMajor, CblasNoTrans, M, N, 1.0f, A, N, x, 1, 0.0f, y, 1);
}

//

/* Algorithm using partitioning with respect to a pivot, chosen randomly,
as given by Duchi et al. in "Efficient Projections onto the l1-Ball for
Learning in High Dimensions" */
static void simplexproj_Duchi(double *y, double *x, const unsigned int length) {
  double *auxlower = (x == y ? (double *)malloc(length * sizeof(double)) : x);
  double *auxupper = (double *)malloc(length * sizeof(double));
  double *aux = auxlower;
  double pivot;
  int auxlowerlength = 0;
  int auxupperlength = 1;
  int upperlength;
  int auxlength;
  int i = 0;
  int pospivot = (int)(rand() / (((double)RAND_MAX + 1.0) / length));
  double tauupper;
  double tau = (pivot = y[pospivot]) - 1.0;
  while (i < pospivot)
    if (y[i] < pivot)
      auxlower[auxlowerlength++] = y[i++];
    else {
      auxupper[auxupperlength++] = y[i];
      tau += (y[i++] - tau) / auxupperlength;
    }
  i++;
  while (i < length)
    if (y[i] < pivot)
      auxlower[auxlowerlength++] = y[i++];
    else {
      auxupper[auxupperlength++] = y[i];
      tau += (y[i++] - tau) / auxupperlength;
    }
  if (tau < pivot) {
    upperlength = auxupperlength;
    tauupper = tau;
    auxlength = auxlowerlength;
  } else {
    tauupper = 0.0;
    upperlength = 0;
    aux = auxupper + 1;
    auxlength = auxupperlength - 1;
  }
  while (auxlength > 0) {
    pospivot = (int)(rand() / (((double)RAND_MAX + 1.0) / auxlength));
    if (upperlength == 0)
      tau = (pivot = aux[pospivot]) - 1.0;
    else
      tau = tauupper + ((pivot = aux[pospivot]) - tauupper) / (upperlength + 1);
    i = 0;
    auxlowerlength = 0;
    auxupperlength = 1;
    while (i < pospivot)
      if (aux[i] < pivot)
        auxlower[auxlowerlength++] = aux[i++];
      else {
        auxupper[auxupperlength++] = aux[i];
        tau += (aux[i++] - tau) / (upperlength + auxupperlength);
      }
    i++;
    while (i < auxlength)
      if (aux[i] < pivot)
        auxlower[auxlowerlength++] = aux[i++];
      else {
        auxupper[auxupperlength++] = aux[i];
        tau += (aux[i++] - tau) / (upperlength + auxupperlength);
      }
    if (tau < pivot) {
      upperlength += auxupperlength;
      tauupper = tau;
      auxlength = auxlowerlength;
      aux = auxlower;
    } else {
      aux = auxupper + 1;
      auxlength = auxupperlength - 1;
    }
  }
  for (i = 0; i < length; i++)
    x[i] = (y[i] > tau ? y[i] - tauupper : 0.0000000001);
  double sum = 0.0;
  for (i = 0; i < length; i++)
    sum += x[i];
  for (i = 0; i < length; i++)
    x[i] /= sum;
  if (x == y)
    free(auxlower);
  free(auxupper);
}

// Infinity norm
static double max_abs_dif(double *a, double *b, sz_long_t len) {
  double dif, max = 0.0;

  for (sz_long_t i = 0; i < len; i++) {
    dif = fabs(a[i] - b[i]);
    max = (dif > max) ? dif : max;
  }

  return max;
}

#if DEBUG
// Evaluates quadratic with Hessian A and linear part b at x
static double cost_func(double *A, double *b, double *x, sz_med_t len) {

  double quad = 0.0f, lin = 0.0f;

  for (sz_med_t i = 0; i < len; i++) {
    for (sz_med_t j = 0; j < len; j++) {
      quad += A[i * len + j] * x[i] * x[j];
    }
    lin += b[i] * x[i];
  }
  return quad + lin;
}
#endif

// frobenious norm of double-valued square matrix
static double frob_norm(double *A, sz_med_t dim) {
  double norm = 0.0f;

  for (sz_med_t i = 0; i < dim * dim; i++) {
    norm += pow(A[i], 2.0f);
  }

  return sqrt(norm);
}


// Copyright (c) 2021 Stig Rune Sellevag
//
// This file is distributed under the MIT License. See the accompanying file
// LICENSE.txt or http://www.opensource.org/licenses/mit-license.php for terms
// and conditions.

#ifndef SCILIB_LINALG_INV_H
#define SCILIB_LINALG_INV_H

#ifdef USE_MKL
#include <mkl.h>
#else
#include <lapacke.h>
#endif

#include <scilib/mdarray.h>
#include <scilib/linalg_impl/lapack_types.h>
#include <experimental/mdspan>
#include <exception>
#include <cassert>
#include <type_traits>

namespace Sci {
namespace Linalg {

// Matrix inversion.
template <class T_a, class T_res, class Layout>
    requires(std::is_same_v<std::remove_cv_t<T_a>, double>)
inline void inv(Sci::Matrix_view<T_a, Layout> a,
                Sci::Matrix_view<T_res, Layout> res)
{
    namespace stdex = std::experimental;

    static_assert(a.is_contiguous());
    static_assert(res.is_contiguous());

    assert(a.extent(0) == a.extent(1));

    if (det(a) == 0.0) {
        throw std::runtime_error("inv: matrix not invertible");
    }
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(0));

    auto matrix_layout = LAPACK_ROW_MAJOR;
    BLAS_INT lda = n;

    if constexpr (std::is_same_v<Layout, stdex::layout_left>) {
        matrix_layout = LAPACK_COL_MAJOR;
    }

    Sci::copy(a, res);

    Sci::Vector<BLAS_INT, Layout> ipiv(n);
    Sci::Linalg::lu(res, ipiv.view()); // perform LU factorization

    BLAS_INT info =
        LAPACKE_dgetri(matrix_layout, n, res.data(), lda, ipiv.data());
    if (info != 0) {
        throw std::runtime_error("dgetri: matrix inversion failed");
    }
}

template <class Layout, class Allocator>
inline Sci::Matrix<double, Layout, Allocator>
inv(const Sci::Matrix<double, Layout, Allocator>& a)
{
    Sci::Matrix<double, Layout, Allocator> res(a.extent(0), a.extent(1));
    inv(a.view(), res.view());
    return res;
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_INV_H


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

/* Author:  G. Jungman */

/* Miscellaneous implementations of use
 * for evaluation of hypergeometric functions.
 */
#include <config.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include "gsl_sf_exp.h"
#include "gsl_sf_gamma.h"

#include "error.h"
#include "hyperg.h"

#define SUM_LARGE  (1.0e-5*GSL_DBL_MAX)

int
gsl_sf_hyperg_1F1_series_e(const double a, const double b, const double x,
                              gsl_sf_result * result
                              )
{
  double an  = a;
  double bn  = b;
  double n   = 1.0;
  double del = 1.0;
  double abs_del = 1.0;
  double max_abs_del = 1.0;
  double sum_val = 1.0;
  double sum_err = 0.0;

  while(abs_del/fabs(sum_val) > GSL_DBL_EPSILON) {
    double u, abs_u;

    if(bn == 0.0) {
      DOMAIN_ERROR(result);
    }
    if(an == 0.0 || n > 1000.0) {
      result->val  = sum_val;
      result->err  = sum_err;
      result->err += 2.0 * GSL_DBL_EPSILON * n * fabs(sum_val);
      return GSL_SUCCESS;
    }

    u = x * (an/(bn*n));
    abs_u = fabs(u);
    if(abs_u > 1.0 && max_abs_del > GSL_DBL_MAX/abs_u) {
      result->val = sum_val;
      result->err = fabs(sum_val);
      GSL_ERROR ("overflow", GSL_EOVRFLW);
    }
    del *= u;
    sum_val += del;
    if(fabs(sum_val) > SUM_LARGE) {
      result->val = sum_val;
      result->err = fabs(sum_val);
      GSL_ERROR ("overflow", GSL_EOVRFLW);
    }

    abs_del = fabs(del);
    max_abs_del = GSL_MAX_DBL(abs_del, max_abs_del);
    sum_err += 2.0*GSL_DBL_EPSILON*abs_del;

    an += 1.0;
    bn += 1.0;
    n  += 1.0;
  }

  result->val  = sum_val;
  result->err  = sum_err;
  result->err += abs_del;
  result->err += 2.0 * GSL_DBL_EPSILON * n * fabs(sum_val);

  return GSL_SUCCESS;
}

int
gsl_sf_hyperg_1F1_large_b_e(const double a, const double b, const double x, gsl_sf_result * result)
{
  if(fabs(x/b) < 1.0) {
    const double u = x/b;
    const double v = 1.0/(1.0-u);
    const double pre = pow(v,a);
    const double uv  = u*v;
    const double uv2 = uv*uv;
    const double t1  = a*(a+1.0)/(2.0*b)*uv2;
    const double t2a = a*(a+1.0)/(24.0*b*b)*uv2;
    const double t2b = 12.0 + 16.0*(a+2.0)*uv + 3.0*(a+2.0)*(a+3.0)*uv2;
    const double t2  = t2a*t2b;
    result->val  = pre * (1.0 - t1 + t2);
    result->err  = pre * GSL_DBL_EPSILON * (1.0 + fabs(t1) + fabs(t2));
    result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
    return GSL_SUCCESS;
  }
  else {
    DOMAIN_ERROR(result);
  }
}

int
gsl_sf_hyperg_U_large_b_e(const double a, const double b, const double x,
                             gsl_sf_result * result,
			     double * ln_multiplier
			     )
{
  double N   = floor(b);  /* b = N + eps */
  double eps = b - N;

  if(fabs(eps) < GSL_SQRT_DBL_EPSILON) {
    double lnpre_val;
    double lnpre_err;
    gsl_sf_result M;
    if(b > 1.0) {
      double tmp = (1.0-b)*log(x);
      gsl_sf_result lg_bm1;
      gsl_sf_result lg_a;
      gsl_sf_lngamma_e(b-1.0, &lg_bm1);
      gsl_sf_lngamma_e(a, &lg_a);
      lnpre_val = tmp + x + lg_bm1.val - lg_a.val;
      lnpre_err = lg_bm1.err + lg_a.err + GSL_DBL_EPSILON * (fabs(x) + fabs(tmp));
      gsl_sf_hyperg_1F1_large_b_e(1.0-a, 2.0-b, -x, &M);
    }
    else {
      gsl_sf_result lg_1mb;
      gsl_sf_result lg_1pamb;
      gsl_sf_lngamma_e(1.0-b, &lg_1mb);
      gsl_sf_lngamma_e(1.0+a-b, &lg_1pamb);
      lnpre_val = lg_1mb.val - lg_1pamb.val;
      lnpre_err = lg_1mb.err + lg_1pamb.err;
      gsl_sf_hyperg_1F1_large_b_e(a, b, x, &M);
    }

    if(lnpre_val > GSL_LOG_DBL_MAX-10.0) {
      result->val  = M.val;
      result->err  = M.err;
      *ln_multiplier = lnpre_val;
      GSL_ERROR ("overflow", GSL_EOVRFLW);
    }
    else {
      gsl_sf_result epre;
      int stat_e = gsl_sf_exp_err_e(lnpre_val, lnpre_err, &epre);
      result->val  = epre.val * M.val;
      result->err  = epre.val * M.err + epre.err * fabs(M.val);
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *ln_multiplier = 0.0;
      return stat_e;
    }
  }
  else {
    double omb_lnx = (1.0-b)*log(x);
    gsl_sf_result lg_1mb;    double sgn_1mb;
    gsl_sf_result lg_1pamb;  double sgn_1pamb;
    gsl_sf_result lg_bm1;    double sgn_bm1;
    gsl_sf_result lg_a;      double sgn_a;
    gsl_sf_result M1, M2;
    double lnpre1_val, lnpre2_val;
    double lnpre1_err, lnpre2_err;
    double sgpre1, sgpre2;
    gsl_sf_hyperg_1F1_large_b_e(    a,     b, x, &M1);
    gsl_sf_hyperg_1F1_large_b_e(1.0-a, 2.0-b, x, &M2);

    gsl_sf_lngamma_sgn_e(1.0-b,   &lg_1mb,   &sgn_1mb);
    gsl_sf_lngamma_sgn_e(1.0+a-b, &lg_1pamb, &sgn_1pamb);

    gsl_sf_lngamma_sgn_e(b-1.0, &lg_bm1, &sgn_bm1);
    gsl_sf_lngamma_sgn_e(a,     &lg_a,   &sgn_a);

    lnpre1_val = lg_1mb.val - lg_1pamb.val;
    lnpre1_err = lg_1mb.err + lg_1pamb.err;
    lnpre2_val = lg_bm1.val - lg_a.val - omb_lnx - x;
    lnpre2_err = lg_bm1.err + lg_a.err + GSL_DBL_EPSILON * (fabs(omb_lnx)+fabs(x));
    sgpre1 = sgn_1mb * sgn_1pamb;
    sgpre2 = sgn_bm1 * sgn_a;

    if(lnpre1_val > GSL_LOG_DBL_MAX-10.0 || lnpre2_val > GSL_LOG_DBL_MAX-10.0) {
      double max_lnpre_val = GSL_MAX(lnpre1_val,lnpre2_val);
      double max_lnpre_err = GSL_MAX(lnpre1_err,lnpre2_err);
      double lp1 = lnpre1_val - max_lnpre_val;
      double lp2 = lnpre2_val - max_lnpre_val;
      double t1  = sgpre1*exp(lp1);
      double t2  = sgpre2*exp(lp2);
      result->val  = t1*M1.val + t2*M2.val;
      result->err  = fabs(t1)*M1.err + fabs(t2)*M2.err;
      result->err += GSL_DBL_EPSILON * exp(max_lnpre_err) * (fabs(t1*M1.val) + fabs(t2*M2.val));
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *ln_multiplier = max_lnpre_val;
      GSL_ERROR ("overflow", GSL_EOVRFLW);
    }
    else {
      double t1 = sgpre1*exp(lnpre1_val);
      double t2 = sgpre2*exp(lnpre2_val);
      result->val  = t1*M1.val + t2*M2.val;
      result->err  = fabs(t1) * M1.err + fabs(t2)*M2.err;
      result->err += GSL_DBL_EPSILON * (exp(lnpre1_err)*fabs(t1*M1.val) + exp(lnpre2_err)*fabs(t2*M2.val));
      result->err += 2.0 * GSL_DBL_EPSILON * fabs(result->val);
      *ln_multiplier = 0.0;
      return GSL_SUCCESS;
    }
  }
}

/* [Carlson, p.109] says the error in truncating this asymptotic series
 * is less than the absolute value of the first neglected term.
 *
 * A termination argument is provided, so that the series will
 * be summed at most up to n=n_trunc. If n_trunc is set negative,
 * then the series is summed until it appears to start diverging.
 */
int
gsl_sf_hyperg_2F0_series_e(const double a, const double b, const double x,
                              int n_trunc,
                              gsl_sf_result * result
                              )
{
  const int maxiter = 2000;
  double an = a;
  double bn = b;
  double n   = 1.0;
  double sum = 1.0;
  double del = 1.0;
  double abs_del = 1.0;
  double max_abs_del = 1.0;
  double last_abs_del = 1.0;

  while(abs_del/fabs(sum) > GSL_DBL_EPSILON && n < maxiter) {

    double u = an * (bn/n * x);
    double abs_u = fabs(u);

    if(abs_u > 1.0 && (max_abs_del > GSL_DBL_MAX/abs_u)) {
      result->val = sum;
      result->err = fabs(sum);
      GSL_ERROR ("overflow", GSL_EOVRFLW);
    }

    del *= u;
    sum += del;

    abs_del = fabs(del);

    if(abs_del > last_abs_del) break; /* series is probably starting to grow */

    last_abs_del = abs_del;
    max_abs_del  = GSL_MAX(abs_del, max_abs_del);

    an += 1.0;
    bn += 1.0;
    n  += 1.0;

    if(an == 0.0 || bn == 0.0) break;        /* series terminated */

    if(n_trunc >= 0 && n >= n_trunc) break;  /* reached requested timeout */
  }

  result->val = sum;
  result->err = GSL_DBL_EPSILON * n + abs_del;
  if(n >= maxiter)
    GSL_ERROR ("error", GSL_EMAXITER);
  else
    return GSL_SUCCESS;
}


#include <string.h>
#include <cblas.h>

#include "util.h"
#include "dense.h"

denseLayer denseLayer_init(int M, int N)
{
     denseLayer dl; DENSEL_INIT(dl); dl.M=M; dl.N=N;
     return dl;
}

void denseLayer_free(denseLayer *dl)
{
     ftens_free(&dl->W);   ftens_free(&dl->b);
     ftens_free(&dl->dW);  ftens_free(&dl->db);
     ftens_free(&dl->out); ftens_free(&dl->in);
}

void denseLayer_print_shape(denseLayer *dl)
{
     printf("dense: %d %d\n", dl->M, dl->N);
}

void denseLayer_set(ftens *W, denseLayer *dl)
{
     const int M=W->M, N=W->N;
     ASSERT(W->D==1 && W->L==1, "err: dense shape\n");
     ftens_free(&dl->W);
     dl->M = M; dl->N = N;
     dl->W = ftens_copy(W);
}

void denseLayer_forward(ftens *t, denseLayer *dl, int save)
{
     const int D=t->D, M=dl->M, N=dl->N;
     ASSERT(t->MNL == dl->N,  "err: dense shape\n");

     if (save) {
          int M=t->M, N=t->N, L=t->L;
          if (!dl->in.data) dl->in = ftens_init(D,M,N,L);
          memcpy(dl->in.data, t->data, t->bytes);
     }

     if (!dl->out.data) dl->out = ftens_init(D, 1, M, 1);
     const float *a=dl->W.data;
     const float *b=t->data;
     float       *c=dl->out.data;

     cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
                 D, M, N, 1, b, N, a, N, 0, c, M);

}

void denseLayer_backward(ftens *dout, denseLayer *dl)
{
     fprintf(stderr, "not implemented yet\n");
     exit(-2);
}


#define NPY_NO_DEPRECATED_API NPY_1_7_API_VERSION

#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <Python.h>
#include <numpy/arrayobject.h>

void mark_debug (void)
{
    static int number;
    printf("---> Mark %d <---\n", number);
    fflush(stdout);
    number++;
}

void label_debug (const char * label)
{
    printf("---> Mark %s <---\n", label);
    fflush(stdout);
}

/* =========================== Pure-C source code =========================== */

/* Regularization workspace */
struct rglz_wksp {
    double lamb; /* Lambda value */
    gsl_vector *Ldiag; /* Pointer to vector with diagonal values of the
			      * regularization matrix L. */
    const gsl_vector *Y; /* Pointer to original Y */
    gsl_vector *Ys; /* Ponter to Y vector of the standard form. */
    const gsl_matrix *X; /* Pointer to original X matrix  */
    gsl_matrix *Xs; /* Pointer to X matrix of the standard form. */
    gsl_multifit_linear_workspace *lwksp; /* Pointer to GSL workspace where SVDs are stored. */
    /* L-curve data */
    struct lcurve {
	gsl_vector *reg_param;
	gsl_vector *rho;
	gsl_vector *eta;
    } lcurve;
};
typedef struct rglz_wksp rglz_wksp;

/* Allocate and initialize inner variables of a regularization workspace. */
rglz_wksp * rglz_init(const double *Ldiag, const int p,
		      const gsl_vector *Y, const gsl_matrix *X, const gsl_vector *W,
		      gsl_multifit_linear_workspace *lwksp)
{
    rglz_wksp *rw;

    /* Allocate space for the workspace */
    rw = (rglz_wksp*) malloc(sizeof(rglz_wksp));

    rw->lamb = 0.0; /* Initial value */
    rw->lcurve.reg_param = NULL;
    rw->lcurve.rho = NULL;
    rw->lcurve.eta = NULL;

    /* Allocate space for the standard-form matrixes */
    rw->Xs = gsl_matrix_alloc(X->size1, X->size2);
    rw->Ys = gsl_vector_alloc(Y->size);

    /* Initialize the regularization weights Ldiag */
    rw->Ldiag = gsl_vector_alloc(p);
    for (int i = 0; i < p; i++)
        gsl_vector_set(rw->Ldiag, i, Ldiag[i]);

    /* Convert the system to standard form */
    gsl_multifit_linear_wstdform1(rw->Ldiag, X, W, Y, rw->Xs, rw->Ys, lwksp);

    /* Calculate SVD of standard-form matrix Xs */
    gsl_multifit_linear_svd(rw->Xs, lwksp);

    rw->lwksp = lwksp;
    rw->X = X;
    rw->Y = Y;
    return rw;
}

void rglz_estimate_lambda(rglz_wksp *rw, const int lcsize)
{
    size_t idx; /* Temporary index to store corner of the L-curve */
    /* Allocate vectors */
    rw->lcurve.reg_param = gsl_vector_alloc(lcsize);
    rw->lcurve.rho = gsl_vector_alloc(lcsize);
    rw->lcurve.eta = gsl_vector_alloc(lcsize);
    /* Create Lcurve */
    gsl_multifit_linear_lcurve(rw->Y, rw->lcurve.reg_param, rw->lcurve.rho, rw->lcurve.eta, rw->lwksp);
#ifdef PRINT_LCURVE
    for (int i = 0; i < lcsize; i++)
    {
        printf("%12.4e%12.4e\n",
                gsl_vector_get(rw->lcurve.rho, i),
                gsl_vector_get(rw->lcurve.eta, i));
    }
#endif
    /* Estimate corner */
    gsl_multifit_linear_lcorner(rw->lcurve.rho, rw->lcurve.eta, &idx);
    /* Set lambda estimate */
    rw->lamb = gsl_vector_get(rw->lcurve.reg_param, idx);
    fprintf(stdout, "From gslpy_ridge: lambda = %.4e\n", rw->lamb);
}

void rglz_solve(gsl_vector *c, double *rnorm, double *snorm, rglz_wksp *rw)
{
    /* Solve in standard form */
    gsl_multifit_linear_solve(rw->lamb, rw->Xs, rw->Ys, c, rnorm, snorm, rw->lwksp);
    /* Convert back to original parameters */
    gsl_multifit_linear_genform1(rw->Ldiag, c, c, rw->lwksp);
    return;
}

inline void rglz_set_lambda(rglz_wksp *rw, const int lamb)
{
    rw->lamb = lamb;
}

void rglz_close(rglz_wksp *rw)
{
    gsl_vector_free(rw->Ldiag);
    gsl_matrix_free(rw->Xs);
    gsl_vector_free(rw->Ys);
    if (rw->lcurve.reg_param != NULL)
	gsl_vector_free(rw->lcurve.reg_param);
    if (rw->lcurve.rho != NULL)
	gsl_vector_free(rw->lcurve.rho);
    if (rw->lcurve.eta != NULL)
	gsl_vector_free(rw->lcurve.eta);
    free(rw);
}

int
gslpy_ridge_regression(const gsl_vector * y,
		       const gsl_matrix * X,
		       const gsl_vector * w,
		       const double lambda,
		       const double * L,
		       const unsigned long Lcurve_size,
		       gsl_vector * coefs)
{

    unsigned long n_samples = X->size1;
    unsigned long n_features = X->size2;

    double snorm;
    double chisq;

    gsl_multifit_linear_workspace
	* gmlw = gsl_multifit_linear_alloc(n_samples, n_features);

    rglz_wksp * rw = rglz_init(L, n_features, y, X, w, gmlw);

    /* If the user specified gamma, do not attempt to optimize it. */
    if (lambda >= 0)
	rglz_set_lambda(rw, lambda);
    else
	rglz_estimate_lambda(rw, Lcurve_size);

    rglz_solve(coefs, &chisq, &snorm, rw);

    rglz_close(rw);

    gsl_multifit_linear_free(gmlw);
    return 0;
}

/* ============================= Python wrapper ============================= */

static PyObject*
gslpy_ridge_fit(PyObject* self, PyObject* args, PyObject* kwargs)
{
    static char * keywords[] = {
	"X",
	"y",
	"sample_weight",
	"lamb",
	"ldiag",
	"lcurve_size",
	NULL,
    };

    PyArrayObject
	*X=NULL,
	*y=NULL,
	*w=NULL,
	*Ldiag=NULL,
	*coefs=NULL;

    /* Negative lambda means that lambda is optimized */
    double lambda = -1;
    unsigned long Lcurve_size = 10000;

    /* Flag to check if things are set */
    int wei_set = 0;
    int L_set = 0;

    /* GSL stuff */
    gsl_vector * g_y;
    gsl_matrix * g_X;
    gsl_vector * g_w;
    gsl_vector * g_coefs;

    if (!PyArg_ParseTupleAndKeywords(args,
                                     kwargs,
                                     "O!O!|O!dO!k",
                                     keywords,
                                     &PyArray_Type, &X,
				     &PyArray_Type, &y,
				     &PyArray_Type, &w,
				     &lambda,
				     &PyArray_Type, &Ldiag,
				     &Lcurve_size)) {
        return NULL;
    }

    /* If weights are not given, set them as 1 */
    if (w == NULL)
    {
	wei_set = 1;
	w = (PyArrayObject*) PyArray_ZEROS(PyArray_NDIM(y),
					   PyArray_DIMS(y),
					   NPY_DOUBLE,
					   0);

	npy_double * w_data = PyArray_DATA(w);

	for (npy_intp i = 0; i < PyArray_SIZE(w); ++i)
	    w_data[i] = 1.0;
    }

    /* If Ldiag is not given, set it to identity. */
    if (Ldiag == NULL)
    {
	npy_intp Lshape[] = {PyArray_DIM(X,1)};
	L_set = 1;
	Ldiag = (PyArrayObject*) PyArray_ZEROS(1,
					       Lshape,
					       NPY_DOUBLE,
					       0);

	for (npy_intp i = 0; i < PyArray_DIM(X, 1); i++)
	    ((double *) PyArray_DATA(Ldiag))[i] = 1.0;
    }

    /* Convert PyArray objects to gsl types */
    g_y = gsl_vector_calloc(PyArray_DIM(y, 0));
    g_w = gsl_vector_calloc(PyArray_DIM(w, 0));
    g_X = gsl_matrix_calloc(PyArray_DIMS(X)[0], PyArray_DIMS(X)[1]);
    g_coefs = gsl_vector_calloc(PyArray_DIM(X, 1));

    for (npy_intp i = 0; i < (npy_intp) g_y->size; i++)
    {
	gsl_vector_set(g_y, i, ((double *) PyArray_DATA(y))[i]);
	gsl_vector_set(g_w, i, ((double *) PyArray_DATA(w))[i]);
    }

    for (npy_intp i = 0; i < PyArray_SIZE(X); i++)
	g_X->data[i] = ((double *) PyArray_DATA(X))[i];

    /* Run regression */
    gslpy_ridge_regression(g_y, g_X,
			   g_w, lambda,
			   (double *) PyArray_DATA(Ldiag),
			   Lcurve_size, g_coefs);

    /* Convert g_coefs back to a Python object */
    /* Local scope just to delete ``coefs_shape``after. */
    {
	npy_intp coefs_shape[] = {g_coefs->size, 1};
	coefs = (PyArrayObject *) PyArray_ZEROS(2,
						coefs_shape,
						NPY_DOUBLE,
						0);
	for (npy_intp i = 0; i < (npy_intp) g_coefs->size; i++)
	    ((double *) PyArray_DATA(coefs))[i] = gsl_vector_get(g_coefs, i);

    }

    if (wei_set)
	Py_XDECREF(w);

    if (L_set)
	Py_XDECREF(Ldiag);

    gsl_matrix_free(g_X);
    gsl_vector_free(g_y);
    gsl_vector_free(g_w);
    gsl_vector_free(g_coefs);

    return (PyObject *) coefs;
}

/* =========================== Python boilerplate =========================== */

PyDoc_STRVAR(module_doc,
	     "Ridge regression model methods using the GNU Scientific Library.");

PyDoc_STRVAR(fit_doc,
	     "Fits a ridge regression model to data.\n"
	     "fit(X, y, sample_weights=None, lambda=None, ldiag=None, Lcurve_size=10000).\n");

PyMethodDef methods[] = {
    {
	"fit",
	(PyCFunction) gslpy_ridge_fit,
	METH_VARARGS | METH_KEYWORDS,
	fit_doc,
    },
    {NULL},
};

PyModuleDef gslpyridge_module = {
    PyModuleDef_HEAD_INIT,
    "gslpyridge",
    module_doc,
    -1,
    methods,
    NULL,
    NULL,
    NULL,
    NULL
};

PyMODINIT_FUNC
PyInit_gslpyridge(void)
{
    import_array();
    return PyModule_Create(&gslpyridge_module);
}


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

#pragma once

#include <opencv2\core.hpp>
#include <gsl\gsl>

namespace mage
{
    namespace clouds
    {
        void ComputeCharacteristics(
            gsl::span<const cv::Point3f> points,
            std::vector<cv::Vec3f>& normals,
            gsl::span<const size_t> mapIds,
            std::vector<float>& effectiveDissimilarities,
            std::vector<float>& distanceScores,
            std::vector<float>& homogeneityScores,
            const size_t maxN);

        void MollifyNormals(
            gsl::span<const cv::Point3f> points,
            gsl::span<cv::Vec3f> normals,
            const float normalScale,
            const float spatialScale,
            const size_t iterations,
            const size_t maxN);

        void RepositionPointSets(
            gsl::span<cv::Point3f> toReposition,
            gsl::span<cv::Vec3f> normals,
            const float stepi,
            const size_t iterations,
            const float lambda,
            const float solverSigmaS,
            const float mollSigmaS,
            const float mollSigmaN,
            const size_t maxN);

        struct Knn final
        {
            Knn(gsl::span<const cv::Point3f> points);
            Knn(gsl::span<const cv::Vec3f> points);
            ~Knn();

            Knn& operator=(Knn&& other);

            void Query(const cv::Vec3f& point, const size_t N, std::vector<size_t>& indexes) const;

        private:
            struct Impl;
            std::unique_ptr<Impl> m_impl;
        };
    }
}