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

#include "ellipsoid/ellipsoid.h"
#include "ellipsoid/ellSolv.h"
#include "sphere/sphere.h"
#include "sphere/sphSolv.h"
#include "constants.h"
#include "testProblem.h"

//compares ellipsoidal and spherical
//has ellipsoidal distribution of charges just inside ellipsoidal boundary
//computes the solution at two point, one inside smallest enclosing sphere where spherical diverges
//other solution point is outside of sphere and ellipse to compare convergence
#undef __FUNCT__
#define __FUNCT__ "EllVsSphConvergence"
PetscErrorCode EllVsSphConvergence()
{
  PetscErrorCode ierr;
  //semi-axes of ellipsoid
  PetscReal a = 3.0;
  PetscReal b = 2.5;
  PetscReal c = 1.2;
  //sphere radius
  PetscReal sphRad = 3;
  //dielecric permitivities both set to 1
  const PetscReal eps1 = 1.0;
  const PetscReal eps2 = 1.0;
  //charges placed on ellipsoid with sem-axes smaller by epsilon
  PetscReal epsilon = .5;
  //interior charge XYZ and magnitude
  const PetscInt SQRT_NUM_CHARGES = 3;
  Vec chargeXYZ;
  Vec chargeMag;
  //maximum expansion order for test
  const PetscInt MAX_N = 20;
  //solutions vector
  const PetscInt NUM_SOL_PTS = 2;
  Vec ellSolutions[MAX_N], sphSolutions[MAX_N];
  Vec ellError[MAX_N], sphError[MAX_N];
  Vec exactSolution;
  Vec solXYZ;
  //solution point inside of sphere
  PetscReal sol1X = .1;
  PetscReal sol1Y = .1;
  PetscReal sol1Z = 1.5;
  //solution point outside of sphere
  PetscReal sol2X = 2.1;
  PetscReal sol2Y = 2.6;
  PetscReal sol2Z = 1;
  //pointer for accessing petsc vectors
  PetscScalar *vecPt;
  PetscFunctionBegin;

  //create, set charge magnitudes to 1
  ierr = VecCreateSeq(PETSC_COMM_SELF, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, &chargeMag);CHKERRQ(ierr);
  ierr = VecSet(chargeMag, 1.0);CHKERRQ(ierr);

  //create, set charge XYZ values to be on a-eps,b-eps,c-eps ellipsoid
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, &chargeXYZ);CHKERRQ(ierr);
  ierr = VecGetArray(chargeXYZ, &vecPt);CHKERRQ(ierr);
  PetscReal pu, pv;
  for(PetscInt i=0; i<SQRT_NUM_CHARGES; ++i) {
    for(PetscInt j=0; j<SQRT_NUM_CHARGES; ++j) {
      pu = i*2.0*PETSC_PI/(SQRT_NUM_CHARGES-1);
      pv = j*PETSC_PI/(SQRT_NUM_CHARGES-1);
      PetscInt ind = i*SQRT_NUM_CHARGES + j;
      //there is a slight issue with on-axes coordinate transforms which is what the +.01 is for
      vecPt[3*ind + 0] = (a-epsilon)*PetscCosReal(pu)*PetscSinReal(pv) +.01;
      vecPt[3*ind + 1] = (b-epsilon)*PetscSinReal(pu)*PetscSinReal(pv) + .01;
      vecPt[3*ind + 2] = (c-epsilon)*PetscCosReal(pv) + .01;
      printf("pu: %3.3f pv: %3.3f\n", pu, pv);
    }
  }
  ierr = VecRestoreArray(chargeXYZ, &vecPt);CHKERRQ(ierr);

  //set solution points vector
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*NUM_SOL_PTS, &solXYZ);CHKERRQ(ierr);
  ierr = VecGetArray(solXYZ, &vecPt);CHKERRQ(ierr);
  vecPt[0] = sol1X; vecPt[1] = sol1Y; vecPt[2] = sol1Z;
  vecPt[3] = sol2X; vecPt[4] = sol2Y; vecPt[5] = sol2Z;
  ierr = VecRestoreArray(solXYZ, &vecPt);CHKERRQ(ierr);

  //caulculate exact solution
  ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, &exactSolution);CHKERRQ(ierr);
  ierr = CoulombExact(eps1, chargeXYZ, chargeMag, solXYZ, exactSolution);CHKERRQ(ierr);

  //initialize solutions and error vectors
  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, ellSolutions+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, sphSolutions+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, ellError+k);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOL_PTS, sphError+k);CHKERRQ(ierr);
  }

  //compute solutions for expansions up to order MAX_N
  for(PetscInt k=1; k<=MAX_N; ++k) {
    printf("N = %d\n", k);
    printf("calculating ellipsoidal solution...\n");
    ierr = CalcEllipsoidCoulombPotential(a, b, c, eps1, eps2, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, chargeXYZ, chargeMag, NUM_SOL_PTS, solXYZ, k, ellSolutions[k-1]);CHKERRQ(ierr);
    printf("calculating spherical solution...\n");
    ierr = CalcSphericalCoulombPotential(1.0, eps1, eps2, SQRT_NUM_CHARGES*SQRT_NUM_CHARGES, chargeXYZ, chargeMag, NUM_SOL_PTS, solXYZ, k, sphSolutions[k-1]);CHKERRQ(ierr);
  }

  //compute errors
  printf("Computing errors...\n");
  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecCopy(exactSolution, ellError[k]);CHKERRQ(ierr);
    ierr = VecCopy(exactSolution, sphError[k]);CHKERRQ(ierr);
    ierr = VecAXPY(ellError[k], -1.0, ellSolutions[k]);CHKERRQ(ierr);
    ierr = VecAXPY(sphError[k], -1.0, sphSolutions[k]);CHKERRQ(ierr);
    ierr = VecAbs(ellError[k]);CHKERRQ(ierr);
    ierr = VecAbs(sphError[k]);CHKERRQ(ierr);
  }

  for(PetscInt k=0; k<MAX_N; ++k) {
    ierr = VecGetArrayRead(ellError[k], &vecPt);
    printf("Error[%d] = %4.4e\n", k, vecPt[0]);
    ierr = VecRestoreArrayRead(ellError[k], &vecPt);
  }
  printf("Exact: \n");
  ierr = VecView(exactSolution, PETSC_VIEWER_STDOUT_SELF);
  printf("Ell: \n");
  ierr = VecView(ellSolutions[MAX_N-1], PETSC_VIEWER_STDOUT_SELF);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "runTest1"
void runTest1() {

  const int nCharges = 1;

  EllipsoidalSystem e;
  initEllipsoidalSystem(&e, 3.0, 2.0, 1.0, 64);

  //grid dimensions
  int nx = 10;
  int ny = 10;
  int nz = 2;
  int nPoints = nx*ny*nz;
  double xa = -5;
  double xb = 5;
  double ya = -5;
  double yb = 5;
  double za = -3;
  double zb = 3;
  double xh = (xb - xa)/(nx-1);
  double yh = (yb - ya)/(ny-1);
  double zh = (zb - za)/(nz-1);

  double charges[nCharges];
  for(int i=0; i<nCharges; ++i)
    charges[i] = 1.0;

  //source charges
  SPoint sPoints[nCharges];
  Point  ePoints[nCharges];
  sPoints[0].x1 = 2.02;
  sPoints[0].x2 = -0.4;
  sPoints[0].x3 = -.463;
  ePoints[0].x1 = sPoints[0].x1;
  ePoints[0].x2 = sPoints[0].x2;
  ePoints[0].x3 = sPoints[0].x3;

  //permitivity constants
  double e1 = 1.0;
  double e2 = 1.0;

  printf("entering\n");
  //solution vector
  SPoint *rs = (SPoint*) malloc(sizeof(SPoint)*nPoints);
  Point  *re = (Point*)  malloc(sizeof(Point) *nPoints);
  SPoint *ro = (SPoint*) malloc(sizeof(SPoint)*nPoints);
  double *exact = (double*) malloc(sizeof(double)*nPoints);
  for(int i=0; i<nx; ++i) {
    for(int j=0; j<ny; ++j) {
      for(int k=0; k<nz; ++k) {
	int index = k*ny*nx + j*nx + i;
	rs[index].x1 = xa + i*xh;
	rs[index].x2 = ya + j*yh;
	rs[index].x3 = za + k*zh;
	printf("rs[3] = %15.15f\n", rs[index].x3);
	re[index].x1 = rs[index].x1;
	re[index].x2 = rs[index].x2;
	re[index].x3 = rs[index].x3;
	ro[index].x1 = re[index].x1;
	ro[index].x2 = re[index].x2;
	ro[index].x3 = re[index].x3;

	exact[index] = 0.0;
	for(int num=0; num<nCharges; ++num)
	  exact[index] += charges[num]/(e1*sqrt((sPoints[num].x1 - ro[index].x1)*(sPoints[num].x1 - ro[index].x1) +
					    (sPoints[num].x2 - ro[index].x2)*(sPoints[num].x2 - ro[index].x2) +
					     (sPoints[num].x3 - ro[index].x3)*(sPoints[num].x3 - ro[index].x3)));
      }
    }
  }
  printf("exiting\n");

  //convert to spherical/ellipsoidal coordinates
  for(int i=0; i<nCharges; ++i) {
    cartesianToSpherical(sPoints+i);
    cartesianToEllipsoidal(&e, ePoints+i);
  }

  //convert to spherical/ellipsoidal coordinates
  for(int i=0; i<nPoints; ++i) {
    cartesianToSpherical(rs + i);
    cartesianToEllipsoidal(&e, re + i);
  }

  //set up problem
  SProblem sProblem = {.positions = sPoints, .charges = charges, .nCharges = nCharges, .e1 = e1, .e2 = e2, .b = 3.0};
  Problem  eProblem = {.positions = ePoints, .charges = charges, .nCharges = nCharges, .e1 = e1, .e2 = e2, .e = &e};

  double *ssolution = (double*) malloc(sizeof(double)*nPoints);
  double *esolution = (double*) malloc(sizeof(double)*nPoints);
  calcCoulombSphericalGrid(&sProblem, 14, rs, nPoints, ssolution);
  calcCoulombEllipsoidalGrid(&eProblem, 14, re, nPoints, esolution);

  //add coulomb inside for plotting. Add later to calculation
  for(int i=0; i<nPoints; ++i) {
    for(int k=0; k<nCharges; ++k) {
  //if inside sphere
  //
      //if(
    }
  }

  printf("exact[0] = %15.15f\n", exact[0]);
  //compare errors
  double *sError = (double*) malloc(sizeof(double)*nPoints);
  double *eError = (double*) malloc(sizeof(double)*nPoints);
  for(int i=0; i<nx; ++i) {
    for(int j=0; j<ny; ++j) {
      for(int k=0; k<nz; ++k) {
	int index = k*ny*nx + j*nx + i;
	sError[index] = fabs(ssolution[index] - exact[index]);
	eError[index] = fabs(esolution[index] - exact[index]);
	if(i==1)
	  printf("%d - s: %4.4e e: %4.4e\n", i, sError[index], eError[index]);
      }
    }
  }

}

#undef __FUNCT__
#define __FUNCT__ "RunArg"
PetscErrorCode RunArg()
{
  PetscErrorCode ierr;
  char buff[64];
  int npts;
  double a, b, c;
  double *xyz;
  double *chargeValues;
  double *solution;
  double freeEnergy;

  PetscFunctionBegin;

  //constants
  const double q     = ELECTRON_CHARGE;
  const double Na    = AVOGADRO_NUMBER;
  const double JperC = 4.184; /* Jouled/Calorie */
  const double cf    = Na * (q*q/EPSILON_0)/JperC * (1e10/1000) * 1/4/PETSC_PI; /* kcal ang/mol */

  FILE *fp = fopen("ellArgData.txt", "r");
  // READ NUMBER OF POINTS
  fscanf(fp, "%s", buff); //skip "size: "
  fscanf(fp, "%d", &npts);

  //initialize local charges/coordinates arrays
  xyz     = (double*) malloc(sizeof(double)*3*npts);
  chargeValues = (double*) malloc(sizeof(double)*npts);
  // READ A,B,C ELLIPSOID VALUES
  fscanf(fp, "%s", buff); //skip "a: "
  fscanf(fp, "%lf", &a); //read a value
  fscanf(fp, "%s", buff); //skip "b: "
  fscanf(fp, "%lf", &b); //read b value
  fscanf(fp, "%s", buff); //skip "c: "
  fscanf(fp, "%lf", &c);
  // READ CHARGE X,Y,Z AND CHARGE DATA
  for(int i=0; i<npts; ++i) {
    fscanf(fp, "%lf", xyz+3*i+0); //read x coordinate
    fscanf(fp, "%lf", xyz+3*i+1); //read y coordinate
    fscanf(fp, "%lf", xyz+3*i+2); //read z coordinate
    fscanf(fp, "%lf", chargeValues+i); //read charge
    printf("%lf %lf %lf %lf\n", xyz[3*i+0], xyz[3*i+1], xyz[3*i+2], chargeValues[i]);
  }
  fclose(fp);

  //initialize ellipsoidal system
  EllipsoidalSystem e;
  initEllipsoidalSystem(&e, a, b, c, 64);

  //set charge positions and calculation points to be the same
  Point *solPoints = (Point*) malloc(sizeof(Point)*npts);
  Point *chargePoints = (Point*) malloc(sizeof(Point)*npts);
  for(int i=0; i<npts; ++i) {
    solPoints[i].x1 = xyz[3*i+0];
    solPoints[i].x2 = xyz[3*i+1];
    solPoints[i].x3 = xyz[3*i+2];
    solPoints[i].type = 'c';
    chargePoints[i] = solPoints[i];
  }

  //convert to ellipsoidal coordinates
  for(int i=0; i<npts; ++i) {
    cartesianToEllipsoidal(&e, solPoints+i);
    cartesianToEllipsoidal(&e, chargePoints+i);
  }

  //initialize ellipsoidal problem context
  Problem prob;
  prob.e = &e;
  prob.positions = chargePoints;
  prob.charges = chargeValues;
  prob.nCharges = npts;
  prob.e1 = 4.0;
  prob.e2 = 80.0;

  //solution vector
  solution = (double*) malloc(sizeof(double)*npts);

  //calculate potential on solPoints
  calcCoulombEllipsoidalGrid(&prob, 10, solPoints, npts, solution);
  printf("wow!\n");
  for(int i=0; i<npts; ++i)
    printf("solution[%d] = %15.15f\n", i, solution[i]);
  for(int i=0; i<npts; ++i)
    printf("chargeValues[%d] = %15.15f\n", i, chargeValues[i]);

  //calculate free energy
  freeEnergy = 0;
  for(int i=0; i<npts; ++i) {
    freeEnergy += solution[i]*chargeValues[i];
    printf("freeEnergy[%d] = %15.15f\n", i, freeEnergy*.5*cf);
  }

  printf("\n");
  printf("Free energy: %15.15f\n", freeEnergy*.5*cf);

  free(xyz);
  free(chargeValues);
  free(solPoints);
  free(solution);
  free(chargePoints);
  PetscFunctionReturn(0);
}

