/*
 # Copyright (C) 2009-2013 Thierry Sousbie
 # University of Tokyo / CNRS, 2009
 #
 # This file is part of porject DisPerSE
 #
 #  Author          : Thierry Sousbie
 #  Contact         : tsousbie@gmail.com
 #
 #  Licenses        : This file is 'dual-licensed', you have to choose one
 #                    of the two licenses below to apply.
 #
 #                    CeCILL-C
 #                    The CeCILL-C license is close to the GNU LGPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL-C_V1-en.html )
 #
 #                or  CeCILL v2.0
 #                    The CeCILL license is compatible with the GNU GPL.
 #                    ( http://www.cecill.info/licences/Licence_CeCILL_V2-en.html )
 #
 #  This software is governed either by the CeCILL or the CeCILL-C license
 #  under French law and abiding by the rules of distribution of free software.
 #  You can  use, modify and or redistribute the software under the terms of
 #  the CeCILL or CeCILL-C licenses as circulated by CEA, CNRS and INRIA
 #  at the following URL : "http://www.cecill.info".
 #
 #  As a counterpart to the access to the source code and  rights to copy,
 #  modify and redistribute granted by the license, users are provided only
 #  with a limited warranty  and the software's author,  the holder of the
 #  economic rights,  and the successive licensors  have only  limited
 #  liability.
 #
 #  In this respect, the user's attention is drawn to the risks associated
 #  with loading,  using,  modifying and/or developing or reproducing the
 #  software by the user in light of its specific status of free software,
 #  that may mean  that it is complicated to manipulate,  and  that  also
 #  therefore means  that it is reserved for developers  and  experienced
 #  professionals having in-depth computer knowledge. Users are therefore
 #  encouraged to load and test the software's suitability as regards their
 #  requirements in conditions enabling the security of their systems and/or
 #  data to be ensured and,  more generally, to use and operate it in the
 #  same conditions as regards security.
 #
 #  The fact that you are presently reading this means that you have had
 #  knowledge of the CeCILL and CeCILL-C licenses and that you accept its terms.
*/
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "simplex.h"

#ifdef HAVE_GSL

#include <gsl/gsl_linalg.h>
#include <gsl/gsl_blas.h>

