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

#include <boost/log/trivial.hpp>
#include <boost/algorithm/string.hpp>

#include "navigation.hpp"
#include "common.hpp"
#include "antenna.hpp"
#include "enums.h"

#include "eigenIncluder.hpp"

/** Compare two time tags (t2-t1)
* Returns julian day difference
*/
double timecomp(
	const double t1[6],		///< time tag in [YMDHMS]
	const double t2[6])		///< time tag in [YMDHMS]
{
	/* convert two time tags to julian day */
	double jd1 = ymdhms2jd(t1);
	double jd2 = ymdhms2jd(t2);

// 	BOOST_LOG_TRIVIAL(debug) << "time difference in julian day is "<< jd2-jd1;

	return jd2-jd1;
}

/* decode antenna field */
int decodef(char *p, int n, double *v)
{
	int i;
	for (i = 0; i < n; i++)
		v[i] = 0;

	for (i = 0, p = strtok(p," "); p && i < n; p = strtok(NULL, " "))
	{
		v[i] = atof(p) * 1E-3;
		i++;
	}
	return i;
}

pcvacs_t* findAntenna(
	string code,
	double tc[6],
	nav_t& nav)
{
// 	BOOST_LOG_TRIVIAL(debug)
// 	<< "Searching for " << type << ", " << code;

	auto it1 = nav.pcvMap.find(code);
	if (it1 == nav.pcvMap.end())
	{
		return nullptr;
	}

	auto& pcvTimeMap = it1->second;

	if (pcvTimeMap.size() == 0)
	{
		return nullptr;
	}

	GTime time = epoch2time(tc);

	auto it2 = pcvTimeMap.lower_bound(time);
	if (it2 == pcvTimeMap.end())
	{
		//just use the first chronologically, (last when sorted as they are) instead
		auto it3 = pcvTimeMap.rbegin();
		pcvacs_t& pcv = it3->second;

		return &pcv;
	}

	pcvacs_t& pcv = it2->second;

	return &pcv;
}

/* linear interpolate pcv ------------------------------------------------------
*
* args     :       double x1              I       x1 lower bound (degree)
*                  double x2              I       x2 upper bound (degree)
*                  double y1              I       y1 lower bound (m)
*                  double y2              I       y2 upper bound (m)
*                  double x               O       x current point (degree)
*
* return   :       interpolated pcv (m)
*----------------------------------------------------------------------------*/
double interp(double x1, double x2, double y1, double y2, double x)
{
#if (0)
	return (y2-y1)*(x-x1)/(x2-x1)+y1;
#endif
	return y2-(y2-y1)*(x2-x)/(x2-x1);
}

/* fetch rec pco ---------------------------------------------------------------
*
* args     :       const pcvacs_t *pc     I       antenna info
*                  const chat sys         I       satellite system
*                  const int freq         I       frequency 1 or 2
*                  double pco[3]          O       rec pco (m)
*
* return   :       none
*
* note     :       frequencies are sorted as GPS, GLONASS, Galileo and BeiDou
*----------------------------------------------------------------------------*/
void recpco(pcvacs_t *pc, int freq, Vector3d& pco)
{
	pcvacs_t& pcv = *pc;
	/* assign rec/sat pco */
	if (pcv.pcoMap.find((E_FType)freq) == pcv.pcoMap.end())		pco = Vector3d::Zero();
	else														pco = pcv.pcoMap[(E_FType) freq];
}

/* fetch rec pcv ---------------------------------------------------------------
*
* args     :       const pcvacs_t pc      I       antenna info
*                  const chat sys         I       satellite system
*                  const int freq         I       frequency 1, 2
*                  const double el        I       satellite elevation (degree)
*                  const double azi       I       azimuth (degree)
*                  double *pcv            O       rec pcv (m)
*
* return   :       none
*
* note     :       frequencies are sorted as GPS, GLONASS, Galileo and BeiDou
*----------------------------------------------------------------------------*/
void recpcv(
	pcvacs_t*	pc,
	int			freq,
	double		el,
	double		azi,
	double&		pcv)
{
	int		nz		= pc->nz;
	int		naz		= pc->naz;
	double	zen1	= pc->zenStart;
	double	dzen	= pc->zenDelta;
	double	dazi	= pc->aziDelta;
	double	zen		= 90 - el;

	if (pc->PCVMap1D.find(freq) == pc->PCVMap1D.end())
	{
		//frequency not found
		return;
	}
	auto& pcvMap1D = pc->PCVMap1D[freq];

	/* select zenith angle range */
	int zen_n;
	for (zen_n = 1; zen_n < nz; zen_n++)
	{
		if ((zen1 + dzen * zen_n) >= zen)
		{
			break;
		}
	}

	double xz1 = zen1 + dzen * (zen_n - 1);
	double xz2 = zen1 + dzen * (zen_n);

	if (naz == 0)
	{
		/* linear interpolate receiver pcv - non azimuth-dependent */
		/* interpolate */

		double	yz1 = pcvMap1D[zen_n - 1];		// lower bound
		double	yz2 = pcvMap1D[zen_n];			// upper bound
		pcv = interp(xz1, xz2, yz1, yz2, zen);
	}
	else
	{
		if (pc->PCVMap2D.find(freq) == pc->PCVMap2D.end())
		{
			//frequency not found
			return;
		}
		auto& pcvMap2D = pc->PCVMap2D[freq];

		/* bilinear interpolate receiver pcv - azimuth-dependent */
		/* select azimuth angle range */
		int az_n;
		for (az_n = 1; az_n < naz; az_n++)
		{
			if ((dazi * az_n) >= azi)
			{
				break;
			}
		}

		double xa1 = dazi * (az_n -1);
		double xa2 = dazi * (az_n);

		double yz3 = pcvMap2D[az_n-1]	[zen_n-1];		double yz1 = pcvMap2D[az_n-1]	[zen_n];
		double yz4 = pcvMap2D[az_n]		[zen_n-1];		double yz2 = pcvMap2D[az_n]		[zen_n];

		/* linear interpolation along zenith angle */
		double ya1	= interp(xz1, xz2, yz3, yz1, zen);
		double ya2 	= interp(xz1, xz2, yz4, yz2, zen);

		/* linear interpolation along azimuth angle */
		pcv	= interp(xa1, xa2, ya1, ya2, azi);
	}

	return;
}
//=============================================================================
// radomeNoneAntennaType = radome2none(antennaType)
//
//       e,g, "AOAD/M_T        JPLA" => "AOAD/M_T        NONE"
//
// Change the last four characters of antenna type to NONE
// This function is useful for when searching for an antenna model in ANTEX
//
// The IGS convention is to default to NONE for the radome if the calibration
// value is not available
//=============================================================================
//void radome2none(char *restrict antenna_type)
void radome2none(string& antenna_type)
{
	size_t length = antenna_type.size();
	if (length != 20)
	{
		printf("\n*** ERROR radome2none(): string length is less then 20 characters received %ld characters\n",length);
		return;
	}
	antenna_type.replace(length - 4, 4, "NONE");
}

map<string, E_FType> antexCodes =
{
	{"G01", L1    },
	{"G02", L2    },
	{"G05", L5    },
	{"R01", G1    },
	{"R02", G2    },
	{"E01", E1    },
	{"E05", E5A   },
	{"E07", E5B   },
	{"E08", E5AB  },
	{"E06", E6    },
	{"C01", E1    },
	{"C02", E2    },
	{"C07", E5B   },
	{"C06", E6    },
	{"J01", L1    },
	{"J02", L2    },
	{"J05", L5    },
	{"J06", LEX   },
	{"S01", L1    },
	{"S05", L5    }
};

