//#include <gsl/gsl_sf_gamma.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_roots.h>

#define NSPLINELD	1001
#define NSPLINEEZ	1001
#define NSPLINERR	10001
#define NSPLINEDF	10001
gsl_interp_accel *accLD, *accRed, *accEz, *accDF;
gsl_spline *splineLD, *splineRed, *splineEz, *splineDF;

#define MPC_IN_M		3.08567758e22
#define GPC_IN_M		(1e3 * MPC_IN_M)
#define C_LIGHT			2.99792458e8
#define HUBBLE0			(1e5 * 0.71 / MPC_IN_M)
#define DH				(C_LIGHT / HUBBLE0 / GPC_IN_M)
#define DH3				(DH * DH * DH)

void load_splines()
{
	long int i;

	// Luminosity distribution

	accLD = gsl_interp_accel_alloc();
	splineLD = gsl_spline_alloc(gsl_interp_cspline, NSPLINELD);

	double z1[NSPLINELD], ld[NSPLINELD];
	FILE *fp1 = fopen("support_data/splines_LuminosityDistance.txt","r");
	for ( i=0; i<NSPLINELD; i++ )
	{
		fscanf(fp1, "%lf\t%lf\n", &z1[i], &ld[i]);
	}
	fclose(fp1);

	gsl_spline_init(splineLD, z1, ld, NSPLINELD);

	// Redshift distribution

	accRed = gsl_interp_accel_alloc();
	splineRed = gsl_spline_alloc(gsl_interp_cspline, NSPLINERR);

	double z2[NSPLINERR], red[NSPLINERR];
	FILE *fp2 = fopen("support_data/splines_RedshiftRate.txt","r");
	for ( i=0; i<NSPLINERR; i++ )
	{
		fscanf(fp2, "%lf\t%lf\n", &z2[i], &red[i]);
	}
	fclose(fp2);

	gsl_spline_init(splineRed, z2, red, NSPLINERR);

	// E(z) cosmology

	accEz = gsl_interp_accel_alloc();
	splineEz = gsl_spline_alloc(gsl_interp_cspline, NSPLINEEZ);

	double z3[NSPLINEEZ], Ez[NSPLINEEZ], temp1, temp2;
	FILE *fp3 = fopen("support_data/splines_Ez.txt","r");
	for ( i=0; i<NSPLINEEZ; i++ )
	{
		fscanf(fp3, "%lf %lf %lf %lf\n", &z3[i], &temp1, &temp2, &Ez[i]);
	}
	fclose(fp3);

	gsl_spline_init(splineEz, z3, Ez, NSPLINEEZ);

	// detection fraction (w.r.t z)

	accDF = gsl_interp_accel_alloc();
	splineDF = gsl_spline_alloc(gsl_interp_cspline, NSPLINEDF);

	double z4[NSPLINEDF], df[NSPLINEDF], temp3;
	FILE *fp4;
	if (runargs.method == NEURALNET)
	{
		fp4 = fopen("support_data/splines_detection_fraction_z.txt","r");
		for ( i=0; i<NSPLINEDF; i++ )
		{
			fscanf(fp4, "%lf %lf %lf\n", &z4[i], &df[i], &temp3);
		}
		fclose(fp4);
	}
	else if (runargs.method == RANDOMFOREST)
	{
		fp4 = fopen("support_data/splines_detection_fraction_z_RF.txt","r");
		for ( i=0; i<NSPLINEDF; i++ )
		{
			fscanf(fp4, "%lf %lf\n", &z4[i], &df[i]);
		}
		fclose(fp4);
	}
	else if (runargs.method == ADABOOST)
	{
		fp4 = fopen("support_data/splines_detection_fraction_z_AB.txt","r");
		for ( i=0; i<NSPLINEDF; i++ )
		{
			fscanf(fp4, "%lf %lf\n", &z4[i], &df[i]);
		}
		fclose(fp4);
	}
	else if (runargs.method == FLUXTHRESH)
	{
		fp4 = fopen("support_data/splines_detection_fraction_z_flux.txt","r");
		for ( i=0; i<NSPLINEDF; i++ )
		{
			fscanf(fp4, "%lf %lf\n", &z4[i], &df[i]);
		}
		fclose(fp4);
	}

	gsl_spline_init(splineDF, z4, df, NSPLINEDF);
}

void unload_splines()
{
	gsl_spline_free(splineLD);
	gsl_interp_accel_free(accLD);

	gsl_spline_free(splineRed);
	gsl_interp_accel_free(accRed);

	gsl_spline_free(splineEz);
	gsl_interp_accel_free(accEz);

	gsl_spline_free(splineDF);
	gsl_interp_accel_free(accDF);
}

static inline double MAX(double v1, double v2)
{
	return (((v1) > (v2)) ? (v1) : (v2));
}

double log_sum_exp(double x, double y)
{
	//convert to natural log
	x /= M_LOG10E;
	y /= M_LOG10E;

	double mx = MAX(x,y);
	double val = log(exp(x - mx) + exp(y - mx)) + mx;

	return val/M_LN10;
}

double log_subtract_exp(double x, double y)
{
	// x >= y must be true!
	if (y>x)
	{
		fprintf(stderr, "%lf < %lf !\n", x, y);
		exit(-1);
	}
	//convert to natural log
	x /= M_LOG10E;
	y /= M_LOG10E;
	double val = x + log1p(-exp(y-x));
	return val/M_LN10;
}

double lumdist_logpowint(double log_lum_min, double log_lum_max, double pwr)
{
	double val;
	if ( pwr > -1.0 )
	{
		val = -pwr * log_lum_star_global - log10(pwr+1.0) + log_subtract_exp((pwr+1.0)*log_lum_max, (pwr+1.0)*log_lum_min);
	}
	else
	{
		val = -pwr * log_lum_star_global - log10(-(pwr+1.0)) + log_subtract_exp((pwr+1.0)*log_lum_min, (pwr+1.0)*log_lum_max);
	}
	//fprintf(stderr, "%lf -> %lf : %lf\n", log_lum_min, log_lum_max, val);
	return val;
}

double lumdist_logpowinv(double prob, double log_lum_min, double log_lum_max, double pwr)
{
	double temp = lumdist_logpowint(log_lum_min, log_lum_max, pwr) + log10(prob);
	temp += pwr * log_lum_star_global;

	if ( pwr > -1.0 )
	{
		temp += log10(pwr+1.0);
		temp = log_sum_exp((pwr+1.0)*log_lum_min, temp);
	}
	else
	{
		temp += log10(-(pwr+1.0));
		temp = log_subtract_exp((pwr+1.0)*log_lum_min, temp);
	}

	return temp/(pwr+1.0);
}

double luminosity_distr_fast(double prob, double log_lum_min, double log_lum_max)
{
	//fprintf(stderr, "%lf, %lf, %lf, %lf, %lf, %lf\n", prob, log_lum_min, log_lum_max, log_lum_star_global, alpha1_global, beta1_global);

	double log_lum_int_lower = lumdist_logpowint(log_lum_min, log_lum_star_global, alpha1_global);
	double log_lum_int_upper = lumdist_logpowint(log_lum_star_global, log_lum_max, beta1_global);
	double log_lum_int_total = log_sum_exp(log_lum_int_lower, log_lum_int_upper);
	double lower_frac = pow(10.0, log_lum_int_lower - log_lum_int_total), upper_frac = 1.0 - lower_frac;

	//fprintf(stderr, "%lf, %lf, %lf, %lf, %lf\n", log_lum_int_lower, log_lum_int_upper, log_lum_int_total, lower_frac, upper_frac);

	if ( prob <= lower_frac )
	{
		return lumdist_logpowinv(prob/lower_frac, log_lum_min, log_lum_star_global, alpha1_global);
	}
	else
	{
		return lumdist_logpowinv((prob - lower_frac)/upper_frac, log_lum_star_global, log_lum_max, beta1_global);
	}
}

double lum2flux_integrand(double E, void *params)
{
	double *vals = (double *)params;
	double alpha = vals[0];
	double beta = vals[1];
	double E0 = vals[2];
	double Ebreak = vals[3];

	if ( E < Ebreak )
	{
		return pow(E/100.0,alpha) * exp(-E/E0) * E;
	}
	else
	{
		return pow((alpha-beta)*E0/100.0,alpha-beta) * exp(beta-alpha) * pow(E/100.0,beta) * E;
	}
}

double lum2flux_integral_numeric(double alpha, double beta, double Epeak, double Emin, double Emax)
{
	double E0 = Epeak / (2.0+alpha);
	double Ebreak = (alpha-beta)*E0;

	//gsl_integration_workspace * w = gsl_integration_workspace_alloc (1000);

	double result, error;
	unsigned long int neval;
	double vals[4] = {alpha, beta, E0, Ebreak};
	gsl_function F;
  	F.function = &lum2flux_integrand;
  	F.params = &vals[0];

  	gsl_set_error_handler_off();
  	//gsl_integration_qag(&F, Emin, Emax, 1e-7, 1e-6, 1000, 1, w, &result, &error);
  	gsl_integration_qng(&F, Emin, Emax, 1e-7, 1e-6, &result, &error, &neval);
  	//fprintf(stderr, "%d evaluations\t", neval);

  	//gsl_integration_workspace_free (w);

  	return result;
}

double Redshift_distribution_unnormalized(double z, double n1, double n2, double z1)
{
	if (z <= z1)
	{
		return pow(1.0 + z, n1);
	} else {
		return pow(1.0 + z1, n1 - n2) * pow(1.0 + z, n2);
	}
}

