/*
Performing approximate Bayesian computation sequential Monte Carlo (Toni et al.
2009) for linear regression.

Synthetic data is generated by `ground_truth_and_analysis.ipynb`.

This script writes particles.csv, where each row corresponds to a particle and
each column corresponds to a round of SMC.

Defining #DEBUG_MODE will silence all writing to stdout. One may then add
printf statements in the code, and perhaps write the output to file as:
`./run.sh > output.txt`

Parameter ordering convention:
0 - gradient
1 - intercept
2 - standard deviation

Author: Juvid Aryaman
*/

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

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_fit.h>

#define N_DATA 30
#define N_PARAMETERS 3

#define N_PARTICLES 2000
#define N_ROUNDS_SMC 25
#define QUANTILE_ACCEPT_DISTANCE 0.8

#define RND gsl_rng_uniform(r)
#define SEED 1
#define DISTANCE_THRESHOLD_INIT_GRADIENT 2
#define DISTANCE_THRESHOLD_INIT_INTERCEPT 50
#define DISTANCE_THRESHOLD_INIT_SIGMA 2

#define X_DATA_FILENAME "x.csv"
#define Y_DATA_FILENAME "y.csv"

//#define DEBUG_MODE

#include "smc.h"
#include "lin_reg.h"

int main(int argc, char *argv[]) {

/* set up GSL RNG */
gsl_rng *r = gsl_rng_alloc(gsl_rng_mt19937);
/* end of GSL setup */

gsl_rng_set(r, SEED);

/////////////////////////
/*Read data*/
/////////////////////////

FILE *data_pointer_x, *data_pointer_y;

data_pointer_x = fopen(X_DATA_FILENAME, "r");
data_pointer_y = fopen(Y_DATA_FILENAME, "r");

double data_x[N_DATA];
double data_y[N_DATA];
int i, j, read_error_status_x, read_error_status_y;
for (i=0; i < N_DATA; i++){
	read_error_status_x = fscanf(data_pointer_x, "%lf\n", &data_x[i]);
	read_error_status_y = fscanf(data_pointer_y, "%lf\n", &data_y[i]);
}
if (read_error_status_x != 1){printf("Error reading X data\n"); return 0;}
if (read_error_status_y != 1){printf("Error reading Y data\n"); return 0;}

/////////////////////////
/*Initialise variables*/
/////////////////////////

/*Fit a linear model to the data, which will be used as summary statistics of
the data*/
double gradient_fit_data, intercept_fit_data, sigma_fit_data, cov00, cov01, cov11, sumsq;
int gsl_fit_return_value;
gsl_fit_return_value = gsl_fit_linear(data_x, 1, data_y, 1, N_DATA,
																			&intercept_fit_data, &gradient_fit_data,
																			&cov00, &cov01, &cov11, &sumsq);
if (gsl_fit_return_value != 0) {printf("Fit failed.\n"); return -1;}
sigma_fit_data = sqrt(sumsq/(N_DATA-2));

#ifndef DEBUG_MODE
	printf("gradient ML = %.8f\n", gradient_fit_data);
	printf("intercept ML = %.8f\n", intercept_fit_data);
	printf("sigma ML = %.8f\n", sigma_fit_data);
#endif

/*Make a (N_PARAMETERS X N_ROUNDS_SMC X N_PARTICLES) array to store all
particles at all rounds of SMC*/
double*** theta_particle;
theta_particle = (double***) malloc(N_PARAMETERS * sizeof(double**));
for (i = 0; i < N_PARAMETERS; i++){
	theta_particle[i] = (double**) malloc(N_ROUNDS_SMC * sizeof(double*));
}
for (i = 0; i < N_PARAMETERS; i++){
	for (j = 0; j < N_ROUNDS_SMC; j++){
		theta_particle[i][j] = (double*) malloc(N_PARTICLES * sizeof(double));
	}
}

double** distance;
distance = (double**) malloc(N_PARAMETERS * sizeof(double*));
for (i = 0; i < N_PARAMETERS; i++) {
	distance[i] = (double*) malloc(N_PARTICLES * sizeof(double));
}
double **distance_threshold_all = malloc(N_PARAMETERS * sizeof(double*));
for (i = 0; i < N_PARAMETERS; i++) {
	distance_threshold_all[i] = (double*) malloc(N_ROUNDS_SMC * sizeof(double));
}

double distance_threshold[] = {DISTANCE_THRESHOLD_INIT_GRADIENT,
															 DISTANCE_THRESHOLD_INIT_INTERCEPT,
														   DISTANCE_THRESHOLD_INIT_SIGMA};

double *simulated_data = (double*) malloc(N_DATA * sizeof(double));
double *weight = malloc(N_PARTICLES * sizeof(double));

double weight_normalizer = 0.0;

int time_smc=0; // an index of each round of SMC
int param_index_chosen, prior_violated;
int particle_index;
/////////////////////////
/*Perform ABC SMC*/
/////////////////////////

/*For every round of SMC*/
for (time_smc = 0; time_smc < N_ROUNDS_SMC; time_smc++) {
	#ifndef DEBUG_MODE
		printf("Round %d of SMC\n", time_smc);
	#endif
	for (i = 0; i < N_PARAMETERS; i++) {
		distance_threshold_all[i][time_smc] = distance_threshold[i];
	}

	/*Draw or perturb a particle and compute distance*/
	for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
		// printf("%d\n", particle_index);
		for (i = 0; i < N_PARAMETERS; i++) {
			// reset distance of particle along each dimension
			distance[i][particle_index] = distance_threshold[i] + 1.0;
		}
		while ((distance[0][particle_index] > distance_threshold[0])||
					 (distance[1][particle_index] > distance_threshold[1])||
				   (distance[2][particle_index] > distance_threshold[2])) {
			if (time_smc == 0) {
				// Sample from the prior
				sample_prior(r, theta_particle, particle_index);
			}
			else{
				/*Sample from the old weights*/
				param_index_chosen = weighted_choice(r, weight);
				if ((param_index_chosen < 0)||(param_index_chosen >= N_PARTICLES)) {
					printf("Error in param_index_chosen\n");
					printf("time_smc = %d\n", time_smc);
					return -1;
				}

				perturb_particle(r, theta_particle, time_smc, param_index_chosen,
												 particle_index);

				// Check if prior support is 0
				prior_violated = check_prior_violated(theta_particle, time_smc,
																							particle_index);
				if(prior_violated == 1) continue;
				}

			simulate_dataset(r, theta_particle, data_x, simulated_data, time_smc,
											 particle_index);

			// Compute distance between data and simulation
			distance_metric_sum_stats(simulated_data,
															 data_x,
															 gradient_fit_data,
															 intercept_fit_data,
															 sigma_fit_data,
														   distance, particle_index);
			//distance[particle_index] = distance_metric_sum_res(simulated_data, data_y);
			// distance[particle_index] = distance_metric_sum_abs_res(simulated_data, data_y);

		}
	}

	#ifndef DEBUG_MODE
		printf("Particles sampled.\n");
	#endif

	/*Compute weights*/
	if (time_smc==0){ for (i = 0; i < N_PARTICLES; i++) weight[i] = 1.0;}
	else{
		weight_normalizer = 0.0;
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			weight_normalizer +=  weight[particle_index]*kernel_pdf();
		}
		// print_double_array(weight, N_PARTICLES);
		// printf("\n" );
		// printf("weight_normalizer=%f\n", weight_normalizer);
		// printf("kernel_pdf()=%f\n", kernel_pdf());
		for (particle_index = 0; particle_index < N_PARTICLES; particle_index++) {
			weight[particle_index] = prior_pdf(theta_particle, time_smc,
																				 particle_index)/weight_normalizer;
		}

	}

	/*Normalise weights*/
	weight_normalizer = 0.0;
	for (i = 0; i < N_PARTICLES; i++)	weight_normalizer += weight[i];
	for (i = 0; i < N_PARTICLES; i++)	weight[i] = weight[i]/weight_normalizer;

	/* Resample weights*/
	distance_threshold[0] = update_distance_threshold(distance[0]);
	distance_threshold[1] = update_distance_threshold(distance[1]);
	distance_threshold[2] = update_distance_threshold(distance[2]);

}

#ifndef DEBUG_MODE
	printf("Writing particles to file\n");
#endif
	write_particles_to_csv(theta_particle);

	char *dist_filename = "distances.txt";
	write_2d_double_array_to_csv(distance_threshold_all, N_PARAMETERS, N_ROUNDS_SMC, dist_filename);
#ifndef DEBUG_MODE
	printf("Done!\n");
#endif

return 0; //return from main
} //close main


/* -----------------------------------------------------------------------------
  JAM_AXI_VEL_LOSINT

    Calculates integrand for line-of-sight integral required for first moment
    calculation.

    INPUTS
      zp     : line-of-sight coordinate z' (integration variable)
      params : function parameters passed as a structure

    NOTES
      * Based on janis1_jeans_mge_los_integrand IDL code by Michele Cappellari.

  Laura L Watkins [lauralwatkins@gmail.com]

  This code is released under a BSD 2-clause license.
  If you use this code for your research, please cite:
  Watkins et al. 2013, MNRAS, 436, 2598
  "Discrete dynamical models of omega Centauri"
  http://adsabs.harvard.edu/abs/2013MNRAS.436.2598W
----------------------------------------------------------------------------- */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_integration.h>
#include <gsl/gsl_errno.h>
#include "jam.h"
#include "../mge/mge.h"

double jam_axi_vel_losint(double zp, void *params) {

    struct params_losint *lp;
    struct params_mgeint mp;
    double xp, yp, si, ci, r, z, r2, z2, nu, intg, result, error, nu_i;
    double sign_kappa, sum;
    int i;

    // get parameters
    lp = params;
    xp = lp->xp;
    yp = lp->yp;

    // if the integrationFlag is already set, do not proceed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // intrinsic R and z
    si = sin(lp->incl);
    ci = cos(lp->incl);
    r = sqrt(pow(zp*si - yp*ci, 2) + pow(xp, 2));                   // eqn 25
    z = sqrt(pow(zp*ci + yp*si, 2));

    // do some prep for the integrand to avoid repeat calculations
    r2 = r * r;
    z2 = z * z;

    // parameters for integrand function
    mp.r2 = r2;
    mp.z2 = z2;
    mp.pot = lp->pot;
    mp.s2p = lp->s2p;
    mp.e2p = lp->e2p;

    // perform integration
    gsl_integration_workspace *w = gsl_integration_workspace_alloc(1000);
    gsl_set_error_handler_off();
    gsl_function F;
    F.function = &jam_axi_vel_mgeint;

    sum = 0.;
    for (i=0; i<lp->lum->ntotal; i++) {
        if (lp->kappa[i]==0.) sign_kappa = 0.;
        else sign_kappa = lp->kappa[i]/fabs(lp->kappa[i]);
        nu_i = lp->lum->area[i] * exp(-0.5/lp->s2l[i]*(r2+z2/lp->q2l[i]));

        mp.bani = lp->bani[i];
        mp.s2l = lp->s2l[i];
        mp.q2l = lp->q2l[i];
        mp.s2q2l = lp->s2q2l[i];
        F.params = ∓
        *lp->integrationFlag += gsl_integration_qag(&F, 0., 1., 0., 1e-5,
            1000, 6, w, &result, &error);
        sum += sign_kappa * pow(lp->kappa[i], 2) * nu_i * fabs(result);
    }

    gsl_integration_workspace_free(w);

    // check if the integration failed
    if (*lp->integrationFlag!=0) {
        return 0.;
    }

    // mge volume density
    nu = mge_dens(lp->lum, r, z);

    // keep track of kappa signs - see note 8 p77 of Cappellari 2008
    intg = nu*sum/fabs(nu*sum)*sqrt(fabs(nu*sum));

    intg *= pow(zp, lp->zpow);

    return intg;

}


/* roots/brent.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Reid Priedhorsky, 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.
 */

/* brent.c -- brent root finding algorithm */

#include <config.h>

#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <float.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_roots.h>

#include "roots.h"

typedef struct
  {
    double a, b, c, d, e;
    double fa, fb, fc;
  }
brent_state_t;

static int brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper);
static int brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper);

static int
brent_init (void * vstate, gsl_function * f, double * root, double x_lower, double x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double f_lower, f_upper ;

  *root = 0.5 * (x_lower + x_upper) ;

  SAFE_FUNC_CALL (f, x_lower, &f_lower);
  SAFE_FUNC_CALL (f, x_upper, &f_upper);

  state->a = x_lower;
  state->fa = f_lower;

  state->b = x_upper;
  state->fb = f_upper;

  state->c = x_upper;
  state->fc = f_upper;

  state->d = x_upper - x_lower ;
  state->e = x_upper - x_lower ;

  if ((f_lower < 0.0 && f_upper < 0.0) || (f_lower > 0.0 && f_upper > 0.0))
    {
      GSL_ERROR ("endpoints do not straddle y=0", GSL_EINVAL);
    }

  return GSL_SUCCESS;

}

