#pragma once

#include <Python.h>  // has to be the first included header

#ifdef HAVE_LAPACK
#include <lapacke.h>
#endif

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

using namespace std;

class SvdUtility
{

 public:

  static void getSVD(double input[], int rows, int cols, double leftSingVec[]);

  static void getSVD(double input[], int rows, int cols, double leftSingVec[], double sigma[], double rightSingVecT[]);

  static void getSVD(double _Complex input[], int rows, int cols, double _Complex leftSingVec[], double sigma[], double _Complex rightSingVecT[]);

  static void reconstructSnapshots(int rows, int cols, double leftSingVec[], double sigma[], double rightSingVecT[], double output[]);

  static void printMatrix(std::string name, double input[], int rows, int cols);

  static vector<double> readCSV(string filename);

  static vector<double> readCSV(string filename, int rows);

  static void writeCSV(string filename, vector<double> values, int m, int n);

  static void writeCSV(string filename, double values[], int m, int n, bool columnWise = false);

  static int getCSVRowCount(string filename);

  static int getCSVColumnCount(string filename);
};


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

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

#include "demof.h"
#include "demof.c"

int
main ()
{
  int status;
  int iterations = 0, max_iterations = 100;
  gsl_root_fsolver *s;
  double r = 0, r_expected = sqrt (5.0);
  double x_lower x = 0.0, x_upper = 5.0;
  gsl_function F;
  struct quadratic_params params =
  {1.0, 0.0, -5.0};

  F.function = &quadratic;
  F.params = &params;

  s = gsl_root_fsolver_alloc (gsl_root_fsolver_bisection);
  gsl_root_fsolver_set (s, &F, x);

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

  printf ("%5s [%9s, %9s] %9s %9s %10s %9s\n",
          "iter", "lower", "upper", "root", "actual", "err", "err(est)");

  do
    {
      iterations++;
      status = gsl_root_fsolver_iterate (s);
      r = gsl_root_fsolver_root (s);
      x_lower = gsl_root_fsolver_x_lower (s);
      x_upper = gsl_root_fsolver_x_upper (s);
      status = gsl_root_test_interval (x, 0, 0.001);

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

      printf ("%5d [%.7f, %.7f] %.7f %.7f %+.7f %.7f\n",
              iterations, x_lower, x_upper,
              r, r_expected, r - r_expected, x_upper - x_lower);
    }
  while (status == GSL_CONTINUE && iterations < max_iterations);

}


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

/* Runge-Kutta 2, Gaussian implicit. Also known as implicit midpoint rule.

   Non-linear equations solved by linearization, LU-decomposition
   and matrix inversion. For reference, see eg.

   Ascher, U.M., Petzold, L.R., Computer methods for ordinary
   differential and differential-algebraic equations, SIAM,
   Philadelphia, 1998.
 */

#include <config.h>
#include <stdlib.h>
#include <string.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_linalg.h>

#include "odeiv_util.h"

typedef struct
{
  double *Y1;
  double *y0;
  double *y0_orig;
  double *ytmp;
  double *dfdy;                 /* Jacobian */
  double *dfdt;                 /* time derivatives, not used */
  double *y_onestep;
  gsl_permutation *p;
}
rk2simp_state_t;

static void *
rk2simp_alloc (size_t dim)
{
  rk2simp_state_t *state =
    (rk2simp_state_t *) malloc (sizeof (rk2simp_state_t));

  if (state == 0)
    {
      GSL_ERROR_NULL ("failed to allocate space for rk2simp_state",
                      GSL_ENOMEM);
    }

  state->Y1 = (double *) malloc (dim * sizeof (double));

  if (state->Y1 == 0)
    {
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for Y1", GSL_ENOMEM);
    }

  state->y0 = (double *) malloc (dim * sizeof (double));

  if (state->y0 == 0)
    {
      free (state->Y1);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0", GSL_ENOMEM);
    }

  state->y0_orig = (double *) malloc (dim * sizeof (double));

  if (state->y0_orig == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y0_orig", GSL_ENOMEM);
    }

  state->ytmp = (double *) malloc (dim * sizeof (double));

  if (state->ytmp == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state->y0_orig);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for ytmp", GSL_ENOMEM);
    }

  state->dfdy = (double *) malloc (dim * dim * sizeof (double));

  if (state->dfdy == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state->y0_orig);
      free (state->ytmp);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdy", GSL_ENOMEM);
    }

  state->dfdt = (double *) malloc (dim * sizeof (double));

  if (state->dfdt == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state->y0_orig);
      free (state->ytmp);
      free (state->dfdy);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for dfdt", GSL_ENOMEM);
    }

  state->y_onestep = (double *) malloc (dim * sizeof (double));

  if (state->y_onestep == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state->y0_orig);
      free (state->ytmp);
      free (state->dfdy);
      free (state->dfdt);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for y_onestep", GSL_ENOMEM);
    }

  state->p = gsl_permutation_alloc (dim);

  if (state->p == 0)
    {
      free (state->Y1);
      free (state->y0);
      free (state->y0_orig);
      free (state->ytmp);
      free (state->dfdy);
      free (state->dfdt);
      free (state->y_onestep);
      free (state);
      GSL_ERROR_NULL ("failed to allocate space for p", GSL_ENOMEM);
    }

  return state;
}

static int
rk2simp_step (double *y, rk2simp_state_t * state,
              const double h, const double t,
              const size_t dim, const gsl_odeiv_system * sys)
{
  /* Makes a Runge-Kutta 2nd order semi-implicit advance with step size h.
     y0 is initial values of variables y.

     The linearized semi-implicit equations to calculate are:

     Y1 = y0 + h/2 * (1 - h/2 * df/dy)^(-1) * f(t + h/2, y0)

     y = y0 + h * f(t + h/2, Y1)
   */

  const double *y0 = state->y0;
  double *Y1 = state->Y1;
  double *ytmp = state->ytmp;

  size_t i;
  int s, ps;

  gsl_matrix_view J = gsl_matrix_view_array (state->dfdy, dim, dim);

  /* First solve Y1.
     Calculate the inverse matrix (1 - h/2 * df/dy)^-1
   */

  /* Create matrix to J */

  s = GSL_ODEIV_JA_EVAL (sys, t, y0, state->dfdy, state->dfdt);

  if (s != GSL_SUCCESS)
    {
      return s;
    }

  gsl_matrix_scale (&J.matrix, -h / 2.0);
  gsl_matrix_add_diagonal(&J.matrix, 1.0);

  /* Invert it by LU-decomposition to invmat */

  s += gsl_linalg_LU_decomp (&J.matrix, state->p, &ps);

  if (s != GSL_SUCCESS)
    {
      return GSL_EFAILED;
    }

  /* Evaluate f(t + h/2, y0) */

  s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, y0, ytmp);

  if (s != GSL_SUCCESS)
    {
      return s;
    }

  /* Calculate Y1 = y0 + h/2 * ((1-h/2 * df/dy)^-1) ytmp */

  {
    gsl_vector_const_view y0_view = gsl_vector_const_view_array(y0, dim);
    gsl_vector_view ytmp_view = gsl_vector_view_array(ytmp, dim);
    gsl_vector_view Y1_view = gsl_vector_view_array(Y1, dim);

    s = gsl_linalg_LU_solve (&J.matrix, state->p,
                             &ytmp_view.vector, &Y1_view.vector);

    gsl_vector_scale (&Y1_view.vector, 0.5 * h);
    gsl_vector_add (&Y1_view.vector, &y0_view.vector);
  }

  /* And finally evaluation of f(t + h/2, Y1) and calculation of y */

  s = GSL_ODEIV_FN_EVAL (sys, t + 0.5 * h, Y1, ytmp);

  if (s != GSL_SUCCESS)
    {
      return s;
    }

  for (i = 0; i < dim; i++)
    {
      y[i] = y0[i] + h * ytmp[i];
    }

  return s;
}

