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

/*
 Contains implementations of AdaDIF, TunedRwR, and PPR.

 Dimitris Berberidis
 University of Minnesota 2017-2018
*/

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

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

#include "comp_engine.h"
#include "csr_handling.h"
#include "my_defs.h"
#include "parameter_opt.h"
#include "my_utils.h"

static sz_short get_threads_and_width(sz_short* , sz_short );
static double* get_coef_A(sz_long , sz_med , sz_med , sz_long* , double* ,double* , const sz_long* , double );
static double* get_coef_b(sz_long , sz_med , sz_med ,sz_med , sz_long* , double* , sz_long* );

//Multi-threaded AdaDIF method (output is soft labels)
void AdaDIF_core_multi_thread( double* soft_labels, csr_graph graph, sz_med num_seeds,
			       const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
			       sz_med* num_per_class, sz_med walk_length, double lambda, bool no_constr, bool single_thread){

	sz_short NUM_THREADS, width;

	NUM_THREADS = get_threads_and_width(&width, num_class);

	if(single_thread) NUM_THREADS = 1;

	printf("NUMBER OF THREADS: %"PRIu16" \n", (uint16_t) NUM_THREADS);

	#if DEBUG
	printf("WIDTH= %"PRIu16"\n", (uint16_t) width);
	#endif

	clock_t begin = clock();

	//The following three blocks create copies of the CSR matrix
	//Each copy will be used by a thread

	csr_graph* graph_copies = csr_mult_deep_copy( graph, NUM_THREADS );

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Time spent copying csr_matrix: %lf\n",time_spent);

	//MAIN LOOP
	//Prepare data to be passed to each thread

	pthread_t tid[NUM_THREADS];
	pass_to_thread_type_2* data= (pass_to_thread_type_2*)malloc(NUM_THREADS*sizeof(pass_to_thread_type_2));
	for(sz_short i=0;i<NUM_THREADS;i++){
		(*(data+i))= (pass_to_thread_type_2) {.soft_labels=soft_labels,
			     			      .num_seeds=num_seeds,
						      .num_per_class=num_per_class,
						      .class_ind=class_ind,
						      .graph = graph_copies[i],
						      .seeds=seed_indices,
						      .walk_length=walk_length,
						      .lambda=lambda,
						      .from=i*width,
						      .no_constr = no_constr };

		if(i==NUM_THREADS-1){
			(data+i)->to=NUM_THREADS;
			(data+i)->num_local_classes= num_class -i*width;
		}else{
			(data+i)->to=(i+1)*width;
			(data+i)->num_local_classes=width;
		}
	}

	//Spawn threads and start running
	for(sz_short i=0;i<NUM_THREADS;i++){
		pthread_create(&tid[i],NULL,AdaDIF_squezze_to_one_thread,(void*)(data+i));
	}

	//Wait for all threads to finish before continuing
	for(sz_short i=0;i<NUM_THREADS;i++){pthread_join(tid[i], NULL);}

	//Free copies and temporary arrays

	csr_array_destroy(graph_copies,(sz_short)NUM_THREADS);
	free(data);

}

//Here I slice the output(soft labels) and input (class_ind)
//Using aliasing on the shifted pointers such that single threaded AdaDIF_core is compeltely "blind" to the slicing process
void* AdaDIF_squezze_to_one_thread( void* param){
	pass_to_thread_type_2* data = param;

	double* soft_labels = data->soft_labels + (data->graph.num_nodes * data->from);
	sz_short* class_ind = data->class_ind + data->num_seeds*data->from;
	sz_med* num_per_class = data->num_per_class + data->from;

	clock_t begin = clock();

	AdaDIF_core( soft_labels, data->graph,  data->num_seeds, data->seeds, data->num_local_classes, class_ind, num_per_class,
		     data->walk_length, data->lambda, data->no_constr);

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Thread from classes with index %"PRIu16" to %"PRIu16" finished in %lf sec \n",(uint16_t) data->from, (uint16_t) data->to, time_spent);

	pthread_exit(0);

}

//Core of AdaDIF method that runs on single thread (output is soft labels)
void AdaDIF_core( double* soft_labels, csr_graph graph, sz_med num_seeds,
		  const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
		  sz_med* num_per_class, sz_med walk_length, double lambda, bool no_constr){

	for(sz_short i=0;i<num_class;i++){

		double* land_prob = (double*) malloc(walk_length*graph.num_nodes*sizeof(double));
		double* dif_land_prob = (double*) malloc(walk_length*graph.num_nodes*sizeof(double));
		sz_long* local_seeds =(sz_long*)malloc(num_per_class[i] *sizeof(sz_long));

		sz_med k=0;
		for(sz_med j=0;j<num_seeds;j++){
			if( class_ind[i*num_seeds + j] ==1 )
				local_seeds[k++] = seed_indices[j];
		}

		perform_random_walk(land_prob, dif_land_prob, graph , walk_length , local_seeds , num_per_class[i] );

		double* theta = get_AdaDIF_parameters(graph.num_nodes,graph.degrees,land_prob,dif_land_prob,seed_indices,local_seeds,
		                                      class_ind+i*num_seeds,walk_length,num_seeds,num_per_class[i],lambda,no_constr );
		#if PRINT_THETAS
		printf("THETA: ");
		for(int n=0;n<walk_length;n++) printf(" %.3lf",theta[n]);
		printf("\n");
		#endif

		matvec_trans_long( soft_labels+i*graph.num_nodes , land_prob, theta, graph.num_nodes, walk_length );

		//free landing probabilities
		free(local_seeds);
		free(land_prob);
		free(dif_land_prob);
		free(theta);
	}
}

//Extract landing and dif probabilities by performing K steps of simple random walk on graph

void perform_random_walk(double* land_prob, double* dif_land_prob, csr_graph graph,
			 sz_med walk_length , sz_long* seeds , sz_med num_seeds ){

	double* seed_vector = (double*) malloc(graph.num_nodes* sizeof(double));
	double one_over_num_seeds = 1.0f /(double) num_seeds ;

	//prepare seed vector
	for(sz_long i=0;i<graph.num_nodes;i++) seed_vector[i] = 0.0f ;

	for(sz_med j=0;j<num_seeds;j++) seed_vector[seeds[j]] = one_over_num_seeds ;

	//do the random walk
	my_CSR_matvec( land_prob, seed_vector , graph);

	for(sz_med j=1;j<walk_length;j++){
		my_CSR_matvec( land_prob+j*graph.num_nodes, land_prob+(j-1)*graph.num_nodes , graph);
		my_array_sub( dif_land_prob+(j-1)*graph.num_nodes, land_prob+(j-1)*graph.num_nodes,
			      land_prob+j*graph.num_nodes, graph.num_nodes);
	}

	#if DEBUG
	printf("LAST LAND PROBs: \n");
	for(sz_long i=0;i<=100;i++) printf(" %lf ",land_prob[(walk_length-1)*graph.num_nodes + i ]) ;
	printf("...................... \n");
	#endif

	//do one final step to obtain the last differential

	double* extra_step = (double*) malloc(graph.num_nodes* sizeof(double));
	my_CSR_matvec( extra_step, land_prob+ (walk_length -1)*graph.num_nodes , graph);
	my_array_sub( dif_land_prob+(walk_length-1)*graph.num_nodes, land_prob+(walk_length-1)*graph.num_nodes,
		      extra_step, graph.num_nodes);

        //free
	free(seed_vector);
	free(extra_step);
}

//Extract the AdaDIF diffusion coefficients

double* get_AdaDIF_parameters( sz_long N, sz_long* degrees, double* land_prob,
			       double* dif_land_prob, const sz_long* seed_indices,
			       sz_long* local_seeds, sz_short* class_ind, sz_med walk_length,
			        sz_med num_seeds, sz_med num_pos,double lambda, bool no_constr){

	double* theta = (double*) malloc(walk_length*sizeof(double));

        //A and b are the Hessian and linear component of the quadratic cost

	double* A = get_coef_A(N,walk_length,num_seeds,degrees, land_prob, dif_land_prob,seed_indices,lambda);

	double* b = get_coef_b(N,walk_length,num_seeds,num_pos,degrees, land_prob, local_seeds);

	if(!no_constr){
		simplex_constr_QP_with_PG(theta,A,b,walk_length);
	}else{
		hyperplane_constr_QP(theta,A,b,walk_length);
	}

        //free
	free(b);
	free(A);

	return theta;
}

// This function computes how many threads will be used and how many classes will be allocated per thread
static sz_short get_threads_and_width(sz_short* width ,sz_short num_class){
	sz_short num_procs=get_nprocs();
	sz_short num_threads;

	if(num_class<=num_procs){
		num_threads = num_class;
		*width=1;
	}else{
		num_threads = num_procs;
		*width = (sz_short)ceil((double)(num_class/(double)num_threads));
	}

	return num_threads;
}

//Obtain slice of G  as stationary distributions. (Unweighted) Seed set must be defined
//Returns numbr of itrations untill convergence
sz_med get_slice_of_G( double* G_s, sz_long* seeds, sz_med num_seeds, double tel_prob,
			 csr_graph graph, bool single_thread ){

	//Do it SMART: Use a temporary G_s_next where you store the left hand side of iteration
	//Then at eah iteration just flip pointers between G_s and G_s_next
	//Multiple threads compute different slices of G_s
	sz_short NUM_THREADS = get_nprocs();
	printf("NUMBER OF THREADS: %"PRIu16" \n",(uint16_t) NUM_THREADS);

	if(single_thread) NUM_THREADS = 1;

	sz_med iter[NUM_THREADS];

	csr_graph graph_scaled = csr_deep_copy_and_scale(graph,1.0f-tel_prob);

	double* G_s_next=malloc(graph.num_nodes*num_seeds*sizeof(double));

	//Initialization
	for(sz_long i=0;i<graph.num_nodes*num_seeds;i++) G_s[i]=0.0f;

	for(sz_med j=0;j<num_seeds;j++) G_s[ num_seeds*seeds[j] +j]=1.0f;

	clock_t begin = clock();

	//The following three blocks create copies of the CSR matrix
	//Each copy will be used by a thread

	csr_graph* graph_copies = csr_mult_deep_copy( graph_scaled, (sz_short) NUM_THREADS );

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Time spent copying csr_matrix: %lf\n",time_spent);

	//MAIN LOOP
	//Prepare data to be passed to each thread
	sz_med width;
	width=(sz_med)floor((double)(num_seeds/(double)NUM_THREADS));
	pthread_t tid[NUM_THREADS];
	pass_to_thread_type_1* data= (pass_to_thread_type_1*)malloc(NUM_THREADS*sizeof(pass_to_thread_type_1));
	for(sz_short i=0;i<NUM_THREADS;i++){

	        (*(data+i))= (pass_to_thread_type_1) {.G_s=G_s,
			 			      .G_s_next=G_s_next,
						      .graph = graph_copies[i],
						      .seeds=seeds,
						      .M=num_seeds,
						      .tel_prob=tel_prob,
						      .from=i*width };

		if(i==NUM_THREADS-1){
			(data+i)->to=num_seeds;
		}else{
			(data+i)->to=(i+1)*width;
		}
		(data+i)->iter=&iter[i];
	}

	//Spawn threads and start running
	for(sz_short i=0;i<NUM_THREADS;i++){
		pthread_create(&tid[i],NULL,my_power_iter,(void*)(data+i));
	}

	//Wait for all threads to finish before continuing
	for(sz_short i=0;i<NUM_THREADS;i++){pthread_join(tid[i], NULL);}

	//Free copies and temporary arrays
	csr_destroy(graph_scaled);
	csr_array_destroy(graph_copies,(sz_short)NUM_THREADS);
	free(G_s_next);
	free(data);

	return iter[0];
}