static int
brent_iterate (void * vstate, gsl_function * f, double * root, double * x_lower, double * x_upper)
{
  brent_state_t * state = (brent_state_t *) vstate;

  double tol, m;

  int ac_equal = 0;

  double a = state->a, b = state->b, c = state->c;
  double fa = state->fa, fb = state->fb, fc = state->fc;
  double d = state->d, e = state->e;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      ac_equal = 1;
      c = a;
      fc = fa;
      d = b - a;
      e = b - a;
    }

  if (fabs (fc) < fabs (fb))
    {
      ac_equal = 1;
      a = b;
      b = c;
      c = a;
      fa = fb;
      fb = fc;
      fc = fa;
    }

  tol = 0.5 * GSL_DBL_EPSILON * fabs (b);
  m = 0.5 * (c - b);

  if (fb == 0)
    {
      *root = b;
      *x_lower = b;
      *x_upper = b;

      return GSL_SUCCESS;
    }

  if (fabs (m) <= tol)
    {
      *root = b;

      if (b < c)
        {
          *x_lower = b;
          *x_upper = c;
        }
      else
        {
          *x_lower = c;
          *x_upper = b;
        }

      return GSL_SUCCESS;
    }

  if (fabs (e) < tol || fabs (fa) <= fabs (fb))
    {
      d = m;            /* use bisection */
      e = m;
    }
  else
    {
      double p, q, r;   /* use inverse cubic interpolation */
      double s = fb / fa;

      if (ac_equal)
        {
          p = 2 * m * s;
          q = 1 - s;
        }
      else
        {
          q = fa / fc;
          r = fb / fc;
          p = s * (2 * m * q * (q - r) - (b - a) * (r - 1));
          q = (q - 1) * (r - 1) * (s - 1);
        }

      if (p > 0)
        {
          q = -q;
        }
      else
        {
          p = -p;
        }

      if (2 * p < GSL_MIN (3 * m * q - fabs (tol * q), fabs (e * q)))
        {
          e = d;
          d = p / q;
        }
      else
        {
          /* interpolation failed, fall back to bisection */

          d = m;
          e = m;
        }
    }

  a = b;
  fa = fb;

  if (fabs (d) > tol)
    {
      b += d;
    }
  else
    {
      b += (m > 0 ? +tol : -tol);
    }

  SAFE_FUNC_CALL (f, b, &fb);

  state->a = a ;
  state->b = b ;
  state->c = c ;
  state->d = d ;
  state->e = e ;
  state->fa = fa ;
  state->fb = fb ;
  state->fc = fc ;

  /* Update the best estimate of the root and bounds on each
     iteration */

  *root = b;

  if ((fb < 0 && fc < 0) || (fb > 0 && fc > 0))
    {
      c = a;
    }

  if (b < c)
    {
      *x_lower = b;
      *x_upper = c;
    }
  else
    {
      *x_lower = c;
      *x_upper = b;
    }

  return GSL_SUCCESS ;
}

static const gsl_root_fsolver_type brent_type =
{"brent",                               /* name */
 sizeof (brent_state_t),
 &brent_init,
 &brent_iterate};

const gsl_root_fsolver_type  * gsl_root_fsolver_brent = &brent_type;


#include <ceed.h>
#include <petsc.h>
#include "../problems/mooney-rivlin.h"

// Build libCEED context object
PetscErrorCode PhysicsContext_MR(MPI_Comm comm, Ceed ceed, Units *units,
                                 CeedQFunctionContext *ctx) {
  PetscErrorCode ierr;
  Physics_MR phys;

  PetscFunctionBegin;

  ierr = PetscMalloc1(1, units); CHKERRQ(ierr);
  ierr = PetscMalloc1(1, &phys); CHKERRQ(ierr);
  ierr = ProcessPhysics_MR(comm, phys, *units); CHKERRQ(ierr);
  CeedQFunctionContextCreate(ceed, ctx);
  CeedQFunctionContextSetData(*ctx, CEED_MEM_HOST, CEED_COPY_VALUES,
                              sizeof(*phys), phys);
  ierr = PetscFree(phys); CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

// Build libCEED smoother context object
PetscErrorCode PhysicsSmootherContext_MR(MPI_Comm comm, Ceed ceed,
    CeedQFunctionContext ctx, CeedQFunctionContext *ctx_smoother) {
  PetscErrorCode ierr;
  PetscScalar nu_smoother = 0;
  PetscBool nu_flag = PETSC_FALSE;
  Physics_MR phys, phys_smoother;

  PetscFunctionBegin;

  ierr = PetscOptionsBegin(comm, NULL,
                           "Mooney Rivlin physical parameters for smoother", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-nu_smoother", "Poisson's ratio for smoother",
                            NULL, nu_smoother, &nu_smoother, &nu_flag);
  CHKERRQ(ierr);
  if (nu_smoother < 0 ||
      nu_smoother >= 0.5) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                    "Mooney-Rivlin model requires Poisson ratio -nu option in [0, .5)");

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  if (nu_flag) {
    // Copy context
    CeedQFunctionContextGetData(ctx, CEED_MEM_HOST, &phys);
    ierr = PetscMalloc1(1, &phys_smoother); CHKERRQ(ierr);
    ierr = PetscMemcpy(phys_smoother, phys, sizeof(*phys)); CHKERRQ(ierr);
    CeedQFunctionContextRestoreData(ctx, &phys);
    // Create smoother context
    CeedQFunctionContextCreate(ceed, ctx_smoother);
    phys_smoother->lambda = 2 * (phys_smoother->mu_1 + phys_smoother->mu_2) *
                            nu_smoother / (1 - 2*nu_smoother);
    CeedQFunctionContextSetData(*ctx_smoother, CEED_MEM_HOST, CEED_COPY_VALUES,
                                sizeof(*phys_smoother), phys_smoother);
    ierr = PetscFree(phys_smoother); CHKERRQ(ierr);
  } else {
    *ctx_smoother = NULL;
  }

  PetscFunctionReturn(0);
}

// Process physics options - Mooney-Rivlin
PetscErrorCode ProcessPhysics_MR(MPI_Comm comm, Physics_MR phys, Units units) {
  PetscErrorCode ierr;
  PetscReal nu = -1;
  phys->mu_1 = -1;
  phys->mu_2 = -1;
  phys->lambda = -1;
  units->meter     = 1;        // 1 meter in scaled length units
  units->second    = 1;        // 1 second in scaled time units
  units->kilogram  = 1;        // 1 kilogram in scaled mass units

  PetscFunctionBeginUser;

  ierr = PetscOptionsBegin(comm, NULL, "Mooney Rivlin physical parameters", NULL);
  CHKERRQ(ierr);

  ierr = PetscOptionsScalar("-mu_1", "Material Property mu_1", NULL,
                            phys->mu_1, &phys->mu_1, NULL); CHKERRQ(ierr);
  if (phys->mu_1 < 0) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                "Mooney-Rivlin model requires non-negative -mu_1 option (Pa)");

  ierr = PetscOptionsScalar("-mu_2", "Material Property mu_2", NULL,
                            phys->mu_2, &phys->mu_2, NULL); CHKERRQ(ierr);
  if (phys->mu_2 < 0) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                "Mooney-Rivlin model requires non-negative -mu_2 option (Pa)");

  ierr = PetscOptionsScalar("-nu", "Poisson ratio", NULL,
                            nu, &nu, NULL); CHKERRQ(ierr);
  if (nu < 0 || nu >= 0.5) SETERRQ(PETSC_COMM_SELF, PETSC_ERR_SUP,
                                     "Mooney-Rivlin model requires Poisson ratio -nu option in [0, .5)");
  phys->lambda = 2 * (phys->mu_1 + phys->mu_2) * nu / (1 - 2*nu);

  ierr = PetscOptionsScalar("-units_meter", "1 meter in scaled length units",
                            NULL, units->meter, &units->meter, NULL);
  CHKERRQ(ierr);
  units->meter = fabs(units->meter);

  ierr = PetscOptionsScalar("-units_second", "1 second in scaled time units",
                            NULL, units->second, &units->second, NULL);
  CHKERRQ(ierr);
  units->second = fabs(units->second);

  ierr = PetscOptionsScalar("-units_kilogram", "1 kilogram in scaled mass units",
                            NULL, units->kilogram, &units->kilogram, NULL);
  CHKERRQ(ierr);
  units->kilogram = fabs(units->kilogram);

  ierr = PetscOptionsEnd(); CHKERRQ(ierr); // End of setting Physics

  // Define derived units
  units->Pascal = units->kilogram / (units->meter * PetscSqr(units->second));

  // Scale material parameters based on units of Pa
  phys->mu_1 *= units->Pascal;
  phys->mu_2 *= units->Pascal;
  phys->lambda *= units->Pascal;

  PetscFunctionReturn(0);
};


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_fit.h>

#include "cosmocalc.h"

double transfer_function(double k)
{
  static int initFlag = 1;
  static int currCosmoNum;
  static gsl_spline *cosmocalc_transfer_function_spline = NULL;
  static gsl_interp_accel *cosmocalc_transfer_function_acc = NULL;
  static double c0,c1;

  double transfer_function_table[COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH];
  double k_table[COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH];
  long i;
  double cov00,cov01,cov11,sumsq;

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

      for(i=0;i<COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH;++i)
        {
          k_table[i] = log(K_MAX/K_MIN)/(COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH-1.0)*((double) i) + log(K_MIN);
          if(cosmoData.useSmoothTransFunc)
	    transfer_function_table[i] = log(transfunct_eh98_smooth(exp(k_table[i])));
	  else if(cosmoData.useNoTransFunc)
	    transfer_function_table[i] = 0.0;
	  else
	    transfer_function_table[i] = log(transfunct_eh98(exp(k_table[i])));
        }

      //init the spline and accelerators
      if(cosmocalc_transfer_function_spline != NULL)
        gsl_spline_free(cosmocalc_transfer_function_spline);
      cosmocalc_transfer_function_spline = gsl_spline_alloc(GSL_SPLINE_TYPE,(size_t) (COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH));
      gsl_spline_init(cosmocalc_transfer_function_spline,k_table,transfer_function_table,(size_t) (COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH));
      if(cosmocalc_transfer_function_acc != NULL)
        gsl_interp_accel_reset(cosmocalc_transfer_function_acc);
      else
        cosmocalc_transfer_function_acc = gsl_interp_accel_alloc();

      gsl_fit_linear(k_table+COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH-COSMOCALC_TRANSFER_FUNCTION_FIT_LENGTH,(size_t) 1,
                     transfer_function_table+COSMOCALC_TRANSFER_FUNCTION_TABLE_LENGTH-COSMOCALC_TRANSFER_FUNCTION_FIT_LENGTH,(size_t) 1,
                     (size_t) COSMOCALC_TRANSFER_FUNCTION_FIT_LENGTH,&c0,&c1,&cov00,&cov01,&cov11,&sumsq);
    }

  if(k < K_MIN)
    return 1.0;
  else if(k < K_MAX)
    return exp(gsl_spline_eval(cosmocalc_transfer_function_spline,log(k),cosmocalc_transfer_function_acc));
  else
    return exp(c0+c1*log(k));
}

#define USEPAPER_EH98

double transfunct_eh98(double kin)
{
  //vars
  double k;
  double Tk,Tb,Tc,T0t,T0t11,T0t1bc;
  double omb,om0,omc,h;
  double theta2p7,f,ac,bc,s,q;
  double C,C11,C1bc,st,bb,ab,ksilk,bnode,Gy,y,zeq;
  double a1,a2,b1,b2,keq,Rd,zd,Req,b1d,b2d;

  //get cosmoparms
  omb = cosmoData.OmegaB;
  om0 = cosmoData.OmegaM;
  omc = cosmoData.OmegaM - cosmoData.OmegaB;
  h = cosmoData.h;

  //convert k from hMpc^-1 to Mpc^-1
  k = kin*h;

  //-----------
  //input parms
  //-----------
  theta2p7 = TCMB/2.7;

  //eqn 2
  zeq = 2.50e4*om0*h*h/(theta2p7*theta2p7*theta2p7*theta2p7);

  //eqn 3
  keq = 7.46e-2*om0*h*h/(theta2p7*theta2p7); //Mpc^-{1} (NOT h/Mpc)

  //eqn 4
  b1d = 0.313*pow(om0*h*h,-0.419)*(1.0 + 0.607*pow(om0*h*h,0.674));
  b2d = 0.238*pow(om0*h*h,0.223);
  zd = 1291.0*pow(om0*h*h,0.251)/(1.0 + 0.659*pow(om0*h*h,0.828))
    *(1.0 + b1d*pow(omb*h*h,b2d));

  //eqn 5
#ifdef USEPAPER_EH98
  Rd = 31.5*omb*h*h
    /(theta2p7*theta2p7*theta2p7*theta2p7)
    /((zd)/1e3); //matches wayne's paper, but not the code (zd -> 1+zd)
  //fprintf(stderr,"paper eh 98 1\n");
#else
  Rd = 31.5*omb*h*h
    /(theta2p7*theta2p7*theta2p7*theta2p7)
    /((1.0+zd)/1e3);
#endif
Req = 31.5*omb*h*h/(theta2p7*theta2p7*theta2p7*theta2p7)/(zeq/1e3);

  //eqn 6
  s = 2.0/3.0/keq*sqrt(6.0/Req)*
    log((sqrt(1.0 + Rd) + sqrt(Rd + Req))/(1.0 + sqrt(Req)));

  //eqn 7
  ksilk = 1.6*pow(omb*h*h,0.52)*pow(om0*h*h,0.73)*(1.0 + pow(10.4*om0*h*h,-0.95));

  //eqn 10
  q = k/13.41/keq;

  //eqn 11
  a1 = pow(46.9*om0*h*h,0.670)*(1.0 + pow(32.1*om0*h*h,-0.532));
  a2 = pow(12.0*om0*h*h,0.424)*(1.0 + pow(45.0*om0*h*h,-0.582));
  ac = pow(a1,-1.0*omb/om0)*pow(a2,-1.0*(omb/om0)*(omb/om0)*(omb/om0));

  //eqn 12
  b1 = 0.944/(1.0 + pow(458.0*om0*h*h,-0.708));
  b2 = pow(0.395*om0*h*h,-0.0266);
  bc = 1.0/(1.0 + b1*(pow(omc/om0,b2) - 1.0));

  //eqn 15
#ifdef USEPAPER_EH98
  y = (1.0 + zeq)/(1.0 + zd); //matches wayne's paper, but not the code (1+zeq -> zeq)
  //fprintf(stderr,"paper eh 98 2\n");
#else
  y = (zeq)/(1.0 + zd);
#endif
  Gy = y*(-6.0*sqrt(1.0 + y) + (2.0 + 3.0*y)*log((sqrt(1.0 + y) + 1.0)/(sqrt(1.0 + y) - 1.0)));

  //eqn 14
  ab = 2.07*keq*s*pow(1.0 + Rd,-3.0/4.0)*Gy;

  //----------------------------------
  // Get CDM part of transfer function
  //----------------------------------

  //eqn 18
  f = 1.0/(1.0 + (k*s/5.4)*(k*s/5.4)*(k*s/5.4)*(k*s/5.4));

  //eqn 20
  C = 14.2/ac + 386.0/(1.0 + 69.9*pow(q,1.08));

  //eqn 19
  T0t = log(M_E + 1.8*bc*q)/(log(M_E + 1.8*bc*q) + C*q*q);

  //eqn 17
  C1bc = 14.2 + 386.0/(1.0 + 69.9*pow(q,1.08));
  T0t1bc = log(M_E + 1.8*bc*q)/(log(M_E + 1.8*bc*q) + C1bc*q*q);
  Tc = f*T0t1bc + (1.0 - f)*T0t;

  //-------------------------------------
  // Get baryon part of transfer function
  //-------------------------------------

  //eqn 24
  bb = 0.5 + omb/om0 + (3.0 - 2.0*omb/om0)*sqrt((17.2*om0*h*h)*(17.2*om0*h*h) + 1.0);

  //eqn 23
  bnode = 8.41*pow(om0*h*h,0.435);

  //eqn 22
  st = s/pow(1.0 + (bnode/k/s)*(bnode/k/s)*(bnode/k/s),1.0/3.0);

  //eqn 21
  C11 = 14.2 + 386.0/(1.0 + 69.9*pow(q,1.08));
  T0t11 = log(M_E + 1.8*q)/(log(M_E + 1.8*q) + C11*q*q);
  Tb = (T0t11/(1.0 + (k*s/5.2)*(k*s/5.2))
	+ ab/(1.0 + (bb/k/s)*(bb/k/s)*(bb/k/s))/exp(pow(k/ksilk,1.4)))*sin(k*st)/(k*st);

  //------------------------
  // total transfer function
  //------------------------
  Tk = omb/om0*Tb + omc/om0*Tc;

  return Tk;
}