static int
rk2simp_apply (void *vstate, size_t dim, double t, double h,
               double y[], double yerr[], const double dydt_in[],
               double dydt_out[], const gsl_odeiv_system * sys)
{
  rk2simp_state_t *state = (rk2simp_state_t *) vstate;

  size_t i;

  double *y0 = state->y0;
  double *y0_orig = state->y0_orig;
  double *y_onestep = state->y_onestep;

  /* Error estimation is done by step doubling procedure */

  DBL_MEMCPY (y0, y, dim);

  /* Save initial values in case of failure */
  DBL_MEMCPY (y0_orig, y, dim);

  /* First traverse h with one step (save to y_onestep) */
  DBL_MEMCPY (y_onestep, y, dim);

  {
    int s = rk2simp_step (y_onestep, state, h, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        return s;
      }
  }

  /* Then with two steps with half step length (save to y) */

  {
    int s = rk2simp_step (y, state, h / 2.0, t, dim, sys);

    if (s != GSL_SUCCESS)
      {
        /* Restore original y vector */
        DBL_MEMCPY (y, y0_orig, dim);
        return s;
      }
  }

  DBL_MEMCPY (y0, y, dim);

  {
    int s = rk2simp_step (y, state, h / 2.0, t + h / 2.0, dim, sys);

    if (s != GSL_SUCCESS)
      {
        /* Restore original y vector */
        DBL_MEMCPY (y, y0_orig, dim);
        return s;
      }
  }

  /* Derivatives at output */

  if (dydt_out != NULL)
    {
      int s = GSL_ODEIV_FN_EVAL (sys, t + h, y, dydt_out);

      if (s != GSL_SUCCESS)
        {
          /* Restore original y vector */
          DBL_MEMCPY (y, y0_orig, dim);
          return s;
        }
    }

  /* Error estimation */

  for (i = 0; i < dim; i++)
    {
      yerr[i] = 4.0 * (y[i] - y_onestep[i]) / 3.0;
    }

  return GSL_SUCCESS;
}

static int
rk2simp_reset (void *vstate, size_t dim)
{
  rk2simp_state_t *state = (rk2simp_state_t *) vstate;

  DBL_ZERO_MEMSET (state->Y1, dim);
  DBL_ZERO_MEMSET (state->y0, dim);
  DBL_ZERO_MEMSET (state->y0_orig, dim);
  DBL_ZERO_MEMSET (state->ytmp, dim);
  DBL_ZERO_MEMSET (state->dfdt, dim * dim);
  DBL_ZERO_MEMSET (state->dfdt, dim);
  DBL_ZERO_MEMSET (state->y_onestep, dim);

  return GSL_SUCCESS;
}

static unsigned int
rk2simp_order (void *vstate)
{
  rk2simp_state_t *state = (rk2simp_state_t *) vstate;
  state = 0;                    /* prevent warnings about unused parameters */
  return 2;
}

static void
rk2simp_free (void *vstate)
{
  rk2simp_state_t *state = (rk2simp_state_t *) vstate;
  free (state->Y1);
  free (state->y0);
  free (state->y0_orig);
  free (state->ytmp);
  free (state->dfdy);
  free (state->dfdt);
  free (state->y_onestep);
  gsl_permutation_free (state->p);
  free (state);
}

static const gsl_odeiv_step_type rk2simp_type = {
  "rk2simp",                    /* name */
  0,                            /* can use dydt_in? */
  1,                            /* gives exact dydt_out? */
  &rk2simp_alloc,
  &rk2simp_apply,
  &rk2simp_reset,
  &rk2simp_order,
  &rk2simp_free
};

const gsl_odeiv_step_type *gsl_odeiv_step_rk2simp = &rk2simp_type;


/*
 *   dotProduct.c
 *
 *   An MPI program that computes the inner product of two vectors
 *
 *   Written by cetinsamet -*- cetin.samet@metu.edu.tr
 *   April, 2019
 *
 */