double SimplexVolumed(double **coord,int nvert, int ndims)
{
    int i,j,k;
    double det;
    int s;
    double m_data[(nvert-1)*(nvert-1)];
    double w_data[(nvert-1)*(nvert-1)];
/*
    double w_data[3*3] = {1.,0.,0.,
			  1.,1.,0.,
			  1.,1.,1.};*/
    //double wt_data[(nvert-1)*ndims];
    gsl_permutation * perm;

    gsl_matrix_view m = gsl_matrix_view_array (m_data, nvert-1, nvert-1);
    gsl_matrix_view w = gsl_matrix_view_array (w_data, nvert-1, ndims);
    //gsl_matrix_view wt = gsl_matrix_view_array (wt_data, nvert-1,ndims);

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(coord[1][i]-coord[0][i])*(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    perm = gsl_permutation_alloc (nvert-1);

    for (i=0,k=0;i<nvert-1;i++)
	for (j=0;j<nvert-1;j++,k++)
	    w_data[k]=coord[i+1][j]-coord[0][j];

    //gsl_matrix_transpose_memcpy (wt,w);

    gsl_blas_dgemm (CblasNoTrans, CblasTrans,
		    1.0, &w.matrix, &w.matrix,
		    0.0, &m.matrix);

    gsl_linalg_LU_decomp(&m.matrix,perm,&s);
    det=gsl_linalg_LU_det(&m.matrix,s);

    gsl_permutation_free(perm);

    for (i=2,j=1;i<=nvert-1;i++) j*=i;

    return sqrt(fabs(det))/j;
}

double SimplexVolumef(float **coord,int nvert, int ndims)
{
    int i,j,k;
    double det;
    int s;
    double m_data[(nvert-1)*(nvert-1)];
    double w_data[(nvert-1)*(nvert-1)];
/*
    double w_data[3*3] = {1.,0.,0.,
			  1.,1.,0.,
			  1.,1.,1.};*/
    //double wt_data[(nvert-1)*ndims];
    gsl_permutation * perm;

    gsl_matrix_view m = gsl_matrix_view_array (m_data, nvert-1, nvert-1);
    gsl_matrix_view w = gsl_matrix_view_array (w_data, nvert-1, ndims);
    //gsl_matrix_view wt = gsl_matrix_view_array (wt_data, nvert-1,ndims);

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(double)(coord[1][i]-coord[0][i])*(double)(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    perm = gsl_permutation_alloc (nvert-1);

    for (i=0,k=0;i<nvert-1;i++)
	for (j=0;j<nvert-1;j++,k++)
	    w_data[k]=coord[i+1][j]-coord[0][j];

    //gsl_matrix_transpose_memcpy (wt,w);

    gsl_blas_dgemm (CblasNoTrans, CblasTrans,
		    1.0, &w.matrix, &w.matrix,
		    0.0, &m.matrix);

    gsl_linalg_LU_decomp(&m.matrix,perm,&s);
    det=gsl_linalg_LU_det(&m.matrix,s);

    gsl_permutation_free(perm);

    for (i=2,j=1;i<=nvert-1;i++) j*=i;

    return sqrt(fabs(det))/j;
}

// center must be allocated to ndims vals at least.
// coord are the vertice coords
// center is the center of the resulting ndims-sphere
// function returns its radius squared
double SimplexSphere(double **coord, int ndims,double *center)
{
    double a_data[ndims*ndims];
    double b_data[ndims];

    gsl_matrix_view m=gsl_matrix_view_array (a_data, ndims, ndims);
    gsl_vector_view b=gsl_vector_view_array (b_data, ndims);
    gsl_vector_view x = gsl_vector_view_array (center,ndims);
    gsl_permutation * p = gsl_permutation_alloc (ndims);

    int s;
    int i,j,k;
    double d;

    for (i=0,k=0;i<ndims;i++)
    {
	b_data[i]=0;
	for (j=0;j<ndims;j++,k++)
	{
	    a_data[k]=((double)coord[0][j]-(double)coord[i+1][j]);
	    b_data[i]+=((double)coord[0][j]*(double)coord[0][j]-(double)coord[i+1][j]*(double)coord[i+1][j]);
	}
	b_data[i]*=0.5;
    }

    gsl_linalg_LU_decomp (&m.matrix, p, &s);
    gsl_linalg_LU_solve (&m.matrix, p, &b.vector, &x.vector);
    gsl_permutation_free (p);

    for (i=0,d=0;i<ndims;i++)
	d+=((double)center[i]-(double)coord[0][i])*((double)center[i]-(double)coord[0][i]);

    return d;
}

#else

double SimplexVolumef(float **coord,int nvert, int ndims)
{
    int i;
    double det;

    if (nvert==2)
    {
	for (i=0,det=0;i<ndims;i++)
	    det+=(double)(coord[1][i]-coord[0][i])*(double)(coord[1][i]-coord[0][i]);
	return sqrt(det);
    }

    fprintf(stderr,"ERROR: Function 'SimplexVolume' needs GSL.\n");
    fprintf(stderr,"Please, recompile with GSL.\n");
    return -1;
}

double SimplexSphere(double **coords, int ndims,double *center)
{
    fprintf(stderr,"ERROR: Function 'SimplexSphere' needs GSL.\n");
    fprintf(stderr,"Please, recompile with GSL.\n");

    return -1;
}

#endif


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

#include <config.h>
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_siman.h>
#include <gsl/gsl_ieee_utils.h>

/* set up parameters for this simulated annealing run */

#define N_TRIES 200		/* how many points do we try before stepping */
#define ITERS_FIXED_T 2000	/* how many iterations for each T? */
#define STEP_SIZE 1.0		/* max step size in random walk */
#define K 1.0			/* Boltzmann constant */
#define T_INITIAL 5000.0	/* initial temperature */
#define MU_T 1.002		/* damping factor for temperature */
#define T_MIN 5.0e-1

gsl_siman_params_t params = {N_TRIES, ITERS_FIXED_T, STEP_SIZE,
			     K, T_INITIAL, MU_T, T_MIN};

struct s_tsp_city {
  const char * name;
  double lat, longitude;	/* coordinates */
};
typedef struct s_tsp_city Stsp_city;

void prepare_distance_matrix(void);
void exhaustive_search(void);
void print_distance_matrix(void);
double city_distance(Stsp_city c1, Stsp_city c2);
double Etsp(void *xp);
double Mtsp(void *xp, void *yp);
void Stsp(const gsl_rng * r, void *xp, double step_size);
void Ptsp(void *xp);

/* in this table, latitude and longitude are obtained from the US
   Census Bureau, at http://www.census.gov/cgi-bin/gazetteer */

Stsp_city cities[] = {{"Santa Fe",    35.68,   105.95},
		      {"Phoenix",     33.54,   112.07},
		      {"Albuquerque", 35.12,   106.62},
		      {"Clovis",      34.41,   103.20},
		      {"Durango",     37.29,   107.87},
		      {"Dallas",      32.79,    96.77},
		      {"Tesuque",     35.77,   105.92},
		      {"Grants",      35.15,   107.84},
		      {"Los Alamos",  35.89,   106.28},
		      {"Las Cruces",  32.34,   106.76},
		      {"Cortez",      37.35,   108.58},
		      {"Gallup",      35.52,   108.74}};

#define N_CITIES (sizeof(cities)/sizeof(Stsp_city))

double distance_matrix[N_CITIES][N_CITIES];

/* distance between two cities */
double city_distance(Stsp_city c1, Stsp_city c2)
{
  const double earth_radius = 6375.000;	/* 6000KM approximately */
  /* sin and cos of lat and long; must convert to radians */
  double sla1 = sin(c1.lat*M_PI/180), cla1 = cos(c1.lat*M_PI/180),
    slo1 = sin(c1.longitude*M_PI/180), clo1 = cos(c1.longitude*M_PI/180);
  double sla2 = sin(c2.lat*M_PI/180), cla2 = cos(c2.lat*M_PI/180),
    slo2 = sin(c2.longitude*M_PI/180), clo2 = cos(c2.longitude*M_PI/180);

  double x1 = cla1*clo1;
  double x2 = cla2*clo2;

  double y1 = cla1*slo1;
  double y2 = cla2*slo2;

  double z1 = sla1;
  double z2 = sla2;

  double dot_product = x1*x2 + y1*y2 + z1*z2;

  double angle = acos(dot_product);

  /* distance is the angle (in radians) times the earth radius */
  return angle*earth_radius;
}

/* energy for the travelling salesman problem */
double Etsp(void *xp)
{
  /* an array of N_CITIES integers describing the order */
  int *route = (int *) xp;
  double E = 0;
  unsigned int i;

  for (i = 0; i < N_CITIES; ++i) {
    /* use the distance_matrix to optimize this calculation; it had
       better be allocated!! */
    E += distance_matrix[route[i]][route[(i + 1) % N_CITIES]];
  }

  return E;
}

double Mtsp(void *xp, void *yp)
{
  int *route1 = (int *) xp, *route2 = (int *) yp;
  double distance = 0;
  unsigned int i;

  for (i = 0; i < N_CITIES; ++i) {
    distance += ((route1[i] == route2[i]) ? 0 : 1);
  }

  return distance;
}

/* take a step through the TSP space */
void Stsp(const gsl_rng * r, void *xp, double step_size)
{
  int x1, x2, dummy;
  int *route = (int *) xp;

  step_size = 0 ; /* prevent warnings about unused parameter */

  /* pick the two cities to swap in the matrix; we leave the first
     city fixed */
  x1 = (gsl_rng_get (r) % (N_CITIES-1)) + 1;
  do {
    x2 = (gsl_rng_get (r) % (N_CITIES-1)) + 1;
  } while (x2 == x1);

  dummy = route[x1];
  route[x1] = route[x2];
  route[x2] = dummy;
}

void Ptsp(void *xp)
{
  unsigned int i;
  int *route = (int *) xp;
  printf("  [");
  for (i = 0; i < N_CITIES; ++i) {
    printf(" %d ", route[i]);
  }
  printf("]  ");
}

int main(void)
{
  int x_initial[N_CITIES];
  unsigned int i;

  const gsl_rng * r = gsl_rng_alloc (gsl_rng_env_setup()) ;

  gsl_ieee_env_setup ();

  prepare_distance_matrix();

  /* set up a trivial initial route */
  printf("# initial order of cities:\n");
  for (i = 0; i < N_CITIES; ++i) {
    printf("# \"%s\"\n", cities[i].name);
    x_initial[i] = i;
  }

  printf("# distance matrix is:\n");
  print_distance_matrix();

  printf("# initial coordinates of cities (longitude and latitude)\n");
  /* this can be plotted with */
  /* ./siman_tsp > hhh ; grep city_coord hhh | awk '{print $2 "   " $3}' | xyplot -ps -d "xy" > c.eps */
  for (i = 0; i < N_CITIES+1; ++i) {
    printf("###initial_city_coord: %g %g \"%s\"\n",
	   -cities[x_initial[i % N_CITIES]].longitude,
	   cities[x_initial[i % N_CITIES]].lat,
	   cities[x_initial[i % N_CITIES]].name);
  }

/*   exhaustive_search(); */

  gsl_siman_solve(r, x_initial, Etsp, Stsp, Mtsp, Ptsp, NULL, NULL, NULL,
		  N_CITIES*sizeof(int), params);

  printf("# final order of cities:\n");
  for (i = 0; i < N_CITIES; ++i) {
    printf("# \"%s\"\n", cities[x_initial[i]].name);
  }

  printf("# final coordinates of cities (longitude and latitude)\n");
  /* this can be plotted with */
  /* ./siman_tsp > hhh ; grep city_coord hhh | awk '{print $2 "   " $3}' | xyplot -ps -d "xy" > c.eps */
  for (i = 0; i < N_CITIES+1; ++i) {
    printf("###final_city_coord: %g %g %s\n",
	   -cities[x_initial[i % N_CITIES]].longitude,
	   cities[x_initial[i % N_CITIES]].lat,
	   cities[x_initial[i % N_CITIES]].name);
  }

  printf("# ");
  fflush(stdout);
#if 0
  system("date");
#endif /* 0 */
  fflush(stdout);

  return 0;
}

void prepare_distance_matrix()
{
  unsigned int i, j;
  double dist;

  for (i = 0; i < N_CITIES; ++i) {
    for (j = 0; j < N_CITIES; ++j) {
      if (i == j) {
	dist = 0;
      } else {
	dist = city_distance(cities[i], cities[j]);
      }
      distance_matrix[i][j] = dist;
    }
  }
}

void print_distance_matrix()
{
  unsigned int i, j;

  for (i = 0; i < N_CITIES; ++i) {
    printf("# ");
    for (j = 0; j < N_CITIES; ++j) {
      printf("%15.8f   ", distance_matrix[i][j]);
    }
    printf("\n");
  }
}

/* [only works for 12] search the entire space for solutions */
static double best_E = 1.0e100, second_E = 1.0e100, third_E = 1.0e100;
static int best_route[N_CITIES];
static int second_route[N_CITIES];
static int third_route[N_CITIES];
static void do_all_perms(int *route, int n);

void exhaustive_search()
{
  static int initial_route[N_CITIES] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11};
  printf("\n# ");
  fflush(stdout);
#if 0
  system("date");
#endif
  fflush(stdout);
  do_all_perms(initial_route, 1);
  printf("\n# ");
  fflush(stdout);
#if 0
  system("date");
#endif /* 0 */
  fflush(stdout);

  printf("# exhaustive best route: ");
  Ptsp(best_route);
  printf("\n# its energy is: %g\n", best_E);

  printf("# exhaustive second_best route: ");
  Ptsp(second_route);
  printf("\n# its energy is: %g\n", second_E);

  printf("# exhaustive third_best route: ");
  Ptsp(third_route);
  printf("\n# its energy is: %g\n", third_E);
}

/* James Theiler's recursive algorithm for generating all routes */
static void do_all_perms(int *route, int n)
{
  if (n == (N_CITIES-1)) {
    /* do it! calculate the energy/cost for that route */
    double E;
    E = Etsp(route);		/* TSP energy function */
    /* now save the best 3 energies and routes */
    if (E < best_E) {
      third_E = second_E;
      memcpy(third_route, second_route, N_CITIES*sizeof(*route));
      second_E = best_E;
      memcpy(second_route, best_route, N_CITIES*sizeof(*route));
      best_E = E;
      memcpy(best_route, route, N_CITIES*sizeof(*route));
    } else if (E < second_E) {
      third_E = second_E;
      memcpy(third_route, second_route, N_CITIES*sizeof(*route));
      second_E = E;
      memcpy(second_route, route, N_CITIES*sizeof(*route));
    } else if (E < third_E) {
      third_E = E;
      memcpy(route, third_route, N_CITIES*sizeof(*route));
    }
  } else {
    int new_route[N_CITIES];
    unsigned int j;
    int swap_tmp;
    memcpy(new_route, route, N_CITIES*sizeof(*route));
    for (j = n; j < N_CITIES; ++j) {
      swap_tmp = new_route[j];
      new_route[j] = new_route[n];
      new_route[n] = swap_tmp;
      do_all_perms(new_route, n+1);
    }
  }
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>

double f  (double x,void *params);
double f  (double x,void *params)
{
        double n=* (double *) params;
        double f=exp (-x) * pow (x,n);
        return f;
}

void integral_gen (int nmin, int nmax, double vals[]);
void integral_gen (int nmin, int nmax, double vals[])
{
	double a=0.,b=1;
        double abserr=1.e-9,relerr=1.e-9;
        double result;
        double error;
        double n=10;
        size_t np=1000;
	 gsl_integration_workspace *w = gsl_integration_workspace_alloc (np);
                gsl_function F;
                F.function = &f;

	for (int i=nmin;i<=nmax;i++)
	{
        	F.params =&n;
        	gsl_integration_qag (&F, a, b, abserr, relerr,np,GSL_INTEG_GAUSS15, w, &result, &error);
		vals[i]=result;
	}
	gsl_integration_workspace_free (w);
}


#include <petsc.h>
#include <petscmath.h>
#include "compressibleFlow.h"
#include "mesh.h"
#include "petscdmplex.h"
#include "petscts.h"

typedef struct {
    PetscInt dim;
    PetscReal k;
    PetscReal gamma;
    PetscReal Rgas;
    PetscReal L;
} Constants;

typedef struct {
    Constants constants;
    FlowData flowData;
} ProblemSetup;

static PetscReal ComputeTExact( PetscReal time, const PetscReal xyz[], Constants *constants, PetscReal rho){

    PetscReal cv = constants->gamma*constants->Rgas/(constants->gamma - 1) - constants->Rgas;

    PetscReal alpha = constants->k/(rho*cv);
    PetscReal Tinitial = 100.0;
    PetscReal T = 300.0;
    for(PetscReal n =1; n < 2000; n ++){
        PetscReal Bn = -Tinitial*2.0*(-1.0 + PetscPowReal(-1.0, n))/(n*PETSC_PI);
        T += Bn*PetscSinReal(n * PETSC_PI*xyz[0]/constants->L)*PetscExpReal(-n*n*PETSC_PI*PETSC_PI*alpha*time/(PetscSqr(constants->L)));
    }

    return T;
}

static PetscErrorCode InitialConditions(PetscInt dim, PetscReal time, const PetscReal xyz[], PetscInt Nf, PetscScalar *node, void *ctx) {
    PetscFunctionBeginUser;

    Constants *constants = (Constants *)ctx;

    PetscReal T = ComputeTExact(time, xyz, constants, 1.0);

    PetscReal u = 0.0;
    PetscReal v = xyz[0];
    PetscReal rho = 1.0;
    PetscReal p= rho*constants->Rgas*T;
    PetscReal e = p/((constants->gamma - 1.0)*rho);
    PetscReal eT = e + 0.5*(u*u + v*v);

    node[RHO] = rho;
    node[RHOE] = rho*eT;
    node[RHOU + 0] = rho*u;
    node[RHOU + 1] = rho*v;

    PetscFunctionReturn(0);
}

static PetscReal computeTemperature(PetscInt dim, const PetscScalar* conservedValues, PetscReal gamma, PetscReal Rgas){
    PetscReal density = conservedValues[RHO];
    PetscReal totalEnergy = conservedValues[RHOE]/density;

    // Get the velocity in this direction
    PetscReal speedSquare = 0.0;
    for (PetscInt d =0; d < dim; d++){
        speedSquare += PetscSqr(conservedValues[RHOU + d]/density);
    }

    // assumed eos
    PetscReal internalEnergy = (totalEnergy) - 0.5 * speedSquare;
    PetscReal p = (gamma - 1.0)*density*internalEnergy;

    PetscReal T = p/(Rgas*density);

    return T;
}

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

    FlowData flowData = (FlowData)ctx;

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

    ierr = FlowViewFromOptions(flowData, "-sol_view");
    CHKERRQ(ierr);

    ierr = PetscPrintf(PetscObjectComm((PetscObject)dm), "TS at %f\n", time);
    CHKERRQ(ierr);

    // Compute the error
    void *exactCtxs[1];
    PetscErrorCode (*exactFuncs[1])(PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx);
    PetscDS ds;
    ierr = DMGetDS(dm, &ds);
    CHKERRQ(ierr);

    // Get the exact solution
    ierr = PetscDSGetExactSolution(ds, 0, &exactFuncs[0], &exactCtxs[0]);
    CHKERRQ(ierr);

    // Create an vector to hold the exact solution
    Vec exactVec;
    ierr = VecDuplicate(u, &exactVec);
    CHKERRQ(ierr);
    ierr = DMProjectFunction(dm, time, exactFuncs, exactCtxs, INSERT_ALL_VALUES, exactVec);
    CHKERRQ(ierr);

    ierr = PetscObjectSetName((PetscObject)exactVec, "exact");
    CHKERRQ(ierr);
    ierr = VecViewFromOptions(exactVec, NULL, "-sol_view");
    CHKERRQ(ierr);

    // For each component, compute the l2 norms
    ierr = VecAXPY(exactVec, -1.0, u);
    CHKERRQ(ierr);

    PetscReal ferrors[4];
    ierr = VecSetBlockSize(exactVec, 4);
    CHKERRQ(ierr);

    ierr = PetscPrintf(PETSC_COMM_WORLD, "Timestep: %04d time = %-8.4g \t\n", (int)step, (double)time);
    CHKERRQ(ierr);
    ierr = VecStrideNormAll(exactVec, NORM_2, ferrors);
    CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_2 Error: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double)(ferrors[0]), (double)(ferrors[1]), (double)(ferrors[2]), (double)(ferrors[3]));
    CHKERRQ(ierr);

    // And the infinity error
    ierr = VecStrideNormAll(exactVec, NORM_INFINITY, ferrors);
    CHKERRQ(ierr);
    ierr = PetscPrintf(PETSC_COMM_WORLD, "\tL_Inf Error: [%2.3g, %2.3g, %2.3g, %2.3g]\n", (double)ferrors[0], (double)ferrors[1], (double)ferrors[2], (double)ferrors[3]);
    CHKERRQ(ierr);

    ierr = PetscObjectSetName((PetscObject)exactVec, "error");
    CHKERRQ(ierr);
    ierr = VecViewFromOptions(exactVec, NULL, "-sol_view");
    CHKERRQ(ierr);

    ierr = VecDestroy(&exactVec);
    CHKERRQ(ierr);
    PetscFunctionReturn(0);
}
//
//static PetscErrorCode FillBoundary(PetscInt dim, PetscInt dof, const PetscFVFaceGeom *faceGeom, const PetscFVCellGeom *cellGeom, const PetscFVCellGeom *cellGeomG, const PetscScalar *a_xI, const PetscScalar *a_xGradI, const PetscScalar *a_xG,  PetscScalar *a_xGradG, void *ctx){
//
//    for(PetscInt pd = 0; pd < dof; ++pd) {
//        PetscReal dPhidS = a_xG[pd] - a_xI[pd];
//
//        // over each direction
//        for (PetscInt dir = 0; dir < dim; dir++) {
//            PetscReal dx = (cellGeomG->centroid[dir] - faceGeom->centroid[dir]);
//
//            // If there is a contribution in this direction
//            if (PetscAbs(dx) > 1E-8) {
//                a_xGradG[pd*dim + dir] = dPhidS / (dx);
//            } else {
//                a_xGradG[pd*dim + dir] = 0.0;
//            }
//        }
//    }
//    return 0;
//}
//
///**
// * this function updates the boundaries with the gradient computed from the boundary cell value
// * @param dm
// * @param auxFvm
// * @param gradLocalVec
// * @return
// */
//static PetscErrorCode DMPlexFillGradientInBoundaryFVM(DM dm, PetscFV auxFvm, Vec localXVec, Vec gradLocalVec){
//    PetscFunctionBeginUser;
//    PetscErrorCode ierr;
//
//    // Get the dmGrad
//    DM dmGrad;
//    ierr = VecGetDM(gradLocalVec, &dmGrad);CHKERRQ(ierr);
//
//    // get the problem
//    PetscDS prob;
//    PetscInt nFields;
//    ierr = DMGetDS(dm, &prob);CHKERRQ(ierr);
//    ierr = PetscDSGetNumFields(prob, &nFields);CHKERRQ(ierr);
//    if (nFields > 1){
//        SETERRQ(PetscObjectComm((PetscObject) dm), PETSC_ERR_ARG_WRONG, "Cannot fill DMPlexFillGradientInBoundaryFVM with more than a single field." );
//    }
//    PetscInt field;
//    ierr = PetscDSGetFieldIndex(prob, (PetscObject) auxFvm, &field);CHKERRQ(ierr);
//    PetscInt dof;
//    ierr = PetscDSGetFieldSize(prob, field, &dof);CHKERRQ(ierr);
//
//    // Obtaining local cell ownership
//    PetscInt cellStart, cellEnd, cEndInterior;
//    ierr = DMPlexGetHeightStratum(dm, 0, &cellStart, &cellEnd);CHKERRQ(ierr);
//    ierr = DMPlexGetGhostCellStratum(dm, &cEndInterior, NULL);CHKERRQ(ierr);
//    cEndInterior = cEndInterior < 0 ? cellEnd: cEndInterior;
//
//    // Get the fvm face and cell geometry
//    Vec cellGeomVec = NULL;/* vector of structs related to cell geometry*/
//    Vec faceGeomVec = NULL;/* vector of structs related to face geometry*/
//    ierr = DMPlexGetGeometryFVM(dm, &faceGeomVec, &cellGeomVec, NULL);CHKERRQ(ierr);
//
//    // get the dm for each geom type
//    DM dmFaceGeom, dmCellGeom;
//    ierr = VecGetDM(faceGeomVec, &dmFaceGeom);CHKERRQ(ierr);
//    ierr = VecGetDM(cellGeomVec, &dmCellGeom);CHKERRQ(ierr);
//
//    // extract the gradLocalVec
//    PetscScalar * gradLocalArray;
//    ierr = VecGetArray(gradLocalVec, &gradLocalArray);CHKERRQ(ierr);
//
//    const PetscScalar* localArray;
//    ierr = VecGetArrayRead(localXVec, &localArray);CHKERRQ(ierr);
//
//    // get the dim
//    PetscInt dim;
//    ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);
//
//    // extract the arrays for the face and cell geom, along with their dm
//    const PetscScalar *cellGeomArray;
//    ierr = VecGetArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//
//    const PetscScalar *faceGeomArray;
//    ierr = VecGetArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    // March over each face
//    for (PetscInt cell = cellStart; cell < cEndInterior; ++cell) {
//        // Get the face information
//        PetscInt               numFaces;
//        const PetscInt        *faces;
//        ierr = DMPlexGetConeSize(dm, cell, &numFaces);CHKERRQ(ierr);
//        ierr = DMPlexGetCone(dm, cell, &faces);CHKERRQ(ierr);
//
//        //March over each face
//        for(PetscInt f =0 ; f < numFaces; f++){
//            PetscBool boundary;
//            ierr = DMIsBoundaryPoint(dm, faces[f], &boundary);CHKERRQ(ierr);
//
//            // if this is on the boundary
//            if(boundary){
//                // get the boundary cell index
//                const PetscInt        *fcells;
//                ierr  = DMPlexGetSupport(dm, faces[f], &fcells);CHKERRQ(ierr);
//                PetscInt side  = (cell != fcells[0]); /* c is on left=0 or right=1 of face */
//                PetscInt ncell = fcells[!side];    /* the neighbor */
//
//                // get the face geom
//                const PetscFVFaceGeom  *faceGeom;
//                ierr  = DMPlexPointLocalRead(dmFaceGeom, faces[f], faceGeomArray, &faceGeom);CHKERRQ(ierr);
//
//                // get the cell centroid information
//                const PetscFVCellGeom       *cellGeom;
//                const PetscFVCellGeom       *cellGeomGhost;
//                ierr  = DMPlexPointLocalRead(dmCellGeom, cell, cellGeomArray, &cellGeom);CHKERRQ(ierr);
//                ierr  = DMPlexPointLocalRead(dmCellGeom, ncell, cellGeomArray, &cellGeomGhost);CHKERRQ(ierr);
//
//                // Read the local point
//                PetscScalar* boundaryGradCellValues;
//                ierr  = DMPlexPointLocalRef(dmGrad, ncell, gradLocalArray, &boundaryGradCellValues);CHKERRQ(ierr);
//
//                const PetscScalar*  cellGradValues;
//                ierr  = DMPlexPointLocalRead(dmGrad, cell, gradLocalArray, &cellGradValues);CHKERRQ(ierr);
//
//                const PetscScalar* boundaryCellValues;
//                ierr  = DMPlexPointLocalRead(dm, ncell, localArray, &boundaryCellValues);CHKERRQ(ierr);
//                const PetscScalar* cellValues;
//                ierr  = DMPlexPointLocalRead(dm, cell, localArray, &cellValues);CHKERRQ(ierr);
//
//                // compute the gradient for the boundary node and pass in
//                //const PetscReal *cellGeom, const PetscReal *cellGeomG, const PetscScalar *a_xI, const PetscScalar *a_xGradI, PetscScalar *a_xG,  PetscScalar *a_xGradG, void *ctx
//                ierr = FillBoundary(dim, dof, faceGeom, cellGeom, cellGeomGhost, cellValues, cellGradValues, boundaryCellValues, boundaryGradCellValues, NULL);CHKERRQ(ierr);
//            }
//        }
//    }
//
//    ierr = VecRestoreArrayRead(localXVec, &localArray);CHKERRQ(ierr);
//    ierr = VecRestoreArray(gradLocalVec, &gradLocalArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    PetscFunctionReturn(0);
//}
//
//static PetscErrorCode DiffusionSource(DM dm, PetscReal time, Vec locXVec, Vec globFVec, void *ctx){
//    // Call the flux calculation
//    PetscErrorCode ierr;
//
//    ProblemSetup *setup = (ProblemSetup *)ctx;
//
//    // call the base rhs function eval
//    ierr = DMPlexTSComputeRHSFunctionFVM(dm, time, locXVec, globFVec, setup->flowData);CHKERRQ(ierr);
//
//    // update the aux fields
//    ierr = UpdateAuxFields(NULL, locXVec, setup->flowData);CHKERRQ(ierr);
//
//    Constants constants = setup->constants;
//
//    // get the fvm field assuming that it is the first
//    PetscFV auxFvm;
//    ierr = DMGetField(setup->flowData->auxDm,0, NULL, (PetscObject*)&auxFvm);CHKERRQ(ierr);
//
//    PetscInt dim;
//    ierr = DMGetDimension(dm, &dim);CHKERRQ(ierr);
//
//    // Get the locXArray
//    const PetscScalar *locXArray;
//    ierr = VecGetArrayRead(locXVec, &locXArray);CHKERRQ(ierr);
//
//    // Get the fvm face and cell geometry
//    Vec cellGeomVec = NULL;/* vector of structs related to cell geometry*/
//    Vec faceGeomVec = NULL;/* vector of structs related to face geometry*/
//
//    // extract the fvm data
//    ierr = DMPlexGetGeometryFVM(setup->flowData->dm, &faceGeomVec, &cellGeomVec, NULL);CHKERRQ(ierr);
//
//    // Get the needed auxDm
//    DM auxFieldGradDM = NULL; /* dm holding the grad information */
//    ierr = DMPlexGetDataFVM(setup->flowData->auxDm, auxFvm, NULL, NULL, &auxFieldGradDM);CHKERRQ(ierr);
//    if(!auxFieldGradDM){
//        SETERRQ(PetscObjectComm((PetscObject)setup->flowData->auxDm), PETSC_ERR_ARG_WRONGSTATE, "The FVM method for aux variables must support computing gradients.");
//    }
//
//    // get the dm for each geom type
//    DM dmFaceGeom, dmCellGeom;
//    ierr = VecGetDM(faceGeomVec, &dmFaceGeom);CHKERRQ(ierr);
//    ierr = VecGetDM(cellGeomVec, &dmCellGeom);CHKERRQ(ierr);
//
//    // extract the arrays for the face and cell geom, along with their dm
//    const PetscScalar *faceGeomArray, *cellGeomArray;
//    ierr = VecGetArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecGetArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//
//    // Obtaining local cell and face ownership
//    PetscInt faceStart, faceEnd;
//    PetscInt cellStart, cellEnd;
//    ierr = DMPlexGetHeightStratum(dm, 1, &faceStart, &faceEnd);CHKERRQ(ierr);
//    ierr = DMPlexGetHeightStratum(dm, 0, &cellStart, &cellEnd);CHKERRQ(ierr);
//
//    // get the ghost label
//    DMLabel ghostLabel;
//    ierr = DMGetLabel(dm, "ghost", &ghostLabel);CHKERRQ(ierr);
//
//    // extract the localFArray from the locFVec
//    PetscScalar *fa;
//    Vec locFVec;
//    ierr = DMGetLocalVector(dm, &locFVec);CHKERRQ(ierr);
//    ierr = VecZeroEntries(locFVec);CHKERRQ(ierr);
//    ierr = VecGetArray(locFVec, &fa);CHKERRQ(ierr);
//
//    // create a global and local grad vector for the auxField
//    Vec gradGlobalVec, gradLocalVec;
//    ierr = DMCreateGlobalVector(auxFieldGradDM, &gradGlobalVec);CHKERRQ(ierr);
//    ierr = VecSet(gradGlobalVec, NAN);CHKERRQ(ierr);
//
//    // compute the global grad values
//    ierr = DMPlexReconstructGradientsFVM(setup->flowData->auxDm, setup->flowData->auxField, gradGlobalVec);CHKERRQ(ierr);
//
//    // Map to a local grad vector
//    ierr = DMCreateLocalVector(auxFieldGradDM, &gradLocalVec);CHKERRQ(ierr);
//    ierr = DMGlobalToLocalBegin(auxFieldGradDM, gradGlobalVec, INSERT_VALUES, gradLocalVec);CHKERRQ(ierr);
//    ierr = DMGlobalToLocalEnd(auxFieldGradDM, gradGlobalVec, INSERT_VALUES, gradLocalVec);CHKERRQ(ierr);
//
//    // fill the boundary conditions
//    ierr = DMPlexFillGradientInBoundaryFVM(setup->flowData->auxDm, auxFvm, setup->flowData->auxField,  gradLocalVec);CHKERRQ(ierr);
//
//    // access the local vector
//    const PetscScalar *localGradArray;
//    ierr = VecGetArrayRead(gradLocalVec,&localGradArray);CHKERRQ(ierr);
//
//    // march over each face
//    for (PetscInt face = faceStart; face < faceEnd; ++face) {
//        PetscFVFaceGeom       *fg;
//        PetscFVCellGeom       *cgL, *cgR;
//        const PetscScalar           *gradL;
//        const PetscScalar           *gradR;
//
//        // make sure that this is a valid face to check
//        PetscInt  ghost, nsupp, nchild;
//        ierr = DMLabelGetValue(ghostLabel, face, &ghost);CHKERRQ(ierr);
//        ierr = DMPlexGetSupportSize(dm, face, &nsupp);CHKERRQ(ierr);
//        ierr = DMPlexGetTreeChildren(dm, face, &nchild, NULL);CHKERRQ(ierr);
//        if (ghost >= 0 || nsupp > 2 || nchild > 0){
//            continue;// skip this face
//        }
//
//        // get the face geometry
//        ierr = DMPlexPointLocalRead(dmFaceGeom, face, faceGeomArray, &fg);CHKERRQ(ierr);
//
//        // Get the left and right cells for this face
//        const PetscInt        *faceCells;
//        ierr = DMPlexGetSupport(dm, face, &faceCells);CHKERRQ(ierr);
//
//        // get the cell geom for the left and right faces
//        ierr = DMPlexPointLocalRead(dmCellGeom, faceCells[0], cellGeomArray, &cgL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalRead(dmCellGeom, faceCells[1], cellGeomArray, &cgR);CHKERRQ(ierr);
//
//        // extract the cell grad
//        ierr = DMPlexPointLocalRead(auxFieldGradDM, faceCells[0], localGradArray, &gradL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalRead(auxFieldGradDM, faceCells[1], localGradArray, &gradR);CHKERRQ(ierr);
//
//        PetscInt f = 0;
//
//        // extract the field values
//        PetscScalar *xL, *xR,
//        ierr = DMPlexPointLocalFieldRead(dm, faceCells[0], f, locXArray, &xL);CHKERRQ(ierr);
//        ierr = DMPlexPointLocalFieldRead(dm, faceCells[1], f, locXArray, &xR);CHKERRQ(ierr);
//
//        // Compute the normal grad
//        PetscReal normalGrad = 0.0;
//        PetscInt dof = 0;
//        for (PetscInt d = 0; d < dim; ++d){
//            normalGrad += fg->normal[d]*0.5*(gradL[dof*dim + d] + gradR[dof*dim + d]);
//        }
//
//        // compute the normal heatFlux
//        PetscReal normalHeatFlux = -constants.k *normalGrad;
//
//        // Add to the source terms of f
//        PetscScalar    *fL = NULL, *fR = NULL;
//        ierr = DMLabelGetValue(ghostLabel,faceCells[0],&ghost);CHKERRQ(ierr);
//        if (ghost <= 0) {ierr = DMPlexPointLocalFieldRef(dm, faceCells[0], f, fa, &fL);CHKERRQ(ierr);}
//        ierr = DMLabelGetValue(ghostLabel,faceCells[1],&ghost);CHKERRQ(ierr);
//        if (ghost <= 0) {ierr = DMPlexPointLocalFieldRef(dm, faceCells[1], f, fa, &fR);CHKERRQ(ierr);}
//
//        if(fL){
//            fL[RHOE] -= normalHeatFlux/cgL->volume;
//        }
//        if(fR){
//            fR[RHOE] += normalHeatFlux/cgR->volume;
//        }
//    }
//
//    // Add the new locFVec to the globFVec
//    ierr = VecRestoreArray(locFVec, &fa);CHKERRQ(ierr);
//    ierr = DMLocalToGlobalBegin(dm, locFVec, INSERT_VALUES, globFVec);CHKERRQ(ierr);
//    ierr = DMLocalToGlobalEnd(dm, locFVec, INSERT_VALUES, globFVec);CHKERRQ(ierr);
//    ierr = DMRestoreLocalVector(dm, &locFVec);CHKERRQ(ierr);
//
//    // restore the arrays
//    ierr = VecRestoreArrayRead(gradLocalVec, &localGradArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(cellGeomVec, &cellGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(faceGeomVec, &faceGeomArray);CHKERRQ(ierr);
//    ierr = VecRestoreArrayRead(locXVec, &locXArray);CHKERRQ(ierr);
//
//    // destroy grad vectors
//    ierr = VecDestroy(&gradGlobalVec);CHKERRQ(ierr);
//    ierr = VecDestroy(&gradLocalVec);CHKERRQ(ierr);
//
////    VecView(globFVec, PETSC_VIEWER_STDOUT_WORLD);
//
//    PetscFunctionReturn(0);
//}

