#include <string>
#include <sstream>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <vector>
#include <random>
#include <iterator>
#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>

#include "SVDPackage.h"
#include "Pest.h"
#include "utilities.h"
#include "covariance.h"
#include "FileManager.h"
#include "constraints.h"
#include "linear_analysis.h"

using namespace std;

Constraints::Constraints(Pest& _pest_scenario, FileManager* _file_mgr_ptr, OutputFileWriter& _of_wr, PerformanceLog& _pfm)
	:pest_scenario(_pest_scenario), file_mgr_ptr(_file_mgr_ptr), of_wr(_of_wr), pfm(_pfm), jco(*_file_mgr_ptr, _of_wr)
{
	;
}

void Constraints::initialize(vector<string>& ctl_ord_dec_var_names, Parameters* _current_pars_and_dec_vars_ptr,
	Observations* _current_constraints_sim_ptr, double _dbl_max)
{
	/* initialize the constraint class, most of this is to deal with chances*/
	dec_var_names = ctl_ord_dec_var_names;
	//store a pointer back to the original simulated constrain values
	//this is so that as these are updated, the constraints instance
	//has access to the recent
	current_constraints_sim_ptr = _current_constraints_sim_ptr;
	//same for dec var values
	current_pars_and_dec_vars_ptr = _current_pars_and_dec_vars_ptr;
	//check for a stack size arg - if postive, then use stacks for chances
	stack_size = pest_scenario.get_pestpp_options().get_opt_stack_size();
	//an existing parameter stack for chances
	string par_stack_name = pest_scenario.get_pestpp_options().get_opt_par_stack();
	//maybe even an existing observations (e.g. constraints) stack!
	string obs_stack_name = pest_scenario.get_pestpp_options().get_opt_obs_stack();
	//by default, we want to use fosm for chances, but it any
	//of those stack options were passed, then use stacks instead
	use_fosm = true;
	if ((stack_size > 0) || (par_stack_name.size() > 0) || (obs_stack_name.size() > 0))
		use_fosm = false;
	//initialize the stack constainers (ensemble class instances)
	stack_pe.set_pest_scenario(&pest_scenario);
	stack_pe.set_rand_gen(&rand_gen);
	stack_oe.set_pest_scenario(&pest_scenario);
	stack_oe.set_rand_gen(&rand_gen);
	//initialize some more things
	dbl_max = _dbl_max;
	rand_gen = std::mt19937(pest_scenario.get_pestpp_options().get_random_seed());
	ctl_ord_obs_constraint_names.clear();
	ctl_ord_pi_constraint_names.clear();
	constraint_sense_map.clear();
	constraint_sense_name.clear();
	nz_obs_names.clear();
	adj_par_names.clear();
	prior_const_var.clear();
	post_const_var.clear();

	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	pfm.log_event("initializing constraints");
	//for the LP solve, we need to augment the PI based constraints with dec var bounds
	//this is to make sure we dont end up with an unbounded problem
	if (pest_scenario.get_pestpp_options().get_opt_include_bnd_pi())
	{
		PriorInformation* pi_ptr = pest_scenario.get_prior_info_ptr();
		ParameterInfo par_info = pest_scenario.get_ctl_parameter_info();
		stringstream ss;
		string s;
		for (auto& dname : ctl_ord_dec_var_names)
		{
			ss.str("");
			ss << "_lb_" << dname << " 1.0 * " << dname << " = " << par_info.get_parameter_rec_ptr(dname)->lbnd << " 1.0 greater_pi";
			pi_ptr->AddRecord(pest_utils::upper_cp(ss.str()));
			ss.str("");
			ss << "_LB_" << pest_utils::upper_cp(dname);
			pest_scenario.get_ctl_ordered_pi_names_ptr()->push_back(ss.str());
			ss.str("");
			ss << "_ub_" << dname << " 1.0 * " << dname << " = " << par_info.get_parameter_rec_ptr(dname)->ubnd << " 1.0 less_pi";
			pi_ptr->AddRecord(pest_utils::upper_cp(ss.str()));
			ss.str("");
			ss << "_UB_" << pest_utils::upper_cp(dname);
			pest_scenario.get_ctl_ordered_pi_names_ptr()->push_back(ss.str());
			ss.str("");

		}
		pest_scenario.get_ctl_ordered_obs_group_names_ptr()->push_back("GREATER_PI");
		pest_scenario.get_ctl_ordered_obs_group_names_ptr()->push_back("LESS_PI");
	}

	//---------------------------
	//  ---  obs constraints  ---
	//---------------------------
	//set the two constraints attribs and ordered constraint name vec
	vector<string> constraint_groups = pest_scenario.get_pestpp_options().get_opt_constraint_groups();
	//if not constraint groups set as ++ arg, then look for all compatible obs groups
	if (constraint_groups.size() == 0)
	{
		for (auto& group : pest_scenario.get_ctl_ordered_obs_group_names())
		{
			pair<ConstraintSense, string> sense = get_sense_from_group_name(group);
			if (sense.first != ConstraintSense::undefined)
			{
				constraint_groups.push_back(group);
			}
		}
	}

	//if we still don't have any constraint groups, something is wrong
	if (constraint_groups.size() == 0)
		throw_constraints_error("no viable observation or prior information constraint groups found.  Constraint group names must start with the following: {'l_','less','g_','greater'}");

	ctl_ord_obs_constraint_names.clear();
	//if the ++opt_constraint_groups arg was passed
	if (constraint_groups.size() != 0)
	{
		//first make sure all the groups are actually listed in the control file
		vector<string> missing;
		vector<string> pst_groups = pest_scenario.get_ctl_ordered_obs_group_names();
		vector<string>::iterator end = pst_groups.end();
		vector<string>::iterator start = pst_groups.begin();
		for (auto grp : constraint_groups)
			if (find(start, end, grp) == end)
				missing.push_back(grp);
		if (missing.size() > 0)
			throw_constraints_error("the following ++opt_constraint_groups were not found: ", missing);

		//find the observations in constraints groups
		ObservationInfo oinfo = pest_scenario.get_ctl_observation_info();
		string group;
		double weight;
		end = constraint_groups.end();
		start = constraint_groups.begin();
		int nobszw = 0;
		for (auto& obs_name : pest_scenario.get_ctl_ordered_obs_names())
		{
			group = oinfo.get_observation_rec_ptr(obs_name)->group;
			weight = oinfo.get_weight(obs_name);
			if (find(start, end, group) != end)
			{
				if (weight == 0.0)
				{
					//cout << "Warning: observation constraint " << obs_name << " has 0.0 weight, skipping" << endl;
					nobszw++;
					f_rec << "Warning: observation constraint " << obs_name << " has 0.0 weight, skipping" << endl;
				}
				else
				{
					ctl_ord_obs_constraint_names.push_back(obs_name);
				}

			}
		}
		if (nobszw > 0)
			cout << "Warning: " << nobszw << " of the observation constraints (see rec file for list) have 0.0 weight, skipping" << endl;

		//look for prior information constraints
		const PriorInformation* pinfo = pest_scenario.get_prior_info_ptr();
		PriorInformationRec pi_rec;
		for (auto& pi_name : pest_scenario.get_ctl_ordered_pi_names())
		{
			group = pinfo->get_pi_rec_ptr(pi_name).get_group();
			if (find(start, end, group) != end)
			{
				ctl_ord_pi_constraint_names.push_back(pi_name);
				pi_rec = pinfo->get_pi_rec_ptr(pi_name);
				//pi_constraint_factors[pi_name] = pi_rec.get_atom_factors();
				//pi_constraint_rhs[pi_name] = pi_rec.get_obs_value();
				constraints_pi.AddRecord(pi_name, &pi_rec);
			}
		}

		//check the pi constraint factors for compatibility with available
		start = ctl_ord_dec_var_names.begin();
		end = ctl_ord_dec_var_names.end();
		map<string, vector<string>> missing_map;
		if (num_pi_constraints() > 0)
		{
			for (auto& pi_name : ctl_ord_pi_constraint_names)
			{
				pi_rec = constraints_pi.get_pi_rec_ptr(pi_name);
				missing.clear();
				for (auto& pi_factor : pi_rec.get_atom_factors())
					if (find(start, end, pi_factor.first) == end)
						missing.push_back(pi_factor.first);
				if (missing.size() > 0)
					missing_map[pi_name] = missing;
			}
		}
		if (missing_map.size() > 0)
		{
			stringstream ss;
			ss << " the following prior information constraints reference parameters that are not treated as decision variables:" << endl;
			for (auto& missing_pi : missing_map)
			{
				ss << missing_pi.first << ": ";
				for (auto& par_name : missing_pi.second)
					ss << par_name << ",";
			}
			throw_constraints_error("errors in prior information constraints:" + ss.str());
		}

		//TODO: investigate a pi constraint only formulation
		if (num_obs_constraints() == 0)
			throw_constraints_error("no constraints found in groups: ", constraint_groups);
	}

	constraints_obs = pest_scenario.get_ctl_observations().get_subset(ctl_ord_obs_constraint_names.begin(), ctl_ord_obs_constraint_names.end());
	//constraints_sim = Observations(constraints_obs);

	//build map of obs constraint sense
	vector<string> problem_constraints;
	for (auto& name : ctl_ord_obs_constraint_names)
	{
		string group = pest_scenario.get_ctl_observation_info().get_observation_rec_ptr(name)->group;
		pair<ConstraintSense, string> sense = get_sense_from_group_name(group);
		if (sense.first == ConstraintSense::undefined)
		{
			problem_constraints.push_back(name + ',' + group);
		}
		constraint_sense_map[name] = sense.first;
		constraint_sense_name[name] = sense.second;

	}
	if (problem_constraints.size() > 0)
	{
		throw_constraints_error("the following obs constraints do not have a correct group name prefix {'l_','less','g_','greater','e_','equal'}: ", problem_constraints);
	}

	//build map of pi constraint sense
	problem_constraints.clear();
	for (auto& name : ctl_ord_pi_constraint_names)
	{
		string group = pest_scenario.get_prior_info_ptr()->get_pi_rec_ptr(name).get_group();
		pair<ConstraintSense, string> sense = get_sense_from_group_name(group);
		if (sense.first == ConstraintSense::undefined)
		{
			problem_constraints.push_back(name + ',' + group);
		}
		constraint_sense_map[name] = sense.first;
		constraint_sense_name[name] = sense.second;
	}
	if (problem_constraints.size() > 0)
	{
		throw_constraints_error("the following prior info constraints do not have a correct group name prefix {'l_','less','g_','greater','e_','equal'}: ", problem_constraints);
	}

	set<string> dec_set(ctl_ord_dec_var_names.begin(), ctl_ord_dec_var_names.end());
	for (auto& name : pest_scenario.get_ctl_ordered_par_names())
	{
		//if this parameter is not a decision var
		//if (find(start, end, name) == end)
		if (dec_set.find(name) == dec_set.end())
		{
			ParameterRec::TRAN_TYPE tt = pest_scenario.get_ctl_parameter_info().get_parameter_rec_ptr(name)->tranform_type;
			if ((tt == ParameterRec::TRAN_TYPE::LOG) || (tt == ParameterRec::TRAN_TYPE::NONE))
				adj_par_names.push_back(name);
		}
	}

	//------------------------------------------
	//  ---  chance constratints  ---
	//------------------------------------------
	risk = pest_scenario.get_pestpp_options().get_opt_risk();
	if (risk != 0.5)
	{
		pfm.log_event("initializing chance constraints");
		use_chance = true;
		std_weights = pest_scenario.get_pestpp_options().get_opt_std_weights();

		//make sure risk value is valid
		if ((risk > 1.0) || (risk < 0.0))
			throw_constraints_error("++opt_risk parameter must between 0.0 and 1.0");

		//reset risk extreme risk values
		if (risk > 0.999)
		{
			f_rec << endl << "  ---  note: resetting risk value of " << risk << " to a practical value of " << 0.999 << endl << endl;
			risk = 0.999;
		}
		if (risk < 0.001)
		{
			f_rec << endl << "  ---  note: resetting risk value of " << risk << " to a practical value of " << 0.001 << endl << endl;
			risk = 0.001;
		}

		probit_val = get_probit();
		//if the std weight options was selected, use it - it overrides all other options
		if (std_weights)
		{
			double std, var;
			cout << "++opt_std_weights = True, using weights as chance constraint uncertainty" << endl;
			f_rec << "++opt_std_weights = True, using the following weights as prior and posterior chance constraint uncertainty: " << endl;
			f_rec << setw(25) << "model-based constraint" << setw(15) << "std (weight)" << setw(15) << "variance" << endl;
			for (auto& cname : ctl_ord_obs_constraint_names)
			{
				std = pest_scenario.get_observation_info_ptr()->get_weight(cname);
				var = std * std;
				f_rec << setw(25) << cname << setw(15) << std << setw(15) << var << endl;
				prior_const_var[cname] = var;
				post_const_var[cname] = var;
			}
		}
		//otherwise, if no stack options, use fosm
		else if (use_fosm)
		{
			//make sure there is at least one non-decision var adjustable parameter
			if (adj_par_names.size() == 0)
				throw_constraints_error("++opt_risk != 0.5, but no adjustable parameters found in control file");

			//look for non-zero weighted obs
			vector<string>::iterator start = ctl_ord_obs_constraint_names.begin();
			vector<string>::iterator end = ctl_ord_obs_constraint_names.end();
			for (auto& name : pest_scenario.get_ctl_ordered_obs_names())
			{
				if (find(start, end, name) == end)
				{
					if (pest_scenario.get_ctl_observation_info().get_observation_rec_ptr(name)->weight > 0.0)
						nz_obs_names.push_back(name);
				}
			}

			f_rec << "  using FOSM-based chance constraints with risk = " << risk << endl;

			parcov.try_from(pest_scenario, *file_mgr_ptr);
			vector<string> drop;
			set<string> sadj(adj_par_names.begin(), adj_par_names.end());
			for (auto& n : parcov.get_row_names())
			{
				if (sadj.find(n) == sadj.end())
					drop.push_back(n);
			}
			parcov.drop(drop);

			//build the nz_obs obs_cov
			if (nz_obs_names.size() != 0)
			{
				ObservationInfo oi = pest_scenario.get_ctl_observation_info();
				map<string, double> obs_std = pest_scenario.get_ext_file_double_map("observation data external", "standard_deviation");
				obscov.from_observation_weights(file_mgr_ptr->rec_ofstream(), nz_obs_names, oi, vector<string>(), null_prior, obs_std);
			}
		}
		//otherwise, stack time baby!
		else
		{
			//make sure there is at least one non-decision var adjustable parameter
			if (adj_par_names.size() == 0)
				throw_constraints_error("++opt_risk != 0.5, but no adjustable parameters found in control file");
			bool size_passed = true;
			set<string> passed = pest_scenario.get_pestpp_options().get_passed_args();
			if (passed.find("OPT_STACK_SIZE") == passed.end())
				size_passed = false;
			string par_csv = pest_scenario.get_pestpp_options().get_opt_par_stack();

			//if a par stack wasnt passed, draw one
			if (par_csv.size() == 0)
			{
				if ((stack_size == 0) && (obs_stack_name.size() == 0))
					throw_constraints_error("++opt_stack_size is zero");
				else if (stack_size > 0)
				{
					f_rec << "drawing " << stack_size << "stack realizations" << endl;
					pfm.log_event("loading parcov");
					parcov.try_from(pest_scenario, *file_mgr_ptr);
					pfm.log_event("drawing stack realizations");
					stack_pe.draw(stack_size, pest_scenario.get_ctl_parameters(), parcov, &pfm,
						pest_scenario.get_pestpp_options().get_ies_verbose_level(), file_mgr_ptr->rec_ofstream());
				}
			}
			//otherwise, load the par stack
			else
			{
				string par_ext = pest_utils::lower_cp(par_csv).substr(par_csv.size() - 3, par_csv.size());
				pfm.log_event("processing par stack file " + par_csv);
				if (par_ext.compare("csv") == 0)
				{
					pfm.log_event("loading par stack from csv file: " + par_csv);
					try
					{
						stack_pe.from_csv(par_csv);
					}
					catch (const exception& e)
					{

						throw_constraints_error("error processing par stack: " + string(e.what()));
					}
					catch (...)
					{
						throw_constraints_error(string("error processing par stack"));
					}
				}
				else if ((par_ext.compare("jcb") == 0) || (par_ext.compare("jco") == 0))
				{
					pfm.log_event("loading par stack from binary file+ " + par_csv);
					try
					{
						stack_pe.from_binary(par_csv);
					}
					catch (const exception& e)
					{

						throw_constraints_error("error processing par stack binary file: " + string(e.what()));
					}
					catch (...)
					{
						throw_constraints_error("error processing par stack binary file");
					}
				}
				else
				{
					throw_constraints_error("unrecognized par stack extension, looking for csv, jcb, jco.  found: " + par_ext);
				}
				vector<string> snames = stack_pe.get_var_names();
				set<string> pset(snames.begin(), snames.end());
				vector<string> missing;
				for (auto name : adj_par_names)
					if (pset.find(name) == pset.end())
						missing.push_back(name);
				if (missing.size() > 0)
					throw_constraints_error("par stack missing the following adjustable parameters: ", missing);

				missing.clear();
				for (auto name : dec_var_names)
					if (pset.find(name) == pset.end())
						missing.push_back(name);
				if (missing.size() > 0)
				{
					Eigen::MatrixXd dmat(stack_pe.shape().first, missing.size());
					dmat.setZero();
					stack_pe.extend_cols(dmat,missing);
				}

				if ((size_passed) && (stack_pe.shape().first > stack_size))
				{
					vector<int> drop_rows;
					for (int i = stack_size - 1; i < stack_pe.shape().first; i++)
						drop_rows.push_back(i);
					f_rec << "droppping " << drop_rows.size() << "realizations from par stack b/c of ++opt_stack_size req" << endl;
					stack_pe.drop_rows(drop_rows);
				}
			}
			string filename = file_mgr_ptr->get_base_filename() + ".0.par_stack";
			if (pest_scenario.get_pestpp_options().get_ies_save_binary())
			{
				filename = filename + ".jcb";
				stack_pe.to_binary(filename);
			}
			else
			{
				filename = filename + ".csv";
				stack_pe.to_csv(filename);
			}
			f_rec << "saved initial parameter stack to " << filename << endl;

			//maybe there was an obs stack passed?
			string obs_csv = pest_scenario.get_pestpp_options().get_opt_obs_stack();
			if (obs_csv.size() == 0)
			{
				stack_oe.reserve(stack_pe.get_real_names(), pest_scenario.get_ctl_ordered_obs_names());
			}
			//if so, load the obs (e.g. constraint) stack
			else
			{
				string obs_ext = pest_utils::lower_cp(obs_csv).substr(obs_csv.size() - 3, obs_csv.size());
				pfm.log_event("processing obs stack file " + obs_csv);
				if (obs_ext.compare("csv") == 0)
				{
					pfm.log_event("loading obs stack from csv file: " + obs_csv);
					try
					{
						stack_oe.from_csv(obs_csv);
					}
					catch (const exception& e)
					{

						throw_constraints_error("error processing obs stack: " + string(e.what()));
					}
					catch (...)
					{
						throw_constraints_error(string("error processing obs stack"));
					}
				}
				else if ((obs_ext.compare("jcb") == 0) || (obs_ext.compare("jco") == 0))
				{
					pfm.log_event("loading obs stack from binary file+ " + obs_csv);
					try
					{
						stack_oe.from_binary(obs_csv);
					}
					catch (const exception& e)
					{

						throw_constraints_error("error processing obs stack binary file: " + string(e.what()));
					}
					catch (...)
					{
						throw_constraints_error("error processing obs stack binary file");
					}
				}
				else
				{
					throw_constraints_error("unrecognized obs stack extension, looking for csv, jcb, jco.  found: " + obs_ext);
				}
				vector<string> snames = stack_oe.get_var_names();
				set<string> pset(snames.begin(), snames.end());
				vector<string> missing;
				for (auto name : ctl_ord_obs_constraint_names)
					if (pset.find(name) == pset.end())
						missing.push_back(name);
				if (missing.size() > 0)
					throw_constraints_error("obs stack missing the following constraints: ", missing);

				if (((size_passed) || (stack_pe.shape().first > 0)) && (stack_oe.shape().first > stack_size))
				{
					vector<int> drop_rows;
					for (int i = stack_size - 1; i < stack_oe.shape().first; i++)
						drop_rows.push_back(i);
					f_rec << "droppping " << drop_rows.size() << "realizations from obs stack b/c of ++opt_stack_size req" << endl;
					stack_oe.drop_rows(drop_rows);
				}
			}
		}
	}
	//otherwise, dont use chance constraints
	else use_chance = false;
}