/** Read antex file */
int readantexf(
	string	file,
	nav_t&	nav)
{
	int offset;
	int noazi_flag		= 0;
	int num_azi_rd		= 0;
	int new_antenna		= 0;
	int num_antennas	= 0;
	int irms			= 0;

	char tmp[10];
	char *p;

	FILE* fp = fopen(file.c_str(),"r");
	if (fp == nullptr)
	{
		BOOST_LOG_TRIVIAL(warning)
		<< "Warning: ANTEX file opening error";

		return 0;
	}

	const pcvacs_t pcv0 = {};
	pcvacs_t ds_pcv;

	E_FType	ft = FTYPE_NONE;

	char buff[512];
	while (fgets(buff, sizeof(buff), fp))
	{
		char* comment = buff + 60;

		if (irms)
			continue;
		/* Read in the ANTEX header information */
		if (strlen(buff) < 60 )								{	continue;	}
		if (strstr(comment, "ANTEX VERSION / SYST"))		{	continue;	}
		if (strstr(comment, "PCV TYPE / REFANT")) 			{	continue;	}
		if (strstr(comment, "COMMENT")) 					{	continue;	}
		if (strstr(comment, "END OF HEADER"))				{	continue;	}
		/* Read in specific Antenna information now */

		if (strstr(comment, "START OF ANTENNA"))
		{
			num_antennas++;
			ds_pcv		= pcv0;
			new_antenna	= 1;        /* flag for new antenna */

			continue;
		}

		if (!new_antenna)
		{
			continue;
		}

		if (strstr(comment, "METH / BY / # / DATE"))
		{
// 			int num_calibrated;
// 			char cal_method[20];
// 			char cal_agency[20];
// 			char cal_date[10];
// 			strncpy(cal_method,	buff,		20);/* Should be CHAMBER or FIELD or ROBOT or COPIED ot CONVERTED */
// 			cal_method[19] = '\0';
// 			strncpy(cal_agency, buff + 20,	20);
// 			cal_agency[19] = '\0';
// 			strncpy(tmp,		buff + 40,	10);
// 			num_calibrated = atoi(tmp);
// 			strncpy(cal_date,	buff + 50,	10);
// 			cal_date[9] = '\0';

			continue;
		}

		if (strstr(comment, "DAZI"))
		{
			strncpy(tmp,buff   ,8);tmp[8] = '\0';
			ds_pcv.aziDelta = atof(tmp);

			if (ds_pcv.aziDelta < 0.0001)	ds_pcv.naz = 0;
			else                     		ds_pcv.naz = (360 / ds_pcv.aziDelta) + 1;

			continue;
		}

		if (strstr(comment, "END OF ANTENNA"))
		{
			/* reset the flags for the next antenna */
			new_antenna	= 0;

			/* stack antenna pco and pcv */
			string id;
			string& satId = ds_pcv.code;
			if (satId.find_first_not_of(' ') == satId.npos)		{ id = ds_pcv.type;	}
			else												{ id = ds_pcv.code;	}

			boost::trim_right(id);

			GTime time = epoch2time(ds_pcv.tf);
			nav.pcvMap[id][time] = ds_pcv;

			continue;
		}

		if (strstr(comment, "TYPE / SERIAL NO"))
		{
			ds_pcv.type		.assign(buff,		20);
			ds_pcv.code		.assign(buff+20,	20);
			ds_pcv.svn		.assign(buff+40,	4);
			ds_pcv.cospar	.assign(buff+50,	10);

			continue;
		}

		if (strstr(comment, "ZEN1 / ZEN2 / DZEN"))
		{
			strncpy(tmp, buff,		8);	tmp[8] = '\0'; 	ds_pcv.zenStart	= atof(tmp);
			strncpy(tmp, buff+8,	7);	tmp[8] = '\0'; 	ds_pcv.zenStop	= atof(tmp);
			strncpy(tmp, buff+16,	7);	tmp[8] = '\0'; 	ds_pcv.zenDelta	= atof(tmp);

			ds_pcv.nz = (ds_pcv.zenStop - ds_pcv.zenStart) / ds_pcv.zenDelta + 1 ;

			continue;
		}

		if (strstr(comment, "# OF FREQUENCIES"))
		{
//			strncpy(tmp, buff,		8);
// 			tmp[8] = '\0';
// 			ds_pcv.nf		= atoi(tmp);

			continue;
		}

		if (strstr(comment, "VALID FROM"))
		{
			char valid_from[44];
			/* if (!str2time(buff,0,43,pcv.ts)) continue;*/
			strncpy(valid_from, buff, 43);
			valid_from[43] = '\0';
			p = strtok(valid_from, " ");
			int j = 0;
			while (p != NULL)
			{
				ds_pcv.tf[j] = (double) atoi(p);
				p = strtok(NULL, " ");
				j++;
			}

			continue;
		}

		if (strstr(comment, "VALID UNTIL"))
		{
			char valid_until[44];
			/* if (!str2time(buff,0,43,pcv.te)) continue;*/
			strncpy(valid_until, buff   ,43);
			valid_until[43] = '\0';
			p = strtok(valid_until, " ");
			int j = 0;
			while (p != nullptr)
			{
				ds_pcv.tu[j] = (double) atoi(p);
				p = strtok(NULL, " ");
				j++;
			}

			continue;
		}

		if (strstr(comment, "NORTH / EAST / UP"))
		{
			double neu[3];
			if (decodef(buff, 3, neu) < 3)
			{
				continue;
			}

			/* assign pco value in ENU */
			Vector3d& enu = ds_pcv.pcoMap[ft];
			enu[0] = neu[1];
			enu[1] = neu[0];
			enu[2] = neu[2];

			continue;
		}

		if (strstr(comment, "START OF FREQUENCY"))
		{
			num_azi_rd = 0;
			noazi_flag = 0;

			string antexFCode;
			antexFCode.assign(&buff[3], 3);

			ft = antexCodes[antexFCode];

			continue;
		}

		if (strstr(comment, "END OF FREQUENCY"))	{	noazi_flag	= 0;	continue;	}
		if (strstr(comment, "START OF FREQ RMS"))	{	irms		= 1;	continue;	}
		if (strstr(comment, "END OF FREQ RMS"))		{	irms		= 0;	continue;	}

		if (!irms && strstr(buff,"NOAZI"))
		{
			for (int i = 0; i < ds_pcv.nz; i++)
			{
				offset = i * 8 + 8;
				strncpy(tmp, buff + offset, 8);
				tmp[8]='\0';
				double pcv_val = atof(tmp);
				ds_pcv.PCVMap1D[ft]			.push_back(pcv_val * 1e-3);
			}
			noazi_flag = 1;

			continue;
		}

		if (!irms && noazi_flag == 1)
		{
			strncpy(tmp, buff, 8);
			tmp[8]='\0';

			for (int i = 0; i < ds_pcv.nz; i++)
			{
				offset = i * 8 + 8;
				strncpy(tmp, buff + offset, 8);
				tmp[8]='\0';
				double pcv_val = atof(tmp);
				ds_pcv.PCVMap2D[ft][num_azi_rd].push_back(pcv_val * 1e-3);
			}
			num_azi_rd++;

			continue;
		}
	}

	fclose(fp);

	return 1;
}

/** Satellite antenna model.
* Compute satellite antenna phase center parameters
*/
// inplace of antmodel_
// this will not work
// for galileo models
void interp_satantmodel(
	pcvacs_t&			pcv,	///< antenna phase center parameters
	double				nadir,	///< nadir angle for satellite (rad)
	map<int, double>&	dant)	///< range offsets for each frequency (m)
{
	for (auto& [ft, pcvVector] : pcv.PCVMap1D)
	{
		double	nadirDeg		= nadir * R2D;				// ang=0-90
		int		numSections		= pcvVector.size();
		double	startAngle		= pcv.zenStart;
		double	sectionWidth	= pcv.zenDelta;
		double	realSection		= ((nadirDeg - startAngle) / sectionWidth);
		double	intSection		= (int) realSection;
		double	fraction		= realSection - intSection;

		if		(intSection < 0)				{	dant[ft] = pcvVector[0];					}
		else if	(intSection >= numSections)		{	dant[ft] = pcvVector[numSections-1];		}
		else
		{
			double a = pcvVector[intSection];
			double b = pcvVector[intSection + 1];

			dant[ft] = a + fraction * (b - a);
		}
	}
}


/* Hector -- A Simple Climate Model
   Copyright (C) 2014-2015  Battelle Memorial Institute

   Please see the accompanying file LICENSE.md for additional licensing
   information.
*/
 /*
 * carbon-cycle-solver.cpp
 *
 * ODE solver for integrating the carbon cycle in hector.
 *
 * See notes in carbon-cycle-solver.cpp
 *
 */

#include <math.h>
#include <string>
#include <boost/numeric/odeint.hpp>

#include "carbon-cycle-solver.hpp"
#include "avisitor.hpp"

