/**
 * @file ica.h
 * @brief PCA and ICA
 * @author Kenichi Kumatani and John McDonough
 */

#ifndef ICA_H
#define ICA_H

#include <gsl/gsl_block.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>

#include "common/refcount.h"
#include "common/jexception.h"

// ----- definition for class `PCA' -----
//
class PCA {
 public:
  PCA(unsigned dimN);
  ~PCA();

  void pca_svd(gsl_matrix* input, gsl_matrix* basis, gsl_vector* eigenVal, gsl_vector* whiten);
  void pca_eigen(gsl_matrix* input, gsl_matrix* basis, gsl_vector* eigenVal, gsl_vector* whiten);

 private:
  const unsigned	_dimN;
  gsl_vector*		_work;
};

typedef refcount_ptr<PCA> PCAPtr;

// ----- definition for class `FastICA' -----
//
class FastICA {
public:
  FastICA(unsigned dimN, unsigned maxIterN);
  ~FastICA();

  void deflation(gsl_matrix* data, gsl_matrix* B, gsl_matrix* A, gsl_matrix* W, gsl_matrix* M,
		 gsl_matrix* neg, double eps, int maxIterN);

 private:
  const unsigned	_dimN;
  const unsigned	_maxIterN;

  gsl_matrix*		_w;
  gsl_matrix*		_a;
  gsl_matrix*		_wr;
  gsl_matrix*		_BTw;
  gsl_matrix*		_BBTw;
  gsl_matrix*		_wOld;
  gsl_matrix*		_wOld2;
  gsl_matrix*		_wSum;
  gsl_matrix*		_wDiff;
  gsl_matrix*		_hypTan;
  gsl_matrix*		_dHypTan;
  gsl_matrix*		_dHypTanT;
  gsl_matrix*		_wDHypTanT;
  gsl_matrix*		_XHypTan;
};

typedef refcount_ptr<FastICA> FastICAPtr;

#endif


#include <stdio.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <gbpLib.h>
#include <gbpRNG.h>
#include <gbpMCMC.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_interp.h>

void read_MCMC_configuration(MCMC_info *MCMC, char *filename_output_dir, int chain) {
    SID_log("Reading the configuration of chain #%d from {%s}...", SID_LOG_OPEN, chain);

    // Set the names of the files we have to read
    char filename_chain_dir[SID_MAX_FILENAME_LENGTH];
    char filename_run[SID_MAX_FILENAME_LENGTH];
    char filename_chain_config[SID_MAX_FILENAME_LENGTH];
    sprintf(filename_chain_dir, "%s/chains/", filename_output_dir);
    sprintf(filename_run, "%s/run.dat", filename_output_dir);
    sprintf(filename_chain_config, "%s/chain_config_%06d.dat", filename_chain_dir, chain);

    // Read the run file to do some sanity checks
    int   n_chains;
    int   n_avg;
    int   flag_autocor_on;
    int   flag_no_map_write;
    int   n_P;
    FILE *fp;
    fp = fopen(filename_run, "r");
    SID_fread_verify(&n_chains, sizeof(int), 1, fp);
    SID_fread_verify(&n_avg, sizeof(int), 1, fp);
    SID_fread_verify(&flag_autocor_on, sizeof(int), 1, fp);
    SID_fread_verify(&flag_no_map_write, sizeof(int), 1, fp);
    SID_fread_verify(&n_P, sizeof(int), 1, fp);
    fclose(fp);
    if(n_P != MCMC->n_P)
        SID_exit_error("The number of parameters for the two runs does not match (ie. %d!=%d)", SID_ERROR_LOGIC, n_P,
                       MCMC->n_P);
    if(chain >= n_chains)
        SID_exit_error("Invalid chain number (ie. %d>%d)", SID_ERROR_LOGIC, chain, n_chains);

    // Read the configuration file
    int     n_iterations_file_total;
    int     n_iterations_file_burn;
    double *V;
    double  temperature;
    V  = (double *)SID_malloc(sizeof(double) * n_P * n_P);
    fp = fopen(filename_chain_config, "r");
    SID_fread_verify(&n_iterations_file_total, sizeof(int), 1, fp);
    SID_fread_verify(&n_iterations_file_burn, sizeof(int), 1, fp);
    SID_fread_verify(V, sizeof(double), n_P * n_P, fp);
    SID_fread_verify(&temperature, sizeof(double), 1, fp);
    fclose(fp);

    // Set the new state
    set_MCMC_covariance(MCMC, V);
    set_MCMC_temperature(MCMC, temperature);

    // Clean-up
    SID_free(SID_FARG V);
    SID_log("Done.", SID_LOG_CLOSE);
}


/**
 * Interface to flatfielding
 */

#ifndef _SPC_FLATFIELD_H
#define _SPC_FLATFIELD_H 1

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

#include <gsl/gsl_spline.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>

#include "aXe_grism.h"
#include "aXe_utils.h"

typedef struct
{
  int poly_order;      /* order of the polynomial */
  gsl_matrix **coeffs; /* the list of gsl_matrix'es containing */
                       /* the polynomial coefficients of the FF */
  double wmin;         /* Minumum wavelength value for the coeff. normalization */
  double wmax;         /* Maximum wavelength value for the coeff. normalization */
}
poly_cube_flatfield;

/**
  Descriptor of flatfield with a single flatfield image and a
  polynomial flatfield dependence.
*/
typedef struct
{
  gsl_matrix *flatfield; /* The flatfield image, giving the relative
                            efficiency for each pixel in the aperture */
  gsl_matrix *errors;	 /* relative errors for flatfield */
  int poly_order;	 /* order of the polynomial */
  double *poly_coeffs;	 /* coefficients for the polynom */
}
polynom_flatfield;

/**
   Descriptor of flatfield with multiple flatfields at different
   wave lengths
*/
typedef struct
{
  int num_flats;                /* number of (lambda,flatfield) pairs */
  gsl_matrix **flatfields;      /* flatfield images, giving the relative
				   efficiency for each pixel in the aperture */
  gsl_matrix **errors;	        /* relative errors for flatfields */
  double *lambdas;              /* wave lengths for entries in flatfields */
}
multi_flatfield;

/**
  Descriptor of a flatfielding function, including the function
  to compute the flat field value of a given pixel, the bounding box
  we handle, and auxillary data (usually a flatfield image)
*/
typedef struct s_flatfield
{
  void (*func) (const double lambda, const int x, const int y,
		PIXEL_T * const val, PIXEL_T * const err,
		const struct s_flatfield * const self);
  int ll_x, ll_y, w, h;	/* bounding box we handle */
  union
  {
    polynom_flatfield *poly;
    multi_flatfield *multi;
  }
  data;
}
flatfield_d;

poly_cube_flatfield * load_flat_poly_cube(char *fname);
void free_flat_poly_cube(poly_cube_flatfield *poly_cube);

flatfield_d *make_poly_flatfield (const beam * const curbeam,
				  const char *const flat_name,
				  const int order,
				  const double *const coeffs);
void free_poly_flatfield (flatfield_d * const flat);
flatfield_d *make_multi_flatfield (const beam * const curbeam,
				   const int num_flats, const double lambda,
				   const char *const flat_name, ...);
void free_multi_flatfield (flatfield_d * const flat);

void apply_flatfield (ap_pixel * const ap_p, const flatfield_d * const flat);
double poly_cube_flatfield_lambda (const double lambda, const int x, const int y,
				   poly_cube_flatfield *poly_cube);

#endif


#include <stdio.h>
#include <stdlib.h>
#include <malloc.h>
#include <string.h>
#include <float.h>
#include <time.h>
#include <errno.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

#include "function.h"
#include "norm.h"

// Exception system configuration
#define ERR_T int
#define ERR_V ERRNO
#define OK 0
#include "exceptions.h"

// Input
#define MAX_BUF 256
#define MATCH(line, str) !strncmp(line, str, strlen(str))

// User options
#define DEFAULT_TOLERANCE 1.0e-12
#define DEFAULT_REL_TOL 1
#define DEFAULT_MAX_ZERO_DIST 1.0e-12
#define DEFAULT_USER_NORM 0
#define DEFAULT_MAX_ITER 25
#define DEFAULT_MAX_DIV_ITER 10
#define DEFAULT_JX_REUSE 5

struct options {
	double tolerance;
	char rel_tol;
	double max_zero_dist;
	char user_norm;
	unsigned int max_iter;
	unsigned int max_div_iter;
	unsigned int jx_reuse;
};

// Result
struct additional_data {
	double max_error;
	double* fx;
	unsigned int iter_count;
	double delta_t;
};

// Function declarations

/*
 * The result will be stored in x and x0 will contain the difference between x
 * and the result of the previous iteration.
 */
ERR_T findroot(int dim, double* x0, double* x, struct options* options,
		struct additional_data* data);

ERR_T norm(int dim, double* x, double* n, struct options* options);

ERR_T input_data(char* path, int* dim, double** x0, struct options* options);

void output_result(int dim, double* result, struct additional_data* data);

void output_vector(int dim, double* x);

void handler(const char* reason, const char* file, int line, int gsl_errno);

// Program

int main(int argc, char** argv) {
	int dim;
	char* path;
	double* x = NULL;
	double* x0 = NULL;
	struct options options;
	struct additional_data additional_data;

	// Save the conf file path
	TRY(1, argc == 2)
	path = argv[1];

	// Initialize options with default data
	options.tolerance = DEFAULT_TOLERANCE;
	options.rel_tol = DEFAULT_REL_TOL;
	options.max_zero_dist = DEFAULT_MAX_ZERO_DIST;
	options.user_norm = DEFAULT_USER_NORM;
	options.max_iter = DEFAULT_MAX_ITER;
	options.max_div_iter = DEFAULT_MAX_DIV_ITER;
	options.jx_reuse = DEFAULT_JX_REUSE;

	// Get conf file data
	TRY(2, input_data(path, &dim, &x0, &options) == OK)

	// Find root
	TRY(3, (x = (double*) malloc(dim * sizeof(double))) != NULL)
	TRY(4, findroot(dim, x0, x, &options, &additional_data) == OK)

	output_result(dim, x, &additional_data);

	RETURN(0)

	EXCEPT(
		case 1:
			printf("Usage: findroot path\n");
			break;
		case 2:
			fprintf(stderr,
					"[x] Ezin izan da konfigurazio fitxategia ondo "
					"irakurri.\n");
			break;
		case 3:
			fprintf(stderr,
					"[x] Ezin izan da memoria nahikoa erreserbatu.\n");
			printf("[?] MEMORIA KOPURUA: %lu\n", dim * sizeof(double));
			break;
		case 4:
			fprintf(stderr, "[x] Ezin izan da emaitza kalkulatu.\n");
			break;
	)

	FINALLY(
		free(x);
		free(x0);
	)
}