void Constraints::initial_report()
{
	/*make a rec file report before any iterations have happened*/
	ofstream& f_rec = file_mgr_ptr->rec_ofstream();

	f_rec << endl << "  ---  observation constraints ---  " << endl;
	f_rec << setw(20) << "name" << setw(20) << "sense" << setw(20) << "value" << endl;
	for (auto& name : ctl_ord_obs_constraint_names)
	{
		f_rec << setw(20) << left << name;
		f_rec << setw(20) << constraint_sense_name[name];
		f_rec << setw(20) << constraints_obs.get_rec(name) << endl;
	}

	if (num_pi_constraints() > 0)
	{
		f_rec << endl << "  ---  prior information constraints   ---  " << endl;
		f_rec << setw(20) << "name" << setw(20) << "sense" << setw(20) << "value" << endl;
		for (auto& name : ctl_ord_pi_constraint_names)
		{
			f_rec << setw(20) << left << name;
			f_rec << setw(20) << constraint_sense_name[name];
			f_rec << setw(20) << constraints_pi.get_pi_rec_ptr(name).get_obs_value() << endl;
		}
	}

	if (use_chance)
	{
		f_rec << endl << endl << "  ---  chance constraint FOSM information  ---  " << endl;
		if (use_fosm)
		{
			f_rec << "-->using FOSM-based chance constraints" << endl;
			f_rec << "-->++opt_risk and corresponding probit function value: " << setw(10) << risk << setw(20) << probit_val << endl;
			f_rec << "-->number of non-zero weight observations for FOSM calcs: " << num_nz_obs() << endl;
		}
		else
		{
			f_rec << "-->using stack-based chance constraints" << endl;
		}
		f_rec << "-->number of adjustable parameters for chance constraint calcs: " << num_adj_pars() << endl;

		f_rec << "-->repeat chance constraint calculations every: " << pest_scenario.get_pestpp_options().get_opt_recalc_fosm_every() << " iterations" << endl << endl;
		f_rec << "   adjustable parameters used in FOSM calculations:" << endl;
		int i = 1;
		for (auto& name : adj_par_names)
		{
			f_rec << setw(15) << name;
			if (i % 6 == 0)
				f_rec << endl;
			i++;
		}
		f_rec << endl;
		if ((num_nz_obs() == 0) && (use_fosm))
		{
			f_rec << endl << endl << "  ---  Note: No nonzero weight observations found." << endl;
			f_rec << "           Prior constraint uncertainty will be used in FOSM-based chance constraint calculations" << endl;
		}
		else if (use_fosm)
		{
			f_rec << "  non-zero weight observations used for conditioning in FOSM-based chance constraint calculations: " << endl;
			int i = 0;
			for (auto& name : nz_obs_names)
			{
				f_rec << setw(15) << name;
				if (i % 6 == 0)
					f_rec << endl;
				i++;
			}
			f_rec << endl;
		}
	}
}