namespace Hector {

//------------------------------------------------------------------------------
/*! \brief Constructor
 */
CarbonCycleSolver::CarbonCycleSolver() : nc( 0 ),
eps_abs( 1.0e-6 ),eps_rel( 1.0e-6 ),
dt( 0.3 )
{
}

//------------------------------------------------------------------------------
/*! \brief Deconstructor
 */
CarbonCycleSolver::~CarbonCycleSolver()
{
}

//------------------------------------------------------------------------------
// documentation is inherited
void CarbonCycleSolver::init( Core* coreptr ) {
    // This component is very verbose at the debug and notice levels, so limit
    // output to the warning level, even if the rest of the model is configured
    // for something lower.
    logger.open(getComponentName(), false, coreptr->getGlobalLogger().getEchoToFile(), Logger::WARNING);
    H_LOG( logger, Logger::DEBUG ) << getComponentName() << " initialized." << std::endl;

    core = coreptr;

    in_spinup = false;

    // We want to run after the carbon box models, to give them a chance to initialize
    core->registerDependency( D_ATMOSPHERIC_C, getComponentName() );
}

//------------------------------------------------------------------------------
// documentation is inherited
unitval CarbonCycleSolver::sendMessage( const std::string& message,
                                       const std::string& datum,
                                       const message_data info ) throw ( h_exception )
{
    unitval returnval;

    if( message==M_GETDATA ) {          //! Caller is requesting data
        return getData( datum, info.date );

    } else if( message==M_SETDATA ) {   //! Caller is requesting to set data
        //TODO: call setData below
        //TODO: change core so that parsing is routed through sendMessage
        //TODO: make setData private

    } else {                        //! We don't handle any other messages
        H_THROW( "Caller sent unknown message: "+message );
    }

    return returnval;
}

//------------------------------------------------------------------------------
// documentation is inherited
void CarbonCycleSolver::setData( const std::string &varName,
                                 const message_data& data ) throw ( h_exception )
{
    H_LOG( logger, Logger::DEBUG ) << "Setting " << varName << "[" << data.date << "]=" << data.value_str << std::endl;

    try {
        if( varName == D_CCS_EPS_ABS ) {
            H_ASSERT( data.date == Core::undefinedIndex() , "date not allowed" );
            eps_abs = data.getUnitval(U_UNDEFINED);;
        }
        else if( varName == D_CCS_EPS_REL ) {
            H_ASSERT( data.date == Core::undefinedIndex() , "date not allowed" );
            eps_rel = data.getUnitval(U_UNDEFINED);;
        }
        else if( varName == D_CCS_DT ) {
            H_ASSERT( data.date == Core::undefinedIndex() , "date not allowed" );
            dt = data.getUnitval(U_UNDEFINED);
        }
        else if( varName == D_EPS_SPINUP ) {
            H_ASSERT( data.date == Core::undefinedIndex() , "date not allowed" );
            eps_spinup = data.getUnitval(U_PGC);
        }
        else {
            H_LOG( logger, Logger::SEVERE ) << "Unknown variable " << varName << std::endl;
            H_THROW( "Unknown variable name while parsing "+ getComponentName() + ": "
                    + varName );
        }
    } catch( h_exception& parseException ) {
        H_RETHROW( parseException, "Could not parse var: "+varName );
    }

    return;
}

//------------------------------------------------------------------------------
// documentation is inherited
void CarbonCycleSolver::prepareToRun() throw( h_exception )
{
    H_LOG( logger, Logger::DEBUG ) << "prepareToRun " << std::endl;

    cmodel = dynamic_cast<CarbonCycleModel*>( core->getComponentByCapability( D_ATMOSPHERIC_C ) );

    // initialize the solver's internal data
    t = core->getStartDate();
    nc = cmodel->ncpool();
    H_LOG( logger, Logger::DEBUG ) << "Carbon model in use is " << cmodel->getComponentName() << std::endl;
    H_LOG( logger, Logger::DEBUG ) << "Carbon model pools: " << nc << std::endl;
    H_ASSERT( nc > 0, "nc must be > 0" );
    // resize the array of carbon pool values
    c.resize(nc);

}

//------------------------------------------------------------------------------
// documentation is inherited
unitval CarbonCycleSolver::getData( const std::string& varName,
                                   const double date ) throw ( h_exception ) {

    unitval returnval;

    H_ASSERT( date == Core::undefinedIndex(), "Date not allowed for CarbonCycleSolver" );

    H_THROW( "Caller is requesting unknown variable: " + varName );

    return returnval;
}

void CarbonCycleSolver::reset(double time) throw(h_exception)
{
    // Only state maintained by this component is the time counter
    t = time;
    in_spinup = false;          // reset this in case we will be expected to rerun the spinup.
    H_LOG(logger, Logger::NOTICE)
        << getComponentName() << " reset to time= " << time << "\n";
}

//------------------------------------------------------------------------------
// documentation is inherited
void CarbonCycleSolver::shutDown()
{
	H_LOG( logger, Logger::DEBUG ) << "goodbye " << getComponentName() << std::endl;
    logger.close();
}

//------------------------------------------------------------------------------
/*! \brief              Dispatch function called by ODE solver
 *  \param[in] y        pools
 *  \param[in] dydt     pool changes
 *  \param[in] t        time
 *  \exception          If the carbon model returned failure flag we must throw
 *                      an exception to stop the ODE solver.
 */
void CarbonCycleSolver::ODEEvalFunctor::operator()( const std::vector<double>& y,
                                                    std::vector<double>& dydt,
                                                    double t ) throw ( bad_derivative_exception )
{
    // Note the std garuntees vetors are contigous so we can convert to array by
    // taking the address of the first value.
    int status = modelptr->calcderivs( t, &y[0], &dydt[0] );

    if( status != ODE_SUCCESS ) {
        bad_derivative_exception e(status);
        throw e;
    }
}

//------------------------------------------------------------------------------
/*! \brief              Observer callback called by ODE solver when a successful
 *                      step has been taken.
 *  \details            We use this callback to update our state variable time (t)
 *                      and in principle carbon pools (c) however the later is
 *                      already updated by the solver (pass by reference) so we will
 *                      skip copying here.
 *  \param[in] y        pools
 *  \param[in] t        time
 */
void CarbonCycleSolver::ODEEvalFunctor::operator()( const std::vector<double>& y, double t ) {
    // copy the current time to the original in CarbonCycleSolver
    (*this->t) = t;
}

//------------------------------------------------------------------------------
/*! \brief Support function for gsl_ode failure
 *  \param[in] stat     failure code
 *  \param[in] t0       start of time step
 *  \param[in] tmid     middle of time step
 *  \exception          will always happen; gsl solver has failed
 */
void CarbonCycleSolver::failure( int stat, double t0, double tmid ) throw( h_exception ) {
    H_LOG( logger, Logger::SEVERE ) << "gsl_ode_evolve_apply failed at t= " <<
    t0 << "  tinit= " << t << "  tmid = " << tmid << "  last dt= " <<
    dt << "\nError code: " << stat << "\ncvals:\n";
    for( int i=0; i<nc; ++i )
        H_LOG( logger,Logger::SEVERE ) << c[ i ] << "  ";
    H_LOG( logger,Logger::SEVERE ) << std::endl;
    H_THROW( "gsl_ode_evolve_apply failed." );
}

//------------------------------------------------------------------------------
// documentation is inherited
void CarbonCycleSolver::run( const double tnew ) throw ( h_exception )
{
    if(tnew <= t) {
        H_LOG(logger, Logger::SEVERE) << "run(): tnew= " << tnew << "   t= " << t << std::endl;
    }
    H_ASSERT( tnew > t, "solver tnew is not greater than t" );

    // Get the initial state data from the box model. c will be filled in
    // Note that we rely on the box model to handle the units.  Inside the
    // solver we strip the unit values and work with raw numbers.
    cmodel->getCValues( t, &c[0] );

    double t0   = t;  // stash this in case we need to report & diagnose an error
    // Now integrate from the beginning of the time step using the updated
    // slow params.  Note we can discard t0 and the values in cc
    cmodel->slowparameval( t, &c[0] );
    int retry = 0;

    H_LOG( logger, Logger::DEBUG ) << "Entering ODE solver " << t << "->" << tnew << std::endl;
    while( t < tnew && retry < MAX_CARBON_MODEL_RETRIES ) {

        H_LOG( logger, Logger::DEBUG ) << "Resetting evolver and stepper" << std::endl;
        double t_start = t;
        double t_target = tnew;

        while( t < t_target && retry < MAX_CARBON_MODEL_RETRIES ) {
            H_LOG( logger, Logger::NOTICE ) << "Attempting ODE solver " << t << "->" << t_target << " (" << t0 << "->" << tnew << ")" << std::endl;

            int stat = ODE_SUCCESS;
            ODEEvalFunctor odeFunctor( cmodel, &t );
            try {
                using namespace boost::numeric::odeint;
                typedef runge_kutta_dopri5<std::vector<double> > error_stepper_type;
                integrate_adaptive( make_controlled<error_stepper_type>( eps_abs, eps_rel ),
                         odeFunctor, c, t_start, t_target, dt, odeFunctor );
            } catch( bad_derivative_exception& e ) {
                stat = e.errorFlag;
            }

            if( stat == CARBON_CYCLE_RETRY ) {
                H_LOG( logger, Logger::NOTICE ) << "Carbon model requests retry #" << ++retry << " at t= " << t << std::endl;
                t_target = t_start + ( t_target - t_start ) / 2.0;
                t = t_start;

                dt = t_target - t;
                cmodel->getCValues( t, &c[0] );     // reset pools and inform model of new starting point
                H_LOG( logger, Logger::NOTICE ) << "New target is " << t_target << std::endl;
            } else if( stat != ODE_SUCCESS )
                failure( stat, t_start, t_target );
        }

        // We have exited GSL solver loop, but did we make it?
        if( retry < MAX_CARBON_MODEL_RETRIES ) {
            H_LOG( logger, Logger::NOTICE ) << "Success: we have reached " << t_target << std::endl;

            retry = 0;
            cmodel->stashCValues( t, &c[0] );   // update state
        } else {
            H_LOG( logger, Logger::SEVERE ) << "Failure after failure: t is " << t << "; we have not reached " << t_target << std::endl;

        }
    }
    H_ASSERT( t == tnew, "solver failure: t != tnew" );

    H_LOG( logger, Logger::NOTICE ) << "ODE solver success at t= " << t <<
    "  last dt= " << dt << std::endl;
    H_LOG( logger, Logger::DEBUG ) << "cvals\terrors\n";

    cmodel->record_state(tnew);

    H_LOG( logger, Logger::NOTICE ) << std::endl;
}

//------------------------------------------------------------------------------
/*! \brief      Run the carbon cycle to steady state, if possible
 *  \exception  epsilon must be >0
 *
 *  We normally want the carbon model to be in steady state, i.e. with unchanging
 *  carbon pools, at the end of the preindustrial period before the main run.
 *  To reach this point, run the carbon model until all dc/dt are ~0.
 */
bool CarbonCycleSolver::run_spinup( const int step ) throw( h_exception )
{

    if( !in_spinup ) {  // first time
        in_spinup = true;
        t = step-1;
        c_original.resize(nc);
        c_old.resize(nc);
        c_new.resize(nc);
        dcdt.resize(nc);
        // initialize to zero
        for(int i=0; i<nc; ++i)
          c_original[i] = c_old[i] = c_new[i] = dcdt[i] = 0.0;

        cmodel->getCValues( t, &c_original[0] );
        cmodel->record_state(t);
    }

    cmodel->getCValues( t, &c_old[0] );
    run( step );
    cmodel->getCValues( step, &c_new[0] );

    int max_dcdt_pool = 0;
    double max_dcdt = 0.0;

    for( int i=0; i<nc; i++ ) {     // find the biggest difference
        dcdt[ i ] = fabs( c_new[ i ] - c_old[ i ] );
        c_old[ i ] = c_new[ i ];
        if (dcdt[ i ] > max_dcdt) {
            max_dcdt = dcdt[i];
        }
    }

    bool spunup = ( max_dcdt < eps_spinup.value( U_PGC ) );

    if( spunup ) {
        Logger& glog = core->getGlobalLogger();
        H_LOG( glog, Logger::NOTICE ) << "Carbon model is spun up after " << step << " steps" << std::endl;
        H_LOG( logger, Logger::NOTICE ) << "Carbon model spun up after " << step << " steps. Max residual dc/dt="
        << max_dcdt << " (pool " << max_dcdt_pool << ")" << std::endl;
        for( int i=0; i<nc; i++ ) {
            H_LOG( logger, Logger::NOTICE ) << "New pool " << i << ":" << c_new[ i ]
            << " (delta=" << c_new[ i ]-c_original[ i ] << ")" << std::endl;
        }
        t = core->getStartDate();
        H_LOG( logger, Logger::NOTICE ) << "Resetting solver time counter to t= " << t << std::endl;
    }

    // Record the state as the state at the model start time.  This
    // will be repeatedly overwritten until the spinup is complete.
    cmodel->record_state(core->getStartDate());

    return spunup;
}

//------------------------------------------------------------------------------
/*! \brief visitor accept code
 */
void CarbonCycleSolver::accept( AVisitor* visitor )
{
    visitor->visit( this );
}

}


//=================================================================================================
// Copyright (c) 2011, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

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

#include <hector_pose_estimation/measurements/baro.h>
#include <hector_pose_estimation/filter/set_filter.h>

#include <boost/bind.hpp>