static PetscErrorCode PhysicsBoundary_Euler(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx) {
    PetscFunctionBeginUser;
    Constants *constants = (Constants *)ctx;

    // compute the centroid location of the real cell
    // Offset the calc assuming the cells are square
    PetscReal x[3];
    for(PetscInt i =0; i < constants->dim; i++){
//        x[i] = c[i] - n[i]*0.5;//TODO:zero boundary
        x[i] = c[i] + n[i]*0.5;//TODO:uniform grad
    }

    // compute the temperature
    PetscReal Tinside =   ComputeTExact(time, x, constants, 1.0);
    PetscReal boundaryValue = 300.0;

    PetscReal T = boundaryValue;//boundaryValue - (Tinside - boundaryValue);//TODO: fix uniform grad

//    PetscReal T = c[0] < constants->L/2.0 ? 300 : 400;

    PetscReal u = 0.0;
    PetscReal v = 0.0;
    PetscReal rho = 1.0;
    PetscReal p= rho*constants->Rgas*T;
    PetscReal e = p/((constants->gamma - 1.0)*rho);
    PetscReal eT = e + 0.5*(u*u + v*v);

    a_xG[RHO] = rho;
    a_xG[RHOE] = rho*eT;
    a_xG[RHOU + 0] = rho*u;
    a_xG[RHOU + 1] = rho*v;

    PetscFunctionReturn(0);
}