//Power iteration that computes a subset (from,to) of the collumns of G_s
//Data are passed via struct that can be readily used by thread
void* my_power_iter(void* param){

	pass_to_thread_type_1* data = param;

	sz_long* seeds=data->seeds;
	double* G_s=data->G_s;
	double* G_s_next= data->G_s_next;
	csr_graph graph=data->graph;

	//Iterate using this "back and forth method" to minimize memory access
	clock_t begin = clock();

	sz_med iter=0;
	sz_short flag=0;

	do{
		iter++;
		flag=(flag==1) ? 0 : 1 ;
		if(flag==1){
			my_CSR_matmat( G_s_next , G_s  , graph , data->M , data->from, data->to);
			for(sz_med j=data->from;j<data->to;j++) G_s_next[data->M*seeds[j] +j]+=data->tel_prob;
		}
		else{
			my_CSR_matmat( G_s, G_s_next  , graph , data->M, data->from, data->to);
			for(sz_med j=data->from;j<data->to;j++) G_s[data->M*seeds[j] +j]+= data->tel_prob;
		}
	}while(iter<MAXIT && max_seed_val_difference( G_s, G_s_next , seeds ,data->M, data->from, data->to) >TOL);

	if(flag==1){
		for(sz_long i=0;i<graph.num_nodes;i++){
			for(sz_med j=data->from;j<data->to;j++)
				G_s[i*data->M + j]=G_s_next[i*data->M + j];
		}
	}

	*(data->iter)=iter;

	clock_t end = clock();
	double time_spent = (double)(end - begin) / CLOCKS_PER_SEC;

	printf("Thread from column %"PRIu32" to %"PRIu32" finished in %lf sec \n", (uint32_t) data->from, (uint32_t) data->to, time_spent);

	pthread_exit(0);
}

//Personalized-Pagerank single thread
void my_PPR_single_thread( double* soft_labels, csr_graph graph, sz_med num_seeds,
			   const sz_long* seed_indices, sz_short num_class, sz_short* class_ind,
			   sz_med* num_per_class, sz_med walk_length, double tel_prob){

	csr_graph graph_scaled = csr_deep_copy_and_scale(graph,1.0f-tel_prob);

	for(sz_short i=0;i<num_class;i++){
		sz_long* local_seeds =(sz_long*)malloc(num_per_class[i] *sizeof(sz_long));

		sz_med k = 0;
		for(sz_med j=0;j<num_seeds;j++){
			if( class_ind[i*num_seeds + j] ==1 )
				local_seeds[k++] = seed_indices[j];
		}

		//prepare seed vector
		double* soft = soft_labels + i*graph.num_nodes;
		double one_over_num_seeds = 1.0f /(double) num_per_class[i] ;
		for(sz_long n=0;n<graph.num_nodes;n++)  soft[n] = 0.0f ;
		for(sz_med j=0;j<num_per_class[i];j++) soft[local_seeds[j]] = one_over_num_seeds ;

		//perform random walk with restart
		sz_short flag=0;
		double* soft_next=malloc(graph.num_nodes*sizeof(double));
		for(sz_med j=0;j<walk_length;j++){
			flag=(flag==1) ? 0 : 1 ;
			if(flag==1){
				my_CSR_matvec(soft_next, soft, graph_scaled );
				for(sz_med k=0;k<num_per_class[i] ;k++)
					soft_next[local_seeds[k]] += tel_prob*one_over_num_seeds;
			}
			else{
				my_CSR_matvec(soft, soft_next, graph_scaled );
				for(sz_med k=0;k<num_per_class[i] ;k++)
					soft[local_seeds[k]] += tel_prob*one_over_num_seeds;
			}

		}

		if(flag==1){
			memcpy(soft, soft_next, graph.num_nodes*sizeof(double));
		}

		free(soft_next);
		free(local_seeds);

	}
	csr_destroy(graph_scaled);
}

//find l_max norm between vecors of known length
double max_seed_val_difference(double* A, double* B, sz_long* points , sz_med num_points, sz_med from, sz_med to){
	double dif;
	double max_dif=0.0f;
	for(sz_med i=from;i<to;i++){
		dif=fabs( A[ num_points*points[i] + i] - B[ num_points*points[i] + i] );
		max_dif = ( dif > max_dif ) ? dif : max_dif ;
	}
	return max_dif;
}

//Extract square submatrix of slice G_l that corresponds to labeled only
void extract_G_ll(double* G_ll, double* G_s, sz_long* seeds, sz_med num_seeds){

	for(sz_long i=0;i<num_seeds;i++){
		for(sz_med j=0;j<num_seeds;j++)
			G_ll[i*num_seeds + j]=G_s[seeds[i]*num_seeds + j];
	}
}

//The following two functions generate the coefficients required by the AdaDIF QP
static double* get_coef_A(sz_long N, sz_med K, sz_med L , sz_long* d ,
			  double* P ,double* P_dif , const sz_long* L_ind , double lambda ){

	double* A = (double*) malloc(K*K*sizeof(double));

	double* d_inv_times_lambda = (double*)malloc(N*sizeof(double));

	for(sz_long i=0;i<N;i++) d_inv_times_lambda[i] =lambda/(double)d[i];

	double* P_temp =(double*) malloc(K*N*sizeof(double));

	for(sz_long i=0;i<N;i++){
		for(sz_med j=0;j<K;j++)
			P_temp[j*N+i]=d_inv_times_lambda[i]*P_dif[j*N+i];
	}

	for(sz_med i=0;i<L;i++){
		for(sz_med j=0;j<K;j++)
			P_temp[j*N + L_ind[i] ]+= P[j*N+L_ind[i]]/(double)d[L_ind[i]];
	}

	cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, (int)K, (int)K, (int)N, 1.0f, P, (int)N, P_temp, (int)N, 0.0f, A, (int)K);

	free(d_inv_times_lambda);
	free(P_temp);
	return A;
}

static double* get_coef_b(sz_long N, sz_med K, sz_med L ,sz_med num_pos , sz_long* d , double* P , sz_long* L_c ){

	double two_over_L = -2.0f/(double) L;

        double*	b = (double*) malloc(K*sizeof(double));

	for(sz_med i=0;i<K;i++) b[i]=0.0f;

	for(sz_med i=0;i<K;i++){
		for(sz_med j=0;j<num_pos;j++)
			b[i]+=P[i*N + L_c[j]]/(double)d[L_c[j]];
		b[i]*= two_over_L;
	}

	return b;
}


#ifndef matops_h
#define matops_h

#include <ceed.h>
#include <petsc.h>
#include "../include/structs.h"

// This function uses libCEED to compute the local action of an operator
PetscErrorCode ApplyLocalCeedOp(Vec X, Vec Y, UserMult user);

// This function uses libCEED to compute the non-linear residual
PetscErrorCode FormResidual_Ceed(SNES snes, Vec X, Vec Y, void *ctx);

// This function uses libCEED to apply the Jacobian for assembly via a SNES
PetscErrorCode ApplyJacobianCoarse_Ceed(SNES snes, Vec X, Vec Y, void *ctx);

// This function uses libCEED to compute the action of the Jacobian
PetscErrorCode ApplyJacobian_Ceed(Mat A, Vec X, Vec Y);

// This function uses libCEED to compute the action of the prolongation operator
PetscErrorCode Prolong_Ceed(Mat A, Vec X, Vec Y);

// This function uses libCEED to compute the action of the restriction operator
PetscErrorCode Restrict_Ceed(Mat A, Vec X, Vec Y);

// This function returns the computed diagonal of the operator
PetscErrorCode GetDiag_Ceed(Mat A, Vec D);

// This function calculates the strain energy in the final solution
PetscErrorCode ComputeStrainEnergy(DM dm_energy, UserMult user,
                                   CeedOperator op_energy, Vec X,
                                   PetscReal *energy);

// this function checks to see if the computed energy is close enough to reference file energy.
PetscErrorCode RegressionTests_solids(AppCtx app_ctx, PetscReal energy);

#endif // matopts_h


/*
Copyright 2017 InitialDLab

Permission is hereby granted, free of charge, to any person obtaining a copy of
this software and associated documentation files (the "Software"), to deal in
the Software without restriction, including without limitation the rights to
use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
of the Software, and to permit persons to whom the Software is furnished to do
so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
*/
#pragma once

//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#include <random>
#include <cassert>

inline
size_t
next_fanout(size_t size, size_t min_fanout, size_t max_fanout)
{
    if(size >= min_fanout + max_fanout)
        return max_fanout;

    if(size < max_fanout)
    {
        return size;
    }

    return size - min_fanout;
}

inline
size_t
calc_node_count(size_t size, size_t min_fanout, size_t max_fanout)
{
    return (size % max_fanout)
        ? (size / max_fanout + 1)
        : (size / max_fanout)
        ;
}

template<typename Point>
void dump_point(std::ostream & out, Point const& point)
{
    out << boost::geometry::wkt(point);
}

template<typename Box>
void dump_box(std::ostream & out, Box const& box)
{
    out << boost::geometry::wkt(box.min_corner())
        << ' ' << boost::geometry::wkt(box.max_corner());
}

/*
template<typename RNG>
size_t alternate_binomial(size_t n, double p, RNG & rng)
{
    if(p > 0.5)
        return n - alternate_binomial(n, 1.0-p, rng);

    std::uniform_real_distribution<float> coin_dist(0,1);
    double log_q = std::log(1.0 - p);
    size_t x = 0;
    double sum = 0;
    for(;;) {
        sum += std::log(coin_dist(rng)) / (n - x);
        if(sum < log_q)
            return x;
        ++x;
    }
}
*/

template<typename RNG>
inline
size_t
next_sample_size(size_t total_sample_size, size_t cur_subtree_size, size_t total_subtree_size, RNG & rng)
{
    if(cur_subtree_size == 0) return 0;
    if(cur_subtree_size == total_sample_size) return total_sample_size;
    if(total_sample_size < 10)
    {
        size_t s = 0;
        std::uniform_real_distribution<float> dist(0,1);
        float prob = ((float)cur_subtree_size) / total_sample_size;
        for(size_t i = 0; i < total_sample_size; ++i)
            if(dist(rng) < prob)
                ++s;
        return s;
    }

    //std::binomial_distribution<int> distribution(((double)cur_subtree_size) / total_subtree_size, total_sample_size);
    std::binomial_distribution<int> distribution(total_sample_size, ((double)cur_subtree_size) / total_subtree_size);
    return distribution(rng);

    //static gsl_rng *_gsl_rng = nullptr;
    //if(!_gsl_rng){ _gsl_rng = gsl_rng_alloc (gsl_rng_taus); }
    //return gsl_ran_binomial(_gsl_rng, ((double)cur_subtree_size)/total_subtree_size, total_sample_size);

/*
    return alternate_binomial(
        total_sample_size,
        ((double)cur_subtree_size) / total_subtree_size,
        rng
    );
*/
/*
    return std::binomial_distribution<size_t>(
        total_sample_size,
        ((double)cur_subtree_size) / total_subtree_size
    )(rng);
*/
}