ERR_T findroot(int dim, double* x0, double* x, struct options* options,
		struct additional_data* data) {
	int s;
	unsigned int iter_count, iter_div_count, jx_reuse_count;
	double max_err, max_err_prev, zero_dist;
	double* fx = NULL;
	double* jx = NULL;
	clock_t begin, end;

	// INITIALIZATION

	TRY(1, (fx = (double*) malloc(dim * sizeof(double))) != NULL)
	TRY(2, (jx = (double*) malloc(dim * dim * sizeof(double))) != NULL)

	gsl_set_error_handler(&handler);

	gsl_vector_view x_gsl = gsl_vector_view_array(x, dim);
	gsl_vector_view x0_gsl = gsl_vector_view_array(x0, dim);
	gsl_vector_view fx_gsl = gsl_vector_view_array(fx, dim);
	gsl_matrix_view jx_gsl = gsl_matrix_view_array(jx, dim, dim);
	gsl_permutation* p = gsl_permutation_alloc(dim);

	// NEWTON-RAPHSON LOOP
	begin = clock();

	/*
	 * a, b and c are vectors of dim dimensions
	 * FX and JX are square matrixes of dim dimensions
	 *
	 * b = a - c
	 * WHERE JX * c = FX
	 *
	 * a beign the result of the previous iteration (or the initial point)
	 * b beign the result of the current iteration (or the final result)
	 */

	// x0 == a

	memcpy(x, x0, dim * sizeof(double));

	// x == a
	// x0 == a

	iter_count = 0;
	iter_div_count = 0;
	max_err = DBL_MAX;
	max_err_prev = DBL_MAX;
	jx_reuse_count = options->jx_reuse;

	do {
		f(dim, x, fx);

		// Reusage of the Jacobian matrix
		if (jx_reuse_count == options->jx_reuse) {
			jakobiarra(dim, x, jx);
			TRY(3, gsl_linalg_LU_decomp(&jx_gsl.matrix, p, &s) == OK)
			jx_reuse_count = 0;
		} else {
			++jx_reuse_count;
		}

		TRY(4,
			gsl_linalg_LU_solve(
				&jx_gsl.matrix, p, &fx_gsl.vector, &x0_gsl.vector) == OK
		)

		// x == a
		// x0 == c

		TRY(5, gsl_vector_sub(&x_gsl.vector, &x0_gsl.vector) == OK)

		TRY(6, norm(dim, x0, &max_err, options) == OK)

		// Relative error
		if (options->rel_tol) {
			// zero_dist is reused to save memory
			TRY(7, norm(dim, x, &zero_dist, options) == OK)
			max_err /= zero_dist;
		}

		// Zero dist
		TRY(8, norm(dim, fx, &zero_dist, options) == OK)

		// x == b
		// x0 == c

		// UPDATES

		++iter_count;

		// Track divergence iterations
		if (max_err > max_err_prev) {
			++iter_div_count;
		} else {
			iter_div_count = 0;
		}
		max_err_prev = max_err;

	} while ((max_err > options->tolerance ||
			zero_dist > options->max_zero_dist) &&
			iter_count < options->max_iter &&
			iter_div_count < options->max_div_iter);

	end = clock();

	if (iter_count == options->max_iter)
		printf("[!] Iterazio mugara iritsi da.\n");
	if (iter_div_count == options->max_div_iter)
		printf("[!] Iterazio dibergente mugara iritsi da.\n");

	data->max_error = max_err;
	data->fx = fx;
	data->iter_count = iter_count;
	data->delta_t = (double) (end - begin) / (double) CLOCKS_PER_SEC;

	EXCEPT(
		case 1:
			fprintf(stderr,
					"[x] Ezin izan da memoria nahikoa erreserbatu.\n");
			printf("[?] MEMORIA KOPURUA: %lu\n", dim * sizeof(double));
			break;
		case 2:
			fprintf(stderr,
					"[x] Ezin izan da memoria nahikoa erreserbatu.\n");
			printf("[?] MEMORIA KOPURUA: %lun", dim * dim * sizeof(double));
			break;
		case 3:
			fprintf(stderr,
					"[x] Ezin izan da JX * x = FX ekuazio sistema "
					"linealaren LU deskonposaketa egin.\n");
			printf("[?] HURBILPEN PARTZIALA: ");
			output_vector(dim, x);
			printf("[?] ERRORE MAXIMOA: %.*g\n", DBL_DIG, max_err);
			printf("[?] F(X): ");
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
		case 4:
			fprintf(stderr,
					"[x] Ezin izan da JX * x = FX ekuazio sistema lineala "
					"ebatzi LU deskonposaketa erabiliz.\n");
			printf("[?] HURBILPEN PARTZIALA: ");
			output_vector(dim, x);
			printf("[?] ERRORE MAXIMOA: %.*g\n", DBL_DIG, max_err);
			printf("[?] F(X): ");
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
		case 5:
			fprintf(stderr,
					"[x] Bektoreen arteko kenketa egitean errore kritiko "
					"bat egon da.");
			printf("[?] F(X): ");
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
		case 6:
			fprintf(stderr,
					"[x] JX * x = FX ekuazio sistema linealaren emaitzaren "
					"norma kalkulatzean errore kritiko bat egon da.\n");
			printf("[?] F(X): ");
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
		case 7:
			fprintf(stderr,
					"[x] x-ren norma kalkulatzean errore kritiko bat egon "
					"da.\n");
			printf("[?] HURBILPEN PARTZIALA: ");
			output_vector(dim, x);
			printf("[?] F(X): ");
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
		case 8:
			fprintf(stderr,
					"[x] f(x)-ren norma kalkulatzean errore kritiko bat egon "
					"da.\n");
			printf("[?] HURBILPEN PARTZIALA: ");
			output_vector(dim, x);
			printf("[?] ERRORE MAXIMOA: %.*g\n", DBL_DIG, max_err);
			output_vector(dim, fx);
			printf("[?] ITERAZIO_KOPURUA: %u\n", iter_count);
			break;
	)

	FINALLY(
		free(fx);
		free(jx);
		gsl_permutation_free(p);
	)
}

ERR_T norm(int dim, double* x, double* n, struct options* options) {
	if (options->user_norm) {
		norma(dim, x, n);
	} else {
		gsl_vector_view x_gsl = gsl_vector_view_array(x, dim);
		TRY(1, (*n = gsl_blas_dnrm2(&x_gsl.vector)) >= 0.0)
	}

	EXCEPT(
		case 1:
			fprintf(stderr, "[x] Ezin izan da norma kalkulatu, posible da "
					"bektorea handiegia izatea.\n");
			break;
	)

	FINALLY()
}

ERR_T input_data(char* path, int* dim, double** x0, struct options* options) {
	FILE* f;
	char line[MAX_BUF];
	int count = 0;

	TRY(1, (f = fopen(path, "r")) != NULL)

	// Read dimension
	*dim = 0;
	while (fgets(line, MAX_BUF, f) != NULL) {
		if (line[0] != '#' && (line[0] != '\0')) {
			if (MATCH(line, "dimentsioa")) {
				TRY(2, sscanf(line, "dimentsioa %d", dim) == 1)
				TRY(3, *dim > 0)
				break;
			}
		}
	}

	TRY(4, *dim != 0)

	TRY(5, (*x0 = (double*) malloc((*dim) * sizeof(double))) != NULL)

	// Read x0 and optional parameters
	while (fgets(line, MAX_BUF, f) != NULL) {
		if (line[0] != '#' && (line[0] != '\0')) {
			if (MATCH(line, "tolerantzia")) {
				TRY(6,
					sscanf(line, "tolerantzia %lf", &(options->tolerance)) == 1)
				TRY(7, options->tolerance > 0.0)
			} else if (MATCH(line, "erlatiboa")) {
				if (MATCH(line, "erlatiboa bai")) {
					options->rel_tol = 1;
				} else if (MATCH(line, "erlatiboa ez")) {
					options->rel_tol = 0;
				} else {
					TRY(8, 0);
				}
			} else if (MATCH(line, "max_zero_distantzia")) {
				TRY(9,
					sscanf(line, "max_zero_distantzia %lf",
							&(options->max_zero_dist)) == 1)
				TRY(10, options->max_zero_dist > 0.0)
			} else if (MATCH(line, "ordezko_norma")) {
				if (MATCH(line, "ordezko_norma bai")) {
					options->user_norm = 1;
				} else if (MATCH(line, "ordezko_norma ez")) {
					options->user_norm = 0;
				} else {
					TRY(11, 0);
				}
			} else if (MATCH(line, "iterazio_maximoa")) {
				TRY(12,	sscanf(line, "iterazio_maximoa %u",
							&(options->max_iter)) == 1)
				TRY(13, options->max_iter != 0)
			} else if (MATCH(line, "dibergentzia_iterazio_maximoa")) {
				TRY(14, sscanf(line, "dibergentzia_iterazio_maximoa %u",
						&(options->max_div_iter)) == 1)
			} else if (MATCH(line, "jakobiar_berrerabilpena")) {
				TRY(15, sscanf(line, "jakobiar_berrerabilpena %u",
							&(options->jx_reuse)) == 1)
			} else {
				TRY(16, count <= *dim)
				TRY(17, sscanf(line, "%lf", (*x0) + count) == 1)
				++count;
			}
		}
	}

	TRY(18, count == *dim)

	EXCEPT(
		case 1:
			fprintf(stdout,
					"[x] Ezin izan da konfigurazio fitxategia ireki.\n");
			break;
		case 2:
			fprintf(stdout,
					"[x] Sintaxi desegokia dimentsioa emateko lerroan\n");
			printf("[?] LERROA: %s", line);
			break;
		case 3:
			fprintf(stdout,
					"[x] Dimentsioak zero baina handiagoa izan behar du.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 4:
			fprintf(stdout, "[x] Ez da aurkitu dimentsioa.\n");
			break;
		case 5:
			fprintf(stdout,
					"[x] Ezin izan da X0-rentzat memoria erreserbatu\n.");
			printf("[?] MEMORIA KOPURUA: %lu", (*dim) * sizeof(double));
			break;
		case 6:
			fprintf(stdout,
					"[x] Sintaxi desegokia tolerantzia emateko lerroan\n.");
			printf("[?] LERROA: %s", line);
			break;
		case 7:
			fprintf(stdout,
					"[x] Tolerantziak zero baina handiagoa izan behar du.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 8:
			fprintf(stdout,
					"[x] Sintaxi desegokia tolerantzia erlatiboa aukeratzeko "
					"lerroan. Aukera honek 'bai' eta 'ez' balioak hartu "
					"ditzake soilik.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 9:
			fprintf(stdout,
					"[x] Sintaxi okerra zerotik distantzia maximoa emateko "
					"lerroan.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 10:
			fprintf(stdout,
					"[x] Zerotik distantzia maximoak zero baina handiagoa izan "
					"behar du.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 11:
			fprintf(stdout,
					"[x] Sintaxi desegokia ordezko norma aukeratzeko "
					"lerroan. Aukera honek 'bai' eta 'ez' balioak hartu "
					"ditzake soilik.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 12:
			fprintf(stdout,
					"[x] Sintaxi okerra iterazio kopuru maximoa emateko "
					"lerroan. Kopuruak ezin du negatiboa izan.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 13:
			fprintf(stdout,
					"[x] Iterazio kopuru maximoak ezin du zero izan.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 14:
			fprintf(stdout,
					"[x] Sintaxi okerra iterazio dibergente kopuru maximoa "
					"emateko lerroan. Kopuruak ezin du negatiboa izan.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 15:
			fprintf(stdout,
					"[x] Sintaxi okerra jakobiarraren berrerabilpen ratioa "
					"emateko lerroan. Ratioak ezin du negatiboa izan.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 16:
			fprintf(stdout,
					"[x] X0-ren elementu kopurura dimentsioa baina handiagoa "
					"da.\n");
			printf("[?] DIMENTSIOA: %i\n", *dim);
			break;
		case 17:
			fprintf(stdout,
					"[x] Sintaxi desegokia.\n");
			printf("[?] LERROA: %s", line);
			break;
		case 18:
			fprintf(stdout,
					"[x] Dimentsioa eta X0-ren tamaina ez datoz bat.\n");
			printf("[?] DIMENTSIOA: %i\n", *dim);
			printf("[?] TAMAINA: %i\n", count);
			break;
		case 19:
			fprintf(stdout,
					"[x] Konfigurazio fitxategia ixtean errore kritiko bat "
					"egon da.\n");
			break;
	)

	FINALLY(
		if (f != NULL) {
			TRY(19, fclose(f) == OK);

			// On fail, so that it doesn't try again and again
			f = NULL;
		}
	)
}

void output_result(int dim, double* result, struct additional_data* data) {
	printf("\n");

	printf("Erroa: ");
	output_vector(dim, result);

	printf("Errore maximoa: %.*g\n", DBL_DIG, data->max_error);

	printf("Erroaren irudia: ");
	output_vector(dim, data->fx);

	printf("Iterazio kopurua: %u\n", data->iter_count);

	printf("Denbora: %.*g seg\n", DBL_DIG, data->delta_t);

	printf("\n");
}

void output_vector(int dim, double* x) {
	int i;

	printf("(");
	printf("%.*g", DBL_DIG, x[0]);
	for (i = 1; i < dim; ++i)
		printf(", %.*g", DBL_DIG, x[i]);
	printf(")\n");
}

void handler(const char* reason, const char* file, int line, int gsl_errno) {
	fprintf(stderr, "[x] ");

	switch(gsl_errno) {
		case GSL_EDOM:
			fprintf(stderr, "GSL DOMAIN ERROR: ");
			break;
		case GSL_ERANGE:
			fprintf(stderr, "GSL RANGE ERROR: ");
			break;
		case GSL_ENOMEM:
			fprintf(stderr, "GSL NO MEMORY AVAILABLE: ");
			break;
		case GSL_EINVAL:
			fprintf(stderr, "GSL INVALID ARGUMENT: ");
			break;
		default:
			fprintf(stderr, "GSL ERROR: ");
			break;
	}

	fprintf(stderr, "%s\n", reason);
}


#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <time.h>
#include <mpfr.h>
#include <petsc.h>

#include "ellipsoid/ellipsoid.h"
#include "tanhsinh.h"

#undef __FUNCT__
#define __FUNCT__ "NormConstantIntFixed"
PetscErrorCode NormConstantIntFixed(EllipsoidalSystem *e, PetscInt n, PetscInt p, PetscInt prec, PetscInt nPts, PetscReal *intVals, PetscReal *normConst)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  FuncInfo2 ctx1 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = 1 };
  FuncInfo2 ctx2 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = 1 };
  FuncInfo2 ctx3 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = -1 };
  FuncInfo2 ctx4 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = -1 };

  PetscReal integrals[4];

  mpfr_t mpfrzero;
  mpfr_t mpfrone;
  mpfr_inits(mpfrzero, mpfrone, NULL);
  mpfr_set_d(mpfrzero, 0.0, MPFR_RNDN);
  mpfr_set_d(mpfrone, 1.0, MPFR_RNDN);

  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+0, &ctx1);
  intVals[0] = integrals[0];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+1, &ctx2);
  intVals[1] = integrals[1];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+2, &ctx3);
  intVals[2] = integrals[2];
  ierr = DEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+3, &ctx4);
  intVals[3] = integrals[3];
  *normConst = 8.0*(integrals[2]*integrals[1] - integrals[0]*integrals[3]); flopCount += 4;

  mpfr_clears(mpfrzero, mpfrone, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "NormConstantIntFixedSE"
PetscErrorCode NormConstantIntFixedSE(EllipsoidalSystem *e, PetscInt n, PetscInt p, PetscInt prec, PetscInt nPts, PetscReal *normConst)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  FuncInfo2 ctx1 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = 1 };
  FuncInfo2 ctx2 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = 1 };
  FuncInfo2 ctx3 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = -1 };
  FuncInfo2 ctx4 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = -1 };

  PetscReal integrals[4];

  mpfr_t mpfrzero;
  mpfr_t mpfrone;
  mpfr_inits(mpfrzero, mpfrone, NULL);
  mpfr_set_d(mpfrzero, 0.0, MPFR_RNDN);
  mpfr_set_d(mpfrone, 1.0, MPFR_RNDN);

  ierr = SEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+0, &ctx1);

  ierr = SEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+1, &ctx2);

  ierr = SEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+2, &ctx3);

  ierr = SEQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+3, &ctx4);

  *normConst = 8.0*(integrals[2]*integrals[1] - integrals[0]*integrals[3]); flopCount += 4;

  mpfr_clears(mpfrzero, mpfrone, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "NormConstantIntFixedERF"
PetscErrorCode NormConstantIntFixedERF(EllipsoidalSystem *e, PetscInt n, PetscInt p, PetscInt prec, PetscInt nPts, PetscReal *normConst)
{
  PetscErrorCode ierr;
  PetscInt flopCount;
  PetscFunctionBegin;
  flopCount = 0;

  FuncInfo2 ctx1 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = 1 };
  FuncInfo2 ctx2 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = 1 };
  FuncInfo2 ctx3 = { .e = e, .n = n, .p = p, .numeratorType = 0, .denomSign = -1 };
  FuncInfo2 ctx4 = { .e = e, .n = n, .p = p, .numeratorType = 1, .denomSign = -1 };

  PetscReal integrals[4];

  mpfr_t mpfrzero;
  mpfr_t mpfrone;
  mpfr_inits(mpfrzero, mpfrone, NULL);
  mpfr_set_d(mpfrzero, 0.0, MPFR_RNDN);
  mpfr_set_d(mpfrone, 1.0, MPFR_RNDN);

  ierr = ERFQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+0, &ctx1);
  //ierr = ERFQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) inte, mpfrzero, mpfrone, prec, nPts, integrals+0, &ctx1);
  //printf("inte: %15.15f\n", integrals[0]);
  ierr = ERFQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, e->hp_h, e->hp_k, prec, nPts, integrals+1, &ctx2);

  ierr = ERFQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+2, &ctx3);

  ierr = ERFQuad((PetscErrorCode (*)(mpfr_t*, mpfr_t*, void*)) normFunction1, mpfrzero, e->hp_h, prec, nPts, integrals+3, &ctx4);

  *normConst = 8.0*(integrals[2]*integrals[1] - integrals[0]*integrals[3]); flopCount += 4;

  mpfr_clears(mpfrzero, mpfrone, NULL);

  ierr = PetscLogFlops(flopCount);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "NormPlot"
