#include <stdio.h>
#include <math.h>
#include <unistd.h> /* for getopt */
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv2.h>

#include "defaults.h"
#include "h2ocalc.h"
//#include"h2ocalc_test.h"

/* gcc h2ocalc.c -o h2ocalc -lgsl -lgslcblas -lm -Wall  */

/* struct ODE_param{ */
/*   int a[NEQ];  /\* dont worry let just run with global vars for now *\/ */
/*   float b[NEQ]; */
/* }; */

int nbprint(double x, double y[], int n) {
    int i;
    double sum = 0;
    printf("%6.4f   ", x);
    for (i = 0; i < n; i++) {
        printf("%6.2f ", y[i]);
        sum += y[i];
    }
    printf("  %6.2f\n",sum);
    return 0;
}

int nbeprint(double x, double y[], int n) {
    int i;
    double sum = 0;

    printf("%6.4e   ",x);
    for (i = 0; i < n; i++) {
        printf("%6.2e ", y[i]);
        sum += y[i];
    }
    printf("  %6.2e\n", sum);
    return 0;
}

int func (double t, const double y[],double f[], void *params) {
    // struct ODE_param *p = (struct ODE_params *) params;
    int i,j,k;
    double rate[NEQ];

    //printf("func\n");

    /* for each species ... */
    for (k = 0; k < NSPECIES; k++) {
        //printf("SPECIES %i\n",k);

        f[k] = 0;

        /* ...find all contributing and removing things */

        for (j = 0; j < NEQ; j++) {

            if (nmatrix[j][k] != 0) {

                rate[j] = nmatrix [j][k]; /* build the final equation */
                //printf("%3i ",nmatrix[j][k]);

                /* add generate I equations */
                rate[j] *= rconst[j]; /* get rate for reaction I_j */

                //printf(" * k%i ",j);
                for (i = 0; i < NSPECIES; i++) {
                    if (nmatrix[j][i] == -1 ) {
                        rate[j] *= y[i]; /* first order kinetics */
                        //printf("* y[%i] ",i);
                    }
                    if (nmatrix[j][i] == -2 ) {
                        rate[j] *= y[i]*y[i]; // second order kinetics
                        //printf("* y[%i]^2 ",i);
                    }
                }

                f[k] += rate[j];
                //printf(" + \n");
            }
        }

        //printf(" ----  k f[k]: %i     %e ---- \n",k,f[k]);

        //    nbeprint(t,y,NSPECIES);
        //    if (k ==8)
        //      exit(0);
    }

    //nbeprint(t,y,NSPECIES);
    //nbeprint(t,f,NSPECIES);

    /* add function for radiation, assumed in last bin */
    //f[NEQ-1] = 0;

    return GSL_SUCCESS;
}

int main(int argc, char **argv) {
    //  struct ODE_param *p = (struct ODE_params *) params;

    double dummy_param = 0;
    gsl_odeiv2_system sys = {func, NULL, NSPECIES, &dummy_param};
    gsl_odeiv2_driver *d = gsl_odeiv2_driver_alloc_y_new(&sys,
                                                         gsl_odeiv2_step_rk8pd,
                                                         1e-6,
                                                         1e-6,
                                                         0.0);
    int i,j,status;
    double t = NSTART;
    //  double t1 = NSTOP;
    //double foobar; /* trash parameter */
    double ti;
    double y[NSPECIES];

    double freq = FREQ;
    double doser = DOSER;
    double simtime = SIMTIME;
    double resol = RESOL;
    double period = 0;
    double dpulse = 0;
    double tick = 0;
    double ta = 1e-3; /* plotting clocks*/
    int pulse_left = 0;
    int pulse_counter = 0;
    signed long int tick_counter = 0;
    char *fname;
    int c;
    int flagp = 0, flagl = 0, flagc = 1; /* default: print each tick */

    /* parse options */
    opterr = 0;
    while ((c = getopt(argc, argv, "f:d:o:r:t:")) != -1) {
        switch(c) {
        case 'l':
            flagl = 1;
            flagp = 0;
            flagc = 0;
            break;
        case 'p':
            flagl = 0;
            flagp = 1;
            flagc = 0;
            break;
        case 'c':
            flagl = 0;
            flagp = 0;
            flagc = 1;
            break;
        case 'f':
            sscanf(optarg, "%lf", &freq);
            break;
        case 'd':
            sscanf(optarg, "%lf", &doser);
            break;
        case 'o':
            fname = optarg; /* not implemented */
            break;
        case 'r':
            sscanf(optarg, "%lf", &resol);
            break;
        case 't':
            sscanf(optarg, "%lf", &simtime);
            break;
        case '?':
            printf("Options:\n");
            printf("  -p output per pulse\n");
            printf("  -c output per tick\n");
            printf("  -l only end of output\n");
            printf("  -f frequency [Hz]\n");
            printf("  -d dose rate [Gy/min]\n");
            printf("  -o output file\n");
            printf("  -r tick resolution [sec]\n");
            printf("  -t simulation time [sec]\n");
            printf("\n");
            exit(0);
            break;
        default:
            printf ("?? no handle ?? %i\n", c);
            exit(-1);
        }
    }

    period = 1 / freq;
    dpulse = (doser / 60.0) / freq * EVJ; // in eV per pulse per liter

    if (period < resol) {
        printf(" *** Error: ");
        printf("Period must be larger than tick resolution.\n");
        exit(-1);
    }

    /* find ticksize closest to the requested one */
    tick = period/(round(period/ resol));

    /* number of pulses to be simulated */
    pulse_left = (int) ((simtime-RSTART) * freq);

    /* print a header */
    printf("# Frequency    : %.3e Hz \n", freq);
    printf("# Period       : %.3e sec\n", period);
    printf("#\n");
    printf("# Tick size    : %.3e sec\n", tick);
    printf("# Time for sim : %.3e sec\n", simtime);
    printf("#\n");
    printf("# Dose rate    : %.3e Gy/min\n", doser );
    printf("# Pulse size   : %.3e eV/l/pulse\n", dpulse );
    printf("# Pulse count  : %i pulses\n", pulse_left);
    printf("#\n");
    printf("# Total delivered dose for this simulation\n");
    printf("#              : %.6e Gy \n", pulse_left * dpulse / EVJ);
    printf("#\n");

    /* copy start conditions into working array */
    for (i = 0; i < NSPECIES; i++)
        y[i] = ystart[i];

    /* Print first line with starting conditions. */
    nbeprint(t, y, NSPECIES);

    /* loop over all pulses */
    while (pulse_left != 0) {

        /* check if we are at a pulse time step */
        if (t >= ((pulse_counter * period) + RSTART)) {
            if (flagp) /* print per pulse (pre pulse) */
                nbeprint(t, y, NSPECIES);

            printf("# Pulse! %i \n", pulse_left);
            for (j=0; j < NSPECIES; j++)
                y[j] += dpulse * gval[j] * 0.01 / NA;
            pulse_left--;
            pulse_counter++;
        }

        tick_counter++;
        ti = tick_counter * tick;

        status = gsl_odeiv2_driver_apply(d, &t, ti, y);

        if (status != GSL_SUCCESS) {
            printf("error, return value = %d\n", status);
            break;
        }

        if (flagc) /* print per tick */
            nbeprint(t, y, NSPECIES);

    } /* end pulses left iterator */

    if (flagl) /* print last */
        nbeprint(t, y, NSPECIES);

    gsl_odeiv2_driver_free(d);
    return 0;
}


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

#define N 65536 //the final number of nodes
#define I 2 // the initial number of nodes

#define MI 0 //the initial trial number
#define MF 33 //(the final trial number) - 1

#define RS 35382 //seed of the random number generator

int main()
{
	FILE *fp;

	double delta=sqrt(1.5);
	int tout[]={512,1024,2048,4096,8192,16384,32768,65536,131072};

	int i,j,k,l,m,t,tt;
	int tmp,flag;

	int **nb; //neighbors of each node

	int x; //new node
	int vdx; //virtual degree of the node x
	int *tmparr;

	int z;
	int nzsize;
	int *nz;

	int *deg; //degree of each node

	double degav; //average degree
	double *knn; //average degree of the nearest neighbors of each node
	double *cc; //local clustering coefficient of each node
	double ccav; //average local clustering coefficient

	char filename[100];

	const gsl_rng_type * TYPE;
	gsl_rng * ran;

	gsl_rng_env_setup();
	TYPE=gsl_rng_default;
	ran=gsl_rng_alloc(TYPE);
	gsl_rng_set(ran,RS);

	tmparr=malloc(sizeof(int)*N);
	nz=malloc(sizeof(int)*N);

	deg=malloc(sizeof(int)*N);
	knn=malloc(sizeof(double)*N);
	cc=malloc(sizeof(double)*N);

	nb=malloc(sizeof(int*)*N);
	for (i=0;i<N;i++){
		nb[i]=malloc(sizeof(int)*N);
	}

	for (m=MI;m<MF;m++){

		//generation of the initial network
		for (i=0;i<I;i++){
			deg[i]=0;
			for (j=0;j<I;j++){
				if (j!=i){
					nb[i][deg[i]]=j;
					deg[i]++;
				}
			}
		}

		tt=0;
		for (t=I;t<N;t++){
			//adding (t+1)-th node: copying degree step
			x=(int)(gsl_rng_uniform(ran)*t);
			vdx=1+(int)(gsl_rng_uniform(ran)*ceil(delta*deg[x]));
			deg[t]=0;

			/*//new OPA pre-adjunction rule
			j=0;
			for (i=0;i<t;i++){
				if (vdx<=ceil(delta*deg[i])){
					tmparr[j]=i;
					j++;
				}
			}
			z=tmparr[(int)(gsl_rng_uniform(ran)*j)];
			nzsize=deg[z]+1;
			for (i=0;i<nzsize-1;i++){
				nz[i]=nb[z][i];
			}
			nz[nzsize-1]=z;
			*/
			/*//OPA pre-adjunction rule
			nzsize=0;
			for (i=0;i<t;i++){
				if (vdx<=ceil(delta*deg[i])){
					nz[nzsize]=i;
					nzsize++;
				}
			}
			*/

			//OPA adjunction rule
			tmp=t;
			for (i=0;i<t;i++){
				if (vdx<=ceil(delta*deg[i]) && deg[i]<tmp)tmp=deg[i];
			}
			nzsize=0;
			for (i=0;i<t;i++){
				if (deg[i]==tmp){
					nz[nzsize]=i;
					nzsize++;
				}
			}

			//choosing the targets of the new node x
			if (vdx>=nzsize){
				for (i=0;i<nzsize;i++){
					nb[nz[i]][deg[nz[i]]]=t; deg[nz[i]]++;
					nb[t][deg[t]]=nz[i]; deg[t]++;
				}
			} else{
				for (j=0;j<vdx;j++){
					i=j+(int)(gsl_rng_uniform(ran)*(nzsize-j));

					nb[nz[i]][deg[nz[i]]]=t; deg[nz[i]]++;
					nb[t][deg[t]]=nz[i]; deg[t]++;

					tmp=nz[j];
					nz[j]=nz[i];
					nz[i]=tmp;
				}
			}

			//output
			if (t+1==tout[tt]){
				//degav
				degav=0.0;
				for (i=0;i<t+1;i++){
					degav+=deg[i];
				}
				degav/=(t+1);

				//knn
				for (i=0;i<t+1;i++){
					knn[i]=0.0;
					for (j=0;j<deg[i];j++){
						knn[i]+=deg[nb[i][j]];
					}
					if (deg[i]>0)knn[i]/=deg[i];
				}

				//cc
				ccav=0.0;
				for (i=0;i<t+1;i++){
					tmp=0;
					for (j=0;j<deg[i];j++){
						for (k=j+1;k<deg[i];k++){
							flag=0;
							for (l=0;l<deg[nb[i][j]];l++){
								if (nb[nb[i][j]][l]==nb[i][k]){
									flag=1;
									break;
								}
							}
							if (flag==1)tmp++;
						}
					}

					if (deg[i]>1)cc[i]=2.0*tmp/(deg[i]*(deg[i]-1));
					else cc[i]=0.0;

					ccav+=cc[i];
				}
				ccav/=(t+1);

				sprintf(filename,"id-deg-knn-cc_t%d_trial%d.txt",t+1,m);
				fp=fopen(filename,"w");
				for (i=0;i<t+1;i++){
					fprintf(fp,"%d %d %.15f %.15f\n",i,deg[i],knn[i],cc[i]);
				}
				fclose(fp);

				printf("m=%d t=%d degav=%f ccav=%f\n",m,t+1,degav,ccav);

				tt++;
			}//end of output

		}//end of t-loop

	}//end of m-loop

	free(tmparr);
	free(nz);
	free(deg);
	free(knn);
	free(cc);
	for (i=0;i<N;i++){
		free(nb[i]);
	}
	free(nb);

	return 0;
}