/* DOESN'T DO ANYTHING YET */
#undef __FUNCT__
#define __FUNCT__ "RunArgWorkPrec"
PetscErrorCode RunArgWorkPrec()
{
  PetscErrorCode ierr;
  char buff[64];
  int npts;
  double a, b, c;
  double *xyz;
  double *chargeValues;
  double *solution;
  double freeEnergy;

  PetscFunctionBegin;

  //constants
  const double q     = ELECTRON_CHARGE;
  const double Na    = AVOGADRO_NUMBER;
  const double JperC = 4.184; /* Jouled/Calorie */
  const double cf    = Na * (q*q/EPSILON_0)/JperC * (1e10/1000) * 1/4/PETSC_PI; /* kcal ang/mol */

  FILE *fp = fopen("ellArgData.txt", "r");
  // READ NUMBER OF POINTS
  fscanf(fp, "%s", buff); //skip "size: "
  fscanf(fp, "%d", &npts);

  //initialize local charges/coordinates arrays
  xyz     = (double*) malloc(sizeof(double)*3*npts);
  chargeValues = (double*) malloc(sizeof(double)*npts);
  // READ A,B,C ELLIPSOID VALUES
  fscanf(fp, "%s", buff); //skip "a: "
  fscanf(fp, "%lf", &a); //read a value
  fscanf(fp, "%s", buff); //skip "b: "
  fscanf(fp, "%lf", &b); //read b value
  fscanf(fp, "%s", buff); //skip "c: "
  fscanf(fp, "%lf", &c);
  // READ CHARGE X,Y,Z AND CHARGE DATA
  for(int i=0; i<npts; ++i) {
    fscanf(fp, "%lf", xyz+3*i+0); //read x coordinate
    fscanf(fp, "%lf", xyz+3*i+1); //read y coordinate
    fscanf(fp, "%lf", xyz+3*i+2); //read z coordinate
    fscanf(fp, "%lf", chargeValues+i); //read charge
    printf("%lf %lf %lf %lf\n", xyz[3*i+0], xyz[3*i+1], xyz[3*i+2], chargeValues[i]);
  }
  fclose(fp);

  //initialize ellipsoidal system
  EllipsoidalSystem e;
  initEllipsoidalSystem(&e, a, b, c, 64);

  //set charge positions and calculation points to be the same
  Point *solPoints = (Point*) malloc(sizeof(Point)*npts);
  Point *chargePoints = (Point*) malloc(sizeof(Point)*npts);
  for(int i=0; i<npts; ++i) {
    solPoints[i].x1 = xyz[3*i+0];
    solPoints[i].x2 = xyz[3*i+1];
    solPoints[i].x3 = xyz[3*i+2];
    solPoints[i].type = 'c';
    chargePoints[i] = solPoints[i];
  }

  //convert to ellipsoidal coordinates
  for(int i=0; i<npts; ++i) {
    cartesianToEllipsoidal(&e, solPoints+i);
    cartesianToEllipsoidal(&e, chargePoints+i);
  }

  //initialize ellipsoidal problem context
  Problem prob;
  prob.e = &e;
  prob.positions = chargePoints;
  prob.charges = chargeValues;
  prob.nCharges = npts;
  prob.e1 = 4.0;
  prob.e2 = 80.0;

  //solution vector
  solution = (double*) malloc(sizeof(double)*npts);

  //calculate potential on solPoints
  calcCoulombEllipsoidalGrid(&prob, 10, solPoints, npts, solution);
  printf("wow!\n");
  for(int i=0; i<npts; ++i)
    printf("solution[%d] = %15.15f\n", i, solution[i]);
  for(int i=0; i<npts; ++i)
    printf("chargeValues[%d] = %15.15f\n", i, chargeValues[i]);

  //calculate free energy
  freeEnergy = 0;
  for(int i=0; i<npts; ++i) {
    freeEnergy += solution[i]*chargeValues[i];
    printf("freeEnergy[%d] = %15.15f\n", i, freeEnergy*.5*cf);
  }

  free(xyz);
  free(chargeValues);
  free(solPoints);
  free(solution);
  free(chargePoints);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "RunArgTester"
PetscErrorCode RunArgTester()
{
  PetscErrorCode ierr;
  char buff[64];
  int npts;
  double a, b, c;
  double *xyz;
  double *chargeValues;
  double *solution;
  double freeEnergy;

  PetscFunctionBegin;

  //constants
  const double q     = ELECTRON_CHARGE;
  const double Na    = AVOGADRO_NUMBER;
  const double JperC = 4.184; /* Jouled/Calorie */
  const double cf    = Na * (q*q/EPSILON_0)/JperC * (1e10/1000) * 1/4/PETSC_PI; /* kcal ang/mol */

  FILE *fp = fopen("../../pointbem/src/ellData.txt", "r");
  // READ NUMBER OF POINTS
  fscanf(fp, "%s", buff); //skip "size: "
  fscanf(fp, "%d", &npts);

  //initialize local charges/coordinates arrays
  xyz     = (double*) malloc(sizeof(double)*3*npts);
  chargeValues = (double*) malloc(sizeof(double)*npts);
  // READ A,B,C ELLIPSOID VALUES
  fscanf(fp, "%s", buff); //skip "a: "
  fscanf(fp, "%lf", &a); //read a value
  fscanf(fp, "%s", buff); //skip "b: "
  fscanf(fp, "%lf", &b); //read b value
  fscanf(fp, "%s", buff); //skip "c: "
  fscanf(fp, "%lf", &c);
  // READ CHARGE X,Y,Z AND CHARGE DATA
  for(int i=0; i<npts; ++i) {
    fscanf(fp, "%lf", xyz+3*i+0); //read x coordinate
    fscanf(fp, "%lf", xyz+3*i+1); //read y coordinate
    fscanf(fp, "%lf", xyz+3*i+2); //read z coordinate
    fscanf(fp, "%lf", chargeValues+i); //read charge
    printf("%lf %lf %lf %lf\n", xyz[3*i+0], xyz[3*i+1], xyz[3*i+2], chargeValues[i]);
  }
  fclose(fp);

  //initialize ellipsoidal system
  EllipsoidalSystem e;
  initEllipsoidalSystem(&e, a, b, c, 64);

  //set charge positions and calculation points to be the same
  Point *solPoints = (Point*) malloc(sizeof(Point)*npts);
  Point *chargePoints = (Point*) malloc(sizeof(Point)*npts);
  for(int i=0; i<npts; ++i) {
    solPoints[i].x1 = xyz[3*i+0];
    solPoints[i].x2 = xyz[3*i+1];
    solPoints[i].x3 = xyz[3*i+2];
    solPoints[i].type = 'c';
    chargePoints[i] = solPoints[i];
  }

  Vec valsbef; //testing
  PetscScalar* valsbefArray; //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, npts, &valsbef); CHKERRQ(ierr); //testing
  ierr = VecGetArray(valsbef, &valsbefArray); CHKERRQ(ierr); //testing
  //convert to ellipsoidal coordinates
  for(int i=0; i<npts; ++i) {
    cartesianToEllipsoidal(&e, solPoints+i);
    ierr = CalcSolidInteriorHarmonic(&e, solPoints[i].x1, solPoints[i].x2, solPoints[i].x3, 2, 3, valsbefArray+i); //testing
    cartesianToEllipsoidal(&e, chargePoints+i);
  }
  ierr = VecRestoreArray(valsbef, &valsbefArray); CHKERRQ(ierr); //testing
  Vec ellPoints, vals; //testing
  PetscScalar* ellPointsArray; //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*npts, &ellPoints); CHKERRQ(ierr); //testing
  ierr = VecGetArray(ellPoints, &ellPointsArray); CHKERRQ(ierr); //testing
  for(PetscInt i=0; i<npts; ++i) { //testing
    ellPointsArray[3*i+0] = solPoints[i].x1; //testing
    ellPointsArray[3*i+1] = solPoints[i].x2; //testing
    ellPointsArray[3*i+2] = solPoints[i].x3; //testing
  }
  ierr = VecRestoreArray(ellPoints, &ellPointsArray); CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, npts, &vals); CHKERRQ(ierr); //testing
  ierr = CalcSolidInteriorHarmonicVec(&e, ellPoints, 2, 3, vals); //testing
  PetscReal diff, val1, val2; //testing
  diff = 0;
  for(PetscInt i=0; i<npts; ++i) { //testing
    ierr = VecGetValues(vals, 1, &i, &val1); CHKERRQ(ierr); //testing
    ierr = VecGetValues(valsbef, 1, &i, &val2); CHKERRQ(ierr); //testing
    diff += PetscSqrtReal(PetscAbsReal(val1 - val2)); //testing
  }
  printf("Solid Harmonic Error: %3.3e\n", diff); //testing

  //initialize ellipsoidal problem context
  Problem prob;
  prob.e = &e;
  prob.positions = chargePoints;
  prob.charges = chargeValues;
  prob.nCharges = npts;
  prob.e1 = 4.0;
  prob.e2 = 80.0;

  /* ---------------------------- */
  /* NOW WE TEST THE Gnp FUNCTION */
  /* ---------------------------- */
  PetscInt NMAX = 5;
  PetscInt count = 0;

  for(PetscInt n=0; n<=NMAX; ++n) {
    for(PetscInt p=0; p<2*n+1; ++p)
      count++;
  }
  Vec srcCharges, GnpVals, GnpValsOld, BnpVals, BnpValsOld, CnpVals, CnpValsOld;
  Vec errorV;
  ierr = VecCreateSeq(PETSC_COMM_SELF, npts, &srcCharges);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &GnpValsOld);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &BnpVals);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &BnpValsOld);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &CnpVals);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &CnpValsOld);CHKERRQ(ierr); //testing
  ierr = VecCreateSeq(PETSC_COMM_SELF, count, &errorV);CHKERRQ(ierr); //testing
  for(PetscInt i=0; i<npts; ++i)
    ierr = VecSetValues(srcCharges, 1, &i, chargeValues+i, INSERT_VALUES);CHKERRQ(ierr); //testing
  ierr = VecAssemblyBegin(srcCharges);CHKERRQ(ierr); ierr = VecAssemblyEnd(srcCharges);CHKERRQ(ierr); //testing

  //WOULD ACTUALLY USE CHARGE POINTS, NOT TARGET POINTS
  //Calculate values with new function
  ierr = CalcCoulombEllCoefs(&e, npts, ellPoints, srcCharges, NMAX, &GnpVals);CHKERRQ(ierr); //testing
  ierr = CalcReactAndExtCoefsFromCoulomb(&e, prob.e1, prob.e2, NMAX, GnpVals, BnpVals, CnpVals);CHKERRQ(ierr);

  //Calculate values with old function
  PetscReal GnpOld, BnpOld, CnpOld;
  count = 0;
  for(PetscInt n=0; n<=NMAX; ++n) {
    for(PetscInt p=0; p<2*n+1; ++p) {
      GnpOld = calcGnp(&e, chargePoints, chargeValues, npts, n, p);
      calcBnpAndCnpFromGnp(&prob, n, p, GnpOld, &BnpOld, &CnpOld);
      ierr = VecSetValues(GnpValsOld, 1, &count, &GnpOld, INSERT_VALUES);CHKERRQ(ierr);
      ierr = VecSetValues(BnpValsOld, 1, &count, &BnpOld, INSERT_VALUES);CHKERRQ(ierr);
      ierr = VecSetValues(CnpValsOld, 1, &count, &CnpOld, INSERT_VALUES);CHKERRQ(ierr);

      count++;
    }
  }
  ierr = VecAssemblyBegin(GnpValsOld);CHKERRQ(ierr); ierr = VecAssemblyEnd(GnpValsOld);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(BnpValsOld);CHKERRQ(ierr); ierr = VecAssemblyEnd(BnpValsOld);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(CnpValsOld);CHKERRQ(ierr); ierr = VecAssemblyEnd(CnpValsOld);CHKERRQ(ierr);

  PetscReal GnpErr, BnpErr, CnpErr;
  ierr = VecWAXPY(errorV, -1.0, GnpVals, GnpValsOld);CHKERRQ(ierr);
  ierr = VecNorm(errorV, NORM_2, &GnpErr);
  ierr = VecWAXPY(errorV, -1.0, BnpVals, BnpValsOld);CHKERRQ(ierr);
  ierr = VecNorm(errorV, NORM_2, &BnpErr);
  ierr = VecWAXPY(errorV, -1.0, CnpVals, CnpValsOld);CHKERRQ(ierr);
  ierr = VecNorm(errorV, NORM_2, &CnpErr);
  printf("GnpErr: %3.3e\n", GnpErr);
  printf("BnpErr: %3.3e\n", BnpErr);
  printf("CnpErr: %3.3e\n", CnpErr);

  //solution vector
  solution = (double*) malloc(sizeof(double)*npts);

  //calculate potential on solPoints
  calcCoulombEllipsoidalGrid(&prob, 10, solPoints, npts, solution);
  printf("wow!\n");
  for(int i=0; i<npts; ++i)
    printf("solution[%d] = %15.15f\n", i, solution[i]);
  for(int i=0; i<npts; ++i)
    printf("chargeValues[%d] = %15.15f\n", i, chargeValues[i]);

  //calculate free energy
  freeEnergy = 0;
  for(int i=0; i<npts; ++i) {
    freeEnergy += solution[i]*chargeValues[i];
    printf("freeEnergy[%d] = %15.15f\n", i, freeEnergy*.5*cf);
  }

  free(xyz);
  free(chargeValues);
  free(solPoints);
  free(solution);
  free(chargePoints);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CoulombExact"