double Redshift_distribution_normalized(double z, double n0, double n1, double n2, double z1)
{
	return n0 * Redshift_distribution_unnormalized(z, n1, n2, z1);
}

double Redshift_rescaled(double z, void *params)
{
	double *pars = (double *) params;
	double n0 = pars[0];
	double n1 = pars[1];
	double n2 = pars[2];
	double z1 = pars[3];

	double Rprime = Redshift_distribution_normalized(z, n0, n1, n2, z1) / (1.0 + z);

	Rprime *= DH3 * gsl_spline_eval(splineEz, z, accEz);

	return Rprime;
}

double Redshift_rejection_sampler(long int *seed, double n0, double n1, double n2, double z1)
{
	double pars[4] = {n0, n1, n2, z1};

	double Rzmax = Redshift_rescaled(z1, (void *) &pars[0]);

	double z = 0.0, p, x;
	for (;;)
	{
		z = ran2d(seed) * 10.0;

		p = Redshift_rescaled(z, (void *) &pars[0]) / Rzmax;

		x = ran2d(seed);

		if (x < p) break;
	}

	return z;
}

double GRBNumberIntegral(double n0, double n1, double n2, double z1)
{
	double pars[4] = {n0, n1, n2, z1};

	double result, error;
	unsigned long int neval;
	gsl_function F;
  	F.function = &Redshift_rescaled;
  	F.params = (void *) &pars[0];

  	gsl_set_error_handler_off();
  	gsl_integration_qng(&F, ZMIN, ZMAX, 1e-7, 1e-6, &result, &error, &neval);

  	result *= 4.0 * M_PI;

  	return result;
}

double GRBRate(double z, double n0, double n1, double n2, double z1)
{
	double Rprime = Redshift_distribution_normalized(z, n0, n1, n2, z1) / (1.0 + z);

	Rprime *= DH3 * gsl_spline_eval(splineEz, z, accEz);

	Rprime *= 4.0 * M_PI * runargs.tobs * gsl_spline_eval(splineDF, z, accDF) / 6.0;

	return Rprime;
}

double GRBRateFunc(double z, void *params)
{
	double *pars = (double *) params;
	double n0 = pars[0];
	double n1 = pars[1];
	double n2 = pars[2];
	double z1 = pars[3];

	return GRBRate(z, n0, n1, n2, z1);
}

double GRBRateIntegral(double n0, double n1, double n2, double z1)
{
	double pars[4] = {n0, n1, n2, z1};

	double result, error;
	unsigned long int neval;
	gsl_function F;
  	F.function = &GRBRateFunc;
  	F.params = (void *) &pars[0];

  	gsl_set_error_handler_off();
  	gsl_integration_qng(&F, ZMIN, ZMAX, 1e-7, 1e-6, &result, &error, &neval);

  	return result;
}


/* rng/ran0.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 <gsl/gsl_errno.h>
#include <gsl/gsl_rng.h>

/* This is an implementation of the algorithm used in Numerical
   Recipe's ran0 generator. It is the same as MINSTD with an XOR mask
   of 123459876 on the seed.

   The period of this generator is 2^31.

   Note, if you choose a seed of 123459876 it would give a degenerate
   series 0,0,0,0, ...  I've made that into an error. */

static inline unsigned long int ran0_get (void *vstate);
static double ran0_get_double (void *vstate);
static void ran0_set (void *state, unsigned long int s);

static const long int m = 2147483647, a = 16807, q = 127773, r = 2836;
static const unsigned long int mask = 123459876;

typedef struct
  {
    unsigned long int x;
  }
ran0_state_t;

static inline unsigned long int
ran0_get (void *vstate)
{
  ran0_state_t *state = (ran0_state_t *) vstate;

  const unsigned long int x = state->x;

  const long int h = x / q;
  const long int t = a * (x - h * q) - h * r;

  if (t < 0)
    {
      state->x = t + m;
    }
  else
    {
      state->x = t;
    }

  return state->x;
}

static double
ran0_get_double (void *vstate)
{
  return ran0_get (vstate) / 2147483647.0 ;
}

static void
ran0_set (void *vstate, unsigned long int s)
{
  ran0_state_t *state = (ran0_state_t *) vstate;

  if (s == mask)
    {
      GSL_ERROR_VOID ("ran0 should not use seed == mask",
                                GSL_EINVAL);
    }

  state->x = s ^ mask;

  return;
}

static const gsl_rng_type ran0_type =
{"ran0",                        /* name */
 2147483646,                    /* RAND_MAX */
 1,                             /* RAND_MIN */
 sizeof (ran0_state_t),
 &ran0_set,
 &ran0_get,
 &ran0_get_double};

const gsl_rng_type *gsl_rng_ran0 = &ran0_type;


/*

 */

#include "kern.h"

#include <cblas.h>
#include <err.h>
#include <fcntl.h>
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <time.h>
#include <unistd.h>

#ifdef WITH_THREADS
#include <omp.h>
#define MP_LOOP() PRAGMA(omp for simd)
#else
#define MP_LOOP()
#endif

#define PRAGMA(X) _Pragma(#X)

/*
 * Transform the input vector to the output stream.
 * The m x n matrix is stored with column arrays.
 *
 * y[k * n + j] = x[k * m + i] * w[m * j + i]
 * y(l, n) = x(l,m) * w(m, n)
 *
 */
void trans(uint32_t batch_len, uint32_t m, uint32_t n, const float *w,
           const float *x, float *y) {
    cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, batch_len, n, m,
                1.0f, x, batch_len, w, m, 0.0f, y, batch_len);
}

/*
 * Original:
 * w(m,n) -= N * x(l,m)^T * dy(l,n)
 * w[m * $j + i] -= N * x[m * $k + i] * dy[n * k + $j]
 *
 * Optimized:
 * w(m,n)^T -= N * dy(l,n)^T * x(l,m)
 * w[m * j + $i] -= N * dy[n * $k + j] * x[m * k + $i]
 */
void train_sgd(uint32_t batch_len, uint32_t m, uint32_t n,
               const float x[m * batch_len], const float y[n * batch_len],
               float rate, float w[m * n]) {
    cblas_sgemm(CblasRowMajor, CblasTrans, CblasNoTrans, n, m, batch_len, -rate,
                y, n, x, m, 1.0f, w, m);
}

/*
 * adam optimizer for the weight matrix
 */
float train_adam(uint32_t batch_len, uint32_t m, uint32_t n,
                 const float x[restrict m * batch_len],
                 const float dy[restrict const n * batch_len], float counter,
                 float N, float beta1, float beta2, float epsilon,
                 float w[m * n], float mom[n * m], float veloc[n * m]) {
    float *restrict wr;
    float *restrict mr;
    float *restrict vr;
    const float *restrict xr;
    float dr;
    uint32_t k, i;
    for (uint32_t j = 0; j < n; j++) {
        for (k = 0, wr = &w[m * j], vr = &veloc[j * m], mr = &mom[j * m];
             k < batch_len; k++) {
            for (i = 0, xr = &x[k * m], dr = dy[k * n + j]; i < m; i++) {
                const float g      = dr * xr[i];
                mr[i]              = beta1 * mr[i] + ((1 - beta1) * g);
                const float mr_hat = mr[i] / (1 - powf(beta1, counter));
                vr[i] = beta2 * vr[i] + ((1 - beta2) * (powf(g, 2)));
                const float vr_hat = vr[i] / (1 - powf(beta2, counter));
                wr[i] -= N * mr_hat / (sqrtf(vr_hat + epsilon));
            }
        }
    }
    return counter + 1.0f;
}

/*
 * Original:
 * dx(l,m) = dy(l,n) * w(m,n)^T
 * dx[m * k + $i] = dy[n * k + $j] * w[m * $j + i]
 */
void loss(uint32_t batch_len, uint32_t m, uint32_t n, const float *w,
          const float *dy, float *dx) {
    cblas_sgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, batch_len, m, n, 1.0f,
                dy, n, w, m, .0f, dx, m);
}

/*
 * Calculate the difference between two vectors of size 'size'.
 *
 * size:   The size of the vectors.
 * vec1:   The first vector
 * vec2:   The second vector
 *
 * Return the mean difference
 */
double vec_delta(uint32_t size, const float *vec1, const float *vec2,
                 float *deltas) {
    double error = 0.0;
    for (size_t i = 0; i < size; i++) {
        deltas[i] = vec1[i] - vec2[i];
        error += pow(deltas[i], 2.0);
    }
    return error / (float)size;
}

bool vec_is_equal_f32(uint32_t n, const float a[n], const float b[n],
                      float epsilon) {
    for (uint32_t i = 0; i < n; i++) {
        if (fabsf(a[i] - b[i]) >= epsilon) return false;
        // better:
        // if(fabs(a[ii]-b[ii]) < 1e-10 * (fabs(a[ii]) + fabs(b[ii]))) {
        // with the appropriate tolerance
    }
    return true;
}

/*
 * In each iteration of the while loop two normal random variables are
 * generated. On the first call of the function, two normal random variables are
 * generated.
 * On the after call, the second generated number will be returned.
 */