PetscErrorCode NormPlot()
{
  const PetscInt MPFR_PREC = 128;
  const PetscInt NUM_SOLUTIONS = 100;
  const PetscInt POINTS_MIN = 4;
  const PetscInt POINTS_STEP = 2;
  const PetscInt prec = 16;
  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;
  PetscReal solExact;
  PetscReal intExact[4];
  PetscReal solutions[NUM_SOLUTIONS];
  PetscReal errors   [NUM_SOLUTIONS];
  PetscReal intSols[4*NUM_SOLUTIONS];
  PetscReal intErrors[4*NUM_SOLUTIONS];
  PetscReal sol2, sol3, sol4;
  PetscLogEvent flopCounts[NUM_SOLUTIONS];
  EllipsoidalSystem e;
  PetscErrorCode ierr;
  PetscInt i;
  PetscEventPerfInfo info;
  PetscInt n = 21;
  PetscInt p = 22;
  PetscFunctionBegin;

  mpfr_set_default_prec(4*MPFR_PREC);
  initEllipsoidalSystem(&e, a, b, c, MPFR_PREC);

  /* calculate "exact" solution */
  mpfr_t exactsol;
  mpfr_init(exactsol);
  ierr = calcNormalizationMPFR(&e, n, p, &exactsol);
  ierr = calcNormalization(&e, n, p, &solExact);CHKERRQ(ierr);
  ierr = calcNormalization2(&e, n, p, intExact, &solExact);CHKERRQ(ierr);
  solExact = mpfr_get_d(exactsol, MPFR_RNDN);
  printf("old norm constant: %15.15f\n", solExact);

  /* calculate approximate solutions and record flops */
  char text[40] = "%d points";
  char sText[40];
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    sprintf(sText, text, POINTS_MIN + POINTS_STEP*i);
    ierr = PetscLogEventRegister(sText, 0, flopCounts+i);CHKERRQ(ierr);
    ierr = PetscLogEventBegin(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = NormConstantIntFixed(&e, n, p, prec, POINTS_MIN + POINTS_STEP*i, intSols+4*i, solutions+i);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    printf("new norm constant: %15.15f\n", solutions[i]);
  }

  /* calculate errors */
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    errors[i] = PetscAbsReal((solExact - solutions[i])/solExact);
    intErrors[4*i+0] = PetscAbsReal((intExact[0] - intSols[4*i+0])/intExact[0]);
    intErrors[4*i+1] = PetscAbsReal((intExact[1] - intSols[4*i+1])/intExact[1]);
    intErrors[4*i+2] = PetscAbsReal((intExact[2] - intSols[4*i+2])/intExact[2]);
    intErrors[4*i+3] = PetscAbsReal((intExact[3] - intSols[4*i+3])/intExact[3]);
    printf("errors[%d] = %15.15f\n", i, errors[i]);
  }

  FILE *fp1 = fopen("out/normInt1Prec.txt", "w");
  FILE *fp2 = fopen("out/normInt2Prec.txt", "w");
  FILE *fp3 = fopen("out/normInt3Prec.txt", "w");
  FILE *fp4 = fopen("out/normInt4Prec.txt", "w");
  FILE *fp = fopen("out/normWorkPrec.txt", "w");
  fprintf(fp, "points flops error\n");
  fprintf(fp1, "points error\n");
  fprintf(fp2, "points error\n");
  fprintf(fp3, "points error\n");
  fprintf(fp4, "points error\n");
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, flopCounts[i], &info);CHKERRQ(ierr);
    fprintf(fp, "%d %4.4e %4.4e\n", POINTS_MIN + POINTS_STEP*i, info.flops, errors[i]);
    fprintf(fp1, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+0]);
    fprintf(fp2, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+1]);
    fprintf(fp3, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+2]);
    fprintf(fp4, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+3]);
  }
  fclose(fp);
  fclose(fp1);
  fclose(fp2);
  fclose(fp3);
  mpfr_clear(exactsol);

  PetscFunctionReturn(0);
}

/*
NormPlot2 - plots convergence of a normalization
            constant for three different ellipsoidal
	    systems to compare convergence
 */