namespace hector_pose_estimation {

template class Measurement_<BaroModel>;

BaroModel::BaroModel()
{
  stddev_ = 1.0;
  qnh_ = 1013.25;
  parameters().add("qnh", qnh_);
}

BaroModel::~BaroModel() {}

void BaroModel::getExpectedValue(MeasurementVector& y_pred, const State& state)
{
  y_pred(0) = qnh_ * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 5.255);
}

void BaroModel::getStateJacobian(MeasurementMatrix& C, const State& state, bool)
{
  if (state.position()) {
    state.position()->cols(C)(0,Z) = qnh_ * 5.255 * pow(1.0 - (0.0065 * (state.getPosition().z() + getElevation())) / 288.15, 4.255) * (-0.0065 / 288.15);
  }
}

double BaroModel::getAltitude(const BaroUpdate& update)
{
  return 288.15 / 0.0065 * (1.0 - pow(update.getVector()(0) / qnh_, 1.0/5.255));
}

BaroUpdate::BaroUpdate() : qnh_(0) {}
BaroUpdate::BaroUpdate(double pressure) : qnh_(0) { *this = pressure; }
BaroUpdate::BaroUpdate(double pressure, double qnh) : qnh_(qnh) { *this = pressure; }

Baro::Baro(const std::string &name)
  : Measurement_<BaroModel>(name)
  , HeightBaroCommon(this)
{
  parameters().add("auto_elevation", auto_elevation_);
}

void Baro::onReset()
{
  HeightBaroCommon::onReset();
}

bool Baro::prepareUpdate(State &state, const Update &update) {
  if (update.qnh() != 0) setQnh(update.qnh());
  // Note: boost::bind is not real-time safe!
  setElevation(resetElevation(state, boost::bind(&BaroModel::getAltitude, getModel(), update)));
  return true;
}

} // namespace hector_pose_estimation


/*
//@HEADER
// ************************************************************************
//
// solvers_linear_traits.hpp
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#ifndef SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_
#define SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_

#ifdef PRESSIO_ENABLE_TPL_EIGEN
#include <Eigen/Core>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/Householder>
#include <Eigen/QR>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>
#include <Eigen/OrderingMethods>
#endif

namespace pressio{ namespace linearsolvers{

template <typename T>
struct Traits {
  static constexpr bool direct        = false;
  static constexpr bool iterative     = false;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif
#ifdef PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = false;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::CG>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
    >
  using eigen_solver_type = Eigen::ConjugateGradient<MatrixT, Eigen::Lower, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::Bicgstab>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
    >
  using eigen_solver_type = Eigen::BiCGSTAB<MatrixT, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::iterative::LSCG>
{
  static constexpr bool direct        = false;
  static constexpr bool iterative     = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <
    typename MatrixT,
    typename PrecT = Eigen::DiagonalPreconditioner<typename MatrixT::Scalar>
  >
  using eigen_solver_type = Eigen::LeastSquaresConjugateGradient<MatrixT, PrecT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::ColPivHouseholderQR>
{

  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  /* if matrix is dense, use Eigen::ColPivHouseholderQR.
   * if the native matrix is sparse, then use Eigen::SparseQR.
   * to use SparseQR, the matrix has to be:
   * (a) sparse
   * (b) column-major
   * (c) compressed mode to use COLAMDOrdering
  */
  template <typename MatrixT>
  using eigen_solver_type =
    typename std::conditional<
      pressio::is_sparse_matrix_eigen<MatrixT>::value &&
      MatrixT::IsRowMajor==0,
      Eigen::SparseQR<MatrixT, Eigen::COLAMDOrdering<typename MatrixT::StorageIndex>>,
      Eigen::ColPivHouseholderQR<MatrixT>
      >::type;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::HouseholderQR>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::HouseholderQR<MatrixT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::PartialPivLU>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::PartialPivLU<MatrixT>;

  static constexpr bool eigen_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::potrsL>
{
  static constexpr bool iterative = false;
  static constexpr bool direct = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::LLT<MatrixT, Eigen::Lower>;
  static constexpr bool eigen_enabled = true;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::potrsU>
{
  static constexpr bool direct = true;
  static constexpr bool eigen_enabled = true;

#ifdef PRESSIO_ENABLE_TPL_EIGEN
  template <typename MatrixT>
  using eigen_solver_type = Eigen::LLT<MatrixT, Eigen::Upper>;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::getrs>
{
  static constexpr bool direct = true;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif

#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

template <>
struct Traits<::pressio::linearsolvers::direct::geqrf>
{
  static constexpr bool direct = true;
#ifdef PRESSIO_ENABLE_TPL_EIGEN
  static constexpr bool eigen_enabled = false;
#endif
#if defined PRESSIO_ENABLE_TPL_TRILINOS or defined PRESSIO_ENABLE_TPL_KOKKOS
  static constexpr bool kokkos_enabled = true;
#endif
};

}}
#endif  // SOLVERS_LINEAR_IMPL_SOLVERS_LINEAR_TRAITS_HPP_


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_MOD_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_MOD_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-arithmetic
    Function object implementing mod capabilities

    Computes the remainder of division.
    The return value is x-n*y, where n is the value x/y,
    truncated to \f$-\infty\f$.

    @par semantic:
    For any given value @c x, @c y of type @c T:

    @code
    T r = mod(x, y);
    @endcode

    The code is similar to:

    @code
    T r = x-div(x, y, floor)*y;
    @endcode

    @see remainder, rem,  modulo

  **/
  const boost::dispatch::functor<tag::mod_> mod = {};
} }
#endif

#include <boost/simd/function/scalar/mod.hpp>
#include <boost/simd/function/simd/mod.hpp>

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_TWO_PROD_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

/*!
  @file
  @brief Definition of the two_prod function
**/

namespace boost { namespace simd
{
  namespace tag
  {
    /// @brief Hierarchy tag for two_prod function
    struct two_prod_ : ext::elementwise_<two_prod_>
    {
      typedef ext::elementwise_<two_prod_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_two_prod_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::two_prod_, Site> dispatching_two_prod_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::two_prod_, Site>();
    }
    template<class... Args>
    struct impl_two_prod_;
  }

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0* a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum

    @return A Fusion Sequence containing @c a0+a1 and the residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::two_prod_, two_prod, 2)

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0 * a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum
    @param a2 L-Value that will receive @c a0+a1

    @return The sum residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_prod_, two_prod
                                            , (A0 const&)(A1 const&)(A2&)
                                            , 3
                                            )

  /*!
    @brief

    For any two reals @c a0 and @c a1 two_prod computes two reals
    @c r0 and @c r1 so that:

    @code
    r0 = a0 * a1
    r1 = r0 -(a0 * a1)
    @endcode

    using perfect arithmetic. Its main usage is to be able to compute
    sum of reals and the residual error using IEEE 754 arithmetic.

    @param a0 First parameter of the sum
    @param a1 Second parameter of the sum
    @param a2 L-Value that will receive @c a0+a1.
    @param a3 L-Value that will receive @c a0+a1 residual.
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION_TPL( tag::two_prod_, two_prod
                                            , (A0 const&)(A1 const&)(A2&)(A3&)
                                            , 4
                                            )
} }

#endif


///////////////////////////////////////////////////////////////////////////////
// extended_p_square_quantile.hpp
//
//  Copyright 2005 Daniel Egloff. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_ACCUMULATORS_STATISTICS_EXTENDED_SINGLE_QUANTILE_HPP_DE_01_01_2006
#define BOOST_ACCUMULATORS_STATISTICS_EXTENDED_SINGLE_QUANTILE_HPP_DE_01_01_2006

#include <vector>
#include <functional>
#include <boost/throw_exception.hpp>
#include <boost/range/begin.hpp>
#include <boost/range/end.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/iterator/transform_iterator.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/iterator/permutation_iterator.hpp>
#include <boost/parameter/keyword.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/accumulators/framework/accumulator_base.hpp>
#include <boost/accumulators/framework/extractor.hpp>
#include <boost/accumulators/numeric/functional.hpp>
#include <boost/accumulators/framework/parameters/sample.hpp>
#include <boost/accumulators/framework/depends_on.hpp>
#include <boost/accumulators/statistics_fwd.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>
#include <boost/accumulators/statistics/extended_p_square.hpp>
#include <boost/accumulators/statistics/weighted_extended_p_square.hpp>
#include <boost/accumulators/statistics/times2_iterator.hpp>

#ifdef _MSC_VER
# pragma warning(push)
# pragma warning(disable: 4127) // conditional expression is constant
#endif

namespace boost { namespace accumulators
{

namespace impl
{
    ///////////////////////////////////////////////////////////////////////////////
    // extended_p_square_quantile_impl
    //  single quantile estimation
    /**
        @brief Quantile estimation using the extended \f$P^2\f$ algorithm for weighted and unweighted samples

        Uses the quantile estimates calculated by the extended \f$P^2\f$ algorithm to compute
        intermediate quantile estimates by means of quadratic interpolation.

        @param quantile_probability The probability of the quantile to be estimated.
    */
    template<typename Sample, typename Impl1, typename Impl2> // Impl1: weighted/unweighted // Impl2: linear/quadratic
    struct extended_p_square_quantile_impl
      : accumulator_base
    {
        typedef typename numeric::functional::fdiv<Sample, std::size_t>::result_type float_type;
        typedef std::vector<float_type> array_type;
        typedef iterator_range<
            detail::lvalue_index_iterator<
                permutation_iterator<
                    typename array_type::const_iterator
                  , detail::times2_iterator
                >
            >
        > range_type;
        // for boost::result_of
        typedef float_type result_type;

        template<typename Args>
        extended_p_square_quantile_impl(Args const &args)
          : probabilities(
                boost::begin(args[extended_p_square_probabilities])
              , boost::end(args[extended_p_square_probabilities])
            )
        {
        }

        template<typename Args>
        result_type result(Args const &args) const
        {
            typedef
                typename mpl::if_<
                    is_same<Impl1, weighted>
                  , tag::weighted_extended_p_square
                  , tag::extended_p_square
                >::type
            extended_p_square_tag;

            extractor<extended_p_square_tag> const some_extended_p_square = {};

            array_type heights(some_extended_p_square(args).size());
            std::copy(some_extended_p_square(args).begin(), some_extended_p_square(args).end(), heights.begin());

            this->probability = args[quantile_probability];

            typename array_type::const_iterator iter_probs = std::lower_bound(this->probabilities.begin(), this->probabilities.end(), this->probability);
            std::size_t dist = std::distance(this->probabilities.begin(), iter_probs);
            typename array_type::const_iterator iter_heights = heights.begin() + dist;

            // If this->probability is not in a valid range return NaN or throw exception
            if (this->probability < *this->probabilities.begin() || this->probability > *(this->probabilities.end() - 1))
            {
                if (std::numeric_limits<result_type>::has_quiet_NaN)
                {
                    return std::numeric_limits<result_type>::quiet_NaN();
                }
                else
                {
                    std::ostringstream msg;
                    msg << "probability = " << this->probability << " is not in valid range (";
                    msg << *this->probabilities.begin() << ", " << *(this->probabilities.end() - 1) << ")";
                    boost::throw_exception(std::runtime_error(msg.str()));
                    return Sample(0);
                }

            }

            if (*iter_probs == this->probability)
            {
                return heights[dist];
            }
            else
            {
                result_type res;

                if (is_same<Impl2, linear>::value)
                {
                    /////////////////////////////////////////////////////////////////////////////////
                    // LINEAR INTERPOLATION
                    //
                    float_type p1 = *iter_probs;
                    float_type p0 = *(iter_probs - 1);
                    float_type h1 = *iter_heights;
                    float_type h0 = *(iter_heights - 1);

                    float_type a = numeric::fdiv(h1 - h0, p1 - p0);
                    float_type b = h1 - p1 * a;

                    res = a * this->probability + b;
                }
                else
                {
                    /////////////////////////////////////////////////////////////////////////////////
                    // QUADRATIC INTERPOLATION
                    //
                    float_type p0, p1, p2;
                    float_type h0, h1, h2;

                    if ( (dist == 1 || *iter_probs - this->probability <= this->probability - *(iter_probs - 1) ) && dist != this->probabilities.size() - 1 )
                    {
                        p0 = *(iter_probs - 1);
                        p1 = *iter_probs;
                        p2 = *(iter_probs + 1);
                        h0 = *(iter_heights - 1);
                        h1 = *iter_heights;
                        h2 = *(iter_heights + 1);
                    }
                    else
                    {
                        p0 = *(iter_probs - 2);
                        p1 = *(iter_probs - 1);
                        p2 = *iter_probs;
                        h0 = *(iter_heights - 2);
                        h1 = *(iter_heights - 1);
                        h2 = *iter_heights;
                    }

                    float_type hp21 = numeric::fdiv(h2 - h1, p2 - p1);
                    float_type hp10 = numeric::fdiv(h1 - h0, p1 - p0);
                    float_type p21  = numeric::fdiv(p2 * p2 - p1 * p1, p2 - p1);
                    float_type p10  = numeric::fdiv(p1 * p1 - p0 * p0, p1 - p0);

                    float_type a = numeric::fdiv(hp21 - hp10, p21 - p10);
                    float_type b = hp21 - a * p21;
                    float_type c = h2 - a * p2 * p2 - b * p2;

                    res = a * this->probability * this-> probability + b * this->probability + c;
                }

                return res;
            }

        }

    public:
        // make this accumulator serializeable
        // TODO: do we need to split to load/save and verify that the parameters did not change?
        template<class Archive>
        void serialize(Archive & ar, const unsigned int file_version)
        {
            ar & probabilities;
            ar & probability;
        }

    private:

        array_type probabilities;
        mutable float_type probability;

    };

} // namespace impl