double transfunct_eh98_smooth(double kin)
{
  //vars
  double k,Tk;
  double omb,om0,omc,h;
  double theta2p7,s,q;
  double Gamma,alphaGamma,L0,C0;

  //get cosmoparms
  omb = cosmoData.OmegaB;
  om0 = cosmoData.OmegaM;
  omc = cosmoData.OmegaM - cosmoData.OmegaB;
  h = cosmoData.h;

  //convert k from hMpc^-1 to Mpc^-1
  k = kin*h;

  //-----------
  //input parms
  //-----------
  theta2p7 = TCMB/2.7;

  //eqn 26
  s = 44.5*log(9.83/om0/h/h)/sqrt(1.0 + 10.0*pow(omb*h*h,0.75));

  //eqn 31
  alphaGamma = 1.0 - 0.328*log(431.0*om0*h*h)*omb/om0 + 0.38*log(22.3*om0*h*h)*(omb/om0)*(omb/om0);

  //eqn 30
  Gamma = om0*h*(alphaGamma + (1.0 - alphaGamma)/(1.0 + pow(0.43*k*s,4.0)));

  //eqn 28
  q = kin*theta2p7*theta2p7/Gamma;

  //eqns 29
  C0 = 14.2 + 731.0/(1.0 + 62.5*q);
  L0 = log(2.0*exp(1.0) + 1.8*q);
  Tk = L0/(L0 + C0*q*q);

  return Tk;
}


/* linalg/balance.c
 *
 * Copyright (C) 2001 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., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Balance a general matrix by scaling the columns
 *
 * B =  A D
 *
 * where D is a diagonal matrix
 */

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

#include "gsl_linalg.h"

int
gsl_linalg_balance_columns (gsl_matrix * A, gsl_vector * D)
{
  const size_t N = A->size2;
  size_t j;

  if (D->size != A->size2)
    {
      GSL_ERROR("length of D must match second dimension of A", GSL_EINVAL);
    }

  gsl_vector_set_all (D, 1.0);

  for (j = 0; j < N; j++)
    {
      gsl_vector_view A_j = gsl_matrix_column (A, j);

      double s = gsl_blas_dasum(&A_j.vector);

      double f = 1.0;

      if (s == 0.0)
        {
          gsl_vector_set (D, j, f);
          continue;
        }

      while (s > 1.0)
        {
          s /= 2.0;
          f *= 2.0;
        }

      while (s < 0.5)
        {
          s *= 2.0;
          f /= 2.0;
        }

      gsl_vector_set (D, j, f);

      if (f != 1.0)
        {
          gsl_blas_dscal(1.0/f, &A_j.vector);
        }
    }

  return GSL_SUCCESS;
}


/**
 *
 * @precisions normal z -> c d s
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include <lapacke.h>
#include <plasma.h>
#include <core_blas.h>
#include "auxiliary.h"

/*-------------------------------------------------------------------
 * Check the orthogonality of Q
 */

int z_check_orthogonality(int M, int N, int LDQ, PLASMA_Complex64_t *Q)
{
    double alpha, beta;
    double normQ;
    int info_ortho;
    int i;
    int minMN = min(M, N);
    double eps;
    double *work = (double *)malloc(minMN*sizeof(double));

    eps = LAPACKE_dlamch_work('e');
    alpha = 1.0;
    beta  = -1.0;

    /* Build the idendity matrix USE DLASET?*/
    PLASMA_Complex64_t *Id = (PLASMA_Complex64_t *) malloc(minMN*minMN*sizeof(PLASMA_Complex64_t));
    memset((void*)Id, 0, minMN*minMN*sizeof(PLASMA_Complex64_t));
    for (i = 0; i < minMN; i++)
        Id[i*minMN+i] = (PLASMA_Complex64_t)1.0;

    /* Perform Id - Q'Q */
    if (M >= N)
        cblas_zherk(CblasColMajor, CblasUpper, CblasConjTrans, N, M, alpha, Q, LDQ, beta, Id, N);
    else
        cblas_zherk(CblasColMajor, CblasUpper, CblasNoTrans, M, N, alpha, Q, LDQ, beta, Id, M);

    normQ = LAPACKE_zlansy_work(LAPACK_COL_MAJOR, 'i', 'u', minMN, Id, minMN, work);

    printf("============\n");
    printf("Checking the orthogonality of Q \n");
    printf("||Id-Q'*Q||_oo / (N*eps) = %e \n",normQ/(minMN*eps));

    if ( isnan(normQ / (minMN * eps)) || (normQ / (minMN * eps) > 10.0) ) {
        printf("-- Orthogonality is suspicious ! \n");
        info_ortho=1;
    }
    else {
        printf("-- Orthogonality is CORRECT ! \n");
        info_ortho=0;
    }

    free(work); free(Id);

    return info_ortho;
}

/*------------------------------------------------------------
 *  Check the factorization QR
 */

int z_check_QRfactorization(int M, int N, PLASMA_Complex64_t *A1, PLASMA_Complex64_t *A2, int LDA, PLASMA_Complex64_t *Q)
{
    double Anorm, Rnorm;
    PLASMA_Complex64_t alpha, beta;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    PLASMA_Complex64_t *Ql       = (PLASMA_Complex64_t *)malloc(M*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *Residual = (PLASMA_Complex64_t *)malloc(M*N*sizeof(PLASMA_Complex64_t));
    double *work              = (double *)malloc(max(M,N)*sizeof(double));

    alpha=1.0;
    beta=0.0;

    if (M >= N) {
        /* Extract the R */
        PLASMA_Complex64_t *R = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
        memset((void*)R, 0, N*N*sizeof(PLASMA_Complex64_t));
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'u', M, N, A2, LDA, R, N);

        /* Perform Ql=Q*R */
        memset((void*)Ql, 0, M*N*sizeof(PLASMA_Complex64_t));
        cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, N, CBLAS_SADDR(alpha), Q, LDA, R, N, CBLAS_SADDR(beta), Ql, M);
        free(R);
    }
    else {
        /* Extract the L */
        PLASMA_Complex64_t *L = (PLASMA_Complex64_t *)malloc(M*M*sizeof(PLASMA_Complex64_t));
        memset((void*)L, 0, M*M*sizeof(PLASMA_Complex64_t));
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'l', M, N, A2, LDA, L, M);

    /* Perform Ql=LQ */
        memset((void*)Ql, 0, M*N*sizeof(PLASMA_Complex64_t));
        cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, N, M, CBLAS_SADDR(alpha), L, M, Q, LDA, CBLAS_SADDR(beta), Ql, M);
        free(L);
    }

    /* Compute the Residual */
    for (i = 0; i < M; i++)
        for (j = 0 ; j < N; j++)
            Residual[j*M+i] = A1[j*LDA+i]-Ql[j*M+i];

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, Residual, M, work);
    Anorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, A2, LDA, work);

    if (M >= N) {
        printf("============\n");
        printf("Checking the QR Factorization \n");
        printf("-- ||A-QR||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }
    else {
        printf("============\n");
        printf("Checking the LQ Factorization \n");
        printf("-- ||A-LQ||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));
    }

    if (isnan(Rnorm / (Anorm * N *eps)) || (Rnorm / (Anorm * N * eps) > 10.0) ) {
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else {
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(work); free(Ql); free(Residual);

    return info_factorization;
}

/*------------------------------------------------------------------------
 *  Check the factorization of the matrix A2
 */

int z_check_LLTfactorization(int N, PLASMA_Complex64_t *A1, PLASMA_Complex64_t *A2, int LDA, int uplo)
{
    double Anorm, Rnorm;
    PLASMA_Complex64_t alpha;
    int info_factorization;
    int i,j;
    double eps;

    eps = LAPACKE_dlamch_work('e');

    PLASMA_Complex64_t *Residual = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *L1       = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    PLASMA_Complex64_t *L2       = (PLASMA_Complex64_t *)malloc(N*N*sizeof(PLASMA_Complex64_t));
    double *work              = (double *)malloc(N*sizeof(double));

    memset((void*)L1, 0, N*N*sizeof(PLASMA_Complex64_t));
    memset((void*)L2, 0, N*N*sizeof(PLASMA_Complex64_t));

    alpha= 1.0;

    LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,' ', N, N, A1, LDA, Residual, N);

    /* Dealing with L'L or U'U  */
    if (uplo == PlasmaUpper){
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L1, N);
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'u', N, N, A2, LDA, L2, N);
        cblas_ztrmm(CblasColMajor, CblasLeft, CblasUpper, CblasConjTrans, CblasNonUnit, N, N, CBLAS_SADDR(alpha), L1, N, L2, N);
    }
    else{
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L1, N);
        LAPACKE_zlacpy_work(LAPACK_COL_MAJOR,'l', N, N, A2, LDA, L2, N);
        cblas_ztrmm(CblasColMajor, CblasRight, CblasLower, CblasConjTrans, CblasNonUnit, N, N, CBLAS_SADDR(alpha), L1, N, L2, N);
    }

    /* Compute the Residual || A -L'L|| */
    for (i = 0; i < N; i++)
        for (j = 0; j < N; j++)
           Residual[j*N+i] = L2[j*N+i] - Residual[j*N+i];

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', N, N, Residual, N, work);
    Anorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', N, N, A1, LDA, work);

    printf("============\n");
    printf("Checking the Cholesky Factorization \n");
    printf("-- ||L'L-A||_oo/(||A||_oo.N.eps) = %e \n",Rnorm/(Anorm*N*eps));

    if ( isnan(Rnorm/(Anorm*N*eps)) || (Rnorm/(Anorm*N*eps) > 10.0) ){
        printf("-- Factorization is suspicious ! \n");
        info_factorization = 1;
    }
    else{
        printf("-- Factorization is CORRECT ! \n");
        info_factorization = 0;
    }

    free(Residual); free(L1); free(L2); free(work);

    return info_factorization;
}

/*--------------------------------------------------------------
 * Check the gemm
 */
double z_check_gemm(PLASMA_enum transA, PLASMA_enum transB, int M, int N, int K,
                   PLASMA_Complex64_t alpha, PLASMA_Complex64_t *A, int LDA,
                   PLASMA_Complex64_t *B, int LDB,
                   PLASMA_Complex64_t beta, PLASMA_Complex64_t *Cplasma,
                   PLASMA_Complex64_t *Cref, int LDC,
                   double *Cinitnorm, double *Cplasmanorm, double *Clapacknorm )
{
    PLASMA_Complex64_t beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(K,max(M, N))* sizeof(double));

    *Cinitnorm   = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref,    LDC, work);
    *Cplasmanorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cplasma, LDC, work);

    cblas_zgemm(CblasColMajor, (CBLAS_TRANSPOSE)transA, (CBLAS_TRANSPOSE)transB, M, N, K,
                CBLAS_SADDR(alpha), A, LDA, B, LDB, CBLAS_SADDR(beta), Cref, LDC);

    *Clapacknorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    cblas_zaxpy(LDC * N, CBLAS_SADDR(beta_const), Cplasma, 1, Cref, 1);

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N, Cref, LDC, work);

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the trsm
 */
double z_check_trsm(PLASMA_enum side, PLASMA_enum uplo, PLASMA_enum trans, PLASMA_enum diag,
                   int M, int NRHS, PLASMA_Complex64_t alpha,
                   PLASMA_Complex64_t *A, int LDA,
                   PLASMA_Complex64_t *Bplasma, PLASMA_Complex64_t *Bref, int LDB,
                   double *Binitnorm, double *Bplasmanorm, double *Blapacknorm )
{
    PLASMA_Complex64_t beta_const = -1.0;
    double Rnorm;
    double *work = (double *)malloc(max(M, NRHS)* sizeof(double));
    /*double eps = LAPACKE_dlamch_work('e');*/

    *Binitnorm   = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, Bref,    LDB, work);
    *Bplasmanorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bplasma, LDB, work);

    cblas_ztrsm(CblasColMajor, (CBLAS_SIDE)side, (CBLAS_UPLO)uplo,
                (CBLAS_TRANSPOSE)trans, (CBLAS_DIAG)diag, M, NRHS,
                CBLAS_SADDR(alpha), A, LDA, Bref, LDB);

    *Blapacknorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);

    cblas_zaxpy(LDB * NRHS, CBLAS_SADDR(beta_const), Bplasma, 1, Bref, 1);

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'm', M, NRHS, Bref, LDB, work);
    Rnorm = Rnorm / *Blapacknorm;
    /* max(M,NRHS) * eps);*/

    free(work);

    return Rnorm;
}