void Constraints::update_chance_offsets()
{
	/* recalculate the chance bits*/
	if ((!use_chance) || (std_weights))
		return;
	prior_const_var.clear();
	post_const_var.clear();
	//if we are using FOSM basd chance constraints, we need to do some new fosm calcs
	if (use_fosm)
	{
		ofstream& f_rec = file_mgr_ptr->rec_ofstream();
		cout << "  ---  calculating FOSM-based chance constraint components  ---  " << endl;
		f_rec << "  ---  calculating FOSM-based chance constraint components  ---  " << endl;

		//the rows of the fosm jacobian include nonzero weight obs (for schur comp)
		//plus the names of the names of constraints, which get treated as forecasts
		vector<string> fosm_row_names(nz_obs_names);
		fosm_row_names.insert(fosm_row_names.end(), ctl_ord_obs_constraint_names.begin(), ctl_ord_obs_constraint_names.end());

		//extract the part of the full jco we need for fosm
		Eigen::SparseMatrix<double> fosm_mat = jco.get_matrix(fosm_row_names, adj_par_names);

		if (fosm_mat.size() == 0)
			throw_constraints_error("FOSM-based chance constraint-to-parameter vectors are all zeros");

		Mat fosm_jco(fosm_row_names, adj_par_names, fosm_mat);

		//create a linear object
		//Logger logger(file_mgr_ptr->get_ofstream("log"), false);
		LinearAnalysis la(fosm_jco, pest_scenario, *file_mgr_ptr, pfm, parcov, &rand_gen);
		la.set_obscov(obscov);

		//set the prior parameter covariance matrix
		la.set_parcov(parcov);

		//set the predictions (the constraints)
		la.set_predictions(ctl_ord_obs_constraint_names);

		//get the prior and posterior variance of the constraints
		prior_const_var = la.prior_prediction_variance();

		//if at least one nz obs was found, then use schur complment, otherwise,
		//just use the prior constraint uncertainty
		if (num_nz_obs() > 0)
			post_const_var = la.posterior_prediction_variance();
		else
			post_const_var = prior_const_var;
		cout << "  ---   done with FOSM-based chance constraint calculations  ---  " << endl << endl;
		f_rec << "  ---   done with FOSM-based chance constraint calculations  ---  " << endl << endl;
	}
	// or if we are using stacks, we just calculate a new token variance
	//this isnt actually used in the shifting since we do a non-parametric shift along the PDF, but
	//we can still report this variance.
	else
	{
		pair<map<string, double>, map<string, double>> stack_oe_mean_stdev = stack_oe.get_moment_maps();
		for (auto cname : ctl_ord_obs_constraint_names)
		{
			prior_const_var[cname] = stack_oe_mean_stdev.second[cname] * stack_oe_mean_stdev.second[cname];
			post_const_var[cname] = prior_const_var[cname];
		}
	}
}

double Constraints::get_max_constraint_change(Observations& upgrade_obs)
{
	/* work out which constraint has changed the most between the pointer to
	current sim constraint values and upgrade_obs constraint values*/
	double max_abs_constraint_change = -1.0E+10;
	double max_abs_constraint_val = -1.0E+10;
	double val, diff;
	for (auto& name : ctl_ord_obs_constraint_names)
	{
		val = current_constraints_sim_ptr->get_rec(name);
		diff = abs(current_constraints_sim_ptr->get_rec(name) - upgrade_obs[name]);
		max_abs_constraint_change = (diff > max_abs_constraint_change) ? diff : max_abs_constraint_change;
		max_abs_constraint_val = (val > max_abs_constraint_val) ? val : max_abs_constraint_val;
	}
	max_abs_constraint_change /= max(max_abs_constraint_val, 1.0);
	return max_abs_constraint_change;
}

Observations Constraints::get_chance_shifted_constraints()
{
	/* one version of this method that doesnt take any args just use the pointer to the
	current sim constraint values*/
	return get_chance_shifted_constraints(*current_constraints_sim_ptr);
}

Observations Constraints::get_chance_shifted_constraints(Observations& _constraints_sim)
{
	/* get the simulated constraint values with the chance shift applied*/
	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	prior_constraint_offset.clear();
	prior_constraint_stdev.clear();
	post_constraint_offset.clear();
	post_constraint_stdev.clear();
	double new_constraint_val, old_constraint_val, required_val;
	double pr_offset, pt_offset;
	Observations iter_fosm;

	if (use_fosm)
	{
		for (auto& name : ctl_ord_obs_constraint_names)
		{
			prior_constraint_stdev[name] = sqrt(prior_const_var[name]);
			post_constraint_stdev[name] = sqrt(post_const_var[name]);
			//the offset (shift) is just the stdev * the risk-based probit value
			pr_offset = probit_val * prior_constraint_stdev[name];
			pt_offset = probit_val * post_constraint_stdev[name];
			old_constraint_val = _constraints_sim.get_rec(name);
			required_val = constraints_obs[name];

			//if less_than constraint, then add to the sim value, to move positive
			// WRT the required constraint value
			if (constraint_sense_map[name] == ConstraintSense::less_than)
			{

				new_constraint_val = old_constraint_val + pt_offset;
				post_constraint_offset[name] = pt_offset;
				prior_constraint_offset[name] = pr_offset;
			}
			//if greater_than constraint, the substract from the sim value to move
			//negative WRT the required constraint value
			else if (constraint_sense_map[name] == ConstraintSense::greater_than)
			{
				new_constraint_val = old_constraint_val - pt_offset;
				post_constraint_offset[name] = -pt_offset;
				prior_constraint_offset[name] = -pr_offset;
			}
			//if its not an ineq constraint, then there is no direction to shift
			else
				new_constraint_val = current_constraints_sim_ptr->get_rec(name);
			iter_fosm.insert(name, new_constraint_val);
		}
	}
	else
	{
		//work out which realization index corresponds to the risk value
		if (stack_oe.shape().first < 3)
			throw_constraints_error("too few (<3) stack members, cannot continue with stack-based chance constraints");
		int cur_num_reals = stack_oe.shape().first;
		//get the inde (which realization number) represents the risk value according to constraint sense
		//
		//the "less than" realization index is just the risk value times the number of reals
		int lt_idx = int(risk * cur_num_reals);
		//the greater than index is the opposite direction
		int gt_idx = int((1.0-risk) * cur_num_reals);
		//the equality contraint risk index
		int eq_idx = int(0.5 * cur_num_reals);
		//get the mean-centered anomalies - we want to subtract off the mean
		//in case these stack values are being re-used from a previous iteration
		Eigen::MatrixXd anom = stack_oe.get_eigen_anomalies();
		//get the map of realization name to index location in the stack
		stack_oe.update_var_map();
		map<string, int> var_map = stack_oe.get_var_map();
		//get the mean and stdev summary containters, summarized by observation (e.g. constraint) name
		pair<map<string, double>, map<string, double>> mm = stack_oe.get_moment_maps();
		for (auto& name : ctl_ord_obs_constraint_names)
		{
			old_constraint_val = _constraints_sim.get_rec(name);
			//the value that must be statified (from the control file)
			required_val = constraints_obs[name];
			// the realized values of this stack are the anomalies added to the
			//current constraint value - this assumes the current value
			//is the mean of the stack distribution
			Eigen::VectorXd cvec = anom.col(var_map[name]).array() + old_constraint_val;
			//now sort the anomolies + current (mean) value vector
			sort(cvec.data(), cvec.data() + cvec.size());
			//set the stdev container info - this isnt used in
			//calculations but gets reported
			prior_constraint_stdev[name] = mm.second[name];
			post_constraint_stdev[name] = mm.second[name];

			// if this is a "less than" constraint
			if (constraint_sense_map[name] == ConstraintSense::less_than)
			{
				//the posterior shifted constraint value is the stack value at the less than index location minus the
				//current constraint value
				pt_offset = cvec[lt_idx] - old_constraint_val;
				new_constraint_val = cvec[lt_idx];
				post_constraint_offset[name] = pt_offset;
				prior_constraint_offset[name] = pt_offset;

			}

			else if (constraint_sense_map[name] == ConstraintSense::greater_than)
			{
				//the posterior shifted constraint value is the stack value at the greater than index location minus the
				//current constraint value
				pt_offset = cvec[gt_idx] - old_constraint_val;
				new_constraint_val = cvec[gt_idx];
				post_constraint_offset[name] = -pt_offset;
				prior_constraint_offset[name] = -pt_offset;

			}
			else
				new_constraint_val = old_constraint_val;
			iter_fosm.insert(name, new_constraint_val);
		}
	}
	vector<string> names = iter_fosm.get_keys();
	Observations constraints_chance(_constraints_sim);
	constraints_chance.update_without_clear(names, iter_fosm.get_data_vec(names));

	return constraints_chance;
}

vector<double> Constraints::get_constraint_residual_vec(Observations& sim)
{
	/* get the current distance to the the constraint bound - the edge of feasible*/
	vector<double> residuals_vec;
	residuals_vec.resize(num_constraints(), 0.0);

	Observations::const_iterator found_obs;
	Observations::const_iterator not_found_obs = sim.end();
	PriorInformation::const_iterator found_prior_info;

	int i = 0;
	for (vector<string>::iterator b = ctl_ord_obs_constraint_names.begin(), e = ctl_ord_obs_constraint_names.end(); b != e; ++b, ++i)
	{
		found_obs = sim.find(*b);
		if (found_obs != not_found_obs)
		{
			residuals_vec[i] = constraints_obs.get_rec(*b) - ((*found_obs).second);
		}
	}
	return residuals_vec;
}

pair<vector<double>,vector<double>> Constraints::get_constraint_bound_vectors()
{
	/* get the upper and lower bound constraint vectors. For less than constraints, the lower bound is
	set to double max, for greater than constraints, the upper bound is set to double max.
	These are needed for the simplex solve*/
	vector<double> residuals;
	if (use_chance)
	{
		Observations current_constraints_chance = get_chance_shifted_constraints();
		residuals = get_constraint_residual_vec(current_constraints_chance);
	}
	else
	{
		residuals = get_constraint_residual_vec(*current_constraints_sim_ptr);
	}

	vector<double> constraint_ub, constraint_lb;
	for (int i = 0; i < num_obs_constraints(); ++i)
	{
		string name = ctl_ord_obs_constraint_names[i];
		if (constraint_sense_map[name] == ConstraintSense::less_than)
			constraint_ub.push_back(residuals[i]);
		else
			constraint_ub.push_back(dbl_max);

		if (constraint_sense_map[name] == ConstraintSense::greater_than)
			constraint_lb.push_back(residuals[i]);
		else
			constraint_lb.push_back(-dbl_max);
		if (constraint_sense_map[name] == ConstraintSense::equal_to)
		{
			constraint_ub.push_back(residuals[i]);
			constraint_lb.push_back(residuals[i]);
		}
	}

	int noc = num_obs_constraints();
	for (int i = 0; i < num_pi_constraints(); ++i)
	{
		string name = ctl_ord_pi_constraint_names[i];
		double residual = -constraints_pi.get_pi_rec_ptr(name).calc_residual(*current_pars_and_dec_vars_ptr);
		if (constraint_sense_map[name] == ConstraintSense::less_than)
			constraint_ub.push_back(residual);
		else
			constraint_ub.push_back(dbl_max);

		if (constraint_sense_map[name] == ConstraintSense::greater_than)
			constraint_lb.push_back(residual);
		else
			constraint_lb.push_back(-dbl_max);
		if (constraint_sense_map[name] == ConstraintSense::equal_to)
		{
			constraint_ub.push_back(residual);
			constraint_lb.push_back(residual);
		}
	}
	current_bounds = pair<vector<double>, vector<double>>(constraint_lb, constraint_ub);
	return current_bounds;
}

pair<Constraints::ConstraintSense, string> Constraints::get_sense_from_group_name(const string& name)
{
	/* work out the constraint sense from the obs group name*/
	if ((name.compare(0, 2, "L_") == 0) || (name.compare(0, 4, "LESS") == 0))
		return pair<ConstraintSense, string>(ConstraintSense::less_than, "less_than");
	else if ((name.compare(0, 2, "G_") == 0) || (name.compare(0, 7, "GREATER") == 0))
		return pair<ConstraintSense, string>(ConstraintSense::greater_than, "greater_than");
	else if ((name.compare(0, 2, "N_") == 0) || (name.compare(0, 2, "E_") == 0) || (name.compare(0, 5, "EQUAL") == 0))
		return pair<ConstraintSense, string>(ConstraintSense::equal_to, "equal_to");
	else
		return pair<ConstraintSense, string>(ConstraintSense::undefined, "undefined");
}