#include <mpi.h>
#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>

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

    int i;
    int rank, size;

    double minStart, maxEnd;
    double dotP, locDotP;

    MPI_Init(&argc, &argv);
    MPI_Comm_size(MPI_COMM_WORLD, &size);
    MPI_Comm_rank(MPI_COMM_WORLD, &rank);

    int ORDER	= atoi(argv[1]);
    int locSize = ORDER / size;

    double* locU;	locU = (double*) malloc(locSize * sizeof(double));
    double* locV;	locV = (double*) malloc(locSize * sizeof(double));

    for (i = 0; i < locSize; ++i) {
        locU[i] = i;
        locV[i] = i;
    }

    double pStart = MPI_Wtime();    // <-- Start Time
    locDotP = cblas_ddot(locSize, locU, 1, locV, 1);
    MPI_Reduce(&locDotP, &dotP, 1, MPI_DOUBLE, MPI_SUM, 0, MPI_COMM_WORLD);
    double pEnd = MPI_Wtime();      // <-- Stop Time

    MPI_Reduce(&pStart,  &minStart, 1, MPI_DOUBLE, MPI_MIN, 0, MPI_COMM_WORLD);
    MPI_Reduce(&pEnd, &maxEnd, 1, MPI_DOUBLE, MPI_MAX, 0, MPI_COMM_WORLD);

    if (rank == 0)
    	printf("Dot product: %f\tVector size: %d\tElapsed time: %f\n", dotP, ORDER, maxEnd - minStart);

    MPI_Finalize();
    return 0;
}


    #ifndef LVVMODEL_H
    #define LVVMODEL_H

    #include <iostream>
    #include <iomanip>
    using std::fixed;
    using std::showpos;

    #include <string>
    #include <gsl/gsl_multifit.h>
    #include <gsl/gsl_poly.h>
    #include "lvv.h"
    #include "math.h"
    using lvv::powi;

    namespace lvv {

    int  const  poly_order = 4;

//	template<int M>
class   Model   {     public: ///////////////////////////////////////////////////////////// Model Data - μ² polinom fitting

Model () : n(0), m(0), x(NULL), y(NULL), X(NULL),  cov(NULL),  c(NULL)              {}

Model (double _x[], double _y[], int _n, int _m=-1) : n(_n), m(_m), x(_x), y(_y)    { ////////////////////////////////   CTOR

        // N - data points [0..N) ( 5 for NF )
 	// M - number of model parameters [0..M)
 	// if polynomial used in model:
 	//      M-1 -  poly order  C[0]..C[m-1]

	assert(_n >= 3 && "not implemented");
	if (_m == -1) {      //  then need autodetect
	    if      (n < 6 ) m = n-2;
	    else if (n < 10) m = n-3;
	    else if (n < 20) m = n-4;
	}

	assert(n >= m);

	X   = gsl_matrix_alloc (n, m) ;		assert(X);
	c   = gsl_vector_alloc (m   ) ;		assert(c);
	cov = gsl_matrix_alloc (m, m) ;		assert(cov);

	xv = gsl_vector_view_array (_x, n);
	yv = gsl_vector_view_array (_y, n);

	/////    INIT - PREP MATRIX for POLIFIT
	for (i = 0; i<n; i++)    for (int p=0; p<m; ++p)     gsl_matrix_set (X, i, p, powi(_x[i], p));

	/////    FIT
	gsl_multifit_linear_workspace *work = gsl_multifit_linear_alloc (n, m);
	//  gsl_multifit_linear     (X, y,                         c, cov, &chisq, work); // int gsl_multifit_linear     (const gsl_matrix *X, const gsl_vector *y,                           gsl_vector *c, gsl_matrix *cov, double *chisq, gsl_multifit_linear_workspace *ws)
	    gsl_multifit_linear_svd (X, &yv.vector, svd_tol, &rank, c, cov, &chisq, work); // int gsl_multifit_linear_svd (const gsl_matrix *X, const gsl_vector *y, double tol, size t* rank, gsl_vector *c, gsl_matrix *cov, double *chisq, gsl_multifit_linear_workspace *ws)
	gsl_multifit_linear_free (work);
 };

~Model() { ////////////////////////////////////////////////////////////////// DTOR
	if(cov)    gsl_matrix_free (cov);
	if(c  )    gsl_vector_free (c);
	if(X  )    gsl_matrix_free (X);
 };

double  estimate (const double x)   const {
	double y_est = gsl_vector_get(c,0) ;
	double pow_x = 1.0                 ; // x[0,0]^0

	for (int p=1;  p<m;  ++p) {
	    pow_x *= x;
	    y_est += gsl_vector_get(c,p)*pow_x;
	}

	return y_est;
 };

double  inverse_estimate (const double yy)    const {

	double xx = -1.0;

	double *C=c->data;

	if        (m>2) {

		double c0_save = C[0];
		C[0] =  c0_save-yy;

		struct { double real; double imag; } z[poly_order-1];

		int order = poly_order;
		//if ( abs(C[poly_order-1]) < 0.000001 )	// reduce polinomial order of last elem == 0
		if ( C[poly_order-1] == 0 )	// reduce polinomial order of last elem == 0
			order = poly_order-1;
		    			assert (C[poly_order-1] != 0);	// hope we don't need to do it twice

		//  solve
		gsl_poly_complex_workspace * w = gsl_poly_complex_workspace_alloc (order);
		gsl_poly_complex_solve (gsl_vector_ptr(c,0), order, w, (double *)z);
		gsl_poly_complex_workspace_free (w);

		//  select real root from list of complex roots
		double oorr_distance         = numeric_limits<double>::max(); // out_of_range root distance to normal interval
		bool   got_root              = false;
		bool   got_out_of_range_root = false;
		double out_of_range_root     = -100;
		double real_root             = -100000;

		//double x_step                = x[1]-x[0];                    // with step p-rmse go from 0.9904215  to 0.9907456
		double x_lower_bound         = x[0]    /* -x_step/2.0 */ ;
		double x_upper_bound         = x[n-1]  /* +x_step/2.0 */ ;

		for  (int i = 0;  i < order-1;  i++)    {				// for all roots
												//cerr << " \t("<<i<<") "<<  z[2*i] << "+i" << z[2*i+1];
			if  (abs(z[i].imag)  >  0.00000001)  continue;

			if   ( x_lower_bound <=  z[i].real   &&  z[i].real <=  x_upper_bound)   {  // in x valid range
				real_root = z[i].real;
				got_root  = true;
															//cerr << " rr=" << real_root;
				break;	// found it
			}

			double below_lower_distance =  x_lower_bound - z[i].real;
			if  (  below_lower_distance > 0 )  {
				if  (( got_out_of_range_root  &&  below_lower_distance < oorr_distance )  ||  ! got_out_of_range_root)  {
					oorr_distance         =   below_lower_distance; 	// new shortest oorr distance
					got_out_of_range_root = true;
					out_of_range_root     = x_lower_bound;
													//cerr  << " oor=" << out_of_range_root ;
					continue;
				}
			}

			double above_upper_distance =  z[i].real - x_upper_bound ;
			if   ( above_upper_distance > 0 )  {
				if (( got_out_of_range_root  && above_upper_distance  < oorr_distance )  ||  ! got_out_of_range_root)  {
					oorr_distance         = above_upper_distance;
					got_out_of_range_root = true;
					out_of_range_root     = x_upper_bound;
													//cerr  << " oor=" << out_of_range_root ;
					continue;
				}
			}

		};

	    if	     (got_root)				xx = real_root;
	    else  if (got_out_of_range_root)		xx = out_of_range_root;
	    else					{ xx=3.66; cerr << "#model::inverse_estimate(): root not found\n";  ; }
														//cerr << "  ROOT=" << xx << endl;

	    gsl_vector_set(c, 0, c0_save);

	    /*
	    double c0 =  gsl_vector_get(c,0);
	    double c1 =  gsl_vector_get(c,1);
	    double c2 =  gsl_vector_get(c,2);
	    double c3 =  gsl_vector_get(c,3);
	    if ( 0 > (27*pow2(c3)*pow2(yy)+(-54*c0*pow2(c3)+18*c1*c2*c3-4*pow3(c2))*yy+27*pow2(c0)*pow2(c3)+(4*pow3(c1)-18*c0*c1*c2)*c3+4*c0*pow3(c2)-pow2(c1)*pow2(c2)))  {
		cout << "inverse_estimate-4-1: cannot solve: c0-2" <<  c0 << " " << c1 << " " <<  c2 << " " << c3 << " y: " << yy << endl;
		return 3.6;
	    }
	    if ( 0 > 27*pow2(c3)*pow2(yy)+(-54*c0*pow2(c3)+18*c1*c2*c3-4*pow3(c2))*yy+27*pow2(c0)*pow2(c3)+(4*pow3(c1)-18*c0*c1*c2)*c3+4*c0*pow3(c2)-pow2(c1)*pow2(c2))  {
		cout << "inverse_estimate-4-2: cannot solve: c0-2" <<  c0 << " " << c1 << " " <<  c2 << " " << c3 << " y: " << yy << endl;
		return 3.6;
	    }
	    xx = cbrt((sqrt(27*pow2(c3)*pow2(yy)+(-54*c0*pow2(c3)+18*c1*c2*c3-4*pow3(c2))*yy+27*pow2(c0)*pow2(c3)+(4*pow3(c1)-18*c0*c1*c2)*c3+4*c0*pow3(c2)-pow2(c1)*pow2(c2))/(6*sqrt(3)*pow2(c3))-(-27*pow2(c3)*yy+27*c0*pow2(c3)-9*c1*c2*c3+2*pow3(c2))/(54*pow3(c3))))-cbrt((3*c1*c3-pow2(c2))/(9*pow2(c3)*(sqrt(27*pow2(c3)*pow2(yy)+(-54*c0*pow2(c3)+18*c1*c2*c3-4*pow3(c2))*yy+27*pow2(c0)*pow2(c3)+(4*pow3(c1)-18*c0*c1*c2)*c3+4*c0*pow3(c2)-pow2(c1)*pow2(c2))/(6*sqrt(3)*pow2(c3))-(-27*pow2(c3)*yy+27*c0*pow2(c3)-9*c1*c2*c3+2*pow3(c2))/(54*pow3(c3)))))-c2/(3*c3);

	} else if (m==3){
	    double c0 =  gsl_vector_get(c,0);
	    double c1 =  gsl_vector_get(c,1);
	    double c2 =  gsl_vector_get(c,2);

	    if(4*c2*yy-4*c0*c2+pow2(c1) < 0) {
		cout << "inverse_estimate-3: cannot solve: c0-2" <<  c0 << " " << c1 << " " <<  c2 << " y: " << yy << endl;
		return 3.6;
	    }

	    xx = (sqrt(4*c2*yy-4*c0*c2+pow2(c1))-c1)/(2*c2);
	    //xx = -(sqrt(4*c2*yy-4*c0*c2+pow2(c1))+c1)/(2*c2);

	   */
	} else if (m==2){
	    double c0 =  gsl_vector_get(c,0);
	    double c1 =  gsl_vector_get(c,1);
	    xx = (yy-c0)/c1;
	} else {
	    assert(false);
	}

	return xx;
 };

void	print (string title="")  const {
	cout << "# :gnuplot: set key bottom; set ytics 0,0.5; set yrange [-0.1:1.1]; set xrange [0:6]; set grid; set pointsize 1.5;"
		<< (title.length()>0  ?  "set title \" " +  title + "\";"  :  "") <<

		    "plot "
			" \"pipe\" using 1:3 title \"model\" with lines 5, "
			" \"pipe\" using 4:2 title \"inv x(y)\" with point 4,"
			" \"pipe\" using 1:2 title \"data\" with points 11 "
		";\n";

	format		table(" % 9f    %20t % 9f   %40t % 9f   %60t % 9f\n");
	string		_("\" \"");
							cout << table	% "# X" % "  Y"		% "Y-mod"     	% "X-mod-inv" ;

	for (int i=0;  i<n; ++i                )	cout << table	% x[i] 	% y[i] 		% _    		% _ ;
	for (double xx=0.0;  xx<=6.0; xx+=1./3.)	cout << table	% xx   	% _		% estimate(xx)	% _  ;
	for (double yy=0.0;  yy<=1.0; yy+=0.05 )	cout << table	% _ 	% yy   		% _    		% inverse_estimate(yy) ;
 };

private: // member vars
        int                  n         , m;
        const static double  svd_tol = 0.000001;
        size_t               rank;
        int                  i;
        double               xi        , yi, ei, chisq;
        gsl_vector_view      xv        , yv;
        double    	   * x;
        double             * y;
        gsl_matrix          *X         , *cov;

    public:
        gsl_vector          *c;
 };

    }      // namespace lvv
    #endif // LVVMODEL_H


#include <stdio.h>
#include <stdlib.h>
#include <cblas.h>
#include "utils.h"

int main() {
    int success_flag = 1;
    char * data_path = "../Test_Data/linear.csv";

    int cols = column_count(data_path);
    int rows = row_count(data_path) - 1;
    double data[rows][cols];

    int success = read_data(data_path, rows, cols, data);
    if(success != 0){
        return 1;
    }

    double *x = (double *) malloc(sizeof(double)*rows);
    double *y = (double *) malloc(sizeof(double)*rows);

    fetch_column(x, data, 1, rows, cols);
    fetch_column(y, data, 2, rows, cols);

    double xTx[rows];

    /* optimization: reuse x as read from data, dont read for each vector used */
    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, 1, rows, 1, x, rows, x, rows, 0.0, xTx, 1 );

    double xTy[rows];

    cblas_dgemm(CblasRowMajor, CblasNoTrans, CblasTrans, 1, 1, rows, 1, x, rows, y, rows, 0.0, xTy, 1 );

    printf("C: %.30f\n", xTx[0]);
    printf("C: %.30f\n", xTy[0]);

    free(x);
    free(y);
    return success_flag;
}