/*--------------------------------------------------------------
 * Check the solution
 */

double z_check_solution(int M, int N, int NRHS, PLASMA_Complex64_t *A, int LDA,
                      PLASMA_Complex64_t *B,  PLASMA_Complex64_t *X, int LDB,
                      double *anorm, double *bnorm, double *xnorm )
{
/*     int info_solution; */
    double Rnorm = -1.00;
    PLASMA_Complex64_t zone  =  1.0;
    PLASMA_Complex64_t mzone = -1.0;
    double *work = (double *)malloc(max(M, N)* sizeof(double));

    *anorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, N,    A, LDA, work);
    *xnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', M, NRHS, X, LDB, work);
    *bnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    cblas_zgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, M, NRHS, N, CBLAS_SADDR(zone), A, LDA, X, LDB, CBLAS_SADDR(mzone), B, LDB);

    Rnorm = LAPACKE_zlange_work(LAPACK_COL_MAJOR, 'i', N, NRHS, B, LDB, work);

    free(work);

    return Rnorm;
}


#include "parameters.h" // Includes all defined parameters
#include "axi.h" // Axisymmetric coordinates
#include "navier-stokes/centered.h" // To solve the Navier-Stokes
#include "two-phase.h" // Implements two-phase flow
// #include <omp.h> // For openMP parallel
#include <gsl/gsl_fit.h>

double * forces_array;
double * times_array;

double gradient = 0.5;
double intercept = 4.9;

int main () {

    forces_array = malloc(INTERP_NO * sizeof(double));
    times_array = malloc(INTERP_NO * sizeof(double));

    DT = 1e-2;

    run();

    free(forces_array);
    free(times_array);
}

event force(i++) {
    if (i < INTERP_NO) {
        forces_array[i] = gradient * t + intercept;
        times_array[i] = t;
    } else {
        double c0, c1, cov00, cov01, cov11, sumsq;
        gsl_fit_linear ( times_array, 1, forces_array, 1, INTERP_NO, &c0, &c1, \
            &cov00, &cov01, &cov11, &sumsq);

        fprintf(stderr, "t = %g, c0 = %g, c1 = %g\n", t, c0, c1);

        double current_force = gradient * t + intercept;
        double interp_force = c0 + c1 * t;
        fprintf(stderr, "f = %g, f_interp = %g", current_force, interp_force);
        fprintf(stderr, "\n");

        // #pragma omp critical
        for (int j = 0; j < INTERP_NO - 1; j++) {
            forces_array[j] = forces_array[j + 1];
            times_array[j] = times_array[j + 1];
        }
        forces_array[INTERP_NO - 1] = current_force;
        times_array[INTERP_NO - 1] = t;
    }
}

event end(t = 1) {
    fprintf(stderr, "Ended\n");
}


#include "ccv.h"
#include "ccv_internal.h"
#include "nnc/ccv_nnc.h"
#include "nnc/ccv_nnc_easy.h"
#include "nnc/ccv_nnc_internal.h"
#include "../_ccv_nnc_gemm_cpu_opt.h"
#if HAVE_ACCELERATE_FRAMEWORK
#include <Accelerate/Accelerate.h>
#elif HAVE_CBLAS
#include <cblas.h>
#endif

int _ccv_nnc_gemm_forw_cpu_sys(const int transpose_a[2], const int transpose_b[2], const ccv_nnc_tensor_view_t* const a, const ccv_nnc_tensor_view_t* const w, const ccv_nnc_tensor_view_t* const bias, ccv_nnc_tensor_view_t* const b)
{
#if (defined HAVE_CBLAS || defined HAVE_ACCELERATE_FRAMEWORK)
	assert(!bias || (bias->info.dim[1] == 0 || bias->info.dim[2] == 0 || bias->info.dim[3] == 0)); // It is a 1-d array
	int a_batch_size, a_rows, a_cols, a_batch_inc, a_rows_inc, a_cols_inc;
	int w_batch_size, w_rows, w_cols, w_batch_inc, w_rows_inc, w_cols_inc;
	int b_batch_size, b_rows, b_cols, b_batch_inc, b_rows_inc, b_cols_inc;
	const static int no_transpose[2] = {};
	ccv_nnc_tensor_get_matrix_params(a->info, CCV_IS_TENSOR_VIEW(a) ? a->inc : a->info.dim, transpose_a, &a_batch_size, &a_rows, &a_cols, &a_batch_inc, &a_rows_inc, &a_cols_inc);
	ccv_nnc_tensor_get_matrix_params(w->info, CCV_IS_TENSOR_VIEW(w) ? w->inc : w->info.dim, transpose_b, &w_batch_size, &w_rows, &w_cols, &w_batch_inc, &w_rows_inc, &w_cols_inc);
	ccv_nnc_tensor_get_matrix_params(b->info, CCV_IS_TENSOR_VIEW(b) ? b->inc : b->info.dim, no_transpose, &b_batch_size, &b_rows, &b_cols, &b_batch_inc, &b_rows_inc, &b_cols_inc);
	assert(a_batch_size == b_batch_size);
	assert(a_batch_size == b_batch_size || a_batch_size == 1);
	if (a_batch_size == 1 && b_batch_size > 1)
		a_batch_inc = 0;
	assert(w_batch_size == a_batch_size || w_batch_size == 1);
	if (w_batch_size == 1 && b_batch_size > 1)
		w_batch_inc = 0;
	assert(a_rows == b_rows);
	assert(a_cols == w_rows);
	assert(w_cols == b_cols);
	const int is_transpose_a = ccv_nnc_is_matrix_transpose(a->info, transpose_a);
	const int is_transpose_w = ccv_nnc_is_matrix_transpose(w->info, transpose_b);
	if (bias)
	{
		float* const ones = (float*)ccmalloc(sizeof(float) * b_rows);
		int i;
		for (i = 0; i < b_rows; i++)
			ones[i] = 1;
		int bias_batch_size, bias_rows, bias_cols, bias_batch_inc, bias_rows_inc, bias_cols_inc;
		ccv_nnc_tensor_get_matrix_params(bias->info, CCV_IS_TENSOR_VIEW(bias) ? bias->inc : bias->info.dim, no_transpose, &bias_batch_size, &bias_rows, &bias_cols, &bias_batch_inc, &bias_rows_inc, &bias_cols_inc);
		assert(bias_batch_size == b_batch_size || bias_batch_size == 1);
		if (bias_batch_size == 1 && b_batch_size > 1)
			bias_batch_inc = 0;
		assert(bias_cols == b_cols);
		for (i = 0; i < b_batch_size; i++)
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, b_cols, b_rows, 1, 1.0, bias->data.f32 + i * bias_batch_inc, bias_rows_inc, ones, 1, 0.0, b->data.f32 + i * b_batch_inc, b_rows_inc);
		ccfree(ones);
		const int transa = is_transpose_w ? CblasTrans : CblasNoTrans;
		const int transb = is_transpose_a ? CblasTrans : CblasNoTrans;
		const int lda_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
		const int ldb_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
		for (i = 0; i < b_batch_size; i++)
			cblas_sgemm(CblasColMajor, transa, transb, b_cols, b_rows, a_cols, 1.0, w->data.f32 + i * w_batch_inc, lda_inc, a->data.f32 + i * a_batch_inc, ldb_inc, 1.0, b->data.f32 + i * b_batch_inc, b_rows_inc);
	} else {
		const int transa = is_transpose_w ? CblasTrans : CblasNoTrans;
		const int transb = is_transpose_a ? CblasTrans : CblasNoTrans;
		const int lda_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
		const int ldb_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
		int i;
		for (i = 0; i < b_batch_size; i++)
			cblas_sgemm(CblasColMajor, transa, transb, b_cols, b_rows, a_cols, 1.0, w->data.f32 + i * w_batch_inc, lda_inc, a->data.f32 + i * a_batch_inc, ldb_inc, 0.0, b->data.f32 + i * b_batch_inc, b_rows_inc);
	}
	return CCV_NNC_EXEC_SUCCESS;
#else
	return CCV_NNC_EXEC_INVALID;
#endif
}

int _ccv_nnc_gemm_back_cpu_sys(const int transpose_a[2], const int transpose_b[2], const ccv_nnc_tensor_view_t* const g, const ccv_nnc_tensor_view_t* const a, const ccv_nnc_tensor_view_t* const w, ccv_nnc_tensor_view_t* const dw, ccv_nnc_tensor_view_t* const bias, ccv_nnc_tensor_view_t* const h, const int flags)
{
#if (defined HAVE_CBLAS || defined HAVE_ACCELERATE_FRAMEWORK)
	// inputs: gradient, forw prop input, [w]
	// outputs: [output gradient], weight updates, bias updates
	assert(!bias || (bias->info.dim[1] == 0 || bias->info.dim[2] == 0 || bias->info.dim[3] == 0)); // It is a 2-d or 3-d array.
	int g_batch_size, g_rows, g_cols, g_batch_inc, g_rows_inc, g_cols_inc;
	const static int no_transpose[2] = {};
	ccv_nnc_tensor_get_matrix_params(g->info, CCV_IS_TENSOR_VIEW(g) ? g->inc : g->info.dim, no_transpose, &g_batch_size, &g_rows, &g_cols, &g_batch_inc, &g_rows_inc, &g_cols_inc);
	int i;
	if (bias)
	{
		int bias_batch_size, bias_rows, bias_cols, bias_batch_inc, bias_rows_inc, bias_cols_inc;
		ccv_nnc_tensor_get_matrix_params(bias->info, CCV_IS_TENSOR_VIEW(bias) ? bias->inc : bias->info.dim, no_transpose, &bias_batch_size, &bias_rows, &bias_cols, &bias_batch_inc, &bias_rows_inc, &bias_cols_inc);
		assert(bias_cols == g_cols);
		assert(bias_batch_size == 1 || bias_batch_size == g_batch_size);
		if (bias_batch_size == 1 && g_batch_size > 1)
			bias_batch_inc = 0;
		float* const ones = (float*)ccmalloc(sizeof(float) * g_rows);
		for (i = 0; i < g_rows; i++)
			ones[i] = 1;
		if (g_batch_size > 1 && bias_batch_size == g_batch_size)
		{
			for (i = 0; i < g_batch_size; i++)
				cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, bias_cols, bias_rows, g_rows, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, ones, g_rows, 0.0, bias->data.f32 + i * bias_batch_inc, bias_rows_inc);
		} else {
			cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, bias_cols, bias_rows, g_rows, 1.0, g->data.f32, g_rows_inc, ones, g_rows, 0.0, bias->data.f32, bias_rows_inc);
			// We cannot use strided batched alternative because on write, the data could race to the same position
			for (i = 1; i < g_batch_size; i++)
				cblas_sgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, bias_cols, bias_rows, g_rows, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, ones, g_rows, 1.0, bias->data.f32, bias_rows_inc);
		}
		ccfree(ones);
	}
	if (dw)
	{
		const int is_transpose_a = ccv_nnc_is_matrix_transpose(a->info, transpose_a);
		const int is_transpose_w = ccv_nnc_is_matrix_transpose(dw->info, transpose_b);
		int a_batch_size, a_rows, a_cols, a_batch_inc, a_rows_inc, a_cols_inc;
		int dw_batch_size, dw_rows, dw_cols, dw_batch_inc, dw_rows_inc, dw_cols_inc;
		ccv_nnc_tensor_get_matrix_params(a->info, CCV_IS_TENSOR_VIEW(a) ? a->inc : a->info.dim, transpose_a, &a_batch_size, &a_rows, &a_cols, &a_batch_inc, &a_rows_inc, &a_cols_inc);
		ccv_nnc_tensor_get_matrix_params(dw->info, CCV_IS_TENSOR_VIEW(dw) ? dw->inc : dw->info.dim, transpose_b, &dw_batch_size, &dw_rows, &dw_cols, &dw_batch_inc, &dw_rows_inc, &dw_cols_inc);
		assert(a_rows == g_rows);
		assert(a_cols == dw_rows);
		assert(dw_cols == g_cols);
		assert(a_batch_size == g_batch_size || a_batch_size == 1);
		if (a_batch_size == 1 && g_batch_size > 1)
			a_batch_inc = 0;
		assert(dw_batch_size == g_batch_size || dw_batch_size == 1);
		if (dw_batch_size == 1 && g_batch_size > 1)
			dw_batch_inc = 0;
		if (g_batch_size > 1 && g_batch_size == dw_batch_size)
		{
			if (is_transpose_w)
			{
				const int transa = is_transpose_a ? CblasTrans : CblasNoTrans;
				const int lda_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
				for (i = 0; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, transa, CblasTrans, dw_rows, dw_cols, a_rows, 1.0, a->data.f32 + i * a_batch_inc, lda_inc, g->data.f32 + i * g_batch_inc, g_rows_inc, 0.0, dw->data.f32 + i * dw_batch_inc, dw_cols_inc);
			} else {
				const int transb = is_transpose_a ? CblasNoTrans : CblasTrans;
				const int ldb_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
				for (i = 0; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, CblasNoTrans, transb, dw_cols, dw_rows, a_rows, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, a->data.f32 + i * a_batch_inc, ldb_inc, 0.0, dw->data.f32 + i * dw_batch_inc, dw_rows_inc);
			}
		} else {
			if (is_transpose_w)
			{
				const int transa = is_transpose_a ? CblasTrans : CblasNoTrans;
				const int lda_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
				cblas_sgemm(CblasColMajor, transa, CblasTrans, dw_rows, dw_cols, a_rows, 1.0, a->data.f32, lda_inc, g->data.f32, g_rows_inc, 0.0, dw->data.f32, dw_cols_inc);
				for (i = 1; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, transa, CblasTrans, dw_rows, dw_cols, a_rows, 1.0, a->data.f32 + i * a_batch_inc, lda_inc, g->data.f32 + i * g_batch_inc, g_rows_inc, 1.0, dw->data.f32, dw_cols_inc);
			} else {
				const int transb = is_transpose_a ? CblasNoTrans : CblasTrans;
				const int ldb_inc = is_transpose_a ? a_cols_inc : a_rows_inc;
				cblas_sgemm(CblasColMajor, CblasNoTrans, transb, dw_cols, dw_rows, a_rows, 1.0, g->data.f32, g_rows_inc, a->data.f32, ldb_inc, 0.0, dw->data.f32, dw_rows_inc);
				for (i = 1; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, CblasNoTrans, transb, dw_cols, dw_rows, a_rows, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, a->data.f32 + i * a_batch_inc, ldb_inc, 1.0, dw->data.f32, dw_rows_inc);
			}
		}
	}
	if (h)
	{
		const int is_transpose_h = ccv_nnc_is_matrix_transpose(h->info, transpose_a);
		const int is_transpose_w = ccv_nnc_is_matrix_transpose(w->info, transpose_b);
		int h_batch_size, h_rows, h_cols, h_batch_inc, h_rows_inc, h_cols_inc;
		int w_batch_size, w_rows, w_cols, w_batch_inc, w_rows_inc, w_cols_inc;
		ccv_nnc_tensor_get_matrix_params(h->info, CCV_IS_TENSOR_VIEW(h) ? h->inc : h->info.dim, transpose_a, &h_batch_size, &h_rows, &h_cols, &h_batch_inc, &h_rows_inc, &h_cols_inc);
		ccv_nnc_tensor_get_matrix_params(w->info, CCV_IS_TENSOR_VIEW(w) ? w->inc : w->info.dim, transpose_b, &w_batch_size, &w_rows, &w_cols, &w_batch_inc, &w_rows_inc, &w_cols_inc);
		assert(h_rows == g_rows);
		assert(h_cols == w_rows);
		assert(w_cols == g_cols);
		assert(h_batch_size == g_batch_size || h_batch_size == 1);
		if (h_batch_size == 1 && g_batch_size > 1)
			h_batch_inc = 0;
		assert(w_batch_size == g_batch_size || w_batch_size == 1);
		if (w_batch_size == 1 && g_batch_size > 1)
			w_batch_inc = 0;
		if (g_batch_size > 1 && g_batch_size == h_batch_size)
		{
			if (is_transpose_h)
			{
				const int transb = is_transpose_w ? CblasTrans : CblasNoTrans;
				const int ldb_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
				for (i = 0; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, CblasTrans, transb, h_rows, h_cols, g_cols, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, w->data.f32 + i * w_batch_inc, ldb_inc, 0.0, h->data.f32 + i * h_batch_inc, h_cols_inc);
			} else {
				const int transa = is_transpose_w ? CblasNoTrans : CblasTrans;
				const int lda_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
				for (i = 0; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, transa, CblasNoTrans, h_cols, h_rows, g_cols, 1.0, w->data.f32 + i * w_batch_inc, lda_inc, g->data.f32 + i * g_batch_inc, g_rows_inc, 0.0, h->data.f32 + i * h_batch_inc, h_rows_inc);
			}
		} else {
			if (is_transpose_h)
			{
				const int transb = is_transpose_w ? CblasTrans : CblasNoTrans;
				const int ldb_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
				cblas_sgemm(CblasColMajor, CblasTrans, transb, h_rows, h_cols, g_cols, 1.0, g->data.f32, g_rows_inc, w->data.f32, ldb_inc, 0.0, h->data.f32, h_cols_inc);
				for (i = 1; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, CblasTrans, transb, h_rows, h_cols, g_cols, 1.0, g->data.f32 + i * g_batch_inc, g_rows_inc, w->data.f32 + i * w_batch_inc, ldb_inc, 1.0, h->data.f32, h_cols_inc);
			} else {
				const int transa = is_transpose_w ? CblasNoTrans : CblasTrans;
				const int lda_inc = is_transpose_w ? w_cols_inc : w_rows_inc;
				cblas_sgemm(CblasColMajor, transa, CblasNoTrans, h_cols, h_rows, g_cols, 1.0, w->data.f32, lda_inc, g->data.f32, g_rows_inc, 0.0, h->data.f32, h_rows_inc);
				for (i = 1; i < g_batch_size; i++)
					cblas_sgemm(CblasColMajor, transa, CblasNoTrans, h_cols, h_rows, g_cols, 1.0, w->data.f32 + i * w_batch_inc, lda_inc, g->data.f32 + i * g_batch_inc, g_rows_inc, 1.0, h->data.f32, h_rows_inc);
			}
		}
	}
	return CCV_NNC_EXEC_SUCCESS;