float rand_normal(float mu, float sigma) {
    float U1, U2, W, mult;
    static float X1, X2;
    static bool call = false;

    if (call == true) {
        call = !call;
        return mu + sigma * X2;
    }

    do {
        U1 = -1 + ((float)random() / (float)(RAND_MAX)) * 2;
        U2 = -1 + ((float)random() / (float)(RAND_MAX)) * 2;
        W  = powf(U1, 2) + powf(U2, 2);
    } while (W >= 1 || W == 0);

    mult = sqrtf((-2 * logf(W)) / W);
    X1   = U1 * mult;
    X2   = U2 * mult;

    call = !call;
    return (mu + sigma * X1);
}

void weights_norm_init(uint32_t in_size, uint32_t out_size, float *weights) {
    srandom(time(NULL));
    for (unsigned long long i = 0; i < (in_size * out_size); i++) {
        weights[i] = rand_normal(0.0f, sqrtf(2.0f / in_size));
    }
}

uint32_t argmax(uint32_t len, const float x[len], float *max) {
    *max             = x[0];
    uint32_t max_pos = 0;
    for (uint32_t i = 0; i < len; i++) {
        if (x[i] > *max) {
            *max    = x[i];
            max_pos = i;
        }
    }
    return max_pos;
}

void softmax(uint32_t len, const float x[len], float xs[len]) {
    float max;
    argmax(len, x, &max);
    float sum = .0f;
    for (uint32_t i = 0; i < len; i++) {
        xs[i] = expf(x[i] - max);
        sum += xs[i];
    }
    for (uint32_t i = 0; i < len; i++) {
        xs[i] = xs[i] / sum;
    }
}

// List of activation functions
// Convention: derived activation functions are prefixed with label 'derived'
/*
 * The sigmoid function
 */
float fsigmoid(float x) { return 1.0f / (1.0f + expf(-x)); }

/*
 * The derived of the sigmoid function
 */
float derived_fsigmoid(float y) { return (y * (1.0f - y)); }

float frelu(float x) { return (float)(x > .0) * x; }

float derived_frelu(float x) { return (float)(x > .0); }

float ftanh(float x) { return tanhf(x); }

float derived_ftanh(float x) { return 1.0f - powf(ftanh(x), 2.0f); }

static void vec_func_f32(uint32_t len, float result[len], float (*f)(float)) {
    for (uint32_t d = 0; d < len; d++) {
        result[d] = f(result[d]);
    }
}

void sigmoid(uint32_t len, float *result) {
    vec_func_f32(len, result, fsigmoid);
}

void relu(uint32_t len, float *result) { vec_func_f32(len, result, frelu); }

void tanhg(uint32_t len, float *result) { vec_func_f32(len, result, ftanh); }

static void vec_derived_f32(uint32_t len, const float result[len],
                            float delta[len], float (*f)(float)) {
    for (uint32_t d = 0; d < len; d++) {
        delta[d] = f(result[d]) * delta[d];
    }
}

void relu_derived(uint32_t len, const float *result, float *delta) {
    vec_derived_f32(len, result, delta, derived_frelu);
}

void tanhg_derived(uint32_t len, const float *result, float *delta) {
    vec_derived_f32(len, result, delta, derived_ftanh);
}

void sigmoid_derived(uint32_t len, const float *result, float *delta) {
    vec_derived_f32(len, result, delta, derived_fsigmoid);
}

/*
 * Allocate memory for a float matrix.
 */
float *matrix_alloc(uint32_t m, uint32_t n) {
    return reallocarray(NULL, m * n, sizeof(float));
}

void matrix_init(uint32_t m, uint32_t n, float matrix[m * n]) {
    for (uint32_t i = 0; i < m * n; i++) {
        matrix[i] = 0.0f;
    }
}

float *weights_create_or_load(const char *filename, uint32_t input_len,
                              uint32_t output_len) {
    float *weight;
    if (filename == NULL) {
        weight = matrix_alloc(input_len, output_len);
        weights_norm_init(input_len, output_len, weight);
    } else {
        struct stat statbuf;
        size_t size_expected = input_len * output_len * sizeof(float);

        int fd = open(filename, O_CREAT | O_RDWR, S_IRUSR | S_IWUSR);
        if (fd < 0) {
            /* failure */
            err(EXIT_FAILURE, "open file '%s'", filename);
        }
        /* find size of input file */
        if (fstat(fd, &statbuf) < 0) {
            err(EXIT_FAILURE, "fstat error");
        } else if (statbuf.st_size == 0) {
            // file does not exist before - create it with the required size
            if (ftruncate(fd, size_expected)) {
                err(EXIT_FAILURE, "file truncate");
            }
            weight = mmap(0, size_expected, PROT_WRITE, MAP_SHARED, fd, 0);
            close(fd);
            weights_norm_init(input_len, output_len, weight);
            return weight;
        } else if (statbuf.st_size > 0 &&
                   (unsigned long)statbuf.st_size < size_expected) {
            errx(EXIT_FAILURE, "invalid data size. Expected: %lu; given: %ld",
                 size_expected, statbuf.st_size);
        }
        weight = mmap(0, size_expected, PROT_WRITE, MAP_SHARED, fd, 0);
        close(fd);
    }
    return weight;
}

static inline float fbernoulli(float p /* must between [0,1] */) {
    return (float)(p < (((float)random() / (float)(RAND_MAX))));
}

void dropout(uint32_t len, const float vec[len], float p, float result[len]) {
    for (uint32_t i = 0; i < len; i++) {
        result[i] = vec[i] * fbernoulli(p);
    }
}


/** \file oposim_fns.c
    \brief Funciones auxiliares
*/

#include <stdio.h>
#include <gsl/gsl_rng.h>
#include <time.h>
#include <stdlib.h>

#include "oposim.h"

static gsl_rng *r = NULL; /**< \a r es el generador de números aleatorios */

/** \fn void random_init(char opc)
\brief Inicializador del generador de números aleatorios.
\param opc a character.
 */
void random_init(char opc)
{

  if (!r) {
    switch(opc) {
    case 'm':
      r = gsl_rng_alloc(gsl_rng_mt19937);
      break;
    case 't':
      r = gsl_rng_alloc(gsl_rng_taus);
      break;
    case 'g':
      r = gsl_rng_alloc(gsl_rng_gfsr4);
      break;
    case 'x':
      r = gsl_rng_alloc(gsl_rng_ranlxs0);
      break;
    default:
      r = gsl_rng_alloc(gsl_rng_mrg);
    }
    printf("Usando RNG %s\n", gsl_rng_name(r));
    gsl_rng_set(r, time(NULL));
  }

}

/** \fn void random_free()
\brief Destructor del generador de números aleatorios
*/
void random_free()
{

  if (!r) {
    gsl_rng_free(r);
  }

}

/** \fn int s_experimento(int temas, int bolas, int estudiados)
\brief Realiza una simulación.
\param temas Número de temas en la oposición.
\param bolas Número de bolas que se extraen.
\param estudiados Número de temas estudiados.
\return Número temas estudiados que estarían entre los extraídos.
*/
int s_experimento(int temas, int bolas, int estudiados)
{

  //printf("bolas extraidas: ");
  int b_extraidas[bolas];
  int bola;

  for (int i = 0; i < bolas; i++) {
    do {
      bola = (int) gsl_rng_uniform_int(r, temas) + 1;
      for (int j = 0; j < i; j++) {
	if (b_extraidas[j] == bola) {
	  bola = 0;
	  break;
	}
      }
    } while (!bola);
    //printf("%d ", bola);
    b_extraidas[i] = bola;
  }

  int res = 0;
  for (int i = 0; i < bolas; i++) {
    if(b_extraidas[i] <= estudiados) res++;
  }

  //printf("\n");
  return res;

}

/** \fn int *experimento(int temas, int bolas, int estudiados, int reps)
\brief Realiza varias simulaciones y devuelve los resultados.
\param temas Número de temas en la oposición.
\param bolas Número de bolas que se extraen.
\param estudiados Número de temas estudiados.
\param reps Número de veces que se realizará el experimento.
\return Histograma con las veces que el número de temas estudiados estaba entre las bolas extraídas.
 */
int *experimento(int temas, int bolas, int estudiados, int reps)
{

  int *res = (int *)calloc(bolas + 1, sizeof(int));

  for (int i = 0; i < reps; i++)
    res[s_experimento(temas, bolas, estudiados)]++;

  return res;

}

/** \fn double *frecs(int *histograma, int l)
\brief Calcula las frecuencias relativas.
\param histograma array de frecuencias absolutas
\param l longitud del histograma
\return array con las frecuencias relativas
 */
double *frecs(int *histograma, int l)
{

  double suma = 0;
  for (int i = 0; i < l; i++)
    suma += histograma[i];

  double *res = (double *)calloc(l, sizeof(double));

  for (int i = 0; i < l; i++)
    res[i] = histograma[i] / suma;

  return res;

}


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

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

/*
 * this module contains routines for the QR factorization of a matrix
 * using the recursive Level 3 BLAS algorithm of Elmroth and Gustavson with
 * additional modifications courtesy of Julien Langou.
 */

static int unpack_Q1(gsl_matrix * Q);
static int unpack_Q2(const gsl_matrix * QR, const gsl_matrix * T, gsl_matrix * Q);
static int aux_ULT(const gsl_matrix * L, gsl_matrix * U);
static int aux_mLU(gsl_matrix * A);
static int aux_ApUBT(const gsl_matrix * U, const gsl_matrix * B, gsl_matrix * A);