#undef __FUNCT__
#define __FUNCT__ "NormPlot2"
PetscErrorCode NormPlot2()
{
  const PetscInt NUM_SOLUTIONS = 43;
  const PetscInt POINTS_MIN = 4;
  const PetscInt POINTS_STEP = 2;
  const PetscInt prec = 16;
  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;
  PetscReal solExact;
  PetscReal intExact[4];
  PetscReal solutions[NUM_SOLUTIONS];
  PetscReal errors   [NUM_SOLUTIONS];
  PetscReal intSols[4*NUM_SOLUTIONS];
  PetscReal intErrors[4*NUM_SOLUTIONS];
  PetscReal sol2, sol3, sol4;
  PetscLogEvent flopCounts[NUM_SOLUTIONS];
  EllipsoidalSystem e1, e2, e3;
  EllipsoidalSystem *e;
  PetscErrorCode ierr;
  PetscInt i;
  PetscEventPerfInfo info;
  PetscInt n = 3;
  PetscInt p = 5;
  PetscFunctionBegin;

  ierr = initEllipsoidalSystem(&e1, 2.5, 2.0, 1.0, 64);CHKERRQ(ierr);
  ierr = initEllipsoidalSystem(&e2, 2.5, 2.0, 1.5, 64);CHKERRQ(ierr);
  ierr = initEllipsoidalSystem(&e3, 2.5, 2.0, 1.89, 64);CHKERRQ(ierr);

  for(PetscInt num=0; num<3; ++num) {
    if(num==0)
      e = &e1;
    else if(num==1)
      e = &e2;
    else
      e = &e3;
    /* calculate "exact" solution */
    ierr = calcNormalization(e, n, p, &solExact);CHKERRQ(ierr);
    ierr = calcNormalization2(e, n, p, intExact, &solExact);CHKERRQ(ierr);
    printf("old norm constant: %15.15f\n", solExact);

    /* calculate approximate solutions and record flops */
    char text[40] = "%d points";
    char sText[40];
    for(i=0; i<NUM_SOLUTIONS; ++i) {
      sprintf(sText, text, POINTS_MIN + POINTS_STEP*i);
      ierr = PetscLogEventRegister(sText, 0, flopCounts+i);CHKERRQ(ierr);
      ierr = PetscLogEventBegin(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
      ierr = NormConstantIntFixed(e, n, p, prec, POINTS_MIN + POINTS_STEP*i, intSols+4*i, solutions+i);CHKERRQ(ierr);
      ierr = PetscLogEventEnd(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
      printf("new norm constant: %15.15f\n", solutions[i]);
    }

    /* calculate errors */
    for(i=0; i<NUM_SOLUTIONS; ++i) {
      errors[i] = PetscAbsReal((solExact - solutions[i])/solExact);
      intErrors[4*i+0] = PetscAbsReal((intExact[0] - intSols[4*i+0])/intExact[0]);
      intErrors[4*i+1] = PetscAbsReal((intExact[1] - intSols[4*i+1])/intExact[1]);
      intErrors[4*i+2] = PetscAbsReal((intExact[2] - intSols[4*i+2])/intExact[2]);
      intErrors[4*i+3] = PetscAbsReal((intExact[3] - intSols[4*i+3])/intExact[3]);
      printf("errors[%d] = %15.15f\n", i, errors[i]);
    }
    FILE *fp1, *fp2, *fp3, *fp4, *fp;
    if(num==0) {
      fp1 = fopen("out/e1normInt1Prec.txt", "w");
      fp2 = fopen("out/e1normInt2Prec.txt", "w");
      fp3 = fopen("out/e1normInt3Prec.txt", "w");
      fp4 = fopen("out/e1normInt4Prec.txt", "w");
      fp = fopen("out/e1normWorkPrec.txt", "w");
    }
    else if(num==1) {
      fp1 = fopen("out/e2normInt1Prec.txt", "w");
      fp2 = fopen("out/e2normInt2Prec.txt", "w");
      fp3 = fopen("out/e2normInt3Prec.txt", "w");
      fp4 = fopen("out/e2normInt4Prec.txt", "w");
      fp = fopen("out/e2normWorkPrec.txt", "w");
    }
    else {
      fp1 = fopen("out/e3normInt1Prec.txt", "w");
      fp2 = fopen("out/e3normInt2Prec.txt", "w");
      fp3 = fopen("out/e3normInt3Prec.txt", "w");
      fp4 = fopen("out/e3normInt4Prec.txt", "w");
      fp = fopen("out/e3normWorkPrec.txt", "w");
    }
    fprintf(fp, "points flops error\n");
    fprintf(fp1, "points error\n");
    fprintf(fp2, "points error\n");
    fprintf(fp3, "points error\n");
    fprintf(fp4, "points error\n");
    for(i=0; i<NUM_SOLUTIONS; ++i) {
      ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, flopCounts[i], &info);CHKERRQ(ierr);
      fprintf(fp, "%d %4.4e %4.4e\n", POINTS_MIN + POINTS_STEP*i, info.flops, errors[i]);
      fprintf(fp1, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+0]);
      fprintf(fp2, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+1]);
      fprintf(fp3, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+2]);
      fprintf(fp4, "%d %4.4e\n", POINTS_MIN + POINTS_STEP*i, intErrors[4*i+3]);
    }
    fclose(fp);
    fclose(fp1);
    fclose(fp2);
    fclose(fp3);

  }

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "NormPlotSE"
PetscErrorCode NormPlotSE()
{
  const PetscInt NUM_SOLUTIONS = 60;
  const PetscInt POINTS_MIN = 4;
  const PetscInt POINTS_STEP = 2;
  const PetscInt prec = 16;
  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;
  PetscReal solOld;
  PetscReal solutions[NUM_SOLUTIONS];
  PetscReal errors   [NUM_SOLUTIONS];
  PetscReal sol2, sol3, sol4;
  PetscLogEvent flopCounts[NUM_SOLUTIONS];
  EllipsoidalSystem e;
  PetscErrorCode ierr;
  PetscInt i;
  PetscEventPerfInfo info;
  PetscInt n = 3;
  PetscInt p = 5;
  PetscFunctionBegin;

  initEllipsoidalSystem(&e, a, b, c, 64);

  /* calculate approximate solutions and record flops */
  char text[40] = "%d points";
  char sText[40];
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    sprintf(sText, text, POINTS_MIN + POINTS_STEP*i);
    ierr = PetscLogEventRegister(sText, 0, flopCounts+i);CHKERRQ(ierr);
    ierr = PetscLogEventBegin(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = NormConstantIntFixedSE(&e, n, p, prec, POINTS_MIN + POINTS_STEP*i, solutions+i);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    printf("new norm constant (N=%d): %15.15f\n", POINTS_MIN+POINTS_STEP*i, solutions[i]);
  }

  /* calculate "exact" solution */
  ierr = calcNormalization(&e, n, p, &solOld);CHKERRQ(ierr);
  printf("old norm constant: %15.15f\n", solOld);

  /* calculate errors */
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    errors[i] = PetscAbsReal((solOld - solutions[i])/solOld);
    printf("errors[%d] = %15.15f\n", i, errors[i]);
  }

  FILE *fp = fopen("out/normWorkPrecSE.txt", "w");
  fprintf(fp, "points flops error\n");
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, flopCounts[i], &info);CHKERRQ(ierr);
    fprintf(fp, "%d %4.4e %4.4e\n", POINTS_MIN + POINTS_STEP*i, info.flops, errors[i]);
  }
  fclose(fp);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "NormPlotERF"
PetscErrorCode NormPlotERF()
{
  const PetscInt NUM_SOLUTIONS = 50;
  const PetscInt POINTS_MIN = 4;
  const PetscInt POINTS_STEP = 2;
  const PetscInt prec = 16;
  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;
  PetscReal solOld;
  PetscReal solutions[NUM_SOLUTIONS];
  PetscReal errors   [NUM_SOLUTIONS];
  PetscReal sol2, sol3, sol4;
  PetscLogEvent flopCounts[NUM_SOLUTIONS];
  EllipsoidalSystem e;
  PetscErrorCode ierr;
  PetscInt i;
  PetscEventPerfInfo info;
  PetscInt n = 5;
  PetscInt p = 3;
  PetscFunctionBegin;

  initEllipsoidalSystem(&e, a, b, c, 64);

  /* calculate "exact" solution */
  ierr = calcNormalization(&e, n, p, &solOld);CHKERRQ(ierr);
  printf("old norm constant: %15.15f\n", solOld);

  /* calculate approximate solutions and record flops */
  char text[40] = "%d points";
  char sText[40];
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    sprintf(sText, text, POINTS_MIN + POINTS_STEP*i);
    ierr = PetscLogEventRegister(sText, 0, flopCounts+i);CHKERRQ(ierr);
    ierr = PetscLogEventBegin(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = NormConstantIntFixedERF(&e, n, p, prec, POINTS_MIN + POINTS_STEP*i, solutions+i);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(flopCounts[i], 0, 0, 0, 0);CHKERRQ(ierr);
    printf("new norm constant (N=%d): %15.15f\n", POINTS_MIN+POINTS_STEP*i, solutions[i]);
  }

  /* calculate errors */
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    errors[i] = PetscAbsReal((solOld - solutions[i])/solOld);
    printf("errors[%d] = %15.15f\n", i, errors[i]);
  }

  FILE *fp = fopen("out/normWorkPrecERF.txt", "w");
  fprintf(fp, "points flops error\n");
  for(i=0; i<NUM_SOLUTIONS; ++i) {
    ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, flopCounts[i], &info);CHKERRQ(ierr);
    fprintf(fp, "%d %4.4e %4.4e\n", POINTS_MIN + POINTS_STEP*i, info.flops, errors[i]);
  }
  fclose(fp);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "main"
PetscErrorCode main(int argc, char **argv)
{

  const PetscInt NUM_DIGITS = 128;
  const PetscInt PRECISION  = 32;
  mpfr_t a, b;
  mpfr_t val;
  PetscErrorCode ierr;
  PetscFunctionBeginUser;
  const PetscInt nPts = 10;

  /* initializations */
  ierr = PetscInitialize(&argc, &argv, NULL, NULL);CHKERRQ(ierr);
  ierr = PetscLogDefaultBegin();CHKERRQ(ierr);

  mpfr_set_default_prec(4*NUM_DIGITS);
  mpfr_inits(a, b, val, NULL);
  mpfr_set_d(a, 2.0, MPFR_RNDN);
  mpfr_set_d(b, 3.0, MPFR_RNDN);

  /*
  int fac = 1;
  for(int i=0; i<10; ++i) {
    printf("\n########################################################\n");
    ierr = DEQuad(inte, a, b, PRECISION, fac*nPts, &val, NULL);CHKERRQ(ierr);
    fac *= 2;
  }
  */

  ierr = NormPlot();CHKERRQ(ierr);
  //ierr = NormPlot2();CHKERRQ(ierr);
  //ierr = NormPlotSE();CHKERRQ(ierr);
  //ierr = NormPlotERF();CHKERRQ(ierr);

  ierr = PetscFinalize();CHKERRQ(ierr);
  PetscFunctionReturn(0);
}


#include <stdio.h>
#include "sweeny_uf.h"
#include <ctype.h>
#include <strings.h>
#include <stdio.h>
#include "../src/uf.h"
#include "../src/queue.h"
#include <math.h>
#include <gsl/gsl_rng.h> //Verwendung von Mersenne-Twister Pseudozufallszahlengenerator
static __s8 dN;

#define MIN(a,b) a <= b ? a: b
#define MAX(a,b) a >= b? a: b

static char setup=0;
static char verbose=0;
static double rcWeights[4]; // array with precalculated mc weights
static double p_min_del,p_max_del, p_min_ins,p_max_ins;
static struct queue collect1,collect2;
static struct queue_node *todo_pool, *collect_pool;
static __u32 offset_1=1,offset_2=2;
static __u64 *sec_cs_moment,*four_cs_moment;
static __u32 *size_giant;
static __u32 *num_bonds;
static __u32 *num_cluster;
static __u32 DX;
static __u32 N;
static __u32 seed;
static double q;
static double coupling;
static double beta;
static double v;
static double K;
static __u32 steps;
static __s8 *bonds;
static gsl_rng * r;
static __u32 edge[2];
static __s32 adj1[4];
static __s32 adj2[4];
static __s32 *uf1;
static __u32 cs1,cs2;
static __u32 activeEdges=0;
static __u32 cutoff;
/******************************************************************************
 *****************************************************************************/
static char init(void)
{
  K=coupling*beta;
  v = exp(K) - 1.0;
  N = DX*DX;
  rcWeights[0] = MIN(pow(v,-1),1); //db==-1,dN==0
  rcWeights[1] = MIN(pow(v,-1)*q,1); //db==-1,dN=1
  rcWeights[2] = MIN(v,1); //db==1,dN==0
  rcWeights[3] = MIN(v*pow(q,-1),1); //db==1,dN==-1
  p_min_del = MIN(rcWeights[0],rcWeights[1]);
  p_max_del = MAX(rcWeights[0],rcWeights[1]);
  p_min_ins = MIN(rcWeights[2],rcWeights[3]);
  p_max_ins = MAX(rcWeights[2],rcWeights[3]);
  if(verbose) {
	  printf("p_min_del = %f\n",p_min_del);
	  printf("p_max_del = %f\n",p_max_del);
	  printf("p_min_ins = %f\n",p_min_ins);
	  printf("p_max_ins = %f\n",p_max_ins);

  }
  r = gsl_rng_alloc(gsl_rng_mt19937);
  if(!r)
      return 0;
  gsl_rng_set(r,seed);
  __u32 i;
  uf1 = initUF(N);
  todo_pool = malloc(N*sizeof(struct queue_node));
  if(!todo_pool) {
      return 0;
  }
  collect_pool = malloc(N*sizeof(struct queue_node));
  if(!collect_pool) {
      return 0;
  }
  for(i=0;i<N;++i) {

      collect_pool[i].next = todo_pool[i].next = NULL;
      collect_pool[i].data = todo_pool[i].data= i;
      collect_pool[i].visited = todo_pool[i].visited = 0;
    }

  bonds = malloc(sizeof(*bonds)*2*N);
  if(!bonds) {
    return 0;
  }
  for(i=0;i<2*N;i++) bonds[i] = -1;
  return 1;
}
/******************************************************************************
 *****************************************************************************/