void Constraints::throw_constraints_error(string message, const vector<string>& messages)
{
	stringstream ss;
	for (auto& mess : messages)
		ss << mess + ',';
	throw_constraints_error(message + ss.str());
}

void Constraints::throw_constraints_error(string message, const set<string>& messages)
{
	stringstream ss;
	for (auto& mess : messages)
		ss << mess + ',';
	throw_constraints_error(message + ss.str());
}

void Constraints::throw_constraints_error(string message)
{
	string error_message = "error in Constraints: " + message;
	file_mgr_ptr->rec_ofstream() << error_message << endl;
	file_mgr_ptr->close_file("rec");
	cout << endl << endl << error_message << endl << endl;
	throw runtime_error(error_message);
}

double  Constraints::ErfInv2(double x)
{
	/* the inverse error function, needed for the FOSM-based chance constraints*/
	float tt1, tt2, lnx, sgn;
	sgn = (x < 0) ? -1.0f : 1.0f;

	x = (1 - x) * (1 + x);        // x = 1 - x*x;
	lnx = logf(x);
	//double PI = 3.14159265358979323846;
	tt1 = 2 / (M_PI * 0.147) + 0.5f * lnx;
	tt2 = 1 / (0.147) * lnx;

	return(sgn * sqrtf(-tt1 + sqrtf(tt1 * tt1 - tt2)));
}

double Constraints::get_probit()
{
	/* the probit function estimate - needed for the fosm-basd chance constraints*/
	double output = sqrt(2.0) * ErfInv2((2.0 * risk) - 1.0);
	return output;
}

void Constraints::presolve_report(int iter)
{
	/* this is a report to the rec file for the status of the constraints before solving the current iteration*/

	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	vector<double> residuals;
	f_rec << endl << "  observation constraint information at start of iteration " << iter << endl;
	f_rec << setw(20) << left << "name" << right << setw(15) << "sense" << setw(15) << "required" << setw(15) << "sim value";
	f_rec << setw(15) << "residual" << setw(15) << "lower bound" << setw(15) << "upper bound" << endl;

	//if we are using chances, then we need to account for that here
	Observations current;
	if (use_chance)
	{
		current = get_chance_shifted_constraints();
	}
	//otherwise, just use the pointer to the current constraint info
	else
	{
		current = *current_constraints_sim_ptr;
	}

	residuals = get_constraint_residual_vec(current);
	for (int i = 0; i < num_obs_constraints(); ++i)
	{
		string name = ctl_ord_obs_constraint_names[i];
		f_rec << setw(20) << left << name;
		f_rec << setw(15) << right << constraint_sense_name[name];
		f_rec << setw(15) << constraints_obs.get_rec(name);
		f_rec << setw(15) << current.get_rec(name);
		f_rec << setw(15) << residuals[i];
		f_rec << setw(15) << current_bounds.first[i];
		f_rec << setw(15) << current_bounds.second[i] << endl;
	}
	if (num_pi_constraints() > 0)
	{

		//report prior information constraints
		f_rec << endl << "  prior information constraint information at start of iteration " << iter << endl;
		f_rec << setw(20) << left << "name" << right << setw(15) << "sense" << setw(15) << "required" << setw(15) << "sim value";
		f_rec << setw(15) << "residual" << setw(15) << "lower bound" << setw(15) << "upper bound" << endl;
		for (int i = 0; i < num_pi_constraints(); ++i)
		{
			string name = ctl_ord_pi_constraint_names[i];
			PriorInformationRec pi_rec = constraints_pi.get_pi_rec_ptr(name);
			f_rec << setw(20) << left << name;
			f_rec << setw(15) << right << constraint_sense_name[name];
			f_rec << setw(15) << pi_rec.get_obs_value();
			f_rec << setw(15) << pi_rec.calc_residual_and_sim_val(*current_pars_and_dec_vars_ptr).first;
			f_rec << setw(15) << pi_rec.calc_residual(*current_pars_and_dec_vars_ptr);
			f_rec << setw(15) << current_bounds.first[num_obs_constraints() + i];
			f_rec << setw(15) << current_bounds.second[num_obs_constraints() + i] << endl;

		}
	}

	if (use_chance)
		presolve_chance_report(iter);

	return;
}

void Constraints::write_res_files(Observations& constraints, Parameters& pars_and_dec_vars, string tag, int iter)
{
	/* one form of this method that doesnt require advanced knowledge of whether or not chances are being used.
	write a pest-style residuals file, and, if using chances, write a sep file that includes the chance offset info*/
	write_res_file(constraints, pars_and_dec_vars, tag, iter, false);
	if (use_chance)
		write_res_file(constraints, pars_and_dec_vars, tag, iter, true);
}

void Constraints::write_res_file(Observations& constraints, Parameters& pars_and_dec_vars, string tag, int iter, bool include_chance)
{
	/* write a pest-style residuals file.  If chances are in use, then add "chance" to the file name
	and write chance-shifted values for the "modelled" column*/
	stringstream ss;
	Observations temp = constraints;
	if (include_chance)
	{
		ss << iter << "." << tag << "+chance.rei";
		for (auto& o : ctl_ord_obs_constraint_names)
			temp[o] = temp[o] + post_constraint_offset[o];
	}
	else
	{

		ss << iter << "." << tag << ".rei";
	}
	of_wr.write_opt_constraint_rei(file_mgr_ptr->open_ofile_ext(ss.str()), iter, pars_and_dec_vars,
		pest_scenario.get_ctl_observations(), temp);
	file_mgr_ptr->close_file(ss.str());

}

void Constraints::presolve_chance_report(int iter)
{
	/* write chance info to the rec file before undertaking the current iteration process*/
	if (!use_chance)
		return;

	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	//vector<double> residuals = get_constraint_residual_vec();
	f_rec << endl << "  FOSM-based chance constraint information at start of iteration " << iter << endl;
	f_rec << setw(20) << left << "name" << right << setw(10) << "sense" << setw(15) << "required" << setw(15) << "sim value";
	f_rec << setw(15) << "prior stdev" << setw(15) << "post stdev" << setw(15) << "offset";
	f_rec << setw(15) << "new sim value" << endl;
	vector<string> out_of_bounds;
	Observations current_constraints_chance = get_chance_shifted_constraints();
	for (int i = 0; i < num_obs_constraints(); ++i)
	{
		string name = ctl_ord_obs_constraint_names[i];
		f_rec << setw(20) << left << name;
		f_rec << setw(10) << right << constraint_sense_name[name];
		f_rec << setw(15) << constraints_obs[name];
		f_rec << setw(15) << current_constraints_sim_ptr->get_rec(name);
		f_rec << setw(15) << prior_constraint_stdev[name];
		f_rec << setw(15) << post_constraint_stdev[name];
		f_rec << setw(15) << post_constraint_offset[name];
		f_rec << setw(15) << current_constraints_chance[name] << endl;
	}
	f_rec << "  note: 'offset' is the value added to the simulated constraint value to account" << endl;
	f_rec << "        for the uncertainty in the constraint value arsing from uncertainty in the " << endl;
	f_rec << "        adjustable parameters identified in the control file." << endl << endl;

	return;

}

bool Constraints::should_update_chance(int iter)
{
	/* this is a total hack - it tries to determine if it is time to update the chance
	information, like should we queue up some JCO pertubation runs for FOSM chances or
	should we queue up some stack runs for stack-based chances*/
	if (!use_chance)
		return false;
	if (std_weights)
		return false;
	if (iter == 1)
	{
		if (use_fosm)
			return true;
		if (pest_scenario.get_pestpp_options().get_opt_obs_stack().size() > 0)
			return false;
		else
			return true;
	}
	else if ((iter + 1) % pest_scenario.get_pestpp_options().get_opt_recalc_fosm_every() == 0)
		return true;
	return false;
}

void Constraints::postsolve_obs_constraints_report(Observations& constraints_sim, string tag, int iter,
									map<string,string> status_map, map<string,double> price_map)
{
	/* write out constraint info after the current iteration solution process is over to the rec file.

	This really only applies to the simplex solution because we can, with the linear assumption,
	project what the resulting constraint values should be*/

	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	f_rec << endl << endl << "     " << tag << " constraint information at end of iteration " << iter << endl << endl;
	f_rec << setw(20) << left << "name" << right << setw(15) << "sense" << setw(15) << "required" << setw(25);
	if (status_map.size() > 0)
		f_rec << "simplex status";
	if (price_map.size() > 0)
		f_rec << setw(15) << "price";
	if (use_chance)
		f_rec << setw(15) << "fosm offset";
	f_rec << setw(15) << "current" << setw(15) << "residual";
	f_rec << setw(15) << "new" << setw(15) << "residual" << endl;

	vector<double> cur_residuals = get_constraint_residual_vec(*current_constraints_sim_ptr);
	vector<double> new_residuals;
	if (use_chance)
	{
		Observations constraints_shift = get_chance_shifted_constraints(constraints_sim);
		new_residuals = get_constraint_residual_vec(constraints_shift);
	}
	else
		new_residuals = get_constraint_residual_vec(constraints_sim);
	double sim_val;
	for (int i = 0; i < num_obs_constraints(); ++i)
	{
		string name = ctl_ord_obs_constraint_names[i];
		sim_val = constraints_sim[name];
		f_rec << setw(20) << left << name;
		f_rec << setw(15) << right << constraint_sense_name[name];
		f_rec << setw(15) << constraints_obs.get_rec(name);
		if (status_map.size() > 0)
			f_rec << setw(25) << status_map[name];
		if (price_map.size() > 0)
			f_rec << setw(15) << price_map[name];
		if (use_chance)
		{
			double offset = post_constraint_offset[name];
			sim_val += offset;
			f_rec << setw(15) << offset;
			f_rec << setw(15) << current_constraints_sim_ptr->get_rec(name);
			f_rec << setw(15) << cur_residuals[i];
			f_rec << setw(15) << sim_val;
			f_rec << setw(15) << new_residuals[i] << endl;
		}
		else
		{
			f_rec << setw(15) << current_constraints_sim_ptr->get_rec(name);
			f_rec << setw(15) << cur_residuals[i];
			f_rec << setw(15) << sim_val;
			f_rec << setw(15) << new_residuals[i] << endl;

		}

	}
}

void Constraints::postsolve_pi_constraints_report(Parameters& pars_and_dec_vars, int iter, map<string,string> status_map, map<string,double> price_map)
{

	/*report prior information constraints stats to the rec file after the current iteration solution process.

	This really only applies to the simplex solution because we can, with the linear assumption,
	project what the resulting constraint values should be
	*/
	ofstream &f_rec = file_mgr_ptr->rec_ofstream();
	if (num_pi_constraints() > 0)
	{
		f_rec << endl << endl << "     prior information constraint information at end of iteration " << iter << endl << endl;
		f_rec << setw(20) << left << "name" << right << setw(15) << "sense" << setw(15) << "required" << setw(25);
		if (status_map.size() > 0)
			f_rec << "simplex status";
		if (price_map.size() > 0)
			f_rec << setw(15);
		f_rec << "price" << setw(15) << "current" << setw(15) << "residual";
		f_rec << setw(15) << "new" << setw(15) << "residual" << endl;
		int i = 0;
		for (auto &name : ctl_ord_pi_constraint_names)
		{
			PriorInformationRec pi_rec = constraints_pi.get_pi_rec_ptr(name);
			pair<double,double> cur_sim_resid = pi_rec.calc_residual_and_sim_val(*current_pars_and_dec_vars_ptr);
			pair<double,double> new_sim_resid = pi_rec.calc_residual_and_sim_val(pars_and_dec_vars);
			f_rec << setw(20) << left << name;
			f_rec << setw(15) << right << constraint_sense_name[name];
			f_rec << setw(15) << pi_rec.get_obs_value();
			if (status_map.size() > 0)
				f_rec << setw(25) << status_map[name];
			if (price_map.size() > 0)
				f_rec << setw(15) << price_map[name];
			f_rec << setw(15) << cur_sim_resid.first;
			f_rec << setw(15) << cur_sim_resid.second;
			f_rec << setw(15) << new_sim_resid.first;
			f_rec << setw(15) << new_sim_resid.second << endl;
			i++;
		}
	}

	return;
}