#else
	return CCV_NNC_EXEC_INVALID;
#endif
}


/*
 * testgen.c
 * Patrick Alken
 *
 * Compile: gcc -g -O2 -Wall -o testgen testgen.c -lm -lgsl -llapack -lf77blas -lcblas -latlas -lg2c
 *
 * Usage: testgen [options]
 *
 * -i             : incremental matrices
 * -z             : compute Schur vectors and test them
 * -n size        : size of matrices
 * -l lower-bound : lower bound for matrix elements
 * -u upper-bound : upper bound for matrix elements
 * -c num         : number of matrices to solve
 */

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_ieee_utils.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>

typedef struct
{
  gsl_eigen_gen_workspace *gen_p;
  gsl_matrix *A;
  gsl_matrix *B;
  gsl_vector_complex *alpha;
  gsl_vector *beta;
  gsl_vector_complex *evals;

  gsl_matrix *Q;
  gsl_matrix *Z;
  int compute_schur;

  size_t n_evals;
} gen_workspace;

gen_workspace *gen_alloc(size_t n, int compute_schur);
void gen_free(gen_workspace *w);
int gen_proc(gen_workspace *w);

typedef struct
{
  gsl_matrix *A;
  gsl_matrix *B;
  gsl_matrix *Q;
  gsl_matrix *Z;
  int N;

  char jobvsl;
  char jobvsr;
  char sort;
  int selctg;
  int lda;
  int ldb;
  int sdim;
  double *alphar;
  double *alphai;
  gsl_vector *beta;
  int ldvsr;
  int lwork;
  int info;
  double *work;

  gsl_vector_complex *evals;
  gsl_vector_complex *alpha;
  size_t n_evals;
} lapack_workspace;

lapack_workspace *lapack_alloc(const size_t n);
void lapack_free(lapack_workspace *w);
int lapack_proc(lapack_workspace *w);

void dgges_(char *jobvsl, char *jobvsr, char *sort, int *selctg, int *n,
            double *a, int *lda, double *b, int *ldb, int *sdim,
            double *alphar, double *alphai, double *beta, double *vsl,
            int *ldvsl, double *vsr, int *ldvsr, double *work, int *lwork,
            int *bwork, int *info);

/*
 * Global variables
 */
unsigned long count = 0;

/*
 * Prototypes
 */

void make_random_matrix(gsl_matrix *m, gsl_rng *r, int lower, int upper);
void make_random_integer_matrix(gsl_matrix *m, gsl_rng *r, int lower,
                                int upper);
void make_start_matrix(gsl_matrix *m, int lower);
int inc_matrix (gsl_matrix *m, int lower, int upper);
void output_matrix(gsl_matrix *m);
void print_matrix(gsl_matrix *m, const char *str);
int test_evals(gsl_vector_complex *obs, gsl_vector_complex *expected,
               gsl_matrix *A, gsl_matrix *B,
               const char *obsname, const char *expname);
int test_alpha(gsl_vector_complex *obs, gsl_vector_complex *expected,
               gsl_matrix *A, gsl_matrix *B, const char *obsname,
               const char *expname);
int test_beta(gsl_vector *obs, gsl_vector *expected,
              gsl_matrix *A, gsl_matrix *B, const char *obsname,
              const char *expname);
void test_schur(gsl_matrix *A, gsl_matrix *S, gsl_matrix *Q, gsl_matrix *Z);
void print_vector(gsl_vector_complex *eval, const char *str);
int cmp(double a, double b);
int compare(const void *a, const void *b);
void sort_complex_vector(gsl_vector_complex *v);

gen_workspace *
gen_alloc(size_t n, int compute_schur)
{
  gen_workspace *w;

  w = (gen_workspace *) calloc(1, sizeof(gen_workspace));

  w->gen_p = gsl_eigen_gen_alloc(n);

  w->A = gsl_matrix_alloc(n, n);
  w->B = gsl_matrix_alloc(n, n);
  w->alpha = gsl_vector_complex_alloc(n);
  w->beta = gsl_vector_alloc(n);
  w->evals = gsl_vector_complex_alloc(n);
  w->compute_schur = compute_schur;

  if (compute_schur)
    {
      w->Q = gsl_matrix_alloc(n, n);
      w->Z = gsl_matrix_alloc(n, n);
      gsl_eigen_gen_params(1, 1, 0, w->gen_p);
    }

  return (w);
} /* gen_alloc() */

void
gen_free(gen_workspace *w)
{
  if (w->gen_p)
    gsl_eigen_gen_free(w->gen_p);

  if (w->A)
    gsl_matrix_free(w->A);

  if (w->B)
    gsl_matrix_free(w->B);

  if (w->alpha)
    gsl_vector_complex_free(w->alpha);

  if (w->beta)
    gsl_vector_free(w->beta);

  if (w->evals)
    gsl_vector_complex_free(w->evals);

  if (w->Q)
    gsl_matrix_free(w->Q);

  if (w->Z)
    gsl_matrix_free(w->Z);

  free(w);
}

int
gen_proc(gen_workspace *w)
{
  int s;

  s = gsl_eigen_gen_QZ(w->A, w->B, w->alpha, w->beta, w->Q, w->Z, w->gen_p);

  w->n_evals = w->gen_p->n_evals;

  return s;
} /* gen_proc() */

lapack_workspace *
lapack_alloc(const size_t n)
{
  lapack_workspace *w;
  double work[1];

  w = (lapack_workspace *) calloc(1, sizeof(lapack_workspace));

  w->A = gsl_matrix_alloc(n, n);
  w->B = gsl_matrix_alloc(n, n);
  w->Q = gsl_matrix_alloc(n, n);
  w->Z = gsl_matrix_alloc(n, n);
  w->alphar = malloc(n * sizeof(double));
  w->alphai = malloc(n * sizeof(double));
  w->beta = gsl_vector_alloc(n);
  w->alpha = gsl_vector_complex_alloc(n);
  w->evals = gsl_vector_complex_alloc(n);

  w->N = (int) n;
  w->n_evals = 0;

  w->jobvsl = 'N';
  w->jobvsr = 'N';
  w->sort = 'N';
  w->info = 0;

  w->lwork = -1;
  dgges_(&w->jobvsl,
         &w->jobvsr,
         &w->sort,
         (int *) 0,
         &w->N,
         w->A->data,
         (int *) &w->A->tda,
         w->B->data,
         (int *) &w->B->tda,
         &w->sdim,
         w->alphar,
         w->alphai,
         w->beta->data,
         w->Q->data,
         (int *) &w->Q->tda,
         w->Z->data,
         (int *) &w->Z->tda,
         work,
         &w->lwork,
         (int *) 0,
         &w->info);

  w->lwork = (int) work[0];
  w->work = malloc(w->lwork * sizeof(double));

  return (w);
} /* lapack_alloc() */

void
lapack_free(lapack_workspace *w)
{
  if (w->A)
    gsl_matrix_free(w->A);

  if (w->B)
    gsl_matrix_free(w->B);

  if (w->Q)
    gsl_matrix_free(w->Q);

  if (w->Z)
    gsl_matrix_free(w->Z);

  if (w->work)
    free(w->work);

  if (w->alphar)
    free(w->alphar);

  if (w->alphai)
    free(w->alphai);

  if (w->beta)
    gsl_vector_free(w->beta);

  if (w->alpha)
    gsl_vector_complex_free(w->alpha);

  if (w->evals)
    gsl_vector_complex_free(w->evals);

  free(w);
} /* lapack_free() */

int
lapack_proc(lapack_workspace *w)
{
  dgges_(&w->jobvsl,
         &w->jobvsr,
         &w->sort,
         (int *) 0,
         &w->N,
         w->A->data,
         (int *) &w->A->tda,
         w->B->data,
         (int *) &w->B->tda,
         &w->sdim,
         w->alphar,
         w->alphai,
         w->beta->data,
         w->Q->data,
         (int *) &w->Q->tda,
         w->Z->data,
         (int *) &w->Z->tda,
         w->work,
         &w->lwork,
         (int *) 0,
         &w->info);

  return (w->info);
} /* lapack_proc() */

/**********************************************
 * General routines
 **********************************************/

void
make_random_matrix(gsl_matrix *m, gsl_rng *r, int lower, int upper)
{
  size_t i, j;
  size_t N = m->size1;

  for (i = 0; i < N; ++i)
  {
    for (j = 0; j < N; ++j)
    {
      gsl_matrix_set(m,
                     i,
                     j,
                     gsl_rng_uniform(r) * (upper - lower) + lower);
    }
  }
} /* make_random_matrix() */

void
make_random_integer_matrix(gsl_matrix *m, gsl_rng *r, int lower, int upper)
{
  size_t i, j;
  size_t N = m->size1;

  for (i = 0; i < N; ++i)
  {
    for (j = 0; j < N; ++j)
    {
      double a = gsl_rng_uniform(r) * (upper - lower) + lower;
      gsl_matrix_set(m, i, j, floor(a));
    }
  }
} /* make_random_integer_matrix() */