///////////////////////////////////////////////////////////////////////////////
// tag::extended_p_square_quantile
//
namespace tag
{
    struct extended_p_square_quantile
      : depends_on<extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, unweighted, linear> impl;
    };
    struct extended_p_square_quantile_quadratic
      : depends_on<extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, unweighted, quadratic> impl;
    };
    struct weighted_extended_p_square_quantile
      : depends_on<weighted_extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, weighted, linear> impl;
    };
    struct weighted_extended_p_square_quantile_quadratic
      : depends_on<weighted_extended_p_square>
    {
        typedef accumulators::impl::extended_p_square_quantile_impl<mpl::_1, weighted, quadratic> impl;
    };
}

///////////////////////////////////////////////////////////////////////////////
// extract::extended_p_square_quantile
// extract::weighted_extended_p_square_quantile
//
namespace extract
{
    extractor<tag::extended_p_square_quantile> const extended_p_square_quantile = {};
    extractor<tag::extended_p_square_quantile_quadratic> const extended_p_square_quantile_quadratic = {};
    extractor<tag::weighted_extended_p_square_quantile> const weighted_extended_p_square_quantile = {};
    extractor<tag::weighted_extended_p_square_quantile_quadratic> const weighted_extended_p_square_quantile_quadratic = {};

    BOOST_ACCUMULATORS_IGNORE_GLOBAL(extended_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(extended_p_square_quantile_quadratic)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_extended_p_square_quantile)
    BOOST_ACCUMULATORS_IGNORE_GLOBAL(weighted_extended_p_square_quantile_quadratic)
}

using extract::extended_p_square_quantile;
using extract::extended_p_square_quantile_quadratic;
using extract::weighted_extended_p_square_quantile;
using extract::weighted_extended_p_square_quantile_quadratic;

// extended_p_square_quantile(linear) -> extended_p_square_quantile
template<>
struct as_feature<tag::extended_p_square_quantile(linear)>
{
    typedef tag::extended_p_square_quantile type;
};

// extended_p_square_quantile(quadratic) -> extended_p_square_quantile_quadratic
template<>
struct as_feature<tag::extended_p_square_quantile(quadratic)>
{
    typedef tag::extended_p_square_quantile_quadratic type;
};

// weighted_extended_p_square_quantile(linear) -> weighted_extended_p_square_quantile
template<>
struct as_feature<tag::weighted_extended_p_square_quantile(linear)>
{
    typedef tag::weighted_extended_p_square_quantile type;
};

// weighted_extended_p_square_quantile(quadratic) -> weighted_extended_p_square_quantile_quadratic
template<>
struct as_feature<tag::weighted_extended_p_square_quantile(quadratic)>
{
    typedef tag::weighted_extended_p_square_quantile_quadratic type;
};

// for the purposes of feature-based dependency resolution,
// extended_p_square_quantile and weighted_extended_p_square_quantile
// provide the same feature as quantile
template<>
struct feature_of<tag::extended_p_square_quantile>
  : feature_of<tag::quantile>
{
};
template<>
struct feature_of<tag::extended_p_square_quantile_quadratic>
  : feature_of<tag::quantile>
{
};
// So that extended_p_square_quantile can be automatically substituted with
// weighted_extended_p_square_quantile when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::extended_p_square_quantile>
{
    typedef tag::weighted_extended_p_square_quantile type;
};

template<>
struct feature_of<tag::weighted_extended_p_square_quantile>
  : feature_of<tag::extended_p_square_quantile>
{
};

// So that extended_p_square_quantile_quadratic can be automatically substituted with
// weighted_extended_p_square_quantile_quadratic when the weight parameter is non-void
template<>
struct as_weighted_feature<tag::extended_p_square_quantile_quadratic>
{
    typedef tag::weighted_extended_p_square_quantile_quadratic type;
};
template<>
struct feature_of<tag::weighted_extended_p_square_quantile_quadratic>
  : feature_of<tag::extended_p_square_quantile_quadratic>
{
};

}} // namespace boost::accumulators

#ifdef _MSC_VER
# pragma warning(pop)
#endif

#endif


#include <deal.II/base/function.h>
#include <deal.II/base/function_parser.h>
#include <deal.II/base/logstream.h>
#include <deal.II/base/parameter_handler.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/timer.h>
#include <deal.II/base/table_handler.h>

#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/lac/dynamic_sparsity_pattern.h>
#include <deal.II/lac/full_matrix.h>
#include <deal.II/lac/petsc_parallel_sparse_matrix.h>
#include <deal.II/lac/petsc_parallel_vector.h>
#include <deal.II/lac/petsc_solver.h>
#include <deal.II/lac/petsc_precondition.h>
#include <deal.II/lac/sparsity_tools.h>
#include <deal.II/lac/vector.h>

#include <deal.II/grid/grid_generator.h>
#include <deal.II/grid/grid_out.h>
#include <deal.II/grid/grid_refinement.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>

#include <deal.II/distributed/grid_refinement.h>
#include <deal.II/distributed/tria.h>

#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_accessor.h>
#include <deal.II/dofs/dof_tools.h>

#include <deal.II/fe/fe_values.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/fe/fe_q.h>

#include <deal.II/numerics/data_out.h>
#include <deal.II/numerics/error_estimator.h>
#include <deal.II/numerics/matrix_tools.h>
#include <deal.II/numerics/vector_tools.h>

#include <mandy/elastic_tensor.h>
#include <mandy/lattice_tensor.h>

#include <mandy/crystal_symmetry_group.h>

#include <fstream>
#include <iostream>

#include <algorithm>    // std::transform
#include <functional>   // std::plus

namespace mandy
{
  /**
   * Solve the system Ax=b, where A is the vector-valued operator of
   * the Laplace-type and b is a linear function.
   */
  template <int dim>
  class ElasticProblem
  {
  public:

    /**
     * Class constructor.
     */
    ElasticProblem (const std::string &prm);

    /**
     * Class destructor.
     */
    ~ElasticProblem ();

    /**
     * Wrapper function, that controls the order of excecution.
     */
    void run ();

    /**
     * Get coefficients from the parameter file specified at run time.
     */
    void get_coefficients ();

  private:

    /**
     * Make intial coarse grid.
     */
    void make_coarse_grid ();

    /**
     * Setup system matrices and vectors.
     */
    void setup_system ();

    /**
     * Assemble system matrices and vectors.
     */
    void assemble_system();

    /**
     * Solve the linear algebra system.
     */
    unsigned int solve ();

    /**
     * Output results, ie., finite element functions and derived
     * quantitites for this cycle.
     */
    void output_results (const unsigned int cycle);