#include <math.h>
#include <gsl/gsl_errno.h>
#include "bicycle.h"
//#include <stdio.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_odeiv.h>

void eval_qdots(double qd[], const double q[], const double u[], const Bicycle_t *bike)
{
  qd[0] = (-sin(q[2])*u[0] + cos(q[2])*u[2]) / cos(q[1]);
  qd[1] = cos(q[2])*u[0] + sin(q[2])*u[2];
  qd[2] = sin(q[2])*tan(q[1])*u[0] + u[1] - cos(q[2])*tan(q[1])*u[2];
  qd[3] = u[3] - u[1];
  qd[4] = u[4] - u[2];
  qd[5] = sin(q[4])*u[0] - cos(q[4])*u[1] + u[5];
  qd[6] = -bike->rr*cos(q[0])*(qd[2] + qd[3]) - bike->rrt*(sin(q[0])*qd[1] +
          cos(q[0])*cos(q[1])*(qd[2] + qd[3]));
  qd[7] = -bike->rr*sin(q[0])*(qd[2] + qd[3]) + bike->rrt*(cos(q[0])*qd[1] +
          sin(q[0])*cos(q[1])*(qd[2] + qd[3]));
} // eval_qdots

void Bicycle_to_Bicycle_benchmark(Bicycle_benchmark_t *bike_b, const Bicycle_t *bike)
{
} // Bicycle_to_Bicycle_benchmark

void Bicycle_benchmark_to_Bicycle(Bicycle_t *bike, const Bicycle_benchmark_t *bike_b)
{
} // Bicycle_benchmark_to_Bicycle

void eval_dependent_speeds(double u[], double B[3][6], const int ind[], const int dep[])
{
  double Bi[3][3], Bd[3][3], Bd_adj[3][3], Bd_det, ui[3], ud[3];

  // Independent speeds
  ui[0] = u[ind[0]];
  ui[1] = u[ind[1]];
  ui[2] = u[ind[2]];

  // Dependent speeds
  ud[0] = u[dep[0]];
  ud[1] = u[dep[1]];
  ud[2] = u[dep[2]];

  // Form -Bi
  Bi[0][0] = -B[0][ind[0]];
  Bi[0][1] = -B[0][ind[1]];
  Bi[0][2] = -B[0][ind[2]];

  Bi[1][0] = -B[1][ind[0]];
  Bi[1][1] = -B[1][ind[1]];
  Bi[1][2] = -B[1][ind[2]];

  Bi[2][0] = -B[2][ind[0]];
  Bi[2][1] = -B[2][ind[1]];
  Bi[2][2] = -B[2][ind[2]];

  // Form Bd
  Bd[0][0] = B[0][dep[0]];
  Bd[0][1] = B[0][dep[1]];
  Bd[0][2] = B[0][dep[2]];

  Bd[1][0] = B[1][dep[0]];
  Bd[1][1] = B[1][dep[1]];
  Bd[1][2] = B[1][dep[2]];

  Bd[2][0] = B[2][dep[0]];
  Bd[2][1] = B[2][dep[1]];
  Bd[2][2] = B[2][dep[2]];

  // Forming Bd_adj and Bd_det
  madj3(Bd_adj, Bd);
  mdet3(&Bd_det, Bd);

  // Bd_adj * (-Bi), store the result in Bd instead of creating a new matrix to
  // store the result
  mm3(Bd, Bd_adj, Bi);
  mv3(ud, Bd, ui);

  // Form the dependent speeds
  u[dep[0]] = ud[0] / Bd_det;
  u[dep[1]] = ud[1] / Bd_det;
  u[dep[2]] = ud[2] / Bd_det;
} // eval_dependent_speeds

/*
 * 3x3 Matrix - Matrix multiplication
 */
void mm3(double AB[3][3], double A[][3], double B[][3])
{
  unsigned short i, j, k;
  double sum;
  for (i = 0; i < 3; i++) {
    for (j = 0; j < 3; j++) {
      sum = 0.0;
      for (k = 0; k < 3; k++) {
        sum += A[i][k] * B[k][j];
      } // for k
      AB[i][j] = sum;
    } // for j
  } // for i
} // mm3

/*
 * 3x3 Matrix - 3x1 Vector multiplication
 */
void mv3(double Ab[3], double A[3][3], double b[3])
{
  unsigned short i, j;
  double sum;
  for (i = 0; i < 3; i++) {
    sum = 0.0;
    for (j = 0; j < 3; j++) {
      sum += A[i][j]*b[j];
    } // for j
    Ab[i] = sum;
  } // for i
} // mv 3

/*
 * 3x3 Matrix adjugate
 */