void
make_start_matrix(gsl_matrix *m, int lower)

{
  size_t i, j;
  size_t N = m->size1;

  for (i = 0; i < N; ++i)
    for (j = 0; j < N; ++j)
      gsl_matrix_set(m, i, j, (double)lower);
} /* make_start_matrix() */

int
inc_matrix (gsl_matrix *m, int lower, int upper)
{
  size_t i = 0;
  size_t N = m->size1 * m->size2;
  int carry = 1;

  for (i = 0; carry > 0 && i < N; i++)
    {
      double v = m->data[i] + carry;
      carry = (v > upper) ? 1 : 0;
      m->data[i] = (v > upper) ? lower : v;
    }

  return carry;
} /* inc_matrix() */

void
output_matrix(gsl_matrix *m)
{
  size_t i, j;
  size_t N = m->size1;
  size_t M = m->size2;

  for (i = 0; i < N; ++i)
  {
    for (j = 0; j < M; ++j)
      {
        printf("%10.18e%s",
        /*printf("%10.18e%s",*/
               gsl_matrix_get(m, i, j),
               (j < M - 1) ? "," : ";\n");
      }
  }
}

void
print_matrix(gsl_matrix *m, const char *str)

{
  size_t i, j;
  size_t N = m->size1;
  size_t M = m->size2;
  gsl_matrix_view v;
  size_t rows, cols;
  size_t r, c;
  char buf[100];

  /*print_octave(m, str);
  return;*/

  /*rows = GSL_MIN(15, N);*/
  rows = N;
  cols = GSL_MIN(15, N);
  /*cols = N;*/

  for (i = 0; i < N; i += rows)
  {
    for (j = 0; j < M; j += cols)
    {
      r = GSL_MIN(rows, N - i);
      c = GSL_MIN(cols, N - j);

      v = gsl_matrix_submatrix(m, i, j, r, c);

      sprintf(buf, "%s(%u:%u,%u:%u)",
              str,
              i + 1,
              i + r,
              j + 1,
              j + c);

      printf("%s = [\n", buf);

      output_matrix(&v.matrix);

      printf("]\n");
    }
  }
} /* print_matrix() */

int
test_evals(gsl_vector_complex *obs, gsl_vector_complex *expected,
           gsl_matrix *A, gsl_matrix *B, const char *obsname,
           const char *expname)
{
  size_t N = expected->size;
  size_t i, k;
  double max, max_abserr, max_relerr;

  max = 0.0;
  max_abserr = 0.0;
  max_relerr = 0.0;
  k = 0;

  for (i = 0; i < N; ++i)
    {
      gsl_complex z = gsl_vector_complex_get(expected, i);
      max = GSL_MAX_DBL(max, gsl_complex_abs(z));
    }

  for (i = 0; i < N; ++i)
    {
      gsl_complex z_obs = gsl_vector_complex_get(obs, i);
      gsl_complex z_exp = gsl_vector_complex_get(expected, i);

      double x_obs = GSL_REAL(z_obs);
      double y_obs = GSL_IMAG(z_obs);
      double x_exp = GSL_REAL(z_exp);
      double y_exp = GSL_IMAG(z_exp);

      double abserr_x = fabs(x_obs - x_exp);
      double abserr_y = fabs(y_obs - y_exp);
      double noise = max * GSL_DBL_EPSILON * N * N;

      max_abserr = GSL_MAX_DBL(max_abserr, abserr_x + abserr_y);

      if (abserr_x < noise && abserr_y < noise)
        continue;

      if (abserr_x > 1.0e-6 || abserr_y > 1.0e-6)
        ++k;
    }

    if (k)
      {
        printf("==== CASE %lu ===========================\n\n", count);

        print_matrix(A, "A");
        print_matrix(B, "B");

        printf("=== eval - %s ===\n", expname);
        print_vector(expected, expname);

        printf("=== eval - %s ===\n", obsname);
        print_vector(obs, obsname);

        printf("max abserr = %g  max relerr = %g\n", max_abserr, max_relerr);

        printf("=========================================\n\n");
      }

    return k;
} /* test_evals() */

int
test_alpha(gsl_vector_complex *obs, gsl_vector_complex *expected,
           gsl_matrix *A, gsl_matrix *B, const char *obsname,
           const char *expname)
{
  size_t N = expected->size;
  size_t i, k;
  double max, max_abserr, max_relerr;

  max = 0.0;
  max_abserr = 0.0;
  max_relerr = 0.0;
  k = 0;

  for (i = 0; i < N; ++i)
    {
      gsl_complex z = gsl_vector_complex_get(expected, i);
      max = GSL_MAX_DBL(max, gsl_complex_abs(z));
    }

  for (i = 0; i < N; ++i)
    {
      gsl_complex z_obs = gsl_vector_complex_get(obs, i);
      gsl_complex z_exp = gsl_vector_complex_get(expected, i);

      double x_obs = GSL_REAL(z_obs);
      double y_obs = fabs(GSL_IMAG(z_obs));
      double x_exp = GSL_REAL(z_exp);
      double y_exp = fabs(GSL_IMAG(z_exp));

      double abserr_x = fabs(x_obs - x_exp);
      double abserr_y = fabs(y_obs - y_exp);
      double noise = max * GSL_DBL_EPSILON * N * N;

      max_abserr = GSL_MAX_DBL(max_abserr, abserr_x + abserr_y);

      if (abserr_x < noise && abserr_y < noise)
        continue;

      if (abserr_x > 1.0e-6 || abserr_y > 1.0e-6)
        ++k;
    }

    if (k)
      {
        printf("==== CASE %lu ===========================\n\n", count);

        print_matrix(A, "A");
        print_matrix(B, "B");

        printf("=== alpha - %s ===\n", expname);
        print_vector(expected, expname);

        printf("=== alpha - %s ===\n", obsname);
        print_vector(obs, obsname);

        printf("max abserr = %g  max relerr = %g\n", max_abserr, max_relerr);

        printf("=========================================\n\n");
      }

    return k;
} /* test_alpha() */

int
test_beta(gsl_vector *obs, gsl_vector *expected,
          gsl_matrix *A, gsl_matrix *B, const char *obsname,
          const char *expname)
{
  size_t N = expected->size;
  size_t i, k;
  double max, max_abserr, max_relerr;

  max = 0.0;
  max_abserr = 0.0;
  max_relerr = 0.0;
  k = 0;

  for (i = 0; i < N; ++i)
    {
      double z = gsl_vector_get(expected, i);
      max = GSL_MAX_DBL(max, fabs(z));
    }

  for (i = 0; i < N; ++i)
    {
      double v_obs = gsl_vector_get(obs, i);
      double v_exp = gsl_vector_get(expected, i);
      double abserr = fabs(v_obs - v_exp);
      double noise = max * GSL_DBL_EPSILON * N * N;

      max_abserr = GSL_MAX_DBL(max_abserr, abserr);

      if (abserr < noise)
        continue;

      if (abserr > 1.0e-6)
        ++k;
    }

    if (k)
      {
        printf("==== CASE %lu ===========================\n\n", count);

        print_matrix(A, "A");
        print_matrix(B, "B");

        printf("=== beta - %s ===\n", expname);
        printf("%s = [\n", expname);
        gsl_vector_fprintf(stdout, expected, "%.12e");
        printf("]\n");

        printf("=== beta - %s ===\n", obsname);
        printf("%s = [\n", obsname);
        gsl_vector_fprintf(stdout, obs, "%.12e");
        printf("]\n");

        printf("max abserr = %g  max relerr = %g\n", max_abserr, max_relerr);

        printf("=========================================\n\n");
      }

    return k;
} /* test_beta() */

/* test if A = Q S Z^t */
void
test_schur(gsl_matrix *A, gsl_matrix *S, gsl_matrix *Q, gsl_matrix *Z)
{
  const size_t N = A->size1;
  gsl_matrix *T1, *T2;
  size_t i, j, k;
  double lhs, rhs;
  double abserr;

  T1 = gsl_matrix_alloc(N, N);
  T2 = gsl_matrix_alloc(N, N);

  /* compute T1 = S Z^t */
  gsl_blas_dgemm(CblasNoTrans,
                 CblasTrans,
                 1.0,
                 S,
                 Z,
                 0.0,
                 T1);

  /* compute T2 = Q T1 = Q S Z^t */
  gsl_blas_dgemm(CblasNoTrans,
                 CblasNoTrans,
                 1.0,
                 Q,
                 T1,
                 0.0,
                 T2);

  k = 0;
  for (i = 0; i < N; ++i)
    {
      for (j = 0; j < N; ++j)
        {
          lhs = gsl_matrix_get(A, i, j);
          rhs = gsl_matrix_get(T2, i, j);

          abserr = fabs(lhs - rhs);

          if (abserr > 1.0e-6)
            ++k;
        }
    }

  if (k)
    {
      printf("==== CASE %lu ===========================\n\n", count);

      print_matrix(A, "A");

      printf("=== Schur Form matrix ===\n");
      print_matrix(S, "S");

      printf("=== Left Schur matrix ===\n");
      print_matrix(Q, "Q");

      printf("=== Right Schur matrix ===\n");
      print_matrix(Z, "Z");

      printf("=== Q S Z^t ===\n");
      print_matrix(T1, "Q S Z^t");

      printf("=== A - Q S Z^t ===\n");
      gsl_matrix_sub(T2, A);
      print_matrix(T1, "A - Q S Z^t");

      printf("=========================================\n\n");
    }

  gsl_matrix_free(T1);
  gsl_matrix_free(T2);
} /* test_schur() */

void
print_vector(gsl_vector_complex *eval, const char *str)
{
  size_t N = eval->size;
  size_t i;
  gsl_complex z;

  printf("%s = [\n", str);

  for (i = 0; i < N; ++i)
    {
      z = gsl_vector_complex_get(eval, i);
      printf("%.18e %.18e;\n", GSL_REAL(z), GSL_IMAG(z));
    }

  printf("]\n");
} /* print_vector() */

int
cmp(double a, double b)
{
  return ((a > b) ? 1 : ((a < b) ? -1 : 0));
} /* cmp() */

int
compare(const void *a, const void *b)
{
  const double *x = a;
  const double *y = b;
  int r1 = cmp(y[0], x[0]);
  int r2 = cmp(y[1], x[1]);

  if (!gsl_finite(x[0]))
    return 1;
  if (!gsl_finite(y[0]))
    return -1;

  if (fabs(x[0] - y[0]) < 1.0e-8)
    {
      /* real parts are very close to each other */
      return r2;
    }
  else
    {
      return r1 ? r1 : r2;
    }
} /* compare() */

void
sort_complex_vector(gsl_vector_complex *v)
{
  qsort(v->data, v->size, 2 * sizeof(double), &compare);
} /* sort_complex_vector() */