static void destroy(void)
{
  destroyUF(uf1);
  free(bonds);
  gsl_rng_free(r);
  free(collect_pool);
  free(todo_pool);
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXnext(__u32 idx)
{
  if((idx+1) % DX)
    return idx+1;
  else
    return idx +1- DX ;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcXprev(__u32 idx)
{
  if((idx%DX))
    return idx-1;
  else
    return idx + DX  -1;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcYnext(__u32 idx)
{

  if(idx <= N -1 && idx >= N - DX)
    return idx +DX- N ;
  else
    return idx+DX;
}
/******************************************************************************
 *****************************************************************************/
static inline __u32 ltcYprev(__u32 idx)
{
  if(idx <= DX - 1)
    return idx + N - DX;
  else
    return idx - DX;

}
/******************************************************************************
 *****************************************************************************/
static void Adjacent(__u32 bidx)
{

  if(bidx%2) {
    bidx = (bidx -1)/2;
    edge[0] = bidx;
    edge[1] = ltcXnext(bidx);
  }
  else {
    bidx = bidx/2;
    edge[0] = bidx;
    edge[1] = ltcYprev(bidx);
  }

}
/******************************************************************************
 *****************************************************************************/
static void Adjacent2(__u32 idx, __u8 a) {
  if(a == 1)
  {
    if(bonds[2*idx] == 1)  adj1[0] = ltcYprev(idx);
    else  adj1[0] = -1;
    if(bonds[(2*idx)+1] == 1)  adj1[1] = ltcXnext(idx);
    else adj1[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj1[2] = ltcYnext(idx);
    else adj1[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] ==1)adj1[3] = ltcXprev(idx);
    else adj1[3] = -1;
  }
  else {
    if(bonds[2*idx] == 1)adj2[0] = ltcYprev(idx);
    else adj2[0] = -1;
    if(bonds[(2*idx)+1] == 1)adj2[1] = ltcXnext(idx);
    else adj2[1] = -1;
    if(bonds[2*ltcYnext(idx)] == 1)	adj2[2] = ltcYnext(idx);
    else adj2[2] = -1;
    if(bonds[(2*ltcXprev(idx))+1] == 1)adj2[3] = ltcXprev(idx);
    else adj2[3] = -1;
  }

}
/******************************************************************************
 *****************************************************************************/
static __u8 breadthFirstSearch(__u32 start, __u32 goal,__u8 accept_split)
{

    static struct queue todo1,todo2;
  __u32 i=0,activeP1=0,activeP2=0;
  init_queue(&todo1,todo_pool);
  init_queue(&todo2,todo_pool);
  init_queue(&collect1,collect_pool);
  init_queue(&collect2,collect_pool);
  cs1=0;
  cs2=0;
  enqueue(&todo1,start); // Put starting point onto queue 1
  enqueue(&collect1,start);
  cs1++; // Increase cluster size of bfs 1
  collect_pool[start].visited = offset_1;
  enqueue(&todo2,goal); // Put goal point onto queue 2
  enqueue(&collect2,goal);
  collect_pool[goal].visited = offset_2;
  cs2++; // increase cluster size of bfs 2
    while(!queue_empty_p(&todo1) && !queue_empty_p(&todo2)) {
        dequeue(&todo2,&activeP2); // get next vertex of bfs 2
        Adjacent2(activeP2,2);	 // get all adjacent vertices of current vertex
        for(i=0;i<4;i++) {
            if(adj2[i] != -1) { // if accessable (edge exists)
                if(collect_pool[adj2[i]].visited == offset_2) continue; // already visited
                if((__u32)adj2[i] == start || collect_pool[adj2[i]].visited == offset_1) { // reconnected
                    while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1); // empty queue 1
                    while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2); // empty queue 2
                    while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
                    while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
                    offset_1+=2;
                    offset_2+=2;
                    return 1; // 1 indicates success
                }
                enqueue(&todo2,adj2[i]);
                enqueue(&collect2,adj2[i]);
                collect_pool[adj2[i]].visited = offset_2; // mark as visited
                cs2++; // increase cluster size
            }
        }
        dequeue(&todo1,&activeP1);
        Adjacent2(activeP1,1);
        for(i=0;i<4;i++) {
            if(adj1[i] != -1) {
                if(collect_pool[adj1[i]].visited == offset_1) continue;
                if((__u32)adj1[i] == goal || collect_pool[adj1[i]].visited == offset_2) {
                    while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1);
                    while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2);
                    while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
                    while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
                    offset_1+=2;
                    offset_2+=2;
                    return 1;
                }
                enqueue(&todo1,adj1[i]);
                enqueue(&collect1,adj1[i]);
                collect_pool[adj1[i]].visited = offset_1;
                cs1++;
            }
        }
    }
    /* Only if a deletion of a cutting edge has to be accepted,
        * completely traverse the unfinished cluster to
        * be able to update the UF datastructure afterwards based on
        * the collect1 and collect2 queues.
    */
    if(accept_split) {
        while(!queue_empty_p(&todo1)) {
            dequeue(&todo1,&activeP1);
            Adjacent2(activeP1,1);
            for(i=0;i<4;i++) {
                if(adj1[i] != -1) {
                    if(collect_pool[adj1[i]].visited == offset_1) continue;
                    enqueue(&todo1,adj1[i]);
                    enqueue(&collect1,adj1[i]);
                    collect_pool[adj1[i]].visited = offset_1;
                    cs1++;
                }
            }

        }
        while(!queue_empty_p(&todo2)) {
            dequeue(&todo2,&activeP2);
            Adjacent2(activeP2,2);
            for(i=0;i<4;i++) {
                if(adj2[i] != -1) {
                    if(collect_pool[adj2[i]].visited == offset_2) continue;
                    enqueue(&todo2,adj2[i]);
                    enqueue(&collect2,adj2[i]);
                    collect_pool[adj2[i]].visited = offset_2;
                    cs2++;
                }
            }
        }
    }
    else {
        while(!queue_empty_p(&todo1)) dequeue(&todo1,&activeP1);
        while(!queue_empty_p(&todo2)) dequeue(&todo2,&activeP2);
        while(!queue_empty_p(&collect1)) dequeue(&collect1,&activeP1);
        while(!queue_empty_p(&collect2)) dequeue(&collect2,&activeP2);
    }
    offset_1+=2;
    offset_2+=2;

    return 0;
}
/******************************************************************************
 *****************************************************************************/
static __u8 removeBond(__u32 a, __u32 b,__u8 accept_split,__u8 accept_non_split) {
    __u32 vertex;
    if(breadthFirstSearch(a,b,accept_split)) //replacement edge found
        return accept_non_split;
    else {  // no replacement edge available and hence cluster will be splitted if move accepted
        if(accept_split) {  // accept cluster splitting case? if yes rebuild uf
            while(!queue_empty_p(&collect1)) {
                dequeue(&collect1,&vertex);
                uf1[vertex] = a;
            }
            uf1[a] = -1*cs1;
            while(!queue_empty_p(&collect2)) {
                dequeue(&collect2,&vertex);
                uf1[vertex] = b;
            }
            uf1[b] = -1*cs2;
            return 1;
        } // not accepted
        else {
            return 0;
        }
    }

}
/******************************************************************************
 *****************************************************************************/
static inline __u8 acceptChange(__s8 d_numCl, __s8 d_numAB, double randV)
{
  return d_numAB == -1 ? (randV < rcWeights[d_numCl]) : (randV < rcWeights[2 - d_numCl]);
}
/******************************************************************************
 *****************************************************************************/
static inline void   mcStep(void)
{

  static __u32 bidx;
  static double rnd_num;
  bidx = gsl_rng_uniform_int(r,2*N);
  rnd_num  = gsl_rng_uniform(r);
  Adjacent(bidx);
  if(bonds[bidx] == 1) { //edge is active hence delete it
    if(rnd_num < p_min_del) {
   	  bonds[bidx] = -1;
      if(removeBond(edge[0],edge[1],1,1))
        activeEdges--;
      else {
          if(verbose)fprintf(stderr,"ERROR!\n");
          exit(-1);
      }
    }
    else {
	if(rnd_num < p_max_del) {
	  bonds[bidx] = -1;
	  if(removeBond(edge[0],edge[1], acceptChange(1,-1,rnd_num),acceptChange(0,-1,rnd_num)))
          activeEdges--;
      else
          bonds[bidx] = 1;
    }

    }

  }
  else {
    if(rnd_num < p_min_ins) {
        bonds[bidx]  = 1;
        activeEdges++;
        unite(edge[0],edge[1],uf1);
      }
    else {
    if(rnd_num < p_max_ins) {
      dN = connected(edge[0],edge[1],uf1) ? 0: -1;
      if(acceptChange(dN,1,rnd_num)) {
        bonds[bidx]=1;
        activeEdges++;
        if(dN == -1)
            unite(edge[0],edge[1],uf1);
      }
    }

  }
  }
}
/******************************************************************************
 *****************************************************************************/
static void extract_observables(__u32 cnt)
{
  __u32 i=0,tn=0,nclust=0;
  size_giant[cnt] = 0;
  sec_cs_moment[cnt] = 0;
  four_cs_moment[cnt] = 0;
  for(i=0;i<N && tn <= N;i++) {
    if(uf1[i] < 0) {
      tn -= uf1[i];
      nclust+=1;
      sec_cs_moment[cnt]+= (__u64)pow(uf1[i],2);
      four_cs_moment[cnt]+= (__u64)pow(uf1[i],4);
      if((__u32)(-uf1[i]) > size_giant[cnt]) size_giant[cnt] = (__u32)(-uf1[i]);
    }
  }
  num_bonds[cnt] = activeEdges;
  num_cluster[cnt] = nclust;
}
/******************************************************************************
 *****************************************************************************/
static void generateTimeSeries(void)
{
  __u32 i=0,j=0;
  for(i=0;i<cutoff;i++){
      for(j=0;j<2*N;j++)mcStep();
      // reset visited array
      // otherwise it will overflow at some point
      // could also be done less frequent
      for(j=0;j<N;j++)collect_pool[j].visited=0;
  }
  for(i=0;i<steps;i++) {
    for(j=0;j<2*N;j++)
      mcStep();
      extract_observables(i);
      for(j=0;j<N;j++)collect_pool[j].visited=0;
    }
}
/******************************************************************************
 *****************************************************************************/
char init_sweeny_uf(double _q,unsigned int _l,double _beta,double _coupl,
        unsigned int _cutoff,unsigned _tslength,unsigned int rng_seed,
        void *ts_0,void *ts_1,void * ts_2,void *ts_3, void *ts_4) {
    q = _q;
    DX = _l;
    beta = _beta;
    coupling = _coupl;
    cutoff = _cutoff;
    steps = _tslength;
    seed = rng_seed;
    num_bonds = (__u32 *)ts_0;
    num_cluster = (__u32 *)ts_1;
    size_giant = (__u32 *)ts_2;
    sec_cs_moment = (__u64 *)ts_3;
    four_cs_moment = (__u64 *)ts_4;
    return setup=init();

}
/******************************************************************************
 *****************************************************************************/
void destroy_sweeny_uf(void) {
    if(setup)
        destroy();
    setup=0;
}
/******************************************************************************
 *****************************************************************************/