static PetscErrorCode PhysicsBoundary_Mirror(PetscReal time, const PetscReal *c, const PetscReal *n, const PetscScalar *a_xI, PetscScalar *a_xG, void *ctx) {
    PetscFunctionBeginUser;
    Constants *constants = (Constants *)ctx;

    // Offset the calc assuming the cells are square
    for(PetscInt f =0; f < RHOU + constants->dim; f++){
        a_xG[f] = a_xI[f];
    }
    PetscFunctionReturn(0);
}

int main(int argc, char **argv)
{
    // Setup the problem
    Constants constants;

    // sub sonic
    constants.dim = 2;
    constants.L = 0.1;
    constants.gamma = 1.4;
    constants.k = 0.3;
    constants.Rgas = 1.0;

    PetscErrorCode ierr;
    // create the mesh
    // setup the ts
    DM dm;                 /* problem definition */
    TS ts;                 /* timestepper */

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

    // Create a ts
    ierr = TSCreate(PETSC_COMM_WORLD, &ts);CHKERRQ(ierr);
    ierr = TSSetProblemType(ts, TS_NONLINEAR);CHKERRQ(ierr);
    ierr = TSSetType(ts, TSEULER);CHKERRQ(ierr);
    ierr = TSSetExactFinalTime(ts, TS_EXACTFINALTIME_MATCHSTEP);CHKERRQ(ierr);

    // Create a mesh
    // hard code the problem setup
    PetscReal start[] = {0.0, 0.0};
    PetscReal end[] = {constants.L, constants.L};
    PetscReal nxHeight = 3;
    PetscInt nx[] = {nxHeight, nxHeight};
    ierr = DMPlexCreateBoxMesh(PETSC_COMM_WORLD, constants.dim, PETSC_FALSE, nx, start, end, NULL, PETSC_TRUE, &dm);CHKERRQ(ierr);

    // Setup the flow data
    FlowData flowData;     /* store some of the flow data*/
    ierr = FlowCreate(&flowData);CHKERRQ(ierr);

    // Combine the flow data
    ProblemSetup problemSetup;
    problemSetup.flowData = flowData;
    problemSetup.constants = constants;

    //Setup
    CompressibleFlow_SetupDiscretization(flowData, &dm);

    // Add in the flow parameters
    PetscScalar params[TOTAL_COMPRESSIBLE_FLOW_PARAMETERS];
    params[CFL] = 0.5;
    params[GAMMA] = constants.gamma;
    params[RGAS] = constants.Rgas;
    params[K] = constants.k;

    // set up the finite volume fluxes
    CompressibleFlow_StartProblemSetup(flowData, TOTAL_COMPRESSIBLE_FLOW_PARAMETERS, params);

    // Add in any boundary conditions
    PetscDS prob;
    ierr = DMGetDS(flowData->dm, &prob);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    const PetscInt idsLeft[]= {2, 4};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "wall left", "Face Sets", 0, 0, NULL, (void (*)(void))PhysicsBoundary_Euler, NULL, 2, idsLeft, &constants);CHKERRQ(ierr);

    const PetscInt idsTop[]= {1, 3};
    ierr = PetscDSAddBoundary(prob, DM_BC_NATURAL_RIEMANN, "top/bottom", "Face Sets", 0, 0, NULL, (void (*)(void))PhysicsBoundary_Mirror, NULL, 2, idsTop, &constants);CHKERRQ(ierr);

//    ierr = FlowRegisterAuxField(flowData, "Temperature", "T", 1, FV);CHKERRQ(ierr);

    // Complete the problem setup
    ierr = CompressibleFlow_CompleteProblemSetup(flowData, ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Override the flow calc for now
//    ierr = DMTSSetRHSFunctionLocal(flowData->dm, DiffusionSource, &problemSetup);CHKERRQ(ierr);

    // Name the flow field
    ierr = PetscObjectSetName(((PetscObject)flowData->flowField), "Numerical Solution");
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // Setup the TS
    ierr = TSSetFromOptions(ts);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);
    ierr = TSMonitorSet(ts, MonitorError, problemSetup.flowData, NULL);CHKERRQ(ierr);
    CHKERRABORT(PETSC_COMM_WORLD, ierr);

    // set the initial conditions
    PetscErrorCode     (*func[2]) (PetscInt dim, PetscReal time, const PetscReal x[], PetscInt Nf, PetscScalar *u, void *ctx) = {InitialConditions};
    void* ctxs[1] ={&constants};
    ierr    = DMProjectFunction(flowData->dm,0.0,func,ctxs,INSERT_ALL_VALUES,flowData->flowField);CHKERRQ(ierr);

    // for the mms, add the exact solution
    ierr = PetscDSSetExactSolution(prob, 0, InitialConditions, &constants);CHKERRQ(ierr);

    // Output the mesh
    ierr = DMViewFromOptions(flowData->dm, NULL, "-dm_view");CHKERRQ(ierr);

    // Compute the end time so it goes around once
    // compute dt
    PetscReal cv = constants.gamma*constants.Rgas/(constants.gamma - 1) - constants.Rgas;
    PetscReal dxMin = constants.L/(10 * PetscPowRealInt(2, 2));

    PetscReal alpha = PetscAbs(constants.k/ (cv * 1.0));
    double dt_conduc = 0.00000625;//.3*PetscSqr(dxMin) / alpha;

    PetscPrintf(PETSC_COMM_WORLD, "dt_conduc: %f\n", dt_conduc);
    TSSetTimeStep(ts, dt_conduc);