int
main(int argc, char *argv[])
{
  gen_workspace *gen_workspace_p;
  lapack_workspace *lapack_workspace_p;
  size_t N;
  int c;
  int lower;
  int upper;
  int incremental;
  size_t nmat;
  gsl_matrix *A, *B;
  gsl_rng *r;
  int s;
  int compute_schur;
  size_t i;

  gsl_ieee_env_setup();
  gsl_rng_env_setup();

  N = 30;
  lower = -10;
  upper = 10;
  incremental = 0;
  nmat = 0;
  compute_schur = 0;

  while ((c = getopt(argc, argv, "ic:n:l:u:z")) != (-1))
    {
      switch (c)
        {
          case 'i':
            incremental = 1;
            break;

          case 'n':
            N = strtol(optarg, NULL, 0);
            break;

          case 'l':
            lower = strtol(optarg, NULL, 0);
            break;

          case 'u':
            upper = strtol(optarg, NULL, 0);
            break;

          case 'c':
            nmat = strtoul(optarg, NULL, 0);
            break;

          case 'z':
            compute_schur = 1;
            break;

          case '?':
          default:
            printf("usage: %s [-i] [-z] [-n size] [-l lower-bound] [-u upper-bound] [-c num]\n", argv[0]);
            exit(1);
            break;
        } /* switch (c) */
    }

  A = gsl_matrix_alloc(N, N);
  B = gsl_matrix_alloc(N, N);
  gen_workspace_p = gen_alloc(N, compute_schur);
  lapack_workspace_p = lapack_alloc(N);

  r = gsl_rng_alloc(gsl_rng_default);

  if (incremental)
    {
      make_start_matrix(A, lower);

      /* we need B to be non-singular */
      make_random_integer_matrix(B, r, lower, upper);
    }

  fprintf(stderr, "testing N = %d", N);
  if (incremental)
    fprintf(stderr, " incrementally");
  else
    fprintf(stderr, " randomly");

  fprintf(stderr, " on element range [%d, %d]", lower, upper);

  if (compute_schur)
    fprintf(stderr, ", with Schur vectors");

  fprintf(stderr, "\n");

  while (1)
    {
      if (nmat && (count >= nmat))
        break;

      ++count;

      if (!incremental)
        {
          make_random_matrix(A, r, lower, upper);
          make_random_matrix(B, r, lower, upper);
        }
      else
        {
          s = inc_matrix(A, lower, upper);
          if (s)
            break; /* all done */

          make_random_integer_matrix(B, r, lower, upper);
        }

      /*if (count != 89120)
        continue;*/

      /* make copies of matrices */
      gsl_matrix_memcpy(gen_workspace_p->A, A);
      gsl_matrix_memcpy(gen_workspace_p->B, B);
      gsl_matrix_transpose_memcpy(lapack_workspace_p->A, A);
      gsl_matrix_transpose_memcpy(lapack_workspace_p->B, B);

      /* compute eigenvalues with LAPACK */
      s = lapack_proc(lapack_workspace_p);

      if (s != GSL_SUCCESS)
        {
          printf("LAPACK failed, case %lu\n", count);
          exit(1);
        }

#if 0
      print_matrix(A, "A");
      print_matrix(B, "B");
      gsl_matrix_transpose(lapack_workspace_p->A);
      gsl_matrix_transpose(lapack_workspace_p->B);
      print_matrix(lapack_workspace_p->A, "S_lapack");
      print_matrix(lapack_workspace_p->B, "T_lapack");
#endif

      /* compute eigenvalues with GSL */
      s = gen_proc(gen_workspace_p);

      if (s != GSL_SUCCESS)
        {
          printf("=========== CASE %lu ============\n", count);
          printf("Failed to converge: found %u eigenvalues\n",
                 gen_workspace_p->n_evals);
          print_matrix(A, "A");
          print_matrix(B, "B");
          print_matrix(gen_workspace_p->A, "Af");
          print_matrix(gen_workspace_p->B, "Bf");
          print_matrix(lapack_workspace_p->A, "Ae");
          print_matrix(lapack_workspace_p->B, "Be");
          exit(1);
        }

#if 0
      print_matrix(gen_workspace_p->A, "S_gsl");
      print_matrix(gen_workspace_p->B, "T_gsl");
#endif

      /* compute alpha / beta vectors */
      for (i = 0; i < N; ++i)
        {
          double beta;
          gsl_complex alpha, z;

          beta = gsl_vector_get(gen_workspace_p->beta, i);
          if (beta == 0.0)
            GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
          else
            {
              alpha = gsl_vector_complex_get(gen_workspace_p->alpha, i);
              z = gsl_complex_div_real(alpha, beta);
            }

          gsl_vector_complex_set(gen_workspace_p->evals, i, z);

          beta = gsl_vector_get(lapack_workspace_p->beta, i);
          GSL_SET_COMPLEX(&alpha,
                          lapack_workspace_p->alphar[i],
                          lapack_workspace_p->alphai[i]);

          if (beta == 0.0)
            GSL_SET_COMPLEX(&z, GSL_POSINF, GSL_POSINF);
          else
            z = gsl_complex_div_real(alpha, beta);

          gsl_vector_complex_set(lapack_workspace_p->evals, i, z);
          gsl_vector_complex_set(lapack_workspace_p->alpha, i, alpha);
        }

#if 0
      gsl_sort_vector(gen_workspace_p->beta);
      gsl_sort_vector(lapack_workspace_p->beta);
      sort_complex_vector(gen_workspace_p->alpha);
      sort_complex_vector(lapack_workspace_p->alpha);

      s = test_alpha(gen_workspace_p->alpha,
                     lapack_workspace_p->alpha,
                     A,
                     B,
                     "gen",
                     "lapack");
      s = test_beta(gen_workspace_p->beta,
                    lapack_workspace_p->beta,
                    A,
                    B,
                    "gen",
                    "lapack");
#endif
#if 1
      sort_complex_vector(gen_workspace_p->evals);
      sort_complex_vector(lapack_workspace_p->evals);

      s = test_evals(gen_workspace_p->evals,
                     lapack_workspace_p->evals,
                     A,
                     B,
                     "gen",
                     "lapack");
#endif

      if (compute_schur)
        {
          test_schur(A,
                     gen_workspace_p->A,
                     gen_workspace_p->Q,
                     gen_workspace_p->Z);
          test_schur(B,
                     gen_workspace_p->B,
                     gen_workspace_p->Q,
                     gen_workspace_p->Z);
        }
    }

  gsl_matrix_free(A);
  gsl_matrix_free(B);
  gen_free(gen_workspace_p);
  lapack_free(lapack_workspace_p);

  if (r)
    gsl_rng_free(r);

  return 0;
} /* main() */


/**
 *
 * @file example_cposv.c
 *
 *  PLASMA testing routines
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @brief Example of solving a linear system with a Cholesky factorization
 *
 * @version 2.6.0
 * @author Bilel Hadri
 * @date 2010-11-15
 * @generated c Tue Jan  7 11:45:21 2014
 *
 **/
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <math.h>

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

int check_solution(int, int, PLASMA_Complex32_t*, int, PLASMA_Complex32_t*, PLASMA_Complex32_t*, int);

int IONE=1;
int ISEED[4] = {0,0,0,1};   /* initial seed for clarnv() */

int main ()
{

    int cores = 2;
    int N     = 10;
    int LDA   = 10;
    int NRHS  = 5;
    int LDB   = 10;
    int info;
    int info_solution;

    PLASMA_Complex32_t *A1   = (PLASMA_Complex32_t *)malloc(LDA*N*sizeof(PLASMA_Complex32_t));
    PLASMA_Complex32_t *A2   = (PLASMA_Complex32_t *)malloc(LDA*N*sizeof(PLASMA_Complex32_t));
    PLASMA_Complex32_t *B1   = (PLASMA_Complex32_t *)malloc(LDB*NRHS*sizeof(PLASMA_Complex32_t));
    PLASMA_Complex32_t *B2   = (PLASMA_Complex32_t *)malloc(LDB*NRHS*sizeof(PLASMA_Complex32_t));

    /* Check if unable to allocate memory */
    if ((!A1)||(!A2)||(!B1)||(!B2)){
        printf("Out of Memory \n ");
        return EXIT_SUCCESS;
    }

    /* Plasma Initialize */
    PLASMA_Init(cores);
    printf("-- PLASMA is initialized to run on %d cores. \n",cores);

    /*-------------------------------------------------------------
    *  TESTING CPOSV
    */

    /* Initialize A1 and A2 for Symmetric Positif Matrix (Hessenberg in the complex case) */
    PLASMA_cplghe( (float)N, N, A1, LDA, 51 );
    PLASMA_clacpy( PlasmaUpperLower, N, N, A1, LDA, A2, LDA );

    /* Initialize B1 and B2 */
    PLASMA_cplrnt( N, NRHS, B1, LDB, 371 );
    PLASMA_clacpy( PlasmaUpperLower, N, NRHS, B1, LDB, B2, LDB );

    /* PLASMA CPOSV */
    info = PLASMA_cposv(PlasmaUpper, N, NRHS, A2, LDA, B2, LDB);

    /* Check the solution */
    info_solution = check_solution(N, NRHS, A1, LDA, B1, B2, LDB);

    if ((info_solution != 0)|(info != 0))
       printf("-- Error in CPOSV example ! \n");
    else
       printf("-- Run of CPOSV example successful ! \n");

    free(A1); free(A2); free(B1); free(B2);

    PLASMA_Finalize();

    return EXIT_SUCCESS;
}

/*------------------------------------------------------------------------
 *  Check the accuracy of the solution of the linear system
 */

int check_solution(int N, int NRHS, PLASMA_Complex32_t *A1, int LDA, PLASMA_Complex32_t *B1, PLASMA_Complex32_t *B2, int LDB)
{
    int info_solution;
    float Rnorm, Anorm, Xnorm, Bnorm;
    PLASMA_Complex32_t alpha, beta;
    float *work = (float *)malloc(N*sizeof(float));
    float eps;

    eps = LAPACKE_slamch_work('e');

    alpha = 1.0;
    beta  = -1.0;

    Xnorm = LAPACKE_clange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B2, LDB, work);
    Anorm = LAPACKE_clange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, N, A1, LDA, work);
    Bnorm = LAPACKE_clange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    cblas_cgemm(CblasColMajor, CblasNoTrans, CblasNoTrans, N, NRHS, N, CBLAS_SADDR(alpha), A1, LDA, B2, LDB, CBLAS_SADDR(beta), B1, LDB);
    Rnorm = LAPACKE_clange_work(LAPACK_COL_MAJOR, lapack_const(PlasmaInfNorm), N, NRHS, B1, LDB, work);

    printf("============\n");
    printf("Checking the Residual of the solution \n");
    printf("-- ||Ax-B||_oo/((||A||_oo||x||_oo+||B||_oo).N.eps) = %e \n",Rnorm/((Anorm*Xnorm+Bnorm)*N*eps));

    if (Rnorm/((Anorm*Xnorm+Bnorm)*N*eps) > 10.0){
        printf("-- The solution is suspicious ! \n");
        info_solution = 1;
     }
    else{
        printf("-- The solution is CORRECT ! \n");
        info_solution = 0;
    }

    free(work);

    return info_solution;
}


// 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_MATRIX_DECOMPOSITION_H
#define SCILIB_LINALG_MATRIX_DECOMPOSITION_H

#ifdef USE_MKL
#include <mkl.h>
#else
#ifdef __clang__
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wreturn-type-c-linkage"
#endif
#include <lapacke.h>
#ifdef __clang__
#pragma clang diagnostic pop
#endif
#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 {

namespace stdex = std::experimental;

// LU factorization.
template <class Layout>
inline void lu(Sci::Matrix_view<double, Layout> a,
               Sci::Vector_view<BLAS_INT, Layout> ipiv)
{
    static_assert(a.is_contiguous());
    static_assert(ipiv.is_contiguous());

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));

    assert(static_cast<BLAS_INT>(ipiv.size()) >= std::min(m, n));

    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;
        lda = m;
    }

    BLAS_INT info =
        LAPACKE_dgetrf(matrix_layout, m, n, a.data(), lda, ipiv.data());
    if (info < 0) {
        throw std::runtime_error("dgetrf: illegal input parameter");
    }
    if (info > 0) {
        throw std::runtime_error("dgetrf: U matrix is singular");
    }
}

template <class Layout, class Allocator_a, class Allocator_ipiv>
inline void lu(Sci::Matrix<double, Layout, Allocator_a>& a,
               Sci::Vector<BLAS_INT, Layout, Allocator_ipiv>& ipiv)
{
    lu(a.view(), ipiv.view());
}

// QR factorization.
template <class Layout>
inline void qr(Sci::Matrix_view<double, Layout> a,
               Sci::Matrix_view<double, Layout> q,
               Sci::Matrix_view<double, Layout> r)
{
    assert(q.extent(0) == a.extent(0) && q.extent(1) == a.extent(1));
    assert(r.extent(0) == a.extent(0) && r.extent(1) == a.extent(1));

    const BLAS_INT m = static_cast<BLAS_INT>(a.extent(0));
    const BLAS_INT n = static_cast<BLAS_INT>(a.extent(1));

    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;
        lda = m;
    }
    Sci::copy(a, q);
    Sci::Vector<double, Layout> tau(std::min(m, n));

    // Compute QR factorization:

    BLAS_INT info =
        LAPACKE_dgeqrf(matrix_layout, m, n, q.data(), lda, tau.data());
    if (info != 0) {
        throw std::runtime_error("dgeqrf failed");
    }

    // Compute Q:

    info = LAPACKE_dorgqr(matrix_layout, m, n, n, q.data(), lda, tau.data());
    if (info != 0) {
        throw std::runtime_error("dorgqr failed");
    }

    // Compute R:

    matrix_product(transposed(q), a, r);
    transposed(q);
}

template <class Layout, class Allocator>
inline void qr(Sci::Matrix<double, Layout, Allocator>& a,
               Sci::Matrix<double, Layout, Allocator>& q,
               Sci::Matrix<double, Layout, Allocator>& r)
{
    qr(a.view(), q.view(), r.view());
}

// Singular value decomposition.
template <class Layout>
inline void svd(Sci::Matrix_view<double, Layout> a,
                Sci::Vector_view<double, Layout> s,
                Sci::Matrix_view<double, Layout> u,
                Sci::Matrix_view<double, Layout> vt)
{
    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 ldu = m;
    const BLAS_INT ldvt = n;

    assert(static_cast<BLAS_INT>(s.extent(0)) == std::min(m, n));
    assert(static_cast<BLAS_INT>(u.extent(0)) == m);
    assert(static_cast<BLAS_INT>(u.extent(1)) == ldu);
    assert(static_cast<BLAS_INT>(vt.extent(0)) == n);
    assert(static_cast<BLAS_INT>(vt.extent(1)) == ldvt);

    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;
        lda = m;
    }

    Sci::Vector<double, Layout> superb(std::min(m, n) - 1);

    BLAS_INT info =
        LAPACKE_dgesvd(matrix_layout, 'A', 'A', m, n, a.data(), lda, s.data(),
                       u.data(), ldu, vt.data(), ldvt, superb.data());
    if (info != 0) {
        throw std::runtime_error("dgesvd failed");
    }
}

template <class Layout, class Allocator>
inline void svd(Sci::Matrix<double, Layout, Allocator>& a,
                Sci::Vector<double, Layout, Allocator>& s,
                Sci::Matrix<double, Layout, Allocator>& u,
                Sci::Matrix<double, Layout, Allocator>& vt)
{
    svd(a.view(), s.view(), u.view(), vt.view());
}

} // namespace Linalg
} // namespace Sci

#endif // SCILIB_LINALG_MATRIX_DECOMPOSITION_H


#ifndef _HEX_GRAPH_H_
#define _HEX_GRAPH_H_

#include <stdlib.h>
#include <gsl/gsl_spmatrix.h>

struct graph_t {
  size_t num_vertices;  // Number of vertices in the graph
  gsl_spmatrix* t;      // Sparse matrix of size n*n,
                        // t[i][j] == 1 means there is an edge from i to j
  gsl_spmatrix* o;      // Sparse matrix of size 2*n, one line per player
                        // o[p][i] == 1 means that the vertex i is owned by p
};

#endif // _HEX_GRAPH_H_


#pragma once

#include <string>
#include <vector>

#include <boost/random.hpp>
#include <cblas.h>

namespace con {

  using std::vector;
  using std::cout;
  using std::cerr;
  using std::endl;
  using std::string;

  static boost::mt19937 rng(0);

  #define BUG(x) std::cout<<#x<<" = "<<(x)<<std::endl;

  typedef double Real;

  typedef vector<Real> Vec;

  Real sigmoid(const Real &z) {
    return 1.0 / (1.0 + std::exp(-z));
  }

  Real derivativeSigmoid(const Real &v) {
    return v * (1.0 - v);
  }

  Real sqr(const Real &x) {
    return x * x;
  }

  Real randomize(const Real &min, const Real &max) {
    boost::uniform_real<Real> dst(min, max);
    return dst(rng);
  }