char simulate_sweeny_uf(void) {
    if(!setup)return 0;
    generateTimeSeries();
    return 1;
}
/******************************************************************************
 *****************************************************************************/


static char help[] = "Solve the p-Laplacian equation in 2D using Q^1 FEM.\n"
"Implements an objective function and a residual (gradient) function, but\n"
"no Jacobian.  Defaults to p=4 and quadrature degree n=2.  Run as one of:\n"
"   ./plap -snes_fd_color             [default]\n"
"   ./plap -snes_mf\n"
"   ./plap -snes_fd                   [does not scale]\n"
"   ./plap -snes_fd_function -snes_fd [does not scale]\n"
"Uses a manufactured solution.\n"
"This is NOT a recommended example for further work because of the weird way\n"
"it handles the Dirichlet boundary values.\n\n";

#include <petsc.h>
#include "../../quadrature.h"

#define COMM PETSC_COMM_WORLD

/* cool dendritic failure:
    ./plap -snes_fd_color -snes_converged_reason -ksp_converged_reason -pc_type mg -plap_p 10.0 -da_refine 6 -snes_monitor_solution draw
succeeds in 11 snes iterations (on fine grid) with grid sequencing:
    ./plap -snes_fd_color -snes_converged_reason -ksp_converged_reason -pc_type mg -plap_p 10.0 -snes_grid_sequence 6 -snes_monitor_solution draw
*/

//STARTCTX
typedef struct {
    double    p, eps, alpha;
    int       quaddegree;
    PetscBool no_residual;
} PLapCtx;
//ENDCTX

PetscErrorCode ConfigureCtx(PLapCtx *user) {
    PetscErrorCode ierr;
    user->p = 4.0;
    user->eps = 0.0;
    user->alpha = 1.0;
    user->quaddegree = 2;
    user->no_residual = PETSC_FALSE;
    ierr = PetscOptionsBegin(COMM,"plap_","p-laplacian solver options",""); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-p","exponent p with  1 <= p < infty",
                      "plap.c",user->p,&(user->p),NULL); CHKERRQ(ierr);
    if (user->p < 1.0) { SETERRQ(COMM,1,"p >= 1 required"); }
    ierr = PetscOptionsReal("-eps","regularization parameter eps",
                      "plap.c",user->eps,&(user->eps),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsReal("-alpha","parameter alpha in exact solution",
                      "plap.c",user->alpha,&(user->alpha),NULL); CHKERRQ(ierr);
    ierr = PetscOptionsInt("-quaddegree","quadrature degree n (= 1,2,3 only)",
                     "plap.c",user->quaddegree,&(user->quaddegree),NULL); CHKERRQ(ierr);
    if ((user->quaddegree < 1) || (user->quaddegree > 3)) {
        SETERRQ(COMM,2,"quadrature degree n=1,2,3 only"); }
    ierr = PetscOptionsBool("-no_residual","do not set the residual evaluation function",
                      "plap.c",user->no_residual,&(user->no_residual),NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);
    return 0;
}

//STARTEXACT
double Uexact(double x, double y, double alpha) {
    return 0.5 * (x+alpha)*(x+alpha) * (y+alpha)*(y+alpha);
}

double Frhs(double x, double y, PLapCtx *user) {
    const double alf = user->alpha,
                 XX = (x+alf)*(x+alf),
                 YY = (y+alf)*(y+alf),
                 D2 = XX + YY,
                 C = PetscPowScalar(XX * YY * D2, (user->p - 2.0) / 2.0),
                 gamma1 = 1.0/(x+alf) + (x+alf)/D2,
                 gamma2 = 1.0/(y+alf) + (y+alf)/D2;
    return - (user->p - 2.0) * C * (gamma1*(x+alf)*YY + gamma2*XX*(y+alf))
           - C * D2;
}
//ENDEXACT

//STARTINITIALITERATE
PetscErrorCode InitialIterateLocal(DMDALocalInfo *info, Vec u, PLapCtx *user) {
    PetscErrorCode ierr;
    const double hx = 1.0 / (info->mx+1), hy = 1.0 / (info->my+1);
    double       x, y, **au;
    int          i, j;
    ierr = DMDAVecGetArray(info->da,u,&au); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = hy * (j + 1);
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = hx * (i + 1);
            au[j][i] = (1.0 - x) * Uexact(0.0,y,user->alpha)
                       + x * Uexact(1.0,y,user->alpha);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,u,&au); CHKERRQ(ierr);
    return 0;
}
//ENDINITIALITERATE

PetscErrorCode GetUexactLocal(DMDALocalInfo *info, Vec uex, PLapCtx *user) {
    PetscErrorCode ierr;
    const double hx = 1.0 / (info->mx+1), hy = 1.0 / (info->my+1);
    double       x, y, **auex;
    int          i, j;
    ierr = DMDAVecGetArray(info->da,uex,&auex); CHKERRQ(ierr);
    for (j = info->ys; j < info->ys + info->ym; j++) {
        y = hy * (j + 1);
        for (i = info->xs; i < info->xs + info->xm; i++) {
            x = hx * (i + 1);
            auex[j][i] = Uexact(x,y,user->alpha);
        }
    }
    ierr = DMDAVecRestoreArray(info->da,uex,&auex); CHKERRQ(ierr);
    return 0;
}

//STARTFEM
static double xiL[4]  = { 1.0, -1.0, -1.0,  1.0},
              etaL[4] = { 1.0,  1.0, -1.0, -1.0};

double chi(int L, double xi, double eta) {
    return 0.25 * (1.0 + xiL[L] * xi) * (1.0 + etaL[L] * eta);
}

// evaluate v(xi,eta) on reference element using local node numbering
double eval(const double v[4], double xi, double eta) {
    double sum = 0.0;
    int    L;
    for (L=0; L<4; L++)
        sum += v[L] * chi(L,xi,eta);
    return sum;
}

typedef struct {
    double  xi, eta;
} gradRef;

gradRef dchi(int L, double xi, double eta) {
    const gradRef result = {0.25 * xiL[L]  * (1.0 + etaL[L] * eta),
                            0.25 * etaL[L] * (1.0 + xiL[L]  * xi)};
    return result;
}

// evaluate partial derivs of v(xi,eta) on reference element
gradRef deval(const double v[4], double xi, double eta) {
    gradRef sum = {0.0,0.0}, tmp;
    int     L;
    for (L=0; L<4; L++) {
        tmp = dchi(L,xi,eta);
        sum.xi += v[L] * tmp.xi;  sum.eta += v[L] * tmp.eta;
    }
    return sum;
}
//ENDFEM

//STARTTOOLS
void GetUorG(DMDALocalInfo *info, int i, int j, double **au, double *u,
             PLapCtx *user) {
    const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1),
                 x = hx * (i + 1),  y = hy * (j + 1);
    u[0] = ((i == info->mx) || (j == info->my))
             ? Uexact(x,   y,   user->alpha) : au[j][i];
    u[1] = ((i == 0)  || (j == info->my))
             ? Uexact(x-hx,y,   user->alpha) : au[j][i-1];
    u[2] = ((i == 0)  || (j == 0))
             ? Uexact(x-hx,y-hy,user->alpha) : au[j-1][i-1];
    u[3] = ((i == info->mx) || (j == 0))
             ? Uexact(x,   y-hy,user->alpha) : au[j-1][i];
}

double GradInnerProd(DMDALocalInfo *info, gradRef du, gradRef dv) {
    const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1),
                 cx = 4.0 / (hx * hx),  cy = 4.0 / (hy * hy);
    return cx * du.xi  * dv.xi + cy * du.eta * dv.eta;
}

double GradPow(DMDALocalInfo *info, gradRef du, double P, double eps) {
    return PetscPowScalar(GradInnerProd(info,du,du) + eps * eps, P / 2.0);
}
//ENDTOOLS

//STARTOBJECTIVE
double ObjIntegrandRef(DMDALocalInfo *info,
                       const double f[4], const double u[4],
                       double xi, double eta, double p, double eps) {
    const gradRef du = deval(u,xi,eta);
    return GradPow(info,du,p,eps) / p - eval(f,xi,eta) * eval(u,xi,eta);
}

PetscErrorCode FormObjectiveLocal(DMDALocalInfo *info, double **au,
                                  double *obj, PLapCtx *user) {
  PetscErrorCode ierr;
  const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1);
  const Quad1D q = gausslegendre[user->quaddegree-1];
  const int    XE = info->xs + info->xm,  YE = info->ys + info->ym;
  double       x, y, lobj = 0.0, u[4];
  int          i,j,r,s;
  MPI_Comm     com;

  // loop over all elements
  for (j = info->ys; j <= YE; j++) {
      y = hy * (j + 1);
      for (i = info->xs; i <= XE; i++) {
          x = hx * (i + 1);
          // owned elements include "right" and "top" edges of grid
          if (i < XE || j < YE || i == info->mx || j == info->my) {
              const double f[4] = {Frhs(x,   y,   user),
                                   Frhs(x-hx,y,   user),
                                   Frhs(x-hx,y-hy,user),
                                   Frhs(x,   y-hy,user)};
              GetUorG(info,i,j,au,u,user);
              // loop over quadrature points
              for (r = 0; r < q.n; r++) {
                  for (s = 0; s < q.n; s++) {
                      lobj += q.w[r] * q.w[s]
                              * ObjIntegrandRef(info,f,u,q.xi[r],q.xi[s],
                                                user->p,user->eps);
                  }
              }
          }
      }
  }
  lobj *= 0.25 * hx * hy;

  ierr = PetscObjectGetComm((PetscObject)(info->da),&com); CHKERRQ(ierr);
  ierr = MPI_Allreduce(&lobj,obj,1,MPI_DOUBLE,MPI_SUM,com); CHKERRQ(ierr);
  return 0;
}
//ENDOBJECTIVE

//STARTFUNCTION
double FunIntegrandRef(DMDALocalInfo *info, int L,
                       const double f[4], const double u[4],
                       double xi, double eta, double p, double eps) {
  const gradRef du    = deval(u,xi,eta),
                dchiL = dchi(L,xi,eta);
  return GradPow(info,du,p - 2.0,eps) * GradInnerProd(info,du,dchiL)
         - eval(f,xi,eta) * chi(L,xi,eta);
}

PetscErrorCode FormFunctionLocal(DMDALocalInfo *info, double **au,
                                 double **FF, PLapCtx *user) {
  const double hx = 1.0 / (info->mx+1),  hy = 1.0 / (info->my+1);
  const Quad1D q = gausslegendre[user->quaddegree-1];
  const int    XE = info->xs + info->xm,  YE = info->ys + info->ym,
               li[4] = {0,-1,-1,0},  lj[4] = {0,0,-1,-1};
  double       x, y, u[4];
  int          i,j,l,r,s,PP,QQ;

  // clear residuals
  for (j = info->ys; j < YE; j++)
      for (i = info->xs; i < XE; i++)
          FF[j][i] = 0.0;

  // loop over all elements
  for (j = info->ys; j <= YE; j++) {
      y = hy * (j + 1);
      for (i = info->xs; i <= XE; i++) {
          x = hx * (i + 1);
          const double f[4] = {Frhs(x,   y,   user),
                               Frhs(x-hx,y,   user),
                               Frhs(x-hx,y-hy,user),
                               Frhs(x,   y-hy,user)};
          GetUorG(info,i,j,au,u,user);
          // loop over corners of element i,j
          for (l = 0; l < 4; l++) {
              PP = i + li[l];
              QQ = j + lj[l];
              // only update residual if we own node
              if (PP >= info->xs && PP < XE
                  && QQ >= info->ys && QQ < YE) {
                  // loop over quadrature points
                  for (r = 0; r < q.n; r++) {
                      for (s = 0; s < q.n; s++) {
                         FF[QQ][PP]
                             += 0.25 * hx * hy * q.w[r] * q.w[s]
                                * FunIntegrandRef(info,l,f,u,
                                                  q.xi[r],q.xi[s],
                                                  user->p,user->eps);
                      }
                  }
              }
          }
      }
  }
  return 0;
}
//ENDFUNCTION