/*
gsl_linalg_QR_decomp_r()
  QR decomposition using Level 3 BLAS recursive algorithm of:

Elmroth, E. and Gustavson, F.G., 2000. Applying recursion to serial and parallel
  QR factorization leads to better performance. IBM Journal of Research and Development,
  44(4), pp.605-624.

Inputs: A - matrix to be factored, M-by-N with M >= N
        T - N-by-N upper triangular factor of block reflector

Return: success/error

Notes:
1) on output, upper triangle of A contains R; elements below the diagonal
are columns of V. The matrix Q is

Q = I - V T V^T

where T is upper triangular. Note that diag(T) = tau

2) implementation provided by Julien Langou
*/

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

  if (M < N)
    {
      GSL_ERROR ("M must be >= N", GSL_EBADLEN);
    }
  else if (T->size1 != T->size2)
    {
      GSL_ERROR ("T matrix must be square", GSL_ENOTSQR);
    }
  else if (T->size1 != N)
    {
      GSL_ERROR ("T matrix does not match dimensions of A", GSL_EBADLEN);
    }
  else if (N == 1)
    {
      /* base case, compute householder transform for single column matrix */

      double * T00 = gsl_matrix_ptr(T, 0, 0);
      gsl_vector_view v = gsl_matrix_column(A, 0);

      *T00 = gsl_linalg_householder_transform(&v.vector);
      return GSL_SUCCESS;
    }
  else
    {
      /*
       * partition matrices:
       *
       *       N1  N2              N1  N2
       * N1 [ A11 A12 ] and  N1 [ T11 T12 ]
       * M2 [ A21 A22 ]      N2 [  0  T22 ]
       */
      int status;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;
      const size_t M2 = M - N1;

      gsl_matrix_view A11 = gsl_matrix_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_view A12 = gsl_matrix_submatrix(A, 0, N1, N1, N2);
      gsl_matrix_view A21 = gsl_matrix_submatrix(A, N1, 0, M2, N1);
      gsl_matrix_view A22 = gsl_matrix_submatrix(A, N1, N1, M2, N2);

      gsl_matrix_view T11 = gsl_matrix_submatrix(T, 0, 0, N1, N1);
      gsl_matrix_view T12 = gsl_matrix_submatrix(T, 0, N1, N1, N2);
      gsl_matrix_view T22 = gsl_matrix_submatrix(T, N1, N1, N2, N2);

      gsl_matrix_view m;

      /*
       * Eq. 2: recursively factor
       *
       * [ A11 ] = Q1 [ R11 ]
       * [ A21 ]      [  0  ]
       *                                          N1
       * Note: Q1 = I - V1 T11 V1^T, where V1 = [ V11 ] N1
       *                                        [ V21 ] M2
       */
      m = gsl_matrix_submatrix(A, 0, 0, M, N1);
      status = gsl_linalg_QR_decomp_r(&m.matrix, &T11.matrix);
      if (status)
        return status;

      /*
       * Eq. 3:
       *
       * [ R12 ] := Q1^T [ A12 ] = [ A12 ] - [ V11 W ]
       * [ A22 ]         [ A22 ]   [ A22 ]   [ V21 W ]
       *
       * where W = T11^T (V11^T A12 + V21^T A22), and using T12 as temporary storage
       */
      gsl_matrix_memcpy(&T12.matrix, &A12.matrix);                                                       /* W := A12 */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasTrans, CblasUnit, 1.0, &A11.matrix, &T12.matrix);       /* W := V11^T * A12 */
      gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &A21.matrix, &A22.matrix, 1.0, &T12.matrix);         /* W := W + V21^T * A22 */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, &T11.matrix, &T12.matrix);    /* W := T11^T * W */
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &A21.matrix, &T12.matrix, 1.0, &A22.matrix);      /* A22 = A22 - V21 * W */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, 1.0, &A11.matrix, &T12.matrix);     /* tmp = V11 * W */
      gsl_matrix_sub(&A12.matrix, &T12.matrix);                                                          /* R12 := A12 - V11 * W */

      /*
       * Eq. 4: recursively factor
       *
       * A22 = Q2~ R22
       *
       *              N1 M2
       * Note: Q2 = [ I   0  ] N1
       *            [ 0  Q2~ ] M2
       */
      status = gsl_linalg_QR_decomp_r(&A22.matrix, &T22.matrix);
      if (status)
        return status;

      /*
       * Eq. 13: update T12 := -T11 * V1^T * V2 * T22
       *
       * where:
       *
       *        N1                N2
       * V1 = [ V11 ] N1   V2 = [  0  ] N1
       *      [ V21 ] N2        [ V22 ] N2
       *      [ V31 ] M-N       [ V32 ] M-N
       *
       * Note: V1^T V2 = V21^T V22 + V31^T V32
       * Also, V11, V22 are unit lower triangular
       */

      m = gsl_matrix_submatrix(&A21.matrix, 0, 0, N2, N1);                                               /* V21 */
      gsl_matrix_transpose_memcpy(&T12.matrix, &m.matrix);                                               /* T12 := V21^T */

      m = gsl_matrix_submatrix(A, N1, N1, N2, N2);                                                       /* V22 */
      gsl_blas_dtrmm(CblasRight, CblasLower, CblasNoTrans, CblasUnit, 1.0, &m.matrix, &T12.matrix);      /* T12 := V21^T * V22 */

      if (M > N)
        {
          gsl_matrix_view V31 = gsl_matrix_submatrix(A, N, 0, M - N, N1);
          gsl_matrix_view V32 = gsl_matrix_submatrix(A, N, N1, M - N, N2);

          gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &V31.matrix, &V32.matrix, 1.0, &T12.matrix);     /* T12 := T12 + V31^T * V32 */
        }

      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &T11.matrix, &T12.matrix); /* T12 := -T11 * T12 */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, 1.0, &T22.matrix, &T12.matrix); /* T12 := T12 * T22 */

      return GSL_SUCCESS;
    }
}

/* Solves the square system A x = b for x using the QR factorisation,
 *
 *  R x = Q^T b
 *
 * where Q = I - V T V^T
 */

int
gsl_linalg_QR_solve_r (const gsl_matrix * QR, const gsl_matrix * T, const gsl_vector * b, gsl_vector * x)
{
  const size_t N = QR->size2;

  if (QR->size1 != N)
    {
      GSL_ERROR ("QR matrix must be square", GSL_ENOTSQR);
    }
  else if (T->size1 != QR->size1 || T->size2 != QR->size2)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (N != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (N != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      size_t i;

      /* compute Q^T b = [I - V T^T V^T] b */

      /* x := V^T b */
      gsl_vector_memcpy(x, b);
      gsl_blas_dtrmv(CblasLower, CblasTrans, CblasUnit, QR, x);

      /* x = T^T * x */
      gsl_blas_dtrmv(CblasUpper, CblasTrans, CblasNonUnit, T, x);

      /* x = V * x */
      gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasUnit, QR, x);

      /* x = b - V * x */
      for (i = 0; i < N; ++i)
        {
          double * xi = gsl_vector_ptr(x, i);
          double bi = gsl_vector_get(b, i);
          *xi = bi - (*xi);
        }

      /* Solve R x = Q^T b, storing x in-place */
      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, QR, x);

      return GSL_SUCCESS;
    }
}

/* Find the least squares solution to the overdetermined system
 *
 *   A x = b
 *
 * for M >= N using the QR factorization A = Q R.
 *
 * Inputs: QR   - [R; V] matrix, M-by-N
 *         T    - upper triangular block reflector, N-by-N
 *         b    - right hand side, size M
 *         x    - (output) solution, size M
 *                x(1:N) = least squares solution vector
 *                x(N+1:M) = vector whose norm equals ||b - Ax||
 *         work - workspace, size N
 */