template<typename RNG>
inline
void
next_sample_size_bulk(int trials, const std::vector<int64_t> &prefix_weights, std::vector<int> &output_counts, RNG & rng)
{
    assert(prefix_weights.size() == output_counts.size());

    // special case
    if (prefix_weights.size() == 1)
    {
        output_counts[0] = trials;
        return;
    }

    int64_t accum = 0;
    for (int i = 0; i < prefix_weights.size(); ++i)
    {
        output_counts[i] = next_sample_size(trials, prefix_weights[i] - accum, prefix_weights.back(), rng);
        accum = prefix_weights[i];
    }

    //std::vector<int64_t> each_trial;
    //each_trial.resize(trials);

    //std::uniform_int_distribution<int64_t> dist(0, prefix_weights.back());

    //std::generate(each_trial.begin(), each_trial.end(), [&rng, &dist]() {return dist(rng);});
    //std::sort(each_trial.begin(), each_trial.end());
    //auto last_max = each_trial.begin();
    //auto output_itr = output_counts.begin();
    //for (auto weight : prefix_weights)
    //{
    //    auto next_max = std::lower_bound(last_max, each_trial.end(), weight);
    //    *output_itr = std::distance(last_max, next_max);
    //    ++output_itr;
    //    last_max = next_max;
    //}

    //if (last_max != each_trial.end())
    //    output_counts.back() += std::distance(last_max, each_trial.end());

    //for (int64_t samples_inserted = 0; samples_inserted < trials; ++samples_inserted)
    //{
    //    int64_t r_val = dist(rng);
    //    auto i_itr = std::lower_bound(prefix_weights.begin(), prefix_weights.end(), r_val);
    //    int index = i_itr - prefix_weights.begin();
    //    ++output_counts[index];
    //}

    //std::uniform_int_distribution<int64_t> dist(0, prefix_weights.back());
    //for (int64_t samples_inserted = 0; samples_inserted < trials; ++samples_inserted)
    //{
    //    int64_t r_val = dist(rng);
    //    auto i_itr = std::lower_bound(prefix_weights.begin(), prefix_weights.end(), r_val);
    //    int index = i_itr - prefix_weights.begin();
    //    ++output_counts[index];
    //}
}


/***************************************************************************
Copyright (c) 2014, The OpenBLAS Project
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
1. Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in
the documentation and/or other materials provided with the
distribution.
3. Neither the name of the OpenBLAS project nor the names of
its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE OPENBLAS PROJECT OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*****************************************************************************/

//#include "bench.h"
#include <cblas.h>
#include <math.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>

// mlir-clang dot.c -I .. -I /home/lchelini/scratch/polygeist/llvm-project/llvm/../clang/lib/Headers -o dot --emit-llvm

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

  float *x, *y;
  float r;

  int inc_x = 1, inc_y = 1;

  int size = 500;
  x = (float *)malloc(sizeof(float) * size);
  y = (float *)malloc(sizeof(float) * size);

  for (int i = 0; i < size; i++) {
    x[i] = ((float)rand() / (float)RAND_MAX) - 0.5;
  }

  for (int i = 0; i < size; i++) {
    y[i] = ((float)rand() / (float)RAND_MAX) - 0.5;
  }

  #pragma plugin(sdot_, "linalg", "r += x(i) * y(i)")
  r = sdot_(&size, x, &inc_x, y, &inc_y);
  fprintf(stderr, "%.6f\n", result);

  return 0;
}


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions manipulating waveforms.
 *
 */

#ifndef _WAVEFORM_H
#define _WAVEFORM_H

#define _XOPEN_SOURCE 500

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "EOBNRv2HMROM.h"

#if defined(__cplusplus)
extern "C" {
#define complex _Complex
#elif 0
} /* so that editors will match preceding brace */
#endif

/* NOTE: uses the list of modes of EOBNRv2HMROM (listmode), to be extended when more waveform models are added */

/***************** Function estimating frequency corresponding to a given time to coalescence ****************/

/* Functions computing relations between chirp mass, eta, m1, m2*/
double Mchirpofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double etaofm1m2(
  const double m1,   /* Mass 1 */
  const double m2);  /* Mass 2 */
double m1ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */
double m2ofMchirpeta(
  const double Mchirp,   /* Chirp mass */
  const double eta);     /* Symmetric mass ratio */

/* Newtonian estimate of the relation Mf(deltat/M) (for the 22 mode) - gives the starting geometric frequency for a given mass ratio and a given geometric duration of the observations */
double NewtonianfoftGeom(const double q, const double t); /* t here is t/M */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode) - gives the starting frequency in Hz for a given mass ratio and a given geometric duration of the observations */
double Newtonianfoft(const double m1, const double m2, const double t); /* t here is in years, m1-m2 in solar masses */
/* Newtonian estimate of the relation f(deltat) (for the 22 mode freq) - gives the starting geometric frequency for a given time to merger and chirp mass - output in Hz */
double Newtonianfoftchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Time in years */
/* Newtonian estimate of the relation deltat(f) (for the 22 mode freq) - gives the time to merger from a starting frequency for a given chirp mass - output in years */
double Newtoniantoffchirp(
  const double mchirp,                 /* Chirp mass (solar masses) */
  const double t);                     /* Freq in Hz */

/***************** Function estimating time to coalescence and min/max frequency ****************/

/* Functions reading from a list of modes the minimal and maximal frequencies */
double ListmodesCAmpPhaseFrequencySeries_maxf(ListmodesCAmpPhaseFrequencySeries* listhlm);
double ListmodesCAmpPhaseFrequencySeries_minf(ListmodesCAmpPhaseFrequencySeries* listhlm);

/* Function estimating initial time from Psi22, according to tf_SPA = -1/(2pi)dPsi/df */
double EstimateInitialTime(ListmodesCAmpPhaseFrequencySeries* listhlm, double fLow);

/***************** Functions to manipulate ReImFrequencySeries structure ****************/

void ReImFrequencySeries_AddCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,              /* Output Re/Im frequency series */
  struct tagCAmpPhaseFrequencySeries* freqseriesCAmpPhase,    /* Input CAmp/Phase frequency series, to be interpolated and added to the output */
  double fLow,                                                /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                               /* Maximal frequency - set to 0 to ignore */
  double fstartobsmode);                                      /* Starting frequency in case of limited duration of observations- assumed to have been scaled with the proper factor m/2 for this mode - set to 0 to ignore */
/* Function evaluating a ReImFrequencySeries by interpolating wach mode of a ListmodesCAmpPhaseFrequencySeries and summing them, given a set of frequencies */
void ReImFrequencySeries_SumListmodesCAmpPhaseFrequencySeries(
  struct tagReImFrequencySeries* freqseriesReIm,                    /* Output Re/Im frequency series - already initialized */
  struct tagListmodesCAmpPhaseFrequencySeries* listmodesCAmpPhase,  /* Input CAmp/Phase frequency series, to be interpolated */
  gsl_vector* freq,                                                 /* Input set of frequencies on which evaluating */
  double fLow,                                                      /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                                     /* Maximal frequency - set to 0 to ignore */
  double fstartobs);                                                /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
/* Helper function to add a mode to hplus, hcross in Fourier domain
 * - copies the function XLALSimAddMode, which was done only for TD structures */
int FDAddMode(
  ReImFrequencySeries* hptilde,       /* Output: frequency series for hplus */
  ReImFrequencySeries* hctilde,       /* Output: frequency series for hcross */
  ReImFrequencySeries* hlmtilde,      /* Input: frequency series for the mode hlm */
  double theta,                       /* First angle for position in the sky of observer */
  double phi,                         /* Second angle for position in the sky of observer  */
  int l,                              /* First mode number l */
  int m,                              /* Second mode number m */
  int sym);                           /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series for hplus, hcross from the modes hlm */
int GeneratehphcFDReImFrequencySeries(
  ReImFrequencySeries** hptilde,                 /* Output: frequency series for hplus */
  ReImFrequencySeries** hctilde,                 /* Output: frequency series for hcross */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: frequency series for the mode hlm  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaF and maximal frequency in input */
  int nbmode,                                    /* Number of modes to add */
  double theta,                                  /* First angle for position in the sky of observer */
  double phi,                                    /* Second angle for position in the sky of observer */
  int sym);                                      /* If 1, assume planar symmetry and add also mode l,-m. Do not if set to 0. */
/* Function evaluating the FD frequency series by summing mode contributions from each hlm */
int GenerateFDReImFrequencySeries(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt);                                     /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
/* Function evaluating the FD frequency series for a single mode contribution (l,m) */
int GenerateFDReImFrequencySeriesSingleMode(
  ReImFrequencySeries** freqseries,              /* Output: frequency series */
  ListmodesCAmpPhaseFrequencySeries* listhlm,    /* Input: FD modes hlm in the form AmpReal/AmpIm/Phase  */
  double fLow,                                   /* Minimal frequency - set to 0 to ignore */
  double fHigh,                                  /* Maximal frequency - set to 0 to ignore */
  double fstartobs,                              /* For limited duration of observation, starting frequency for the 22 mode - set to 0 to ignore */
  double deltaf,                                 /* Frequency step */
  int nbpt,                                      /* Number of points of output - if 0, determined from deltaf and maximal frequency in input */
  int l,                                         /* Mode index l */
  int m);                                        /* Mode index m */
/* Function to restrict a frequency series (typically output of a FFT) to a given frequency range */
int RestrictFDReImFrequencySeries(
  ReImFrequencySeries** freqseriesout,           /* Output: truncated frequency series */
  ReImFrequencySeries* freqseriesin,             /* Input: frequency series */
  double fLow,                                   /* Minimal frequency */
  double fHigh);                                 /* Maximal frequency */
/* Function for getting a phase mod 2pi (between -pi and pi) */
double mod2pi(double phase);
/* Function for getting a phase mod pi (between 0 and pi, e.g. polarization) */
double modpi(double phase);
/* Function to unwrap the phase mod 2pi  - acts directly on the gsl_vector representing the phase */
int UnwrapPhase(
  gsl_vector*  phaseout,   /* Output: unwrapped phase vector - already allocated */
  gsl_vector*  phasein);   /* Input: phase vector */
/* Function to convert a time series from Re/Im form to Amp/Phase form - unwrapping the phase */
int ReImTimeSeries_ToAmpPhase(
  AmpPhaseTimeSeries** timeseriesout,             /* Output: Amp/Phase time series */
  ReImTimeSeries* timeseriesin);                  /* Input: Re/Im time series */
/* Function to convert a time series from Amp/Phase form to Re/Im form */
int AmpPhaseTimeSeries_ToReIm(
  ReImTimeSeries** timeseriesout,                 /* Output: Re/Im time series */
  AmpPhaseTimeSeries* timeseriesin);              /* Input: Amp/Phase time series */
/* Function to compute a linear resampling at high frequencies to enforce a maximal deltaf */
/* NOTE: Assumes input frequencies are logarithmic (except maybe first interval) to evaluate when to resample */
int SetMaxdeltafResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double maxf,               /* Input: maximal frequency - set to 0. to ignore */
  const double deltaf);            /* Input: maximal deltaf aimed for - 0.002Hz appropriate for LISA */
/* Function to compute a linear-in-time resampling at low frequencies to enforce a maximal deltat */
int SetMaxdeltatResampledFrequencies(
  gsl_vector** freqr,              /* Output: resampled frequencies */
  gsl_vector* freq,                /* Input: original frequencies */
  const double deltat,             /* Input: maximal deltat aimed for - fraction of a year, 1/24 (half month) appropriate for 1e-4 interpolation errors */
  const double mchirp,             /* Input: chirp mass, used for approximate t-f correspondence */
  const int m);                    /* Input: chirp mass, used for approximate t-f correspondence */