//    TSSetMaxTime(ts, 0.001);
    TSSetMaxSteps(ts, 600);

    PetscDSView(prob, PETSC_VIEWER_STDOUT_WORLD);

    DMView(flowData->auxDm, PETSC_VIEWER_STDOUT_WORLD);

    ierr = TSSolve(ts,flowData->flowField);CHKERRQ(ierr);

    FlowDestroy(&flowData);
    TSDestroy(&ts);

    return PetscFinalize();

}


/****************************************************************
  wigner_gsl.h / wigner_gsl_twice.h

  Defines Wigner coupling and recoupling symbols as wrappers for GSL
  angular momentum functions:

    - wigner_gsl.h -- takes HalfInt angular momentum arguments (RECOMMENDED)
    - wigner_gsl_twice.h -- takes integer "twice value" angular momentum arguments

  Naming convention:
    - Function names *not* ending in '2' accept HalfInt arguments J.
    - Function names ending in '2' accept integer arguments 2*J.

  See, e.g., appendix to de Shalit and Talmi for underlying formulas.

  DOCUMENTATION: See wigner_gsl.h (rather than wigner_gsl_twice.h) for
  more complete function comments.

  Language: C++

  Mark A. Caprio
  University of Notre Dame

  2/16/10 (mac): Created.
  11/13/15 (mac): Add unitary 6J for (12)3-(13)2 recoupling
    and Racah reduction factor.
  2/27/16 (mac): Update includes for restructured header files.
  3/8/16 (mac): Enclose in namespace.
  6/8/16 (mac): Update #define guard directive.
  6/21/16 (mac): Remove Racah reduction factor. Update comments.
  10/18/16 (mac): Update Unitary6J comment. Rename wigner2_gsl.h to
    wigner_gsl_twice.h.
  4/28/18 (mac): Restore missing Hat2 and ParitySign2 to
    wigner_gsl_twice.h.

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

#ifndef WIGNER_GSL_H_
#define WIGNER_GSL_H_

#include <gsl/gsl_sf_coupling.h>

#include "am.h"

namespace am {

  // Wigner3J(ja,jb,jc,ma,mb,mc)
  //   returns Wigner 3-J symbol
  //   wrapper for gsl_sf_coupling_3j

  inline
    double Wigner3J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& ma, const HalfInt& mb, const HalfInt& mc
      )
  {
    return gsl_sf_coupling_3j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(ma), TwiceValue(mb), TwiceValue(mc)
      );
  }

  // ClebschGordan(ja,ma,jb,mb,jc,mc)
  //   returns Clebsch-Gordan coefficient
  //   wrapper for gsl_sf_coupling_3j

  inline
    double ClebschGordan(
        const HalfInt& ja, const HalfInt& ma,
        const HalfInt& jb, const HalfInt& mb,
        const HalfInt& jc, const HalfInt& mc
      )
  {
    return Hat(jc)*ParitySign(ja-jb+mc)
      *gsl_sf_coupling_3j(
          TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
          TwiceValue(ma), TwiceValue(mb), -TwiceValue(mc)
        );
  }

  // Wigner6J(ja,jb,jc,jd,je,jf)
  //   returns Wigner 6-J symbol
  //   wrapper for gsl_sf_coupling_6j

  inline
    double Wigner6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return gsl_sf_coupling_6j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf)
      );
  }

  // Unitary6J(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-1(23) recoupling
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6J(J1,J2,J12,J3,J,J23)
  //   This is equivalent to U(J1,J2,J,J3,J12,J23), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J23,J)?!

  inline
    double Unitary6J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(ja+jb+jd+je)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Unitary6JZ(ja,jb,jc,jd,je,jf)
  //   wrapper for gsl_sf_coupling_6j
  //   returns unitary recoupling symbol for (12)3-(13)2 recoupling
  //   the name "Z" follows Millener for this purpose
  //
  //   Arguments follow row order in 6J symbol:
  //     Unitary6JZ(J1,J2,J12,J,J3,J13)
  //   This is equivalent to Z(J2,J1,J,J3,J12,J13), though neither is a
  //   particularly memorable ordering.  Why not (J1,J2,J3,J12,J13,J)?!

  inline
    double Unitary6JZ(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf
      )
  {
    return ParitySign(jb+je+jc+jf)*Hat(jc)*Hat(jf)
      *Wigner6J(ja,jb,jc,jd,je,jf);
  }

  // Wigner9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns Wigner 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Wigner9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return gsl_sf_coupling_9j(
        TwiceValue(ja), TwiceValue(jb), TwiceValue(jc),
        TwiceValue(jd), TwiceValue(je), TwiceValue(jf),
        TwiceValue(jg), TwiceValue(jh), TwiceValue(ji)
      );
  }

  // Unitary9J(ja,jb,jc,jd,je,jf,jg,jh,ji)
  //   returns unitary 9-J symbol
  //   wrapper for gsl_sf_coupling_9j

  inline
    double Unitary9J(
        const HalfInt& ja, const HalfInt& jb, const HalfInt& jc,
        const HalfInt& jd, const HalfInt& je, const HalfInt& jf,
        const HalfInt& jg, const HalfInt& jh, const HalfInt& ji
      )
  {
    return Hat(jc)*Hat(jf)*Hat(jg)*Hat(jh)
      *Wigner9J(
          ja, jb, jc,
          jd, je, jf,
          jg, jh, ji
        );
  }

} // namespace

#endif


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_spline.h>

int
main(void)
{
  size_t i;
  const size_t N = 9;

  /* this dataset is taken from
   * J. M. Hyman, Accurate Monotonicity preserving cubic interpolation,
   * SIAM J. Sci. Stat. Comput. 4, 4, 1983. */
  const double x[] = { 7.99, 8.09, 8.19, 8.7, 9.2,
                       10.0, 12.0, 15.0, 20.0 };
  const double y[] = { 0.0, 2.76429e-5, 4.37498e-2,
                       0.169183, 0.469428, 0.943740,
                       0.998636, 0.999919, 0.999994 };

  gsl_interp_accel *acc = gsl_interp_accel_alloc();
  gsl_spline *spline_cubic = gsl_spline_alloc(gsl_interp_cspline, N);
  gsl_spline *spline_akima = gsl_spline_alloc(gsl_interp_akima, N);
  gsl_spline *spline_steffen = gsl_spline_alloc(gsl_interp_steffen, N);

  gsl_spline_init(spline_cubic, x, y, N);
  gsl_spline_init(spline_akima, x, y, N);
  gsl_spline_init(spline_steffen, x, y, N);

  for (i = 0; i < N; ++i)
    printf("%g %g\n", x[i], y[i]);

  printf("\n\n");

  for (i = 0; i <= 100; ++i)
    {
      double xi = (1 - i / 100.0) * x[0] + (i / 100.0) * x[N-1];
      double yi_cubic = gsl_spline_eval(spline_cubic, xi, acc);
      double yi_akima = gsl_spline_eval(spline_akima, xi, acc);
      double yi_steffen = gsl_spline_eval(spline_steffen, xi, acc);

      printf("%g %g %g %g\n", xi, yi_cubic, yi_akima, yi_steffen);
    }

  gsl_spline_free(spline_cubic);
  gsl_spline_free(spline_akima);
  gsl_spline_free(spline_steffen);
  gsl_interp_accel_free(acc);

  return 0;
}


//
//  her.h
//  Linear Algebra Template Library
//
//  Created by Rodney James on 12/23/11.
//  Copyright (c) 2011 University of Colorado Denver. All rights reserved.
//

#ifndef _her_h
#define _her_h

/// @file her.h Performs complex vector outer product.

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

namespace LATL
{
   /// @brief Performs a vector outer product of a complex vector with itself.
   ///
   /// For a complex Hermitian matrix A, complex vector x and real scalar alpha,
   ///
   ///        A := alpha * x * x' + A
   ///
   /// is computed.  The result is Hermitian and is stored as either upper or lower triangular in A.
   /// @return 0 if success.
   /// @return -i if the ith argument is invalid.
   /// @tparam real_t Floating point type.
   /// @param uplo Specifies whether A is stored as upper or lower triangular.
   ///
   ///        if uplo = 'U' or 'u' then A is upper triangular
   ///        if uplo = 'L' or 'l' then A is lower triangular
   ///
   /// @param n Specifies the order of the matrix A.  n>=0
   /// @param alpha Real scalar.
   /// @param x Pointer to complex vector x.
   /// @param incx Increment of the vector x.  x!=0
   /// @param A Pointer to complex Hermitian n-by-n matrix A.  If uplo = 'U' or 'u' then only the upper
   /// triangular part of A is referenced and the lower part is not referenced.  If uplo = 'L' or 'l' then
   /// only the lower triangular part of A is referenced and the upper part is not referenced.
   /// @param ldA Column length of matrix A.  ldA>=n.
   /// @ingroup BLAS

   template <typename real_t>
   int HER(char uplo, int_t n, real_t alpha, complex<real_t> *x, int_t incx, complex<real_t> *A, int_t ldA)
   {
      using std::conj;
      using std::real;
      using std::toupper;
      const real_t zero(0.0);
      complex<real_t> t;
      int_t i,j,kx,jx,ix;

      uplo=toupper(uplo);

      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;
      else if((n==0)||(alpha==zero))
         return 0;

      if(incx==1)
      {
         if(uplo=='U')
         {
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[j]);
               for(i=0;i<j;i++)
                  A[i]+=x[i]*t;
               A[j]=real(A[j])+real(x[j]*t);
               A+=ldA;
            }
         }
         else
         {
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[j]);
               A[j]=real(A[j])+real(t*x[j]);
               for(i=j+1;i<n;i++)
                  A[i]+=x[i]*t;
               A+=ldA;
            }
         }
      }
      else
      {
         kx=(incx>0)?0:(1-n)*incx;
         if(uplo=='U')
         {
            jx=kx;
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[jx]);
               ix=kx;
               for(i=0;i<j;i++)
               {
                  A[i]+=x[ix]*t;
                  ix+=incx;
               }
               A[j]=real(A[j])+real(x[jx]*t);
               A+=ldA;
               jx+=incx;
            }
         }
         else
         {
            jx=kx;
            for(j=0;j<n;j++)
            {
               t=alpha*conj(x[jx]);
               A[j]=real(A[j])+real(t*x[jx]);
               ix=jx;
               for(i=j+1;i<n;i++)
               {
                  ix+=incx;
                  A[i]+=x[ix]*t;
               }
               jx+=incx;
               A+=ldA;
            }
         }
      }
      return 0;
   }

#ifdef __latl_cblas
#include <cblas.h>

   template <> int HER<float>(char uplo, int_t n, float alpha, complex<float> *x, int_t incx, complex<float> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_cher(CblasColMajor,Uplo,n,alpha,x,incx,A,ldA);

      return 0;
   }

   template <> int HER<double>(char uplo, int_t n, double alpha, complex<double> *x, int_t incx, complex<double> *A, int_t ldA)
   {
      uplo=std::toupper(uplo);
      if((uplo!='U')&&(uplo!='L'))
         return -1;
      else if(n<0)
         return -2;
      else if(incx==0)
         return -5;
      else if(ldA<n)
         return -7;

      const CBLAS_UPLO Uplo=(uplo=='U')?CblasUpper:CblasLower;

      cblas_zher(CblasColMajor,Uplo,n,alpha,x,incx,A,ldA);

      return 0;
   }
#endif
}
#endif


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

struct quadratic_params
{
    double a, b, c;
};

double
quadratic (double x, void *params)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;
    double c = p->c;

    return (a * x + b) * x + c;
}