#include <math.h>

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

#include "qdm.h"

#define TAU_RESET_L_SIZE 11
#define TAU_RESET_M_SIZE  8
#define TAU_RESET_H_SIZE 11
#define TAU_RESET_SIZE (TAU_RESET_L_SIZE + TAU_RESET_M_SIZE + TAU_RESET_H_SIZE)

#define TAU_RESET_STEP 0.0001
#define TAU_RESET_GAP  0.001

#define TAU_TABLE_SIZE 1048576

#define TAU_INITIAL_GUESS 0.5
#define TAU_EPS 0.001
#define TAU_ITERATION_MAX 1000

static
void
qdm_tau_reset_setup(
    qdm_tau *t
)
{
  t->reset = gsl_vector_alloc(TAU_RESET_SIZE);

  size_t offset = 0;

  gsl_vector_view view;

  view = gsl_vector_subvector(t->reset, offset, TAU_RESET_L_SIZE);
  qdm_vector_set_seq(&view.vector, t->low, t->low + (TAU_RESET_L_SIZE - 1) * TAU_RESET_STEP);
  offset += TAU_RESET_L_SIZE;

  view = gsl_vector_subvector(t->reset, offset, TAU_RESET_M_SIZE);
  qdm_vector_set_seq(&view.vector, t->low + TAU_RESET_GAP, t->high - TAU_RESET_GAP);
  offset += TAU_RESET_M_SIZE;

  view = gsl_vector_subvector(t->reset, offset, TAU_RESET_H_SIZE);
  qdm_vector_set_seq(&view.vector, t->high - (TAU_RESET_H_SIZE - 1) * TAU_RESET_STEP, t->high);
  offset += TAU_RESET_H_SIZE;
}

static
void
qdm_tau_table_setup(
    qdm_tau *t
)
{
  if (!t->use_table) {
    t->ispline_table = NULL;
    t->mspline_table = NULL;

    return;
  }

  t->ispline_table = gsl_matrix_alloc(TAU_TABLE_SIZE, (t->knots->size - t->spline_df) + 1);
  t->mspline_table = gsl_matrix_alloc(TAU_TABLE_SIZE, (t->knots->size - t->spline_df) + 1);

  double tau = 0;
  gsl_vector_view row;
  for (size_t i = 0; i < TAU_TABLE_SIZE; i++) {
    tau = (double)(i) / TAU_TABLE_SIZE;

    row = gsl_matrix_row(t->ispline_table, i);
    qdm_ispline_vector(
        &row.vector,
        tau,
        t->spline_df,
        t->knots
    );

    row = gsl_matrix_row(t->mspline_table, i);
    qdm_mspline_vector(
        &row.vector,
        tau,
        t->spline_df,
        t->knots
    );
  }
}

qdm_tau *
qdm_tau_alloc(
    int use_table,

    double low,
    double high,

    size_t spline_df,
    const gsl_vector *knots
)
{
  qdm_tau *t = malloc(sizeof(qdm_tau));

  if (use_table == 1) {
    t->use_table = true;
  } else {
    t->use_table = false;
  }

  t->low = low;
  t->high = high;

  t->spline_df = spline_df;
  t->knots = qdm_vector_copy(knots);

  qdm_tau_reset_setup(t);
  qdm_tau_table_setup(t);

  return t;
}

void
qdm_tau_free(
    qdm_tau *t
)
{
  if (t == NULL) {
    return;
  }

  t->use_table = 0;

  t->low = 0;
  t->high = 0;

  t->spline_df = 0;
  t->knots = NULL;

  gsl_vector_free(t->reset);
  t->reset = NULL;

  gsl_matrix_free(t->ispline_table);
  t->ispline_table = NULL;

  gsl_matrix_free(t->mspline_table);
  t->mspline_table = NULL;

  free(t);
}

double
qdm_tau_ispline_mmm(
    const qdm_tau *t,
    const double value,
    const gsl_vector *mmm
)
{
  double result = 0;

  if (t->use_table) {
    size_t i = (size_t)fmin(floor(value * TAU_TABLE_SIZE), TAU_TABLE_SIZE - 1);
    gsl_vector_view ispline = gsl_matrix_row(t->ispline_table, i);

    gsl_blas_ddot(&ispline.vector, mmm, &result);
  } else {
    size_t m = (t->knots->size - t->spline_df) + 1;
    double ispline_data[m];
    gsl_vector_view ispline = gsl_vector_view_array(ispline_data, m);

    qdm_ispline_vector(&ispline.vector, value, t->spline_df, t->knots);
    gsl_blas_ddot(&ispline.vector, mmm, &result);
  }

  return result;
}

double
qdm_tau_mspline_mmm(
    const qdm_tau *t,
    const double value,
    const gsl_vector *mmm
)
{
  double result = 0;

  if (t->use_table) {
    size_t i = (size_t)fmin(floor(value * TAU_TABLE_SIZE), TAU_TABLE_SIZE - 1);
    gsl_vector_view mspline = gsl_matrix_row(t->mspline_table, i);

    gsl_blas_ddot(&mspline.vector, mmm, &result);
  } else {
    size_t m = (t->knots->size - t->spline_df) + 1;
    double mspline_data[m];
    gsl_vector_view mspline = gsl_vector_view_array(mspline_data, m);

    qdm_mspline_vector(&mspline.vector, value, t->spline_df, t->knots);
    gsl_blas_ddot(&mspline.vector, mmm, &result);
  }

  return result;
}

double
qdm_tau_find(
    const qdm_tau *t,
    const double v,
    const gsl_vector *mmm
)
{
  double tau = TAU_INITIAL_GUESS;
  double qi_u = 0;
  double qm_u = 0;

  size_t reset_i = 0;
  for (size_t i = 0; i < TAU_ITERATION_MAX; i++) {
    if (reset_i >= t->reset->size) {
      tau = TAU_INITIAL_GUESS;

      break;
    }

    /* Calculate position... */
    qi_u = qdm_tau_ispline_mmm(t, tau, mmm);

    /* Calculate slope... */
    qm_u = qdm_tau_mspline_mmm(t, tau, mmm);

    /* Check if the update is within our desired interval [0, 1]. */
    double update = tau - (qi_u - v) / qm_u;
    if (update < 0 || update > 1) {
      tau = gsl_vector_get(t->reset, reset_i);
      reset_i++;

      continue;
    } else {
      tau = update;
    }

    if (fabs(qi_u - v) <= TAU_EPS) {
      break;
    }
  }

  return tau;
}


/*
  NAME:
     minmax
  PURPOSE:
     returns the (finite) minimum and the maximum of a vector which is the row/column of a matrix
  CALLING SEQUENCE:
     minmax(gsl_matrix * q, int row, bool isrow, double * min, double * max, bool partial, int partial_indx[3])
  INPUT:
     q            - matrix which holds the vector
     row          - row/column which holds the vector
     isrow        - is it the row or is it the column
  OUTPUT:
     min   - finite minimum
     max   - finite maximum
  REVISION HISTORY:
     2008-09-21 - Written Bovy
*/
#include <gsl/gsl_matrix.h>
#include <float.h>
#include <stdbool.h>
#include <proj_gauss_mixtures.h>

void minmax(gsl_matrix * q, int row, bool isrow, double * min,
	    double * max){
  *max = -DBL_MAX;
  *min = DBL_MAX;
  int dd;
  double temp;
  if (isrow) {
    for (dd = 0; dd != q->size2; ++dd){
	temp  = gsl_matrix_get(q,row,dd);
	if (temp > *max && bovy_isfin(temp))
	  *max = temp;
	if (temp < *min && bovy_isfin(temp))
	  *min = temp;
    }
  }
  else {
    for (dd = 0; dd != q->size1; ++dd){
	temp  = gsl_matrix_get(q,dd,row);
	if (temp > *max && bovy_isfin(temp))
	  *max = temp;
	if (temp < *min && bovy_isfin(temp))
	  *min = temp;
    }
  }

  return ;
}


#include <assert.h>
#include <stdio.h>
#include <math.h>
#include <string.h>
#include <float.h>
#include "nn.h"
#ifdef __APPLE__
#include <Accelerate/Accelerate.h>
#else
#include <cblas.h>
#endif

#define AVERAGE_FISHER 10

/**
 * A function for generating random numbers according to a N(mu, sigma) Gaussian
 * distribution.
 */
double randn (double mu, double sigma)
{
  double U1, U2, W, mult;
  static double X1, X2;
  static int call = 0;

  if (call == 1) {
      call = !call;
      return (mu + sigma * (double) X2);
  }
  do {
      U1 = -1 + ((double) rand () / RAND_MAX) * 2;
      U2 = -1 + ((double) rand () / RAND_MAX) * 2;
      W = pow (U1, 2) + pow (U2, 2);
  } while (W >= 1 || W == 0);

  mult = sqrt ((-2 * log (W)) / W);
  X1 = U1 * mult;
  X2 = U2 * mult;

  call = !call;

  return (mu + sigma * (double) X1);
}

/**
 * Shorthand to generate normally distributed random numbers.
 */
double rand_normal()
{
  return randn(0.0, 1.0);
}

/**
 * @brief Shuffle the index array.
 */
void shuffle_index(size_t num_pattern, size_t random_idx[num_pattern])
{
  size_t p, np, op;
  for (p = 0; p < num_pattern; ++p) {
    random_idx[p] = p;
  }
  for (p = 0; p < num_pattern; ++p) {
    np = p + ((double)rand()/((double)RAND_MAX+1)) * (num_pattern - 1 - p);
    op = random_idx[p];
    random_idx[p] = random_idx[np];
    random_idx[np] = op;
  }
}

/**
 * This function fills the input and target vector with the list of training
 * examples from automaton.
 */
void fill_input_target(size_t size, double* input, uint8_t* target,
                       uint8_t* automaton, int offset, int states)
{
  size_t index;
  int counter;
  int side = 2 * offset + 1;
  int num_input = states * (side * side - 1);
  uint8_t val;

  for (size_t i = 0; i < size; ++i) {
    for (size_t j = 0; j < size; ++j) {
      index = i * size + j;
      counter = 1;

      /* Add bias in the main vector */
      input[index * (num_input + 1)] = 1.0;

      for (int a = -offset; a < offset + 1; ++a) {
        for (int b = -offset; b < offset + 1; ++b) {
          if (a != 0 || b != 0) {  /* Don't take index i,j */
            val = automaton[((i + a + size) % size) * size
                            + ((j + b + size) % size)];
            for (uint8_t s = 0; s < states; ++s) {
              input[index * (num_input + 1) + counter] = (val == s) ? 1.: 0.;
              counter++;
            }
          }
        }
      }

      target[index] = automaton[i * size + j];
    }
  }
}

void init_weights(int num_hidden, int num_input, int num_output,
                  double* delta_w_ih, double* weight_ih,
                  double* delta_w_ho, double* weight_ho)
{
  int i, j, k;
  /* Initialize weights input -> hidden */
  for (j = 0; j < num_hidden; ++j) {
    delta_w_ih[j] = 0.0;
    weight_ih[j] = 0.0;
    for (i = 1; i < num_input + 1; ++i) {
      delta_w_ih[i * num_hidden + j] = 0.0;
      weight_ih[i * num_hidden + j] = rand_normal() *
        sqrt(1 / (double)(num_input));
    }
  }

  /* Initialize weights hidden -> output */
  for (k = 0; k < num_output; ++k) {
    delta_w_ho[k] = 0.0;
    weight_ho[k] = 0.0;
    for (j = 1; j < num_hidden + 1; ++j) {
      delta_w_ho[j * num_output + k] = 0.0;
      weight_ho[j * num_output + k] = rand_normal() *
        sqrt(1 / (double)(num_hidden));
    }
  }
}

void forward(double* input, double* output,
             int num_hidden, int num_pattern,
             int num_input, int num_output,
             double* hidden,
             double* hidden_bias,
             double* weight_ih,
             double* weight_ho)
{
  int j, k, p;
  double max_out, agg_out = 0.0;

  /* Compute hidden activations */
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
              num_pattern, num_hidden, num_input + 1, 1.0,
              input, num_input + 1, weight_ih, num_hidden,
              0.0, hidden, num_hidden);

  /* ReLU non-linearity */
  for (p = 0; p < num_pattern; ++p) {
    hidden_bias[p * (num_hidden + 1)] = 1.0;
    for (j = 1; j < num_hidden + 1; ++j) {
      hidden_bias[p * (num_hidden + 1) + j] =
        (hidden[p * num_hidden + j - 1] > 0.0) ?
        hidden[p * num_hidden + j - 1]: 0.0;
    }
  }

  /* Compute output unit activations */
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans,
              num_pattern, num_output, num_hidden + 1, 1.0,
              hidden_bias, num_hidden + 1, weight_ho, num_output,
              0.0, output, num_output);

  /* Compute softmax of output */
  for (p = 0; p < num_pattern; ++p) {
    max_out = output[p * num_output];
    for (k = 1; k < num_output; ++k) {
      if (output[p * num_output + k] > max_out) {
        max_out = output[p * num_output + k];
      }
    }
    agg_out = 0.0;
    for (k = 0; k < num_output; ++k) {
      output[p * num_output + k] = exp(output[p * num_output + k] - max_out);
      agg_out += output[p * num_output + k];
    }
    for (k = 0; k < num_output; ++k) {
      output[p * num_output + k] /= agg_out;
    }
  }
}