/* Function to resample a CAmp/Phase frequency series on the specified frequencies */
int CAmpPhaseFrequencySeries_Resample(
  CAmpPhaseFrequencySeries** freqseriesout,         /* Output: CAmp/Phase freq series */
  CAmpPhaseFrequencySeries* freqseriesin,           /* Input: CAmp/Phase freq series */
  gsl_vector* freqr);                                 /* Input: freq vector to resample on */

/***************** Spin weighted spherical harmonics ****************/

/* Additional function reproducing XLALSpinWeightedSphericalHarmonic */
double complex SpinWeightedSphericalHarmonic(double theta, double phi, int s, int l, int m); /* Currently only supports s=-2, l=2,3,4,5 modes */

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _WAVEFORM_H */


#ifndef SPN_H
#define SPN_H
#include "init_cy_fde.h"
#include "matrix_util.h"
//#include <cblas.h>

typedef struct SemiCircularNet{
  int p;
  int d;
  DCOMPLEX *Pa_h_A;         // d
  DCOMPLEX *Pa_omega;       // 2*d

  DCOMPLEX F_A[2];         //
  DCOMPLEX h_A[2];         //

  DCOMPLEX TG_Ge_sc[4] ;   //
  DCOMPLEX DG_sc[4];       //

  DCOMPLEX temp_T_eta[4];  //
  DCOMPLEX Dh_sc[4];       //

  DCOMPLEX Psigma_G_sc[2]; //

  DCOMPLEX Psigma_h_sc[2] ;//

  DCOMPLEX DG_A[4];        //
  DCOMPLEX Dh_A[4];        //

  DCOMPLEX S[4];           //

  DCOMPLEX temp_mat[4];    //

  DCOMPLEX Psigma_omega[2];//

}SCN;

void
SCN_construct(SCN* self, int p , int d);

void
SCN_init(SCN* self);

void
SCN_init_forward(SCN* self);
void
SCN_init_backward(SCN* self);

void
SCN_destroy(SCN* self);

int
SCN_cauchy(SCN* self);

void
SCN_grad(SCN* self,  int p, int d, double  *a,  double  sigma, \
  DCOMPLEX z,DCOMPLEX *G, DCOMPLEX *omega, DCOMPLEX *omega_sc,\
  DCOMPLEX *o_grad_a, DCOMPLEX *o_grad_sigma);

/** Compute Cauchy transform of SemiCircular( returns total iterations)
* @param Z : input matrix
* @param o_G : out_put Cauchy transform
*
*/
int
cauchy_sc( int p,  int d,  double sigma, DCOMPLEX* Z, \
   int max_iter, double thres,\
   DCOMPLEX* o_G);

 /** Compute Cauchy transform of Signal-Plus-Noise model( returns total iterations)
 * @param Z : input matrix
 * @param o_G_sc : out_put Cauchy transform
 *
 */
int
cauchy_spn(int p_dim, int dim, double* a, double sigma,\
     DCOMPLEX* B,\
     int max_iter,double thres, \
     DCOMPLEX* o_G_sc, DCOMPLEX* o_omega, DCOMPLEX* o_omega_sc);

/*
Only for debug
*/
void
grad_cauchy_spn(int p, int d,  double  *a, double  sigma, \
  DCOMPLEX z, DCOMPLEX *G, DCOMPLEX *omega, DCOMPLEX *omega_sc,\
  DCOMPLEX *o_grad_a, DCOMPLEX *o_grad_sigma);

// transpose of derivation of Ge
// G : 2
// o_DGe: 2 x 2
void TG_Ge( const int p, const int d, const double sigma, \
  const DCOMPLEX *G, DCOMPLEX *o_DGe);

// transpose of derivation of cauchy_sc
// G: 2
// DG: 2 x 2
void DG(const DCOMPLEX *G,  const  DCOMPLEX *DGe,  DCOMPLEX *o_DG);

void T_eta(const int p, const int d, DCOMPLEX *o_T_eta);

void Dh(const DCOMPLEX* DG, const DCOMPLEX *T_eta, const double sigma,DCOMPLEX *o_Dh);

void Psigma_G(const int p,const int d, const double sigma, const DCOMPLEX *G, const DCOMPLEX *DGe, DCOMPLEX *o_Psigma_G);

void Psigma_h(const int p, const int d, const double sigma, const DCOMPLEX * G, const DCOMPLEX* P_sigma_G, const DCOMPLEX *T_eta,\
DCOMPLEX* o_Psigma_h);

//// Descrete

void des_DG( int p, int d, const double *a, const DCOMPLEX *W,DCOMPLEX*o_DG);

void des_Dh( const DCOMPLEX *DG, const DCOMPLEX *F,DCOMPLEX*o_Dh);

void des_Pa_h( int p, int d, const double *a, const DCOMPLEX *W, DCOMPLEX *F, DCOMPLEX *Pa_h);

/** compute gradient and loss of likelihood
* return total number of forward_iter
*
*/
int
grad_loss_cauchy_spn(  int p, int d, double  *a, double  sigma, double scale, \
  int  batch_size, double *batch, \
  double *o_grad_a, double *o_grad_sigma, double *o_loss);

#endif


#pragma once

#include "schedule.h"
#include <boost/random/uniform_01.hpp>
#include <boost/random/uniform_int_distribution.hpp>
#include <gsl/gsl-lite.hpp>
#include <pcg_random.hpp>
#include <random>

namespace angonoka::stun {
using RandomEngine = pcg32;

/**
    Miscellaneous random number generators.
*/
class RandomUtils {
public:
    /**
        Default constructor.
    */
    RandomUtils();

    /**
        Constructor with a fixed PRNG seed.

        @param seed Random engine seed
    */
    RandomUtils(gsl::index seed);

    /**
        Uniformally distributed real value between 0 and 1.

        @return Random number
    */
    float uniform_01() noexcept;

    /**
        Uniformally distributed discrete value between 0 and max.

        @param max Maximum value

        @return Random number
    */
    int16 uniform_int(int16 max) noexcept;

private:
    RandomEngine generator{
        pcg_extras::seed_seq_from<std::random_device>{}};
    boost::random::uniform_01<float> uniform_01_;
    boost::random::uniform_int_distribution<std::int_fast16_t>
        uniform_int_;
};
} // namespace angonoka::stun


/**
 * The driver function and some auxillaries
 * to compute the spectrum from an image and
 * information on the location and distortion of the spectra.
 */

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

#include <gsl/gsl_roots.h>
#include <gsl/gsl_vector.h>

#include "aXe_grism.h"
#include "aXe_utils.h"
#include "spce_sect.h"
#include "spce_is_in.h"
#include "spce_pathlength.h"

#define DEBUG_ME 0x10

/**
  fill in the path length field (xi in ap_pixel) from the abscissas (xs in
  ap_pixel) (helper function for spc_extract).

  @param func the spectrum trace to use for the transformation from xs to xi
  @param table a pointer to the table of aperture pixels, terminated with
  x=-1
*/
static int
transform_to_pathlen (const trace_func * const func, ap_pixel * const table)
{
  int i, len;
  ap_pixel *cur_p = table;
  gsl_vector *section_points;

  while (cur_p->p_x != -1)
    cur_p++;
  len = cur_p - table;
  if (len==0) return -1; // exit now if the table is empty

  section_points = gsl_vector_alloc (len);
  for (i = 0; i < len; i++)
    {
      gsl_vector_set (section_points, i, table[i].xs);
    }

  if (abscissa_to_pathlength (func, section_points))
    {
      gsl_vector_free (section_points);
      return -1;
    }

  for (i = 0; i < len; i++)
    {
      table[i].xi = gsl_vector_get (section_points, i);
    }

  gsl_vector_free (section_points);

  return 0;
}

/**
   creates an ap_pixel.

   @param x x coordinate of pixel relative to beam's reference point
   @param y y coordinate of pixel relative to beam's reference point
   @param px absolute x coordinate of the pixel
   @param py absolute y coordinate of the pixel
   @param ob the observation to get the pixels from
   @param sf pointer to sectionfun structure for this beam
   @param cur_ap a pointer to the next free aperture pixel
   @return a pointer to the next free aperture pixel after the new pixels
    have been added.

*/
static ap_pixel *
handle_one_pixel (const double x, const double y, const int px, const int py,
		  const observation * const obs, sectionfun * const sf,
		  const trace_func * const tracefun, ap_pixel * cur_ap)
{
  double res;
  double sect_y;
  double tmp;
  double phi_trace;

  if (find_section_point (sf, x, y, &res))
    {
      return cur_ap;	/* FIXME: Issue warning here */
    }
  /* set the extraction weight to 1. */
  cur_ap->weight = 1.;
  cur_ap->xs = res;
  phi_trace = atan (tracefun->deriv (cur_ap->xs, tracefun->data));
  cur_ap->dxs = phi_trace;
  cur_ap->ys = tracefun->func (cur_ap->xs, tracefun->data);
  sect_y = tracefun->func (res, tracefun->data);
  tmp = tracefun->func (x, tracefun->data);

  cur_ap->contam = -1.0;
  cur_ap->model = 0.0;
  cur_ap->p_x = px;
  cur_ap->p_y = py;
  cur_ap->x = x;
  cur_ap->y = y;
  cur_ap->dist =
    sqrt ((sect_y - y) * (sect_y - y) +
	  (cur_ap->xs - x) * (cur_ap->xs - x));

  if ( y < tmp ) {
    cur_ap->dist = cur_ap->dist * (-1.); /* If pixel is bwlow the trace, dist is neg. */
  }
  cur_ap->count = gsl_matrix_get (obs->grism, px, py);
  cur_ap->error = gsl_matrix_get (obs->pixerrs, px, py);
  if (obs->dq != NULL) cur_ap->dq = (long) gsl_matrix_get (obs->dq, px, py);
  else cur_ap->dq = 0;

  cur_ap++;
  return cur_ap;
}

/**
   Does some sanity checks on make_spc_table's input.

   @param ob the object to check
   @return 0 if everything is ok, -1 otherwise
*/
static int
sanitycheck (object * const ob)
{
  int i;

  for (i = 0; i < ob->nbeams; i++)
    {
      while (ob->beams[i].orient < 0)
	{
	  ob->beams[i].orient += M_PI;
	}
      while (ob->beams[i].orient > M_PI)
	{
	  ob->beams[i].orient -= M_PI;
	}
    }
  return 0;
}