double
quadratic_deriv (double x, void *params)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;

    return 2.0 * a * x + b;
}

void
quadratic_fdf (double x, void *params,
               double *y, double *dy)
{
    struct quadratic_params *p
            = (struct quadratic_params *) params;

    double a = p->a;
    double b = p->b;
    double c = p->c;

    *y = (a * x + b) * x + c;
    *dy = 2.0 * a * x + b;
}

int
main (int argc, char *argv[])
{
    int status;
    int iter = 0, max_iter = 100;
    const gsl_root_fdfsolver_type *T;
    gsl_root_fdfsolver *s;
    double x0, x = 5.0, r_expected = sqrt (5.0);
    gsl_function_fdf FDF;
    struct quadratic_params params = {1.0, -2.0, 1.0};

    FDF.f = &quadratic;
    FDF.df = &quadratic_deriv;
    FDF.fdf = &quadratic_fdf;
    FDF.params = &params;

    if (argc < 2) {
        errno = EINVAL;
        perror("");
        return errno;
    }
    char *method = argv[1];
    if (0 == strcmp("newton", method)) {
        T = gsl_root_fdfsolver_newton;
    } else if (0 == strcmp("secant", method)) {
        T = gsl_root_fdfsolver_secant;
    } else if (0 == strcmp("steffenson", method)) {
        T = gsl_root_fdfsolver_steffenson;
    } else {
        errno = EINVAL;
        perror("");
        return errno;
    }
    s = gsl_root_fdfsolver_alloc (T);
    gsl_root_fdfsolver_set (s, &FDF, x);

    printf ("using %s method\n",
            gsl_root_fdfsolver_name (s));

    printf ("%-5s %10s %10s %10s\n",
            "iter", "root", "err", "err(est)");
    do
    {
        iter++;
        status = gsl_root_fdfsolver_iterate (s);
        x0 = x;
        x = gsl_root_fdfsolver_root (s);
        status = gsl_root_test_delta (x, x0, 0, 1e-3);

        if (status == GSL_SUCCESS)
            printf ("Converged:\n");

        printf ("%5d %10.7f %+10.7f %10.7f\n",
                iter, x, x - r_expected, x - x0);
    }
    while (status == GSL_CONTINUE && iter < max_iter);

    gsl_root_fdfsolver_free (s);
    return status;
}


#include <stdio.h>
#include <gsl/gsl_statistics.h>
#include <gsl/gsl_rng.h>

#define NUM 5

int main() {
  int i;
  double data[NUM];
  double mean, variance, largest, smallest;

  gsl_rng* r = gsl_rng_alloc(gsl_rng_default);
  for (i = 0; i < NUM; ++i) data[i] = gsl_rng_uniform(r);

  mean = gsl_stats_mean(data, 1, NUM);
  variance = gsl_stats_variance(data, 1, NUM);
  largest = gsl_stats_max(data, 1, NUM);
  smallest = gsl_stats_min(data, 1, NUM);

  printf("The data set is");
  for (i = 0; i < NUM; ++i) printf(" %6.3f", data[i]);
  printf("\n");

  printf("Mean:     %6.3f\n", mean);
  printf("Variance: %6.3f\n", variance);
  printf("Max value:%6.3f\n", largest);
  printf("Min value:%6.3f\n", smallest);

  return 0;
}


#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_chebyshev.h>
#include <gsl/gsl_fit.h>
#include <gsl/gsl_check_range.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_multifit_nlinear.h>
#include <gsl/gsl_blas_types.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_randist.h>

double funcExp(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return exp(pow(x, 2));
}

double funcAbs(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return fabs(x + pow(x, 3));
}

double funcSign(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    if (x == 0) return 0.0;
    else if (x < 0) return -1.0;

    return 1.0;
}

void calculateExp()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_exp_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_exp_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_exp_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_exp_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_exp_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_exp_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_exp_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcExp;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void calculateAbs()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_abs_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_abs_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_abs_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_abs_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_abs_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_abs_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_abs_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcAbs;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void calculateSign()
{
    const int stepsCount = 1000;
    FILE* outputR1 = fopen("cheb_sign_r1.txt", "w");
    FILE* outputR2 = fopen("cheb_sign_r2.txt", "w");
    FILE* outputR4 = fopen("cheb_sign_r4.txt", "w");
    FILE* outputR8 = fopen("cheb_sign_r8.txt", "w");
    FILE* outputR16 = fopen("cheb_sign_r16.txt", "w");
    FILE* outputR25 = fopen("cheb_sign_r25.txt", "w");
    FILE* outputR40 = fopen("cheb_sign_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);

    gsl_function function;

    function.function = funcSign;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r1 = GSL_FN_EVAL(&function, x);
        double r2 = gsl_cheb_eval_n(cs, 2, x);
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r8 = gsl_cheb_eval_n(cs, 8, x);
        double r16 = gsl_cheb_eval_n(cs, 16, x);
        double r25 = gsl_cheb_eval_n(cs, 25, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);

        fprintf (outputR1,"%g %g\n", x, r1);
        fprintf (outputR2,"%g %g\n", x, r2);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR8,"%g %g\n", x, r8);
        fprintf (outputR16,"%g %g\n", x, r16);
        fprintf (outputR25,"%g %g\n", x, r25);
        fprintf (outputR40,"%g %g\n", x, r40);
    }

    gsl_cheb_free(cs);
}

void fitLinear()
{
    const int stepsCount = 1000;
    double xArray[stepsCount * 2];
    double yArray[stepsCount * 2];

    FILE* outputOriginal = fopen("linear_original.txt", "w");
    FILE* outputFit = fopen("linear_fit.txt", "w");
    FILE* outputCheb = fopen("linear_cheb.txt", "w");

    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double y = pow(0.5, pow(x, 2) + 2 * x);

        xArray[i + stepsCount] = x;
        yArray[i + stepsCount] = y;

        fprintf (outputOriginal,"%g %g\n", x, y);
    }

    double c0 = 0;
    double c1 = 0;
    double cov00 = 0;
    double cov01 = 0;
    double cov11 = 0;
    double sumsq = 0;
    gsl_fit_linear(xArray, 1, yArray, 1, stepsCount * 2, &c0, &c1, &cov00, &cov01, &cov11, &sumsq);

//------
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double y = c0 + c1 * x;

        xArray[i + stepsCount] = x;
        yArray[i + stepsCount] = y;

        fprintf (outputFit,"%g %g\n", x, y);
    }

    printf("c0: %f --- c1: %f\n", c0, c1);
}

double funcCheb(double x, void *p)
{
    (void)(p); //avoid unused parameter warning
    return pow(0.5, pow(x, 2) + 2 * x);
}

void fitCheb()
{
    const int stepsCount = 1000;
    FILE* outputR4 = fopen("cheb_fit_r4.txt", "w");
    FILE* outputR40 = fopen("cheb_fit_r40.txt", "w");

    gsl_cheb_series* cs = gsl_cheb_alloc(40);
    gsl_function function;
    function.function = funcCheb;
    function.params = 0;

    gsl_cheb_init(cs, &function, -1.0, 1.0);
    for (int i = -stepsCount; i <= stepsCount; i++)
    {
        double x = (double)i / (double)stepsCount;
        double r4 = gsl_cheb_eval_n(cs, 4, x);
        double r40 = gsl_cheb_eval_n(cs, 40, x);
        fprintf (outputR4,"%g %g\n", x, r4);
        fprintf (outputR40,"%g %g\n", x, r40);
    }
    gsl_cheb_free(cs);
}

int main (int argc, char* argv[])
{
    calculateExp();
    calculateSign();
    calculateAbs();
    fitLinear();
    fitCheb();
    return 0;
}

/*

plot "cheb_exp_r1.txt" with lines, \
    "cheb_exp_r2.txt" with lines, \
    "cheb_exp_r4.txt" with lines, \
    "cheb_exp_r8.txt" with lines, \
    "cheb_exp_r16.txt" with lines, \
    "cheb_exp_r25.txt" with lines, \
    "cheb_exp_r40.txt" with lines

plot "cheb_abs_r1.txt" with lines, \
    "cheb_abs_r2.txt" with lines, \
    "cheb_abs_r4.txt" with lines, \
    "cheb_abs_r8.txt" with lines, \
    "cheb_abs_r16.txt" with lines, \
    "cheb_abs_r25.txt" with lines, \
    "cheb_abs_r40.txt" with lines

plot "cheb_sign_r1.txt" with lines, \
    "cheb_sign_r2.txt" with lines, \
    "cheb_sign_r4.txt" with lines, \
    "cheb_sign_r8.txt" with lines, \
    "cheb_sign_r16.txt" with lines, \
    "cheb_sign_r25.txt" with lines, \
    "cheb_sign_r40.txt" with lines

================

plot "linear_original.txt" with lines title 'Original', \
    "linear_fit.txt" with lines title 'Linear least sqr fit', \
    "cheb_fit_r4.txt" with lines title 'Chebyshev approx R4'

*/


#include <stdio.h>
#include <gsl/gsl_statistics_double.h>
#include <gsl/gsl_matrix_double.h>
// #include "covar.h"

void print_mat(gsl_matrix* C)
{
    int i, j;
    for (i = 0; i < C->size1; i++)
    {
        printf("\n");
        for (j = 0; j < C->size2; j++)
        {
            printf("%f", C->data[C->size2*i+j]);
            if(j < C->size2 - 1) printf(", ");
        }
    }
    printf("\n");
}

int main()
{
    int i, j;

/*
    Okay, here's what we want to do
    1)  Collect *all* the data - 10 seconds at standstill should be sufficient, with
            x, y, z, yaw, pitch, roll; for both pose and twist
    2)  twist gives us euler angles (yaw pitch roll), but pose gives us quaternions,
            so we'll need to convert
    2)  This leads to a 6 by N matrix of values
    3)  We propose a cov_gen() function
            input: 6 by N matrix of observation values
            output: 6 by 6 matrix of covariance values
    4) We can put a ROS wrapper around this function as a service routine,
            i.e. the output of cov_gen() will serve as our estimate of
            the covariance matrix for loam_velodyne's odometry topic
 */

    // const int rsize = 5;
    // const int csize = 3;
    #define rsize 8
    #define csize 6

    double dataA[rsize][csize] =
        {{9, 6, 9, 3, 2, 1},
         {9, 9, 3, 4, 3, 5},
         {6, 6, 6, 6, 7, 3},
         {6, 6, 9, 4, 5, 1},
         {9, 9, 3, 4, 3, 5},
         {6, 6, 6, 6, 7, 3},
         {6, 6, 9, 4, 5, 1},
         {3, 3, 3, 6, 4, 1}};

    // double dataA[rsize][csize] =
    //     {{90, 60, 90},
    //      {90, 90, 30},
    //      {60, 60, 60},
    //      {60, 60, 90},
    //      {30, 30, 30}};

    // double dataA[5][3] =   {{ 24,   0,  30},
    //                         { 24,  30, -30},
    //                         { -6,   0,   0},
    //                         { -6,   0,  30},
    //                         {-36, -30, -30}};

    // double dataA[6][6] = {{1,1,1,1,1,1},
    //                       {2,2,2,2,2,2},
    //                       {3,3,3,3,3,3},
    //                       {4,4,4,4,4,4},
    //                       {5,5,5,5,5,5},
    //                       {6,6,6,6,6,6}};

    gsl_vector_view a, b;

    gsl_matrix *A, *C;
    A = gsl_matrix_alloc(rsize, csize);
    C = gsl_matrix_alloc(csize, csize);

    for ( i = 0; i < rsize; i++)
        for ( j = 0; j < csize; j++)
            gsl_matrix_set (A, i, j, dataA[i][j]);

    print_mat(A);

    for (i = 0; i < A->size2; i++) {
        for (j = 0; j < A->size2; j++) {
          a = gsl_matrix_column (A, i);
          b = gsl_matrix_column (A, j);
          double cov = gsl_stats_covariance(a.vector.data,
                                            a.vector.stride,
                                            b.vector.data,
                                            b.vector.stride,
                                            rsize);
          gsl_matrix_set (C, i, j, cov);
        }
      }

    print_mat(C);

    return 0;
}