void update_weights(int num_input, int num_hidden, int num_output,
                    double eta,
                    double* batch_error, double reg, double alpha,
                    double* weight_ih, double* delta_w_ih,
                    double* delta_w_ih_prev,
                    double* weight_ho, double* delta_w_ho,
                    double* delta_w_ho_prev)
{
  int i, j, k;
  /* Update weights with average gradient */
  for (i = 0; i < num_input + 1; ++i) {
    for (j = 0; j < num_hidden; ++j) {
      if (reg > 0.) {
        *batch_error += 0.5 * reg *
          weight_ih[i * num_hidden + j] * weight_ih[i * num_hidden + j] ;
        weight_ih[i * num_hidden + j] += (1 + alpha) *
          reg * weight_ih[i * num_hidden + j];
      }

      weight_ih[i * num_hidden + j] -= (1 + alpha) *
        (eta * delta_w_ih[i * num_hidden + j]);

      if (alpha > 0.) {
        weight_ih[i * num_hidden + j] += (-alpha) *
          delta_w_ih_prev[i * num_hidden + j];
      }
    }
  }
  for (j = 0; j < num_hidden + 1; ++j) {
    for (k = 0; k < num_output; ++k) {
      if (reg > 0.) {
        *batch_error += 0.5 * reg *
          weight_ho[j * num_output + k] * weight_ho[j * num_output + k];
        weight_ho[j * num_output + k] += (1 + alpha) *
        reg * weight_ho[j * num_output + k];
      }

      weight_ho[j * num_output + k] -= (1 + alpha) *
        (eta * delta_w_ho[j * num_output + k]);

      if (alpha > 0.) {
        weight_ho[j * num_output + k] += (-alpha) *
          delta_w_ho_prev[j * num_output + k];
      }
    }
  }
}

void compute_batch_gradients(int base_index, double alpha,
                             int batch_size, int num_input, int num_output,
                             int num_hidden, size_t* random_idx,
                             double* output,
                             uint8_t* target, double* delta_output,
                             double* delta_w_ho, double* hidden_bias,
                             double* weight_ho, double* delta_h,
                             double* delta_w_ih, double* input,
                             double* delta_w_ih_prev,
                             double* delta_w_ho_prev,
                             network_opts_t* opts)
{
  int i, j, k, p;

  /* Gradient initialization (Nesterov momentum) */
  if (opts->optim_type == NESTEROV) {
    memcpy(delta_w_ih_prev, delta_w_ih,
           sizeof(double) * num_hidden * (num_input + 1));
    for (i = 0; i < num_input + 1; ++i) {
      for (j = 0; j < num_hidden; ++j) {
        delta_w_ih[i * num_hidden + j] *= alpha;
      }
    }

    memcpy(delta_w_ho_prev, delta_w_ho,
           sizeof(double) * num_output * (num_hidden + 1));
    for (j = 0; j < num_hidden + 1; ++j) {
      for (k = 0; k < num_output; ++k) {
        delta_w_ho[j * num_output + k] *= alpha;
      }
    }
  }

  for (int b = 0; b < batch_size; ++b) {
    p = random_idx[(base_index + b)];

    /* Backpropagation */
    for (k = 0; k < num_output; ++k) {
      /* Output gradients */
      delta_output[b * num_output + k] =
        (output[b * num_output + k] - ((k == target[p])? 1.0: 0.0));
      delta_output[b * num_output + k] /= (double) batch_size;
    }
  }

  /* Dot product hidden_bias x delta_output stored in delta_w_ho */
  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
              num_hidden + 1, num_output, batch_size,
              1 , hidden_bias, num_hidden + 1,
              delta_output, num_output,
              (opts->optim_type == NESTEROV)? 1.0: 0.0,
              delta_w_ho, num_output);

  /* Dot product weight_ho x delta_output (we don't count the bias in
     weight_ho) stored in delta_h */
  cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans,
              batch_size, num_hidden, num_output,
              1.0, delta_output, num_output,
              &weight_ho[num_output], num_output,
              0.0, delta_h, num_hidden);

  /* Hidden layer non linearity gradients */
  for (int b = 0; b < batch_size; ++b) {
    for (j = 0; j < num_hidden; ++j) {
      delta_h[b * num_hidden + j] *=
        ((hidden_bias[b * (num_hidden + 1) + j + 1] > 0) ? 1.0: 0.0);
    }
  }

  cblas_dgemm(CblasRowMajor, CblasTrans, CblasNoTrans,
              num_input + 1, num_hidden, batch_size,
              1 , input, num_input + 1,
              delta_h, num_hidden,
              (opts->optim_type == NESTEROV)? 1.0: 0.0,
              delta_w_ih, num_hidden);

}

double compute_loss(int base_index, int batch_size,
                    size_t* random_idx,
                    int num_output, double* output,
                    uint8_t* target)
{
  double batch_error = 0.0;
  int p;
  for (int b = 0; b < batch_size; ++b) {
    p = random_idx[(base_index + b)];
    /* Compute loss */
    batch_error +=
      - log((output[b * num_output + target[p]] > 0) ?
            output[b * num_output + target[p]]: DBL_MIN);
  }
  batch_error /= (double) batch_size;
  return batch_error;
}

double compute_fisher(int states, int neighbors,
                      int num_pattern, int num_output,
                      int num_hidden, int num_input,
                      double* input,
                      double* weight_ih, double* weight_ho)
{
  double fisher_information = 0.0;
  int base, index, delta;

  /* Build the secondary input that will be perturbed */
  double* perturbed_input =
    (double*) malloc(sizeof(double) * num_pattern * (num_input + 1));

  /* Allocate placeholders in the function to make it more
     adaptable to the input dataset. */
  double* output =
    (double*) malloc(sizeof(double) * num_pattern * num_output);
  double* output_pert =
    (double*) malloc(sizeof(double) * num_pattern * num_output);
  double* hidden =
    (double*) malloc(sizeof(double) * num_pattern * num_hidden);
  double* hidden_bias =
    (double*) malloc(sizeof(double) * num_pattern * (num_hidden + 1));

  /* Compute the output of the network on the base dataset */
  forward(input, output, num_hidden, num_pattern, num_input, num_output,
          hidden, hidden_bias, weight_ih, weight_ho);

  for (int n = 0; n < AVERAGE_FISHER; ++n) {
    for (int neighbor_n = 0; neighbor_n < neighbors; ++neighbor_n) {

      /* Perturb second input */
      memcpy(perturbed_input, input,
            sizeof(double) * num_pattern * (num_input + 1));
      for (int i = 0; i < num_pattern; ++i) {
        /* Choose index of the input cell to perturb */
        index = i * (num_input + 1) + neighbor_n * states;

        for (int t = 0; t < states; ++t) {
          base = 1 + (rand() % (states - 1));
          perturbed_input[index + ((base + t) % states)] =
            input[index + t];
        }
      }
      /* Compute the output of the network on the perturbed dataset */
      forward(perturbed_input, output_pert, num_hidden, num_pattern,
              num_input, num_output, hidden, hidden_bias, weight_ih, weight_ho);

      for (int i = 0; i < num_pattern; ++i) {
        for (int j = 0; j < num_output; ++j) {
          if (output_pert[i * num_output + j] > 0
              && output[i * num_output + j] > 0) {
            delta = log(output[i * num_output + j]
                        / output_pert[i * num_output + j]);
            fisher_information += output[i * num_output + j]
              * delta * delta;
          }
        }
      }

    }
  }

  free(output);
  free(hidden);
  free(hidden_bias);
  free(perturbed_input);
  free(output_pert);

  return fisher_information / (AVERAGE_FISHER * num_pattern);
}

double compute_error(int num_pattern, int num_output,
                     int num_hidden, int num_input,
                     double* input, uint8_t* target,
                     double* weight_ih, double* weight_ho)
{
  double test_error = 0.0;
  double val;

  /* Allocate placeholders in the function to make it more
     adaptable to the input dataset. */
  double* output =
    (double*) malloc(sizeof(double) * num_pattern * num_output);
  double* hidden =
    (double*) malloc(sizeof(double) * num_pattern * num_hidden);
  double* hidden_bias =
    (double*) malloc(sizeof(double) * num_pattern * (num_hidden + 1));

  /* Compute the output of the network */
  forward(input, output, num_hidden, num_pattern, num_input, num_output,
          hidden, hidden_bias, weight_ih, weight_ho);

  /* Compute loss */
  for (int p = 0; p < num_pattern; ++p) {
    val = output[p * num_output + target[p]];
    test_error += - log((val > 0) ? val: DBL_MIN);
  }
  test_error /= num_pattern;

  free(output);
  free(hidden);
  free(hidden_bias);

  return test_error;
}