int main(int argc,char **argv) {
  PetscErrorCode ierr;
  DM             da, da_after;
  SNES           snes;
  Vec            u_initial, u, u_exact;
  PLapCtx        user;
  DMDALocalInfo  info;
  double         err, hx, hy;

  PetscInitialize(&argc,&argv,NULL,help);
  ierr = ConfigureCtx(&user); CHKERRQ(ierr);

  ierr = DMDACreate2d(COMM,
               DM_BOUNDARY_NONE, DM_BOUNDARY_NONE, DMDA_STENCIL_BOX,
               3,3,PETSC_DECIDE,PETSC_DECIDE,1,1,NULL,NULL,&da); CHKERRQ(ierr);
  ierr = DMSetFromOptions(da); CHKERRQ(ierr);
  ierr = DMSetUp(da); CHKERRQ(ierr);
  ierr = DMSetApplicationContext(da,&user);CHKERRQ(ierr);

  ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
  hx = 1.0 / (info.mx+1);
  hy = 1.0 / (info.my+1);
  ierr = DMDASetUniformCoordinates(da,hx,1.0-hx,hy,1.0-hy,0.0,1.0); CHKERRQ(ierr);

  ierr = SNESCreate(COMM,&snes); CHKERRQ(ierr);
  ierr = SNESSetDM(snes,da); CHKERRQ(ierr);
  ierr = DMDASNESSetObjectiveLocal(da,
             (DMDASNESObjective)FormObjectiveLocal,&user); CHKERRQ(ierr);
  if (!user.no_residual) {
      ierr = DMDASNESSetFunctionLocal(da,INSERT_VALUES,
                 (DMDASNESFunction)FormFunctionLocal,&user); CHKERRQ(ierr);
  }
  ierr = SNESSetFromOptions(snes); CHKERRQ(ierr);

  ierr = DMCreateGlobalVector(da,&u_initial);CHKERRQ(ierr);
  ierr = InitialIterateLocal(&info,u_initial,&user); CHKERRQ(ierr);
  ierr = SNESSolve(snes,NULL,u_initial); CHKERRQ(ierr);
  ierr = VecDestroy(&u_initial); CHKERRQ(ierr);
  ierr = DMDestroy(&da); CHKERRQ(ierr);

  ierr = SNESGetSolution(snes,&u); CHKERRQ(ierr);
  ierr = VecDuplicate(u,&u_exact); CHKERRQ(ierr);
  ierr = SNESGetDM(snes,&da_after); CHKERRQ(ierr);
  ierr = DMDAGetLocalInfo(da_after,&info); CHKERRQ(ierr);
  ierr = PetscPrintf(COMM,
            "grid of %d x %d = %d interior nodes\n",
            info.mx,info.my,info.mx*info.my); CHKERRQ(ierr);
  ierr = GetUexactLocal(&info,u_exact,&user); CHKERRQ(ierr);
  ierr = VecAXPY(u,-1.0,u_exact); CHKERRQ(ierr);    // u <- u + (-1.0) uexact
  ierr = VecNorm(u,NORM_INFINITY,&err); CHKERRQ(ierr);
  ierr = PetscPrintf(COMM,"numerical error:  |u-u_exact|_inf = %.3e\n",
           err); CHKERRQ(ierr);

  VecDestroy(&u_exact);  SNESDestroy(&snes);
  return PetscFinalize();
}


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

#include <config.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_errno.h>

#include "recurse.h"

/*
 * This module contains routines related to the Cholesky decomposition
 * of a complex Hermitian positive definite matrix.
 */

static void cholesky_complex_conj_vector(gsl_vector_complex *v);
static int complex_cholesky_decomp_L2(gsl_matrix_complex * A);
static int complex_cholesky_decomp_L3(gsl_matrix_complex * A);

/*
gsl_linalg_complex_cholesky_decomp()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 3 BLAS.

Inputs: A - (input/output) complex positive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition
*/

int
gsl_linalg_complex_cholesky_decomp(gsl_matrix_complex *A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      return complex_cholesky_decomp_L3(A);
    }
}

/*
gsl_linalg_complex_cholesky_solve()
  Solve A x = b where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_solve (const gsl_matrix_complex * cholesky,
                                   const gsl_vector_complex * b,
                                   gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size1 != b->size)
    {
      GSL_ERROR ("matrix size must match b size", GSL_EBADLEN);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      gsl_vector_complex_memcpy (x, b);
      return gsl_linalg_complex_cholesky_svx(cholesky, x);
    }
}

/*
gsl_linalg_complex_cholesky_svx()
  Solve A x = b in place where A is in cholesky form
*/

int
gsl_linalg_complex_cholesky_svx (const gsl_matrix_complex * cholesky,
                                 gsl_vector_complex * x)
{
  if (cholesky->size1 != cholesky->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else if (cholesky->size2 != x->size)
    {
      GSL_ERROR ("matrix size must match solution size", GSL_EBADLEN);
    }
  else
    {
      /* solve for y using forward-substitution, L y = b */
      gsl_blas_ztrsv (CblasLower, CblasNoTrans, CblasNonUnit, cholesky, x);

      /* perform back-substitution, L^H x = y */
      gsl_blas_ztrsv (CblasLower, CblasConjTrans, CblasNonUnit, cholesky, x);

      return GSL_SUCCESS;
    }
}

/******************************************************************************

gsl_linalg_complex_cholesky_invert()
  Compute the inverse of an Hermitian positive definite matrix in
  Cholesky form.

Inputs: LLT - matrix in cholesky form on input
              A^{-1} = L^{-H} L^{-1} on output

Return: success or error
******************************************************************************/

int
gsl_linalg_complex_cholesky_invert(gsl_matrix_complex * LLT)
{
  if (LLT->size1 != LLT->size2)
    {
      GSL_ERROR ("cholesky matrix must be square", GSL_ENOTSQR);
    }
  else
    {
      int status;
      size_t N = LLT->size1;
      size_t i, j;

      /* invert the lower triangle of LLT */
      status = gsl_linalg_complex_tri_invert(CblasLower, CblasNonUnit, LLT);
      if (status)
        return status;

      /* compute A^{-1} = L^{-H} L^{-1} */
      status = gsl_linalg_complex_tri_LHL(LLT);
      if (status)
        return status;

      /* copy the Hermitian lower triangle to the upper triangle */
      for (i = 1; i < N; ++i)
        {
          for (j = 0; j < i; ++j)
            {
              gsl_complex z = gsl_matrix_complex_get(LLT, i, j);
              gsl_matrix_complex_set(LLT, j, i, gsl_complex_conjugate(z));
            }
        }

      return GSL_SUCCESS;
    }
}

/********************************************
 *           INTERNAL ROUTINES              *
 ********************************************/

static void
cholesky_complex_conj_vector(gsl_vector_complex *v)
{
  size_t i;

  for (i = 0; i < v->size; ++i)
    {
      gsl_complex * vi = gsl_vector_complex_ptr(v, i);
      GSL_IMAG(*vi) = -GSL_IMAG(*vi);
    }
}

/*
complex_cholesky_decomp_L2()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 2 BLAS. See Golub & Van Loan, "Matrix Computations" (3rd ed),
algorithm 4.2.2.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition
*/

static int
complex_cholesky_decomp_L2(gsl_matrix_complex * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else
    {
      size_t j;
      gsl_complex z;
      double ajj;

      for (j = 0; j < N; ++j)
        {
          z = gsl_matrix_complex_get(A, j, j);
          ajj = GSL_REAL(z);

          if (j > 0)
            {
              gsl_vector_complex_const_view aj =
                gsl_matrix_complex_const_subrow(A, j, 0, j);

              gsl_blas_zdotc(&aj.vector, &aj.vector, &z);
              ajj -= GSL_REAL(z);
            }

          if (ajj <= 0.0)
            {
              GSL_ERROR("matrix is not positive definite", GSL_EDOM);
            }

          ajj = sqrt(ajj);
          GSL_SET_COMPLEX(&z, ajj, 0.0);
          gsl_matrix_complex_set(A, j, j, z);

          if (j < N - 1)
            {
              gsl_vector_complex_view av =
                gsl_matrix_complex_subcolumn(A, j, j + 1, N - j - 1);

              if (j > 0)
                {
                  gsl_vector_complex_view aj =
                    gsl_matrix_complex_subrow(A, j, 0, j);
                  gsl_matrix_complex_view am =
                    gsl_matrix_complex_submatrix(A, j + 1, 0, N - j - 1, j);

                  cholesky_complex_conj_vector(&aj.vector);

                  gsl_blas_zgemv(CblasNoTrans,
                                 GSL_COMPLEX_NEGONE,
                                 &am.matrix,
                                 &aj.vector,
                                 GSL_COMPLEX_ONE,
                                 &av.vector);

                  cholesky_complex_conj_vector(&aj.vector);
                }

              gsl_blas_zdscal(1.0 / ajj, &av.vector);
            }
        }

      return GSL_SUCCESS;
    }
}

/*
complex_cholesky_decomp_L3()
  Perform the Cholesky decomposition on a Hermitian positive definite
matrix using Level 3 BLAS.

Inputs: A - (input/output) complex postive definite matrix

Return: success or error

Notes:
1) The lower triangle of A is overwritten with the Cholesky decomposition

2) Based on ReLAPACK recursive variant with Level 3 BLAS
*/

static int
complex_cholesky_decomp_L3(gsl_matrix_complex * A)
{
  const size_t N = A->size1;

  if (N != A->size2)
    {
      GSL_ERROR("Cholesky decomposition requires square matrix", GSL_ENOTSQR);
    }
  else if (N <= CROSSOVER_CHOLESKY)
    {
      /* use unblocked Level 2 algorithm */
      return complex_cholesky_decomp_L2(A);
    }
  else
    {
      /*
       * partition matrix:
       *
       * A11 A12
       * A21 A22
       *
       * where A11 is N1-by-N1
       */
      int status;
      const size_t N1 = GSL_LINALG_SPLIT_COMPLEX(N);
      const size_t N2 = N - N1;
      gsl_matrix_complex_view A11 = gsl_matrix_complex_submatrix(A, 0, 0, N1, N1);
      gsl_matrix_complex_view A21 = gsl_matrix_complex_submatrix(A, N1, 0, N2, N1);
      gsl_matrix_complex_view A22 = gsl_matrix_complex_submatrix(A, N1, N1, N2, N2);

      /* recursion on A11 */
      status = complex_cholesky_decomp_L3(&A11.matrix);
      if (status)
        return status;

      /* A21 = A21 * A11^{-1} */
      gsl_blas_ztrsm(CblasRight, CblasLower, CblasConjTrans, CblasNonUnit, GSL_COMPLEX_ONE, &A11.matrix, &A21.matrix);

      /* A22 -= A21 A21^H */
      gsl_blas_zherk(CblasLower, CblasNoTrans, -1.0, &A21.matrix, 1.0, &A22.matrix);

      /* recursion on A22 */
      status = complex_cholesky_decomp_L3(&A22.matrix);
      if (status)
        return status;

      return GSL_SUCCESS;
    }
}


/*
**
** G.Lohmann, MPI-KYB, May 2018
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#include "viaio/Vlib.h"
#include "viaio/VImage.h"
#include "viaio/mu.h"
#include "viaio/option.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>

extern void VCheckImage(VImage src);
extern VImage VFilterGauss3d (VImage src,VImage dest,double *sigma);

void Gauss4d(VAttrList list,double *sigma)
{
  /* get pointers to image data */
  double u=0;
  int j,b,r,c;
  int nrows=0,ncols=0,nt=0;
  int nslices = VAttrListNumImages(list);
  VImage *src = VAttrListGetImages(list,nslices);
  VImageDimensions(src,nslices,&nt,&nrows,&ncols);
  fprintf(stderr," image dimensions:  %d x %d x %d,  nt: %d\n",nslices,nrows,ncols,nt);

  VImage tmp = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(tmp,VAllBands,0);
  VImage dest = VCreateImage(nslices,nrows,ncols,VFloatRepn);
  VFillImage(dest,VAllBands,0);

  for (j=0; j<nt; j++) {
    if (j%5==0) fprintf(stderr," %5d  of  %d\r",j,nt);
    for (b=0; b<nslices; b++) {
      for (r=0; r<nrows; r++) {
	for (c=0; c<ncols; c++) {
	  u = VGetPixel(src[b],j,r,c);
	  VSetPixel(tmp,b,r,c,u);
	}
      }
    }
    dest = VFilterGauss3d (tmp,dest,sigma);

    for (b=0; b<nslices; b++) {
      for (r=0; r<nrows; r++) {
	for (c=0; c<ncols; c++) {
	  u = VGetPixel(dest,b,r,c);
	  VSetPixel(src[b],j,r,c,u);
	}
      }
    }
  }
  fprintf(stderr,"\n");
  VDestroyImage(tmp);
  VDestroyImage(dest);
}