int
gsl_linalg_QR_lssolve_r (const gsl_matrix * QR, const gsl_matrix * T, const gsl_vector * b, gsl_vector * x, gsl_vector * work)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("QR matrix must have M >= N", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (M != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (M != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else if (N != work->size)
    {
      GSL_ERROR ("matrix size must match work size", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view R = gsl_matrix_const_submatrix (QR, 0, 0, N, N);
      gsl_vector_view x1 = gsl_vector_subvector(x, 0, N);

      /* compute x = Q^T b */
      gsl_vector_memcpy(x, b);
      gsl_linalg_QR_QTvec_r (QR, T, x, work);

      /* Solve R x = Q^T b */
      gsl_blas_dtrsv (CblasUpper, CblasNoTrans, CblasNonUnit, &R.matrix, &x1.vector);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_QR_unpack_r()
  Unpack matrices Q and R

Inputs: QR - packed QR format, M-by-N
        T  - block reflector matrix, N-by-N
        Q  - (output) Q matrix, M-by-M
        R  - (output) R matrix, N-by-N

Return: success/error

Notes:
1) Implementation provided by Julien Langou
*/

int
gsl_linalg_QR_unpack_r(const gsl_matrix * QR, const gsl_matrix * T, gsl_matrix * Q, gsl_matrix * R)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("M must be >= N", GSL_EBADLEN);
    }
  else if (Q->size1 != M || Q->size2 != M)
    {
      GSL_ERROR ("Q matrix must be M-by-M", GSL_EBADLEN);
    }
  else if (R->size1 != N || R->size2 != N)
    {
      GSL_ERROR ("R matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view RV = gsl_matrix_const_submatrix(QR, 0, 0, N, N);
      gsl_matrix_view Q1 = gsl_matrix_submatrix(Q, 0, 0, M, N);
      gsl_matrix_view m;

      /*
       * set Q1 = [ T ]
       *          [ V ]
       */
      m = gsl_matrix_submatrix(Q, 0, 0, N, N);
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, &m.matrix, T);
      gsl_matrix_tricpy(CblasLower, CblasUnit, &m.matrix, &RV.matrix);

      if (M > N)
        {
          gsl_matrix_const_view tmp = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);
          m = gsl_matrix_submatrix(Q, N, 0, M - N, N);
          gsl_matrix_memcpy(&m.matrix, &tmp.matrix);
        }

      unpack_Q1(&Q1.matrix);

      if (M > N)
        {
          gsl_matrix_view Q2 = gsl_matrix_submatrix(Q, 0, N, M, M - N);
          unpack_Q2(QR, T, &Q2.matrix);
        }

      /* copy R */
      gsl_matrix_tricpy(CblasUpper, CblasNonUnit, R, &RV.matrix);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_QR_QTvec_r()
  Apply M-by-M Q^T to the M-by-1 vector b

Inputs: QR   - [R; V] matrix encoded by gsl_linalg_QR_decomp_r
        T    - block reflector matrix
        b    - M-by-1 vector replaced by Q^T b on output
        work - workspace, length N

Notes:
1) Q^T b = (I - V T^T V^T) b
         = b - V T^T [ V1^T V2^T ] [ b1 ]
                                   [ b2 ]
         = b - V T^T [ V1^T b1 + V2^T b2 ]
         = [ b1 ] - [ V1 w ]
           [ b2 ]   [ V2 w ]

where w = T^T ( V1^T b1 + V2^T b2 )
*/

int
gsl_linalg_QR_QTvec_r(const gsl_matrix * QR, const gsl_matrix * T, gsl_vector * b, gsl_vector * work)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M < N)
    {
      GSL_ERROR ("M must be >= N", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (b->size != M)
    {
      GSL_ERROR ("b vector must have length M", GSL_EBADLEN);
    }
  else if (work->size != N)
    {
      GSL_ERROR ("workspace must be length N", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view V1 = gsl_matrix_const_submatrix(QR, 0, 0, N, N);
      gsl_vector_view b1 = gsl_vector_subvector(b, 0, N);
      gsl_vector_view b2;

      /* work := V1^T b1 */
      gsl_vector_memcpy(work, &b1.vector);
      gsl_blas_dtrmv(CblasLower, CblasTrans, CblasUnit, &V1.matrix, work);

      if (M > N)
        {
          gsl_matrix_const_view V2 = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);

          /* work = work + V2^T b2 */
          b2 = gsl_vector_subvector(b, N, M - N);
          gsl_blas_dgemv(CblasTrans, 1.0, &V2.matrix, &b2.vector, 1.0, work);
        }

      /* work = T^T * work */
      gsl_blas_dtrmv(CblasUpper, CblasTrans, CblasNonUnit, T, work);

      if (M > N)
        {
          /* b2 = b2 - V2 * work */
          gsl_matrix_const_view V2 = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);
          gsl_blas_dgemv(CblasNoTrans, -1.0, &V2.matrix, work, 1.0, &b2.vector);
        }

      /* b1 = b1 - V1 * work */
      gsl_blas_dtrmv(CblasLower, CblasNoTrans, CblasUnit, &V1.matrix, work);
      gsl_vector_sub(&b1.vector, work);

      return GSL_SUCCESS;
    }
}

/*
gsl_linalg_QR_QTmat_r()
  Apply M-by-M Q^T to the M-by-K matrix B

Inputs: QR   - [R; V] matrix encoded by gsl_linalg_QR_decomp_r
        T    - block reflector matrix
        B    - M-by-K matrix replaced by Q^T B on output
        work - N-by-K workspace

Notes:
1) Provided by Julien Langou
*/

int
gsl_linalg_QR_QTmat_r(const gsl_matrix * QR, const gsl_matrix * T, gsl_matrix * B, gsl_matrix * work)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;
  const size_t K = B->size2;

  if (M < N)
    {
      GSL_ERROR ("M must be >= N", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (B->size1 != M)
    {
      GSL_ERROR ("B matrix must have M rows", GSL_EBADLEN);
    }
  else if (work->size1 != N || work->size2 != K)
    {
      GSL_ERROR ("workspace must be N-by-K", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view V1 = gsl_matrix_const_submatrix(QR, 0, 0, N, N);
      gsl_matrix_view B1 = gsl_matrix_submatrix(B, 0, 0, N, K);
      gsl_matrix_view B2;

      /* work := V1^T B1 */
      gsl_matrix_memcpy(work, &B1.matrix);
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasTrans, CblasUnit, 1.0, &V1.matrix, work);

      if (M > N)
        {
          gsl_matrix_const_view V2 = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);

          /* work = work + V2^T B2 */
          B2 = gsl_matrix_submatrix(B, N, 0, M - N, K);
          gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, &V2.matrix, &B2.matrix, 1.0, work);
        }

      /* work = T^T * work */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasTrans, CblasNonUnit, 1.0, T, work);

      if (M > N)
        {
          /* B2 = B2 - V2 * work */
          gsl_matrix_const_view V2 = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);
          gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &V2.matrix, work, 1.0, &B2.matrix);
        }

      /* B1 = B1 - V1 * work */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, 1.0, &V1.matrix, work);
      gsl_matrix_sub(&B1.matrix, work);

      return GSL_SUCCESS;
    }
}

/*
unpack_Q1()
  Compute Q_1

Inputs: Q  - on input, contains T in upper triangle and V in lower trapezoid
             on output, contains Q_1
             M-by-N

Return: success/error

Notes:
1)                                                         N
Q1 = [ Q1 Q2 ] [ I_n ] = (I - V T V^T) [ I; 0 ] = [ I - V1 T V1^T ] N
               [  0  ]                            [   - V2 T V1^T ] M - N
*/

static int
unpack_Q1(gsl_matrix * Q)
{
  int status;
  const size_t M = Q->size1;
  const size_t N = Q->size2;
  gsl_matrix_view Q1 = gsl_matrix_submatrix(Q, 0, 0, N, N);
  gsl_vector_view diag = gsl_matrix_diagonal(&Q1.matrix);

  /* Q1 := T V1^T */
  status = aux_ULT(&Q1.matrix, &Q1.matrix);
  if (status)
    return status;

  if (M > N)
    {
      /* compute Q2 := - V2 T V1^T */
      gsl_matrix_view V2 = gsl_matrix_submatrix(Q, N, 0, M - N, N);
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &Q1.matrix, &V2.matrix);
    }

  /* Q1 := - V1 T V1^T */
  status = aux_mLU(&Q1.matrix);
  if (status)
    return status;

  /* Q1 := I - V1 T V1^T */
  gsl_vector_add_constant(&diag.vector, 1.0);

  return GSL_SUCCESS;
}

/*
unpack_Q2()
  Compute Q_2

Inputs: QR - [R; V] from QR_decomp_r, M-by-N
        T  - upper triangular T factor, N-by-N
        Q  - (output) Q_2 factor, M-by-(M-N)

Return: success/error

Notes:                         N   M-N
1) Since Q = I - V T V^T = M [ Q1  Q2  ], we have

           M-N
Q2 = Q [    0    ] N
       [ I_{M-N} ] M-N

So, Q2 = Q [ 0; I ] = (I - V T V^T) [ 0; I ] = [   - V1 T V2^T ]
                                               [ I - V2 T V2^T ]
*/

static int
unpack_Q2(const gsl_matrix * QR, const gsl_matrix * T, gsl_matrix * Q)
{
  const size_t M = QR->size1;
  const size_t N = QR->size2;

  if (M <= N)
    {
      GSL_ERROR ("M must be > N", GSL_EBADLEN);
    }
  else if (T->size1 != N || T->size2 != N)
    {
      GSL_ERROR ("T matrix must be N-by-N", GSL_EBADLEN);
    }
  else if (Q->size1 != M || Q->size2 != (M - N))
    {
      GSL_ERROR ("Q matrix must be M-by-(M-N)", GSL_EBADLEN);
    }
  else
    {
      gsl_matrix_const_view V1 = gsl_matrix_const_submatrix(QR, 0, 0, N, N);
      gsl_matrix_const_view V2 = gsl_matrix_const_submatrix(QR, N, 0, M - N, N);
      gsl_matrix_view Q1 = gsl_matrix_submatrix(Q, 0, 0, N, M - N);
      gsl_matrix_view Q2 = gsl_matrix_submatrix(Q, N, 0, M - N, M - N);
      gsl_vector_view diag = gsl_matrix_diagonal(&Q2.matrix);

      /* Q1 := V2^T */
      gsl_matrix_transpose_memcpy(&Q1.matrix, &V2.matrix);

      /* Q1 := - T V2^T */
      gsl_blas_dtrmm(CblasLeft, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, T, &Q1.matrix);

      /* Q2 := - V2 T V2^T */
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, &V2.matrix, &Q1.matrix, 0.0, &Q2.matrix);

      /* Q2 := I - V2 T V2^T */
      gsl_vector_add_constant(&diag.vector, 1.0);

      /* Q1 := - V1 T V2^T */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, 1.0, &V1.matrix, &Q1.matrix);

      return GSL_SUCCESS;
    }
}