void train_nn_on_automaton(size_t size, int states,
                           uint8_t* train_automaton,
                           uint8_t** test_automata,
                           int n_tests,
                           network_opts_t* opts,
                           network_result_t* res)
{
  if (opts->verbosity >= 1) {
    fprintf(stdout, "\nProcessing with options: h%i r%i e%i\n",
            opts->num_hid, opts->offset, opts->max_epoch);
  }

  size_t num_pattern = size * size;
  int side = 2 * opts->offset + 1;
  int num_input = states * (side * side - 1);
  int num_hidden = opts->num_hid;
  int num_output = states;

  double batch_error, error, eta = 1, alpha = 0.9;
  if (opts->optim_type != NESTEROV) {
    alpha = 0.;
  }

  const int batch_size = 8;
  /* Regression parameter */
  double reg = 0.;

  /* ====== Network and training variables declaration ====== */

  /* num_pattern x (num_input + 1) array that holds all the training set */
  double* base_input =
    (double *) malloc(num_pattern * (num_input + 1) * sizeof(double));
  /* Array that holds the training labels */
  uint8_t* target = (uint8_t *) malloc(num_pattern * sizeof(uint8_t));
  /* Fill those arrays with the automaton's content */
  fill_input_target(size, base_input, target,
                    train_automaton, opts->offset, states);

  /* Arrays for the test data */
  double* test_input =
    (double *) malloc(num_pattern * (num_input + 1) * sizeof(double));
  uint8_t* test_target = (uint8_t *) malloc(num_pattern * sizeof(uint8_t));

  /* Weights of the network */
  double weight_ih[(num_input + 1) * num_hidden];
  double weight_ho[(num_hidden + 1) * num_output];

  /* Gradients of the output and hidden layer placeholders */
  double delta_output[batch_size * num_output];
  double delta_h[batch_size * num_hidden];

  /* Weight gradients and previous gradients for Nesterov momentum */
  double delta_w_ih[(num_input + 1) * num_hidden];
  double delta_w_ho[(num_input + 1) * num_hidden];
  double* delta_w_ih_prev = NULL;
  double* delta_w_ho_prev = NULL;

  /* Allocate only if being used later */
  if (opts->optim_type == NESTEROV) {
    delta_w_ih_prev = malloc(sizeof(double) * (num_input + 1) * num_hidden);
    delta_w_ho_prev =  malloc(sizeof(double) * (num_hidden + 1) * num_output);
  }

  /* Allocate the arrays that will hold data for each batch */
  double input[(batch_size) * (num_input + 1)];
  double hidden[batch_size * num_hidden];
  double hidden_bias[batch_size * (num_hidden + 1)];
  double output[batch_size * num_output];

  int epoch;
  size_t random_idx[num_pattern];
  double test_error = 0.0;
  double test_var = 0.0;

  /* ===================== End declarations ====================== */

  /* Initialize the weights of the network */
  init_weights(num_hidden, num_input, num_output,
               delta_w_ih, weight_ih,
               delta_w_ho, weight_ho);

  for (epoch = 0; epoch < opts->max_epoch; ++epoch) {
    /* Initialize error */
    error = 0.0;

    /* Learning rate decay */
    if (epoch > 0 && epoch%10 == 0 && opts->decay == DECAY) {
      eta -= .5 * eta;
    }

    /* Random ordering of input patterns done at each epoch */
    shuffle_index(num_pattern, random_idx);

    /* Loop through every batch in the dataset */
    for (size_t s = 0; s < num_pattern; s += batch_size) {
      for (int b = 0; b < batch_size; ++b) {
        if (s + b >= num_pattern) {
          break;
        }
        /* Copy batch elements to the input array for processing */
        memcpy(input + b * (num_input + 1),
               base_input + (random_idx[(s + b)] *
                             (num_input + 1)),
               sizeof(double) * (num_input + 1));
      }

      /* Forward pass */
      forward(input, output, num_hidden, batch_size, num_input, num_output,
              hidden, hidden_bias, weight_ih, weight_ho);

      batch_error = compute_loss(s, batch_size, random_idx, num_output,
                                 output, target);

      /* Compute the gradients for each weight matrix */
      compute_batch_gradients(s, alpha, batch_size, num_input, num_output,
                              num_hidden,
                              random_idx, output, target,
                              delta_output, delta_w_ho, hidden_bias, weight_ho,
                              delta_h, delta_w_ih, input,
                              delta_w_ih_prev, delta_w_ho_prev, opts);

      /* Update the weights of the network */
      update_weights(num_input, num_hidden, num_output, eta, &batch_error, reg,
                     alpha, weight_ih, delta_w_ih, delta_w_ih_prev, weight_ho,
                     delta_w_ho, delta_w_ho_prev);

      /* Error is only updated here because it might have been changed by
         `update_weights` */
      error += batch_error * batch_size;
    }
    error /= (double)(num_pattern);

    if (opts-> verbosity >= 1 && epoch%5 == 0) {
      fprintf(stdout, "\nEpoch %d: Error = %f", epoch, error);
    }
  }

  /* Compute error on the training set */
  error = compute_error(num_pattern, num_output, num_hidden,
                        num_input, base_input, target,
                        weight_ih, weight_ho);

  if (opts->verbosity >= 1) {
    fprintf(stdout, "\nTrain error: %f", error);
  }

  /* Compute Fisher information if the flag requires it */
  if (opts->fisher == FISHER) {
    res->fisher_info = compute_fisher(states, (side*side - 1), num_pattern,
                                      num_output, num_hidden, num_input,
                                      base_input, weight_ih, weight_ho);
  }

  /* Was an array of states to test on provided ? */
  if (test_automata != NULL) {
    double test_errors[n_tests];
    for (int i = 0; i < n_tests; ++i) {
      /* Fill the placeholders with test data */
      fill_input_target(size, test_input, test_target,
                        test_automata[i], opts->offset, states);

      /* Compute error on the test set */
      test_errors[i] = compute_error(num_pattern, num_output, num_hidden,
                                     num_input, test_input, test_target,
                                     weight_ih, weight_ho);
      test_error += error / test_errors[i];
    }

    /* Average score across all tested states */
    test_error /= n_tests;

    /* Compute variance of scores */
    for (int i = 0; i < n_tests; ++i) {
      test_var += (error / test_errors[i] - test_error)
        * (error / test_errors[i] - test_error);
    }
    test_var /= n_tests;
    test_var = sqrt(test_var);

    if (opts->verbosity >= 1) {
      fprintf(stdout, "\tTest error: %f\tVar: %f\tRatio: %f",
              error / test_error, test_var, test_error);
    }
  }

  /* Log results */
  if (opts->verbosity >= 1 && opts->fisher== FISHER) {
      fprintf(stdout, "\tFisher: %f\n", res->fisher_info);
  } else if (opts->verbosity >= 1){
      fprintf(stdout, "\n");
  }

  /* Output data */
  res->train_error = error;
  res->test_error = error / test_error;
  res->error_var = test_var;

  /* Cleanup allocated arrays */
  free(target);
  free(base_input);
  free(test_input);
  free(test_target);
  free(delta_w_ih_prev);
  free(delta_w_ho_prev);
}


/// @file mlr.h
/// @brief multiple linear regression support
/// @author Jeff Perry <jeffsp@gmail.com>
/// @version 1.0
/// @date 2013-06-13

#ifndef MLR_H
#define MLR_H

#include "raster_utils.h"
//#include <lapacke.h>

namespace horny_toad
{

    /// @brief helper function
    template<typename T>
    T matrix_multiply (const T &a, const T &b)
    {
        assert (a.cols () == b.rows ());
        const size_t N = a.rows ();
        const size_t M = a.cols ();
        const size_t P = b.cols ();
        T y (a.rows (), b.cols ());
        for (size_t i = 0; i < N; ++i)
            for (size_t j = 0; j < P; ++j)
                for (size_t k = 0; k < M; ++k)
                    y (i, j) += a (i, k) * b (k, j);
        return y;
    }

    /// @brief multiple linear regression
    ///
    /// @tparam T matrix types
    /// @param y responses
    /// @param x predictors
    ///
    /// @return linear estimates of y=b*x
    template<typename T>
    T mlr_inverse (const T &y, const T &x)
    {
        // add a column of 1's to x on the left
        T xx (x.rows (), x.cols () + 1);
        for (size_t i = 0; i < x.rows (); ++i)
        {
            xx (i, 0) = 1;
            for (size_t j = 0; j < x.cols (); ++j)
            {
                xx (i, j + 1) = x (i, j);
            }
        }
        // b = (x^T * x)^-1 * x^T * y
        T xt = transpose (xx);
        T tmp = matrix_multiply (xt, xx);
        tmp = invert (tmp);
        tmp = matrix_multiply (tmp, xt);
        tmp = matrix_multiply (tmp, y);
        return tmp;
    }

    /// @brief multiple linear regression
    ///
    /// @tparam T matrix types
    /// @param y responses
    /// @param x predictors
    ///
    /// @return linear estimates of y=b*x
    /*
    template<typename T>
    T mlr_lapack (const T &y, const T &x)
    {
        assert (x.cols () <= x.rows ());
        assert (y.rows () == x.rows ());
        // result gets stored in b
        T b (y);
        // add a column of 1's to x on the left
        T xx (x.rows (), x.cols () + 1);
        for (size_t i = 0; i < x.rows (); ++i)
        {
            xx (i, 0) = 1;
            for (size_t j = 0; j < x.cols (); ++j)
            {
                xx (i, j + 1) = x (i, j);
            }
        }
        const size_t M = xx.rows ();
        const size_t N = xx.cols ();
        // solve it
        if (LAPACKE_dgels (LAPACK_ROW_MAJOR, 'N', M, N, 1, &xx[0], N, &b[0], 1) != 0)
            throw std::runtime_error ("LAPACK error: can't solve mlr");
        // copy result into matrix with correct dimensions
        T z (xx.cols (), 1);
        for (size_t i = 0; i < z.rows (); ++i)
            z[i] = b[i];
        return z;
    }
    */

    /// @brief multiple linear regression
    ///
    /// @tparam T matrix types
    /// @param y responses
    /// @param x predictors
    ///
    /// @return linear estimates of y=b*x
    template<typename T>
    T mlr (const T &y, const T &x)
    {
        return mlr_inverse (y, x);
        //return mlr_lapack (y, x);
    }
} // namespace horny_toad

#endif // MLR_H


#include <gsl/gsl_complex.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_fft_real.h>
#include <gsl/gsl_fft_halfcomplex.h>
#include <gsl/gsl_fft_complex.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_statistics_float.h>

#include <stdio.h>

int vector_float_convolve(int cs, const float* c, int as, const float* a, int rs, float* r)
{
  int h = cs / 2;
  int li, ri;
  int i,j;

  for (i = 0; i < cs; i++) {
    li = i - h;
    ri = i + h;
    r[i] = 0;
    for (j = (li >= 0 ? li : 0); j < (ri < as ? ri : (as - 1)); j++) {
      r[i] += a[j]*c[j+h+1];
    }
  }
  return 0;
}

int vector_double_convolve(int cs, const double* c, int as, const double* a, int rs, double* r)
{
  int h = cs / 2;
  int li, ri;
  int i,j;

  for (i = 0; i < cs; i++) {
    li = i - h;
    ri = i + h;
    r[i] = 0;
    for (j = (li >= 0 ? li : 0); j < (ri < as ? ri : (as - 1)); j++) {
      r[i] += a[j]*c[j+h+1];
    }
  }
  return 0;
}

int vector_complex_convolve(int cs, const gsl_complex* c, int as, const gsl_complex* a, int rs, gsl_complex* r)
{
  int h = cs / 2;
  int li, ri;
  int i,j;

  for (i = 0; i < cs; i++) {
    li = i - h;
    ri = i + h;
    r[i].dat[0] = 0;
    r[i].dat[1] = 0;
    for (j = (li >= 0 ? li : 0); j < (ri < as ? ri : (as - 1)); j++) {
      r[i].dat[0] += a[j].dat[0]*c[j+h+1].dat[0]-a[j].dat[1]*c[j+h+1].dat[1];
      r[i].dat[1] += a[j].dat[0]*c[j+h+1].dat[1]+a[j].dat[1]*c[j+h+1].dat[0];
    }
  }
  return 0;
}

int filter_double(int ls, const double* l, int ks, const double* k, int vs, const double* v, int rs, double* r)
{
  if (ls > vs || ks > vs) return 2000; // BAD_SIZE

  int i,j;

  double L = l[0];
  double K = k[0];

  int N = ls - 1;
  int M = ks - 1;

  for (i = 0; i < vs; i++) {
    r[i] = 0;
    for (j = 0; j < N; j++) {
      if (i - j > 0) r[i] -= (l[j+1])*v[i-j];
    }
    for (j = 0; j < M; j++) {
      if (i - j > 0) r[i] += (k[j+1])*r[i-j];
    }
  }
  return 0;
}

int filter_float(int ls, const float* l, int ks, const float* k, int vs, const float* v, int rs, float* r)
{
  if (ls > vs || ks > vs) return 2000; // BAD_SIZE

  int i,j;

  float L = l[0];
  float K = k[0];

  int N = ls - 1;
  int M = ks - 1;

  for (i = 0; i < vs; i++) {
    r[i] = 0;
    for (j = 0; j < N; j++) {
      if (i - j > 0) r[i] -= (l[j+1])*v[i-j];
    }
    for (j = 0; j < M; j++) {
      if (i - j > 0) r[i] += (k[j+1])*r[i-j];
    }
  }
  return 0;
}

int hilbert(int rs, gsl_complex* r)
{
  int s = rs;

  gsl_fft_complex_wavetable * wavetable = gsl_fft_complex_wavetable_alloc (s);
  gsl_fft_complex_workspace * workspace = gsl_fft_complex_workspace_alloc (s);

  // forward fourier transform
  gsl_fft_complex_forward ((double*)r, 1, s, wavetable, workspace);
  // zero negative coefficients and double positive

  int i;
  int m = s/2;
  for (i = 1; i < s; i++) {
    if (i <= m) {
      r[i].dat[0] *= 2;
      r[i].dat[1] *= 2;
    }
    else if (s % 2 == 0 && i == m+1) {
    }
    else {
      r[i].dat[0] = 0;
      r[i].dat[1] = 0;
    }
  }

  // inverse fourier transform
  gsl_fft_complex_inverse ((double*)r, 1, s, wavetable, workspace);

  gsl_fft_complex_wavetable_free (wavetable);
  gsl_fft_complex_workspace_free (workspace);

  return 0;
}

int pwelch(int w, int vs, const gsl_complex* v, int rs, double* r)
{
  if (w > vs) return 2000; // BAD_SIZE

  int i,j;

  int fs = w;

  int num_windows = vs / fs; // ignore end

  double s[fs];
  for (i = 0; i < fs; i++) s[i] = 0;

  gsl_fft_complex_wavetable * wavetable = gsl_fft_complex_wavetable_alloc (fs);
  gsl_fft_complex_workspace * workspace = gsl_fft_complex_workspace_alloc (fs);

  gsl_complex* f = malloc(sizeof(gsl_complex)*fs);
  gsl_vector_view F = gsl_vector_view_array((double*)f, 2*fs);

  gsl_vector_view X;

  for (i = 0; i < num_windows; i++) {
    X = gsl_vector_view_array((double*)(&v[i*fs]), 2*fs); // v is gsl_complex*
    gsl_blas_dcopy(&X.vector,&F.vector);
    gsl_fft_complex_forward ((double*)f, 1, fs, wavetable, workspace);
    for (j = 0; j < fs; j++) s[j] += f[j].dat[0]*f[j].dat[0] + f[j].dat[1]*f[j].dat[1];
  }
  for (j = 0; j < rs; j++) {
    if (j == 0) r[j] = s[j];
    else if (j == (rs-1)) r[j] = s[j];
    else r[j] = s[j] + s[fs-j+1];

    r[j] /= num_windows;
    r[j] = sqrt(r[j]);
  }
  gsl_fft_complex_wavetable_free (wavetable);
  gsl_fft_complex_workspace_free (workspace);

  free(f);

  return 0;
}