    /**
     * Refine grid based on Kelly's error estimator working on the
     * material id (solution vector).
     */
    void refine_grid ();

    /**
     * MPI communicator.
     */
    MPI_Comm mpi_communicator;

    /**
     * A distributed grid on which all computations are done.
     */
    dealii::parallel::distributed::Triangulation<dim> triangulation;

    /**
     * Scalar DoF handler primarily used for interpolating material
     * identification.
     */
    dealii::DoFHandler<dim> dof_handler;

    /**
     * Scalar valued finite element primarily used for interpolating
     * material iudentification.
     */
    dealii::FESystem<dim> finite_element;

    /**
     * Index set of locally owned DoFs.
     */
    dealii::IndexSet locally_owned_dofs;

    /**
     * Index set of locally relevant DoFs.
     */
    dealii::IndexSet locally_relevant_dofs;

    /**
     * A list of (hanging node) constraints.
     */
    dealii::ConstraintMatrix constraints;

    /**
     * System matrix - a mass matrix.
     */
    dealii::PETScWrappers::MPI::SparseMatrix system_matrix;

    /**
     * Locally relevant solution vector.
     */
    dealii::PETScWrappers::MPI::Vector locally_relevant_solution;

    /**
     * System right hand side function - interpolated function.
     */
    dealii::PETScWrappers::MPI::Vector system_rhs;

    /**
     * Parallel iostream.
     */
    dealii::ConditionalOStream pcout;

    /**
     * Stop clock.
     */
    dealii::TimerOutput timer;

    /**
     * Input parameter file.
     */
    dealii::ParameterHandler parameters;

    /**
     * Tensor of elastic coefficients.
     */
    mandy::Physics::ElasticTensor<mandy::CrystalSymmetryGroup::wurtzite> elastic_tensor;

    /**
     * Vector of elastic coefficients.
     */
    std::vector<double> elastic_coefficients;

    /**
     * Tensor of lattice coefficients.
     */
    mandy::Physics::LatticeTensor<mandy::CrystalSymmetryGroup::wurtzite> lattice_tensor;

    /**
     * Vector of lattice coefficients.
     */
    std::vector<double> lattice_coefficients;

  }; // LinearElasticity

  /**
   * Class constructor.
   */
  template <int dim>
  ElasticProblem<dim>::ElasticProblem (const std::string &prm)
    :
    mpi_communicator (MPI_COMM_WORLD),
    triangulation (mpi_communicator,
                   typename dealii::Triangulation<dim>::MeshSmoothing
                   (dealii::Triangulation<dim>::smoothing_on_refinement |
                    dealii::Triangulation<dim>::smoothing_on_coarsening)),
    dof_handler (triangulation),
    finite_element (dealii::FE_Q<dim> (2), dim),
    // ---
    pcout (std::cout, (dealii::Utilities::MPI::this_mpi_process (mpi_communicator) == 0)),
    timer (mpi_communicator, pcout,
	   dealii::TimerOutput::summary,
	   dealii::TimerOutput::wall_times)
  {
    parameters.enter_subsection ("Material");
    {
      parameters.declare_entry ("Material function", "0",
				dealii::Patterns::Anything (),
				"A functional description of the material.");

      parameters.declare_entry ("Lattice background",
				"0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 3, ","),
				"Size of the lattice of the background");

      parameters.declare_entry ("Lattice inclusion",
				"0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 3, ","),
				"Size of the lattice of an inclusion");

      parameters.declare_entry ("Elastic background",
				"0, 0, 0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 5, ","),
				"Elastic coefficients of the background");

      parameters.declare_entry ("Elastic inclusion",
				"0, 0, 0, 0, 0",
				dealii::Patterns::List (dealii::Patterns::Anything (), 1, 5, ","),
				"Elastic coefficients of an inclusion");
    }
    parameters.leave_subsection ();

    parameters.parse_input (prm);
  }

  /**
   * Class destructor.
   */
  template <int dim>
  ElasticProblem<dim>::~ElasticProblem ()
  {
    // Wipe DoF handlers.
    dof_handler.clear ();
  }

  /**
   * Setup system matrices and vectors.
   */
  template <int dim>
  void ElasticProblem<dim>::setup_system ()
  {
    dealii::TimerOutput::Scope time (timer, "setup system");

    // Determine locally relevant DoFs.
    dof_handler.distribute_dofs (finite_element);
    locally_owned_dofs = dof_handler.locally_owned_dofs ();
    dealii::DoFTools::extract_locally_relevant_dofs (dof_handler, locally_relevant_dofs);

    // Initialise distributed vectors.
    locally_relevant_solution.reinit (locally_owned_dofs, locally_relevant_dofs,
				      mpi_communicator);
    system_rhs.reinit (locally_owned_dofs,
		       mpi_communicator);

    // Setup hanging node constraints.
    constraints.clear ();
    constraints.reinit (locally_relevant_dofs);

    dealii::DoFTools::make_hanging_node_constraints (dof_handler, constraints);
    dealii::DoFTools::make_zero_boundary_constraints (dof_handler, constraints);
    constraints.close ();

    // Finally, create a distributed sparsity pattern and initialise
    // the system matrix from that.
    dealii::DynamicSparsityPattern dsp (locally_relevant_dofs);
    dealii::DoFTools::make_sparsity_pattern (dof_handler, dsp, constraints, false);
    dealii::SparsityTools::distribute_sparsity_pattern (dsp,
							dof_handler.n_locally_owned_dofs_per_processor (),
							mpi_communicator,
							locally_relevant_dofs);

    system_matrix.reinit (locally_owned_dofs, locally_owned_dofs,
                          dsp, mpi_communicator);

  }

  /**
   * Assemble the linear algebra system.
   */
  template <int dim>
  void
  ElasticProblem<dim>::assemble_system ()
  {
    dealii::TimerOutput::Scope time (timer, "assemble system");

    // Define quadrature rule to be used.
    const dealii::QGauss<dim> quadrature_formula (3);

    dealii::FEValues<dim> fe_values (finite_element, quadrature_formula,
				     dealii::update_values            |
				     dealii::update_gradients         |
				     dealii::update_quadrature_points |
				     dealii::update_JxW_values);

    const unsigned int dofs_per_cell = finite_element.dofs_per_cell;
    const unsigned int n_q_points    = quadrature_formula.size ();

    dealii::FullMatrix<double> cell_matrix (dofs_per_cell, dofs_per_cell);
    dealii::Vector<double> cell_vector (dofs_per_cell);
    std::vector<dealii::types::global_dof_index> local_dof_indices (dofs_per_cell);

    // A vector of material values at each quadrature point and the
    // function to be parsed from the input file.
    dealii::FunctionParser<dim> material_function;

    parameters.enter_subsection ("Material");
    {
      material_function.initialize (dealii::FunctionParser<dim>::default_variable_names (),
				    parameters.get ("Material function"),
				    typename dealii::FunctionParser<dim>::ConstMap ());
    }
    parameters.leave_subsection ();

    std::vector<double> material_function_values (n_q_points);

    // Get lattice parameters from file.
    std::vector<double> lattice_coefficients_background;
    std::vector<double> lattice_coefficients_inclusion;

    // Get elastic coefficients from input file.
    std::vector<double> elastic_coefficients_background;
    std::vector<double> elastic_coefficients_inclusion;

    parameters.enter_subsection ("Material");
    {
      lattice_coefficients_background = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Lattice background"), ','));

      lattice_coefficients_inclusion = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Lattice inclusion"), ','));

      elastic_coefficients_background = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Elastic background"), ','));

      elastic_coefficients_inclusion = dealii::Utilities::string_to_double
	(dealii::Utilities::split_string_list (parameters.get ("Elastic inclusion"), ','));
    }
    parameters.leave_subsection ();

    AssertThrow (elastic_coefficients_background.size ()==elastic_coefficients_inclusion.size (),
		 dealii::ExcDimensionMismatch (elastic_coefficients_background.size (),
					       elastic_coefficients_inclusion.size ()));

    typename dealii::DoFHandler<dim>::active_cell_iterator
      cell = dof_handler.begin_active (),
      endc = dof_handler.end ();

    for (; cell!=endc; ++cell)
      if (cell->subdomain_id () == dealii::Utilities::MPI::this_mpi_process (mpi_communicator))
	{
	  cell_matrix = 0;
	  cell_vector = 0;
	  fe_values.reinit (cell);

	  // Extract vector-values from FEValues.
	  const dealii::FEValuesExtractors::Vector u (0);

	  // Obtain the material identification on this cell and
	  // transfer it to a strain description.
	  material_function.value_list (fe_values.get_quadrature_points (),
					material_function_values);

	  for (unsigned int q_point=0; q_point<n_q_points; ++q_point)
	    {

	      elastic_coefficients.clear ();

	      for (unsigned int i=0; i<elastic_coefficients_inclusion.size (); ++i)
		elastic_coefficients.push_back (material_function_values[q_point]*elastic_coefficients_inclusion[i] +
						(1.-material_function_values[q_point])*elastic_coefficients_background[i]);

	      elastic_tensor.set_coefficients (elastic_coefficients);
	      elastic_tensor.distribute_coefficients ();

	      Assert (elastic_tensor.is_symmetric (), dealii::ExcMessage ("Tensor not symmetric"));

	      lattice_coefficients.clear ();

	      for (unsigned int i=0; i<lattice_coefficients_inclusion.size (); ++i)
		lattice_coefficients.push_back (material_function_values[q_point] *
						(lattice_coefficients_inclusion[i]/lattice_coefficients_background[i]) - 1.);
	      lattice_tensor.set_coefficients (lattice_coefficients);
	      lattice_tensor.distribute_coefficients ();

	      for (unsigned int i=0; i<dofs_per_cell; ++i)
		{
		  const dealii::Tensor<2, dim> u_i_grad = fe_values[u].symmetric_gradient (i, q_point);

		  for (unsigned int j=0; j<dofs_per_cell; ++j)
		    {
		      const dealii::Tensor<2, dim> u_j_grad = fe_values[u].symmetric_gradient (j, q_point);

		      // Local stiffness matrix.
		      cell_matrix (i,j) +=
			contract (u_i_grad, elastic_tensor, u_j_grad) *
			fe_values.JxW (q_point);

		    }

		  // Local right hand side vector.
		  cell_vector (i) +=
		    contract (u_i_grad, elastic_tensor, lattice_tensor) *
		    fe_values.JxW (q_point);

		}

	    } // q_point

	  cell->get_dof_indices (local_dof_indices);

	  constraints.distribute_local_to_global (cell_matrix, cell_vector,
						  local_dof_indices,
						  system_matrix, system_rhs);
	} // cell!=endc

    system_matrix.compress (dealii::VectorOperation::add);
    system_rhs.compress (dealii::VectorOperation::add);
  }