/* U := U L^T for triangular matrices L and U; L is unit lower triangular */
static int
aux_ULT(const gsl_matrix * L, gsl_matrix * U)
{
  const size_t N = L->size1;

  if (N != L->size2)
    {
      GSL_ERROR ("L matrix must be square", GSL_ENOTSQR);
    }
  else if (U->size1 != N || U->size2 != N)
    {
      GSL_ERROR ("U matrix must be same size as L", GSL_EBADLEN);
    }
  else if (N == 1)
    {
      /* nothing to do */
      return GSL_SUCCESS;
    }
  else
    {
      int status;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_const_view L11 = gsl_matrix_const_submatrix(L, 0, 0, N1, N1);
      gsl_matrix_const_view L21 = gsl_matrix_const_submatrix(L, N1, 0, N2, N1);
      gsl_matrix_const_view L22 = gsl_matrix_const_submatrix(L, N1, N1, N2, N2);

      gsl_matrix_view U11 = gsl_matrix_submatrix(U, 0, 0, N1, N1);
      gsl_matrix_view U12 = gsl_matrix_submatrix(U, 0, N1, N1, N2);
      gsl_matrix_view U22 = gsl_matrix_submatrix(U, N1, N1, N2, N2);

      /* U12 = U12 * L22^T */
      gsl_blas_dtrmm(CblasRight, CblasLower, CblasTrans, CblasUnit, 1.0, &L22.matrix, &U12.matrix);

      /* U12 = U12 + U11 * L21^T */
      status = aux_ApUBT(&U11.matrix, &L21.matrix, &U12.matrix);
      if (status)
        return status;

      status = aux_ULT(&L11.matrix, &U11.matrix);
      if (status)
        return status;

      status = aux_ULT(&L22.matrix, &U22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

/* store -L*U in A */
static int
aux_mLU(gsl_matrix * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR ("matrix must be square", GSL_ENOTSQR);
    }
  else if (N == 1)
    {
      double *A00 = gsl_matrix_ptr(A, 0, 0);
      *A00 = -(*A00);
      return GSL_SUCCESS;
    }
  else
    {
      int status;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_view A11 = gsl_matrix_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_view A12 = gsl_matrix_submatrix(A, 0, N1, N1, N2);
      gsl_matrix_view A21 = gsl_matrix_submatrix(A, N1, 0, N2, N1);
      gsl_matrix_view A22 = gsl_matrix_submatrix(A, N1, N1, N2, N2);

      /* A22 = - L22 U22 */
      status = aux_mLU(&A22.matrix);
      if (status)
        return status;

      /* A22 = A22 - L21 U12 */
      gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, -1.0, &A21.matrix, &A12.matrix, 1.0, &A22.matrix);

      /* A12 - -L11 U12 */
      gsl_blas_dtrmm(CblasLeft, CblasLower, CblasNoTrans, CblasUnit, -1.0, &A11.matrix, &A12.matrix);

      /* A21 = -L21 U11 */
      gsl_blas_dtrmm(CblasRight, CblasUpper, CblasNoTrans, CblasNonUnit, -1.0, &A11.matrix, &A21.matrix);

      /* A11 = - L11 U11 */
      status = aux_mLU(&A11.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}

/* A := A + U B^T where U is upper triangular */
static int
aux_ApUBT(const gsl_matrix * U, const gsl_matrix * B, gsl_matrix * A)
{
  const size_t M = A->size1;
  const size_t N = A->size2;

  if (U->size1 != M || U->size2 != M)
    {
      GSL_ERROR ("U matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (B->size1 != N || B->size2 != M)
    {
      GSL_ERROR ("B matrix has wrong dimensions", GSL_EBADLEN);
    }
  else if (M == 1 && N == 1)
    {
      double *aptr = gsl_matrix_ptr(A, 0, 0);
      const double *uptr = gsl_matrix_const_ptr(U, 0, 0);
      const double *bptr = gsl_matrix_const_ptr(B, 0, 0);
      *aptr += (*uptr) * (*bptr);
      return GSL_SUCCESS;
    }
  else if (M == 1)
    {
      double U00 = gsl_matrix_get(U, 0, 0);
      gsl_vector_view v = gsl_matrix_row(A, 0);
      gsl_vector_const_view w = gsl_matrix_const_column(B, 0);
      gsl_blas_daxpy(U00, &w.vector, &v.vector);
      return GSL_SUCCESS;
    }
  else if (N == 1)
    {
      /*
       * partition:
       *
       *         M1  M2
       * B = 1 [ B11 B12 ]
       *
       *       1        M1  M2       1
       * M1 [ A11 ] + [ U11 U12 ] [ B11^T ] M1
       * M2 [ A21 ]   [  0  U22 ] [ B12^T ] M2
       */
      int status;
      const size_t M1 = M / 2;
      const size_t M2 = M - M1;

      gsl_matrix_view A11 = gsl_matrix_submatrix(A, 0, 0, M1, 1);
      gsl_matrix_view A21 = gsl_matrix_submatrix(A, M1, 0, M2, 1);
      gsl_vector_view a1 = gsl_matrix_subcolumn(A, 0, 0, M1);

      gsl_matrix_const_view U11 = gsl_matrix_const_submatrix(U, 0, 0, M1, M1);
      gsl_matrix_const_view U12 = gsl_matrix_const_submatrix(U, 0, M1, M1, M2);
      gsl_matrix_const_view U22 = gsl_matrix_const_submatrix(U, M1, M1, M2, M2);

      gsl_matrix_const_view B11 = gsl_matrix_const_submatrix(B, 0, 0, 1, M1);
      gsl_matrix_const_view B12 = gsl_matrix_const_submatrix(B, 0, M1, 1, M2);
      gsl_vector_const_view b2 = gsl_matrix_const_subrow(B, 0, M1, M2);

      /* A(1:M1,1) += U12 * B12^T */
      gsl_blas_dgemv(CblasNoTrans, 1.0, &U12.matrix, &b2.vector, 1.0, &a1.vector);

      /* A11 := A11 + U11 B11^T */
      status = aux_ApUBT(&U11.matrix, &B11.matrix, &A11.matrix);
      if (status)
        return status;

      /* A21 := A21 + U22 B12^T */
      status = aux_ApUBT(&U22.matrix, &B12.matrix, &A21.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
  else
    {
      int status;
      const size_t M1 = M / 2;
      const size_t M2 = M - M1;
      const size_t N1 = N / 2;
      const size_t N2 = N - N1;

      gsl_matrix_view A11 = gsl_matrix_submatrix(A, 0, 0, M1, N1);
      gsl_matrix_view A12 = gsl_matrix_submatrix(A, 0, N1, M1, N2);
      gsl_matrix_view A21 = gsl_matrix_submatrix(A, M1, 0, M2, N1);
      gsl_matrix_view A22 = gsl_matrix_submatrix(A, M1, N1, M2, N2);

      gsl_matrix_const_view U11 = gsl_matrix_const_submatrix(U, 0, 0, M1, M1);
      gsl_matrix_const_view U12 = gsl_matrix_const_submatrix(U, 0, M1, M1, M2);
      gsl_matrix_const_view U22 = gsl_matrix_const_submatrix(U, M1, M1, M2, M2);

      gsl_matrix_const_view B11 = gsl_matrix_const_submatrix(B, 0, 0, N1, M1);
      gsl_matrix_const_view B12 = gsl_matrix_const_submatrix(B, 0, M1, N1, M2);
      gsl_matrix_const_view B21 = gsl_matrix_const_submatrix(B, N1, 0, N2, M1);
      gsl_matrix_const_view B22 = gsl_matrix_const_submatrix(B, N1, M1, N2, M2);

      /* A11 := A11 + U11 B11^T */
      status = aux_ApUBT(&U11.matrix, &B11.matrix, &A11.matrix);
      if (status)
        return status;

      /* A11 := A11 + U12 B12^T */
      gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &U12.matrix, &B12.matrix, 1.0, &A11.matrix);

      /* A12 := A12 + U11 B21^T */
      status = aux_ApUBT(&U11.matrix, &B21.matrix, &A12.matrix);
      if (status)
        return status;

      /* A12 := A12 + U12 B22^T */
      gsl_blas_dgemm(CblasNoTrans, CblasTrans, 1.0, &U12.matrix, &B22.matrix, 1.0, &A12.matrix);

      /* A21 := A21 + U22 B12^T */
      status = aux_ApUBT(&U22.matrix, &B12.matrix, &A21.matrix);
      if (status)
        return status;

      /* A22 := A22 + U22 B22^T */
      status = aux_ApUBT(&U22.matrix, &B22.matrix, &A22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}


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

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_odeiv2.h>
#include "control_utils.c"

typedef struct
{
  double eps_abs;
  double eps_rel;
  double a_y;
  double a_dydt;
  double *scale_abs;
}
sc_control_state_t;

static void *
sc_control_alloc (void)
{
  sc_control_state_t *s =
    (sc_control_state_t *) malloc (sizeof (sc_control_state_t));

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

  return s;
}

static int
sc_control_init (void *vstate,
                 double eps_abs, double eps_rel, double a_y, double a_dydt)
{
  sc_control_state_t *s = (sc_control_state_t *) vstate;

  if (eps_abs < 0)
    {
      GSL_ERROR ("eps_abs is negative", GSL_EINVAL);
    }
  else if (eps_rel < 0)
    {
      GSL_ERROR ("eps_rel is negative", GSL_EINVAL);
    }
  else if (a_y < 0)
    {
      GSL_ERROR ("a_y is negative", GSL_EINVAL);
    }
  else if (a_dydt < 0)
    {
      GSL_ERROR ("a_dydt is negative", GSL_EINVAL);
    }

  s->eps_rel = eps_rel;
  s->eps_abs = eps_abs;
  s->a_y = a_y;
  s->a_dydt = a_dydt;

  return GSL_SUCCESS;
}

static int
sc_control_hadjust (void *vstate, size_t dim, unsigned int ord,
                    const double y[], const double yerr[], const double yp[],
                    double *h)
{
  sc_control_state_t *state = (sc_control_state_t *) vstate;

  const double eps_abs = state->eps_abs;
  const double eps_rel = state->eps_rel;
  const double a_y = state->a_y;
  const double a_dydt = state->a_dydt;
  const double *scale_abs = state->scale_abs;

  const double S = 0.9;
  const double h_old = *h;

  double rmax = DBL_MIN;
  size_t i;

  for (i = 0; i < dim; i++)
    {
      const double D0 =
        eps_rel * (a_y * fabs (y[i]) + a_dydt * fabs (h_old * yp[i]))
        + eps_abs * scale_abs[i];
      const double r = fabs (yerr[i]) / fabs (D0);
      rmax = GSL_MAX_DBL (r, rmax);
    }

  if (rmax > 1.1)
    {
      /* decrease step, no more than factor of 5, but a fraction S more
         than scaling suggests (for better accuracy) */
      double r = S / pow (rmax, 1.0 / ord);

      if (r < 0.2)
        r = 0.2;

      *h = r * h_old;

      return GSL_ODEIV_HADJ_DEC;
    }
  else if (rmax < 0.5)
    {
      /* increase step, no more than factor of maxscale */
      double r = S / pow (rmax, 1.0 / (ord + 1.0));
      const double maxscale = 4.9;

      if (r > maxscale)
        r = maxscale;

      if (r < 1.0)              /* don't allow any decrease caused by S<1 */
        r = 1.0;

      *h = r * h_old;

      return GSL_ODEIV_HADJ_INC;
    }
  else
    {
      /* no change */
      return GSL_ODEIV_HADJ_NIL;
    }
}

static int
sc_control_errlevel (void *vstate, const double y, const double dydt,
                     const double h, const size_t ind, double *errlev)
{
  sc_control_state_t *state = (sc_control_state_t *) vstate;

  const double eps_abs = state->eps_abs;
  const double eps_rel = state->eps_rel;
  const double a_y = state->a_y;
  const double a_dydt = state->a_dydt;
  const double *scale_abs = state->scale_abs;

  *errlev = eps_rel * (a_y * fabs (y) + a_dydt * fabs (h * dydt))
    + eps_abs * scale_abs[ind];

  if (*errlev <= 0.0)
    {
      GSL_ERROR ("errlev <= zero", GSL_ESANITY);
    }

  return GSL_SUCCESS;
}

static void
sc_control_free (void *vstate)
{
  sc_control_state_t *state = (sc_control_state_t *) vstate;
  free (state->scale_abs);
  free (state);
}

static const gsl_odeiv2_control_type sc_control_type = { "scaled",      /* name */
  &sc_control_alloc,
  &sc_control_init,
  &sc_control_hadjust,
  &sc_control_errlevel,
  &control_set_driver_null,
  &sc_control_free
};

const gsl_odeiv2_control_type *gsl_odeiv2_control_scaled = &sc_control_type;

gsl_odeiv2_control *
gsl_odeiv2_control_scaled_new (double eps_abs, double eps_rel,
                               double a_y, double a_dydt,
                               const double scale_abs[], size_t dim)
{
  gsl_odeiv2_control *c =
    gsl_odeiv2_control_alloc (gsl_odeiv2_control_scaled);

  int status = gsl_odeiv2_control_init (c, eps_abs, eps_rel, a_y, a_dydt);

  if (status != GSL_SUCCESS)
    {
      gsl_odeiv2_control_free (c);
      GSL_ERROR_NULL ("error trying to initialize control", status);
    }

  {
    sc_control_state_t *s = (sc_control_state_t *) c->state;

    s->scale_abs = (double *) malloc (dim * sizeof (double));

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

    memcpy (s->scale_abs, scale_abs, dim * sizeof (double));
  }

  return c;
}


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

int main(void)
{
  size_t i, j;

  gsl_matrix *m = gsl_matrix_alloc(10,10);

  for (i=0; i<10; i++)
    for (j=0; j<10; j++)
      gsl_matrix_set(m, i, j, sin(i) + cos(j));

  for (j=0; j<10; j++)
  {
    gsl_vector_view column = gsl_matrix_column(m, j);
    double d;

    d = gsl_blas_dnrm2(&column.vector);

    printf("matrix column %zu, norm = %g\n", j, d);
  }

  gsl_matrix_free(m);

  return 0;
}


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

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

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

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

    /*
    * exponential cone is not self dual, if s \in K
    * then y \in K^* and so if K is the primal cone
    * here we project onto K^*, via Moreau
    */
    scaleArray(&(x[count]), -1, 3*k->ep); // x = -x;
    double r,s,t;
    int idx;
#pragma omp parallel for private(r,s,t,idx)
    for (i=0; i < k->ep; ++i) {
        idx = count + 3*i;
        r = x[idx];
        s = x[idx+1];
        t = x[idx+2];

        projExpCone(&(x[idx]));

        x[idx] -= r;
        x[idx+1] -= s;
        x[idx+2] -= t;
    }
    count += 3*k->ep;

    // exponential cone:
#pragma omp parallel for
    for (i=0; i < k->ed; ++i) {
        projExpCone(&(x[count + 3*i]));
    }
    count += 3*k->ed;
    /* project onto OTHER cones */
}

double expNewtonOneD(double rho, double y_hat, double z_hat) {
    double t = fmax( -z_hat , 1e-6);
    double f, fp;
    for (int i=0; i<100; ++i){

        f = t * (t + z_hat) / rho / rho - y_hat/rho + log(t/rho) + 1;
        fp = (2 * t + z_hat) / rho / rho + 1/t;

        t = t - f/fp;

        if (t <= -z_hat) {
            return 0;
        } else if (t <= 0) {
            return z_hat;
        } else if ( fabs(f) < 1e-9 ) {
            break;
        }
    }
    return t + z_hat;
}

void expSolveForXWithRho(double * v, double * x, double rho) {
    x[2] = expNewtonOneD(rho, v[1], v[2]);
    x[1] = (x[2] - v[2]) * x[2] / rho;
    x[0] = v[0] - rho;
}

double expCalcGrad(double * v, double * x, double rho) {
    expSolveForXWithRho(v, x, rho);
    if (x[1] <= 1e-12) {
        return x[0];
    } else {
        return x[0] + x[1] * log( x[1] / x[2] );
    }
}

void expGetRhoUb(double * v, double * x, double * ub, double * lb) {
    *lb = 0;
    *ub = 0.125;
    while(expCalcGrad(v, x, *ub) > 0) {
        *lb = *ub;
        (*ub) *= 2;
    }
}

// project onto the exponential cone, v has dimension *exactly* 3
void projExpCone(double * v) {

    double r = v[0], s = v[1], t = v[2];
    // v in cl(Kexp)
    if( (s*exp(r/s) <= t && s > 0) || (r <= 0 && s == 0 && t >= 0) ) {
        return;
    }

    // -v in Kexp^*
    if ( (-r < 0 && r*exp(s/r) <= -exp(1)*t) || (-r == 0 && -s >= 0 && -t >= 0) ) {
        memset(v, 0, 3*sizeof(double));
        return;
    }

    // special case with analytical solution
    if(r < 0 && s < 0) {
        v[1] = 0.0;
        v[2] = fmax(v[2],0);
        return;
    }

    double ub, lb, rho, g, x[3];
    expGetRhoUb(v, x, &ub, &lb);
    for(int i = 0; i < 100; ++i){
        rho = (ub + lb)/2;
        g = expCalcGrad(v, x, rho);
        if (g > 0) {
            lb = rho;
        } else{
            ub = rho;
        }
        if (ub - lb < 1e-9) {
            break;
        }
    }
    v[0] = x[0];
    v[1] = x[1];
    v[2] = x[2];
}

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

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

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

  double EIG_TOL = 1e-8;
  double vupper = calcNorm(Xs,n*n);
  LAPACKE_dsyevr( LAPACK_COL_MAJOR, 'V', 'V', 'U', n, Xs, n, 0.0, vupper, -1, -1, EIG_TOL, &m, e, Z, n , NULL);
  //printf("m is %i, n is %i\n", m ,n);
  //printf("vupper is %f, max eig is %f\n",vupper, e[m>0 ? m-1:0]/2);
  memset(X, 0, n*n*sizeof(double));
  for (i = 0; i < m; ++i) {
    cblas_dsyr(CblasColMajor, CblasLower, n, e[i]/2, &(Z[i*n]), 1, X, n);
    //b_dsyr('L', n, -e[i]/2, &(Z[i*n]), 1, Xs, n);
  }
  // fill in upper half
  for (i = 0; i < n; ++i){
    for (j = i+1; j < n; ++j){
      X[i + j*n] = X[j + i*n];
    }
  }
}
#endif


/*! @copyright (c) 2017 King Abdullah University of Science and
 *                      Technology (KAUST). All rights reserved.
 *
 * STARS-H is a software package, provided by King Abdullah
 *             University of Science and Technology (KAUST)
 *
 * @file testing/mpi_cauchy.c
 * @version 1.3.0
 * @author Aleksandr Mikhalev
 * @date 2017-11-07
 * */

#ifdef MKL
    #include <mkl.h>
#else
    #include <cblas.h>
    #include <lapacke.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <mpi.h>
#include <starsh.h>
#include <starsh-cauchy.h>

int main(int argc, char **argv)
{
    MPI_Init(&argc, &argv);
    int mpi_size, mpi_rank;
    MPI_Comm_size(MPI_COMM_WORLD, &mpi_size);
    MPI_Comm_rank(MPI_COMM_WORLD, &mpi_rank);
    if(argc < 5)
    {
        if(mpi_rank == 0)
        {
            printf("%d arguments provided, but 4 are needed\n", argc-1);
            printf("mpi_cauchy N block_size maxrank tol\n");
        }
        MPI_Finalize();
        return 1;
    }
    int N = atoi(argv[1]), block_size = atoi(argv[2]);
    int maxrank = atoi(argv[3]);
    double tol = atof(argv[4]);
    int onfly = 0;
    char dtype = 'd', symm = 'N';
    int ndim = 2;
    STARSH_int shape[2] = {N, N};
    int info;
    srand(0);
    // Init STARS-H
    info = starsh_init();
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    // Generate data for spatial statistics problem
    STARSH_cauchy *data;
    STARSH_kernel *kernel;
    info = starsh_application((void **)&data, &kernel, N, dtype, STARSH_CAUCHY,
            STARSH_CAUCHY_KERNEL1, 0);
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    // Init problem with given data and kernel and print short info
    STARSH_problem *P;
    info = starsh_problem_new(&P, ndim, shape, symm, dtype, data, data,
            kernel, "Cauchy example");
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    if(mpi_rank == 0)
        starsh_problem_info(P);
    // Init plain clusterization and print info
    STARSH_cluster *C;
    info = starsh_cluster_new_plain(&C, data, N, block_size);
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    if(mpi_rank == 0)
        starsh_cluster_info(C);
    // Init tlr division into admissible blocks and print short info
    STARSH_blrf *F;
    STARSH_blrm *M;
    info = starsh_blrf_new_tlr_mpi(&F, P, symm, C, C);
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    if(mpi_rank == 0)
        starsh_blrf_info(F);
    // Approximate each admissible block
    MPI_Barrier(MPI_COMM_WORLD);
    double time1 = MPI_Wtime();
    info = starsh_blrm_approximate(&M, F, maxrank, tol, onfly);
    if(info != 0)
    {
        MPI_Finalize();
        return 1;
    }
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime()-time1;
    if(mpi_rank == 0)
    {
        starsh_blrf_info(F);
        starsh_blrm_info(M);
    }
    if(mpi_rank == 0)
        printf("TIME TO APPROXIMATE: %e secs\n", time1);
    // Measure approximation error
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime();
    double rel_err = starsh_blrm__dfe_mpi(M);
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime()-time1;
    if(mpi_rank == 0)
    {
        printf("TIME TO MEASURE ERROR: %e secs\nRELATIVE ERROR: %e\n",
                time1, rel_err);
        if(rel_err/tol > 10.)
        {
            printf("Resulting relative error is too big\n");
            MPI_Finalize();
            return 1;
        }
    }
    if(rel_err/tol > 10.)
    {
        MPI_Finalize();
        return 1;
    }
    // Measure time for 10 BLRM matvecs and for 10 BLRM TLR matvecs
    double *x, *y, *y_tlr;
    int nrhs = 1;
    x = malloc(N*nrhs*sizeof(*x));
    y = malloc(N*nrhs*sizeof(*y));
    y_tlr = malloc(N*nrhs*sizeof(*y_tlr));
    if(mpi_rank == 0)
    {
        int iseed[4] = {0, 0, 0, 1};
        LAPACKE_dlarnv_work(3, iseed, N*nrhs, x);
        cblas_dscal(N*nrhs, 0.0, y, 1);
        cblas_dscal(N*nrhs, 0.0, y_tlr, 1);
    }
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime();
    for(int i = 0; i < 10; i++)
        starsh_blrm__dmml_mpi(M, nrhs, 1.0, x, N, 0.0, y, N);
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime()-time1;
    if(mpi_rank == 0)
    {
        printf("TIME FOR 10 BLRM MATVECS: %e secs\n", time1);
    }
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime();
    for(int i = 0; i < 10; i++)
        starsh_blrm__dmml_mpi_tlr(M, nrhs, 1.0, x, N, 0.0, y_tlr, N);
    MPI_Barrier(MPI_COMM_WORLD);
    time1 = MPI_Wtime()-time1;
    if(mpi_rank == 0)
    {
        cblas_daxpy(N, -1.0, y, 1, y_tlr, 1);
        printf("TIME FOR 10 TLR MATVECS: %e secs\n", time1);
        printf("MATVEC DIFF: %e\n", cblas_dnrm2(N, y_tlr, 1)
                /cblas_dnrm2(N, y, 1));
    }
    MPI_Finalize();
    return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <float.h>
#include <math.h>
#include <gsl/gsl_interp.h>
#include "field.h"

#define StartTest -4E-7
#define StopTest 4E-7

signal_type *
formatExpImpResp(int numPnts, double *timeValues, double *voltageValues,
	int samplingFrequencyHz, int verbose)
{
double *newTimeValues;
double *newVoltageValues;
double timeAtMaxIntensity;
double minTime = DBL_MIN;
double maxTime = -DBL_MIN;
double maxVoltage = -DBL_MIN;
double stepSize;
int i, j, maxVoltageIndex, numSteps;
int startIndex, stopIndex;
signal_type *impulseResponse;
int pntsSampled;

/* find the max voltage */

	for (i = 0; i < numPnts; i++) {
		if (maxVoltage < voltageValues[i]) {
			maxVoltage = voltageValues[i];
			maxVoltageIndex = i;
			}
		}

	if (verbose >= 1) fprintf(stderr, "index %d max voltage %e\n",
		maxVoltageIndex, maxVoltage);

/* normalize the voltages */

	for (i = 0; i < numPnts; i++)
		voltageValues[i] /= maxVoltage;

/*
 * center the time axis around the max intensity. the matlab code recalculated
 * the index of the maximum value, but the normalization shouldn't change
 * that, so I skipped it. note that I have to save the time associated with
 * the max intensity, because in the time array, it goes to 0 as I do the
 * centering.
 */

	timeAtMaxIntensity = timeValues[maxVoltageIndex];

	for (i = 0; i < numPnts; i++)
		timeValues[i] = timeValues[i] - timeAtMaxIntensity;

/* now find the min and max times */

	for (i = 0; i < numPnts; i++) {
		if (minTime > timeValues[i]) minTime = timeValues[i];

		if (maxTime < timeValues[i]) maxTime = timeValues[i];
		}

	if (verbose >= 1) fprintf(stderr, "min time %e max time %e\n", minTime,
		maxTime);

/* re-sample the data to match the Field II sampling frequency */

	if (verbose >= 1) fprintf(stderr, "sampling %d\n", samplingFrequencyHz);
	stepSize = 1.0 / samplingFrequencyHz;

	if (verbose >= 1) fprintf(stderr, "diff %e\n", maxTime - minTime);
	numSteps = (int )ceil(((maxTime - minTime) * samplingFrequencyHz));

	if (verbose >= 1) fprintf(stderr, "step size %e numSteps %d\n", stepSize, numSteps);

	if ((newTimeValues = (double *)malloc(sizeof(double) * numSteps)) == NULL) {
		fprintf(stderr, "in readExpData, couldn't allocate space for new times\n");
		return(0);
		}

	for (i = 0; i < numSteps; i++) {
		newTimeValues[i] = minTime + i * stepSize;
		}

/*
 * now we find the voltage values at each of the new times by interpolating
 * from the old times and voltages.
 */

/* initialize and allocate the gsl objects */

	gsl_interp *interpolation = gsl_interp_alloc (gsl_interp_linear, numPnts);

	gsl_interp_init(interpolation, timeValues, voltageValues, numPnts);

	gsl_interp_accel * accelerator =  gsl_interp_accel_alloc();

/* get interpolation the new times */

	if ((newVoltageValues = (double *)malloc(sizeof(double) * numSteps)) == NULL) {
		fprintf(stderr, "in readExpData, couldn't allocate space for new voltage\n");
		return(0);
		}

	for (i = 0; i < numSteps; i++) {
		newVoltageValues[i] = gsl_interp_eval(interpolation, timeValues,
			voltageValues, newTimeValues[i], accelerator);

		if (verbose >= 3) fprintf(stderr, "\n%e", newVoltageValues[i]);
		}

/* find the indices of NewTime to use for the Field II impulse response */

	for (i = 0; i < numSteps; i++) {
		if (newTimeValues[i] > StartTest) {
			startIndex = i;
			break;
			}
		}

	for (i = 0; i < numSteps; i++)
		if (newTimeValues[i] > StopTest) {
			stopIndex = i;
			break;
			}

	pntsSampled = stopIndex - startIndex;

	if (verbose >= 1) fprintf(stderr,
		"\nstartIndex %d, stopIndex %d pntsSampled %d\n", startIndex,
		stopIndex, pntsSampled);

	impulseResponse = alloc_signal(pntsSampled, 0);

	for (i = startIndex, j = 0; i < stopIndex; i++, j++) {
		impulseResponse->data[j] = newVoltageValues[i];
		if (verbose >= 3) fprintf(stderr, "%e\n", newVoltageValues[i]);
		}

	return(impulseResponse);
}