int hamming_double(int rs, double* r)
{
  int i;

  for (i = 0; i < rs; i++) r[i] = 0.54 - 0.46 * cos(2*M_PI*i/rs);

  return 0;
}

int hamming_float(int rs, float* r)
{
  int i;

  for (i = 0; i < rs; i++) r[i] = 0.54 - 0.46 * cos(2*M_PI*i/rs);

  return 0;
}

int real_poly_complex_eval(int cs, const double* c, int zs, const gsl_complex* z, int rs, gsl_complex* r)
{
  int i;

  for (i = 0; i < zs; i++)
    r[i] = gsl_poly_complex_eval(c,cs,z[i]);

  return 0;
}

int complex_power_double(int cs, const gsl_complex* c, int rs, double* r)
{
  if (rs != cs) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < cs; i++)
    r[i] = c[i].dat[0]*c[i].dat[0] + c[i].dat[1]*c[i].dat[1];

  return 0;
}

int complex_power_float(int cs, const gsl_complex* c, int rs, float* r)
{
  if (rs != cs) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < cs; i++)
    r[i] = c[i].dat[0]*c[i].dat[0] + c[i].dat[1]*c[i].dat[1];

  return 0;
}

int downsample_double(int n, int xs, const double* x, int rs, double* r)
{
  if (rs != xs/n) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < rs; i++)
    r[i] = x[i*n];

  return 0;
}

int downsample_float(int n, int xs, const float* x, int rs, float* r)
{
  if (rs != xs/n) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < rs; i++)
    r[i] = x[i*n];

  return 0;
}

int vector_diff_double(int xs, const double* x, int rs, double* r)
{
  if (rs != xs - 1) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < rs; i++)
    r[i] = x[i+1] - x[i];

  return 0;
}

int vector_diff_float(int xs, const float* x, int rs, float* r)
{
  if (rs != xs - 1) return 2000; // BAD_SIZE

  int i;

  for (i = 0; i < rs; i++)
    r[i] = x[i+1] - x[i];

  return 0;
}

int unwrap_double(int xs, const double* x, int rs, double* r)
{
  if (rs != xs) return 2000; // BAD_SIZE

  int i;

  r[0] = x[0];

  double c = 0;

  int tmp;

  for (i = 1; i < rs; i++) {
    tmp = x[i-1] - x[i];
    if (tmp > M_PI) {
      r[i] = 2*M_PI;
    }
    else if (tmp < (-M_PI)) {
      r[i] = -2*M_PI;
    }
    else {
      r[i] = 0;
    }
    c += r[i];
    r[i] = c + x[i];
  }

  return 0;
}

int unwrap_float(int xs, const float* x, int rs, float* r)
{
  if (rs != xs) return 2000; // BAD_SIZE

  int i;

  r[0] = x[0];

  double c = 0;

  int tmp;

  for (i = 1; i < rs; i++) {
    tmp = x[i-1] - x[i];
    if (tmp > M_PI) {
      r[i] = 2*M_PI;
    }
    else if (tmp < (-M_PI)) {
      r[i] = -2*M_PI;
    }
    else {
      r[i] = 0;
    }
    c += r[i];
    r[i] = c + x[i];
  }

  return 0;
}

int cross_covariance_double(int max_lag,
			    double* sx,
			    double* sy,
			    int xs, const double* x,
			    int ys, const double* y,
			    int rs, double* r)
{
  if (xs != ys)        return 2000; // BAD_SIZE
  if (rs != 2*max_lag) return 2000; // BAD_SIZE

  double mx = gsl_stats_mean(x,1,xs);
  double my = gsl_stats_mean(y,1,ys);

  *sx = gsl_stats_sd(x,1,xs);
  *sy = gsl_stats_sd(y,1,ys);

  int delay;
  int i, j;

  double sxy;

  for (delay=-max_lag; delay < max_lag; delay++) {
    sxy = 0;
    for (i=0;i<xs;i++) {
      j = i+delay;
      if (j < 0 || j >= xs) sxy += (x[i]-mx)*(-my);
      else sxy += (x[i]-mx)*(y[j]-my);
      /* or should it be:
      if (j < 0 || j >= xs) continue;
      else sxy += (x[i]-mx)*(y[j]-my);
      */
    }
    r[delay+max_lag] = sxy/xs;
  }

  return 0;
}

int cross_covariance_float(int max_lag,
			    float* sx,
			    float* sy,
			    int xs, const float* x,
			    int ys, const float* y,
			    int rs, double* r)
{
  if (xs != ys)        return 2000; // BAD_SIZE
  if (rs != 2*max_lag) return 2000; // BAD_SIZE

  float mx = gsl_stats_float_mean(x,1,xs);
  float my = gsl_stats_float_mean(y,1,ys);

  *sx = gsl_stats_float_sd(x,1,xs);
  *sy = gsl_stats_float_sd(y,1,ys);

  int delay;
  int i, j;

  float sxy;

  for (delay=-max_lag; delay < max_lag; delay++) {
    sxy = 0;
    for (i=0;i<xs;i++) {
      j = i+delay;
      if (j < 0 || j >= xs) sxy += (x[i]-mx)*(-my);
      else sxy += (x[i]-mx)*(y[j]-my);
      /* or should it be:
      if (j < 0 || j >= xs) continue;
      else sxy += (x[i]-mx)*(y[j]-my);
      */
    }
    r[delay+max_lag] = sxy/xs;
  }

  return 0;
}

int cum_sum_double(int xs, const double* x, int rs, double* r)
{
  int i;

  r[0] = x[0];

  for (i=1;i<xs;i++) {
    r[i] = r[i-1]+x[i];
  }

  return 0;
}

int cum_sum_float(int xs, const float* x, int rs, float* r)
{
  int i;

  r[0] = x[0];

  for (i=1;i<xs;i++) {
    r[i] = r[i-1]+x[i];
  }

  return 0;
}


#include <stdio.h>
#include <math.h>
#include <gbpLib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_deriv.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_min.h>
#include <gbpInterpolate.h>

double interpolate_maximum_function(double x, void *params);
double interpolate_maximum_function(double x, void *params) {
    return (-interpolate((interp_info *)params, x));
}

void interpolate_maximum(interp_info *interp,
                         double       x_lo_in,
                         double       x_guess_in,
                         double       x_hi_in,
                         double       threshold,
                         double *     x_maxima,
                         double *     y_maxima) {
    double                         x_lo;
    double                         x_hi;
    double                         x_guess;
    double                         r_val;
    const gsl_min_fminimizer_type *T;
    gsl_min_fminimizer *           s;
    gsl_function                   F;
    int                            status;
    int                            iter = 0;
    int                            max_iter;

    x_lo       = x_lo_in;
    x_guess    = x_guess_in;
    x_hi       = x_hi_in;
    max_iter   = GBP_MAX(100, interp->n);
    F.function = interpolate_maximum_function;
    F.params   = (void *)interp;
    T          = gsl_min_fminimizer_brent;
    s          = gsl_min_fminimizer_alloc(T);
    gsl_min_fminimizer_set(s, &F, x_guess, x_lo, x_hi);

    do {
        iter++;
        status  = gsl_min_fminimizer_iterate(s);
        x_lo    = gsl_min_fminimizer_x_lower(s);
        x_guess = gsl_min_fminimizer_x_minimum(s);
        x_hi    = gsl_min_fminimizer_x_upper(s);
        status  = gsl_min_test_interval(x_lo, x_hi, 0., threshold);
    } while(status == GSL_CONTINUE && iter < max_iter);
    gsl_min_fminimizer_free(s);
    (*x_maxima) = x_guess;
    (*y_maxima) = interpolate(interp, x_guess);
}


#include <petsc.h>
#include "flow.h"
#include "mesh.h"

#define DIM 2                   /* Geometric dimension */
#define NFIELDS 3
static const FlowFieldDescriptor PhysicsFields[] = {{"Density", "Den",1},{"Momentum", "Momentum",DIM},{"Energy", "Energy",1},{NULL,0}};
typedef enum {EULER_PAR_GAMMA,EULER_PAR_RHOR,EULER_PAR_AMACH,EULER_PAR_ITANA,EULER_PAR_SIZE} EulerParamIdx;

typedef struct {
    PetscReal rho;
    PetscReal rhoU[DIM];
    PetscReal rhoE;
} EulerNode;
typedef union {
    EulerNode eulernode;
    PetscReal vals[DIM+2];
} EulerNodeUnion;

typedef struct {
    PetscReal gamma;
    PetscReal rhoL;
    PetscReal rhoR;
    PetscReal uL;
    PetscReal uR;
    PetscReal pL;
    PetscReal pR;
    PetscReal maxTime;
    PetscReal length;
} Setup;

typedef struct {
    PetscReal pstar,ustar,rhostarL,astarL,SL,SHL,STL,rhostarR,astarR,SR,SHR,STR,gamm1,gamp1;
} StarState;

typedef struct {
    StarState starState;
    Setup setup;
    PetscReal cfl;
} ProblemSetup;

typedef struct{
    PetscReal f;
    PetscReal fprm;
} PressureFunction;

static PressureFunction f_and_fprm_rarefaction(PetscReal pstar, PetscReal pLR, PetscReal aLR, PetscReal gam, PetscReal gamm1,PetscReal gamp1) {
    // compute value of pressure function for rarefaction
    PressureFunction function;
    function.f = ((2. * aLR) / gamm1) * (pow(pstar / pLR, 0.5 * gamm1 / gam) - 1.);
    function.fprm = (aLR / pLR / gam) * pow(pstar / pLR, -0.5 * gamp1 / gam);
    return function;
}

static PressureFunction f_and_fprm_shock(PetscReal pstar, PetscReal pLR, PetscReal rhoLR, PetscReal gam, PetscReal gamm1, PetscReal gamp1){
    // compute value of pressure function for shock
    PetscReal A = 2./gamp1/rhoLR;
    PetscReal B = gamm1*pLR/gamp1;
    PetscReal sqrtterm =  PetscSqrtReal(A/(pstar+B));
    PressureFunction function;
    function.f = (pstar-pLR)*sqrtterm;
    function.fprm = sqrtterm*(1.-0.5*(pstar-pLR)/(B+pstar));
    return function;
}

#define EPS 1.e-6
#define MAXIT 100
static PetscErrorCode DetermineStarState(const Setup* setup, StarState* starState){
    // compute the speed of sound
    PetscReal aL = PetscSqrtReal(setup->gamma*setup->pL/setup->rhoL);
    PetscReal aR = PetscSqrtReal(setup->gamma*setup->pR/setup->rhoR);

    //first guess pstar based on two-rarefacation approximation
    starState->pstar = aL+aR - 0.5*(setup->gamma-1.)*(setup->uR-setup->uL);
    starState->pstar  = starState->pstar  / (aL/pow(setup->pL,0.5*(setup->gamma-1.)/setup->gamma) + aR/pow(setup->pR,0.5*(setup->gamma-1.)/setup->gamma) );
    starState->pstar = pow(starState->pstar,2.*setup->gamma/(setup->gamma-1.));
    starState->gamm1 = setup->gamma-1.;
    starState->gamp1 = setup->gamma+1.;

    PressureFunction fL;
    if (starState->pstar <= setup->pL){
        fL = f_and_fprm_rarefaction(starState->pstar, setup->pL,aL,setup->gamma,starState->gamm1,starState->gamp1);
    }else{
        fL = f_and_fprm_shock(starState->pstar,setup->pL,setup->rhoL,setup->gamma,starState->gamm1,starState->gamp1);
    }

    PressureFunction fR;
    if (starState->pstar <= setup->pR) {
        fR = f_and_fprm_rarefaction(starState->pstar, setup->pR, aR, setup->gamma, starState->gamm1, starState->gamp1);
    }else {
        fR = f_and_fprm_shock(starState->pstar, setup->pR, setup->rhoR, setup->gamma, starState->gamm1, starState->gamp1);
    }
    PetscReal delu = setup->uR-setup->uL;

    // iterate using Newton-Rapson
    if ((fL.f+fR.f+delu)> EPS) {
        // iterate using Newton-Rapson
        for(PetscInt it =0; it < MAXIT+4; it++){
            PetscReal pold = starState->pstar;
            starState->pstar = pold - (fL.f+fR.f+delu)/(fL.fprm+fR.fprm);

            if(starState->pstar < 0){
                starState->pstar = EPS;
            }

            if(2.0*PetscAbsReal(starState->pstar - pold)/(starState->pstar + pold) < EPS){
                break;
            }else{
                if(starState->pstar < setup->pL){
                    fL = f_and_fprm_rarefaction(starState->pstar, setup->pL,aL,setup->gamma,starState->gamm1,starState->gamp1);
                }else{
                    fL = f_and_fprm_shock(starState->pstar,setup->pL,setup->rhoL,setup->gamma,starState->gamm1,starState->gamp1);
                }
                if (starState->pstar<=setup->pR) {
                    fR = f_and_fprm_rarefaction(starState->pstar, setup->pR, aR, setup->gamma, starState->gamm1, starState->gamp1);
                }else {
                    fR = f_and_fprm_shock(starState->pstar, setup->pR, setup->rhoR, setup->gamma, starState->gamm1, starState->gamp1);
                }
            }

            if (it>MAXIT){
                SETERRQ(PETSC_COMM_WORLD,1,"error in Riemann.find_pstar - did not converage for pstar" );
            }
        }
    }

    // determine rest of star state
    starState->ustar = 0.5*(setup->uL+setup->uR+fR.f-fL.f);

    // left star state
    PetscReal pratio = starState->pstar/setup->pL;
    if (starState->pstar<=setup->pL) {  // rarefaction
        starState->rhostarL = setup->rhoL * PetscPowReal(pratio, 1. / setup->gamma);
        starState->astarL = aL * PetscPowReal(pratio, 0.5 * starState->gamm1 / setup->gamma);
        starState->SHL = setup->uL - aL;
        starState->STL = starState->ustar - starState->astarL;
    }else {  // #shock
        starState->rhostarL = setup->rhoL * (pratio + starState->gamm1 / starState->gamp1) / (starState->gamm1 * pratio / starState->gamp1 + 1.);
        starState->SL = setup->uL - aL * PetscSqrtReal(0.5 * starState->gamp1 / setup->gamma * pratio + 0.5 * starState->gamm1 / setup->gamma);
    }

    // right star state
    pratio = starState->pstar/setup->pR;
    if (starState->pstar<=setup->pR) {  // # rarefaction
        starState->rhostarR = setup->rhoR * PetscPowReal(pratio, 1. / setup->gamma);
        starState->astarR = aR * PetscPowReal(pratio, 0.5 * starState->gamm1 / setup->gamma);
        starState-> SHR = setup->uR + aR;
        starState->STR = starState->ustar + starState->astarR;
    }else {  // shock
        starState->rhostarR = setup->rhoR * (pratio + starState->gamm1 / starState->gamp1) / (starState->gamm1 * pratio / starState->gamp1 + 1.);
        starState->SR = setup->uR + aR * PetscSqrtReal(0.5 * starState->gamp1 / setup->gamma * pratio + 0.5 * starState->gamm1 / setup->gamma);
    }
    return 0;
}