void madj3(double adj[3][3], double m[3][3])
{
  adj[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
  adj[0][1] = m[0][2]*m[2][1] - m[0][1]*m[2][2];
  adj[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];

  adj[1][0] = m[1][2]*m[2][0] - m[1][0]*m[2][2];
  adj[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
  adj[1][2] = m[0][2]*m[1][0] - m[0][0]*m[1][2];

  adj[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
  adj[2][1] = m[0][1]*m[2][0] - m[0][0]*m[2][1];
  adj[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
} // madj3

/*
 * Determinant of a 3x3 matrix
 */
void mdet3(double *det, double m[3][3])
{
  *det = m[0][0]*(m[1][1]*m[2][2] - m[1][2]*m[2][1]) +
         m[0][1]*(m[1][2]*m[2][0] - m[1][0]*m[2][2]) +
         m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]);
} // mdet3

void form_constraint_matrix(double B[3][6], const double q[], const Bicycle_t *bike)
{
  double den = sqrt(pow(sin(q[1])*sin(q[4]) - cos(q[1])*sin(q[2])*cos(q[4]), 2)
               + pow(cos(q[1])*cos(q[3]), 2)),
         g31 = (sin(q[1])*sin(q[4]) - cos(q[1])*sin(q[2])*cos(q[5])) / den,
         g33 = cos(q[1])*cos(q[2]) / den;

  B[0][0] = cos(q[4])*sin(q[4])*(g33*bike->rf + cos(q[1])*cos(q[2])*bike->rft);
  B[0][1] = bike->ls +
            pow(sin(q[4]), 2)*(g33*bike->rf + cos(q[1])*cos(q[2])*bike->rft);
  B[0][2] = bike->rrt*sin(q[2]);
  B[0][3] = - cos(q[2])*(bike->rr + cos(q[1])*bike->rrt);
  B[0][4] = - (sin(q[1])*bike->rft + sin(q[4])*(g31*bike->rf + bike->lf));
  B[0][5] = cos(q[4])*(g33*bike->rf + cos(q[1])*cos(q[2])*bike->rft);

  B[1][0] = - bike->ls + cos(q[2])*(bike->rr + cos(q[1])*bike->rrt)
            - pow(cos(q[4]), 2)*(g33*bike->rf + cos(q[1])*cos(q[2])*bike->rft);
  B[1][1] = -cos(q[4])*sin(q[4])*(g33*bike->rf + cos(q[1])*cos(q[2])*bike->rft);
  B[1][2] = bike->lr + sin(q[2])*(bike->rr + cos(q[1])*bike->rrt);
  B[1][3] = 0.0;
  B[1][4] = cos(q[4])*(bike->lf + g31*bike->rf) + cos(q[1])*sin(q[2])*bike->rft;
  B[1][5] = 0.0;

  B[2][0] = 0.0;
  B[2][1] = 0.0;
  B[2][2] = 0.0;
  B[2][3] = 0.0;
  B[2][4] = 0.0;
  B[2][5] = 0.0;
} // form_constraint_matrix


#include <iostream>
#include <math.h>
#include <Windows.h>
#include <string>
#include <vector>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/legacy/compat.hpp>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <gsl/gsl_multifit.h>
#include <stdbool.h>
#include <CLEyeMulticam.h>

#include "SerialClass.h"

int i = 0;

using namespace cv;
using namespace std;
using namespace Eigen;

void setupScreen();
void setupCamera(int i);
bool processCamera(int i, Mat imgOriginal);
Vector3d rotateX(Vector3d v, double angle);
Vector3d rotateY(Vector3d v, double angle);
Vector3d rotateZ(Vector3d v, double angle);
bool polynomialfit(int obs, int degree, double *dx, double *dy, double *store);
void fpsPrint();
void triangulate();
void trajectoryCalc();
void arduinoSend();

int demo = 1;
int dataCollectEnabled = 0;
vector<double> xCoords;
vector<double> yCoords;
vector<double> zCoords;
double XZcoeff[3] = { 0 };
double XYcoeff[2] = { 0 };

int estXY[2] = { 0 }; // sum of estimated values

#include "CLEye.h" // This needs to be here b/c it uses the functions, namespaces, variables above

char *port = "COM4";

class Camera {
public:
	double rot[3];
	double rot2[3];
	Vector3d pos;
	double planeDist;
	double planeX;
	double planeY;
	double screenX;
	double screenY;
	Vector3d ballDir;
};
Camera cameras[2];

CLEyeCameraCapture *cam[2];

int minHue, maxHue, minSat, maxSat, minValue, maxValue;

int catcherHeight = 15; // Height of robot end-effector, in cm (at what height the ball will be caught)
double minX = 8.7; // Minimum distance of center of catcher to origin, in cm
double minY = 8.7;
double maxX = 52.7;
double maxY = 48.2;

int lastTick = 0;

char buffer[256] = "";

Serial* arduino;

#include "Functions.h"


static char help[] =
"Time-dependent advection equation, in flux-conservative form, in 2D.\n"
"Option prefix -adv_.  Domain is (-1,1) x (-1,1).  Equation is\n"
"  u_t + div(a(x,y) u) = g(x,y,u).\n"
"Boundary conditions are periodic in x and y.  Cells are grid-point centered.\n"
"Allows flux-limited (non-oscillatory) method-of-lines discretization:\n"
"  none       O(h^1)  first-order upwinding (limiter = 0)\n"
"  centered   O(h^2)  linear centered\n"
"  vanleer    O(h^2)  van Leer (1974) limiter\n"
"  koren      O(h^3)  Koren (1993) limiter [default].\n"
"(There is separate control over the limiter in the residual and in the\n"
"Jacobian.  Only none and centered are implemented for the Jacobian.)\n"
"Solves either of two problems with initial conditions:\n"
"  straight   Figure 6.2, page 303, in Hundsdorfer & Verwer (2003) [default]\n"
"  rotation   Figure 20.5, page 461, in LeVeque (2002).\n"
"For straight, if final time is an integer and velocities are kept at default\n"
"values, then exact solution is known and L1,L2 errors are reported.\n\n";

#include <petsc.h>

//STARTCTX
typedef enum {STUMP, SMOOTH, CONE, BOX} InitialType;
static const char *InitialTypes[] = {"stump", "smooth", "cone", "box",
                                     "InitialType", "", NULL};

typedef enum {NONE, CENTERED, VANLEER, KOREN} LimiterType;
static const char *LimiterTypes[] = {"none","centered","vanleer","koren",
                                     "LimiterType", "", NULL};

typedef enum {STRAIGHT, ROTATION} ProblemType;
static const char *ProblemTypes[] = {"straight","rotation",
                                     "ProblemType", "", NULL};

typedef struct {
    ProblemType    problem;
    double         windx, windy,            // x,y velocity in STRAIGHT
                   (*initial_fcn)(double,double), // for STRAIGHT
                   (*limiter_fcn)(double),  // limiter used in RHS
                   (*jac_limiter_fcn)(double); // used in Jacobian
} AdvectCtx;
//ENDCTX

//STARTINITIAL
// equal to 1 in a disc of radius 0.2 around (-0.6,-0.6)
static double stump(double x, double y) {
    const double r = PetscSqrtReal((x+0.6)*(x+0.6) + (y+0.6)*(y+0.6));
    return (r < 0.2) ? 1.0 : 0.0;
}

// smooth (C^6) version of stump
static double smooth(double x, double y) {
    const double r = PetscSqrtReal((x+0.6)*(x+0.6) + (y+0.6)*(y+0.6));
    if (r < 0.2)
        return PetscPowReal(1.0 - PetscPowReal(r / 0.2,6.0),6.0);
    else
        return 0.0;
}

// cone of height 1 of base radius 0.35 centered at (-0.45,0.0)
static double cone(double x, double y) {
    const double r = PetscSqrtReal((x+0.45)*(x+0.45) + y*y);
    return (r < 0.35) ? 1.0 - r / 0.35 : 0.0;
}

// equal to 1 in square of side-length 0.5 (0.1,0.6) x (-0.25,0.25)
static double box(double x, double y) {
    if ((0.1 < x) && (x < 0.6) && (-0.25 < y) && (y < 0.25))
        return 1.0;
    else
        return 0.0;
}

static void* initialptr[] = {&stump, &smooth, &cone, &box};
//ENDINITIAL

//STARTLIMITERS
/* the centered-space method is linear */
static double centered(double theta) {
    return 0.5;
}

/* van Leer (1974) limiter is formula (1.11) in section III.1 of
Hundsdorfer & Verwer (2003) */
static double vanleer(double theta) {
    const double abstheta = PetscAbsReal(theta);
    return 0.5 * (theta + abstheta) / (1.0 + abstheta);
}

/* Koren (1993) limiter is formula (1.7) in section III.1 of
Hundsdorfer & Verwer (2003) */
static double koren(double theta) {
    const double z = (1.0/3.0) + (1.0/6.0) * theta;
    return PetscMax(0.0, PetscMin(1.0, PetscMin(z, theta)));
}

static void* limiterptr[] = {NULL, &centered, &vanleer, &koren};
//ENDLIMITERS

// velocity  a(x,y) = ( a^x(x,y), a^y(x,y) )
static double a_wind(double x, double y, int dir, AdvectCtx* user) {
    switch (user->problem) {
        case STRAIGHT:
            return (dir == 0) ? user->windx : user->windy;
        case ROTATION:
            return (dir == 0) ? y : - x;
        default:
            return 0.0;
    }
}

// source  g(x,y,u)
static double g_source(double x, double y, double u, AdvectCtx* user) {
    return 0.0;
}

//         d g(x,y,u) / d u
static double dg_source(double x, double y, double u, AdvectCtx* user) {
    return 0.0;
}

extern PetscErrorCode FormInitial(DMDALocalInfo*, Vec, AdvectCtx*);
extern PetscErrorCode DumpBinary(const char*, const char*, Vec);
extern PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo*, double,
        double**, double**, AdvectCtx*);
extern PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo*, double,
        double**, Mat, Mat, AdvectCtx*);