  void randomizeVec(const Real &min, const Real &max, Vec *a) {
    for (auto it = a->begin(); it != a->end(); it++) {
      (*it) = randomize(min, max);
    }
  }

  void gaussianRng(const Real &mean, const Real &std, Vec *a) {
    boost::normal_distribution<Real> random_distribution(mean, std);
    boost::variate_generator<boost::mt19937, boost::normal_distribution<Real>>
      variate_generator(rng, random_distribution);

    for (auto it = a->begin(); it != a->end(); it++) {
      *it = variate_generator();
    }
  }

  void clear(Vec *a) {
    for (auto it = a->begin(); it != a->end(); it++) {
      (*it) = 0;
    }
  }

  void clear(vector<Vec> *a) {
    for (int i = 0; i < a->size(); i++) {
      clear(&a->at(i));
    }
  }

  void print(const Vec &a) {
    for (auto x : a) {
      cout << x << " ";
    }
    cout << endl;
  }

  void reshape(const int num, const int width, const int height, const int depth, vector<Vec> *a) {
    a->resize(num);
    for (int i = 0; i < num; i++) {
      a->at(i).resize(width * height * depth);
    }
  }

  int ceilDiv(const int &x, const int &y) {
    return (x + y - 1) / y;
  }

  void gemm(
      const CBLAS_TRANSPOSE &TransA, const CBLAS_TRANSPOSE &TransB,
      const int &M, const int &N, const int &K,
      const Real &alpha, const Vec &vecA, const Vec &vecB,
      const Real &beta, Vec *vecC) {

    const Real *A = &vecA[0];
    const Real *B = &vecB[0];
    Real *C = &vecC->at(0);

    int lda = (TransA == CblasNoTrans) ? K : M;
    int ldb = (TransB == CblasNoTrans) ? N : K;

    cblas_dgemm(CblasRowMajor, TransA, TransB, M, N, K, alpha, A, lda, B,
        ldb, beta, C, N);
  }

  void gemv(
      const CBLAS_TRANSPOSE &TransA,
      const int &M, const int &N,
      const Real &alpha, const Vec &matA, const Vec &vecX,
      const double beta, Vec *vecY) {

    const Real *A = &matA[0];
    const Real *x = &vecX[0];
    Real *y = &vecY->at(0);

    cblas_dgemv(CblasRowMajor, TransA, M, N, alpha, A, N, x, 1, beta, y, 1);
  }

  void vexp(const int &n, const Vec &input, Vec *output) {
    // vsExp(n, &input[0], &output->at(0));
    for (int i = 0; i < n; i++) {
      output->at(i) = exp(input[i]);
    }
  }

  void vdiv(const int &n, const Vec &a, const Vec &b, Vec *c) {
    // vsDiv(n, &a[0], &b[0], &c->at(0));
    for (int i = 0; i < n; i++) {
      c->at(i) = a[i] / b[i];
    }
  }

  void copy(const int &n, const Vec &input, Vec *output) {
    for (int i = 0; i < n; i++) {
      output->at(i) = input[i];
    }
  }

  void ones(const int &n, Vec *v) {
    v->resize(n);
    std::fill(v->begin(), v->end(), 1.0);
  }
}


/* blas/gsl_blas.h
 *
 * 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

/*
 * Author:  G. Jungman
 */
#ifndef __GSL_BLAS_H__
#define __GSL_BLAS_H__

#include <gsl/vector/gsl_vector.h>
#include <gsl/matrix/gsl_matrix.h>

#include <gsl/blas/gsl_blas_types.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

/**
	MODIFIED BY STELIAN:
		- only keep a subset of the methods that I think are important
		- link to an external blas library
*/

/* ========================================================================
 * Level 1
 * ========================================================================
 */

/**
*     forms the dot product of two vectors.
*/
GSL_FUNC int gsl_blas_ddot (const gsl_vector * X, const gsl_vector * Y, double * result);

/**
*  DNRM2 returns the euclidean norm of a vector via the function
*  name, so that
*
*     DNRM2 := sqrt( x'*x )
*/
GSL_FUNC double gsl_blas_dnrm2  (const gsl_vector * X);

/**
*     interchanges two vectors.
*/
GSL_FUNC int  gsl_blas_dswap (gsl_vector * X, gsl_vector * Y);

/**
*     copies a vector, x, to a vector, y.
*/
GSL_FUNC int  gsl_blas_dcopy (const gsl_vector * X, gsl_vector * Y);

/* ===========================================================================
 * Level 2
 * ===========================================================================
 */

/**
*  DGEMV  performs one of the matrix-vector operations
*
*     y := alpha*A*x + beta*y,   or   y := alpha*A'*x + beta*y,
*
*  where alpha and beta are scalars, x and y are vectors and A is an
*  m by n matrix.
*/
GSL_FUNC int  gsl_blas_dgemv (CBLAS_TRANSPOSE_t TransA, double alpha, const gsl_matrix * A, const gsl_vector * X, double beta, gsl_vector * Y);

/*
 * ===========================================================================
 * Prototypes for level 3 BLAS
 * ===========================================================================
 */

/**
*  DGEMM  performs one of the matrix-matrix operations
*
*     C := alpha*op( A )*op( B ) + beta*C,
*
*  where  op( X ) is one of
*
*     op( X ) = X   or   op( X ) = X',
*
*  alpha and beta are scalars, and A, B and C are matrices, with op( A )
*  an m by k matrix,  op( B )  a  k by n matrix and  C an m by n matrix.
*/
GSL_FUNC int  gsl_blas_dgemm (CBLAS_TRANSPOSE_t TransA, CBLAS_TRANSPOSE_t TransB, double alpha, const gsl_matrix * A, const gsl_matrix * B, double beta, gsl_matrix * C);

__END_DECLS

#endif /* __GSL_BLAS_H__ */


#ifndef IFT_COMMON_H_
#define IFT_COMMON_H_

#include <omp.h>
#include <stdlib.h>
#include <stdio.h>
#if !defined(__APPLE__)
	#include <malloc.h>
#endif
#include <math.h>
#include <string.h>
#include <limits.h>
#include <float.h>
#include <sys/time.h>
#include <time.h>
#include <unistd.h>
#include <assert.h>
#include <cblas.h>
#include <dirent.h>
#include <regex.h>

/*
 * Common data types
 */

#define INFINITY_INT  INT_MAX
#define INFINITY_FLT  FLT_MAX
#define INFINITY_DBL  DBL_MAX
#define INFINITY_LDBL LDBL_MAX

typedef struct timeval timer;

typedef unsigned char  uchar;
typedef unsigned short ushort;
typedef unsigned int   uint;
typedef unsigned long long ullong;

typedef struct ift_band {
  float *val;
} iftBand;

typedef struct ift_vector {
  float x,y,z;
} iftVector, iftPoint;

typedef struct ift_voxel {
  int x,y,z;
} iftVoxel;

typedef struct ift_dcomplex
{
  double r;
  double i;
} iftComplex;

typedef struct file_list {
  char **filesRoutes;
  char **filesNames;
  int    n;
} fileList;

typedef struct ift_name_metric_pair {
  char *name;
  float metric;
} iftNameMetricPair;

/**
 * Common definitions
 */

#define IFT_RANDOM_SEED (unsigned int) 213344
#define MAXWEIGHT     4095.0
#define AXIS_X 0
#define AXIS_Y 1
#define AXIS_Z 2
#define PI          3.1415926536
#define INTERIOR    0
#define EXTERIOR    1
#define BOTH        2
#define WHITE       0
#define GRAY        1
#define BLACK       2
#define NIL        -1
#define INCREASING  1
#define DECREASING  0
#define Epsilon     1E-05

/**
 * Common operations
 */

#ifndef MAX
#define MAX(x,y) (((x) > (y))?(x):(y))
#endif

#ifndef MIN
#define MIN(x,y) (((x) < (y))?(x):(y))
#endif

#define ROUND(x) ((x < 0)?(int)(x-0.5):(int)(x+0.5))
#define SIGN(x) ((x >= 0)?1:-1)

/**
 * Common functions to allocate memory
 */

char     *iftAllocCharArray(int n);
uchar    *iftAllocUCharArray(int n);
short    *iftAllocShortArray(int n);
ushort   *iftAllocUShortArray(int n);
uint     *iftAllocUIntArray(int n);
ullong   *iftAllocULLongArray(int n);
int      *iftAllocIntArray(int n);
float    *iftAllocFloatArray(int n);
double   *iftAllocDoubleArray(int n);
iftComplex  *iftAllocComplexArray(int n);
long double *iftAllocLongDoubleArray(int n);

void      iftPrintFloatArray(float* v, int n);
/**
 * Error messages
 */

#define MSG1  "Cannot allocate memory space"
#define MSG2  "Cannot open file"

/**
 *  Error message msg is printed in function func and the program
 *  exits abnormally.
 */

void iftError(char *msg,char *func);

/**
 *  Warning message msg is printed in function func and the program
 *  continues.
 */

void iftWarning(char *msg,char *func);

/**
 *  The contents of a and b are interchanged.
 */

void iftSwitchValues(int *a, int *b);
void iftSSwitchValues(char *a, char *b, int size);
void iftFSwitchValues(float *a, float *b);
void iftDSwitchValues(double *a, double *b);
void iftSwitchVoxels(iftVoxel *u, iftVoxel *v);

/**
 * Returns a random integer number between low and high.
 */

int iftRandomInteger (int low, int high);

/*
 * Randomly selects nelems of the set [low, high]
 */
int *iftRandomIntegers (int low, int high, int nelems);

/**
 * Randomly selects a normal distributed (N(0,1)) float number
 */

float iftRandomNormalFloat(void);

/**
 * Returns the distance between P0 and the line from P1 to P2, whose
 * size is P1P2
 */

float iftVoxelLineDist2D(iftVoxel P0, iftVoxel P1, iftVoxel P2, float P1P2);

/**
 * Returns the position of P0 with respect to the line from P1 to
 * P2. Negative values indicate left side, 0 indicates on the line,
 * and positive values indicate right side.
 */

int iftVoxelLinePosition2D (iftVoxel P0, iftVoxel P1, iftVoxel P2);

/**
 * Returns initial time
 */

timer *iftTic(void);

/**
 * Returns final time
 */

timer *iftToc(void);

/**
 * Computes the difference in ms from the initial time to the final time
 */

float iftCompTime(timer *tic, timer *toc);

/**
 * Generates seed for rand(), used in iftRandomInteger.
 */

void iftRandomSeed(unsigned int);

/**
 * Returns the factorial of a number or NIL in case of overflow
 */

long double iftFactorial(int n);

/**
 * Returns the limit to avoid overflow in factorial computation
 */

int iftFactorialLimit(void);

float iftVoxelDistance(iftVoxel u, iftVoxel v);
int iftSquaredVoxelDistance(iftVoxel u, iftVoxel v);

float iftPointDistance(iftPoint u, iftPoint v);

int   iftVoxelSquareDistance(iftVoxel u, iftVoxel v);

float iftInnerProduct(iftVector a, iftVector b);

iftVector iftCrossProduct(iftVector a, iftVector b);

char iftCollinearPoints(iftPoint P1, iftPoint P2, iftPoint P3);

char iftCollinearVoxels(iftVoxel P1, iftVoxel P2, iftVoxel P3);

iftVector iftNormalizeVector(iftVector v);

void iftNormalizeFloatArray(float *array, int nelems);

float iftVectorMagnitude(iftVector v);

void iftRemoveCarriageReturn(char *token); /* useful to get rid of the
					      carriage return and the
					      line feed characteres
					      introduced by DOS
					      systems when reading
					      strings from ASCII
					      files */

void   iftWriteFloatArray(float *v, int size, char *filename);
float *iftReadFloatArray(char *filename, int *size);

/**
 * Evaluates the sigmoid function, with x = value.
 * Alfa controls the decay of the function.
 */

float iftSigmoidalValue(float value, float alfa);

void iftVerifyToken(FILE *fp, char *token, char *function);
void iftReadIntValue(FILE *fp, int *value, char *token, char *function);
void iftReadIntValues(FILE *fp, int **value, int nvalues, char *token, char *function);
void iftWriteIntValue(FILE *fp, int value, char *token);
void iftWriteIntValues(FILE *fp, int *value, int nvalues, char *token);
void iftReadFloatValue(FILE *fp, float *value, char *token, char *function);
void iftReadFloatValues(FILE *fp, float **value, int nvalues, char *token, char *function);
void iftWriteFloatValue(FILE *fp, float value, char *token);
void iftWriteFloatValues(FILE *fp, float *value, int nvalues, char *token);
void iftReadDoubleValue(FILE *fp, double *value, char *token, char *function);
void iftReadDoubleValues(FILE *fp, double **value, int nvalues, char *token, char *function);
void iftWriteDoubleValue(FILE *fp, double value, char *token);
void iftWriteDoubleValues(FILE *fp, double *value, int nvalues, char *token);
void iftSkipComments(FILE *fp);
char iftVoxelsAreEqual(iftVoxel u1, iftVoxel u2);
char iftPointsAreEqual(iftPoint u1, iftPoint u2);

/**
 * Common function to handle arrays
 */

void iftCopyFloatArray(float *array1, float *array2, int nelems);
void iftCopyIntArray(int *array1, int *array2, int nelems);
int *iftMergeIntArray(int *array1, int n1, int *array2, int n2, int *nelems);
int *iftIntArrayOfUniqueElemsTransform(int *array, int *n);
fileList             *iftCreateFileList(void);
void                 iftDestroyFileList(fileList **list);
fileList             *iftGetFiles(char *dirname, char *type);

/* These functions are currently used to communicate with numpy */
void iftWriteRawIntArray(char *filename, int *array, int n);
int* iftReadRawIntArray(char *filename, int n);

float iftMean(float *x, int n);
float iftVar(float *x, int n);
float iftCov(float *x, float *y, int n);

int iftAlmostZero(float x);

#endif