PetscErrorCode CoulombExact(PetscReal eps1, Vec srcXYZ, Vec srcMag, Vec tarXYZ, Vec sol)
{
  PetscErrorCode ierr;
  PetscReal srcX, srcY, srcZ;
  PetscReal tarX, tarY, tarZ;
  PetscReal dx2, dy2, dz2;
  PetscReal val, mag;
  const PetscScalar *srcXYZArray;
  const PetscScalar *srcMagArray;
  const PetscScalar *tarXYZArray;
  PetscScalar *solArray;
  PetscInt nSrc, nTar;
  PetscInt i, k;
  PetscFunctionBeginUser;

  ierr = VecGetSize(srcMag, &nSrc);CHKERRQ(ierr);
  ierr = VecGetSize(sol, &nTar);CHKERRQ(ierr);

  ierr = VecGetArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecGetArray    (sol   , &solArray   );CHKERRQ(ierr);
  for(i=0; i<nTar; ++i) {
    /* get target X,Y,Z */
    tarX = tarXYZArray[3*i+0];
    tarY = tarXYZArray[3*i+1];
    tarZ = tarXYZArray[3*i+2];
    for(k=0; k<nSrc; ++k) {
      /* get source X,Y,Z */
      srcX = srcXYZArray[3*k+0];
      srcY = srcXYZArray[3*k+1];
      srcZ = srcXYZArray[3*k+2];
      mag  = srcMagArray[k];

      dx2 = (tarX-srcX)*(tarX-srcX);
      dy2 = (tarY-srcY)*(tarY-srcY);
      dz2 = (tarZ-srcZ)*(tarZ-srcZ);

      val = mag/(eps1*PetscSqrtReal(dx2 + dy2 + dz2));
      solArray[i] += val;
    }
  }
  ierr = VecRestoreArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecRestoreArray    (sol   , &solArray   );CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "CoulombExactZeroInside"
PetscErrorCode CoulombExactZeroInside(PetscReal a, PetscReal b, PetscReal c, PetscReal eps1, Vec srcXYZ, Vec srcMag, Vec tarXYZ, Vec sol)
{
  PetscErrorCode ierr;
  PetscReal srcX, srcY, srcZ;
  PetscReal tarX, tarY, tarZ;
  PetscReal dx2, dy2, dz2;
  PetscReal val, mag;
  const PetscScalar *srcXYZArray;
  const PetscScalar *srcMagArray;
  const PetscScalar *tarXYZArray;
  PetscScalar *solArray;
  PetscInt nSrc, nTar;
  PetscInt i, k;
  PetscFunctionBeginUser;

  ierr = VecGetSize(srcMag, &nSrc);CHKERRQ(ierr);
  ierr = VecGetSize(sol, &nTar);CHKERRQ(ierr);

  ierr = VecZeroEntries(sol);CHKERRQ(ierr);

  ierr = VecGetArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecGetArray    (sol   , &solArray   );CHKERRQ(ierr);
  for(i=0; i<nTar; ++i) {
    /* get target X,Y,Z */
    tarX = tarXYZArray[3*i+0];
    tarY = tarXYZArray[3*i+1];
    tarZ = tarXYZArray[3*i+2];
    for(k=0; k<nSrc; ++k) {
      /* get source X,Y,Z */
      srcX = srcXYZArray[3*k+0];
      srcY = srcXYZArray[3*k+1];
      srcZ = srcXYZArray[3*k+2];
      mag  = srcMagArray[k];

      dx2 = (tarX-srcX)*(tarX-srcX);
      dy2 = (tarY-srcY)*(tarY-srcY);
      dz2 = (tarZ-srcZ)*(tarZ-srcZ);

      val = mag/(eps1*PetscSqrtReal(dx2 + dy2 + dz2));
      solArray[i] += val;
    }
    if((tarX*tarX)/(a*a) + (tarY*tarY)/(b*b) + (tarZ*tarZ)/(c*c) <= 1)
      solArray[i] = 0;
  }
  ierr = VecRestoreArrayRead(srcXYZ, &srcXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(srcMag, &srcMagArray);CHKERRQ(ierr);
  ierr = VecRestoreArray    (sol   , &solArray   );CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "RandomEllipsoidPoints"
PetscErrorCode RandomEllipsoidPoints(PetscReal a, PetscReal b, PetscReal c, Vec xyz)
{
  PetscErrorCode ierr;
  PetscReal r, theta, phi;
  PetscReal x, y, z;
  const PetscReal theta_min = 0;
  const PetscReal theta_max = 2*PETSC_PI;
  const PetscReal phi_min   = 0;
  const PetscReal phi_max   = PETSC_PI;
  const PetscReal r_min     = 0;
  const PetscReal r_max     = 1;
  PetscScalar *xyzArray;
  PetscInt nvals;
  PetscFunctionBegin;

  /* petsc random object */
  PetscRandom rnd1, rnd2;
  ierr = PetscRandomCreate(PETSC_COMM_WORLD, &rnd1);CHKERRQ(ierr);
  ierr = PetscRandomCreate(PETSC_COMM_WORLD, &rnd2);CHKERRQ(ierr);
  ierr = PetscRandomSetInterval(rnd1, .1, 1.0);CHKERRQ(ierr);
  ierr = PetscRandomSetInterval(rnd2, .8, .9);CHKERRQ(ierr);
  ierr = PetscRandomSetFromOptions(rnd1);CHKERRQ(ierr);
  ierr = PetscRandomSetFromOptions(rnd2);CHKERRQ(ierr);
  srand(time(NULL));
  ierr = PetscRandomSetSeed(rnd1, rand());
  ierr = PetscRandomSetSeed(rnd2, rand());
  ierr = PetscRandomSeed(rnd1);CHKERRQ(ierr);
  ierr = PetscRandomSeed(rnd2);CHKERRQ(ierr);

  ierr = VecGetSize(xyz, &nvals);CHKERRQ(ierr);
  nvals = nvals/3;

  ierr = VecGetArray(xyz, &xyzArray);CHKERRQ(ierr);
  for(PetscInt i=0; i<nvals; ++i) {
    ierr = PetscRandomGetValue(rnd2, &r);CHKERRQ(ierr);
    ierr = PetscRandomGetValue(rnd1, &theta);CHKERRQ(ierr);
    ierr = PetscRandomGetValue(rnd1, &phi);CHKERRQ(ierr);

    //r = r_min + (r_max-r_min)*i/(nvals-1);
    //theta = theta_min + (theta_max-theta_min)*i/(nvals-1);
    //phi = phi_min + (phi_max-phi_min)*i/(nvals-1);
    r = r_min + (r_max-r_min)*r;
    theta = theta_min + (theta_max-theta_min)*theta;
    phi = phi_min + (phi_max-phi_min)*phi;

    x = a*r*PetscCosReal(theta)*PetscSinReal(phi);
    y = b*r*PetscSinReal(theta)*PetscSinReal(phi);
    z = c*r*PetscCosReal(phi);

    xyzArray[3*i+0] = x;
    xyzArray[3*i+1] = y;
    xyzArray[3*i+2] = z;
    if(nvals==1) {
      xyzArray[3*i+0] = .1;
      xyzArray[3*i+1] = .2;
      xyzArray[3*i+2] = .4;
    }
    /*if(nvals==2) {
      xyzArray[0] = .1;
      xyzArray[1] = .2;
      xyzArray[2] = .4;
      xyzArray[3] = .1;
      xyzArray[4] = .2;
      xyzArray[5] = .3;
      }*/
  }
  ierr = VecRestoreArray(xyz, &xyzArray);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "RandomMag"
PetscErrorCode RandomMag(Vec mags)
{
  PetscErrorCode ierr;
  PetscInt nMag, i;
  PetscReal val;
  PetscRandom rnd;
  PetscScalar *magsArray;
  PetscFunctionBegin;

  ierr = VecGetSize(mags, &nMag);CHKERRQ(ierr);

  /* petsc random object */
  ierr = PetscRandomCreate(PETSC_COMM_WORLD, &rnd);CHKERRQ(ierr);
  ierr = PetscRandomSetInterval(rnd, -1.0, 1.0);CHKERRQ(ierr);
  ierr = PetscRandomSetFromOptions(rnd);CHKERRQ(ierr);

  ierr = VecGetArray(mags, &magsArray);CHKERRQ(ierr);
  for(i=0; i<nMag; ++i) {
    ierr = PetscRandomGetValue(rnd, &val);CHKERRQ(ierr);
    magsArray[i] = val;
  }
  ierr = VecRestoreArray(mags, &magsArray);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "SphereLimitExample"
PetscErrorCode SphereLimitExample()
{
  PetscErrorCode ierr;
  const PetscInt NUM_DELTAS = 14;
  const PetscReal DELTAS[14] = {1.3, 1.0, .5, .25, .15, .05, .01, .005, .001, .0005, .0001, .00005, .00001, .000005};
  const PetscInt  Nmax = 10;
  const PetscReal eps1 = 4.0;
  const PetscReal eps2 = 80.0;
  Vec srcXYZ[NUM_DELTAS], srcMag[NUM_DELTAS], solution[NUM_DELTAS];
  PetscScalar *srcXYZArray, *srcMagArray;
  PetscReal a, b, c;
  PetscReal delta;
  PetscReal *freeE;
  PetscInt i;
  PetscLogEvent events[NUM_DELTAS];
  char tText[40] = "Free energy with delta=%8.8f";
  char eText[40];
  EllipsoidalSystem ells[NUM_DELTAS];
  PetscFunctionBegin;

  ierr = PetscMalloc1(sizeof(PetscReal)*NUM_DELTAS, &freeE);

  for(i=0; i<NUM_DELTAS; ++i) {
    /* get size of delta and set a,b,c */
    delta = DELTAS[i];
    a = 1.0 + delta;
    b = 1.0 + (delta/5.0);
    c = 1.0 + (delta/10.0);
    /* init ellipsoidal system */
    ierr = initEllipsoidalSystem(ells+i, a, b, c, 64);CHKERRQ(ierr);
    /* initialize xyz and solution vectors */
    ierr = VecCreateSeq(PETSC_COMM_SELF, 3, srcXYZ+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, 1, srcMag+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, 1, solution+i);CHKERRQ(ierr);
    ierr = VecZeroEntries(srcXYZ[i]);CHKERRQ(ierr);
    ierr = VecGetArray(srcMag[i], &srcMagArray);CHKERRQ(ierr);
    srcMagArray[0] = 1.0; // set only charge to have magnitude 1.0
    ierr = VecRestoreArray(srcMag[i], &srcMagArray);CHKERRQ(ierr);
    /* register events for flop counting */
    sprintf(eText, tText, delta);
    ierr = PetscLogEventRegister((const char*) eText, 0, events+i);CHKERRQ(ierr);
  }
  /* solutions */
  for(i=0; i<NUM_DELTAS; ++i) {
    printf("calculating solution with delta=%8.8f\n", DELTAS[i]);
    ierr = PetscLogEventBegin(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = CalcEllipsoidFreeEnergy(ells+i, eps1, eps2, 1, srcXYZ[i], srcMag[i], 1e-5, Nmax, solution[i], freeE+i);CHKERRQ(ierr);
    printf("freeE: %15.15f\n", freeE[i]);

    ierr = PetscLogEventEnd(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
  }
  FILE *fp = fopen("out/sphConv.txt", "w");
  fprintf(fp, "delta error");
  PetscReal err;
  PetscReal exact = 1./eps2 - 1./eps1;
  for(i=0; i<NUM_DELTAS; ++i) {
    err = PetscAbsReal((exact - freeE[i])/exact);
    fprintf(fp, "%4.4e %4.4e\n", DELTAS[i], err);
    ierr = VecDestroy(srcXYZ+i);CHKERRQ(ierr);
    ierr = VecDestroy(srcMag+i);CHKERRQ(ierr);
    ierr = VecDestroy(solution+i);CHKERRQ(ierr);
  }
  fclose(fp);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "ChargeFlopsExample"
PetscErrorCode ChargeFlopsExample(PetscInt Nmax)
{
  PetscErrorCode ierr;

  const PetscInt NUM_SOLUTIONS = 10;
  const PetscInt CHARGE_START = 100;
  const PetscInt CHARGE_INC   = 50;
  Vec srcXYZ[NUM_SOLUTIONS], srcMag[NUM_SOLUTIONS], solution[NUM_SOLUTIONS];
  PetscInt i;
  PetscInt chargeNums[NUM_SOLUTIONS];
  PetscLogEvent events[NUM_SOLUTIONS];
  PetscInt charges;

  char tText[30] = "Free energy with %d charges";
  char eText[30];
  PetscInt tempNum;

  EllipsoidalSystem e;
  PetscFunctionBegin;

  const PetscReal eps1 = 4.0;
  const PetscReal eps2 = 4.0;

  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;

  ierr = initEllipsoidalSystem(&e, a, b, c, 64);CHKERRQ(ierr);

  /* create the vector with charge numbers to use */
  for(i=0; i < NUM_SOLUTIONS; ++i) {
    charges = CHARGE_START + CHARGE_INC*i;
    chargeNums[i] = charges;
    /* initialize xyz and solution vectors */
    ierr = VecCreateSeq(PETSC_COMM_SELF, 3*charges, srcXYZ+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, charges, srcMag+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, charges, solution+i);CHKERRQ(ierr);
    /* generate random points and magnitudes */
    ierr = RandomEllipsoidPoints(a, b, c, srcXYZ[i]);CHKERRQ(ierr);
    ierr = RandomMag(srcMag[i]);CHKERRQ(ierr);

    /* register events for flop counting */
    sprintf(eText, tText, charges);
    ierr = PetscLogEventRegister((const char*) eText, 0, events+i);CHKERRQ(ierr);
  }

  PetscReal freeE;
  for(i=0; i < NUM_SOLUTIONS; ++i) {
    printf("calculating solution %d/%d\n", i+1, NUM_SOLUTIONS);
    ierr = PetscLogEventBegin(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = CalcEllipsoidFreeEnergy(&e, eps1, eps2, chargeNums[i], srcXYZ[i], srcMag[i], 1e-5, Nmax, solution[i], &freeE);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
  }
  //ierr = EasyExample(Nmax, nSrc, nx, xl, xr, ny, yl, yr, zConst);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "WorkPrecExample"
PetscErrorCode WorkPrecExample()
{
  PetscErrorCode ierr;

  const PetscInt NUM_SOLUTIONS = 45;
  const PetscInt EXACT_NUM   = NUM_SOLUTIONS+10;
  const PetscInt NUM_CHARGES = 4;
  Vec srcXYZ, srcMag;
  Vec potential[NUM_SOLUTIONS];
  Vec potExact;
  Vec solution;
  PetscScalar *solutionArray;
  PetscReal exact;
  PetscInt i;
  PetscLogEvent events[NUM_SOLUTIONS];
  const PetscInt Nmin = 0;
  const PetscInt Nstep = 1;
  PetscInt N;
  char tText[30] = "Free energy with Nmax = %d";
  char eText[30];
  PetscInt tempNum;

  EllipsoidalSystem e;
  PetscFunctionBegin;

  const PetscReal eps1 = 4.0;
  const PetscReal eps2 = 80.0;

  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;

  ierr = initEllipsoidalSystem(&e, a, b, c, 64);CHKERRQ(ierr);

  /* create charge vectors and initialize with random values */
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*NUM_CHARGES, &srcXYZ);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF,   NUM_CHARGES, &srcMag);CHKERRQ(ierr);
  ierr = RandomEllipsoidPoints(a, b, c, srcXYZ);CHKERRQ(ierr);
  printf("######\nXYZ\n#######\n");
  ierr = VecView(srcXYZ, PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
  ierr = RandomMag(srcMag);CHKERRQ(ierr);
  ierr = VecSet(srcMag, 1.0);CHKERRQ(ierr);
  //ierr = VecScale(srcMag, 1./NUM_CHARGES);CHKERRQ(ierr); //scale charge magnitudes so total energy is more normalized

  for(i=0; i<NUM_SOLUTIONS; ++i) {
    /* register events for flop counting */
    N = Nmin + Nstep*i;
    sprintf(eText, tText, N);
    ierr = PetscLogEventRegister((const char*) eText, 0, events+i);CHKERRQ(ierr);
    /* create potential vectors */
    ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_CHARGES, potential+i);CHKERRQ(ierr);
  }
  /* create free energy solution vector */
  ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_SOLUTIONS, &solution);CHKERRQ(ierr);

  ierr = VecGetArray(solution, &solutionArray);CHKERRQ(ierr);
  /* approximate solutions */
  for(i=0; i < NUM_SOLUTIONS; ++i) {
    N = Nmin + Nstep*i;
    printf("calculating solution %d/%d\n", i+1, NUM_SOLUTIONS);
    printf("NNNNN = %d\n", N);
    ierr = PetscLogEventBegin(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
    ierr = CalcEllipsoidFreeEnergy(&e, eps1, eps2, NUM_CHARGES, srcXYZ, srcMag, 1e-16, N, potential[i], solutionArray+i);CHKERRQ(ierr);
    //ierr = VecView(solution, PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
    ierr = VecView(potential[i], PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
    ierr = PetscLogEventEnd(events[i], 0, 0, 0, 0);CHKERRQ(ierr);
  }

  /* exact(ish) solution */
  ierr = VecCreateSeq(PETSC_COMM_SELF, NUM_CHARGES, &potExact);CHKERRQ(ierr);
  printf("calculating exact solution\n");
  ierr = CalcEllipsoidFreeEnergy(&e, eps1, eps2, NUM_CHARGES, srcXYZ, srcMag, 1e-16, EXACT_NUM, potExact, &exact);CHKERRQ(ierr);

  FILE *fp = fopen("out/workprec.txt", "w");
  fprintf(fp, "work   precision\n");
  PetscReal errVal;
  PetscEventPerfInfo info;
  //* calculate error for approximate solutions */
  for(PetscInt i=0; i < NUM_SOLUTIONS; ++i) {
    errVal = PetscAbsReal((solutionArray[i] - exact)/(exact));
    ierr = PetscLogEventGetPerfInfo(PETSC_DETERMINE, events[i], &info);CHKERRQ(ierr);
    fprintf(fp, "%4.4e %4.4e\n", info.flops, errVal);
    printf("approx: %15.15f\n", solutionArray[i]);
  }
  printf("exact: %15.15f\n", exact);
  fclose(fp);
  ierr = VecRestoreArray(solution, &solutionArray);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "InitGrid"
PetscErrorCode InitGrid(PetscInt nx, PetscReal xl, PetscReal xr, PetscInt ny, PetscReal yl, PetscReal yr, PetscReal zConst, Vec *xyz)
{
  PetscErrorCode ierr;
  PetscInt i, j;
  PetscReal xh, yh;
  PetscInt index;
  PetscScalar *xyzArray;
  PetscFunctionBegin;

  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nx*ny, xyz);CHKERRQ(ierr);

  xh = (xr - xl)/(nx - 1);
  yh = (yr - yl)/(ny - 1);

  ierr = VecGetArray(*xyz, &xyzArray);CHKERRQ(ierr);
  for(i=0; i < nx; ++i) {
    for(j=0; j<ny; ++j) {
      index = 3*(i*ny + j);
      xyzArray[index+0] = xl + xh*i;
      xyzArray[index+1] = yl + yh*j;
      xyzArray[index+2] = zConst;
    }
  }
  ierr = VecRestoreArray(*xyz, &xyzArray);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "GridSolution"
/*
  Example is of a some charges
*/
PetscErrorCode GridSolution(PetscInt Nmax, PetscInt nSrc, PetscInt nx, PetscReal xl, PetscReal xr, PetscInt ny, PetscReal yl, PetscReal yr, PetscReal zConst, Vec *xvals, Vec *yvals, Vec *sol)
{
  PetscErrorCode ierr;
  Vec sourceXYZ, sourceMag, tarXYZ, solution;
  PetscReal xh, yh;
  PetscReal xc, yc;
  PetscReal randVal;
  PetscInt ind;
  PetscFunctionBegin;

  const PetscReal eps1 = 4.0;
  const PetscReal eps2 = 4.0;

  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;

  // initialize XYZ and solution vectors
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nSrc , &sourceXYZ);CHKERRQ(ierr);
  //ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nx*ny, &tarXYZ);   CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, nx*ny  , &solution); CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, nSrc   , &sourceMag);CHKERRQ(ierr);

  xh = (xr - xl) / (nx - 1);
  yh = (yr - yl) / (ny - 1);

  ierr = RandomEllipsoidPoints(a, b, c, sourceXYZ);CHKERRQ(ierr);

  // populate source vec with points
  for(PetscInt k=0; k<nSrc; ++k) {
    ind = 3*k;
    ierr = VecSetValue(sourceMag, k  , 1.0      , INSERT_VALUES);CHKERRQ(ierr);
  }
  ierr = VecAssemblyBegin(sourceMag);CHKERRQ(ierr); ierr = VecAssemblyEnd(sourceMag);CHKERRQ(ierr);

  ierr = InitGrid(nx, xl, xr, ny, yl, yr, zConst, &tarXYZ);

  // calculate the solvation potential
  ierr = CalcEllipsoidTester( a , b , c,
			      eps1, eps2,
			      nSrc, sourceXYZ, sourceMag,
			      nx*ny, tarXYZ,
			      Nmax, solution); CHKERRQ(ierr);

  /*
  FILE *fp = fopen("solvPlot.txt", "w");
  fprintf(fp, "xh: %4.4e\n", xh);
  fprintf(fp, "yh: %4.4e\n", yh);
  const PetscScalar *solutionArray;
  ierr = VecGetArrayRead(solution, &solutionArray);CHKERRQ(ierr);
  PetscInt index = 0;
  for(PetscInt i=0; i<nx; ++i) {
    for(PetscInt j=0; j<ny; ++j) {
      index = i*ny + j;
      fprintf(fp, "%4.4f ", solutionArray[index]);
    }
    fprintf(fp, "\n");
  }
  ierr = VecRestoreArrayRead(solution, &solutionArray);CHKERRQ(ierr);
  */
  if(xvals != NULL) {
    ierr = VecCreateSeq(PETSC_COMM_WORLD, nx, xvals);CHKERRQ(ierr);
    for(PetscInt i=0; i<nx; ++i)
      ierr = VecSetValue(*xvals, i, xl + i*xh, INSERT_VALUES);CHKERRQ(ierr);
    ierr = VecAssemblyBegin(*xvals);CHKERRQ(ierr); ierr = VecAssemblyEnd(*xvals);CHKERRQ(ierr);
  }
  if(yvals != NULL) {
    ierr = VecCreateSeq(PETSC_COMM_WORLD, nx, yvals);CHKERRQ(ierr);
    for(PetscInt i=0; i<ny; ++i)
      ierr = VecSetValue(*yvals, i, yl + i*yh, INSERT_VALUES);CHKERRQ(ierr);
    ierr = VecAssemblyBegin(*yvals);CHKERRQ(ierr); ierr = VecAssemblyEnd(*yvals);CHKERRQ(ierr);
  }
  if(sol != NULL) {
    ierr = VecDuplicate(solution, sol);CHKERRQ(ierr);
    ierr = VecCopy(solution, *sol);CHKERRQ(ierr);
  }
  ierr = VecDestroy(&sourceXYZ);CHKERRQ(ierr);
  ierr = VecDestroy(&tarXYZ);CHKERRQ(ierr);
  ierr = VecDestroy(&solution);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "SubtractCoulombInside"
PetscErrorCode SubtractCoulombInside(PetscReal a, PetscReal b, PetscReal c, Vec tarXYZ, Vec solution, Vec solutionEx, Vec ncSolution)
{
  PetscErrorCode ierr;
  const PetscScalar *tarXYZArray, *solutionArray, *solutionExArray;
  PetscScalar *ncSolutionArray;
  PetscInt nTar;
  PetscInt i;
  PetscReal x, y, z;
  PetscFunctionBegin;

  ierr = VecGetSize(solution, &nTar);CHKERRQ(ierr);

  ierr = VecGetArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(solution, &solutionArray);CHKERRQ(ierr);
  ierr = VecGetArrayRead(solutionEx, &solutionExArray);CHKERRQ(ierr);
  ierr = VecGetArray(ncSolution, &ncSolutionArray);CHKERRQ(ierr);

  for(i=0; i<nTar; ++i) {
    x = tarXYZArray[3*i+0];
    y = tarXYZArray[3*i+1];
    z = tarXYZArray[3*i+2];

    if( ((x*x)/(a*a)) + ((y*y)/(b*b)) + ((z*z)/(c*c)) <= 1) {
      ncSolutionArray[i] = solutionArray[i];
    }
    else {
      ncSolutionArray[i] = solutionArray[i] - solutionExArray[i];
    }
  }

  ierr = VecRestoreArrayRead(tarXYZ, &tarXYZArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(solution, &solutionArray);CHKERRQ(ierr);
  ierr = VecRestoreArrayRead(solutionEx, &solutionExArray);CHKERRQ(ierr);
  ierr = VecRestoreArray(ncSolution, &ncSolutionArray);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "GridAnimation"
PetscErrorCode GridAnimation()
{
  const PetscInt NUM_SOLUTIONS = 8;
  PetscErrorCode ierr;
  Vec sourceXYZ, sourceMag, tarXYZ;
  Vec solution[NUM_SOLUTIONS], solutionEx[NUM_SOLUTIONS], ncSolution[NUM_SOLUTIONS];
  Vec errorVec[NUM_SOLUTIONS];
  Vec xOut, yOut, zOut;
  PetscReal xh, yh;
  PetscReal xc, yc;
  PetscReal randVal;
  PetscInt ind;
  PetscFunctionBegin;

  PetscReal eps1 = 2.0;
  PetscReal eps2 = 20.0;
  PetscInt nSrc  = 5;
  PetscInt nx    = 15;
  PetscReal xl   = -5.13;
  PetscReal xr   = 5.13;
  PetscInt ny    = 15;
  PetscReal yl   = -4.02;
  PetscReal yr   =  4.02;
  PetscReal zConst = .1;

  xh = (xr-xl)/ny;
  yh = (yr-yl)/ny;

  const PetscReal a = 3.0;
  const PetscReal b = 2.0;
  const PetscReal c = 1.0;

  // initialize source XYZ, mag
  ierr = VecCreateSeq(PETSC_COMM_SELF, 3*nSrc , &sourceXYZ);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, nSrc   , &sourceMag);CHKERRQ(ierr);
  // initialize solution vectors
  for(PetscInt i=0; i < NUM_SOLUTIONS; ++i) {
    ierr = VecCreateSeq(PETSC_COMM_SELF, nx*ny  , solution+i); CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, nx*ny  , solutionEx+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, nx*ny  , ncSolution+i);CHKERRQ(ierr);
    ierr = VecCreateSeq(PETSC_COMM_SELF, nx*ny  , errorVec+i);CHKERRQ(ierr);

  }

  ierr = RandomEllipsoidPoints(a, b, c, sourceXYZ);CHKERRQ(ierr); // on z=0

  //ierr = RandomMag(sourceMag); CHKERRQ(ierr);
  ierr = VecSet(sourceMag, 1.0);CHKERRQ(ierr);

  ierr = InitGrid(nx, xl, xr, ny, yl, yr, zConst, &tarXYZ);CHKERRQ(ierr); //zConst = 0

  //Vec tester;
  //ierr = VecDuplicate(tarXYZ, &tester);
  PetscReal err;
  // calculate the solvation potential
  for(PetscInt i=0; i < NUM_SOLUTIONS; ++i) {
    printf("wow\n");
    ierr = CoulombExact(eps1, sourceXYZ, sourceMag, tarXYZ, solutionEx[i]);
    //ierr = VecCopy(tarXYZ, tester);
    printf("eps: %15.15f\n", eps1);
    ierr = CalcEllipsoidTester( a , b , c,
				eps1, eps2,
				nSrc, sourceXYZ, sourceMag,
				nx*ny, tarXYZ,
				i, solution[i]); CHKERRQ(ierr);
    ierr = SubtractCoulombInside(a, b, c, tarXYZ, solution[i], solutionEx[i], ncSolution[i]);CHKERRQ(ierr);
    PetscInt zero = 0;
    PetscReal sol0, solEx0;
    ierr = VecGetValues(solution[i], 1, &zero, &sol0);CHKERRQ(ierr);
    ierr = VecGetValues(solutionEx[i], 1, &zero, &solEx0);CHKERRQ(ierr);
    //ierr = VecAXPY(tester, -1.0, tarXYZ);CHKERRQ(ierr);
    //ierr = VecView(solutionEx[i], PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
    //ierr = VecView(solution[i], PETSC_VIEWER_STDOUT_SELF);CHKERRQ(ierr);
    ierr = VecCopy(solutionEx[i], errorVec[i]);CHKERRQ(ierr);
    ierr = VecAXPY(errorVec[i], -1.0, solution[i]);
    ierr = VecNorm(errorVec[i], NORM_2, &err);
    printf("\n");
    printf("the error on %d is %15.15f\n", i, err*xh*yh);
  }

  /* subtract coulomb from solution inside */

  char fname[20] = "out/sol%d.txt";
  char fnameEx[20] = "out/solEx%d.txt";
  char fnameErr[20] = "out/solErr%d.txt";
  char fnameS[20];
  char fnameExS[20];
  char fnameErrS[20];
  for(PetscInt i=0; i < NUM_SOLUTIONS; ++i) {
    sprintf(fnameS, fname, i);
    sprintf(fnameExS, fnameEx, i);
    sprintf(fnameErrS, fnameErr, i);
    ierr = WriteToFile((const char*) fnameS, ny, ncSolution[i]);CHKERRQ(ierr);
    ierr = WriteToFile((const char*) fnameExS, ny, solutionEx[i]);CHKERRQ(ierr);
    ierr = WriteToFile((const char*) fnameErrS, ny, errorVec[i]);CHKERRQ(ierr);
  }

  ierr = VecCreateSeq(PETSC_COMM_SELF, nx, &xOut);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, ny, &yOut);CHKERRQ(ierr);
  ierr = VecCreateSeq(PETSC_COMM_SELF, 1, &zOut);CHKERRQ(ierr);
  xh = (xr - xl) / (nx - 1);
  yh = (yr - yl) / (ny - 1);
  for(PetscInt i=0; i<nx; ++i) {
    ierr = VecSetValue(xOut, i, xl + xh*i, INSERT_VALUES);CHKERRQ(ierr);
  }
  for(PetscInt i=0; i<ny; ++i) {
    ierr = VecSetValue(yOut, i, yl + yh*i, INSERT_VALUES);CHKERRQ(ierr);
  }
  ierr = VecSetValue(zOut, 0, zConst, INSERT_VALUES);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(xOut);CHKERRQ(ierr); ierr = VecAssemblyEnd(xOut);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(yOut);CHKERRQ(ierr); ierr = VecAssemblyEnd(yOut);CHKERRQ(ierr);
  ierr = VecAssemblyBegin(zOut);CHKERRQ(ierr); ierr = VecAssemblyEnd(zOut);CHKERRQ(ierr);

  ierr = WriteToFile((const char*) "out/xVals.txt", 1, xOut);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/yVals.txt", 1, yOut);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/zVals.txt", 1, zOut);CHKERRQ(ierr);

  /* output source xyz values */
  ierr = WriteToFile((const char*) "out/chargeXYZ.txt", 3, sourceXYZ);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/chargeMag.txt", 1, sourceMag);CHKERRQ(ierr);
  FILE *fp = fopen("out/otherinfo.txt", "w");
  fprintf(fp, "%6.6f %6.6f %6.6f\n", a, b, c);
  fclose(fp);

  ierr = VecDestroy(&sourceXYZ);CHKERRQ(ierr);
  ierr = VecDestroy(&tarXYZ);CHKERRQ(ierr);
  for(PetscInt i=0; i < NUM_SOLUTIONS; ++i) {
    ierr = VecDestroy(solution+i);CHKERRQ(ierr);
    ierr = VecDestroy(solutionEx+i);CHKERRQ(ierr);
    ierr = VecDestroy(ncSolution+i);CHKERRQ(ierr);
  }
  ierr = VecDestroy(&sourceMag);CHKERRQ(ierr);
  ierr = VecDestroy(&xOut);CHKERRQ(ierr);
  ierr = VecDestroy(&yOut);CHKERRQ(ierr);
  ierr = VecDestroy(&zOut);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "numChargesPlot"
PetscErrorCode numChargesPlot(PetscInt nMin, PetscInt nMax, PetscInt nStep)
{
  PetscErrorCode ierr;
  PetscInt Nmax = 4;
  //PetscInt nSrc = 20;
  PetscInt nx   = 7;
  PetscReal xl  = -4.6;
  PetscReal xr  = 4.6;
  PetscInt ny   = 7;
  PetscReal yl  = -4.6;
  PetscReal yr  = 4.6;
  PetscReal zConst = .1;
  PetscInt nSource;
  PetscLogStage *chargeStages;
  PetscStageLog stageLog;
  PetscFunctionBegin;

  PetscInt count = 0;
  for(nSource = nMin; nSource <= nMax; nSource += nStep) {
    count++;
  }
  ierr = PetscMalloc1(sizeof(PetscLogStage)*count, &chargeStages);
  char bStr[40] = "Num Charges: %d";
  char indvStr[40];
  count = 0;
  for(nSource = nMin; nSource <= nMax; nSource += nStep) {
    sprintf(indvStr, bStr, nSource);
    ierr = PetscLogStageRegister(indvStr, chargeStages+count);CHKERRQ(ierr);
    count++;
  }
  count = 0;
  for(nSource = nMin; nSource <= nMax; nSource += nStep) {
    printf("wow\n");
    ierr = PetscLogStagePush(chargeStages[count]);CHKERRQ(ierr);
    ierr = GridSolution(Nmax, nSource, nx, xl, xr, ny, yl, yr, zConst, NULL, NULL, NULL);CHKERRQ(ierr);
    ierr = PetscLogStagePop();CHKERRQ(ierr);
    count++;
  }

  ierr = PetscLogGetStageLog(&stageLog);CHKERRQ(ierr);
  count = 0;
  for(nSource = nMin; nSource <= nMax; nSource += nStep) {
    ierr = PetscPrintf(PETSC_COMM_WORLD, "n = %d\nflops = %4.4e\n", nSource, stageLog->stageInfo[chargeStages[count]].perfInfo.flops);CHKERRQ(ierr);
    count++;
  }
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "WriteToFile"
PetscErrorCode WriteToFile(const char *fname, PetscInt rowsize, Vec values)
{
  PetscErrorCode ierr;
  PetscInt k;
  PetscInt nPts;
  const PetscScalar *valuesArray;
  PetscFunctionBegin;

  ierr = VecGetSize(values, &nPts);CHKERRQ(ierr);

  ierr = VecGetArrayRead(values, &valuesArray);CHKERRQ(ierr);
  FILE *fp = fopen(fname, "w");
  for(k=0; k < nPts; ++k) {
    fprintf(fp, "%4.4f ", valuesArray[k]);
    if((k+1)%rowsize == 0)
      fprintf(fp, "\n");
  }
  fclose(fp);
  ierr = VecRestoreArrayRead(values, &valuesArray);CHKERRQ(ierr);
  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "SolutionAnimation"
PetscErrorCode SolutionAnimation()
{
  PetscErrorCode ierr;
  Vec x1, y1, s0, s1, s2, s3;
  PetscFunctionBegin;
  PetscInt nSrc = 10;
  PetscInt nx   = 15;
  PetscReal xl  = -4.6;
  PetscReal xr  = 4.6;
  PetscInt ny   = 15;
  PetscReal yl  = -4.6;
  PetscReal yr  = 4.6;
  PetscReal zConst = .1;

  ierr = GridSolution(0, nSrc, nx, xl, xr, ny, yl, yr, zConst, &x1, &y1, &s0);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/sol0.txt", ny, s0);CHKERRQ(ierr);
  ierr = GridSolution(1, nSrc, nx, xl, xr, ny, yl, yr, zConst, &x1, &y1, &s1);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/sol1.txt", ny, s1);CHKERRQ(ierr);
  ierr = GridSolution(2, nSrc, nx, xl, xr, ny, yl, yr, zConst, &x1, &y1, &s2);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/sol2.txt", ny, s2);CHKERRQ(ierr);
  ierr = GridSolution(3, nSrc, nx, xl, xr, ny, yl, yr, zConst, &x1, &y1, &s3);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/sol3.txt", ny, s3);CHKERRQ(ierr);

  ierr = WriteToFile((const char*) "out/xVals.txt", 1, x1);CHKERRQ(ierr);
  ierr = WriteToFile((const char*) "out/yVals.txt", 1, y1);CHKERRQ(ierr);

  PetscFunctionReturn(0);
}

#undef __FUNCT__
#define __FUNCT__ "testLame"
PetscErrorCode testLame()
{
  PetscErrorCode ierr;
  PetscInt n, p;
  PetscInt Nmax;
  PetscReal Fnp;
  PetscReal Enp;
  PetscReal a, b, c;
  PetscReal cval;
  PetscInt signm, signn;
  EllipsoidalSystem e;
  PetscFunctionBegin;

  cval = .3;
  signm = -1;
  signn = 1;

  a = 3.0; b = 2.0; c = 1.0;
  initEllipsoidalSystem(&e, a, b, c, 64);
  FILE *fp = fopen("newellout.txt", "w");
  Nmax = 6;
  for(n=0; n < Nmax; ++n) {
    for(p=0; p < 2*n+1; ++p) {
      ierr = calcLame(&e, n, p, cval, signm, signn, &Enp);CHKERRQ(ierr);
      fprintf(fp, "%d %d %15.15f\n", n, p, Enp);
    }
  }
  fclose(fp);

  PetscFunctionReturn(0);
}

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

  PetscErrorCode ierr;

  PetscFunctionBeginUser;
  //initialize Petsc
  ierr = PetscInitialize(&argc, &argv, NULL, NULL); CHKERRQ(ierr);
  ierr = PetscLogDefaultBegin(); CHKERRQ(ierr);

  //ierr = EllVsSphConvergence();CHKERRQ(ierr);

  //testSphericalCoordinates();
  //runTest1();
  //testLegendre();
  //runTest4();
  //test4sphere();
  //RunArg();
  //ierr = RunArgTester(); CHKERRQ(ierr);
  //PetscErrorCode GridSolution(PetscInt Nmax, PetscInt nSrc, PetscInt nx, PetscReal xl, PetscReal xr, PetscInt ny, PetscReal yl, PetscReal yr, PetscReal zConst)
  /*
  PetscInt Nmax = 4;
  PetscInt nSrc = 25;
  PetscInt nx   = 15;
  PetscReal xl  = -5.23;
  PetscReal xr  = 5.23;
  PetscInt ny   = 15;
  PetscReal yl  = -4.45;
  PetscReal yr  = 4.45;
  PetscReal zConst = .1;
  PetscReal eps1 = 4.0;
  PetscReal eps2 = 80.0;
  */
  //ierr = GridSolution(Nmax, nSrc, nx, xl, xr, ny, yl, yr, zConst, NULL, NULL);CHKERRQ(ierr);
  //ierr = SolutionAnimation(); //<---- total crap
  //runTest1();
  //testLame();

//  ierr = runTest1();CHKERRQ(ierr);
  //ierr = RunArg();
  //ierr = GridAnimation();CHKERRQ(ierr);
  //ierr = ChargeFlopsExample(10);CHKERRQ(ierr);
  ierr = WorkPrecExample();CHKERRQ(ierr);
  //ierr = SphereLimitExample();CHKERRQ(ierr);

  //ierr = numChargesPlot(100, 500, 100);
  ierr = PetscFinalize();CHKERRQ(ierr);
  PetscFunctionReturn(0);
}


#include <stdio.h>
#include <math.h>
#include <gsl/gsl_integration.h>  /* GSL Integration Library */
#include "funs.c"                 /* Integration functions   */
#include "timing_funs.h"          /* Timing functions        */

#define NMAX 100

int main(void)
{
	/* Declaration and initialization */
	int i;
	double vals1[NMAX+1], vals2[NMAX+1];

	/*Uses functions declared in "funs.c" to compute I(n) using the recurrence and the 		generic integrator methods, and stores the results in their respective arrays.*/
	integral_recur(0, 100, vals1);
	integral_gen(0, 100, vals2);

	/*Prints "n" and the result I(n) for each method above.  The absolute error 		multiplied by a factor of 1.e9 is reported in the last column.*/

	printf("n    Recurrence    Generic       Scaled Abs Err\n");

	for(i = 0; i <=NMAX; i++)
	{
		printf("%d    %.8f    %.8f    %f\n", i, vals1[i], vals2[i],
		(fabs(vals1[i]-vals2[i]))*1.e9);
	}

	/* Define the intial number of counts, tmin, tmax*/
	double time, trr, tgsl;
	int tmin = 1;
	int tmax = 2;
	int count = 1000;

	/* Computes the time per function call of the recurrence integration method.  Calls 		the function integral_recur() "count" times and adjusts count until the total time 		is between 1 and 2 seconds.*/
	printf("\n Timing for Recurrence Relation:\n");
	do
	{
		timer_start ();
		for(i = 0; i < count; i++)
		{
			integral_recur(0, 100, vals1);
		}
		time = timer_stop ();
		trr = time /count;
		printf (" %10.2f usec     %10.6f sec    %10d\n",trr * 1.e6, time, count);
		count = adjust_rep_count (count, time, tmin, tmax);
	}
	while ((time >tmax) || (time <tmin));

	/* Computes the time per function call of the GSL integrator method.  Calls 		the function integral_gen() "count" times and adjusts count until the total time is 		between 1 and 2 seconds. */
	printf("\n Timing for GSL Integrator:\n");
	do
	{
		timer_start ();
		for(i = 0; i < count; i++)
		{
			integral_gen(0, 100, vals1);
		}
		time = timer_stop ();
		tgsl = time /count;
		printf (" %10.2f usec     %10.6f sec    %10d\n",tgsl * 1.e6, time, count);
		count = adjust_rep_count (count, time, tmin, tmax);
	}
	while ((time >tmax) || (time <tmin));

	/*Prints time per function call for leibniz and bbp series, and then takes
	the ratio between the times to compare the relative speed*/
	printf("\nTime per function call for Recur:      %10.6f usec", trr*1.e6);
	printf("\nTime per function call for GSL:        %10.6f usec", tgsl*1.e6);
	int ratio = round(tgsl/trr);
	printf("\nRatio of function times tgsl/trr:        %d\n",ratio);

}


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

double square(double x, void* params){
  return x*x;
}

double square_int(double x){
  return (x*x*x)/3;
}

double root(double x, void * params){
  return sqrt(x);
}

double root_int(double x){
  return (2.0/3) * sqrt(x*x*x);
}

double osc(double x, void* params){
  double a = *(double *) params;
  return a * cos(a* x);
}

double osc_int(double x, double a){
  return sin(a* x);
}

double rev(double x, void* params){
  return 1.0/ (x*x);
}

double rev_int(double x){
  return (1.0) /x;
}

int
main (void)
{
  gsl_integration_workspace * w
    = gsl_integration_workspace_alloc (1000);

  double result;
  double error;
  gsl_function F;
  printf("method,funct,result,expected,est_error,intervals\n" );

  F.function = □
  // F.params = α
  size_t neval = 5;
  double desired_error;

  //////////////////////////////
  // QNG
  desired_error = 1.0;
  for(int i=0; i<6; i++){
    F.function = □

    gsl_integration_qng(&F, 0, 1, 0, desired_error, &result, &error, &neval);
    printf("qng,square,%.18f,%.18f,%.18f,%zd\n",result,square_int(1),error,neval );
    F.function = root;
    gsl_integration_qng(&F, 0.001, 1, 0, desired_error, &result, &error, &neval);
    printf("qng,root,%.18f,%.18f,%.18f,%zd\n",result,root_int(1),error,neval );
    desired_error = desired_error / 10;
  }

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

  ////////////////////////
  //QAG

  desired_error = 1.0;

  neval = 15;
  for(int i=0; i<6; i++){
    F.function = □
    gsl_integration_qag(&F, 0, 1, 0, desired_error, 1000, i, w, &result, &error);
    printf("qag,square,%.18f,%.18f,%.18f,%zd\n",result,square_int(1),error,w->size);
    F.function = root;
    gsl_integration_qag(&F, 0.001, 1, 0, desired_error, 1000, i, w, &result, &error);
    printf("qag,root,%.18f,%.18f,%.18f,%zd\n",result,root_int(1),error,w->size );
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }

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

  ////////////////////////
  //QAGS

  desired_error = 1.0;

  neval = 15;
  for(int i=0; i<6; i++){
    F.function = □
    gsl_integration_qags(&F, 0, 1, 0, desired_error, 1000, w, &result, &error);
    printf("qags,square,%.18f,%.18f,%.18f,%zd\n",result,square_int(1),error,w->size);
    F.function = root;
    gsl_integration_qags(&F, 0.001, 1, 0, desired_error, 1000, w, &result, &error);
    printf("qags,root,%.18f,%.18f,%.18f,%zd\n",result,root_int(1),error,w->size);
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }
  /////

  ////////////////////////
  //QAGP

  double pts[2];
  pts[0] = 0;
  pts[1] = 1;
  desired_error = 1.0;

  neval = 15;
  for(int i=0; i<6; i++){
    F.function = □
    gsl_integration_qagp(&F, pts, 2, 0, desired_error, 1000, w, &result, &error);
    printf("qags,square,%.18f,%.18f,%.18f,%zd\n",result,square_int(1),error,w->size);
    F.function = root;
    gsl_integration_qagp(&F,pts, 2, 0, desired_error, 1000, w, &result, &error);
    printf("qags,root,%.18f,%.18f,%.18f,%zd\n",result,root_int(1),error,w->size);
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }
  /////

  /////////////////
  // Oscillation
  //////////
  // QNG
  double stop;
  double alpha;
  alpha = 4.0;
  F.params = α
  F.function = &osc;
  desired_error = 1.0;
  double start=1.0;
  stop = (9.0 / (2.0 * M_PI));
  for(int i=0; i<6; i++){
    gsl_integration_qng(&F, start, stop, 0, desired_error, &result, &error, &neval);
    printf("qng,osc,%.18f,%.18f,%.18f,%zd\n",result,(osc_int(stop, alpha) - osc_int(start, alpha)),error,neval);
    desired_error = desired_error / 10;
  }

  // QAG
  alpha = 4.0;
  F.params = α
  F.function = &osc;
  desired_error = 1.0;
  stop = (9.0 / (2.0 * M_PI));
  neval=15;
  for(int i=0; i<6; i++){
    gsl_integration_qag(&F, start, stop, 0, desired_error, 1000, i, w, &result, &error);
    printf("qag,osc,%.18f,%.18f,%.18f,%zd\n",result,(osc_int(stop, alpha) - osc_int(start, alpha)),error,neval);
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }

  //QAWO
  gsl_integration_qawo_table * wf = gsl_integration_qawo_table_alloc(alpha, (stop - start), GSL_INTEG_SINE, 2);
  desired_error = 1.0;

  for(int i=0; i<6; i++){
    gsl_integration_qawo(&F, start, 0, desired_error, 1000, w, wf, &result, &error);
    printf("qawo,osc,%.18f,%.18f,%.18f,%zd\n",result,(osc_int(stop, alpha) - osc_int(start, alpha)),error, w->size);
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }
  gsl_integration_qawo_table_free(wf);

  ///QAGIU
  F.function = &rev;
  desired_error = 1.0;
  start = 2.5;
  for(int i=0; i<6; i++){
    gsl_integration_qagiu(&F, start, 0, desired_error, 1000, w, &result, &error);
    printf("qagiu,osc,%.18f,%.18f,%.18f,%zd\n",result,(rev_int(start)),error, w->size);
    desired_error = desired_error / 10;
    if(i==0) neval+=6;
    else neval +=10;
  }

  gsl_integration_workspace_free (w);
  return 0;
}


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

#include "optimization.h"

/*****************************************************************************/
/* vector routines */

double vec_dot_prod(const double *v1, const double *v2, const int N)
{
    int i;
    double nn = 0;
    for (i=0;i<N;i++)
    {
        nn += v1[i] * v2[i];
    }
    return nn;
}

double vec_norm(const double *v1, const int N)
{
    return sqrt(vec_dot_prod(v1, v1, N));
}

/*****************************************************************************/
/* Wolfe conditions */

int check_armijo(double new_misfit, double old_misfit,
                 double dir_deriv, double step_len)
{
    /* returns 1 if Armijo condition satisfied, 0 otherwise */
    if (new_misfit <= old_misfit + WOLFE_C1 * step_len * dir_deriv)
    {
        /* step length okay */
        return 1;
    }
    return 0;
}

int check_curvature(double dir_deriv_old, double dir_deriv_new)
{
    /* returns 1 if curvature satisfied, 0 otherwise */
    if (dir_deriv_new >= WOLFE_C2 * dir_deriv_old)
    {
        return 1;
    }
    return 0;
}

int check_strong_curvature(double dir_deriv_old, double dir_deriv_new)
{
    /* returns 1 if strong curvature satisfied, 0 otherwise */
    if (dir_deriv_new >=  WOLFE_C2 * dir_deriv_old &&
        dir_deriv_new <= -WOLFE_C2 * dir_deriv_old)
    {
        return 1;
    }
    return 0;
}

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

double initial_step_length(search_dir sd, double alpha_old, double dir_deriv_old, double dir_deriv_new)
{
    /* calculate initial step length for a line search step         */
    /* inputs:                                                      */
    /*  sd: search direction, CG, STEEPEST, or LBFGS                */
    /*  alpha_old: previous (accepted) step length                  */
    /*  dir_deriv_old: directional derivative at previous step      */
    /*      (dot product of previous gradient and search direction) */
    /*  dir_deriv_new: directional derivative at current step       */
    /*      (dot product of current gradient and search direction)  */

    if (sd == CG || sd == STEEPEST)
    {
        /* see Nocedal  & Wright 2006, p. 59 */
        return alpha_old * (dir_deriv_old / dir_deriv_new);
    }

    /* quasi-Newton methods are well scaled use steplength of 1.0 for L-BFGS */
    return 1.0;

}

/*****************************************************************************/
/* steepest descent direction */

void steepest_descent_direction(const double *grad, double *p, const int N)
{
    int i;
    for (i=0;i<N;i++)
    {
        p[i] = -grad[i];
    }
}

/*****************************************************************************/
/* conjugate gradient direction */

void conjugate_grad_direction(double *grad_old, double *grad_new, double *p_old, double *p_new, int N)
{
    /*
    Determine a search direction by the conjugate gradient method

    Uses PR-FR conjugate gradient method to calculate a conjugate direection,
    see Nocedal & Wright 2006, p. 123.

    All vectors should be double precision of length N.
    grad_old: previous gradient
    grad_new: current gradient
    p_old: previous search direction
    p_new: new search direction (output, must be allocated already)
    */

    /* calculate beta values for both PR and FR methods */
    double beta_lower = 0;
    double beta_upper_FR = 0;
    double beta_upper_PR = 0;

    int i;
    for (i=0;i<N;i++)
    {
        beta_lower += grad_old[i] * grad_old[i];
        beta_upper_FR += grad_new[i] * grad_new[i];
        beta_upper_PR += grad_new[i] * (grad_new[i] - grad_old[i]);
    }

    double beta_FR = beta_upper_FR / beta_lower;
    double beta_PR = beta_upper_PR / beta_lower;

    /* select beta based on criteria given in Nocedal and Wright 2006 */
    double beta = 0;

    if (beta_PR < -beta_FR)
    {
        beta = -beta_FR;
    }
    else if (fabs(beta_PR) <= beta_FR)
    {
        beta = beta_PR;
    }
    else if (beta_PR > beta_FR)
    {
        beta = beta_FR;
    }

    /* calculate new search direction */
    for (i=0;i<N;i++)
    {
        p_new[i] = -grad_new[i] + beta * p_old[i];
    }
}

/*****************************************************************************/
/* L-BFGS direction */

void LBFGS_direction(double *grad_new, double *grad_old, double *model_new, double *model_old, double *p_new, int N, int m, int k)
{
    /* call with the last parameter negative to clean up local variables */

    int ii, jj;

    static double *s = NULL;
    static double *y = NULL;

    static double *q = NULL;
    static double *aa = NULL;
    static double *rho = NULL;
    static int *ind = NULL;

    double *r = p_new;

    /* allocate memory */
    /* note: opt_allocate is the same as malloc, but checks to make sure    */
    /* memory allocate was successful                                       */
    if (!s)
    {
        s = opt_allocate(sizeof(*s) * N * m);
    }

    if (!y)
    {
        y = opt_allocate(sizeof(*y) * N * m);
    }

    if (!q)
    {
        q = opt_allocate(sizeof(*q) * N);
    }

    if (!aa)
    {
        aa = opt_allocate(sizeof(*aa) * m);
    }

    if (!rho)
    {
        rho = opt_allocate(sizeof(*rho) * m);
    }

    if (!ind)
    {
        ind = opt_allocate(sizeof(*ind) * m);
        for (ii=0;ii<m;ii++)
        {
            ind[ii] = ii * N;
        }
    }

    /* j is the min of k and m */
    int j = (k < m) ? k : m;

    /* position to write new y and s vectors */
    int pos = ((k-1) % m) * N;

    /* write new y and s vectors */
    for (ii=0;ii<N;ii++)
    {
        y[ii+pos] = grad_new[ii] - grad_old[ii];
        s[ii+pos] = model_new[ii] - model_old[ii];
    }

    /* calculate rho */
    rho[pos / N] = 1.0 / vec_dot_prod(y+pos, s+pos, N);

    /* calculate gamma */
    double gamma = vec_dot_prod(s+pos, y+pos, N) / vec_dot_prod(y+pos, y+pos, N);

    if (k<m)
    {
        /* shift ind vector so ind[0] is still the index
        of the oldest element */
        int first_ele = ind[0];
        for (ii=0;ii<m-1;ii++)
        {
            ind[ii] = ind[ii+1];
        }
        ind[m-1] = first_ele;
    }

    /* assign initial q value */
    for (jj=0;jj<N;jj++)
    {
        q[jj] = grad_new[jj];
    }

    /*begin backwards loop */
    for (ii=j-1;ii>=0;ii--)
    {
        aa[ii] = rho[ind[ii] / N] * vec_dot_prod(s+ind[ii], q, N);

        for (jj=0;jj<N;jj++)
        {
            q[jj] = q[jj] - aa[ii] * (y+ind[ii])[jj];
        }
    }

    /* assign r */
    for (jj=0;jj<N;jj++)
    {
        r[jj] = gamma * q[jj];
    }

    /* begin forwards loop */
    double beta;
    for (ii=0;ii<j;ii++)
    {
        beta = rho[ind[ii] / N] * vec_dot_prod(y+ind[ii], r, N);

        for (jj=0;jj<N;jj++)
        {
            r[jj] = r[jj] + (s+ind[ii])[jj] * (aa[ii] - beta);
        }
    }

    /* negate r */
    for (jj=0;jj<N;jj++)
    {
        r[jj] = -r[jj];
    }

}

/*****************************************************************************/
#include <lapacke.h>
void newton_direction(const double *gradient, const double *hessian, const int N, double *p_new)
{
    /********************************************************/
    /* not implemented yet - use steepest descent for now   */
    //fprintf(stderr,"Newton direction not implemented yet\n");
    //fprintf(stderr,"Reverting to steepest descent\n");
    //steepest_descent_direction(gradient, p_new, N);
    /********************************************************/

    /* make a temp copy of the gradient*/
    double *temp_g = opt_allocate(sizeof(*temp_g) * N);
    double *temp_h = opt_allocate(sizeof(*temp_h) * N * N);

    memcpy(temp_g, gradient, sizeof(*temp_h) * N );

    /* need to check if Hessian is PD and modify if not                 */
    /* Note that this repeated performs a Cholesky factorization on     */
    /* the Hessian and will be expensive if N is large                  */
    /* In those cases, a modified Cholesky factorization (see Nocedal   */
    /* and Wright 2006, pp. 52-54) may be a better approach             */
    double beta = sqrt(DBL_EPSILON);
    int pd = hessian_modification(hessian, temp_h, N, beta);

    /* if we found a pd Hessian approximation, solve system for p       */
    /* otherwise, return steepest descent direction                     */
    if (pd != 1)
    {
        fprintf(stderr,"Could not find a positive definite "
                       "Hessian approximation\n");
        fprintf(stderr,"Returning steepest descent direction\n");
    }
    else
    {
        /* solve system for p: Hp = -g */
        int nrhs = 1;
        int lda = N;
        int ldb = N;
        LAPACKE_dpotrs(LAPACK_COL_MAJOR, 'L', N, nrhs, temp_h, lda, temp_g, ldb);
    }

    /* negate gradient or solution to system */
    int i;
    for (i = 0;i<N;i++)
        p_new[i]  = -temp_g[i];

    free(temp_h);
    free(temp_g);
}

#define HESSMOD_MAX_IT 100
int hessian_modification(const double *hessian, double *upper_factor, int N, double beta)
{
    /* beta may need to be adjusted based on the scaling of the problem */

    /* find minimum diagonal element of the hessian */
    int i;
    double min_diag = hessian[0];
    for (i=0;i<N;i++)
    {
        double diag = hessian[i*(N+1)];
        min_diag = (diag < min_diag) ? diag : min_diag;
    }

    /* find initial tau */
    double tau;
    if (min_diag > 0)
    {
        /* if it is already pd, min_diag will be positive */
        tau = 0.0;
    }
    else
    {
        tau = - min_diag + beta;
    }

    /* add multiples of the identity until hessian is positive definite */
    int k;
    int pd;
    int lda = N;
    for (k=0;k<HESSMOD_MAX_IT;k++)
    {
        /* copy hessian and add identity multiple */
        /* upper_factor will change every loop because of the dpotrf call */
        memcpy(upper_factor, hessian, sizeof(*hessian) * N *N);
        for (i=0;i<N;i++)
        {
            upper_factor[i * (N+1)] = hessian[i * (N+1)] + tau;
        }

        /* try cholesky factorization */
        pd = LAPACKE_dpotrf(LAPACK_COL_MAJOR, 'L', N, upper_factor, lda);
        if (pd == 0)
        {
            /* positive definite */
            return 1;
        }
        /* increment tau */
        tau = 2.0 * tau > beta ? 2.0 * tau : beta;

    }
    /* return 0 if not positive definite */
    return 0;

}

/*****************************************************************************/
/* function for performing line search */

void line_search(double *x_in,
                       double *x_out,
                       const int N,
                       optimization_parameters *opt_par,
                       void *aux
                       )
{

    double (*misfit)(const double *, const int, void *) = opt_par->objective;
    void (*grad)(const double *, double *, int, void *) = opt_par->grad;
    void (*hess)(const double *, double *, int, void *) = opt_par->hess;

    int success = 0;
    int i,j;

    /* note: opt_allocate is the same as malloc, but checks to make sure    */
    /* memory allocate was successful                                       */

    double *gradient = opt_allocate(sizeof(*gradient) * N);
    double *grad_old = opt_allocate(sizeof(*grad_old) * N);

    double *p = opt_allocate(sizeof(*p) * N);
    double *p_old = opt_allocate(sizeof(*p_old) * N);

    double *x_old = opt_allocate(sizeof(*x_old) * N);
    double *x_trial = opt_allocate(sizeof(*x_trial) * N);

    double *hessian = NULL;
    if (opt_par->sd == NEWTON)
        hessian = opt_allocate(sizeof(*hessian) * N * N);

    /* initial alpha_old and dir_deriv_old to keep the compiler from complaining,   */
    /* but these initialization values will never be used                           */
    double alpha_old = 1;
    double alpha_new;

    double dir_deriv_old = 1;
    double dir_deriv_new;

    /* copy x_in to x_old */
    for (i=0;i<N;i++)
    {
        x_out[i] = x_in[i];
    }

    /* evaluate function and gradient at initial point */
    double f_old = misfit(x_out, N, aux);
    grad(x_out, gradient, N, aux);

    /* check grad norm for exit condition */
    if (vec_norm(gradient, N) < opt_par->stopping_tolerance)
    {
        success = 1;
        free(p);
        free(gradient);
        return;
    }

    int m = opt_par->LBFGS_mem;

    /* start main loop */
    search_dir sd;
    int max_it = opt_par->max_iterations;
    for (j=0;j<max_it;j++)
    {
        printf("Starting line search, iteration %d\n",j);
        if (j == 0 && opt_par->sd != NEWTON)
        {
            /* first iteration - use steepest descent */
            /* get steepest descent direction */
            steepest_descent_direction(gradient, p, N);
            sd = STEEPEST;
            dir_deriv_new = vec_dot_prod(gradient, p, N);
            alpha_new = opt_par->initial_step_length;
        }
        else
        {
            if (opt_par->sd == LBFGS)
            {
                LBFGS_direction(gradient, grad_old, x_out, x_old, p, N, m, j);

                sd = LBFGS;
                dir_deriv_new = vec_dot_prod(gradient, p, N);
            }
            else if (opt_par->sd == CG)
            {
                conjugate_grad_direction(grad_old, gradient, p_old, p, N);
                sd = CG;
                dir_deriv_new = vec_dot_prod(gradient, p, N);
            }
            else if (opt_par->sd == NEWTON)
            {
                /* compute hessian */
                hess(x_out, hessian, N, aux);
                newton_direction(gradient, hessian, N, p);
                sd = NEWTON;
                dir_deriv_new = vec_dot_prod(gradient, p, N);
            }

            if (opt_par->sd == STEEPEST || (opt_par->angle_restart_allowed && check_angle_restart(gradient, p, N)))
            {
                steepest_descent_direction(gradient, p, N);
                sd = STEEPEST;
                dir_deriv_new = vec_dot_prod(gradient, p, N);
            }

            alpha_new = initial_step_length(sd, alpha_old, dir_deriv_old, dir_deriv_new);
        }

        if (opt_par->bracket_only || j==0)
        {
            alpha_new = ls_bracket(x_out, p, N, opt_par, alpha_new, aux);
        }
        else
        {
            alpha_new = back_track(x_out, p, N, opt_par, alpha_new, aux);
        }

        for (i=0;i<N;i++)
        {
            p_old[i] = p[i];
            grad_old[i] = gradient[i];
            x_old[i] = x_out[i];
        }

        if (!opt_par->backtrack_only && !opt_par->bracket_only && j!=0)
        {
            /* update model */
            for (i=0;i<N;i++)
            {
                x_trial[i] = x_out[i] + alpha_new * p[i];
            }

            /* evaluate gradient at new point */
            grad(x_trial, gradient, N, aux);

            /* check (strong) curvature */
            double ppi = vec_dot_prod(gradient, p, N);
            if (!check_strong_curvature(dir_deriv_new, ppi))
            {
                //alpha_new = initial_step_length(sd, alpha_old, dir_deriv_old, dir_deriv_new);
                alpha_new = ls_bracket(x_out, p, N, opt_par, alpha_new, aux);
            }

        }

        alpha_old = alpha_new;
        dir_deriv_old = dir_deriv_new;

        /* calculate new x value */
        for (i=0; i<N; i++)
        {
            x_out[i] = x_out[i] + alpha_new * p[i];
        }

        /* evaluate gradient at new point */
        grad(x_out, gradient, N, aux);

        /* check grad norm for exit condition */
        if (vec_norm(gradient, N) < opt_par->stopping_tolerance)
        {
            success = 1;
            break;
        }

    }

    printf("No. iterations: %d\n",j);

    free(p);
    free(p_old);
    free(gradient);
    free(grad_old);
    free(x_trial);
    free(x_old);

    if (hessian)
        free(hessian);

    if (!success)
    {
        fprintf(stderr,"Error: line search failed to find a solution\n");
        fprintf(stderr,"Point returned is not stationary\n");
    }

}

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

double ls_zoom(const double *x,
               const double *p,
               const int N,
               optimization_parameters *opt_par,
               double a_lo,
               double a_hi,
               void *aux
               )
{
    double (*misfit)(const double *, const int, void *) = opt_par->objective;
    void (*grad)(const double *, double *, int, void *) = opt_par->grad;

    int success = 0;

    double bb_star = 0.0;

    double mf;
    double *gradient = opt_allocate(sizeof(*gradient) * N);
    double *x_update = opt_allocate(sizeof(*x_update) * N);

    /* calculate initial misfit and gradient */
    mf = misfit(x, N, aux);
    grad(x, gradient, N, aux);

    double phi0 = mf;
    double phi_prime0 = vec_dot_prod(gradient, p, N);

    double phi_aj;
    double phi_prime_aj;

    double phi_a_lo;

    double mean_step;

    double tol = 0;

    /* start iterations */
    double a_j;
    int i, j;
    for (i=0;i<opt_par->bracket_zoom_iterations;i++)
    {
        printf("Bracket successful, starting zoom, iteration %d\n",i);
        printf("a_lo, a_hi: %f, %f\n", a_lo, a_hi);
        fflush(stdout);

        /* check if a_lo and a_hi are too close together */
        mean_step = 0.5 * (a_lo + a_hi);
        if (fabs(a_lo - a_hi) / mean_step < 0.01)
        {
            bb_star = mean_step;
            success = 1;
            break;
        }
        if (mean_step < 1e-6) /* <-- this should really depend on the scaling of the problem */
        {
            success = 0;
            break;
        }

        a_j = ls_cubic_interpolation(x, p, N, opt_par, a_lo, a_hi, aux);
        //a_j = (a_lo + a_hi) / 2.0;

        tol = mean_step * 0.01; /* one percent of the mean step */
        //fprintf(stderr, "Trial step: %f\n",a_j);
        if (fabs(a_j - a_lo) < tol || fabs(a_j - a_hi) < tol)
        {
            /* too close to boundary of interval */
            //fprintf(stderr,"ls_zoom: too close to boundary\n");
            //success = 0;
            //break;
            a_j = mean_step;
        }

        if (isnan(a_j) || isinf(a_j))
        {
            fprintf(stderr,"nan/inf value\n");
            success = 0;
            break;
        }

        /* evaluate function at a_lo (don't need dir. derivative) */
        for (j=0;j<N;j++)
        {
            x_update[j] = x[j] + a_lo * p[j];
        }
        phi_a_lo = misfit(x_update, N, aux);

        /* evaluate function at a_j. directional derivative will be         */
        /* calculated later if needed                                       */
        for (j=0;j<N;j++)
        {
            x_update[j] = x[j] + a_j * p[j];
        }
        phi_aj = misfit(x_update, N, aux);

        printf("Trial step length %E misfit: %E\n", a_j, phi_aj);

        if (phi_aj > phi0 + WOLFE_C1 * a_j * phi_prime0
            || phi_aj >= phi_a_lo)
        {
            /* trial step length violates Armijo or                         */
            /* trial step evaluate to a function value greater than a_lo    */
            /* need to reduce step length                                   */
            a_hi = a_j;
        }
        else
        {
            /* trial step length satisfies Armijo and function value is     */
            /* less than a_lo                                               */
            /* check curvature                                              */
            grad(x_update, gradient, N, aux);
            phi_prime_aj = vec_dot_prod(gradient, p, N);
            printf("Trial step length %E dir deriv: %E\n", a_j, phi_prime_aj);
            if (fabs(phi_prime_aj) <= -WOLFE_C2 * phi_prime0)
            {
                /* trial step length satisfies strong curvature condition   */
                /* accept trial step length                                 */
                bb_star = a_j;
                success = 1;
                break;
            }

            if (phi_prime_aj * (a_hi - a_lo) >= 0)
            {
                a_hi = a_lo;
            }

            /* satisfies Armijo, but not curvature                          */
            /* increase step length                                         */
            a_lo = a_j;

        }
    }

    free(x_update);
    free(gradient);

    if (!success)
    {
        fprintf(stderr,"Line search zoom failed to return an "
        "appropriate step length\n");
        //exit(-90);
        return -1;
    }

    return bb_star;
}

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

double ls_cubic_interpolation(const double *x,
               const double *p,
               const int N,
               optimization_parameters *opt_par,
               const double bound1,
               const double bound2,
               void *aux
               )
{
    /* calculates a new trial step length from cubic interpolation  */
    /* interpolation is based on two function values and the        */
    /* corresponding directional derivatives                        */
    /*                                                              */
    /* This interpolation requires two function evaluations and     */
    /* two gradient evaluations. If the function or gradient is     */
    /* expensive to evaluate, there are probably better ways to     */
    /* calculate an appropriate step length                         */
    /*                                                              */
    /* see Nocedal and Wright 2006, equation 3.59                   */
    /*                                                              */
    /* inputs:                                                      */
    /*      x: current point in parameter space                     */
    /*      p: search direction                                     */
    /*      N: number of dimensions                                 */
    /*      misfit: function that takes point and returns objective */
    /*              function value. x and N as defined above        */
    /*      grad: function that takes point and returns gradient    */
    /*            value (returned as parameter)                     */
    /*      bound1, bound2: bounds of step length search            */
    /*                                                              */
    /*  new trial step length is returned                           */

    double (*misfit)(const double *, const int, void *) = opt_par->objective;
    void (*grad)(const double *, double *, int, void *) = opt_par->grad;

    double *gradient = opt_allocate(sizeof(*gradient) * N);
    double *x_update = opt_allocate(sizeof(*x_update) * N);

    double f1, f2, g1, g2;

    int i;

    /* evaluate objective function and directional derivative at bound1 */
    for (i=0;i<N;i++)
    {
        x_update[i] = x[i] + bound1 * p[i];
    }
    f1 = misfit(x_update, N, aux);
    grad(x_update, gradient, N, aux);
    g1 = vec_dot_prod(gradient, p, N);

    printf("misfit at    %E: %E\n", bound1, f1);
    printf("dir deriv at %E: %E\n", bound1, g1);

    /* evaluate objective function and directional derivative at bound2 */
    for (i=0;i<N;i++)
    {
        x_update[i] = x[i] + bound2 * p[i];
    }
    f2 = misfit(x_update, N, aux);
    grad(x_update, gradient, N, aux);
    g2 = vec_dot_prod(gradient, p, N);

    printf("misfit at    %E: %E\n", bound2, f2);
    printf("dir deriv at %E: %E\n", bound2, g2);

    /*calculate cubic approximateion */
    double d1 = g1 + g2 - 3 * ((f1 - f2)/(bound1 - bound2));

    /* note that both bound1 and bound2 should always be positive   */
    /* they should never be equal                                   */
    /* d2_sign is sign(bound2 - bound1)                             */
    double d2_sign = bound2 > bound1 ? 1.0 : -1.0;
    if ((bound2 - bound1) > 0)
        d2_sign = 1.0;
    else if ((bound2 - bound1) < 0)
        d2_sign = -1.0;
    else
        d2_sign = 0.0;

    double d2 = d2_sign * sqrt(d1 * d1 - g1 * g2);

    double a_new = bound2 - (bound2 - bound1) * ((g2 + d2 - d1)/(g2 - g1 + 2 * d2));

    /* the minimum is always either at a_new as calculated above orj    */
    /* at one of the end points. Check those here.                      */
    //a_new = (a_new < bound1) ? a_new : bound1;
    //a_new = (a_new < bound2) ? a_new : bound2;

    free(x_update);
    free(gradient);
    return a_new;
}

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

double ls_bracket(const double *x,
               const double *p,
               const int N,
               optimization_parameters *opt_par,
               double alpha0,
               void *aux
               )
{

    double (*misfit)(const double *, const int, void *) = opt_par->objective;
    void (*grad)(const double *, double *, int, void *) = opt_par->grad;

    int success = 0;
    double b_star;

    int steplength_max = 50;
    double beta_max = 5e30;
    double beta0 = 0;
    double beta1 = alpha0; /* initial step length */

    double *gradient = opt_allocate(sizeof(*gradient) * N);
    double *x_update = opt_allocate(sizeof(*x_update) * N);

    double p0;      /* phi(0)  */
    double pp0;     /* phi'(0) */
    /* evaluate objective and dir. derivative at initial point */
    p0 = misfit(x, N, aux);
    grad(x, gradient, N, aux);
    pp0 = vec_dot_prod(gradient, p, N);

    double pi;              /* phi(i)       */
    double ppi;             /* phi'(i)      */
    double pi_1  = p0;      /* phi(i-1)     */
    //double ppi_1 = pp0;     /* phi'(i-1)    */
    double b_i   = beta1;   /* beta_i       */
    double b_i_1 = beta0;   /* beta_{i-1}   */

    int i, j;
    for (i=0;i<steplength_max;i++)
    {
        printf("Starting bracketing, iteration %d\n",i);
        printf("Bracket bounds: %E, %E\n",b_i_1, b_i);
        fflush(stdout);

        /* calculate objective and derivative values at b_i */
        for (j=0;j<N;j++)
        {
            x_update[j] = x[j] + b_i * p[j];
        }
        pi = misfit(x_update, N, aux);

        if ((pi > p0 + WOLFE_C1 * b_i * pp0) || (pi >= pi_1 && i>0))
        {
            b_star = ls_zoom(x, p, N, opt_par, b_i_1, b_i, aux);
            if (b_star < 0)
            {
                /* ls_zoom failed - try bigger steps */
                b_i = 2 * alpha0;
                b_i_1 = alpha0;
                continue;
            }
            success = 1;
            break;
        }

        grad(x_update, gradient, N, aux);
        ppi = vec_dot_prod(gradient, p, N);
        if (fabs(ppi) <= -WOLFE_C2 * pp0)
        {
            b_star = b_i;
            success = 1;
            break;
        }

        if (ppi >= 0.0)
        {
            b_star = ls_zoom(x, p, N, opt_par, b_i, b_i_1, aux);
            if (b_star < 0)
            {
                /* ls_zoom failed - try bigger steps */
                b_i = 2 * alpha0;
                b_i_1 = alpha0;
                continue;
            }
            success = 1;
            break;
        }

        /* update b_i and b_i_1 */
        b_i_1 = b_i;
        b_i = 2 * b_i;

        if (b_i > beta_max)
        {
            fprintf(stderr,"Error: ls_bracket: max step length reached\n");
            success = 0;
            break;
        }
    }

    free(x_update);
    free(gradient);

    if (!success)
    {
        fprintf(stderr,"Line search bracket failed to return an "
        "appropriate step length\n");
        exit(-90);
    }

    printf("Step length successfully selected. Step length: %E\n",b_star);
    fflush(stdout);
    return b_star;
}

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

int check_angle_restart(double *grad, double *p, int N)
{
    int restart = 0;
    double angle_tol_deg = 95.0;
    double angle_tol_rad = angle_tol_deg * (M_PI / 180.0);

    double grad_norm = vec_norm(grad, N);
    double p_norm = vec_norm(p, N);
    double pg = vec_dot_prod(p, grad, N);
    double ang = pg / (p_norm * grad_norm);
    if(ang > cos(angle_tol_rad))
    {
        restart = 1;
    }

    return restart;
}

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

double back_track_quad(double phi0, double phip0, double phi_a0, double a0)
{
    /* see Nodedal and Wright, p. 58 (equation 3.57 and 3.58) */

    double a1;

    double num = phip0 * a0 * a0;
    double den = phi_a0 - phi0 - phip0 * a0;

    a1 = -0.5 * (num / den);
    return a1;
}

double back_track_cubic(double phi0,
                        double phip0,
                        double phi_a0,
                        double phi_a1,
                        double a0,
                        double a1)
{
    /* see Nocedal and Wright 2006, p. 58 */

    double a, b, a2;

    double a02 = a0 * a0;   /* a0 squared   */
    double a12 = a1 * a1;   /* a1 squared   */
    double a03 = a02 * a0;  /* a0 cubed     */
    double a13 = a12 * a1;  /* a1 cubed     */

    double v1 = phi_a1 - phi0 - phip0 * a1;
    double v2 = phi_a0 - phi0 - phip0 * a0;

    double norm = 1.0 / (a02 * a12 * (a1 - a0));

    a = norm * (a02 * v1 - a12 * v2);
    b = norm * (-a03 * v1 + a13 * v2);

    a2 = (-b + sqrt(b * b - 3 * a * phip0)) / (3 * a);

    return a2;
}

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

double back_track(double *x,
                  double *p,
                  int N,
                  optimization_parameters *opt_par,
                  double alpha,
                  void *aux
                  )
{
    static double a[2]; /* holds a set of step lengths that can be used for bracketing */
    a[0] = 0;
    a[1] = 0;

    double (*misfit)(const double *, const int, void *) = opt_par->objective;
    void (*grad)(const double *, double *, int, void *) = opt_par->grad;

    int i;

    double phi0, phip0;
    double a0, a1, a2;
    double phi_a0, phi_a1, phi_a2;

    double *gradient = opt_allocate(sizeof(*gradient) * N);
    double *x_update = opt_allocate(sizeof(*x_update) * N);

    /* evaluate initial misfit and directional derivative */
    phi0 = misfit(x, N, aux);
    grad(x, gradient, N, aux);
    phip0 = vec_dot_prod(gradient, p, N);

    /* try a0*/
    a0 = alpha;
    a[1] = a0;
    for (i=0;i<N;i++)
    {
        x_update[i] = x[i] + a0 * p[i];
    }
    phi_a0 = misfit(x_update, N, aux);

    /* check Armijo condition */
    if (phi_a0 <= phi0 + WOLFE_C1 * a0 * phip0)
    {
        /* condition satisfied */
        free(x_update);
        free(gradient);
        return a0;
    }

    /* try quadratic approximation */
    a1 = back_track_quad(phi0, phip0, phi_a0, a0);
    a[0] = a1;

    for (i=0;i<N;i++)
    {
        x_update[i] = x[i] + a1 * p[i];
    }
    phi_a1 = misfit(x_update, N, aux);

    /* check Armijo condition */
    if (phi_a1 <= phi0 + WOLFE_C1 * a1 * phip0)
    {
        /* condition satisfied */
        free(x_update);
        free(gradient);
        return a1;
    }

    /* move into cubic approximations */
    int cubic_max = 10; /* <----------May need to adjust max number of iterations */
    int j;

    for (j=0;j<cubic_max;j++)
    {
        a2 = back_track_cubic(phi0, phip0, phi_a0, phi_a1, a0, a1);

        /* make sure a2 is not too close to a1 */
        if (fabs(a2 - a1) / a1 < 0.01) /* <--------- May need to adjust the factor of 0.01 here */
            a2 = 0.5 * a1;

        /* make sure a2 is not too much smaller than a1 */
        if (a2 < (0.01 * a1)) /* <-------- May need to adjust the factor of 0.01 here */
            a2 = 0.5 * a1;

        a[1] = a[0];
        a[0] = a2;

        for (i=0;i<N;i++)
        {
            x_update[i] = x[i] + a2 * p[i];
        }
        phi_a2 = misfit(x_update, N, aux);

        /* check Armijo condition */
        if (phi_a2 <= phi0 + WOLFE_C1 * a2 * phip0)
        {
            /* condition satisfied */
            break;
        }

        /* keep only the two newest values */
        a0 = a1;
        phi_a0 = phi_a1;
        a1 = a2;
        phi_a1 = phi_a2;
    }

    if (j >= cubic_max)
    {
        fprintf(stderr,"Backtrack failed to return an "
                       "appropriate step length\n");
        //exit(1);
    }

    free(x_update);
    free(gradient);
    return a2;
}

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

double max3(double x1, double x2, double x3)
{
    double mm = (x1 > x2) ? x1 : x2;
    mm = (mm > x3) ? mm : x3;
    return mm;

}

void modified_chol(double *A, int N)
{

    /* make temp copy of A */
    double *temp = opt_allocate(sizeof(*temp) * N * N);
    double *c = opt_allocate(sizeof(*c) * N * N);
    double *d = opt_allocate(sizeof(*d) * N);
    memcpy(temp, A, sizeof(*temp) * N *N);

    int i, j, s;

    double sum = 0;

    double beta = 1.0;
    double delta = 1.0e-3;
    double theta = 0.0;

    for (j = 0; j < N; j++)
    {
        for (s=0;s<j;s++)
        {
            sum = d[s] * A[j+s*N] * A[j+s*N];
        }

        c[j+j*N] = temp[j+j*N] - sum;
        A[j+j*N] = 1.0;

        //d[j] = c[j+j*N];
        d[j] = max3(fabs(c[j+j*N]),pow(theta / beta,2.0),delta);
        theta = DBL_MIN;
        for (i = j+1; i<N;i++)
        {
            for (s=0;s<j;s++)
            {
                sum = d[s] * A[i+s*N] * A[j+s*N];
            }

            c[i+j*N] = temp[j+i*N] -sum;
            theta = theta > c[i+j*N] ? theta : c[i+j*N];
            A[i+j*N] = c[i+j*N] / d[j];
        }
    }

    for (j=0;j<N;j++)
    {
        for (i=j;i<N;i++)
        {
            A[i+j*N] = A[i+j*N] * sqrt(d[j]);
        }
    }

    //printf("Diag:\n");
    //for (i=0;i<N;i++)
    //    printf("%f\n",d[i]);

    free(d);
    free(c);
    free(temp);
}

void* opt_allocate(long nbytes)
{
    void *ptr = malloc(nbytes);
    if (!ptr)
    {
        fprintf(stderr, "Error: optimization.c : memory allocation failure\n");
        exit (1);
    }
    return ptr;
}


#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_multifit.h>
#include <gsl/gsl_multilarge.h>
#include <gsl/gsl_blas.h>

/* function to be fitted */
double
func(const double t)
{
  double x = sin(10.0 * t);
  return exp(x*x*x);
}

/* construct a row of the least squares matrix */
int
build_row(const double t, gsl_vector *row)
{
  const size_t p = row->size;
  double Xj = 1.0;
  size_t j;

  for (j = 0; j < p; ++j)
    {
      gsl_vector_set(row, j, Xj);
      Xj *= t;
    }

  return 0;
}

int
solve_system(const int print_data, const gsl_multilarge_linear_type * T,
             const double lambda, const size_t n, const size_t p,
             gsl_vector * c)
{
  const size_t nblock = 5;         /* number of blocks to accumulate */
  const size_t nrows = n / nblock; /* number of rows per block */
  gsl_multilarge_linear_workspace * w =
    gsl_multilarge_linear_alloc(T, p);
  gsl_matrix *X = gsl_matrix_alloc(nrows, p);
  gsl_vector *y = gsl_vector_alloc(nrows);
  gsl_rng *r = gsl_rng_alloc(gsl_rng_default);
  const size_t nlcurve = 200;
  gsl_vector *reg_param = gsl_vector_alloc(nlcurve);
  gsl_vector *rho = gsl_vector_alloc(nlcurve);
  gsl_vector *eta = gsl_vector_alloc(nlcurve);
  size_t rowidx = 0;
  double rnorm, snorm, rcond;
  double t = 0.0;
  double dt = 1.0 / (n - 1.0);

  while (rowidx < n)
    {
      size_t nleft = n - rowidx;         /* number of rows left to accumulate */
      size_t nr = GSL_MIN(nrows, nleft); /* number of rows in this block */
      gsl_matrix_view Xv = gsl_matrix_submatrix(X, 0, 0, nr, p);
      gsl_vector_view yv = gsl_vector_subvector(y, 0, nr);
      size_t i;

      /* build (X,y) block with 'nr' rows */
      for (i = 0; i < nr; ++i)
        {
          gsl_vector_view row = gsl_matrix_row(&Xv.matrix, i);
          double fi = func(t);
          double ei = gsl_ran_gaussian (r, 0.1 * fi); /* noise */
          double yi = fi + ei;

          /* construct this row of LS matrix */
          build_row(t, &row.vector);

          /* set right hand side value with added noise */
          gsl_vector_set(&yv.vector, i, yi);

          if (print_data && (i % 100 == 0))
            printf("%f %f\n", t, yi);

          t += dt;
        }

      /* accumulate (X,y) block into LS system */
      gsl_multilarge_linear_accumulate(&Xv.matrix, &yv.vector, w);

      rowidx += nr;
    }

  if (print_data)
    printf("\n\n");

  /* compute L-curve */
  gsl_multilarge_linear_lcurve(reg_param, rho, eta, w);

  /* solve large LS system and store solution in c */
  gsl_multilarge_linear_solve(lambda, c, &rnorm, &snorm, w);

  /* compute reciprocal condition number */
  gsl_multilarge_linear_rcond(&rcond, w);

  fprintf(stderr, "=== Method %s ===\n", gsl_multilarge_linear_name(w));
  fprintf(stderr, "condition number = %e\n", 1.0 / rcond);
  fprintf(stderr, "residual norm    = %e\n", rnorm);
  fprintf(stderr, "solution norm    = %e\n", snorm);

  /* output L-curve */
  {
    size_t i;
    for (i = 0; i < nlcurve; ++i)
      {
        printf("%.12e %.12e %.12e\n",
               gsl_vector_get(reg_param, i),
               gsl_vector_get(rho, i),
               gsl_vector_get(eta, i));
      }
    printf("\n\n");
  }

  gsl_matrix_free(X);
  gsl_vector_free(y);
  gsl_multilarge_linear_free(w);
  gsl_rng_free(r);
  gsl_vector_free(reg_param);
  gsl_vector_free(rho);
  gsl_vector_free(eta);

  return 0;
}

int
main(int argc, char *argv[])
{
  const size_t n = 50000;   /* number of observations */
  const size_t p = 16;      /* polynomial order + 1 */
  double lambda = 0.0;      /* regularization parameter */
  gsl_vector *c_tsqr = gsl_vector_alloc(p);
  gsl_vector *c_normal = gsl_vector_alloc(p);

  if (argc > 1)
    lambda = atof(argv[1]);

  /* solve system with TSQR method */
  solve_system(1, gsl_multilarge_linear_tsqr, lambda, n, p, c_tsqr);

  /* solve system with Normal equations method */
  solve_system(0, gsl_multilarge_linear_normal, lambda, n, p, c_normal);

  /* output solutions */
  {
    gsl_vector *v = gsl_vector_alloc(p);
    double t;

    for (t = 0.0; t <= 1.0; t += 0.01)
      {
        double f_exact = func(t);
        double f_tsqr, f_normal;

        build_row(t, v);
        gsl_blas_ddot(v, c_tsqr, &f_tsqr);
        gsl_blas_ddot(v, c_normal, &f_normal);

        printf("%f %e %e %e\n", t, f_exact, f_tsqr, f_normal);
      }

    gsl_vector_free(v);
  }

  gsl_vector_free(c_tsqr);
  gsl_vector_free(c_normal);

  return 0;
}