/*----------------------------------------------------------------------------*/
/*                                                                            */
/*  quad_golden.c                                                             */
/*                                                                            */
/*  Copyright (C) 2007 James Howse                                            */
/*  Copyright (C) 2009 Brian Gough                                             */
/*                                                                            */
/*  This program is free software; you can redistribute it and/or modify      */
/*  it under the terms of the GNU General Public License as published by      */
/*  the Free Software Foundation; either version 3 of the License, or (at     */
/*  your option) any later version.                                           */
/*                                                                            */
/*  This program is distributed in the hope that it will be useful, but       */
/*  WITHOUT ANY WARRANTY; without even the implied warranty of                */
/*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU         */
/*  General Public License for more details.                                  */
/*                                                                            */
/*  You should have received a copy of the GNU General Public License         */
/*  along with this program; if not, write to the Free Software               */
/*  Foundation, Inc., 51 Franklin Street, Fifth Floor,                        */
/*  Boston, MA 02110-1301, USA.                                               */
/*                                                                            */
/*  ::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::  */
/*                                                                            */
/*  This algorithm performs univariate minimization (i.e., line search).      */
/*  It requires only objective function values g(x) to compute the minimum.   */
/*  The algorithm maintains an interval of uncertainty [a,b] and a point x    */
/*  in the interval [a,b] such that a < x < b, and g(a) > g(x) and            */
/*  g(x) < g(b).  The algorithm also maintains the three points with the      */
/*  smallest objective values x, v and w such that g(x) < g(v) < g(w).  The   */
/*  algorithm terminates when max( x - a, b - x ) <  2(r |x| + t) where r     */
/*  and t are small positive reals.  At a given iteration, the algorithm      */
/*  first fits a quadratic through the three points (x, g(x)), (v, g(v))      */
/*  and (w, g(w)) and computes the location of the minimum u of the           */
/*  resulting quadratic.  If u is in the interval [a,b] then g(u) is          */
/*  computed.  If u is not in the interval [a,b], and either v < x and        */
/*  w < x, or v > x and w > x (i.e., the quadratic is extrapolating), then    */
/*  a point u' is computed using a safeguarding procedure and g(u') is        */
/*  computed.  If u is not in the interval [a,b], and the quadratic is not    */
/*  extrapolating, then a point u'' is computed using approximate golden      */
/*  section and g(u'') is computed.  After evaluating g() at the              */
/*  appropriate new point, a, b, x, v, and w are updated and the next         */
/*  iteration is performed.  The algorithm is based on work presented in      */
/*  the following references.                                                 */
/*                                                                            */
/*  Algorithms for Minimization without derivatives                           */
/*  Richard Brent                                                             */
/*  Prentice-Hall Inc., Englewood Cliffs, NJ, 1973                            */
/*                                                                            */
/*  Safeguarded Steplength Algorithms for Optimization using Descent Methods  */
/*  Philip E. Gill and Walter Murray                                          */
/*  Division of Numerical Analysis and Computing                              */
/*  National Physical Laboratory, Teddington, United Kingdom                  */
/*  NPL Report NAC 37, August 1974                                            */
/*                                                                            */
/*----------------------------------------------------------------------------*/
#include <config.h>

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

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

#include "min.h"

#define REL_ERR_VAL   1.0e-06
#define ABS_ERR_VAL   1.0e-10
#define GOLDEN_MEAN   0.3819660112501052	/* (3 - sqrt(5))/2 */
#define GOLDEN_RATIO  1.6180339887498950	/* (1 + sqrt(5))/2 */

#define DEBUG_PRINTF(x) /* do nothing */

typedef struct
{
  double step_size, stored_step, prev_stored_step;
  double x_prev_small, f_prev_small, x_small, f_small;
  unsigned int num_iter;
}
quad_golden_state_t;

static int
quad_golden_init (void *vstate, gsl_function * f, double x_minimum,
		  double f_minimum, double x_lower, double f_lower,
		  double x_upper, double f_upper)
{
  quad_golden_state_t *state = (quad_golden_state_t *) vstate;

  /* For the original behavior, the first value for x_minimum_minimum
     passed in by the user should be a golden section step but we
     don't enforce this here. */

  state->x_prev_small = x_minimum;
  state->x_small = x_minimum;

  state->f_prev_small = f_minimum;
  state->f_small = f_minimum;

  state->step_size = 0.0;
  state->stored_step = 0.0;
  state->prev_stored_step = 0.0;
  state->num_iter = 0;

  x_lower = 0 ;  /* avoid warnings about unused variables */
  x_upper = 0 ;
  f_lower = 0 ;
  f_upper = 0 ;
  f = 0;

  return GSL_SUCCESS;
}

static int
quad_golden_iterate (void *vstate, gsl_function * f, double *x_minimum,
		     double *f_minimum, double *x_lower, double *f_lower,
		     double *x_upper, double *f_upper)
{
  quad_golden_state_t *state = (quad_golden_state_t *) vstate;

  const double x_m = *x_minimum;
  const double f_m = *f_minimum;

  const double x_l = *x_lower;
  const double x_u = *x_upper;

  const double x_small = state->x_small;
  const double f_small = state->f_small;

  const double x_prev_small = state->x_prev_small;
  const double f_prev_small = state->f_prev_small;

  double stored_step = state->stored_step; /* update on exit */
  double prev_stored_step = state->prev_stored_step; /* update on exit */
  double step_size = state->step_size; /* update on exit */

  double quad_step_size = prev_stored_step;

  double x_trial;
  double x_eval, f_eval;

  double x_midpoint = 0.5 * (x_l + x_u);
  double tol = REL_ERR_VAL * fabs (x_m) + ABS_ERR_VAL; /* total error tolerance */

  if (fabs (stored_step) - tol > -2.0 * GSL_DBL_EPSILON)
    {
      /* Fit quadratic */
      double c3 = (x_m - x_small) * (f_m - f_prev_small);
      double c2 = (x_m - x_prev_small) * (f_m - f_small);
      double c1 = (x_m - x_prev_small) * c2 - (x_m - x_small) * c3;

      c2 = 2.0 * (c2 - c3);

      if (fabs (c2) > GSL_DBL_EPSILON)	/* if( c2 != 0 ) */
	{
	  if (c2 > 0.0)
	    c1 = -c1;

	  c2 = fabs (c2);

	  quad_step_size = c1 / c2;
	}
      else
	{
	  /* Handle case where c2 ~=~ 0  */
	  /* Insure that the line search will NOT take a quadratic
	     interpolation step in this iteration */
	  quad_step_size = stored_step;
	}

      prev_stored_step = stored_step;
      stored_step = step_size;
    }

  x_trial = x_m + quad_step_size;

  if (fabs (quad_step_size) < fabs (0.5 * prev_stored_step) && x_trial > x_l && x_trial < x_u)
    {
      /* Take quadratic interpolation step */
      step_size = quad_step_size;

      /* Do not evaluate function too close to x_l or x_u */
      if ((x_trial - x_l) < 2.0 * tol || (x_u - x_trial) < 2.0 * tol)
        {
          step_size = (x_midpoint >= x_m ? +1.0 : -1.0) * fabs(tol);
        }

      DEBUG_PRINTF(("quadratic step: %g\n", step_size));
    }
  else if ((x_small != x_prev_small && x_small < x_m && x_prev_small < x_m) ||
           (x_small != x_prev_small && x_small > x_m && x_prev_small > x_m))
    {
      /* Take safeguarded function comparison step */
      double outside_interval, inside_interval;

      if (x_small < x_m)
	{
	  outside_interval = x_l - x_m;
	  inside_interval = x_u - x_m;
	}
      else
	{
	  outside_interval = x_u - x_m;
	  inside_interval = x_l - x_m;
	}

      if (fabs (inside_interval) <= tol)
	{
          /* Swap inside and outside intervals */
          double tmp = outside_interval;
	  outside_interval = inside_interval;
	  inside_interval = tmp;
	}

      {
        double step = inside_interval;
        double scale_factor;

        if (fabs (outside_interval) < fabs (inside_interval))
          {
            scale_factor = 0.5 * sqrt (-outside_interval / inside_interval);
          }
        else
          {
            scale_factor = (5.0 / 11.0) * (0.1 - inside_interval / outside_interval);
          }

        state->stored_step = step;
        step_size = scale_factor * step;
      }

      DEBUG_PRINTF(("safeguard step: %g\n", step_size));
    }
  else
    {
      /* Take golden section step */
      double step;

      if (x_m < x_midpoint)
        {
          step = x_u - x_m;
        }
      else
        {
          step = x_l - x_m;
        }

      state->stored_step = step;
      step_size = GOLDEN_MEAN * step;

      DEBUG_PRINTF(("golden step: %g\n", step_size));
    }

  /* Do not evaluate function too close to x_minimum */
  if (fabs (step_size) > tol)
    {
      x_eval = x_m + step_size;
    }
  else
    {
      x_eval = x_m + (step_size >= 0 ? +1.0 : -1.0) * fabs(tol);
    }

  /* Evaluate function at the new point x_eval */
  SAFE_FUNC_CALL(f, x_eval, &f_eval);

  /* Update {x,f}_lower, {x,f}_upper, {x,f}_prev_small, {x,f}_small, and {x,f}_minimum */
  if (f_eval <= f_m)
    {
      if (x_eval < x_m)
	{
          *x_upper = x_m;
          *f_upper = f_m;
        }
      else
      	{
          *x_lower = x_m;
          *f_upper = f_m;
        }

      state->x_prev_small = x_small;
      state->f_prev_small = f_small;

      state->x_small = x_m;
      state->f_small = f_m;

      *x_minimum = x_eval;
      *f_minimum = f_eval;
    }
  else
    {
      if (x_eval < x_m)
        {
          *x_lower = x_eval;
          *f_lower = f_eval;
        }
      else
        {
          *x_upper = x_eval;
          *f_upper = f_eval;
        }

      if (f_eval <= f_small || fabs (x_small - x_m) < 2.0 * GSL_DBL_EPSILON)
	{
	  state->x_prev_small = x_small;
	  state->f_prev_small = f_small;

	  state->x_small = x_eval;
	  state->f_small = f_eval;
	}
      else if (f_eval <= f_prev_small ||
	       fabs (x_prev_small - x_m) < 2.0 * GSL_DBL_EPSILON ||
	       fabs (x_prev_small - x_small) < 2.0 * GSL_DBL_EPSILON)
	{
	  state->x_prev_small = x_eval;
	  state->f_prev_small = f_eval;
	}
    }

  /* Update stored values for next iteration */

  state->stored_step = stored_step;
  state->prev_stored_step = prev_stored_step;
  state->step_size = step_size;
  state->num_iter++;

  DEBUG_PRINTF(("[%d] Final State: %g  %g  %g\n", state->num_iter, x_l, x_m, x_u));

  return GSL_SUCCESS;
}

static const gsl_min_fminimizer_type quad_golden_type = { "quad-golden",	/* name */
  sizeof (quad_golden_state_t),
  &quad_golden_init,
  &quad_golden_iterate
};

const gsl_min_fminimizer_type *gsl_min_fminimizer_quad_golden =
  &quad_golden_type;


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

#include <gsl/gsl_math.h>
#include <gsl/gsl_movstat.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_vector.h>