#define MIN(x,y) (((x)<(y))?(x):(y))
#define MAX(x,y) (((x)>(y))?(x):(y))
/**
   computes a table of aperture pixels, i.e. of tuples containing the
   source coordinates, the distance to the spectrum trace, the path
   length along the trace, and the intensity.  The end of the table
   is marked with an ap_pixel->x==-1.  This routine can do subsampling,
   whereby each pixel is divided into n_sub*n_sub smaller pixels.
   If n_sub==1, a special (faster) handling is enabled.

   @param ob the object struct to examine
   @param beamorder the order of the spectrum to examine
   @param flags warning flags like in the warning field of the
     spectrum structure.  This has to be passed in initialized.
*/
ap_pixel *
make_spc_table (object * const ob, const int beamorder,
		int *const flags)
{
  int bb_x, bb_y, bb_w, bb_h;
  int x, y;
  beam *curbeam = ob->beams + beamorder;
  double dx, dy;
  ap_pixel *table, *cur_ap;
  is_in_descriptor iid;
  sectionfun sf;
  trace_func *tracefun = curbeam->spec_trace;

  quad_to_bbox (curbeam->corners, curbeam->bbox, curbeam->bbox + 1);
  bb_x = curbeam->bbox[0].x;
  bb_y = curbeam->bbox[0].y;
  bb_w = curbeam->bbox[1].x - curbeam->bbox[0].x + 1;
  bb_h = curbeam->bbox[1].y - curbeam->bbox[0].y + 1;

  if (sanitycheck (ob))
    {
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Input data failed sanity check");
      return NULL;
    }

  if (fill_is_in_descriptor (&iid, curbeam->corners))
    return NULL;

  if (fill_in_sectionfun (&sf, curbeam->orient, curbeam))
    return NULL;

  if (!(table =
	malloc ((bb_w * bb_h + 1) * sizeof (ap_pixel))))
    return NULL;

  /* We have a little coordinate confusion here.  There are three
     systems:
     (a) the absolute one, rooted in (0,0) of the image.
     (b) the relative one, rooted in the reference point and used
     when evaluating the spectrum trace.  This one is used
     for finding the section points, etc.
     (c) one relative to the bounding box; x and y below live in this
     system and have to be converted into whatever system is
     required.  dx, dy are used for the conversion to (b)
  */
  dx = bb_x - curbeam->refpoint.x;
  dy = bb_y - curbeam->refpoint.y;
  cur_ap = table;
  for (y = 0; y <= bb_h; y++)
    {
      for (x = 0; x < bb_w; x++)
	{
	  if ((bb_x + x < 0) || (bb_y + y < 0)
	      || (bb_x + x >= (int)ob->grism_obs->grism->size1)
	      || (bb_y + y >= (int)ob->grism_obs->grism->size2))
	    {
	      *flags |= SPC_W_BORDER;
	      continue;
	    }

	  // check whether the trace description has
	  // an order higher than linear
	  if (curbeam->spec_trace->type > 1)
	    {
	      // new criteria based on the true trace distance
	      // which means the true distance from the section point
	      if (!tracedist_criteria(x + dx, y + dy, &sf, tracefun, curbeam->width+2.0))
		{
		  continue;
		}
	    }
	  else
	    {
	      // old criteria based on the box model
	      if (!is_in (bb_x + x, bb_y + y, &iid))
		{
		  continue;
		}
	    }

	  if (isnan
	      (gsl_matrix_get
	       (ob->grism_obs->grism, bb_x + x, bb_y + y)))
	    {
	      continue;
	    }
	  //	  if (ob->ID == 11 && x + bb_x == 59)
	    //	    fprintf(stdout, "xx: %i, yy: %i: %i\n", x + bb_x, y + bb_y, is_in (x + bb_x, y + bb_y, &iid));
	  cur_ap =
	    handle_one_pixel (x + dx, y + dy, x + bb_x,
			      y + bb_y, ob->grism_obs, &sf,
			      tracefun, cur_ap);

	}
    }

  cur_ap->p_x = -1;
  cur_ap->p_y = -1;
  cur_ap->count = -1;

  free_sectionfun (&sf);

  if (transform_to_pathlen (curbeam->spec_trace, table))
    {
      //   free (table);
      //   table = NULL;
      //   return NULL;
    }
  return table;
}

/**
   Similar to make_spc_table, it just works only
   on one spot given in the parameters.
*/
ap_pixel *
make_gps_table (object * const ob, const int beamorder,
		int *const flags, int xval, int yval)
{
  int bb_x, bb_y, bb_w, bb_h;
  int x, y;
  beam *curbeam = ob->beams + beamorder;
  double dx, dy;
  ap_pixel *table, *cur_ap;
  is_in_descriptor iid;
  sectionfun sf;
  trace_func *tracefun = curbeam->spec_trace;

  quad_to_bbox (curbeam->corners, curbeam->bbox, curbeam->bbox + 1);
  bb_x = curbeam->bbox[0].x;
  bb_y = curbeam->bbox[0].y;
  bb_w = curbeam->bbox[1].x - curbeam->bbox[0].x + 1;
  bb_h = curbeam->bbox[1].y - curbeam->bbox[0].y + 1;

  if (sanitycheck (ob))
    {
      aXe_message (aXe_M_FATAL, __FILE__, __LINE__,
		   "Input data failed sanity check");
      return NULL;
    }

  if (fill_is_in_descriptor (&iid, curbeam->corners))
    return NULL;

  if (fill_in_sectionfun (&sf, curbeam->orient, curbeam))
    return NULL;

  if (!
      (table =
       malloc (2 * sizeof (ap_pixel))))
    return NULL;

  /* We have a little coordinate confusion here.  There are three
     systems:
     (a) the absolute one, rooted in (0,0) of the image.
     (b) the relative one, rooted in the reference point and used
     when evaluating the spectrum trace.  This one is used
     for finding the section points, etc.
     (c) one relative to the bounding box; x and y below live in this
     system and have to be converted into whatever system is
     required.  dx, dy are used for the conversion to (b)
  */
  dx = bb_x - curbeam->refpoint.x;
  dy = bb_y - curbeam->refpoint.y;
  x  = xval - bb_x;
  y  = yval - bb_y;

  cur_ap = table;

  cur_ap =
    handle_one_pixel (x + dx, y + dy, x + bb_x,
		      y + bb_y, ob->grism_obs, &sf,
		      tracefun, cur_ap);

  cur_ap->p_x = -1;
  cur_ap->p_y = -1;
  cur_ap->count = -1;

  free_sectionfun (&sf);

  transform_to_pathlen (curbeam->spec_trace, table);

  return table;
}

void
print_ap_pixel_table (const ap_pixel * ap_p)
{
  printf ("# x y pathlen distance lambda count error\n");
  while (ap_p->p_x != -1)
    {
      printf ("%d %d %f %f %f %f %f %f %f %ld\n", ap_p->p_x, ap_p->p_y,
	      ap_p->x, ap_p->y, ap_p->xi, ap_p->dist, ap_p->lambda,
	      ap_p->count, ap_p->error, ap_p->dq);
      ap_p++;
    }
}

/*
int
tracedist_criteria(const double x, const double y, sectionfun * const sf,
		   const trace_func *tracefun, const double width)
{
  double x_sect, y_sect;
  double dist, max_dist;

  int ireturn=0;

  find_section_point (sf, x, y, &x_sect);
  y_sect = tracefun->func (x_sect, tracefun->data);

  dist =  sqrt((y_sect-y)*(y_sect-y)+(x_sect-x)*(x_sect-x));

  if (dist < width)
    ireturn=1;

  return ireturn;
}
*/


/**
 * Copyright (C) 2019 Samsung Electronics Co., Ltd. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *   http://www.apache.org/licenses/LICENSE-2.0
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *
 * @file	tensor.h
 * @date	04 December 2019
 * @brief	This is Tensor class for calculation
 * @see		https://github.com/nnstreamer/nntrainer
 * @author	Jijoong Moon <jijoong.moon@samsung.com>
 * @bug		No known bugs except for NYI items
 *
 */

#ifndef __TENSOR_H__
#define __TENSOR_H__
#ifdef __cplusplus

#ifdef USE_BLAS
extern "C" {
#include <cblas.h>
}
#endif

#include <cmath>
#include <fstream>
#include <iostream>
#include <memory>
#include <regex>
#include <vector>

namespace nntrainer {

#define MAXDIM 4

class TensorDim {
public:
  TensorDim() {
    for (int i = 0; i < MAXDIM; ++i) {
      dim[i] = 1;
    }
  }
  ~TensorDim(){};
  unsigned int batch() { return dim[0]; };
  unsigned int channel() { return dim[1]; };
  unsigned int height() { return dim[2]; };
  unsigned int width() { return dim[3]; };

  void batch(unsigned int b) { dim[0] = b; };
  void channel(unsigned int c) { dim[1] = c; };
  void height(unsigned int h) { dim[2] = h; };
  void width(unsigned int w) { dim[3] = w; };

  unsigned int *getDim() { return dim; }

  int setTensorDim(std::string input_shape);

private:
  unsigned int dim[4];
};

/**
 * @class   Tensor Class for Calculation
 * @brief   Tensor Class for Calculation
 */
class Tensor {
public:
  /**
   * @brief     Constructor of Tensor
   */
  Tensor() : height(0), width(0), batch(0), ndim(0), len(0){};

  /**
   * @brief     Constructor of Tensor with batch size one
   * @param[in] heihgt Height of Tensor
   * @param[in] width Width of Tensor
   */
  Tensor(int height, int width);

  /**
   * @brief     Constructor of Tensor
   * @param[in] batch Batch of Tensor
   * @param[in] heihgt Height of Tensor
   * @param[in] width Width of Tensor
   */
  Tensor(int batch, int height, int width);

  /**
   * @brief   Constructor of Tensor
   * @param[in] d data for the Tensor with batch size one
   */
  Tensor(std::vector<std::vector<float>> const &d);

  /**
   * @brief     Constructor of Tensor
   * @param[in] d data for the Tensor
   */
  Tensor(std::vector<std::vector<std::vector<float>>> const &d);

  /**
   * @brief     return value at specific location
   * @param[in] batch batch location
   * @param[in] h height location
   * @param[in] w width location
   */
  float getValue(int batch, int h, int w);

  /**
   * @brief     Multiply value element by element
   * @param[in] value multiplier
   * @retval    Calculated Tensor
   */
  Tensor multiply(float const &value);

  /**
   * @brief     Divide value element by element
   * @param[in] value Divisor
   * @retval    Calculated Tensor
   */
  Tensor divide(float const &value);

  /**
   * @brief     Add Tensor Element by Element
   * @param[in] m Tensor to be added
   * @retval    Calculated Tensor
   */
  Tensor add(Tensor const &m) const;

  /**
   * @brief     Add value Element by Element
   * @param[in] value value to be added
   * @retval    Calculated Tensor
   */
  Tensor add(float const &value);

  /**
   * @brief     Substract Tensor Element by Element
   * @param[in] m Tensor to be added
   * @retval    Calculated Tensor
   */
  Tensor subtract(Tensor const &m) const;

  /**
   * @brief     subtract value Element by Element
   * @param[in] value value to be added
   * @retval    Calculated Tensor
   */
  Tensor subtract(float const &value);

  /**
   * @brief     Multiply Tensor Element by Element ( Not the MxM )
   * @param[in] m Tensor to be multiplied
   * @retval    Calculated Tensor
   */
  Tensor multiply(Tensor const &m) const;

  /**
   * @brief     Divide Tensor Element by Element
   * @param[in] m Divisor Tensor
   * @retval    Calculated Tensor
   */
  Tensor divide(Tensor const &m) const;

  /**
   * @brief     Dot Product of Tensor ( equal MxM )
   * @param[in] m Tensor
   * @retval    Calculated Tensor
   */
  Tensor dot(Tensor const &m) const;

  /**
   * @brief     Transpose Tensor
   * @retval    Calculated Tensor
   */
  Tensor transpose() const;

  /**
   * @brief     sum all the Tensor elements according to the batch
   * @retval    Calculated Tensor(batch, 1, 1)
   */
  Tensor sum() const;

  /**
   * @brief     sum all the Tensor elements according to the axis
   * @retval    Calculated Tensor
   */
  Tensor sum(int axis) const;

  /**
   * @brief     Averaging the Tensor elements according to the batch
   * @retval    Calculated Tensor(1, height, width)
   */
  Tensor average() const;

  /**
   * @brief     Softmax the Tensor elements
   * @retval    Calculated Tensor
   */
  Tensor softmax() const;

  /**
   * @brief     l2norm the Tensor elements
   * @retval    Calculated l2norm
   */
  float l2norm() const;

  /**
   * @brief     Normalize the Tensor elements
   * @retval    Calculated Tensor
   */
  Tensor normalization() const;