static void SetExactSolutionAtPoint(PetscInt dim, PetscReal xDt, const Setup* setup, const StarState* starState, EulerNode* uu){
    PetscReal p;
    // compute the speed of sound
    PetscReal aL = PetscSqrtReal(setup->gamma*setup->pL/setup->rhoL);
    PetscReal aR = PetscSqrtReal(setup->gamma*setup->pR/setup->rhoR);

    for(PetscInt i =0; i < dim; i++){
        uu->rhoU[i] = 0.0;
    }

    if (xDt <= starState->ustar) {  //# left of contact surface
        if (starState->pstar <= setup->pL) {  // # rarefaction
            if (xDt <= starState->SHL) {
                uu->rho = setup->rhoL;
                p = setup->pL;
                uu->rhoU[0] = setup->uL*uu->rho;
            }else if (xDt <=starState->STL) {  //#SHL < x / t < STL
                PetscReal tmp = 2. / starState->gamp1 + (starState->gamm1 / starState->gamp1 / aL) * (setup->uL - xDt);
                uu->rho = setup->rhoL * pow(tmp, 2. / starState->gamm1);
                uu->rhoU[0] = uu->rho * (2. / starState->gamp1) * (aL + 0.5 * starState->gamm1 * setup->uL + xDt);
                p = setup->pL * pow(tmp, 2. * setup->gamma / starState->gamm1);
            }else {  //# STL < x/t < u*
                uu->rho = starState->rhostarL;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }else{  //# shock
            if (xDt<= starState->SL) {  // # xDt < SL
                uu->rho = setup->rhoL;
                p = setup->pL;
                uu->rhoU[0] = uu->rho * setup->uL;
            }else {  //# SL < xDt < ustar
                uu->rho = starState->rhostarL;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }
    }else{//# right of contact surface
        if (starState->pstar<=setup->pR) {  //# rarefaction
            if (xDt>= starState->SHR) {
                uu->rho = setup->rhoR;
                p = setup->pR;
                uu->rhoU[0] = uu->rho * setup->uR;
            }else if (xDt >= starState->STR) {  // # SHR < x/t < SHR
                PetscReal tmp = 2./starState->gamp1 - (starState->gamm1/starState->gamp1/aR)*(setup->uR-xDt);
                uu->rho = setup->rhoR*PetscPowReal(tmp,2./starState->gamm1);
                uu->rhoU[0] = uu->rho * (2./starState->gamp1)*(-aR + 0.5*starState->gamm1*setup->uR+xDt);
                p = setup->pR*PetscPowReal(tmp,2.*setup->gamma/starState->gamm1);
            }else{ //# u* < x/t < STR
                uu->rho = starState->rhostarR;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }else {//# shock
            if (xDt>= starState->SR) {  // # xDt > SR
                uu->rho = setup->rhoR;
                p = setup->pR;
                uu->rhoU[0] = uu->rho * setup->uR;
            }else {//#ustar < xDt < SR
                uu->rho = starState->rhostarR;
                p = starState->pstar;
                uu->rhoU[0] = uu->rho * starState->ustar;
            }
        }
    }
    PetscReal e =  p/starState->gamm1/uu->rho;
    PetscReal E = e + 0.5*(uu->rhoU[0]/uu->rho)*(uu->rhoU[0]/uu->rho);
    uu->rhoE = uu->rho*E;
}

static PetscErrorCode SetExactSolutionRho(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);

    u[0] = uu.rho;
    return 0;
}

static PetscErrorCode SetExactSolutionRhoU(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);
    u[0] = uu.rhoU[0];
    u[1] = uu.rhoU[1];
    return 0;
}

static PetscErrorCode SetExactSolutionRhoE(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx){
    ProblemSetup* prob = (ProblemSetup*)ctx;

    PetscReal xDt = (x[0]-prob->setup.length/2)/time;
    EulerNode uu;
    SetExactSolutionAtPoint(dim, xDt, &prob->setup, &prob->starState, &uu);
    u[0] = uu.rhoE;
    return 0;
}

static PetscErrorCode PrintVector(DM dm, Vec v, PetscInt step, const char * fileName){
    Vec                cellgeom;
    PetscErrorCode ierr = DMPlexGetGeometryFVM(dm, NULL, &cellgeom, NULL);CHKERRQ(ierr);
    PetscInt cStart, cEnd;
        ierr = DMPlexGetSimplexOrBoxCells(dm, 0, &cStart, &cEnd);CHKERRQ(ierr);
    DM dmCell;
    ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
    const PetscScalar *cgeom;
    ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    const PetscScalar      *x;
    ierr = VecGetArrayRead(v, &x);CHKERRQ(ierr);
    // print the header for each file
    char filename[100];
    ierr = PetscSNPrintf(filename,sizeof(filename),"%s.%d.txt",fileName, step);CHKERRQ(ierr);

    PetscInt rank = 0;
    PetscInt size;
    ierr = MPI_Comm_rank(PetscObjectComm(dm), &rank);CHKERRMPI(ierr);
    ierr = MPI_Comm_size(PetscObjectComm(dm), &size);CHKERRMPI(ierr);

    for(PetscInt r =0; r < size; r++ ) {
        if(r == rank) {
            FILE *fptr;
            if(r == 0){
                fptr = fopen(filename, "w");
                fprintf(fptr, "x rho u e\n");
            }else{
                fptr = fopen(filename, "a");
            }
            for (PetscInt c = cStart; c < cEnd; ++c) {
                PetscFVCellGeom *cg;
                const EulerNode *xc;

                ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);
                CHKERRQ(ierr);
                ierr = DMPlexPointGlobalFieldRead(dm, c, 0, x, &xc);
                CHKERRQ(ierr);
                if(xc) {// must be real cell and not ghost
                    PetscReal u0 = xc->rhoU[0] / xc->rho;
                    fprintf(fptr, "%f %f %f %f\n", cg->centroid[0], xc->rho, u0, (xc->rhoE / xc->rho) - 0.5 * u0 * u0);
                }
            }

            fclose(fptr);
        }
        MPI_Barrier(PetscObjectComm(dm));
    }
    ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(v, &x);CHKERRQ(ierr);
    return 0;
}

static PetscErrorCode MonitorError(TS ts, PetscInt step, PetscReal time, Vec u, void *ctx) {
    PetscFunctionBeginUser;
    PetscErrorCode     ierr;

    // Get the DM
    DM dm;
    ierr = TSGetDM(ts, &dm);CHKERRQ(ierr);

    // Create a copy of the u vector
    Vec e;
    ierr = DMCreateGlobalVector(dm, &e);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject)e, "exact");CHKERRQ(ierr);

    // Set the values
    PetscErrorCode     (*func[3]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {{SetExactSolutionRho, SetExactSolutionRhoU, SetExactSolutionRhoE}};
    void* ctxs[3] ={ctx, ctx, ctx};
    ierr    = DMProjectFunction(dm,time,func,ctxs,INSERT_ALL_VALUES,e);CHKERRQ(ierr);

    // just print to a file for now
    ierr = PrintVector(dm, e, step, "exact");CHKERRQ(ierr);
    ierr = PrintVector(dm, u, step, "solution");CHKERRQ(ierr);

    PetscPrintf(PETSC_COMM_WORLD, "TS %d: %f\n", step, time);

    DMRestoreGlobalVector(dm, &e);
    PetscFunctionReturn(0);
}

static PetscErrorCode PhysicsBoundary_Euler_Mirror(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup* prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = xI->rho;
    xG->rhoE = xI->rhoE;
    xG->rhoU[0] = xI->rhoU[0];
    xG->rhoU[1] = xI->rhoU[1];

    PetscFunctionReturn(0);
}

/* PetscReal* => EulerNode* conversion */
static PetscErrorCode PhysicsBoundary_Euler_Left(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup* prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = prob->setup.rhoL;
    PetscReal eT = prob->setup.rhoL*((prob->setup.pL /(prob->setup.gamma -1) / prob->setup.rhoL) + 0.5 * prob->setup.uL * prob->setup.uL);
    xG->rhoE = eT;
    xG->rhoU[0] = prob->setup.rhoL * prob->setup.uL;
    xG->rhoU[1] = 0.0;

    PetscFunctionReturn(0);
}

/* PetscReal* => EulerNode* conversion */
static PetscErrorCode PhysicsBoundary_Euler_Right(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx)
{
    const EulerNode *xI = (const EulerNode*)a_xI;
    EulerNode       *xG = (EulerNode*)a_xG;
    ProblemSetup*        prob = (ProblemSetup*)ctx;
    PetscFunctionBeginUser;
    xG->rho = prob->setup.rhoR;
    PetscReal eT = prob->setup.rhoR*((prob->setup.pR /(prob->setup.gamma -1)/ prob->setup.rhoR) + 0.5 * prob->setup.uR * prob->setup.uR);
    xG->rhoE = eT;
    xG->rhoU[0] = prob->setup.rhoR * prob->setup.uR;
    xG->rhoU[1] = 0.0;

    PetscFunctionReturn(0);
}

static PetscErrorCode  ComputeTimeStep(TS ts){
    DM dm;
    PetscErrorCode ierr = TSGetDM(ts, &dm);CHKERRQ(ierr);
    Vec v;
    TSGetSolution(ts, &v);
    ProblemSetup *problem;
    ierr = DMGetApplicationContext(dm, &problem);CHKERRQ(ierr);

    Vec                cellgeom;
    ierr = DMPlexGetGeometryFVM(dm, NULL, &cellgeom, NULL);CHKERRQ(ierr);
    PetscInt cStart, cEnd;
    ierr = DMPlexGetSimplexOrBoxCells(dm, 0, &cStart, &cEnd);CHKERRQ(ierr);
    DM dmCell;
    ierr = VecGetDM(cellgeom, &dmCell);CHKERRQ(ierr);
    const PetscScalar *cgeom;
    ierr = VecGetArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    const PetscScalar      *x;
    ierr = VecGetArrayRead(v, &x);CHKERRQ(ierr);

    PetscReal dtMin = 1.0;

    for (PetscInt c = cStart; c < cEnd; ++c) {
        PetscFVCellGeom       *cg;
        const EulerNode           *xc;

        ierr = DMPlexPointLocalRead(dmCell, c, cgeom, &cg);CHKERRQ(ierr);
        ierr = DMPlexPointGlobalFieldRead(dm, c, 0, x, &xc);CHKERRQ(ierr);

        if(xc) {  // must be real cell and not ghost

            PetscReal rho = xc->rho;
            PetscReal u = xc->rhoU[0] / rho;
            PetscReal e = (xc->rhoE / rho) - 0.5 * u * u;
            PetscReal p = (problem->setup.gamma - 1) * rho * e;

            PetscReal a = PetscSqrtReal(problem->setup.gamma * p / rho);
            PetscReal dt = problem->cfl * cg->volume / (a + PetscAbsReal(u));
            dtMin = PetscMin(dtMin, dt);
        }
    }
    PetscInt rank;
    MPI_Comm_rank(PetscObjectComm(ts), &rank);
    printf("dtMin(%d): %f\n", rank,dtMin );

    PetscReal dtMinGlobal;
    ierr = MPI_Allreduce(&dtMin, &dtMinGlobal, 1,MPIU_REAL, MPI_MIN, PetscObjectComm(ts));

    PetscPrintf(PetscObjectComm(ts), "TimeStep: %f\n", dtMinGlobal);
//    ierr = TSSetTimeStep(ts, dtMinGlobal);CHKERRQ(ierr);

    ierr = VecRestoreArrayRead(cellgeom, &cgeom);CHKERRQ(ierr);
    ierr = VecRestoreArrayRead(v, &x);CHKERRQ(ierr);
    return 0;
}