int main(int argc,char **argv) {
    PetscErrorCode ierr;
    TS               ts;
    DM               da;
    Vec              u;
    DMDALocalInfo    info;
    double           hx, hy, t0, c, dt, tf;
    char             fileroot[PETSC_MAX_PATH_LEN] = "";
    int              steps;
    PetscBool        oneline = PETSC_FALSE, snesfdset, snesfdcolorset;
    InitialType      initial = STUMP;
    LimiterType      limiter = KOREN, jac_limiter = NONE;
    AdvectCtx        user;

    PetscInitialize(&argc,&argv,(char*)0,help);

    user.problem = STRAIGHT;
    user.windx = 2.0;
    user.windy = 2.0;
    ierr = PetscOptionsBegin(PETSC_COMM_WORLD,
           "adv_", "options for advect.c", ""); CHKERRQ(ierr);
    ierr = PetscOptionsString("-dumpto","filename root for binary files with initial/final state",
           "advect.c",fileroot,fileroot,PETSC_MAX_PATH_LEN,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnum("-initial",
           "shape of initial condition if problem==straight",
           "advect.c",InitialTypes,
           (PetscEnum)initial,(PetscEnum*)&initial,NULL); CHKERRQ(ierr);
    user.initial_fcn = initialptr[initial];
    ierr = PetscOptionsEnum("-limiter",
           "flux-limiter type used in RHS evaluation",
           "advect.c",LimiterTypes,
           (PetscEnum)limiter,(PetscEnum*)&limiter,NULL); CHKERRQ(ierr);
    user.limiter_fcn = limiterptr[limiter];
    ierr = PetscOptionsEnum("-jac_limiter",
           "flux-limiter type used in Jacobian (of RHS) evaluation",
           "advect.c",LimiterTypes,
           (PetscEnum)jac_limiter,(PetscEnum*)&jac_limiter,NULL); CHKERRQ(ierr);
    user.jac_limiter_fcn = limiterptr[jac_limiter];
    ierr = PetscOptionsEnum("-problem",
           "problem type",
           "advect.c",ProblemTypes,
           (PetscEnum)user.problem,(PetscEnum*)&user.problem,NULL); CHKERRQ(ierr);
    ierr = PetscOptionsBool("-oneline",
           "in exact solution cases, show one-line output",
           "advect.c",oneline,&oneline,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-windx",
           "x component of wind for problem==straight",
           "advect.c",user.windx,&user.windx,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsReal("-windy",
           "y component of wind for problem==straight",
           "advect.c",user.windy,&user.windy,NULL);CHKERRQ(ierr);
    ierr = PetscOptionsEnd(); CHKERRQ(ierr);

    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd",&snesfdset); CHKERRQ(ierr);
    ierr = PetscOptionsHasName(NULL,NULL,"-snes_fd_color",&snesfdcolorset); CHKERRQ(ierr);
    if (snesfdset || snesfdcolorset) {
        user.jac_limiter_fcn = NULL;
        jac_limiter = 5;   // corresponds to empty string
    }

    ierr = DMDACreate2d(PETSC_COMM_WORLD,
               DM_BOUNDARY_PERIODIC, DM_BOUNDARY_PERIODIC,
               DMDA_STENCIL_STAR,              // no diagonal differencing
               5,5,PETSC_DECIDE,PETSC_DECIDE,  // default to hx=hx=0.2 grid
                                               //   (mx=my=5 allows -snes_fd_color)
               1, 2,                           // d.o.f & stencil width
               NULL,NULL,&da); CHKERRQ(ierr);
    ierr = DMSetFromOptions(da); CHKERRQ(ierr);
    ierr = DMSetUp(da); CHKERRQ(ierr);
    ierr = DMSetApplicationContext(da,&user); CHKERRQ(ierr);
    ierr = DMDAGetLocalInfo(da,&info); CHKERRQ(ierr);
    hx = 2.0 / info.mx;  hy = 2.0 / info.my;
    ierr = DMDASetUniformCoordinates(da,    // grid is cell-centered
        -1.0+hx/2.0,1.0-hx/2.0,-1.0+hy/2.0,1.0-hy/2.0,0.0,1.0);CHKERRQ(ierr);

    ierr = TSCreate(PETSC_COMM_WORLD,&ts); CHKERRQ(ierr);
    ierr = TSSetProblemType(ts,TS_NONLINEAR); CHKERRQ(ierr);
    ierr = TSSetDM(ts,da); CHKERRQ(ierr);
    ierr = DMDATSSetRHSFunctionLocal(da,INSERT_VALUES,
           (DMDATSRHSFunctionLocal)FormRHSFunctionLocal,&user); CHKERRQ(ierr);
    ierr = DMDATSSetRHSJacobianLocal(da,
           (DMDATSRHSJacobianLocal)FormRHSJacobianLocal,&user); CHKERRQ(ierr);
    ierr = TSSetType(ts,TSRK); CHKERRQ(ierr);  // defaults to -ts_rk_type 3bs

    // time axis: use CFL number of 0.5 to set initial time step, but note
    //            most methods adapt anyway
    if (user.problem == STRAIGHT)
        c = PetscMax(PetscAbsReal(user.windx)/hx, PetscAbsReal(user.windy)/hy);
    else
        c = PetscMax(1.0/hx, 1.0/hy);
    dt = 0.5 / c;
    ierr = TSSetTime(ts,0.0); CHKERRQ(ierr);
    ierr = TSSetMaxTime(ts,0.6); CHKERRQ(ierr);
    ierr = TSSetTimeStep(ts,dt); CHKERRQ(ierr);
    ierr = TSSetExactFinalTime(ts,TS_EXACTFINALTIME_MATCHSTEP); CHKERRQ(ierr);
    ierr = TSSetFromOptions(ts);CHKERRQ(ierr);

    ierr = DMCreateGlobalVector(da,&u); CHKERRQ(ierr);
    ierr = FormInitial(&info,u,&user); CHKERRQ(ierr);
    ierr = DumpBinary(fileroot,"_initial",u); CHKERRQ(ierr);
    ierr = TSGetTime(ts,&t0); CHKERRQ(ierr);
    ierr = TSGetTimeStep(ts,&dt); CHKERRQ(ierr);

    if (!oneline) {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
               "solving problem %s with %s initial state on %d x %d grid,\n"
               "    cells dx=%g x dy=%g, limiter = %s, and jac_limiter = %s ...\n",
               ProblemTypes[user.problem],InitialTypes[initial],info.mx,info.my,
               hx,hy,LimiterTypes[limiter],LimiterTypes[jac_limiter]); CHKERRQ(ierr);
    }

    ierr = TSSolve(ts,u); CHKERRQ(ierr);

    ierr = TSGetStepNumber(ts,&steps); CHKERRQ(ierr);
    ierr = TSGetTime(ts,&tf); CHKERRQ(ierr);
    ierr = DumpBinary(fileroot,"_final",u); CHKERRQ(ierr);

    if (!oneline) {
        ierr = PetscPrintf(PETSC_COMM_WORLD,
                "completed %d steps to time %g\n",steps,tf); CHKERRQ(ierr);
    }

    if ( (user.problem == STRAIGHT) && (PetscAbs(fmod(tf+0.5e-8,1.0)) <= 1.0e-8)
         && (fmod(user.windx,2.0) == 0.0) && (fmod(user.windy,2.0) == 0.0) ) {
        // exact solution is same as initial condition
        Vec    uexact;
        double norms[2];
        ierr = VecDuplicate(u,&uexact); CHKERRQ(ierr);
        ierr = FormInitial(&info,uexact,&user); CHKERRQ(ierr);
        ierr = VecAXPY(u,-1.0,uexact); CHKERRQ(ierr); // u <- u + (-1.0) uexact
        ierr = VecNorm(u,NORM_1_AND_2,norms); CHKERRQ(ierr);
        norms[0] *= hx * hy;
        norms[1] *= PetscSqrtReal(hx * hy);
        VecDestroy(&uexact);
        if (oneline) {
            ierr = PetscPrintf(PETSC_COMM_WORLD,
                "%s,%s,%s,%d,%d,%g,%g,%d,%g,%.4e,%.4e\n",
                ProblemTypes[user.problem],InitialTypes[initial],
                LimiterTypes[limiter],info.mx,info.my,hx,hy,steps,tf,
                norms[0],norms[1]); CHKERRQ(ierr);
        } else {
            ierr = PetscPrintf(PETSC_COMM_WORLD,
                "errors |u-uexact|_{1,h} = %.4e, |u-uexact|_{2,h} = %.4e\n",
                norms[0],norms[1]); CHKERRQ(ierr);
        }
    }

    VecDestroy(&u);  TSDestroy(&ts);  DMDestroy(&da);
    return PetscFinalize();
}

PetscErrorCode FormInitial(DMDALocalInfo *info, Vec u, AdvectCtx* user) {
    PetscErrorCode ierr;
    int          i, j;
    double       hx, hy, x, y, **au;

    ierr = VecSet(u,0.0); CHKERRQ(ierr);  // clear it first
    ierr = DMDAVecGetArray(info->da, u, &au); CHKERRQ(ierr);
    hx = 2.0 / info->mx;  hy = 2.0 / info->my;
    for (j=info->ys; j<info->ys+info->ym; j++) {
        y = -1.0 + (j+0.5) * hy;
        for (i=info->xs; i<info->xs+info->xm; i++) {
            x = -1.0 + (i+0.5) * hx;
            switch (user->problem) {
                case STRAIGHT:
                    au[j][i] = (*user->initial_fcn)(x,y);
                    break;
                case ROTATION:
                    au[j][i] = cone(x,y) + box(x,y);
                    break;
                default:
                    SETERRQ(PETSC_COMM_WORLD,1,"invalid user->problem\n");
            }
        }
    }
    ierr = DMDAVecRestoreArray(info->da, u, &au); CHKERRQ(ierr);
    return 0;
}

// dumps to file; does nothing if string root is empty or NULL
PetscErrorCode DumpBinary(const char* root, const char* append, Vec u) {
    PetscErrorCode ierr;
    if ((root) && (strlen(root) > 0)) {
        PetscViewer  viewer;
        char         filename[PETSC_MAX_PATH_LEN] = "";
        sprintf(filename,"%s%s.dat",root,append);
        ierr = PetscPrintf(PETSC_COMM_WORLD,
            "writing PETSC binary file %s ...\n",filename); CHKERRQ(ierr);
        ierr = PetscViewerBinaryOpen(PETSC_COMM_WORLD,filename,
            FILE_MODE_WRITE,&viewer); CHKERRQ(ierr);
        ierr = VecView(u,viewer); CHKERRQ(ierr);
        ierr = PetscViewerDestroy(&viewer); CHKERRQ(ierr);
    }
    return 0;
}

/* method-of-lines discretization gives ODE system  u' = G(t,u)
so our finite volume scheme computes
    G_ij = - (fluxE - fluxW)/hx - (fluxN - fluxS)/hy + g(x,y,U_ij)
but only east (E) and north (N) fluxes are computed
*/
//STARTFUNCTION
PetscErrorCode FormRHSFunctionLocal(DMDALocalInfo *info, double t,
        double **au, double **aG, AdvectCtx *user) {
    int         i, j, q, dj, di;
    double      hx, hy, halfx, halfy, x, y, a,
                u_up, u_dn, u_far, theta, flux;

    // clear G first
    for (j = info->ys; j < info->ys + info->ym; j++)
        for (i = info->xs; i < info->xs + info->xm; i++)
            aG[j][i] = 0.0;
    // fluxes on cell boundaries are traversed in E,N order with indices
    // q=0 for E and q=1 for N; cell center has coordinates (x,y)
    hx = 2.0 / info->mx;  hy = 2.0 / info->my;
    halfx = hx / 2.0;     halfy = hy / 2.0;
    for (j = info->ys-1; j < info->ys + info->ym; j++) { // note -1 start
        y = -1.0 + (j+0.5) * hy;
        for (i = info->xs-1; i < info->xs + info->xm; i++) { // -1 start
            x = -1.0 + (i+0.5) * hx;
            if ((i >= info->xs) && (j >= info->ys)) {
                aG[j][i] += g_source(x,y,au[j][i],user);
            }
            for (q = 0; q < 2; q++) {   // E (q=0) and N (q=1) bdry fluxes
                if (q == 0 && j < info->ys)  continue;
                if (q == 1 && i < info->xs)  continue;
                di = 1 - q;
                dj = q;
                a = a_wind(x + halfx*di,y + halfy*dj,q,user);
                // first-order flux
                u_up = (a >= 0.0) ? au[j][i] : au[j+dj][i+di];
                flux = a * u_up;
                // use flux-limiter
                if (user->limiter_fcn != NULL) {
                    // formulas (1.2),(1.3),(1.6); H&V pp 216--217
                    u_dn = (a >= 0.0) ? au[j+dj][i+di] : au[j][i];
                    if (u_dn != u_up) {
                        u_far = (a >= 0.0) ? au[j-dj][i-di]
                                           : au[j+2*dj][i+2*di];
                        theta = (u_up - u_far) / (u_dn - u_up);
                        flux += a * (*user->limiter_fcn)(theta)
                                  * (u_dn-u_up);
                    }
                }
                // update owned G_ij on both sides of computed flux
                if (q == 0) {
                    if (i >= info->xs)
                        aG[j][i]   -= flux / hx;
                    if (i+1 < info->xs + info->xm)
                        aG[j][i+1] += flux / hx;
                } else {
                    if (j >= info->ys)
                        aG[j][i]   -= flux / hy;
                    if (j+1 < info->ys + info->ym)
                        aG[j+1][i] += flux / hy;
                }
            }
        }
    }
    return 0;
}
//ENDFUNCTION

PetscErrorCode FormRHSJacobianLocal(DMDALocalInfo *info, double t,
        double **au, Mat J, Mat P, AdvectCtx *user) {
    PetscErrorCode ierr;
    const int   dir[4] = { 0, 1, 0, 1},  // use x (0) or y (1) component
                xsh[4] = { 1, 0,-1, 0},  ysh[4]   = { 0, 1, 0,-1};
    int         i, j, l, nc;
    double      hx, hy, halfx, halfy, x, y, a, v[9];
    MatStencil  col[9],row;

    ierr = MatZeroEntries(P); CHKERRQ(ierr);
    hx = 2.0 / info->mx;  hy = 2.0 / info->my;
    halfx = hx / 2.0;     halfy = hy / 2.0;
    for (j = info->ys; j < info->ys+info->ym; j++) {
        y = -1.0 + (j+0.5) * hy;
        row.j = j;
        for (i = info->xs; i < info->xs+info->xm; i++) {
            x = -1.0 + (i+0.5) * hx;
            row.i = i;
            col[0].j = j;  col[0].i = i;
            v[0] = dg_source(x,y,au[j][i],user);
            nc = 1;
            for (l = 0; l < 4; l++) {   // loop over cell boundaries: E, N, W, S
                a = a_wind(x + halfx*xsh[l],y + halfy*ysh[l],dir[l],user);
                if (user->jac_limiter_fcn == NULL) {
                    // Jacobian is from upwind fluxes
                    switch (l) {
                        case 0:
                            col[nc].j = j;
                            col[nc].i = (a >= 0.0) ? i : i+1;
                            v[nc++] = - a / hx;
                            break;
                        case 1:
                            col[nc].j = (a >= 0.0) ? j : j+1;
                            col[nc].i = i;
                            v[nc++] = - a / hy;
                            break;
                        case 2:
                            col[nc].j = j;
                            col[nc].i = (a >= 0.0) ? i-1 : i;
                            v[nc++] = a / hx;
                            break;
                        case 3:
                            col[nc].j = (a >= 0.0) ? j-1 : j;
                            col[nc].i = i;
                            v[nc++] = a / hy;
                            break;
                    }
                } else if (user->jac_limiter_fcn == &centered) {
                    // Jacobian is from centered fluxes
                    switch (l) {
                        case 0:
                            col[nc].j = j;  col[nc].i = i;    v[nc++] = - a / (2.0*hx);
                            col[nc].j = j;  col[nc].i = i+1;  v[nc++] = - a / (2.0*hx);
                            break;
                        case 1:
                            col[nc].j = j;    col[nc].i = i;  v[nc++] = - a / (2.0*hy);
                            col[nc].j = j+1;  col[nc].i = i;  v[nc++] = - a / (2.0*hy);
                            break;
                        case 2:
                            col[nc].j = j;  col[nc].i = i-1;  v[nc++] = a / (2.0*hx);
                            col[nc].j = j;  col[nc].i = i;    v[nc++] = a / (2.0*hx);
                            break;
                        case 3:
                            col[nc].j = j-1;  col[nc].i = i;  v[nc++] = a / (2.0*hy);
                            col[nc].j = j;    col[nc].i = i;  v[nc++] = a / (2.0*hy);
                            break;
                    }
                } else {
                    SETERRQ(PETSC_COMM_WORLD,1,"only Jacobian cases none|centered are implemented\n");
                }
            }
            ierr = MatSetValuesStencil(P,1,&row,nc,col,v,ADD_VALUES); CHKERRQ(ierr);
        }
    }
    ierr = MatAssemblyBegin(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    ierr = MatAssemblyEnd(P,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    if (J != P) {
        ierr = MatAssemblyBegin(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
        ierr = MatAssemblyEnd(J,MAT_FINAL_ASSEMBLY); CHKERRQ(ierr);
    }
    return 0;
}


#include <vector>
#include <string>
#include <math.h>
#include <cmath>
#include <memory>
#include <iostream>

#include "hdf5.h"
#include "hdf5_hl.h"
#include "H5Gpublic.h"
#include "H5Fpublic.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_integration.h>

#ifndef NUFATE_H
#define NUFATE_H

namespace nufate{

/// \struct Square_matrix_double
/// \brief Very simple matrix struct used for pybinding.
/// vec_ is the front address of a chank of double data,
/// which will be shaped in NxN 2dim matrix in python.
struct Square_matrix_double {
 public :
 unsigned int n_;
 std::shared_ptr<double> vec_;
};

///\class Result
///\brief Simple class to hold the results.
class Result {
  public :
   std::vector<double> eval;          // eigen values
   std::shared_ptr<double> evec;      // pointer to top address of eigen vectors
   std::vector<double> ci;            // coefficients
   std::vector<double> energy_nodes_; // energy nodes in GeV
   std::vector<double> phi_0_;        // initial flux * E^(pedestal_gamma)

   //
   // functions below are not used by main nuFATE program,
   // but may be used for pybinding.
   //

   /// \brief getter for eigenvalues
   /// @return 1D vector
   std::vector<double> get_eigenvalues() { return eval; }

   /// \brief getter for coefficients
   /// @return 1D vector
   std::vector<double> get_coefficients()   { return ci; }

   /// \brief getter for energy nodes
   /// @return 1D vector
   std::vector<double> get_energy_nodes() { return energy_nodes_; }

   /// \brief getter for initial flux * E^(pedestal_gamma)
   /// @return 1D vector
   std::vector<double> get_phi_0() { return phi_0_; }

   /// \brief converter from pointer of double to Square_matrix_double object
   /// @return Square_matrix_double
   Square_matrix_double get_eigenvec_matrix() {
      unsigned int n = energy_nodes_.size();
      Square_matrix_double smatrix;
      // evec is n x n data
      smatrix.n_ = n;
      smatrix.vec_ = evec;
      return smatrix;
   }

   /// \brief debug print function
   void Print(unsigned int target_index = 0) {
      unsigned int dim = energy_nodes_.size();

      std::cout << "***** Result print *****"  << std::endl;
      std::cout << "eigenvectors at row " << target_index << " ---"  << std::endl;
      for (unsigned int i=target_index; i<target_index+1; ++i) {
         for (unsigned int j=0; j<dim; ++j) {
             std::cout << *(evec.get() + i*dim + j) << " " ;
         }
         std::cout << std::endl;
      }

      std::cout << std::endl;
      std::cout << "eigenvalue at i = " << target_index << " ---"  << std::endl;
      std::cout << eval[target_index] << std::endl;

      std::cout << std::endl;
      std::cout << "coefficient at i = " << target_index << " ---"  << std::endl;
      std::cout << ci[target_index] << std::endl;

      std::cout << std::endl;
      std::cout << "energy_node at i = " << target_index << " ---"  << std::endl;
      std::cout << energy_nodes_[target_index] << std::endl;
   }
};

///\class nuFATE
///\brief nuFATE main class
class nuFATE {
  private:
    const double GF = 1.16e-5;
    const double hbarc = 1.97e-14;
    const double GW = 2.085;
    const double MW = 80.385e0;
    const double mmu = 0.106e0;
    const double me = 511.e-6;
    const double pi = M_PI;
    const double MZ = 91.18;
    const double s2t = 0.23;
    const double gL =  s2t-0.5;
    const double REarth = 6371.;
    const double gR = s2t;
  private:
    int newflavor_;
    double newgamma_;
    std::string newh5_filename_;
  public:
    /// \brief Constructor
    /// @param flavor position ID of the system (1:NuE, -1:NuEBar, 2:NuMu, -2:NuMuBar, 3:NuTau, -3:NuTauBar)
    /// @param gamma spectral index of input flux.
    /// @param h5_filename name of hdf5 file containing the cross sections.
    /// @param include_secondaries if true secondaries are added to the flux propagation.
    nuFATE(int flv, double gamma, std::string h5_filename, bool include_secondaries);
    /// \brief Constructor
    /// @param flavor position ID of the system (1:NuE, -1:NuEBar, 2:NuMu, -2:NuMuBar, 3:NuTau, -3:NuTauBar)
    /// @param gamma spectral index of input flux.
    /// @param energy_nodes energy nodes in GeV.
    /// @param sigma_array total cross section(CC+NC) at each energy node in cm^2.
    /// @param dsigma_dE square array of differential cross section (NC only) in cm^2/GeV.
    /// @param include_secondaries if true secondaries are added to the flux propagation.
    nuFATE(int flv, double gamma, std::vector<double> energy_nodes, std::vector<double> sigma_array, std::vector<std::vector<double>> dsigma_dE, bool include_secondaries);
    /// \brief Constructor
    /// @param flavor position ID of the system (1:NuE, -1:NuEBar, 2:NuMu, -2:NuMuBar, 3:NuTau, -3:NuTauBar)
    /// @param gamma spectral index of input flux.
    /// @param cc_sigma_file file path for charge current(CC) total cross section in input format of nuSQuIDS
    /// @param nc_sigma_file file path for neutral current(NC) total cross section in input format of nuSQuIDS
    /// @param nc_dsigma_file file path to NC dSigma/dE differential cross section in input format of nuSQuIDS
    /// @param include_secondaries if true secondaries are added to the flux propagation.
    nuFATE(int flv, double gamma, std::string cc_sigma_file, std::string nc_sigma_file, std::string nc_dsigma_file, bool include_secondaries);

    /// \brief Eigensystem calculator
    Result getEigensystem();
    /// \brief Function to get Earth column density
    /// @param theta Zenith angle in radians.
    double getEarthColumnDensity(double theta);
    /// \brief Function to get flavor
    int getFlavor() const;
    /// \brief Function to get input spectral index
    double getGamma() const;
    /// \brief Function to get input filename
    std::string getFilename() const;
    /// \brief Function to get number of energy nodes
    double getNumNodes() const;
    /// \brief Function to toggle secondaries
    void setAddSecondaries(bool opt) { add_secondary_term_ = opt;}

    /// \brief Function to get energy nodes
    std::vector<double> getEnergyNodes() { return energy_nodes_; }
    /// \brief Function to get total crosssections (CC + NC)
    std::vector<double> getTotalCrossSections() { return sigma_array_; }
    /// \brief Function to get dsigma_dE differential crossection (for neutral-current only)
    Square_matrix_double getNCDifferentialCrossSections() {
       Square_matrix_double smatrix;
       smatrix.n_ = NumNodes_;
       smatrix.vec_ = dxs_array_;
       return smatrix;
    }

    /// \brief Calculate Relative Attenuation
    /// @param total number of targets, X[g/cm^2]*Na(Avogadro number) for example
    /// @return attenuation factor (arrival_flux / initial_flux), 1D array in energy bins
    std::vector<double> getRelativeAttenuation(double number_of_targets);

  protected:
    void Init(double gamma, const std::vector<double> &enodes, const std::vector<double> &sigmas, const std::vector<std::vector<double> > &dsigma);
    void AddAdditionalTerms();
    void LoadCrossSectionFromHDF5();
    void SetCrossSectionsFromInput(const std::vector<double> &sigma, const std::vector<std::vector<double>> &dsigma_dE);
    void SetInitialFlux();
    void set_glashow_total();
    void set_glashow_partial();
    void set_RHS_matrices(std::shared_ptr<double> RMatrix_, std::shared_ptr<double> dxs_array);
    static double rho_earth(double theta, void * p); //returns density for a given angle in radians along x distance
    double readDoubleAttribute(hid_t, std::string) const;
    unsigned int readUIntAttribute(hid_t, std::string) const;
    std::vector<double> logspace(double min,double max,unsigned int samples) const;
  private:
    void AllocateMemoryForMembers(unsigned int num_nodes);
    void SetEnergyBinWidths();
    std::string grpdiff_;
    std::string grptot_;
    hid_t file_id_;
    hid_t root_id_;
    unsigned int NumNodes_;
    unsigned int rsize_;
    double Emax_;
    double Emin_;
    int dxsdim_[2];
    std::vector<double> energy_nodes_;
    std::vector<double> sigma_array_;
    std::vector<double> sigma_array_orig_;
    std::vector<double> DeltaE_;
    std::vector<double> phi_0_;
    std::vector<double> glashow_total_;
    std::vector<double> sig3_array_;
    std::shared_ptr<double> glashow_partial_;
    std::shared_ptr<double> dxs_array_;
    std::shared_ptr<double> tau_array_;
    std::shared_ptr<double> sec_array_;
    std::shared_ptr<double> regen_array_;
    std::shared_ptr<double> RHSMatrix_;
    std::shared_ptr<double> RHSMatrix1_;
    std::shared_ptr<double> RHSMatrix2_;
    std::shared_ptr<double> RHSMatrix3_;
    std::shared_ptr<double> RHSMatrix4_;
    std::shared_ptr<double> RHregen_;
    std::shared_ptr<double> Enuin_;
    std::shared_ptr<double> Enu_;
    std::shared_ptr<double> selectron_;
    std::shared_ptr<double> den_;
    std::shared_ptr<double> t1_;
    std::shared_ptr<double> t2_;
    std::shared_ptr<double> t3_;
  private:
    bool include_secondaries_ = false;
    bool memory_allocated_ = false;
    bool initial_flux_set_ = false;
    bool total_cross_section_set_ = false;
    bool differential_cross_section_set_ = false;
    bool RHS_set_ = false;
    bool add_tau_regeneration_ = true;
    bool add_glashow_term_= true;
    bool add_secondary_term_ = true;
};

}

#endif