  /**
   * @brief     Standardize the Tensor elements
   * @retval    Calculated Tensor
   */
  Tensor standardization() const;

  /**
   * @brief     Fill the Tensor elements with zero
   */
  void setZero();

  /**
   * @brief     Reduce Rank ( Tensor to Vector )
   * @retval    Saved vector
   */
  std::vector<float> mat2vec();

  /**
   * @brief     Apply function element by element
   * @param[in] *function function pointer applied
   * @retval    Tensor
   */
  Tensor apply(float (*function)(float)) const;

  /**
   * @brief     Apply function to Tensor
   * @param[in] *function function pointer applied
   * @retval    Tensor
   */
  Tensor apply(Tensor (*function)(Tensor)) const;

  /**
   * @brief     Print element
   * @param[in] out out stream
   * @retval    Tensor
   */
  void print(std::ostream &out) const;

  /**
   * @brief     Get Width of Tensor
   * @retval    int Width
   */
  int getWidth() { return width; };

  /**
   * @brief     Get Height of Tensor
   * @retval    int Height
   */
  int getHeight() { return height; };

  /**
   * @brief     Get Batch of Tensor
   * @retval    int Batch
   */
  int getBatch() { return batch; };

  /**
   * @brief     Set the elelemnt value
   * @param[in] batch batch location
   * @param[in] i height location
   * @param[in] j width location
   * @param[in] value value to be stored
   */
  void setValue(int batch, int i, int j, float value);

  /**
   * @brief     Copy the Tensor
   * @param[in] from Tensor to be Copyed
   * @retval    Matix
   */
  Tensor &copy(Tensor const &from);

  /**
   * @brief     Save the Tensor into file
   * @param[in] file output file stream
   */
  void save(std::ofstream &file);

  /**
   * @brief     Read the Tensor from file
   * @param[in] file input file stream
   */
  void read(std::ifstream &file);

  /**
   * @brief     return argument index which value is max
   * @retval    int argument index
   */
  int argmax();

  /**
   * @brief     return Data pointer of Tensor
   * @retval    float pointer
   */
  float *getData() { return data.data(); }

private:
  /**< handle the data as a std::vector type */
  std::vector<float> data;
  int height;
  int width;
  int batch;
  int ndim;
  int len;
};

/**
 * @brief   Overriding output stream
 */
std::ostream &operator<<(std::ostream &out, Tensor const &m);

} /* namespace nntrainer */

#endif /* __cplusplus */
#endif /* __TENSOR_H__ */


/*
** hemodynamic modelling for single-subject LISA
**
** G.Lohmann, MPI-KYB,  2018
*/
#include <viaio/Vlib.h>
#include <viaio/VImage.h>
#include <viaio/mu.h>
#include <viaio/option.h>

#include <gsl/gsl_cblas.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_spline.h>

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

#define ABS(x) ((x) > 0 ? (x) : -(x))
#define SQR(x) ((x)*(x))

#define LEN     10000   /* buffer length        */
#define NTRIALS 10000   /* max number of trials */

/* standard parameter values for gamma function, Glover 99 */
double a1 = 6;
double b1 = 0.9;
double a2 = 12;
double b2 = 0.9;
double cc = 0.35;

typedef struct TrialStruct {
  int   id;
  float onset;
  float duration;
  float height;
} Trial;