void Constraints::process_runs(RunManagerAbstract* run_mgr_ptr,int iter)
{
	/* using the passed in run mgr pointer, process any runs that were queued up for chance-based
	calculations

	*/
	ofstream& f_rec = file_mgr_ptr->rec_ofstream();
	if (!use_chance)
		return;
	//if we are using fosm chances, then process some jco runs
	if (use_fosm)
	{
		if (jco.get_par_run_map().size() == 0)
			return;
		pfm.log_event("reading FOSM-based parameter pertubation runs into JCO");
		ParamTransformSeq par_trans = pest_scenario.get_base_par_tran_seq();
		bool success = jco.process_runs(par_trans, pest_scenario.get_base_group_info(), *run_mgr_ptr,
			*null_prior, false, false);
		if (!success)
			throw_constraints_error("error processing FOSM JCO matrix runs ", jco.get_failed_parameter_names());
	}
	//otherwise, process some stack runs
	else
	{

		vector<int> failed_runs = stack_oe.update_from_runs(stack_pe_run_map, run_mgr_ptr);

		if (failed_runs.size() > 0)
		{
			f_rec << "WARNING: " << failed_runs.size() << " stack realizations failed" << endl;
			stack_pe.drop_rows(failed_runs);
		}
		stringstream ss;
		ss << file_mgr_ptr->get_base_filename() << "." << iter << ".par_stack";
		if (pest_scenario.get_pestpp_options().get_ies_save_binary())
		{
			ss << ".jcb";
			stack_pe.to_binary(ss.str());
		}
		else
		{
			ss << ".csv";
			stack_pe.to_csv(ss.str());
		}
		ss.str("");
		ss << file_mgr_ptr->get_base_filename() << "." << iter << ".obs_stack";
		if (pest_scenario.get_pestpp_options().get_ies_save_binary())
		{
			ss << ".jcb";
			stack_oe.to_binary(ss.str());
		}
		else
		{
			ss << ".csv";
			stack_oe.to_csv(ss.str());
		}
		f_rec << "Note: " << stack_oe.shape().first << " stack realizations active" << endl;

	}

}

void Constraints::add_runs(RunManagerAbstract* run_mgr_ptr)
{
	/* using the passed run mgr pointer, queue up chance runs

	*/
	if (!use_chance)
		return;
	//for fosm, we need to queue up parameter pertubation runs, centered at the current dec var values
	if (use_fosm)
	{
		pfm.log_event("building FOSM-based parameter pertubation runs");
		ParamTransformSeq pts = pest_scenario.get_base_par_tran_seq();
		set<string> out_of_bounds;
		//the  second-to-last true arg is to run the parvals listed in _current_pars_and_dec_vars as the "base" run
		//this is to make sure we get good pertubations at the current point in par/dec var space.
		//but this means we might have to run the base run twice at least for the SLP process.
		//the way around this is to just add the runs all at once wihin the SLP add run
		//bit using Constraints::get_fosm_par_names()
		//the last false says not to reinitialize the run mgr since the calling process may have also
		//added runs
		cout << "  ---  running " << num_adj_pars() << " model runs for FOSM-based chance constraints  ---  " << endl;
		bool success = jco.build_runs(*current_pars_and_dec_vars_ptr, *current_constraints_sim_ptr, adj_par_names, pts,
			pest_scenario.get_base_group_info(), pest_scenario.get_ctl_parameter_info(),
			*run_mgr_ptr, out_of_bounds, false, true, false);
		if (!success)
		{
			const set<string> failed = jco.get_failed_parameter_names();
			throw_constraints_error("failed to calc derviatives for the following FOSM parameters: ", failed);
		}
	}
	//for stacks, we need to queue up the stack realizations, but replace the dec var entries in each realization
	//with the current dec var values.
	else
	{

		//update stack_pe parameter values for decision variables using current_pars_and_dec_vars_ptr
		int num_reals = stack_pe.shape().first;
		map<string, int> var_map = stack_pe.get_var_map();
		for (auto dname : dec_var_names)
		{
			Eigen::VectorXd dvec(num_reals);
			dvec.setConstant(current_pars_and_dec_vars_ptr->get_rec(dname));
			stack_pe.replace_col(dname, dvec);
		}
		pfm.log_event("building stack-based parameter runs");
		cout << "  ---  running " << stack_pe.shape().first << " model runs for stack-based chance constraints  ---  " << endl;
		stack_pe_run_map.clear();
		stack_pe_run_map = stack_pe.add_runs(run_mgr_ptr);
	}
}

vector<string> Constraints::get_fosm_par_names()
{
	/* get a vector of the fosm-based "parameter" names - adjustable, non-dec-var parameters in the control file*/
	if (use_fosm)
		return adj_par_names;
	else
		return vector<string>();
}

map<string, double> Constraints::get_unsatified_pi_constraints(Parameters& par_and_dec_vars, double tol)
{
	/* get a map of name, distance for each of the prior info constraints that are not satisfied in the par_and_dec_vars container.
	tol is a percent-based tolerance to accont for constraints that are very near their required (rhs) value

	*/
	double sim_val, obs_val, scaled_diff;
	map<string, double> unsatisfied;

	for (auto& name : ctl_ord_pi_constraint_names)
	{
		PriorInformationRec pi_rec = constraints_pi.get_pi_rec_ptr(name);
		//pair<double, double> cur_sim_resid = pi_rec.calc_residual_and_sim_val(*current_pars_and_dec_vars_ptr);
		pair<double, double> new_sim_resid = pi_rec.calc_residual_and_sim_val(par_and_dec_vars);
		//check for invalid pi constraints
		sim_val = new_sim_resid.first;
		obs_val = pi_rec.get_obs_value();
		scaled_diff = abs((obs_val - sim_val) / obs_val);
		if ((constraint_sense_map[name] == ConstraintSense::less_than) && (sim_val > obs_val) && (scaled_diff > tol))
			unsatisfied[name] = sim_val - obs_val;
		else if ((constraint_sense_map[name] == ConstraintSense::greater_than) && (sim_val < obs_val) && (scaled_diff > tol))
			unsatisfied[name] = obs_val - sim_val;
		else if ((constraint_sense_map[name] == ConstraintSense::equal_to) && (sim_val != obs_val) && (scaled_diff > tol))
			unsatisfied[name] = abs(sim_val - obs_val);
	}
	return unsatisfied;
}

map<string, double> Constraints::get_unsatified_obs_constraints(Observations& constraints_sim, double tol)
{
	/* get a map of name, distance for each of the obs-based (e.g. model-based) constraints that are not satisfied in the constraint_obs container.
	tol is a percent-based tolerance to accont for constraints that are very near their required (rhs) value

	*/
	double sim_val, obs_val, scaled_diff;
	map<string, double> unsatisfied;
	Observations _constraints_sim = constraints_sim;
	if (use_chance)
		_constraints_sim = get_chance_shifted_constraints(constraints_sim);
	for (int i = 0; i < num_obs_constraints(); ++i)
	{
		string name = ctl_ord_obs_constraint_names[i];
		sim_val = constraints_sim[name];
		if (use_chance)
		{
			double offset = post_constraint_offset[name];
			sim_val += offset;
		}
		obs_val = constraints_obs[name];
		scaled_diff = abs((obs_val - sim_val) / obs_val);
		//check for invalid obs constraints (e.g. satified)
		if ((constraint_sense_map[name] == ConstraintSense::less_than) && (sim_val > obs_val) && (scaled_diff > tol))
			unsatisfied[name] = sim_val - obs_val;
		else if ((constraint_sense_map[name] == ConstraintSense::greater_than) && (sim_val < obs_val) && (scaled_diff > tol))
			unsatisfied[name] = obs_val - sim_val;
		else if ((constraint_sense_map[name] == ConstraintSense::equal_to) && (sim_val != obs_val) && (scaled_diff > tol))
			unsatisfied[name] = abs(sim_val - obs_val);
	}
	return unsatisfied;

}

int Constraints::get_num_nz_pi_constraint_elements()
{
	/* work out how many elements are in the PI constraints.  This is needed for dimensioning the
	simplex response matrix*/
	int num = 0;

	for (auto &pi_name : ctl_ord_pi_constraint_names)
	{
		num += constraints_pi.get_pi_rec_ptr(pi_name).get_atom_factors().size();
	}
	return num;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2006 Joseph Wang

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 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 license for more details.
*/

/*! \file surface.hpp
    \brief Surface
*/

#ifndef quantlib_surface_hpp
#define quantlib_surface_hpp

#include <ql/math/domain.hpp>
#include <boost/shared_ptr.hpp>
#include <functional>

namespace QuantLib {

// we know Domain is deprecated, we're deprecating Surface too.
#if defined(BOOST_MSVC)
#pragma warning( push )
#pragma warning( disable : 4996 )
#elif defined(__clang__)
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wdeprecated-declarations"
#elif defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#endif

    //! %Surface abstract class
    /*! \deprecated */
    class QL_DEPRECATED Surface
        : public std::binary_function<Real, Real, Real> {
      public:
        virtual Real operator()(Real x, Real y) const = 0;
        virtual boost::shared_ptr<Domain> domain() const = 0;
        virtual ~Surface() {};
    };

    class QL_DEPRECATED TestSurface : public Surface {
      public:
        Real operator()(Real x, Real y) const;
        boost::shared_ptr<Domain> domain() const;
    };

#if defined(BOOST_MSVC)
#pragma warning( pop )
#elif defined(__clang__)
#pragma clang diagnostic pop
#elif defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

}

#endif


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "stuck-dye-transition.h"

// Local project headers:
#include "hmm/fit/error-model-fitter.h"
#include "hmm/state-vector/stuck-dye-state-vector.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(stuck_dye_transition_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    double loss_rate = 0.035;
    StuckDyeTransition sdt(loss_rate);
    BOOST_TEST(sdt.loss_rate == loss_rate);
}

BOOST_AUTO_TEST_CASE(forward_test, *tolerance(TOL)) {
    double loss_rate = 0.035;
    StuckDyeTransition sdt(loss_rate);
    int num_edmans = 0;
    StuckDyeStateVector sdsv;
    sdsv.dye = 0.3;
    sdsv.no_dye = 0.7;
    sdt.forward(&num_edmans, &sdsv);
    BOOST_TEST(sdsv.dye == 0.3 * (1 - loss_rate));
    BOOST_TEST(sdsv.no_dye == 0.7 + 0.3 * loss_rate);
    BOOST_TEST(num_edmans == 1);
}

BOOST_AUTO_TEST_CASE(backward_test, *tolerance(TOL)) {
    double loss_rate = 0.035;
    StuckDyeTransition sdt(loss_rate);
    int num_edmans = 1;
    StuckDyeStateVector input;
    input.dye = 0.3;
    input.no_dye = 0.7;
    StuckDyeStateVector output;
    sdt.backward(input, &num_edmans, &output);
    BOOST_TEST(output.dye == loss_rate * 0.7 + (1 - loss_rate) * 0.3);
    BOOST_TEST(output.no_dye == 0.7);
    BOOST_TEST(num_edmans == 0);
}