/*
 * @Description  : Phase Class store the information of a phase in a temperature period
 */
#ifndef PHASES_H
#define PHASES_H
#include <string>
#include "VTypes.h"
#include <gsl/gsl_spline.h>

typedef std::vector<gsl_spline*> VSP;

class Phase
{
private:
    static int NPhases;
    int key; // Phase ID
    int DimX; // Dimension of X
    int DimData; // Dimension of T
    VVD X; // Store the Field information at every temperature
    VD T; // Temperature
    VVD dXdT; // The derivative of X respect to Temperature
    SI low_trans;
    SI high_trans;

    gsl_interp_accel *acc;
    VSP inters;

public:
    Phase();
    Phase(int _key, VVD _X, VD _T, VVD _dXdT);
    Phase(const Phase &Ph);
    Phase(const Phase &ph1, const Phase &ph2); //Merge two phases // ! When using this constructor, please make sure one of the two phases are redundant. We don't do such check
    ~Phase();

    void SetPhase(int _key, VVD _X, VD _T, VVD _dXdT);
    void SetUpInterpolation();
    void FreeInterpolation();
    VD valAt(double _T);
    int GetKey() const {return key;}
    double GetTmin() const {return T.front();}
    double GetTmax() const {return T.back();}
    VD GetXatTmin() const {VD Xmin; for(int i = 0; i < DimX; i++ ) {Xmin.push_back(X[i].front());} return Xmin;}
    VD GetXatTmax() const {VD Xmax; for(int i = 0; i < DimX; i++ ) {Xmax.push_back(X[i].back());} return Xmax;}
    void addLowTrans(int _key) {low_trans.insert(_key);}
    void eraseLowTrans(int _key) {low_trans.erase(_key);}
    void addHighTrans(int _key) {high_trans.insert(_key);}
    void eraseHighTrans(int _key) {high_trans.erase(_key);}
    void addLinkFrom(Phase *other_phase);
    std::string repr();
    std::string repr_full();

    VVD GetX() const {return X;}
    VVD GetdXdT() const {return dXdT;}
    VD GetT() const {return T;}
    SI GetLowTrans() const {return low_trans;}
    SI GetHighTrans() const {return high_trans;}
    static int GetNPhases() {return NPhases;}
};

#endif


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

void
cblas_zaxpy (const int N, const void *alpha, const void *X, const int incX,
             void *Y, const int incY)
{
#define BASE double
#include "source_axpy_c.h"
#undef BASE
}


/*
** 1st level statistical inference using LISA
** GLM (general linear modeling) using precoloring
**
** G.Lohmann, MPI-KYB, 2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

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

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

extern gsl_matrix *PseudoInv(gsl_matrix *A,gsl_matrix *B);
extern void printmat(gsl_matrix *R,char *str);
extern void printvec(gsl_vector *x,char *str);

/*
** general linear model (GLM) using pecoloring
*/
void VGLM(gsl_matrix *Data,gsl_matrix *X,gsl_matrix *XInv,gsl_vector *con,VImage map,VImage zmap)
{
  int i;
  int m = Data->size2;
  int n = con->size;
  gsl_set_error_handler_off();
  gsl_vector *y = gsl_vector_calloc (m);
  gsl_vector *beta = gsl_vector_calloc (n);

  /* compute pseudoinverse */
  XInv = PseudoInv(X,XInv);

  /* main loop */
  VFillImage(zmap,VAllBands,0);
  size_t nvox=0;
  for (nvox=0; nvox < Data->size1; nvox++) {

    y->data = gsl_matrix_ptr(Data,nvox,0);
    gsl_blas_dgemv(CblasNoTrans,1.0,XInv,y,0.0,beta);

    /* contrast image */
    double sum=0;
    for (i=0; i<beta->size; i++) {
      sum += (beta->data[i]*con->data[i]);
    }
    if (gsl_isnan(sum) || gsl_isinf(sum)) continue;
    int b = VPixel(map,0,0,nvox,VShort);
    int r = VPixel(map,0,1,nvox,VShort);
    int c = VPixel(map,0,2,nvox,VShort);
    VPixel(zmap,b,r,c,VFloat) = sum;
  }

  gsl_vector_free(beta);
  gsl_vector_free(y);
}


#include <stdio.h>
#include <string.h>
#include <gsl/gsl_statistics.h>
#include "tz_error.h"
#include "tz_constant.h"
#include "tz_image_lib.h"
#include "tz_stack_lib.h"
#include "tz_objdetect.h"
#include "tz_imatrix.h"
#include "tz_stack_math.h"
#include "tz_stack_bwdist.h"
#include "tz_stack_bwmorph.h"
#include "tz_voxel_linked_list.h"
#include "tz_stack_sampling.h"
#include "tz_stack_attribute.h"
#include "tz_dimage_lib.h"
#include "tz_arrayview.h"
#include "tz_dmatrix.h"

INIT_EXCEPTION_MAIN(e)

int main(int argc, char* argv[]) {
  char neuron_name[100];
  if (argc == 1) {
    strcpy(neuron_name, "fly_neuron");
  } else {
    strcpy(neuron_name, argv[1]);
  }

  char file_path[100];

#if 0 /* local maxima */
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  sprintf(file_path, "../data/%s/highpass_locmin.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);

  Stack_Add(stack1, stack2, stack1);
  sprintf(file_path, "../data/%s/cell_mark.tif", neuron_name);
  Write_Stack(file_path, stack1);
#endif

#if 0
  sprintf(file_path, "../data/%s.tif", neuron_name);
  Stack *stack1 = Read_Stack(file_path);

  DMatrix *filter = Mexihat_2D_D(2.0, NULL);
  filter->ndim = 3;
  filter->dim[2] = 1;

  DMatrix *out = Filter_Stack_Fast_D(stack1, filter, NULL, 0);

# if 1
  int nvoxel = Stack_Voxel_Number(stack1);
  int i;
  for (i = 0; i < nvoxel; i++) {
    out->array[i] = fabs(out->array[i]);
  }
# endif

  Stack *stack2 = Scale_Double_Stack(out->array, out->dim[0], out->dim[1],
				   out->dim[2], GREY);

  sprintf(file_path, "../data/%s/edge.tif", neuron_name);
  Write_Stack(file_path, stack2);

  Kill_Stack(stack1);
  Kill_Stack(stack2);
#endif

#if 1
  sprintf(file_path, "../data/%s/mask.tif", neuron_name);
  Stack *stack2 = Read_Stack(file_path);
  sprintf(file_path, "../data/%s/seeds.pa", neuron_name);
  Pixel_Array *pa = Pixel_Array_Read(file_path);
  double *pa_array = (double *) pa->array;
  double max_dist = gsl_stats_max(pa_array, 1, pa->size);
  double mean_dist = gsl_stats_mean(pa_array, 1, pa->size);
  double std_dist = sqrt(gsl_stats_variance(pa_array, 1, pa->size));

  int erode_size = (int) (mean_dist + std_dist * 2 + 0.5);
  Struct_Element *se = Make_Ball_Se(erode_size);
  Stack *stack3 = Stack_Erode_Fast(stack2, NULL, se);
  int dilate_size = (int) (max_dist + 0.5);
  se = Make_Ball_Se(dilate_size);
  Stack *stack4 = Stack_Dilate(stack3, NULL, se);

  Stack_Sub(stack2, stack4, stack2);
  Write_Stack("../data/test.tif", stack2);

  Kill_Stack(stack2);
  Kill_Stack(stack3);
  Kill_Stack(stack4);
  Kill_Pixel_Array(pa);

#endif

  return 0;
}


/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      gerkey@usc.edu    kaspers@robotics.usc.edu
 *
 *  This library is free software; you can redistribute it and/or
 *  modify it under the terms of the GNU Lesser General Public
 *  License as published by the Free Software Foundation; either
 *  version 2.1 of the License, or (at your option) any later version.
 *
 *  This library 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
 *  Lesser General Public License for more details.
 *
 *  You should have received a copy of the GNU Lesser General Public
 *  License along with this library; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
/**************************************************************************
 * Desc: Useful pdf functions
 * Author: Andrew Howard
 * Date: 10 Dec 2002
 * CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
 *************************************************************************/

#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>

#include "amcl/pf/pf_pdf.h"

// Random number generator seed value
static unsigned int pf_pdf_seed;

/**************************************************************************
 * Gaussian
 *************************************************************************/

// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
  pf_matrix_t cd;
  pf_pdf_gaussian_t *pdf;

  pdf = calloc(1, sizeof(pf_pdf_gaussian_t));

  pdf->x = x;
  pdf->cx = cx;
  //pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);

  // Decompose the convariance matrix into a rotation
  // matrix and a diagonal matrix.
  pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
  pdf->cd.v[0] = sqrt(cd.m[0][0]);
  pdf->cd.v[1] = sqrt(cd.m[1][1]);
  pdf->cd.v[2] = sqrt(cd.m[2][2]);

  // Initialize the random number generator
  //pdf->rng = gsl_rng_alloc(gsl_rng_taus);
  //gsl_rng_set(pdf->rng, ++pf_pdf_seed);
  srand48(++pf_pdf_seed);

  return pdf;
}

// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
{
  //gsl_rng_free(pdf->rng);
  free(pdf);
  return;
}

/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
  int i, j;
  pf_vector_t z;
  double zz, p;

  z = pf_vector_sub(x, pdf->x);

  zz = 0;
  for (i = 0; i < 3; i++)
    for (j = 0; j < 3; j++)
      zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];

  p =  1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);

  return p;
}
*/

// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
{
  int i, j;
  pf_vector_t r;
  pf_vector_t x;

  // Generate a random vector
  for (i = 0; i < 3; i++)
  {
    //r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
    r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
  }

  for (i = 0; i < 3; i++)
  {
    x.v[i] = pdf->x.v[i];
    for (j = 0; j < 3; j++)
      x.v[i] += pdf->cr.m[i][j] * r.v[j];
  }

  return x;
}

// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
//   http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma)
{
  double x1, x2, w, r;

  do
  {
    do { r = drand48(); } while (r==0.0);
    x1 = 2.0 * r - 1.0;
    do { r = drand48(); } while (r==0.0);
    x2 = 2.0 * r - 1.0;
    w = x1*x1 + x2*x2;
  } while(w > 1.0 || w==0.0);

  return(sigma * x2 * sqrt(-2.0*log(w)/w));
}