void printmat(gsl_matrix *R,char *str)
{
  int i,j;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<R->size1; i++) {
    for (j=0; j<R->size2; j++) {
      fprintf(stderr," %9.6f",gsl_matrix_get(R,i,j));
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void tprintmat(gsl_matrix *R,char *str)
{
  int i,j;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<R->size2; i++) {
    for (j=0; j<R->size1; j++) {
      fprintf(stderr," %9.6f",gsl_matrix_get(R,j,i));
    }
    fprintf(stderr,"\n");
  }
  fprintf(stderr,"\n");
}

void printvec(gsl_vector *x,char *str)
{
  int i;
  fprintf(stderr," %s: \n",str);
  for (i=0; i<x->size; i++) {
    fprintf(stderr," %f\n",x->data[i]);
  }
  fprintf(stderr,"\n");
}

/* output txt file for plotting */
void PlotDesign(gsl_matrix *X,double tr,VString filename)
{
  int i,j;
  double u=0;

  FILE *fp = fopen(filename,"w");
  if (fp == NULL) VError("err opening plot file");
  double t = 0;
  for (i=0; i<X->size1; i++) {
    fprintf(fp," %8.2f",t);
    for (j=0; j<X->size2; j++) {
      u = gsl_matrix_get(X,i,j);
      fprintf(fp," %10.6f",u);
    }
    t += tr;
    fprintf(fp,"\n");
  }
  fclose(fp);
}

/* create design X */
gsl_matrix *VCreateDesign(int ntimesteps,int nevents,int hemomodel,VBoolean firstcol,gsl_matrix *covariates)
{
  int dim = nevents;
  if (hemomodel == 1) dim = nevents*2;
  if (hemomodel == 2) dim = nevents*3;
  if (firstcol == TRUE) dim++;
  if (covariates != NULL) dim += covariates->size2;  /* nuisance covariates without task labels */

  gsl_matrix *X = gsl_matrix_calloc(ntimesteps,dim);
  return X;
}

/* Glover kernel, gamma function */
double xgamma(double xx, double t0)
{
  double x, y, scale = 20;
  double y1, y2;
  double d1, d2;
  x = xx - t0;
  if(x < 0 || x > 50)
    return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(x / d1, a1) * exp(-(x - d1) / b1);
  y2 = pow(x / d2, a2) * exp(-(x - d2) / b2);
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* Glover kernel, gamma function, parameters changed for block designs */
double bgamma(double xx, double t0)
{
  double x, y, scale = 120;
  double y1, y2;
  double d1, d2;
  double aa1 = 6;
  double bb1 = 0.9;
  double aa2 = 12;
  double bb2 = 0.9;
  double cx  = 0.1;
  x = xx - t0;
  if(x < 0 || x > 50)
    return 0;
  d1 = aa1 * bb1;
  d2 = aa2 * bb2;
  y1 = pow(x / d1, aa1) * exp(-(x - d1) / bb1);
  y2 = pow(x / d2, aa2) * exp(-(x - d2) / bb2);
  y = y1 - cx * y2;
  y /= scale;
  return y;
}

/* first derivative */
double deriv1_gamma(double x, double t0)
{
  double d1, d2, y1, y2, y, xx;
  double scale = 20.0;
  xx = x - t0;
  if(xx < 0 || xx > 50) return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(d1, -a1) * a1 * pow(xx, (a1 - 1.0)) * exp(-(xx - d1) / b1)
    - (pow((xx / d1), a1) * exp(-(xx - d1) / b1)) / b1;
  y2 = pow(d2, -a2) * a2 * pow(xx, (a2 - 1.0)) * exp(-(xx - d2) / b2)
    - (pow((xx / d2), a2) * exp(-(xx - d2) / b2)) / b2;
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* second derivative */
double deriv2_gamma(double x, double t0)
{
  double d1, d2, y1, y2, y3, y4, y, xx;
  double scale = 20.0;
  xx = x - t0;
  if (xx < 0 || xx > 50) return 0;
  d1 = a1 * b1;
  d2 = a2 * b2;
  y1 = pow(d1, -a1) * a1 * (a1 - 1) * pow(xx, a1 - 2) * exp(-(xx - d1) / b1)
    - pow(d1, -a1) * a1 * pow(xx, (a1 - 1)) * exp(-(xx - d1) / b1) / b1;
  y2 = pow(d1, -a1) * a1 * pow(xx, a1 - 1) * exp(-(xx - d1) / b1) / b1
    - pow((xx / d1), a1) * exp(-(xx - d1) / b1) / (b1 * b1);
  y1 = y1 - y2;
  y3 = pow(d2, -a2) * a2 * (a2 - 1) * pow(xx, a2 - 2) * exp(-(xx - d2) / b2)
    - pow(d2, -a2) * a2 * pow(xx, (a2 - 1)) * exp(-(xx - d2) / b2) / b2;
  y4 = pow(d2, -a2) * a2 * pow(xx, a2 - 1) * exp(-(xx - d2) / b2) / b2
    - pow((xx / d2), a2) * exp(-(xx - d2) / b2) / (b2 * b2);
  y2 = y3 - y4;
  y = y1 - cc * y2;
  y /= scale;
  return y;
}

/* Gaussian function */
double xgauss(double xx, double t0)
{
  double sigma=1.0;
  double x, y, z, a = 2.506628273;
  x = (xx - t0);
  z = x / sigma;
  y = exp(-0.5*z*z) / (sigma * a);
  return y;
}

void XConvolve(double *src,double *dst,double *kernel,int nt,int kernelsize)
{
  int i,j,jj,k;
  double sum=0;

  for (i=0; i<nt; i++) {
    sum = 0;
    k=0;
    for (j=i; j<i+kernelsize; j++) {
      jj = i-k;
      if (jj >= 0 && jj < nt) {
	sum += src[jj] * kernel[k];
      }
      k++;
    }
    dst[i] = sum;
  }
}

/* hemodynamic modelling at high temporal resolution */
void VHemoModel(Trial *trial,int ntrials,int nevents,int nt,double xtr,int hemomodel,VBoolean firstcol,
		gsl_matrix *X,gsl_matrix *covariates)
{
  int i,j,jj,k;
  double t,t0,t1,h;

  /* set temporal resolution to 0.5 sec, reduce discretization artefacts */
  double hfactor = floor(xtr/0.5);
  if (hfactor < 1.0) hfactor = 1.0;

  double tr = xtr/hfactor;
  int jfactor = (int)hfactor;
  int ntimesteps = jfactor*nt;

  for(i = 0; i < nevents; i++) {
    double xmin = VRepnMaxValue(VFloatRepn);
    for(j = 0; j < ntrials; j++) {
      if(trial[j].id != i) continue;
      if(trial[j].duration < xmin) xmin = trial[j].duration;
    }
  }

  /* get kernels */
  t1 = 30.0;    /* kernel duration = 30 secs */
  int kernelsize = t1 / tr;
  if (tr < 0.01) VWarning(" implausible TR (%f seconds)",tr);

  /* fprintf(stderr," hf= %f,  tr= %f  %f, kernelsize: %d\n",hfactor,xtr,tr,kernelsize); */

  double *kernel1=NULL,*kernel2=NULL;
  double *bkernel  = (double *)VCalloc(kernelsize,sizeof(double));
  double *kernel0  = (double *)VCalloc(kernelsize,sizeof(double));
  if (hemomodel == 1 || hemomodel == 2) {
    kernel1 = (double *)VCalloc(kernelsize,sizeof(double));
  }
  if (hemomodel == 2) {
    kernel2 = (double *)VCalloc(kernelsize,sizeof(double));
  }

  i = 0;
  for (t = 0; t < t1; t += tr) {
    if (i >= kernelsize) break;

    bkernel[i] = xgauss(t, 5.0*tr);
    kernel0[i] = xgamma(t, 0.0);

    if(hemomodel == 1 || hemomodel == 2)
      kernel1[i] = deriv1_gamma(t, 0);
    if(hemomodel == 2)
      kernel2[i] = deriv2_gamma(t, 0);
    i++;
  }

  /* tmp storage */
  double *x = (double *) VCalloc(ntimesteps,sizeof(double));
  double *y = (double *) VCalloc(ntimesteps,sizeof(double));

  /* constant in column 0 */
  int col=0;
  if (firstcol == TRUE) {
    gsl_matrix_set_zero(X);
    for (j=0; j<nt; j++) gsl_matrix_set(X,j,0,1.0);
    col=1;
  }

  /* for each trial,event, do... */
  for (i = 0; i < nevents; i++) {
    for (k=0; k<ntimesteps; k++) x[k] = y[k] = 0;

    /* read design info */
    int trialcount = 0;
    for (j = 0; j < ntrials; j++) {
      if(trial[j].id != i+1) continue;
      trialcount++;
      t0 = trial[j].onset;
      t1 = trial[j].onset + trial[j].duration;
      h  = trial[j].height;
      int k0 = (int) (t0/tr + 0.5);
      int k1 = (int) (t1/tr + 0.5);

      int klen=0;
      for (k=k0; k<=k1; k++) {
	if (k < 0 || k >= ntimesteps) continue;
	x[k] = h;
	klen++;
      }
      if(trialcount < 1)
	VError(" no trials in event %d, please re-number event-ids,  ntrials= %d", i + 1,ntrials);
    }

    /* convolve */
    if (hemomodel == 3)  {       /* block design, gaussian kernel */
      XConvolve(x,y,bkernel,ntimesteps,kernelsize);
    }
    else {                       /* gamma function kernel */
      XConvolve(x,y,kernel0,ntimesteps,kernelsize);
    }

    for (j=0; j<X->size1; j++) {
      jj = j*jfactor;
      if (jj >= ntimesteps) continue;
      gsl_matrix_set(X,j,col,y[jj]/hfactor);
    }
    col++;

    if (hemomodel == 1 || hemomodel == 2) {   /* gamma function kernel, first derivative */
      XConvolve(x,y,kernel1,ntimesteps,kernelsize);
      for (j=0; j<X->size1; j++) {
	jj = j*jfactor;
	if (jj >= ntimesteps) continue;
	gsl_matrix_set(X,j,col,y[jj]/hfactor);
      }
      col++;
    }
    if (hemomodel == 2) {   /* gamma function kernel, second derivative */
      XConvolve(x,y,kernel2,ntimesteps,kernelsize);
      for (j=0; j<X->size1; j++) {
	jj = j*jfactor;
	if (jj >= ntimesteps) continue;
	gsl_matrix_set(X,j,col,y[jj]/hfactor);
      }
      col++;
    }
  }

  /* add further covariates that have no task labels, will not be permuted */
  if (covariates != NULL) {
    for (i=0; i<covariates->size2; i++) {
      for (j=0; j<covariates->size1; j++) {
	if (col >= X->size2) VError(" VHemoModel, column= %d  %d\n",col,(int)X->size2);
	gsl_matrix_set(X,j,col,gsl_matrix_get(covariates,j,i));
      }
      col++;
    }
  }

  /* cleanup */
  VFree(x);
  VFree(y);
  VFree(kernel0);
  VFree(bkernel);
  if (kernel1 != NULL) VFree(kernel1);
  if (kernel2 != NULL) VFree(kernel2);
}


//Causal FIR filtering of each vector in X along dim.
//FIR impulse response is given in vector B with length Q+1.
//(I use P for IIR filter order, since same as polynomial order.)

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

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

int fir_s (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_d (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_c (float *Y, const float *X, const float *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);
int fir_z (double *Y, const double *X, const double *B, const size_t R, const size_t C, const size_t S, const size_t H, const int iscolmajor, const size_t Q, const size_t dim);

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

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Initialize Y
    float b = *B++;
    for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    X -= N; Y -= N - 1u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fmaf(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=Q, ++Y)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fmaf(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=L, Y+=L-Q)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_saxpy((int)(L-q),*B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += K - 1u;
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, B-=Q, Y-=K*Q-1u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, Y+=K) { cblas_saxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

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

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;

    //Initialize Y
    double b = *B++;
    for (size_t n=N; n>0u; --n, ++X, ++Y) { *Y = b * *X; }
    X -= N; Y -= N - 1u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, ++B, X-=L-q+1u, Y-=L-q)
            {
                b = *B;
                for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fma(b,*X,*Y); }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=Q, ++Y)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B)
                    {
                        b = *B;
                        for (size_t l=q; l<L; ++l, ++X, ++Y) { *Y = fma(b,*X,*Y); }
                        if (q<Q) { X -= L-q; Y -= L-q-1u; }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, B-=Q, X+=L, Y+=L-Q)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, ++Y) { cblas_daxpy((int)(L-q),*B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += K - 1u;
            for (size_t g=G; g>0u; --g, X+=BS*(L-1u), Y+=BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, ++X, B-=Q, Y-=K*Q-1u)
                {
                    for (size_t q=1u; q<=Q; ++q, ++B, Y+=K) { cblas_daxpy((int)(L-q),*B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

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

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    float xr, xi, br, bi;

    //Initialize Y
    br = *B++; bi = *B++;
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = br*xr - bi*xi;
        *Y++ = br*xi + bi*xr;
    }
    X -= 2u*N; Y -= 2u*N - 2u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
            {
                br = *B++; bi = *B++;
                for (size_t l=q; l<L; ++l)
                {
                    xr = *X++; xi = *X++;
                    *Y++ += br*xr - bi*xi;
                    *Y++ += br*xi + bi*xr;
                }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_caxpy((int)(L-q),B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=2u*Q, X+=2u*Q, Y+=2u)
                {
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l)
                        {
                            xr = *X++; xi = *X++;
                            *Y++ += br*xr - bi*xi;
                            *Y++ += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, X+=2u*L, B-=2u*Q, Y+=2u*(L-Q-1u))
                {
                    for (size_t q=0u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_caxpy((int)(L-q),B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += 2u*K - 2u;
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X+=2u, B-=2u*Q, Y-=2u*K*Q-2u)
                {
                    for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u*K) { cblas_caxpy((int)(L-q),B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

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

    const size_t N = R*C*S*H;
    const size_t L = (dim==0u) ? R : (dim==1u) ? C : (dim==2u) ? S : H;
    double xr, xi, br, bi;

    //Initialize Y
    br = *B++; bi = *B++;
    for (size_t n=N; n>0u; --n)
    {
        xr = *X++; xi = *X++;
        *Y++ = br*xr - bi*xi;
        *Y++ = br*xi + bi*xr;
    }
    X -= 2u*N; Y -= 2u*N - 2u;

    if (N==0u || L==1u || Q==0u) {}
    else if (L==N)
    {
        if (L<30000u)
        {
            for (size_t q=1u; q<=Q; ++q, X-=2u*(L-q+1u), Y-=2u*(L-q))
            {
                br = *B++; bi = *B++;
                for (size_t l=q; l<L; ++l)
                {
                    xr = *X++; xi = *X++;
                    *Y++ += br*xr - bi*xi;
                    *Y++ += br*xi + bi*xr;
                }
            }
        }
        else
        {
            for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_zaxpy((int)(L-q),B,X,1,Y,1); }
        }
    }
    else
    {
        const size_t K = (iscolmajor) ? ((dim==0u) ? 1u : (dim==1u) ? R : (dim==2u) ? R*C : R*C*S) : ((dim==0u) ? C*S*H : (dim==1u) ? S*H : (dim==2u) ? H : 1u);
        const size_t BS = (iscolmajor && dim==0u) ? C*S*H : K;
        const size_t V = N/L, G = V/BS;

        if (K==1u && (G==1u || BS==1u))
        {
            if (L<30000u)
            {
                for (size_t v=V; v>0u; --v, B-=2u*Q, X+=2u*Q, Y+=2u)
                {
                    for (size_t q=1u; q<=Q; ++q)
                    {
                        br = *B++; bi = *B++;
                        for (size_t l=q; l<L; ++l)
                        {
                            xr = *X++; xi = *X++;
                            *Y++ += br*xr - bi*xi;
                            *Y++ += br*xi + bi*xr;
                        }
                        if (q<Q) { X -= 2u*(L-q); Y -= 2u*(L-q-1u); }
                    }
                }
            }
            else
            {
                for (size_t v=V; v>0u; --v, X+=2u*L, B-=2u*Q, Y+=2u*(L-Q-1u))
                {
                    for (size_t q=0u; q<=Q; ++q, B+=2u, Y+=2u) { cblas_zaxpy((int)(L-q),B,X,1,Y,1); }
                }
            }
        }
        else
        {
            Y += 2u*K - 2u;
            for (size_t g=G; g>0u; --g, X+=2u*BS*(L-1u), Y+=2u*BS*(L-1u))
            {
                for (size_t bs=BS; bs>0u; --bs, X+=2u, B-=2u*Q, Y-=2u*K*Q-2u)
                {
                    for (size_t q=1u; q<=Q; ++q, B+=2u, Y+=2u*K) { cblas_zaxpy((int)(L-q),B,X,(int)K,Y,(int)K); }
                }
            }
        }
    }

    return 0;
}

#ifdef __cplusplus
}
}
#endif


#include <stdio.h>
#include <gsl/gsl_sf_bessel.h>

int
main (void)
{
  double x = 5.0;
  double y = gsl_sf_bessel_J0 (x);
  printf ("J0(%g) = %.18e\n", x, y);
  return 0;
}


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

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

#include <gsl/gsl_math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_sort.h>
#include <gsl/gsl_sort_vector.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_ieee_utils.h>

int test_nist (void);

/* Test program for mean.c.  JimDavies 7.96 */

#define BASE_LONG_DOUBLE
#include "templates_on.h"
#include "test_float_source.c"
#include "templates_off.h"
#undef  BASE_LONG_DOUBLE

#define BASE_DOUBLE
#include "templates_on.h"
#include "test_float_source.c"
#include "templates_off.h"
#undef  BASE_DOUBLE

#define BASE_FLOAT
#include "templates_on.h"
#include "test_float_source.c"
#include "templates_off.h"
#undef  BASE_FLOAT

#define BASE_ULONG
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_ULONG

#define BASE_LONG
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_LONG

#define BASE_UINT
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_UINT

#define BASE_INT
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_INT

#define BASE_USHORT
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_USHORT

#define BASE_SHORT
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_SHORT

#define BASE_UCHAR
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_UCHAR

#define BASE_CHAR
#include "templates_on.h"
#include "test_int_source.c"
#include "templates_off.h"
#undef  BASE_CHAR

int
main (void)
{
  size_t s1, s2;

  gsl_ieee_env_setup ();

  for (s1 = 1; s1 < 4 ; s1++)
    {
      s2 = (s1 < 3) ? 1 : (s1 - 1) ;

      test_func (s1,s2);
      test_float_func (s1,s2);
      test_long_double_func (s1,s2);

      test_ulong_func (s1,s2);
      test_long_func (s1,s2);
      test_uint_func (s1,s2);
      test_int_func (s1,s2);
      test_ushort_func (s1,s2);
      test_short_func (s1,s2);
      test_uchar_func (s1,s2);
      test_char_func (s1,s2);
    }

  test_nist();

  exit (gsl_test_summary ());
}


/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

  1. Redistributions of source code must retain the above copyright notice,
    this list of conditions and the following disclaimer.

  2. Redistributions in binary form must reproduce the above copyright notice,
    this list of conditions and the following disclaimer in the
    documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY AUTHORS ``AS IS'' AND ANY EXPRESS OR IMPLIED
WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL AUTHORS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR
BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
OF SUCH DAMAGE.
*/

/**
 * \file utils.c
 * \brief Source file with the useful data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include <glib.h>
#include "config.h"
#include "equation.h"

#define DEBUG_UTILS 0           ///< macro to debug the useful functions.

char *error_message = NULL;     ///< error message string.

/**
 * Function to add an error message.
 */
void
error_add (const char *message) ///< error message.
{
	char *buffer;
  buffer = error_message;
  error_message = (char *) g_strconcat (message, "\n", buffer, NULL);
  g_free (buffer);
}

/**
 * Function to calculate the distance between two vectors.
 *
 * \return vectors distance.
 */
long double
distance (long double *r1,      ///< 1st vector.
          long double *r2)      ///< 2nd vector.
{
  long double d, dr[3];
#if DEBUG_EQUATION
  fprintf (stderr, "distance: start\n");
  fprintf (stderr, "distance: x1=%Lg y1=%Lg z1=%Lg\n", r1[0], r1[1], r1[2]);
  fprintf (stderr, "distance: x2=%Lg y2=%Lg z2=%Lg\n", r2[0], r2[1], r2[2]);
#endif
  dr[0] = r1[0] - r2[0];
  dr[1] = r1[1] - r2[1];
  dr[2] = r1[2] - r2[2];
  d = sqrtl (dr[0] * dr[0] + dr[1] * dr[1] + dr[2] * dr[2]);
#if DEBUG_EQUATION
  fprintf (stderr, "distance: d=%Lg\n", d);
  fprintf (stderr, "distance: end\n");
#endif
  return d;
}

/**
 * Function to calculate the solution of a reduced 2nd order equation.
 *
 * This function calculates the solution of a reduced 2nd order equation in the
 * form:
 * \f[x^2+a\,x+b=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_quadratic_reduced (long double a, ///< a equation coefficient.
                         long double b, ///< b equation coefficient.
                         long double x1,        ///< lower solution limit.
                         long double x2)        ///< higher solution limit.
{
  long double a2, k, x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic_reduced: start\n");
  fprintf (stderr, "solve_quadratic_reduced: a=%Lg b=%Lg\n", a, b);
  fprintf (stderr, "solve_quadratic_reduced: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  a2 = -0.5L * a;
  k = sqrtl (a2 * a2 - b);
  x = a2 + k;
  if (x < x1 || x > x2)
    x = a2 - k;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic_reduced: x=%Lg\n", x);
  fprintf (stderr, "solve_quadratic_reduced: end\n");
#endif
  return x;
}

/**
 * Function to calculate the solution of a 2nd order equation.
 *
 * This function calculates the solution of a 2nd order equation in the
 * form:
 * \f[a\,x^2+b\,x+c=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_quadratic (long double a, ///< a equation coefficient.
                 long double b, ///< b equation coefficient.
                 long double c, ///< c equation coefficient.
                 long double x1,        ///< lower solution limit.
                 long double x2)        ///< higher solution limit.
{
  long double x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic: start\n");
  fprintf (stderr, "solve_quadratic: a=%Lg b=%Lg c=%Lg\n", a, b, c);
  fprintf (stderr, "solve_quadratic: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  if (a == 0.L)
    x = -c / b;
  else
    x = solve_quadratic_reduced (b / a, c / a, x1, x2);
#if DEBUG_EQUATION
  fprintf (stderr, "solve_quadratic: x=%Lg\n", x);
  fprintf (stderr, "solve_quadratic: end\n");
#endif
  return x;
}

/**
 * Function to calculate the solution of a reduced 3rd order equation.
 *
 * This function calculates the solution of a reduced 3rd order equation in the
 * form:
 * \f[x^3+a\,x^2+b\,x+c=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_cubic_reduced (long double a,     ///< a equation coefficient.
                     long double b,     ///< b equation coefficient.
                     long double c,     ///< c equation coefficient.
                     long double x1,    ///< lower solution limit.
                     long double x2)    ///< higher solution limit.
{
  long double k0, k1, k2;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic_reduced: start\n");
  fprintf (stderr, "solve_cubic_reduced: a=%Lg b=%Lg c=%Lg\n", a, b, c);
  fprintf (stderr, "solve_cubic_reduced: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  a /= 3.L;
  k0 = a * a;
  k1 = b / 3.L - k0;
  k0 = (b * a - c) / 2.L - a * k0;
  k2 = k1 * k1 * k1 + k0 * k0;
  if (k2 < 0.L)
    {
      k1 = sqrtl (-k1);
      k0 = acosl (k0 / (k1 * k1 * k1)) / 3.L;
      k1 *= 2.L;
      k2 = k1 * cosl (k0) - a;
      if (k2 < x1 || k2 > x2)
        {
          k2 = k1 * cosl (k0 + 2.L * M_PIl / 3.L) - a;
          if (k2 < x1 || k2 > x2)
            k2 = k1 * cosl (k0 - 2.L * M_PIl / 3.L) - a;
        }
    }
  else
    {
      k1 = sqrtl (k2);
      k2 = k0 + k1;
      k2 = cbrtl (k2);
      k0 -= k1;
      k2 += cbrtl (k0);
      k2 -= a;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic_reduced: x=%Lg\n", k2);
  fprintf (stderr, "solve_cubic_reduced: end\n");
#endif
  return k2;
}

/**
 * Function to calculate the solution of a 3rd order equation.
 *
 * This function calculates the solution of a 3rd order equation in the
 * form:
 * \f[a\,x^3+b\,x^2+c\,x+d=0\f]
 * in the interval \f$x\in\left[x_1,\;x_2\right]\f$.
 *
 * \return solution value. If the equation can not be solved or the solution is
 * not in the interval the value is undetermined.
 */
long double
solve_cubic (long double a,     ///< a equation coefficient.
             long double b,     ///< b equation coefficient.
             long double c,     ///< c equation coefficient.
             long double d,     ///< d equation coefficient.
             long double x1,    ///< lower solution limit.
             long double x2)    ///< higher solution limit.
{
  long double x;
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic: start\n");
  fprintf (stderr, "solve_cubic: a=%Lg b=%Lg c=%Lg d=%Lg\n", a, b, c, d);
  fprintf (stderr, "solve_cubic: x1=%Lg x2=%Lg\n", x1, x2);
#endif
  if (a == 0.L)
    x = solve_quadratic (b, c, d, x1, x2);
  else
    x = solve_cubic_reduced (b / a, c / a, d / a, x1, x2);
#if DEBUG_EQUATION
  fprintf (stderr, "solve_cubic: x=%Lg\n", x);
  fprintf (stderr, "solve_cubic: end\n");
#endif
  return x;
}

/**
 * Function to get an integer number of a XML node property.
 *
 * \return Integer number value.
 */
int
xml_node_get_int (xmlNode * node,       ///< XML node.
                  const xmlChar * prop, ///< XML property.
                  int *error_code)      ///< Error code.
{
  int i = 0;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%d", &i) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return i;
}

/**
 * Function to get an unsigned integer number of a XML node property.
 *
 * \return Unsigned integer number value.
 */
unsigned int
xml_node_get_uint (xmlNode * node,      ///< XML node.
                   const xmlChar * prop,        ///< XML property.
                   int *error_code)     ///< Error code.
{
  unsigned int i = 0;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%u", &i) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return i;
}

/**
 * Function to get an unsigned integer number of a XML node property with a
 *   default value.
 *
 * \return Unsigned integer number value.
 */
unsigned int
xml_node_get_uint_with_default (xmlNode * node, ///< XML node.
                                const xmlChar * prop,   ///< XML property.
                                unsigned int default_value,
                                ///< default value.
                                int *error_code)        ///< Error code.
{
  unsigned int i;
  if (xmlHasProp (node, prop))
    i = xml_node_get_uint (node, prop, error_code);
  else
    {
      i = default_value;
      *error_code = 0;
    }
  return i;
}

/**
 * Function to get a floating point number of a XML node property.
 *
 * \return Floating point number value.
 */
long double
xml_node_get_float (xmlNode * node,     ///< XML node.
                    const xmlChar * prop,       ///< XML property.
                    int *error_code)    ///< Error code.
{
  long double x = 0.L;
  xmlChar *buffer;
  buffer = xmlGetProp (node, prop);
  if (!buffer)
    *error_code = 1;
  else
    {
      if (sscanf ((char *) buffer, "%Lf", &x) != 1)
        *error_code = 2;
      else
        *error_code = 0;
      xmlFree (buffer);
    }
  return x;
}

/**
 * Function to get a floating point number of a XML node property with a
 *   default value.
 *
 * \return Floating point number value.
 */
long double
xml_node_get_float_with_default (xmlNode * node,        ///< XML node.
                                 const xmlChar * prop,  ///< XML property.
                                 long double default_value,
                                 ///< default value.
                                 int *error_code)       ///< Error code.
{
  long double x;
  if (xmlHasProp (node, prop))
    x = xml_node_get_float (node, prop, error_code);
  else
    {
      x = default_value;
      *error_code = 0;
    }
  return x;
}


//
// Created by david on 2018-11-30.
//

#ifndef DMRG_CLASS_XDMRG_FULL_FUNCTOR_H
#define DMRG_CLASS_XDMRG_FULL_FUNCTOR_H
#ifdef OpenMP_AVAILABLE
#include <omp.h>
#endif

#ifdef OpenBLAS_AVAILABLE
#include <cblas.h>
#endif

#include <Eigen/Core>
#include <unsupported/Eigen/CXX11/Tensor>
#include <general/nmspc_tensor_extra.h>
#include <general/class_tic_toc.h>
template<typename Scalar>
class class_xDMRG_full_functor {
private:
    double variance;
    double energy  ;
    double energy_lower_bound;
    double energy_upper_bound;
    double energy_target;
    double energy_window;
public:
    template <typename T>
    int sgn(const T val) const {
        return (T(0) < val) - (val < T(0));
    }

    using MatrixType_ = Eigen::Matrix<Scalar,Eigen::Dynamic, Eigen::Dynamic>;
    using VectorType_ = Eigen::Matrix<Scalar,Eigen::Dynamic, 1>;
    size_t   counter = 0;
//    const size_t shape;
    void set_energy_bounds(double E_lower, double E_upper);
    bool have_bounds_on_energy = false;
    double get_variance(){return variance;}
    double get_energy  (){return energy  ;}
    size_t get_count   (){return counter;}
    Eigen::Tensor<double,4> HA_MPO;
    Eigen::Tensor<double,4> HB_MPO;
    Eigen::Tensor<double,3> Lblock;
    Eigen::Tensor<double,3> Rblock;
    Eigen::Tensor<double,4> Lblock2;
    Eigen::Tensor<double,4> Rblock2;

    Eigen::Tensor<double,6> HAHB;
    Eigen::Tensor<double,8> HAHB2;
    Eigen::DSizes<long,4>   dsizes;
    class_tic_toc t_lbfgs;
    class_xDMRG_full_functor(
            const Eigen::Tensor<Scalar,4> &HA_MPO_,
            const Eigen::Tensor<Scalar,4> &HB_MPO_,
            const Eigen::Tensor<Scalar,3> &Lblock_,
            const Eigen::Tensor<Scalar,3> &Rblock_,
            const Eigen::Tensor<Scalar,4> &Lblock2_,
            const Eigen::Tensor<Scalar,4> &Rblock2_,
            const Eigen::DSizes<long,4>   &dsizes_
            );

    double get_vH2v(const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    double get_vHv(const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    Eigen::VectorXd get_vH2 (const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    Eigen::VectorXd get_vH (const Eigen::Matrix<double,Eigen::Dynamic,1> &v);
    double operator()(const Eigen::Matrix<double,Eigen::Dynamic,1> &v, Eigen::Matrix<double,Eigen::Dynamic,1> &grad);

};

#endif //DMRG_CLASS_XDMRG_FULL_FUNCTOR_H