  /**
   * Solve the linear algebra system.
   */
  template <int dim>
  unsigned int
  ElasticProblem<dim>::solve ()
  {
    dealii::TimerOutput::Scope time (timer, "solve");

    dealii::PETScWrappers::MPI::Vector completely_distributed_solution (locally_owned_dofs, mpi_communicator);
    dealii::SolverControl solver_control (dof_handler.n_dofs (), 1e-06);
    dealii::PETScWrappers::SolverBicgstab solver (solver_control, mpi_communicator);
    dealii::PETScWrappers::PreconditionBlockJacobi preconditioner (system_matrix);

    solver.solve (system_matrix, completely_distributed_solution, system_rhs,
		  preconditioner);

    // Ensure that all ghost elements are also copied as necessary.
    constraints.distribute (completely_distributed_solution);
    locally_relevant_solution = completely_distributed_solution;

    // Return the number of iterations (last step) of the solve.
    return solver_control.last_step ();
  }

  /**
   * Output results.
   */
  template <int dim>
  void
  ElasticProblem<dim>::output_results (const unsigned int cycle)
  {
    dealii::TimerOutput::Scope time (timer, "output_results");

    dealii::DataOut<dim> data_out;
    data_out.attach_dof_handler (dof_handler);
    data_out.add_data_vector (locally_relevant_solution, "displacement");

    dealii::Vector<float> subdomain (triangulation.n_active_cells ());
    for (unsigned int i=0; i<subdomain.size(); ++i)
      subdomain (i) = triangulation.locally_owned_subdomain ();
    data_out.add_data_vector (subdomain, "subdomain");

    data_out.build_patches ();

    const std::string filename = ("displacement-" +
                                  dealii::Utilities::int_to_string (cycle, 2) +
                                  "." +
                                  dealii::Utilities::int_to_string
                                  (triangulation.locally_owned_subdomain (), 4));

    std::ofstream output ((filename + ".vtu").c_str ());
    data_out.write_vtu (output);

    if (dealii::Utilities::MPI::this_mpi_process(mpi_communicator) == 1)
      {
	std::vector<std::string> filenames;

	for (unsigned int i=0;
	     i<dealii::Utilities::MPI::n_mpi_processes (mpi_communicator);
	     ++i)
	  filenames.push_back ("displacement-" +
			       dealii::Utilities::int_to_string (cycle, 2) +
			       "." +
			       dealii::Utilities::int_to_string (i, 4) +
			       ".vtu");
	std::ofstream master_output (("displacement-" +
				      dealii::Utilities::int_to_string (cycle, 2) +
				      ".pvtu").c_str ());

	data_out.write_pvtu_record (master_output, filenames);
      }
  }

  /**
   * Run the application in the order specified.
   */
  template <int dim>
  void
  ElasticProblem<dim>::run ()
  {
    // To solve the Elastic problem, first we need to solve the
    // problem of a mterial identification. This is done by solving a
    // function against the mass matrix.

    // Create a coarse grid according to the parameters given in the
    // input file.
    dealii::GridGenerator::hyper_cube (triangulation, -2.5, 2.5);
    triangulation.refine_global (3);

    pcout << "   Number of active cells:       "
	  << triangulation.n_global_active_cells ()
	  << std::endl;

    setup_system ();

    pcout << "   Number of degrees of freedom: "
	  << dof_handler.n_dofs ()
	  << std::endl;

    assemble_system ();

    const unsigned int n_iterations = solve ();

    pcout << "   Solved in " << n_iterations
	  << " iterations."
	  << std::endl;

    pcout << "   Linfty-norm:                  "
	  << locally_relevant_solution.linfty_norm ()
	  << std::endl;

    output_results (0);
  }

} // namespace mandy

/**
 * Main function: Initialise problem and run it.
 */
int main (int argc, char *argv[])
{
  // Initialise MPI
  dealii::Utilities::MPI::MPI_InitFinalize mpi_initialization (argc, argv, 1);

  try
    {
      // mandy::MaterialID<3> material_id ("step-4.prm");
      // material_id.run ();

      mandy::ElasticProblem<3> elastic_problem ("elastic.prm");
      elastic_problem.run ();
    }

  catch (std::exception &exc)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Exception on processing: " << std::endl
                << exc.what() << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;

      return 1;
    }

  catch (...)
    {
      std::cerr << std::endl << std::endl
                << "----------------------------------------------------"
                << std::endl;
      std::cerr << "Unknown exception!" << std::endl
                << "Aborting!" << std::endl
                << "----------------------------------------------------"
                << std::endl;
      return 1;
    }

  return 0;
}


#include "libsnark/gadgetlib1/gadgets/basic_gadgets.hpp"
#include "libsnark/zk_proof_systems/ppzksnark/r1cs_ppzksnark/r1cs_ppzksnark.hpp"
#include "libsnark/common/default_types/r1cs_ppzksnark_pp.hpp"
#include "libsnark/common/utils.hpp"
#include <boost/optional.hpp>

using namespace libsnark;
using namespace std;