int
main(void)
{
  const size_t N = 500;                      /* length of time series */
  const size_t K = 11;                       /* window size */
  gsl_movstat_workspace * w = gsl_movstat_alloc(K);
  gsl_vector *x = gsl_vector_alloc(N);
  gsl_vector *xmean = gsl_vector_alloc(N);
  gsl_vector *xmin = gsl_vector_alloc(N);
  gsl_vector *xmax = gsl_vector_alloc(N);
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  size_t i;

  for (i = 0; i < N; ++i)
    {
      double xi = cos(4.0 * M_PI * i / (double) N);
      double ei = gsl_ran_gaussian(r, 0.1);

      gsl_vector_set(x, i, xi + ei);
    }

  /* compute moving statistics */
  gsl_movstat_mean(GSL_MOVSTAT_END_PADVALUE, x, xmean, w);
  gsl_movstat_minmax(GSL_MOVSTAT_END_PADVALUE, x, xmin, xmax, w);

  /* print results */
  for (i = 0; i < N; ++i)
    {
      printf("%zu %f %f %f %f\n",
             i,
             gsl_vector_get(x, i),
             gsl_vector_get(xmean, i),
             gsl_vector_get(xmin, i),
             gsl_vector_get(xmax, i));
    }

  gsl_vector_free(x);
  gsl_vector_free(xmean);
  gsl_rng_free(r);
  gsl_movstat_free(w);

  return 0;
}


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

#include <config.h>
#include <gsl/gsl_integration.h>

/* Gauss quadrature weights and kronrod quadrature abscissae and
   weights as evaluated with 80 decimal digit arithmetic by
   L. W. Fullerton, Bell Labs, Nov. 1981. */

static const double xgk[8] =    /* abscissae of the 15-point kronrod rule */
{
  0.991455371120812639206854697526329,
  0.949107912342758524526189684047851,
  0.864864423359769072789712788640926,
  0.741531185599394439863864773280788,
  0.586087235467691130294144838258730,
  0.405845151377397166906606412076961,
  0.207784955007898467600689403773245,
  0.000000000000000000000000000000000
};

/* xgk[1], xgk[3], ... abscissae of the 7-point gauss rule.
   xgk[0], xgk[2], ... abscissae to optimally extend the 7-point gauss rule */

static const double wg[4] =     /* weights of the 7-point gauss rule */
{
  0.129484966168869693270611432679082,
  0.279705391489276667901467771423780,
  0.381830050505118944950369775488975,
  0.417959183673469387755102040816327
};

static const double wgk[8] =    /* weights of the 15-point kronrod rule */
{
  0.022935322010529224963732008058970,
  0.063092092629978553290700663189204,
  0.104790010322250183839876322541518,
  0.140653259715525918745189590510238,
  0.169004726639267902826583426598550,
  0.190350578064785409913256402421014,
  0.204432940075298892414161999234649,
  0.209482141084727828012999174891714
};

void
gsl_integration_qk15 (const gsl_function * f, double a, double b,
      double *result, double *abserr,
      double *resabs, double *resasc)
{
  double fv1[8], fv2[8];
  gsl_integration_qk (8, xgk, wg, wgk, fv1, fv2, f, a, b, result, abserr, resabs, resasc);
}


static char help[] =
   "Newton's method for a two-variable system.  Implements analytical Jacobian.\n"
   "Adds struct to hold parameter.\n";

#include <petsc.h>

typedef struct {
  double  b;
} AppCtx;

extern PetscErrorCode FormFunction(SNES, Vec, Vec, void*);
extern PetscErrorCode FormJacobian(SNES, Vec, Mat, Mat, void*);

//STARTMAIN
int main(int argc,char **argv) {
  SNES   snes;         // nonlinear solver context
  Vec    x,r;          // solution, residual vectors
  Mat    J;
  AppCtx user;
  PetscErrorCode ierr;

  PetscInitialize(&argc,&argv,NULL,help);
  user.b = 2.0;

  ierr = VecCreate(PETSC_COMM_WORLD,&x); CHKERRQ(ierr);
  ierr = VecSetSizes(x,PETSC_DECIDE,2); CHKERRQ(ierr);
  ierr = VecSetFromOptions(x); CHKERRQ(ierr);
  ierr = VecDuplicate(x,&r); CHKERRQ(ierr);

  ierr = MatCreate(PETSC_COMM_WORLD,&J); CHKERRQ(ierr);
  ierr = MatSetSizes(J,PETSC_DECIDE,PETSC_DECIDE,2,2); CHKERRQ(ierr);
  ierr = MatSetFromOptions(J); CHKERRQ(ierr);
  ierr = MatSetUp(J); CHKERRQ(ierr);

  ierr = SNESCreate(PETSC_COMM_WORLD,&snes); CHKERRQ(ierr);
  ierr = SNESSetFunction(snes,r,FormFunction,&user);CHKERRQ(ierr);
  ierr = SNESSetJacobian(snes,J,J,FormJacobian,&user);CHKERRQ(ierr);
  ierr = SNESSetFromOptions(snes);CHKERRQ(ierr);

  ierr = VecSet(x,1.0);CHKERRQ(ierr);
  ierr = SNESSolve(snes,NULL,x);CHKERRQ(ierr);
  ierr = VecView(x,PETSC_VIEWER_STDOUT_WORLD); CHKERRQ(ierr);

  VecDestroy(&x);  VecDestroy(&r);  SNESDestroy(&snes);  MatDestroy(&J);
  return PetscFinalize();
}
//ENDMAIN

//STARTFORM
PetscErrorCode FormFunction(SNES snes, Vec x, Vec F, void *ctx) {
    PetscErrorCode ierr;
    AppCtx       *user = (AppCtx*)ctx;
    const double b = user->b, *ax;
    double       *aF;

    ierr = VecGetArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecGetArray(F,&aF);CHKERRQ(ierr);
    aF[0] = (1.0 / b) * PetscExpReal(b * ax[0]) - ax[1];
    aF[1] = ax[0] * ax[0] + ax[1] * ax[1] - 1.0;
    ierr = VecRestoreArrayRead(x,&ax);CHKERRQ(ierr);
    ierr = VecRestoreArray(F,&aF);CHKERRQ(ierr);
    return 0;
}

PetscErrorCode FormJacobian(SNES snes, Vec x, Mat J, Mat P, void *ctx) {
    PetscErrorCode ierr;
    AppCtx       *user = (AppCtx*)ctx;
    const double b = user->b, *ax;
    double       v[4];
    int          row[2] = {0,1}, col[2] = {0,1};

    ierr = VecGetArrayRead(x,&ax); CHKERRQ(ierr);
    v[0] = PetscExpReal(b * ax[0]);  v[1] = -1.0;
    v[2] = 2.0 * ax[0];              v[3] = 2.0 * ax[1];
    ierr = VecRestoreArrayRead(x,&ax); CHKERRQ(ierr);
    ierr = MatSetValues(P,2,row,2,col,v,INSERT_VALUES); CHKERRQ(ierr);
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}
//ENDFORM


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>
#include <cblas.h>
#include "linear_algebra.h"

Matrix *new_matrix(int row_num, int col_num)
{
    Matrix *matrix = malloc(sizeof(Matrix));
    if (matrix == NULL) return NULL;
    matrix->data = malloc((unsigned long) (row_num * col_num) * sizeof(double));

    if (matrix->data == NULL)
    {
        perror("Error allocating memory for a matrix");
        exit(EXIT_FAILURE);
    }

    matrix->row_num = row_num;
    matrix->col_num = col_num;

    return matrix;
}

Matrix *new_matrix_from_array(double *array, int row_num, int col_num)
{
   Matrix *matrix = new_matrix(row_num, col_num);
   memcpy(matrix->data, array, (unsigned long)(row_num * col_num) * sizeof(double));
   return matrix;
}

void free_matrix(Matrix *matrix)
{
    free(matrix->data);
    free(matrix);
}

Matrix *add_matrices(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->row_num != matrix2->row_num ||
        matrix1->col_num != matrix2->col_num)
    {
        perror("Dimensions of the two matrices do not match");
        exit(EXIT_FAILURE);
    }

    Matrix *result_matrix = new_matrix(matrix1->row_num, matrix1->col_num);

    int i;
    int elements = matrix1->row_num * matrix1->col_num;

    for (i = 0; i < elements; i++)
    {
        result_matrix->data[i] = matrix1->data[i] + matrix2->data[i];
    }

    return result_matrix;
}

Matrix *multiply_matrices(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->col_num != matrix2->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix1->row_num, matrix2->col_num);

    int m = matrix1->row_num;
    int n = matrix2->col_num;
    int k = matrix1->col_num;

    // Use high-performace matrix multiplication from BLAS
    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasNoTrans, m, n, k, 1,
        matrix1->data, k, matrix2->data, n, 0, product->data, n);

    return product;
}

void multiply_vector_by_vector_transpose(Matrix *vector1, Matrix *vector2, double scalar, Matrix *matrix)
{
    if (vector1->col_num != 1 || vector2->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    int m = vector1->row_num;
    int n = vector2->row_num;

    cblas_dger(CblasRowMajor, m, n, scalar, vector1->data, 1, vector2->data, 1, matrix->data, n);
}

Matrix *multiply_matrix_with_vector(Matrix *matrix, Matrix *vector, double scalar)
{
    if (matrix->col_num != vector->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    if (vector->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix->row_num, 1);

    int m = matrix->row_num;
    int n = matrix->col_num;

    cblas_dgemv(CblasRowMajor, CblasNoTrans, m, n, scalar, matrix->data, n,
                vector->data, 1,  0, product->data, 1);

    return product;
}

Matrix *multiply_upper_symmetric_matrix_with_vector(Matrix *matrix, Matrix *vector)
{
    if (matrix->col_num != matrix->row_num)
    {
        perror("Matrix is not square");
        exit(EXIT_FAILURE);
    }

    if (matrix->col_num != vector->row_num)
    {
        perror("Incompatible matrix dimensions");
        exit(EXIT_FAILURE);
    }

    if (vector->col_num != 1)
    {
        perror("Not a vector.");
        exit(EXIT_FAILURE);
    }

    Matrix *product = new_matrix(matrix->row_num, 1);

    int n = matrix->col_num;

    cblas_dsymv(CblasRowMajor, CblasUpper, n, 1, matrix->data,
                n, vector->data, 1, 0, product->data, 1);

    return product;
}

void multiply_matrix_with_a_number(Matrix *matrix, double number)
{
    int i;
    int n = matrix->col_num;
    int row_num = matrix->row_num;

    for (i = 0; i < row_num; i++)
    {
        cblas_dscal(n, number, matrix->data + i * n, 1);
    }
}

Matrix *transpose_matrix(Matrix *matrix)
{
    Matrix *product = new_matrix(matrix->col_num, matrix->row_num);

    int i, j;

    for (i = 0; i < matrix->col_num; i++)
    {
        for (j = 0; j < matrix->row_num; j++)
        {
            product->data[i * matrix->row_num + j] = matrix->data[j * matrix->col_num + i];
        }
    }

    return product;
}

double dot_product(Matrix *matrix1, Matrix *matrix2)
{
    if (matrix1->row_num != matrix2->row_num ||
        matrix1->col_num != 1 || matrix2->col_num != 1)
    {
        perror("Matrices are not n by 1.");
        exit(EXIT_FAILURE);
    }

    int i;
    double result = 0;

    for (i = 0; i < matrix1->row_num; i++)
    {
        result += matrix1->data[i] * matrix2->data[i];
    }

    return result;
}

double norm(Matrix *matrix)
{
    return sqrt(dot_product(matrix, matrix));
}

Matrix *gramian(Matrix *matrix)
{
    Matrix *product = new_matrix(matrix->col_num, matrix->col_num);

    int n = matrix->col_num;
    int k = matrix->row_num;

    cblas_dsyrk(CblasRowMajor, CblasUpper, CblasTrans,
		 n, k, 1, matrix->data, n, 0, product->data, n);

    return product;
}