BOOST_AUTO_TEST_CASE(improve_fit_test, *tolerance(TOL)) {
    double loss_rate = 0.035;
    StuckDyeTransition sdt(loss_rate);
    int num_edmans = 1;
    double probability = 0.98765;
    StuckDyeStateVector forward_sdsv;
    forward_sdsv.dye = 0.3;
    forward_sdsv.no_dye = 0.7;
    StuckDyeStateVector backward_sdsv;
    backward_sdsv.dye = 0.4;
    backward_sdsv.no_dye = 0.6;
    StuckDyeStateVector next_backward_sdsv;
    next_backward_sdsv.dye = 0.2;
    next_backward_sdsv.no_dye = 0.8;
    ErrorModelFitter emf;
    sdt.improve_fit(forward_sdsv,
                    backward_sdsv,
                    next_backward_sdsv,
                    num_edmans,
                    probability,
                    &emf);
    BOOST_TEST(emf.p_stuck_dye_loss_fit.get()
               == (0.3 * loss_rate * 0.8) / (0.3 * 0.4));
}

BOOST_AUTO_TEST_SUITE_END()  // stuck_dye_transition_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


//==============================================================================
//         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_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SQRT_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
      @brief  sqrt generic tag

      Represents the sqrt function in generic contexts.

      @par Models:
    **/
    struct sqrt_ : ext::elementwise_<sqrt_>
    {
      typedef ext::elementwise_<sqrt_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_sqrt_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::sqrt_, Site> dispatching_sqrt_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::sqrt_, Site>();
   }
   template<class... Args>
   struct impl_sqrt_;
  }
  /*!
    Computes the square root of its parameter. For integers it is the
    truncation of the real square root.

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

    @code
    T r = sqrt(x);
    @endcode

    @param  a0

    @return      a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::sqrt_, sqrt, 1)
} }

#endif


/***************************************************************************************************************************
 * ellipse_det.cpp
 * Author: Jario
 * Update Time: 2020.1.14
 *
 * 说明: 椭圆识别程序(同时可以判断椭圆中心标识，需要训练)，具体训练方式见Prometheus/Modules/object_detection/README.md
 *      1. 【订阅】图像话题 (默认来自web_cam)
 *         /prometheus/camera/rgb/image_raw
 *      2. 【发布】目标位置，发布话题见 Prometheus/Modules/msgs/msg/DetectionInfo.msg
 *         /prometheus/object_detection/ellipse_det
 *      3. 【发布】检测结果的可视化图像话题
 *         /prometheus/camera/rgb/image_ellipse_det
***************************************************************************************************************************/
#include <math.h>
#include <string>
#include <vector>
#include <iostream>
#include <pthread.h>
#include <thread>
#include <chrono>
#include <boost/thread/mutex.hpp>
#include <boost/thread/shared_mutex.hpp>

#include <ros/ros.h>
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>
#include <prometheus_msgs/DetectionInfo.h>
#include <geometry_msgs/Pose.h>
#include <std_msgs/Bool.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/ml.hpp>
#include "spire_ellipsedetector.h"
#include "message_utils.h"

using namespace std;
using namespace cv;
using namespace cv::ml;
using namespace spire;

// #define MARKER_SIZE 0.18

#define ELLIPSE_DET
#define ELLIPSE_PUB

// circle eg. IM_NAME.jpg 0 is background 1 is circle
// std::string imlist_dir = "/home/nvidia/vision_ws/src/ellipse_det_ros/labeled_img_class.txt";  // 进行一个字符串赋值，方便下面调用
// images, include above information
std::string base_path = "/home/nvidia/vision_ws/src/ellipse_det_ros/images_from_camera/";  // 同上

//【订阅】输入开关量
ros::Subscriber switch_subscriber;
// 接收消息，允许暂停检测
bool is_suspanded = false;
// 使用cout打印消息
bool local_print = true;
// 使用prometheus_msgs::Message打印消息
bool message_print = true;
//【发布】调试消息
ros::Publisher message_pub;
std::string msg_node_name;

// 相机话题中的图像同步相关变量
int frame_width, frame_height;
std_msgs::Header image_header;
cv::Mat cam_image_copy;
boost::shared_mutex mutex_image_callback;
bool image_status = false;
boost::shared_mutex mutex_image_status;

EllipseDetector ellipse_detector;
HOGDescriptor hog(Size(28, 28), Size(4, 4), Size(4, 4), Size(4, 4), 9);
bool use_hog = true;

// 图像接收回调函数，接收web_cam的话题，并将图像保存在cam_image_copy中
void cameraCallback(const sensor_msgs::ImageConstPtr& msg)
{
    if (local_print)
        ROS_DEBUG("[EllipseDetector] USB image received.");

    cv_bridge::CvImagePtr cam_image;

    try {
        cam_image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
        image_header = msg->header;
    } catch (cv_bridge::Exception& e) {
        if (local_print)
            ROS_ERROR("cv_bridge exception: %s", e.what());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::ERROR, msg_node_name, "cv_bridge exception");
        return;
    }

    if (cam_image) {
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            cam_image_copy = cam_image->image.clone();
        }
        {
            boost::unique_lock<boost::shared_mutex> lockImageStatus(mutex_image_status);
            image_status = true;
        }
        frame_width = cam_image->image.size().width;
        frame_height = cam_image->image.size().height;
    }
    return;
}

// 用此函数查看是否收到图像话题
bool getImageStatus(void)
{
    boost::shared_lock<boost::shared_mutex> lock(mutex_image_status);
    return image_status;
}

void ellipse_det(cv::Mat& input, cv::Mat& output, std::vector<Ellipse>& ells)
{
    static bool first_im = true;
    if (first_im)
    {
        float   fThScoreScore = 0.7f;   // 0.8
        float   fMinReliability = 0.4f; // Const parameters to discard bad ellipses 0.4
        float   fTaoCenters = 0.04f;    // 0.05
        int     iThLength = 16;         // 16
        float   fMinOrientedRectSide = 3.0f;
        int     iNs = 16;
        Size    szPreProcessingGaussKernelSize = Size(5, 5);
        double  dPreProcessingGaussSigma = 1.0;
        float   fThPos = 1.0f;
        float   fDistanceToEllipseContour = .1f;

        Size sz = input.size();

        float fMaxCenterDistance = sqrt(float(sz.width*sz.width + sz.height*sz.height)) * fTaoCenters;
        // Initialize Detector with selected parameters
        ellipse_detector.SetParameters(szPreProcessingGaussKernelSize,
            dPreProcessingGaussSigma,
            fThPos,
            fMaxCenterDistance,
            iThLength,
            fMinOrientedRectSide,
            fDistanceToEllipseContour,
            fThScoreScore,
            fMinReliability,
            iNs
        );
        first_im = false;
    }

    input.copyTo(output);
    ellipse_detector.Detect(input, ells);
    ellipse_detector.DrawDetectedEllipses(output, ells);
}

vector<float> hist_feature(cv::Mat& resized_im)  // 生成直方图，带入形参 Mat resized_im
{
    float feats0[10], feats1[10], feats2[10];    // 创建三个数组，用于承载三个通道的数据
    for (int i = 0; i < 10; i++)  // 全部赋值为0
    {
        feats0[i] = 0; feats1[i] = 0; feats2[i] = 0;
    }
    for (int i = 0; i < resized_im.rows; i++) for (int j = 0; j < resized_im.cols; j++)  // 对形参Mat resized_im图像的每一个像素点都进行遍历
    {
        uchar s = resized_im.ptr<uchar>(i, j)[0];  // 并将每一个像素的通道中的值赋值给SHV
        uchar h = resized_im.ptr<uchar>(i, j)[1];
        uchar v = resized_im.ptr<uchar>(i, j)[2];

        float gird = (256. / 10.);         // 设置一个参数，用于分割各通道。这里之所以除以10,是因为feat0,feat1,feat2数组长度设置为10。
        int ind = (int)((float)s / gird);  // 将feat数组中对应的数值进行++
        feats0[ind] ++;

        ind = (int)((float)h / gird);
        feats1[ind] ++;

        ind = (int)((float)v / gird);
        feats2[ind] ++;
    }

    vector<float> feats(30, 0); float total(0);  // 创建一个vector，长度为30,全部为0;创建一个变量total=0
    for (int i = 0; i < 30; i++)                 // 就是将feat0,feat0，feat2中30个数的值依次赋值给feats，并全部相加赋值给total，为了下一步的归一化做准备。
    {
        if (i < 10)
        {
            feats[i] = feats0[i];
            total += feats[i];
        }
        else if (i < 20)
        {
            feats[i] = feats1[i-10];
            total += feats[i];
        }
        else
        {
            feats[i] = feats2[i-20];
            total += feats[i];
        }
    }

    for (int i = 0; i < 30; i++)  // 进行归一化
    {
        feats[i] /= total;
    }

    if (use_hog)
    {
        cv::Mat resized_im_gray;
        cvtColor(resized_im, resized_im_gray, CV_BGR2GRAY);

        // cout << "w, h: " << resized_im_gray.cols << ", " << resized_im_gray.rows << endl;
        vector<float> descriptors;  // HOG描述子向量
        hog.compute(resized_im_gray, descriptors, Size(4, 4));
        feats.insert(
            feats.end(),
            std::make_move_iterator(descriptors.begin()),
            std::make_move_iterator(descriptors.end())
        );
        // cout << "descriptors size" << descriptors.size() << endl;
        // cout << "feats size" << feats.size() << endl;
    }
    return feats;  // 返回feats
}

// 用标注的图像训练SVM分类器
Ptr<SVM> train_svm_classifier(std::string train_imlist, std::string train_imdir)
{
    ifstream ifs(train_imlist);
    string im_name;
    int label;

    Mat resized_im;

    vector<vector<float> > all_feats;
    vector<int> all_labels;

    while (ifs >> im_name >> label)
    {
        cout << im_name << " " << label << endl;
        cout << train_imdir + "/" + im_name << endl;
        Mat im_one = imread(train_imdir + "/" + im_name, 1);
        all_labels.push_back(label);

        cv::resize(im_one, resized_im, Size(28, 28));
        cv::cvtColor(resized_im, resized_im, COLOR_BGR2HSV);

        vector<float> feats = hist_feature(resized_im);

        all_feats.push_back(feats);
    }

    int feats_len = 30;
    if (use_hog)
    {
        feats_len = 471;
    }
    Mat trainingDataMat(all_feats.size(), feats_len, CV_32FC1);
    Mat labelsMat(all_feats.size(), 1, CV_32S);
    for (int i = 0; i < all_feats.size(); i++) {
        for (int t = 0; t < feats_len; t++) {
            float tmp = all_feats[i][t]; // !!!!!!!!!!!!!!!
            float* pf = trainingDataMat.ptr<float>(i, t);
            *pf = tmp;
        }

        int gt = all_labels[i]; // !!!!!!!!!!!!!!!
        int* pi = labelsMat.ptr<int>(i);
        *pi = gt;
    }

    Ptr<SVM> svm = SVM::create();
    svm->setType(SVM::C_SVC);
    svm->setKernel(SVM::LINEAR);
    svm->setTermCriteria(TermCriteria(TermCriteria::MAX_ITER, 100, 1e-6));
    //! [init]
    //! [train]
    svm->train(trainingDataMat, ROW_SAMPLE, labelsMat);
    // svm->save("trained_svm.xml");

    return svm;
}

void switchCallback(const std_msgs::Bool::ConstPtr& msg)
{
    is_suspanded = !(bool)msg->data;
    // cout << is_suspanded << endl;
}