#include "gadget.hpp"
#include "gadget_neg.hpp"

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair(const int jw[32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1=R2+R3+X)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
r1cs_ppzksnark_keypair<ppzksnark_ppT> generate_keypair_neg(const int jw[2][32])
{
    typedef Fr<ppzksnark_ppT> FieldT;

    //根据预先定义的计算门和约束生成公共参数秘钥对
    //证明生成端若需要采用对应的公共秘钥生成证明数据成功，则必须使两端的数据符合预先定于的计算约束(R1+X=R2+R3)
    //如此，当验证端根据对应的验证秘钥验证证明数据为真时，验证者就能够相信对应的交易中是符合预定义的计算约束的，而不是生成假证明以通过检查
    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    const r1cs_constraint_system<FieldT> constraint_system = pb.get_constraint_system();

    cout << "Number of R1CS constraints: " << constraint_system.num_constraints() << endl;

    return r1cs_ppzksnark_generator<ppzksnark_ppT>(constraint_system);
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h1,
                                                                   const bit_vector &h2,
                                                                   const bit_vector &h3,
                                                                   const bit_vector &r1,
                                                                   const bit_vector &r2,
                                                                   const bit_vector &r3,
																   const bit_vector &x,
																   const int jw[32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(h1, h2, h3, r1, r2, r3,x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
boost::optional<r1cs_ppzksnark_proof<ppzksnark_ppT>> generate_proof_neg(r1cs_ppzksnark_proving_key<ppzksnark_ppT> proving_key,
                                                                   const bit_vector &h1,
                                                                   const bit_vector &h2,
                                                                   const bit_vector &h3,
                                                                   const bit_vector &r1,
                                                                   const bit_vector &r2,
                                                                   const bit_vector &r3,
																   const bit_vector &x,
																   const int jw[2][32]
                                                                   )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    protoboard<FieldT> pb;
    l_gadget_neg<FieldT> g(pb);
    g.generate_r1cs_constraints();
    g.generate_r1cs_witness(h1, h2, h3, r1, r2, r3,x);

    if (!pb.is_satisfied()) {
      std::cout << "System not satisfied!" << std::endl;
        return boost::none;
    }

    return r1cs_ppzksnark_prover<ppzksnark_ppT>(proving_key, pb.primary_input(), pb.auxiliary_input());
}

template<typename ppzksnark_ppT>
bool verify_proof(r1cs_ppzksnark_verification_key<ppzksnark_ppT> verification_key,
                  r1cs_ppzksnark_proof<ppzksnark_ppT> proof,
                  const bit_vector &h1,
                  const bit_vector &h2,
                  const bit_vector &h3,
				  const bit_vector &x
                 )
{
    typedef Fr<ppzksnark_ppT> FieldT;

    const r1cs_primary_input<FieldT> input = l_input_map<FieldT>(h1, h2, h3,x);

    std::cout << "**** After l_input_map *****" << std::endl;

    return r1cs_ppzksnark_verifier_strong_IC<ppzksnark_ppT>(verification_key, input, proof);

}


#include <cassert>
#include <cmath>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <random>
#include <stdexcept>
#include <string>
#include <tuple>
#include <utility>
#include <vector>

#include <boost/geometry.hpp>
#include <csv.h>

#include "DataStructures/Geometry/SummedAreaTables/OctagonalSummedAreaTable.h"
#include "DataStructures/Geometry/Area.h"
#include "DataStructures/Geometry/CoordinateTransformation.h"
#include "DataStructures/Geometry/Point.h"
#include "DataStructures/Graph/Attributes/LatLngAttribute.h"
#include "DataStructures/Graph/Attributes/SequentialVertexIdAttribute.h"
#include "DataStructures/Graph/Graph.h"
#include "DataStructures/Utilities/Matrix.h"
#include "DataStructures/Utilities/OriginDestination.h"
#include "Tools/CommandLine/CommandLineParser.h"
#include "Tools/CommandLine/ProgressBar.h"
#include "Tools/Constants.h"
#include "Tools/Math.h"
#include "Tools/Timer.h"

namespace bg = boost::geometry;
namespace bgi = boost::geometry::index;

void printUsage() {
  std::cout <<
      "Usage: RadiationModel [-s <factor>] -g <file> -r <file> -grid <file> -o <file>\n"
      "Generates travel demand data for the specified road network according to the\n"
      "radiation model, using the German Census 2011 population grid.\n"
      "  -s <factor>       scale the population by <factor> (defaults to 1.0)\n"
      "  -d <meters>       the max. distance between a cell's center and mapped segment\n"
      "  -seed <seed>      start the random number generator with <seed>\n"
      "  -g <file>         the network under study in binary format\n"
      "  -r <file>         restrict origins/destinations to area from OSM POLY file\n"
      "  -grid <file>      the German Census 2011 population grid\n"
      "  -o <file>         place output in <file>\n"
      "  -help             display this help and exit\n";
}

int main(int argc, char* argv[]) {
  try {
    CommandLineParser clp(argc, argv);
    if (clp.isSet("help")) {
      printUsage();
      return EXIT_SUCCESS;
    }

    const auto areaFilename = clp.getValue<std::string>("r");
    const auto gridFilename = clp.getValue<std::string>("grid");
    const auto graphFilename = clp.getValue<std::string>("g");
    const auto outputFilename = clp.getValue<std::string>("o");
    const double scale = clp.getValue<double>("s", 1.0);
    const int maxDist = clp.getValue<int>("d", 5000);

    // Read the graph from file.
    std::ifstream graphFile(graphFilename, std::ios::binary);
    if (!graphFile.good())
      throw std::invalid_argument("file not found -- '" + graphFilename + "'");
    StaticGraph<VertexAttrs<LatLngAttribute, SequentialVertexIdAttribute>> graph(graphFile);
    graphFile.close();

    if (graph.numVertices() > 0 && graph.sequentialVertexId(0) == INVALID_VERTEX)
      FORALL_VERTICES(graph, v)
        graph.sequentialVertexId(v) = v;

    // Read the population grid from file.
    std::vector<std::pair<::Point, int>> gridCells;
    ::Point minCell(INFTY, INFTY);
    ::Point maxCell(-INFTY, -INFTY);
    ::Point cell;
    int numInhabitants;
    io::CSVReader<3, io::trim_chars<>, io::no_quote_escape<';'>> gridFile(gridFilename);
    gridFile.read_header(io::ignore_extra_column, "x_mp_100m", "y_mp_100m", "Einwohner");
    while (gridFile.read_row(cell.x(), cell.y(), numInhabitants))
      if (numInhabitants > 0) {
        gridCells.emplace_back(cell, numInhabitants);
        minCell.min(cell);
        maxCell.max(cell);
      }

    // Allocate and fill the population matrix.
    const auto gridBounds = maxCell - minCell;
    const int numRows = gridBounds.y() / 100 + 1;
    const int numCols = gridBounds.x() / 100 + 1;
    Matrix<int> populationGrid(numRows, numCols, 0);
    for (const auto& cell : gridCells) {
      const auto pos = cell.first - minCell;
      populationGrid(numRows - pos.y() / 100 - 1, pos.x() / 100) = cell.second;
    }

    // Build an R-tree storing the road segments.
    using Point = bg::model::point<double, 2, bg::cs::spherical_equatorial<bg::degree>>;
    using Segment = bg::model::segment<Point>;
    using RoadSegment = std::tuple<Segment, int, int>;
    std::vector<RoadSegment> roadSegments;
    FORALL_VALID_EDGES(graph, u, e) {
      const int v = graph.edgeHead(e);
      const Point tail(graph.latLng(u).lngInDeg(), graph.latLng(u).latInDeg());
      const Point head(graph.latLng(v).lngInDeg(), graph.latLng(v).latInDeg());
      roadSegments.emplace_back(Segment(tail, head), u, v);
    }
    bgi::rtree<RoadSegment, bgi::quadratic<16>> rTree(roadSegments);

    // Map each cell C inside the area under study to the road segment nearest to the center of C.
    const auto sourceCrs = CoordinateTransformation::WGS_84;
    const auto targetCrs = CoordinateTransformation::ETRS89_LAEA_EUROPE;
    CoordinateTransformation trans(sourceCrs, targetCrs);
    Area area;
    area.importFromOsmPolyFile(areaFilename);
    const auto box = area.boundingBox();
    LatLng nw(box.northEast().y(), box.southWest().x());
    LatLng se(box.southWest().y(), box.northEast().x());
    double easting, northing;
    trans.forward(nw.lngInDeg(), nw.latInDeg(), easting, northing);
    const auto min = ::Point(std::round(easting), std::round(northing)) - minCell;
    trans.forward(se.lngInDeg(), se.latInDeg(), easting, northing);
    const auto max = ::Point(std::round(easting), std::round(northing)) - minCell;
    const ::Point minCoveredCell((min.x() + 50) / 100, numRows - (min.y() + 50) / 100 - 1);
    const ::Point maxCoveredCell((max.x() + 50) / 100, numRows - (max.y() + 50) / 100 - 1);
    Matrix<int> representative(numRows, numCols, INVALID_VERTEX);
    int population = 0;
    int numUnmappedCells = 0;
    for (int x = minCoveredCell.x(); x <= maxCoveredCell.x(); ++x)
      for (int y = minCoveredCell.y(); y <= maxCoveredCell.y(); ++y) {
        const auto c = ::Point(x * 100, (numRows - y - 1) * 100) + minCell;
        double lng, lat;
        trans.reverse(c.x(), c.y(), lng, lat);
        const LatLng center(lat, lng);
        if (area.contains({center.longitude(), center.latitude()})) {
          population += populationGrid(y, x);
          const Point queryPoint(center.lngInDeg(), center.latInDeg());
          RoadSegment nearestRoadSegment;
          rTree.query(bgi::nearest(queryPoint, 1), &nearestRoadSegment);
          Segment segment;
          int u, v;
          std::tie(segment, u, v) = nearestRoadSegment;
          if (bg::distance(queryPoint, segment) * EARTH_RADIUS <= maxDist) {
            const auto distToU = bg::comparable_distance(queryPoint, segment.first);
            const auto distToV = bg::comparable_distance(queryPoint, segment.second);
            representative(y, x) = distToU < distToV && graph.containsEdge(v, u) ? u : v;
          } else {
            ++numUnmappedCells;
          }
        }
      }

    // Open the output file.
    std::ofstream out(outputFilename + ".csv");
    if (!out.good())
      throw std::invalid_argument("file cannot be opened -- '" + outputFilename + ".csv'");
    out << "# Input graph: " << graphFilename << '\n';
    out << "# Methodology: radiation model (" << scale << ")\n";
    out << "origin,destination\n";

    // Generate trips between the grid cells.
    Timer timer;
    std::cout << "Generating trips: ";
    const auto coveredGridBounds = maxCoveredCell - minCoveredCell;
    ProgressBar bar((coveredGridBounds.x() + 1) * (coveredGridBounds.y() + 1));
    OctagonalSummedAreaTable sat(populationGrid);
    std::vector<OriginDestination> result;
    std::default_random_engine rand(clp.getValue<int>("seed", 19900325));
    for (int srcX = minCoveredCell.x(); srcX <= maxCoveredCell.x(); ++srcX)
      for (int srcY = minCoveredCell.y(); srcY <= maxCoveredCell.y(); ++srcY) {
        if (representative(srcY, srcX) != INVALID_VERTEX && populationGrid(srcY, srcX) > 0)
          for (int dstX = minCoveredCell.x(); dstX <= maxCoveredCell.x(); ++dstX)
            for (int dstY = minCoveredCell.y(); dstY <= maxCoveredCell.y(); ++dstY)
              if (representative(dstY, dstX) != INVALID_VERTEX && populationGrid(dstY, dstX) > 0) {
                if (srcX == dstX && srcY == dstY)
                  continue;
                const ::Point src(srcX, srcY);
                const ::Point dst(dstX, dstY);
                const double radius = src.getEuclideanDistanceTo(dst);
                int64_t srcPop = populationGrid(srcY, srcX);
                int64_t dstPop = populationGrid(dstY, dstX);
                int64_t surroundingPop = sat.sumOverOctagon(src, radius);

                // Does the surrounding population include the destination population?
                const int height = std::round(0.92387953251128675613 * radius); // r * cos(pi / 8)
                const int side = std::round(0.38268343236508977173 * radius);   // r * sin(pi / 8)
                if (src.getManhattanDistanceTo(dst) <= height + side &&
                    src.getChebyshevDistanceTo(dst) <= height)
                  surroundingPop -= dstPop;
                assert(surroundingPop > 0);

                // Pick the number of trips between the source and destination cell.
                double p = 1.0 * srcPop * dstPop / (surroundingPop * (dstPop + surroundingPop));
                assert(p > 0); assert(p <= 1);
                int numTrips = std::binomial_distribution<>(std::round(scale * srcPop), p)(rand);

                // Generate the chosen number of trips.
                for (int i = 0; i < numTrips; ++i)
                  result.emplace_back(representative(srcY, srcX), representative(dstY, dstX));
              }
        ++bar;
      }
    const int elapsed = timer.elapsed();
    std::cout << "done (" << elapsed << "ms)." << std::endl;

    std::cout << "Writing OD-pairs to file..." << std::flush;
    for (const auto& record : result) {
      out << graph.sequentialVertexId(record.origin) << ',';
      out << graph.sequentialVertexId(record.destination) << '\n';
    }
    std::cout << " done." << std::endl;
  } catch (std::exception& e) {
    std::cerr << argv[0] << ": " << e.what() << std::endl;
    std::cerr << "Try '" << argv[0] <<" -help' for more information." << std::endl;
    return EXIT_FAILURE;
  }
  return EXIT_SUCCESS;
}


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_20_PI_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_20_PI_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief _20_pi generic tag

     Represents the _20_pi constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( _20_pi, double
                                , 63, 0x427b53d1       //20\pi
                                , 0x404f6a7a2955385ell  //20\pi
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::_20_pi, Site> dispatching__20_pi(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::_20_pi, Site>();
   }
   template<class... Args>
   struct impl__20_pi;
  }
  /*!
    Constant \f$20\pi\f$.

    @par Semantic:

    For type T0:

    @code
    T0 r = _20_pi<T0>();
    @endcode

    is similar to:

    @code
    T0 r = Twenty<T0>()*Pi<T0>();
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::_20_pi, _20_pi);
}

#endif