static void ComputeFluxRho(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

        flux[0] = rho * u * PetscSignReal(n[0]);
        printf("flux qp[%f]: %f  n:%f rL:%f rR:%f\n ", qp[0], flux[0], n[0], currentValues.rhoL, currentValues.rhoR);
//        flux->rhoU[0] = (rho * u * u + p)* PetscSignReal(n[0]);
//        flux->rhoU[1] = 0.0;
//        PetscReal et = e + 0.5 * u * u;
//        flux->rhoE = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f,%f\n", qp[0], qp[1], flux[0], n[0], n[1]);mm

    }else{
        flux[0] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

static void ComputeFluxU(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

//        flux[0] = (rho * u) * PetscSignReal(n[0]);
        flux[0] = (rho * u * u + p)* PetscSignReal(n[0]);
        flux[1] = 0.0;
//        PetscReal et = e + 0.5 * u * u;
//        flux->rhoE = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f %f %f\n", qp[0], qp[1], flux->rho, flux->rhoU[0], flux->rhoE, n[0]);

    }else{
        flux[0] = 0.0;
        flux[1] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

static void ComputeFluxE(PetscInt dim, PetscInt Nf, const PetscReal *qp, const PetscReal *n, const EulerNode *xL, const EulerNode *xR, PetscInt numConstants, const PetscScalar constants[], PetscReal *flux, void* ctx) {
//    dim	- The spatial dimension
//    Nf	- The number of fields
//    x	- The coordinates at a point on the interface
//    n	- The normal vector to the interface
//    uL	- The state vector to the left of the interface
//    uR	- The state vector to the right of the interface
//    flux	- output array of flux through the interface
//    numConstants	- number of constant parameters
//    constants	- constant parameters
//    ctx	- optional user context
    ProblemSetup* prob = (ProblemSetup*)ctx;

    // this is a hack, only add in flux from left/right
    if(PetscAbs(n[0]) > 1E-5) {
        // Setup Godunov
        Setup currentValues;

        currentValues.gamma = prob->setup.gamma;
        currentValues.length = prob->setup.length;

        if (n[0] > 0) {
            currentValues.rhoL = xL->rho;
            currentValues.uL = xL->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xL->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;

            currentValues.rhoR = xR->rho;
            currentValues.uR = xR->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xR->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;
        }else{
            currentValues.rhoR = xL->rho;
            currentValues.uR = xL->rhoU[0] / currentValues.rhoR;
            PetscReal eR = (xL->rhoE / currentValues.rhoR) - 0.5 * currentValues.uR * currentValues.uR;
            currentValues.pR = (prob->setup.gamma - 1) * currentValues.rhoR * eR;

            currentValues.rhoL = xR->rho;
            currentValues.uL = xR->rhoU[0] / currentValues.rhoL;
            PetscReal eL = (xR->rhoE / currentValues.rhoL) - 0.5 * currentValues.uL * currentValues.uL;
            currentValues.pL = (prob->setup.gamma - 1) * currentValues.rhoL * eL;
        }
        StarState result;
        DetermineStarState(&currentValues, &result);
        EulerNode exact;
        SetExactSolutionAtPoint(dim, 0.0, &currentValues, &result, &exact);

        PetscReal rho = exact.rho;
        PetscReal u = exact.rhoU[0]/rho;
        PetscReal e = (exact.rhoE / rho) - 0.5 * u * u;
        PetscReal p = (prob->setup.gamma-1) * rho * e;

//        flux[0] = (rho * u) * PetscSignReal(n[0]);
//        flux[0] = (rho * u * u + p)* PetscSignReal(n[0]);
//        flux[1] = 0.0;
        PetscReal et = e + 0.5 * u * u;
        flux[0] = (rho * u * (et + p / rho))* PetscSignReal(n[0]);

//        printf("%f,%f %f %f %f %f\n", qp[0], qp[1], flux->rho, flux->rhoU[0], flux->rhoE, n[0]);

    }else{
        flux[0] = 0.0;
//        flux[1] = 0.0;
//        flux->rhoU[0] =0.0;
//        flux->rhoU[1] = 0.0;
//        flux->rhoE = 0.0;

    }

//    F[0][i]=rho[i]*u[i]
//    F[1][i]=rho[i]*u[i]*u[i]+p[i]
//    et = e[i]+0.5*u[i]*u[i]
//    F[2][i]=rho[i]*u[i]*(et+p[i]/rho[i])

}

int main(int argc, char **argv)
{
    PetscErrorCode ierr;
    // create the mesh
    // setup the ts
    DM dm;                 /* problem definition */
    TS ts;                 /* timestepper */

    // initialize petsc and mpi
    PetscInitialize(&argc, &argv, NULL, "HELP");

    // Setup the problem
    ProblemSetup problem;

    // case 1 - Sod problem
    problem.setup.rhoL=1.0;
    problem.setup.uL=0.0;
    problem.setup.pL=1.0;
    problem.setup.rhoR=0.125;
    problem.setup.uR=0.0;
    problem.setup.pR=0.1;
    problem.setup.maxTime = 0.25;
    problem.setup.length = 1;
    problem.setup.gamma = 1.4;
    problem.cfl = .5;

    // case 2 - 123 problem - expansion left and expansion right
//    problem.setup.rhoL=1.0;
//    problem.setup.uL=-2.0;
//    problem.setup.pL=0.4;
//    problem.setup.rhoR=1.0;
//    problem.setup.uR=2.0;
//    problem.setup.pR=0.4;
//    problem.setup.maxTime = 0.15;
//    problem.setup.length = 1;
//    problem.setup.gamma = 1.4;
//    problem.cfl = 0.4;

    ierr = TSCreate(PETSC_COMM_WORLD, &ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetType(ts, TSEULER);CHKERRQ(ierr);

    //PetscErrorCode DMPlexCreateBoxMesh(MPI_Comm comm, PetscInt dim, PetscBool simplex, const PetscInt faces[], const PetscReal lower[], const PetscReal upper[], const DMBoundaryType periodicity[], PetscBool interpolate, DM *dm)
    PetscReal start[] = {0.0, 0.0};
    PetscReal end[] = {problem.setup.length, 1};
    PetscInt nx[] = {100, 1};
    DMBoundaryType bcType[] = {DM_BOUNDARY_NONE, DM_BOUNDARY_NONE};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, DIM, PETSC_FALSE, nx, start, end, bcType, PETSC_TRUE, &dm);CHKERRQ(ierr);

    {
        DM dmDist;

//        ierr = DMSetBasicAdjacency(dm, PETSC_TRUE, PETSC_FALSE);CHKERRQ(ierr);
        ierr = DMPlexDistribute(dm, 1, NULL, &dmDist);CHKERRQ(ierr);
        if (dmDist) {
            ierr = DMDestroy(&dm);CHKERRQ(ierr);
            dm   = dmDist;
        }
    }

    ierr = DMSetFromOptions(dm);CHKERRQ(ierr);

    {
        DM gdm;
        ierr = DMPlexConstructGhostCells(dm, NULL, NULL, &gdm);CHKERRQ(ierr);
        ierr = DMDestroy(&dm);CHKERRQ(ierr);
        dm   = gdm;
    }

//    DMLabel label;
//    ierr = DMGetLabel(dm, "marker", &label );CHKERRQ(ierr);
//    ierr = DMLabelView(label, PETSC_VIEWER_STDOUT_WORLD);;CHKERRQ(ierr);

    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetDM(ts, dm);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSSetExactFinalTime(ts, TS_EXACTFINALTIME_MATCHSTEP);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // setup the FV

    {// Setup the fields
        PetscInt f, dof;
        for (f=0,dof=0; f < 3; f++) {
            PetscInt newDof = PhysicsFields[f].components;

//            if (newDof == 1) {
                PetscFV           fvm;
                ierr = PetscFVCreate(PETSC_COMM_WORLD, &fvm);CHKERRQ(ierr);

                ierr = PetscFVSetFromOptions(fvm);CHKERRQ(ierr);
                ierr = PetscFVSetNumComponents(fvm, newDof);CHKERRQ(ierr);
                ierr = PetscFVSetSpatialDimension(fvm, DIM);CHKERRQ(ierr);
                ierr = PetscObjectSetName((PetscObject) fvm,PhysicsFields[f].fieldName);CHKERRQ(ierr);

                /* FV is now structured with one field having all physics as components */
                ierr = DMAddField(dm, NULL, (PetscObject) fvm);CHKERRQ(ierr);
//            }
//            else {
//                PetscInt j;
//
//                for (j = 0; j < newDof; j++) {
//                    char     compName[256]  = "Unknown";
//
//                    ierr = PetscSNPrintf(compName,sizeof(compName),"%s_%d",PhysicsFields[f].fieldName,j);CHKERRQ(ierr);
//                    ierr = PetscFVSetComponentName(fvm,dof+j,compName);CHKERRQ(ierr);
//                }
//            }
//            dof += newDof;
        }
    }

    // Compute the star state
    ierr = DetermineStarState(&problem.setup, &problem.starState);CHKERRQ(ierr);

    PetscDS           prob;
    ierr = DMCreateDS(dm);CHKERRQ(ierr);
    ierr = DMGetDS(dm, &prob);CHKERRQ(ierr);
    ierr = DMSetApplicationContext(dm, &problem);CHKERRQ(ierr);

    //TODO: Add flux
    ierr = PetscDSSetRiemannSolver(prob, 0,ComputeFluxRho);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 0, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetRiemannSolver(prob, 1,ComputeFluxU);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 1, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetRiemannSolver(prob, 2,ComputeFluxE);CHKERRQ(ierr);
    ierr = PetscDSSetContext(prob, 2, &problem);CHKERRQ(ierr);
    ierr = PetscDSSetFromOptions(prob);CHKERRQ(ierr);
    //TODO: Apply boundary

    // setup the solution vector, this olds everything
    Vec X;
    ierr = DMCreateGlobalVector(dm, &X);CHKERRQ(ierr);
    ierr = PetscObjectSetName((PetscObject) X, "solution");CHKERRQ(ierr);

//    ierr = DMTSSetBoundaryLocal(dm, DMPlexTSComputeBoundary, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetIFunctionLocal(dm, DMPlexTSComputeIFunctionFEM, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetIJacobianLocal(dm, DMPlexTSComputeIJacobianFEM, NULL);CHKERRQ(ierr);
//    ierr = DMTSSetRHSFunctionLocal(dm, DMPlexTSComputeRHSFunctionFVM, NULL);CHKERRQ(ierr);//TODO: This is were we set the RHS function

    const PetscInt idsLeft[]= {4};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall left", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Left, NULL, 1, idsLeft, &problem);CHKERRQ(ierr);
    const PetscInt idsRight[]= {2};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall right", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Right, NULL, 1, idsRight, &problem);CHKERRQ(ierr);

    const PetscInt mirror[]= {1, 3};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "mirrorWall", "Face Sets", 0, 0, NULL, (void (*)(void)) PhysicsBoundary_Euler_Mirror, NULL, 2, mirror, &problem);CHKERRQ(ierr);

//    ierr = DMTSSetBoundaryLocal(dm, DMPlexTSComputeBoundary, NULL);CHKERRQ(ierr);
    ierr = DMTSSetRHSFunctionLocal(dm, DMPlexTSComputeRHSFunctionFVM, NULL);CHKERRQ(ierr);//TODO: This is were we set the RHS function
    ierr = TSSetMaxTime(ts,problem.setup.maxTime);CHKERRQ(ierr);
    ierr = TSMonitorSet(ts, MonitorError, &problem, NULL);CHKERRQ(ierr);
    ierr = TSSetTimeStep(ts, 0.0008);CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts);CHKERRQ(ierr);
    ierr = TSSetPostStep(ts, ComputeTimeStep);CHKERRQ(ierr);

    // set the initial conditions
    PetscErrorCode     (*func[3]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {SetExactSolutionRho, SetExactSolutionRhoU, SetExactSolutionRhoE};
    void* ctxs[3] ={&problem, &problem, &problem};
    ierr    = DMProjectFunction(dm,0.0,func,ctxs,INSERT_ALL_VALUES,X);CHKERRQ(ierr);

//    PetscInt rank, size;
//    MPI_Comm_rank(PetscObjectComm(dm), &rank);
//    MPI_Comm_size(PetscObjectComm(dm), &size);
//    for(int r =0; r < size; r++) {
//        if(r == rank) {
//            printf("Rank: %d\n", r);
//            ierr = DMView(dm, PETSC_VIEWER_STDOUT_SELF);
//            CHKERRQ(ierr);
////            ierr = DMViewFromOptions(dm, NULL, "-dm_view");
////            CHKERRQ(ierr);
//        }
//        MPI_Barrier(PetscObjectComm(dm));
//    }
//    TSSetMaxSteps(ts, 1);
    ierr = TSSolve(ts,X);CHKERRQ(ierr);
//    ierr = TSGetSolveTime(ts,&ftime);CHKERRQ(ierr);
//    ierr = TSGetStepNumber(ts,&nsteps);
    //    CHKERRQ(ierr);

    return PetscFinalize();

}


//
//  hemm.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 1/6/12.
//  Copyright (c) 2012 University of Colorado Denver. All rights reserved.
//

#ifndef _hemm_h
#define _hemm_h

/// @file hemm.h Performs general Hermitian complex matrix-matrix multiplication.

#include <cctype>
#include "latl.h"

namespace LATL
{
   /// @brief  Performs general Hermitian complex matrix-matrix multiplication.
   ///
   /// For complex matrices B and C, Hermitian matrix A, and complex scalars alpha and beta,
   ///
   ///         C := alpha*A*B + beta*C  or   C := alpha*B*A + beta*C
   /// is computed.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param side Specifies whether the matrix A appears on the left or right side as follows:
   ///
   ///        if side = 'L' or 'l' then C := alpha*A*B + beta*C,
   ///        if side = 'R' or 'r' then C := alpha*B*A + beta*C.
   /// @param uplo Specifies whether the upper or lower triangular part of the Hermitian matrix A
   /// is to be referenced:
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular,
   ///        if uplo = 'L' or 'l' then A is lower triangular.
   /// @param m Specifies the number of rows of the matrices B and C.  m>=0
   /// @param n Specifies the number of columns of the matrices B and C.  n>=0
   /// @param alpha Complex scalar.
   /// @param A Pointer to complex Hermitian matrix A.  If side = 'L' or 'l', then A is m-by-m;
   /// if side = 'R' or 'r', then A is n-by-n.
   /// @param ldA Column length of the matrix A.  If side = 'L' or 'l', ldA>=m.  If side = 'R' or 'r', ldA>=n.
   /// @param B Pointer to complex m-by-n matrix B.
   /// @param ldB Column length of the matrix B.  ldB>=m.
   /// @param beta Complex scalar.
   /// @param C Pointer to complex m-by-n matrix C.
   /// @param ldC Column length of the matrix C.  ldC>=m.
   /// @ingroup BLAS

   template <typename real_t>
   int HEMM(char side, char uplo, int_t m, int_t n, complex<real_t> alpha, complex<real_t> *A, int_t ldA, complex<real_t> *B, int_t ldB, complex<real_t> beta, complex<real_t> *C, int_t ldC)
   {
      using std::conj;
      using std::real;
      using std::toupper;

      const complex<real_t> zero(0.0,0.0);
      const complex<real_t> one(1.0,0.0);
      int_t i,j,k;
      complex<real_t> *a,*b,*c,*at,*bt;
      complex<real_t> s,t;

      side=toupper(side);
      uplo=toupper(uplo);

      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;
      else if((m==0)||(n==0)||((alpha==zero)&&(beta==one)))
         return 0;

      if(alpha==zero)
      {
         c=C;
         for(j=0;j<n;j++)
         {
            for(i=0;i<m;i++)
               c[i]*=beta;
            c+=ldC;
         }
      }
      else if(side=='L')
      {
         if(uplo=='U')
         {
            b=B;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A;
               for(i=0;i<m;i++)
               {
                  t=alpha*b[i];
                  s=zero;
                  for(k=0;k<i;k++)
                  {
                     c[k]+=t*a[k];
                     s+=b[k]*conj(a[k]);
                  }
                  c[i]=beta*c[i]+t*real(a[i])+alpha*s;
                  a+=ldA;
               }
               b+=ldB;
               c+=ldC;
            }
         }
         else
         {
            b=B;
            c=C;
            for(j=0;j<n;j++)
            {
               a=A+m*ldA;
               for(i=m-1;i>=0;i--)
               {
                  a-=ldA;
                  t=alpha*b[i];
                  s=zero;
                  for(k=i+1;k<m;k++)
                  {
                     c[k]+=t*a[k];
                     s+=b[k]*conj(a[k]);
                  }
                  c[i]=beta*c[i]+t*real(a[i])+alpha*s;
               }
               b+=ldB;
               c+=ldC;
            }
         }
      }
      else
      {
         if(uplo=='U')
         {
            a=A;
            c=C;
            b=B;
            for(j=0;j<n;j++)
            {
               t=alpha*real(a[j]);
               for(i=0;i<m;i++)
                  c[i]=c[i]*beta+t*b[i];
               bt=B;
               for(k=0;k<j;k++)
               {
                  t=alpha*a[k];
                  for(i=0;i<m;i++)
                     c[i]+=bt[i]*t;
                  bt+=ldB;
               }
               at=A+(j+1)*ldA;
               bt=B+(j+1)*ldB;
               for(k=j+1;k<n;k++)
               {
                  t=alpha*conj(at[j]);
                  for(i=0;i<m;i++)
                     c[i]+=t*bt[i];
                  at+=ldA;
                  bt+=ldB;
               }
               a+=ldA;
               b+=ldB;
               c+=ldC;
            }
         }
         else
         {
            a=A;
            c=C;
            b=B;
            for(j=0;j<n;j++)
            {
               t=alpha*real(a[j]);
               for(i=0;i<m;i++)
                  c[i]=c[i]*beta+t*b[i];
               bt=B;
               at=A;
               for(k=0;k<j;k++)
               {
                  t=alpha*conj(at[j]);
                  for(i=0;i<m;i++)
                     c[i]+=bt[i]*t;
                  at+=ldA;
                  bt+=ldB;
               }
               bt=B+(j+1)*ldB;
               for(k=j+1;k<n;k++)
               {
                  t=alpha*at[k];
                  for(i=0;i<m;i++)
                     c[i]+=t*bt[i];
                  bt+=ldB;
               }
               a+=ldA;
               b+=ldB;
               c+=ldC;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int HEMM<float>(char side, char uplo, int_t m, int_t n, complex<float> alpha, complex<float> *A, int_t ldA, complex<float> *B, int_t ldB, complex<float> beta, complex<float> *C, int_t ldC)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_chemm(CblasColMajor,Side,Uplo,m,n,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }

   template <> int HEMM<double>(char side, char uplo, int_t m, int_t n, complex<double> alpha, complex<double> *A, int_t ldA, complex<double> *B, int_t ldB, complex<double> beta, complex<double> *C, int_t ldC)
   {
      using std::toupper;
      side=toupper(side);
      uplo=toupper(uplo);
      if((side!='L')&&(side!='R'))
         return -1;
      else if((uplo!='U')&&(uplo!='L'))
         return -2;
      else if(m<0)
         return -3;
      else if(n<0)
         return -4;
      else if(ldA<((side=='L')?m:n))
         return -7;
      else if(ldB<m)
         return -9;
      else if(ldC<m)
         return -12;

      const CBLAS_SIDE Side=(side=='L')?CblasLeft:CblasRight;
      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zhemm(CblasColMajor,Side,Uplo,m,n,&alpha,A,ldA,B,ldB,&beta,C,ldC);

      return 0;
   }
#endif
}
#endif


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

/* Author:  G. Jungman */

#include <config.h>

#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_sf_bessel.h>
#include "bessel_amp_phase.h"

/* chebyshev expansions for amplitude and phase
   functions used in bessel evaluations

   These are the same for J0,Y0 and for J1,Y1, so
   they sit outside those functions.
*/

static double bm0_data[21] = {
   0.09284961637381644,
  -0.00142987707403484,
   0.00002830579271257,
  -0.00000143300611424,
   0.00000012028628046,
  -0.00000001397113013,
   0.00000000204076188,
  -0.00000000035399669,
   0.00000000007024759,
  -0.00000000001554107,
   0.00000000000376226,
  -0.00000000000098282,
   0.00000000000027408,
  -0.00000000000008091,
   0.00000000000002511,
  -0.00000000000000814,
   0.00000000000000275,
  -0.00000000000000096,
   0.00000000000000034,
  -0.00000000000000012,
   0.00000000000000004
};
const cheb_series _gsl_sf_bessel_amp_phase_bm0_cs = {
  bm0_data,
  20,
  -1, 1,
  10
};

static double bth0_data[24] = {
  -0.24639163774300119,
   0.001737098307508963,
  -0.000062183633402968,
   0.000004368050165742,
  -0.000000456093019869,
   0.000000062197400101,
  -0.000000010300442889,
   0.000000001979526776,
  -0.000000000428198396,
   0.000000000102035840,
  -0.000000000026363898,
   0.000000000007297935,
  -0.000000000002144188,
   0.000000000000663693,
  -0.000000000000215126,
   0.000000000000072659,
  -0.000000000000025465,
   0.000000000000009229,
  -0.000000000000003448,
   0.000000000000001325,
  -0.000000000000000522,
   0.000000000000000210,
  -0.000000000000000087,
   0.000000000000000036
};
const cheb_series _gsl_sf_bessel_amp_phase_bth0_cs = {
  bth0_data,
  23,
  -1, 1,
  12
};

static double bm1_data[21] = {
   0.1047362510931285,
   0.00442443893702345,
  -0.00005661639504035,
   0.00000231349417339,
  -0.00000017377182007,
   0.00000001893209930,
  -0.00000000265416023,
   0.00000000044740209,
  -0.00000000008691795,
   0.00000000001891492,
  -0.00000000000451884,
   0.00000000000116765,
  -0.00000000000032265,
   0.00000000000009450,
  -0.00000000000002913,
   0.00000000000000939,
  -0.00000000000000315,
   0.00000000000000109,
  -0.00000000000000039,
   0.00000000000000014,
  -0.00000000000000005,
};
const cheb_series _gsl_sf_bessel_amp_phase_bm1_cs = {
  bm1_data,
  20,
  -1, 1,
  10
};

static double bth1_data[24] = {
   0.74060141026313850,
  -0.004571755659637690,
   0.000119818510964326,
  -0.000006964561891648,
   0.000000655495621447,
  -0.000000084066228945,
   0.000000013376886564,
  -0.000000002499565654,
   0.000000000529495100,
  -0.000000000124135944,
   0.000000000031656485,
  -0.000000000008668640,
   0.000000000002523758,
  -0.000000000000775085,
   0.000000000000249527,
  -0.000000000000083773,
   0.000000000000029205,
  -0.000000000000010534,
   0.000000000000003919,
  -0.000000000000001500,
   0.000000000000000589,
  -0.000000000000000237,
   0.000000000000000097,
  -0.000000000000000040,
};
const cheb_series _gsl_sf_bessel_amp_phase_bth1_cs = {
  bth1_data,
  23,
  -1, 1,
  12
};

int
gsl_sf_bessel_asymp_Mnu_e(const double nu, const double x, double * result)
{
  const double r  = 2.0*nu/x;
  const double r2 = r*r;
  const double x2 = x*x;
  const double term1 = (r2-1.0/x2)/8.0;
  const double term2 = (r2-1.0/x2)*(r2-9.0/x2)*3.0/128.0;
  const double Mnu2_c = 2.0/(M_PI) * (1.0 + term1 + term2);
  *result = sqrt(Mnu2_c)/sqrt(x); /* will never underflow this way */
  return GSL_SUCCESS;
}

int
gsl_sf_bessel_asymp_thetanu_corr_e(const double nu, const double x, double * result)
{
  const double r  = 2.0*nu/x;
  const double r2 = r*r;
  const double x2 = x*x;
  const double term1 = x*(r2 - 1.0/x2)/8.0;
  const double term2 = x*(r2 - 1.0/x2)*(r2 - 25.0/x2)/384.0;
  *result = (-0.25*M_PI + term1 + term2);
  return GSL_SUCCESS;
}