//! ROS subscriber and publisher.
image_transport::Subscriber imageSubscriber_;
#ifdef ELLIPSE_PUB
image_transport::Publisher ellipse_pub;
#endif
ros::Publisher pose_pub;
// cv::ml::SVM
Ptr<SVM> svm;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ellipse_det");
    ros::NodeHandle nh("~");
    image_transport::ImageTransport it(nh);

    // 发布调试消息
    msg_node_name = "/prometheus/message/ellipse_det";
    message_pub = nh.advertise<prometheus_msgs::Message>(msg_node_name, 10);

    std::string camera_topic, camera_info;
    if (nh.getParam("camera_topic", camera_topic)) {
        if (local_print)
            ROS_INFO("camera_topic is %s", camera_topic.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_topic is" + camera_topic);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_topic");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_topic");
        camera_topic = "/prometheus/camera/rgb/image_raw";
    }

    if (nh.getParam("camera_info", camera_info)) {
        if (local_print)
            ROS_INFO("camera_info is %s", camera_info.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "camera_info is" + camera_info);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter camera_info");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter camera_info");
        camera_info = "camera_param.yaml";
    }

    // With Training
    bool wt = false;
    if (nh.getParam("with_training", wt)) {
        if (local_print)
            ROS_INFO("with_training is %d", wt);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter with_training");
        wt = false;
    }

    std::string train_imlist, train_imdir;
    if (nh.getParam("train_imlist", train_imlist)) {
        if (local_print)
            ROS_INFO("train_imlist is %s", train_imlist.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "train_imlist is" + train_imlist);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter train_imlist");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter train_imlist");
        train_imlist = "ellipse/labeled_img_class.txt";
    }

    if (nh.getParam("train_imdir", train_imdir)) {
        if (local_print)
            ROS_INFO("train_imdir is %s", train_imdir.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "train_imdir is" + train_imdir);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter train_imdir");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter train_imdir");
        train_imdir = "ellipse/images_from_camera/";
    }

    // Saving ellipse center
    bool saving_center = false;
    if (nh.getParam("saving_center", saving_center)) {
        if (local_print)
            ROS_INFO("saving_center is %d", saving_center);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter saving_center");
        saving_center = false;
    }

    std::string saving_path;
    if (nh.getParam("saving_path", saving_path)) {
        if (local_print)
            ROS_INFO("saving_path is %s", saving_path.c_str());
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "saving_path is" + saving_path);
    } else {
        if (local_print)
            ROS_WARN("didn't find parameter saving_path");
        if (message_print)
            pub_message(message_pub, prometheus_msgs::Message::WARN, msg_node_name, "didn't find parameter saving_path");
        saving_path = "ellipse/images_from_camera/";
    }

    bool switch_state = is_suspanded;
    // 接收开关话题
    switch_subscriber = nh.subscribe("/prometheus/switch/ellipse_det", 10, switchCallback);

    // for (int i=0; i<argc; i++)
    // {
    //     if (strcmp("wt", argv[i]) == 0) { wt = true; break; }
    // }

    if (wt)
        svm = train_svm_classifier(train_imlist, train_imdir);

    ros::Rate loop_rate(30);

    std::string ros_path = ros::package::getPath("prometheus_detection");
    if (local_print)
        cout << "DETECTION_PATH: " << ros_path << endl;
    if (message_print)
        pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "DETECTION_PATH: " + ros_path);

    //读取参数文档camera_param.yaml中的参数值；
    YAML::Node camera_config = YAML::LoadFile(camera_info);
    //相机内部参数
    double fx = camera_config["fx"].as<double>();
    double fy = camera_config["fy"].as<double>();
    double cx = camera_config["x0"].as<double>();
    double cy = camera_config["y0"].as<double>();
    //相机畸变系数
    double k1 = camera_config["k1"].as<double>();
    double k2 = camera_config["k2"].as<double>();
    double p1 = camera_config["p1"].as<double>();
    double p2 = camera_config["p2"].as<double>();
    double k3 = camera_config["k3"].as<double>();

    double ellipse_det_r = camera_config["ellipse_det_r"].as<double>();

    // 接收图像的话题
    imageSubscriber_ = it.subscribe(camera_topic, 1, cameraCallback);
#ifdef ELLIPSE_PUB
    // 发布椭圆检测结果的话题
    ellipse_pub = it.advertise("/prometheus/camera/rgb/image_ellipse_det", 1);
#endif
    // 椭圆检测结果，xyz
    pose_pub = nh.advertise<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/ellipse_det", 1);

    sensor_msgs::ImagePtr msg_ellipse;

    int saving_ellipse_cnt = 0;
    // const auto wait_duration = std::chrono::milliseconds(2000);
    ros::Rate loopRate_1Hz(1);
    while (ros::ok())
    {
        while (!getImageStatus() && ros::ok())
        {
            if (local_print)
                cout << "Waiting for image." << endl;
            if (message_print)
                pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Waiting for image.");
            // std::this_thread::sleep_for(wait_duration);
            ros::spinOnce();
            loopRate_1Hz.sleep();
        }

        if (switch_state != is_suspanded)
        {
            switch_state = is_suspanded;
            if (!is_suspanded)
            {
                if (local_print)
                    cout << "Start Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Start Detection.");
            }
            else
            {
                if (local_print)
                    cout << "Stop Detection." << endl;
                if (message_print)
                    pub_message(message_pub, prometheus_msgs::Message::NORMAL, msg_node_name, "Stop Detection.");
            }
        }

        if (!is_suspanded)
        {

#ifdef ELLIPSE_DET
        Mat ellipse_show, frame;
        std::vector<Ellipse> ells, ells_copy;
        {
            boost::unique_lock<boost::shared_mutex> lockImageCallback(mutex_image_callback);
            frame = cam_image_copy.clone();
        }
        ellipse_det(frame, ellipse_show, ells);

        static double last_x(0), last_y(0), last_z(0);
        bool deted = false;

        if (saving_center)
        {
            // 遍历每个检测到的椭圆
            for (int i=0; i<ells.size(); i++)
            {
                Ellipse e = ells[i];

                int xc = ells[i].xc_;
                int yc = ells[i].yc_;
                int width = (ells[i].a_ + ells[i].b_)*0.65;

                // 提取椭圆中心区域
                Rect output_r(xc - width / 2, yc - width / 2, width, width);
                if (output_r.x < 0 || output_r.y < 0 ||
                    output_r.x + output_r.width >= frame.cols ||
                    output_r.y + output_r.height >= frame.rows)
                    continue;
                Mat center_det = frame(output_r);

                // 提取中心区域的颜色直方图
                cv::resize(center_det, center_det, cv::Size(28,28));

                char output_name[256];
                sprintf(output_name, "%s/%010d.jpg", saving_path.c_str(), saving_ellipse_cnt++);
                cv::imwrite(output_name, center_det);
            }
        }
        else if (wt)
        {
            // 遍历每个检测到的椭圆
            for (int i=0; i<ells.size(); i++)
            {
                Ellipse e = ells[i];

                int xc = ells[i].xc_;
                int yc = ells[i].yc_;
                int width = (ells[i].a_ + ells[i].b_)*0.65;

                // 提取椭圆中心区域
                Rect output_r(xc - width / 2, yc - width / 2, width, width);
                if (output_r.x < 0 || output_r.y < 0 ||
                    output_r.x + output_r.width >= frame.cols ||
                    output_r.y + output_r.height >= frame.rows)
                    continue;
                Mat center_det = frame(output_r);

                // 提取中心区域的颜色直方图
                cv::resize(center_det, center_det, cv::Size(28,28));
                cv::cvtColor(center_det, center_det, COLOR_BGR2HSV);
                std::vector<float> feat = hist_feature(center_det);

                int feats_len = 30;
                if (use_hog)
                {
                    feats_len = 471;
                }
                cv::Mat predictDataMat(1, feats_len, CV_32F);
                for (int i=0; i<feats_len; i++)
                {
                    predictDataMat.at<float>(i) = feat[i];
                }
                // 利用SVM分类器判断是否是需要的检测结果
                float pred = svm->predict(predictDataMat);

                // 如果是的，则将结果发布出去
                if (pred == 1)
                {
                    deted = true;

                    float theta_x = atan((e.xc_ - cx) / fx);  //315.06 calibration
                    float theta_y = atan((e.yc_ - cy) / fy);  //241.27 calibration

                    float depth = ellipse_det_r*fx/e.b_; // shendu

                    float real_x = depth*tan(theta_x);
                    float real_y = depth*tan(theta_y);

                    // geometry_msgs::Pose pose_now;
                    prometheus_msgs::DetectionInfo pose_now;
                    pose_now.header.stamp = ros::Time::now();
                    // pose_now.orientation.w = 1;
                    // pose_now.position.x = depth;
                    // pose_now.position.y = real_x;
                    // pose_now.position.z = real_y;
                    pose_now.detected = true;
                    pose_now.frame = 0;
                    pose_now.position[0] = real_x;
                    pose_now.position[1] = real_y;
                    pose_now.position[2] = depth;
                    pose_pub.publish(pose_now);

                    last_x = real_x;
                    last_y = real_y;
                    last_z = depth;

                    //cout << "flag_detected: " << int(pose_now.detected) <<endl;
                    //cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<<endl;

                    ells_copy.push_back(e);
                }
            }
        }
        else if (ells.size() > 0)
        {
            Ellipse e = ells[0];
            deted = true;

            float theta_x = atan((e.xc_ - cx) / fx);  //315.06 calibration
            float theta_y = atan((e.yc_ - cy) / fy);  //241.27 calibration

            float depth = ellipse_det_r*fx/e.b_; // shendu

            float real_x = depth*tan(theta_x);
            float real_y = depth*tan(theta_y);

            // geometry_msgs::Pose pose_now;
            prometheus_msgs::DetectionInfo pose_now;
            pose_now.header.stamp = ros::Time::now();
            // pose_now.orientation.w = 1;
            // pose_now.position.x = depth;
            // pose_now.position.y = real_x;
            // pose_now.position.z = real_y;
            pose_now.detected = true;
            pose_now.frame = 0;
            pose_now.position[0] = real_x;
            pose_now.position[1] = real_y;
            pose_now.position[2] = depth;
            pose_pub.publish(pose_now);

            last_x = real_x;
            last_y = real_y;
            last_z = depth;

            //cout << "flag_detected: " << int(pose_now.detected) <<endl;
            //cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<<endl;

            ells_copy.push_back(e);
        }
        if (!deted)
        {
            // 如果没检测到，则发布上次的检测结果，并用标志orientation.w = 0告知未检测到
            // geometry_msgs::Pose pose_now;
            prometheus_msgs::DetectionInfo pose_now;
            pose_now.header.stamp = ros::Time::now();
            // pose_now.orientation.w = 0;
            // pose_now.position.x = last_x;
            // pose_now.position.y = last_y;
            // pose_now.position.z = last_z;
            pose_now.detected = false;
            pose_now.frame = 0;
            pose_now.position[0] = last_x;
            pose_now.position[1] = last_y;
            pose_now.position[2] = last_z;
            pose_pub.publish(pose_now);

            //cout << "flag_detected: " << int(pose_now.detected) <<endl;
            //cout << "pos_target: [X Y Z] : " << " " << pose_now.position[0] << " [m] "<< pose_now.position[1] <<" [m] "<< pose_now.position[2] <<" [m] "<<endl;

        }
#ifdef ELLIPSE_PUB
        // 发布椭圆检测的可视化结果，用于测试
        Mat img_copy;
        frame.copyTo(img_copy);
        ellipse_detector.DrawDetectedEllipses(img_copy, ells_copy);
        msg_ellipse = cv_bridge::CvImage(std_msgs::Header(), "bgr8", img_copy).toImageMsg();
        ellipse_pub.publish(msg_ellipse);
#endif
#endif
        }

        ros::spinOnce();
        loop_rate.sleep();
    }
}


/*********************************************************************************
 *  OKVIS - Open Keyframe-based Visual-Inertial SLAM
 *  Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
 *
 *  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 Autonomous Systems Lab / ETH Zurich 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 OWNER 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.
 *
 *  Created on: Jan 7, 2014
 *      Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
 *********************************************************************************/

/**
 * @file ode.hpp
 * @brief File for ODE integration functionality.
 * @author Stefan Leutenegger
 */

#ifndef INCLUDE_OKVIS_CERES_ODE_ODE_HPP_
#define INCLUDE_OKVIS_CERES_ODE_ODE_HPP_

#include <Eigen/Core>
#include <okvis/kinematics/Transformation.hpp>
#include <okvis/kinematics/operators.hpp>
#include <okvis/FrameTypedefs.hpp>
#include <okvis/Measurements.hpp>
#include <okvis/Variables.hpp>
#include <okvis/assert_macros.hpp>

/// \brief okvis Main namespace of this package.
namespace okvis {
/// \brief ceres Namespace for ceres-related functionality implemented in okvis.
namespace ceres {
/// \brief ode Namespace for functionality related to ODE integration implemented in okvis.
namespace ode {

// to make things a bit faster than using angle-axis conversion:
inline double sinc(double x) {
  if (fabs(x) > 1e-6) {
   return sin(x) / x;
   } else{
    static const double c_2 = 1.0 / 6.0;
    static const double c_4 = 1.0 / 120.0;
    static const double c_6 = 1.0 / 5040.0;
    const double x_2 = x * x;
    const double x_4 = x_2 * x_2;
    const double x_6 = x_2 * x_2 * x_2;
    return 1.0 - c_2 * x_2 + c_4 * x_4 - c_6 * x_6;
  }
}

// world-centric velocities
inline void evaluateContinuousTimeOde(const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
                                          const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
                                          const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
                                          Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
                                          Eigen::Matrix<double, 15, 15>* F_c_ptr = 0){
  // "true" rates and accelerations
  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
  const Eigen::Vector3d acc_S = acc - sb.tail<3>();

  // nonlinear states
  // start with the pose
  p_WS_W_dot = sb.head<3>();

  // now the quaternion
  Eigen::Vector4d dq;
  q_WS_dot.head<3>() = 0.5 * omega_S;
  q_WS_dot[3] = 0.0;
  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();

  // the rest is straightforward
  // consider Earth's radius. Model the Earth as a sphere, since we neither
  // know the position nor yaw (except if coupled with GPS and magnetometer).
  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009); // vector to Earth center
  sb_dot.head<3>() = (C_WS * acc_S + g * G.normalized()); // s
  // biases
  sb_dot.tail<6>().setZero();

  // linearized system:
  if (F_c_ptr) {
    F_c_ptr->setZero();
    F_c_ptr->block<3, 3>(0, 6) += Eigen::Matrix3d::Identity();
    F_c_ptr->block<3, 3>(3, 9) -= C_WS;
    F_c_ptr->block<3, 3>(6, 3) -= okvis::kinematics::crossMx(C_WS * acc_S);
    F_c_ptr->block<3, 3>(6, 12) -= C_WS;
  }
}

/*/ robo-centric velocities
__inline__ void evaluateContinuousTimeOde(
    const Eigen::Vector3d& gyr, const Eigen::Vector3d& acc, double g,
    const Eigen::Vector3d& p_WS_W, const Eigen::Quaterniond& q_WS,
    const okvis::SpeedAndBias& sb, Eigen::Vector3d& p_WS_W_dot,
    Eigen::Vector4d& q_WS_dot, okvis::SpeedAndBias& sb_dot,
    Eigen::Matrix<double, 15, 15>* F_c_ptr = 0) {

  // "true" rates and accelerations
  const Eigen::Vector3d omega_S = gyr - sb.segment<3>(3);
  const Eigen::Vector3d acc_S = acc - sb.tail<3>();

  // rotation matrix
  Eigen::Matrix3d C_WS = q_WS.toRotationMatrix();
  Eigen::Matrix3d C_SW = C_WS.transpose();

  // nonlinear states
  // start with the pose
  p_WS_W_dot = C_WS*sb.head<3>();

  // now the quaternion
  Eigen::Vector4d dq;
  q_WS_dot.head<3>() = 0.5 * omega_S;
  q_WS_dot[3] = 0.0;

  // the rest is straightforward
  // consider Earth's radius. Model the Earth as a sphere, since we neither
  // know the position nor yaw (except if coupled with GPS and magnetometer).
  Eigen::Vector3d G = -p_WS_W - Eigen::Vector3d(0, 0, 6371009);  // vector to Earth center
  Eigen::Vector3d g_W = g * G.normalized();
  sb_dot.head<3>() = acc_S - okvis::kinematics::crossMx(omega_S)*sb.head<3>() + C_SW * g_W;
  // biases
  sb_dot.tail<6>().setZero();
  //sb_dot.tail<3>()=-sb.tail<3>()/360.0;

  // linearized system:
  if (F_c_ptr) {
    F_c_ptr->setZero();
    F_c_ptr->block<3, 3>(0, 3)  += -okvis::kinematics::crossMx(C_WS * sb.head<3>());
    F_c_ptr->block<3, 3>(0, 6)  += C_WS;
    F_c_ptr->block<3, 3>(3, 9)  += -C_WS;
    F_c_ptr->block<3, 3>(6, 3)  += C_SW * okvis::kinematics::crossMx(g_W);
    F_c_ptr->block<3, 3>(6, 6)  += -okvis::kinematics::crossMx(omega_S);
    F_c_ptr->block<3, 3>(6, 9)  += -okvis::kinematics::crossMx(sb.head<3>());
    F_c_ptr->block<3, 3>(6, 12) += -Eigen::Matrix3d::Identity();
    //F_c_ptr->block<3, 3>(12, 12) = - 1.0/360.0*Eigen::Matrix3d::Identity();
  }
}*/

inline void integrateOneStep_RungeKutta(
    const Eigen::Vector3d& gyr_0, const Eigen::Vector3d& acc_0,
    const Eigen::Vector3d& gyr_1, const Eigen::Vector3d& acc_1, double g,
    double sigma_g_c, double sigma_a_c, double sigma_gw_c, double sigma_aw_c,
    double dt, Eigen::Vector3d& p_WS_W, Eigen::Quaterniond& q_WS,
    okvis::SpeedAndBias& sb, Eigen::Matrix<double, 15, 15>* P_ptr = 0,
    Eigen::Matrix<double, 15, 15>* F_tot_ptr = 0) {

  Eigen::Vector3d k1_p_WS_W_dot;
  Eigen::Vector4d k1_q_WS_dot;
  okvis::SpeedAndBias k1_sb_dot;
  Eigen::Matrix<double, 15, 15> k1_F_c;
  evaluateContinuousTimeOde(gyr_0, acc_0, g, p_WS_W, q_WS, sb, k1_p_WS_W_dot,
                            k1_q_WS_dot, k1_sb_dot, &k1_F_c);

  Eigen::Vector3d p_WS_W1 = p_WS_W;
  Eigen::Quaterniond q_WS1 = q_WS;
  okvis::SpeedAndBias sb1 = sb;
  // state propagation:
  p_WS_W1 += k1_p_WS_W_dot * 0.5 * dt;
  Eigen::Quaterniond dq;
  double theta_half = k1_q_WS_dot.head<3>().norm() * 0.5 * dt;
  double sinc_theta_half = sinc(theta_half);
  double cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k1_q_WS_dot.head<3>() * 0.5 * dt;
  dq.w() = cos_theta_half;
  q_WS1 = q_WS * dq;
  sb1 += k1_sb_dot * 0.5 * dt;

  Eigen::Vector3d k2_p_WS_W_dot;
  Eigen::Vector4d k2_q_WS_dot;
  okvis::SpeedAndBias k2_sb_dot;
  Eigen::Matrix<double, 15, 15> k2_F_c;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W1, q_WS1, sb1, k2_p_WS_W_dot, k2_q_WS_dot,
                            k2_sb_dot, &k2_F_c);

  Eigen::Vector3d p_WS_W2 = p_WS_W;
  Eigen::Quaterniond q_WS2 = q_WS;
  okvis::SpeedAndBias sb2 = sb;
  // state propagation:
  p_WS_W2 += k2_p_WS_W_dot * dt;
  theta_half = k2_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k2_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  //std::cout<<dq.transpose()<<std::endl;
  q_WS2 = q_WS2 * dq;
  sb2 += k1_sb_dot * dt;

  Eigen::Vector3d k3_p_WS_W_dot;
  Eigen::Vector4d k3_q_WS_dot;
  okvis::SpeedAndBias k3_sb_dot;
  Eigen::Matrix<double, 15, 15> k3_F_c;
  evaluateContinuousTimeOde(0.5 * (gyr_0 + gyr_1), 0.5 * (acc_0 + acc_1), g,
                            p_WS_W2, q_WS2, sb2, k3_p_WS_W_dot, k3_q_WS_dot,
                            k3_sb_dot, &k3_F_c);

  Eigen::Vector3d p_WS_W3 = p_WS_W;
  Eigen::Quaterniond q_WS3 = q_WS;
  okvis::SpeedAndBias sb3 = sb;
  // state propagation:
  p_WS_W3 += k3_p_WS_W_dot * dt;
  theta_half = k3_q_WS_dot.head<3>().norm() * dt;
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * k3_q_WS_dot.head<3>() * dt;
  dq.w() = cos_theta_half;
  //std::cout<<dq.transpose()<<std::endl;
  q_WS3 = q_WS3 * dq;
  sb3 += k3_sb_dot * dt;

  Eigen::Vector3d k4_p_WS_W_dot;
  Eigen::Vector4d k4_q_WS_dot;
  okvis::SpeedAndBias k4_sb_dot;
  Eigen::Matrix<double, 15, 15> k4_F_c;
  evaluateContinuousTimeOde(gyr_1, acc_1, g, p_WS_W3, q_WS3, sb3, k4_p_WS_W_dot,
                            k4_q_WS_dot, k4_sb_dot, &k4_F_c);

  // now assemble
  p_WS_W +=
      (k1_p_WS_W_dot + 2 * (k2_p_WS_W_dot + k3_p_WS_W_dot) + k4_p_WS_W_dot) * dt
          / 6.0;
  Eigen::Vector3d theta_half_vec = (k1_q_WS_dot.head<3>()
      + 2 * (k2_q_WS_dot.head<3>() + k3_q_WS_dot.head<3>())
      + k4_q_WS_dot.head<3>()) * dt / 6.0;
  theta_half = theta_half_vec.norm();
  sinc_theta_half = sinc(theta_half);
  cos_theta_half = cos(theta_half);
  dq.vec() = sinc_theta_half * theta_half_vec;
  dq.w() = cos_theta_half;
  q_WS = q_WS * dq;
  sb += (k1_sb_dot + 2 * (k2_sb_dot + k3_sb_dot) + k4_sb_dot) * dt / 6.0;

  q_WS.normalize(); // do not accumulate errors!

  if (F_tot_ptr) {
    // compute state transition matrix
    Eigen::Matrix<double, 15, 15>& F_tot = *F_tot_ptr;
    const Eigen::Matrix<double, 15, 15>& J1 = k1_F_c;
    const Eigen::Matrix<double, 15, 15> J2=k2_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J1);
    const Eigen::Matrix<double, 15, 15> J3=k3_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+0.5*dt*J2);
    const Eigen::Matrix<double, 15, 15> J4=k4_F_c*(Eigen::Matrix<double, 15, 15>::Identity()+dt*J3);
    Eigen::Matrix<double, 15, 15> F = Eigen::Matrix<double, 15, 15>::Identity()
        + dt * (J1+2*(J2+J3)+J4) / 6.0;
        //+ dt * J1;
    //std::cout<<F<<std::endl;
    F_tot = (F * F_tot).eval();

    if (P_ptr) {
      Eigen::Matrix<double, 15, 15>& cov = *P_ptr;
      cov = F * (cov * F.transpose()).eval();

      // add process noise
      const double Q_g = sigma_g_c * sigma_g_c * dt;
      const double Q_a = sigma_a_c * sigma_a_c * dt;
      const double Q_gw = sigma_gw_c * sigma_gw_c * dt;
      const double Q_aw = sigma_aw_c * sigma_aw_c * dt;
      cov(3, 3) += Q_g;
      cov(4, 4) += Q_g;
      cov(5, 5) += Q_g;
      cov(6, 6) += Q_a;
      cov(7, 7) += Q_a;
      cov(8, 8) += Q_a;
      cov(9, 9) += Q_gw;
      cov(10, 10) += Q_gw;
      cov(11, 11) += Q_gw;
      cov(12, 12) += Q_aw;
      cov(13, 13) += Q_aw;
      cov(14, 14) += Q_aw;

      // force symmetric - TODO: is this really needed here?
      // cov = 0.5 * cov + 0.5 * cov.transpose().eval();
    }
  }

}

}

}  // namespace ceres
}  // namespace okvis

#endif /* INCLUDE_OKVIS_CERES_ODE_ODE_HPP_ */