//
//  lung_model_discrete_branching.hpp
//
//
//  Created by Carl Whitfield on 09/03/2017.
//
//
// Checked and commented 16/05/2018
#if defined(_WIN32) || defined(_WIN64)
#define _USE_MATH_DEFINES
#include <windows.h>
#endif

#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <algorithm>
#include <unordered_map>
#include <string.h>
#include <math.h>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <vector>
#include <chrono>
#include <ctime>
#include <Eigen/Eigen/Dense>
#include <Eigen/Eigen/LU>
#include <Eigen/Eigen/Sparse>
#include <Eigen/Eigen/SparseLU>
#include <Eigen/Eigen/IterativeLinearSolvers>

//Options for tree config
#define LINEARPERT 1       //option TREE l
#define NOPERT 2    //option TREE n

//options for BCs
#define BAG 101            //option BC b
#define NOFLUX 102         //option BC n
#define SINK 103

//options for Defects
#define BLOCKAGE 201
#define AREA 202
#define LENGTH 203
#define ELASTICITY 204
#define BAG_RESISTANCE 205

//options for upwind scheme
#define CENTRAL_UPWIND 301
#define FIRST_ORDER_UPWIND 302      //First order upwind interpolation

//options for flow type
#define POISEUILLE 401   //--Poiseuille Flow Assumption--//
#define PEDLEY 402       //--See Pedley 1970--//

//option for elastic response function
#define LINEAR_RESP 501
#define NONLINEAR_RESP 502

//options for pressure profile
#define STEP_FUNCTION 601
#define SIGMOIDAL 602
#define SINUSOIDAL 603
#define LINEAR 604

//options for parameters to use
#define GEOMETRIC    701
#define HOMOGENISED  702
#define WEIBEL  703
#define LOBE_GEOMETRIC 704
#define ALT_LOBE_GEOMETRIC 705

//options for linear solve
#define ITERATIVE    801
#define DIRECT       802

//options for initialisation
#define EMPTY 901
#define FULL 902

//options for perturbations
#define PERT_AREA 1001
#define PERT_LENGTH 1002
#define PERT_ELASTICITY 1003

//options for volume/pressure input
#define VOLUME 1101
#define PRESSURE 1102

//options for dispersion
#define TAYLOR 1201
#define SCHERER 1202
#define NONE 1203

//Options for concentration file printout
#define VTK 1301
#define CSV 1302
#define VTK_CSV 1303

//Options for leak types
#define INSPIRATORY_LEAK 1401
#define EXPIRATORY_LEAK 1402
#define INSPIRATORY_AND_EXPIRATORY_LEAK 1403

/********************************************/
//Unit conversions
#define cmH20_Pa 100    //pressure conv from cm^2H20 to Pa
#define L_m3 0.001      //volume from litres to m^3
#define cm_m 0.01       //cm to metres
#define PEDLEY_C 1.85   //pedley constant

//Default values of parameters
#define NGENTOT 23              //Number of generations total
#define NGENDEF 15              //Number of generations before model terminates
#define VISCDEF 1.93E-07            //Air Viscosity (cmH20 s) (actual, 1.93E-07 at 37C) -- engineering toolbox
#define DENSITYDEF 1.138        //Air Density (kg m^-3) 1.138 at 37C -- engineering toolbox
#define EDEF 5.0              //Elastance (cmH20 L^-1)
#define RBDEF 0.2              //bag resistance
#define RMDEF 0.6              //mouth resistance
#define DIFFUSIONDEF 0.105       //Diffusion constant (cm2/s) (Helium is approx 0.71, Oxygen 0.19)
#define MAXPECDEF 10            //Max element peclet number
#define K0DEF 0                 //Uptake term
#define LDDEF 3.0                //Length to diameter ratio
#define LD2DEF 2.3                //Length to diameter ratio
#define VFRCDEF 3.0            //Functional residual capacity in L
#define VDDEF 0.12             //Conducting airway dead space
#define VDMDEF 0.05            //Volume of mouth cavity
#define VDUCTDEF 0.20            //fraction of acinus consisting of duct at FRC
#define TINDEF 2.5              //Duration of first breath in (secs)
#define P0DEF 1.0           //Max pressure applied at distal end (cmH20)
#define VTDEF 1.0
#define RUNDEF 40               //Total Simulation Time (number of breathing cycles)
#define LAMBDADEF 0.794          //Geometric parameter
#define LAMBDA2DEF 0.92         //Geom parameter acinus
#define STDEF 0               //Time source is left at opening (in number of breaths)
#define DEFPERTSIZE 0.1       //perturbation size
#define DEFPRINTERVAL 10       //time between prints (in terms of length of first breath)
#define DEFMINGENSIZE 1          //minimum number of points in a generation
#define MINGENSIZE_ACIN 4
#define DEFSTARTDELAY 10         //number of breaths to simulate before starting transport simulation
#define DFACTOR 0.2   //increase in eff area in acinus (default)

//Tolerances precisions etc.
#define PTOL 1E-12  //precision for stree calc
#define TOL 1E-12   //precision for ltree calc
#define OUTPUT_PRECISION_CONC 12   //precision for conc file output
#define OUTPUT_PRECISION_FLUX 12   //precision for flux file output
#define DEF_TS 0.01               //defined timestep
#define DEF_DX 0.025               //defined max spacestep

//define stree numbers for lobar models -- trachea is 0
#define BR 1    //right branch
#define BL 2    //left branch
#define BRML 3   //right middle/lower
#define BRU 4    //right upper
#define BLL 5    //left lower
#define BLU 6    //left upper
#define BRM 7    //right middle
#define BRL 8    //right lower
#define BRL1 9   //right lower major
#define BRL2 10  //right lower minor
#define BLL1 11  //left lower major
#define BLL2 12  //left lower minor

using namespace std;

class Defect    //class for storing ariway/acinar defect information
{
public:
	unsigned jn;   //generation number
	unsigned long kstart, kend;   //branch number start and end
	unsigned type;       //defect type
	double mag;      //magnitude of defect
	Defect()
	{
		jn = 0;
		kstart = 0;
		kend = 0;
		type = 0;   //should return error if unchanged
		mag = 0;
	}
	inline bool operator==(Defect d)   //equality operator
	{
		if (type == BLOCKAGE) return(type == d.type);  //defects are equal if both blocked
		else return (type == d.type && ((int)(1000000 * mag)) == ((int)(1000000 * d.mag)));  //defects equal if same type and magnitude
	}
};

class Leak    //class for storing leak details - for simulating inspiratory and expiratory leaks at mouth
{
public:
	bool exists;
	double start, end, size;    //start time (breaths), end time (breaths), size (fraction)
	unsigned type;             //Insp only, exp only or both
	Leak()
	{
		exists = false;
		start = 0;
		end = 100000;
		size = 0;
		type = INSPIRATORY_AND_EXPIRATORY_LEAK;
	}
};

class Conversions    //class for storing unit conversions
{
public:
	double LL_to_cm;    //lung length (sim units) to cm
	double P_to_cmH20;   //pressure (sim units) to cmH20
	double t_to_s;      //time (sim units) to seconds
	double V_to_Litres;    //volume (sim units) to litres
};

class Options    //for storing input options
{
public:
	/***Determined by input file***/

	/**Overarching options**/
	unsigned TreeOp;         //Tree format
	unsigned BcOp;           //Boundary condition
	unsigned UpwindOp;       //Upwind interpolation scheme
	unsigned PressOp;        //Function for pressure profile
	unsigned RespFunc;       //Function for elastic resistance
	unsigned FlowType;       //Poiseuille or turbulent
	unsigned ShapeOp;        //Determines the shape and size of the tubes
	unsigned SolverOp;
	unsigned InitOp;        //Determines c initialisation (empty c=0, full c=1)
	unsigned InputOp;         //Determines whether volume or pressure is inputted
	unsigned TaylorDisp;    //True if Taylor Dispersion is considered
	unsigned OutputOp;   //output format option
	bool output_perts;  //whether to output pert files or not
	bool output_lung_volumes;
	Leak N2leak;

	/**Parameters**/
	unsigned Ngen;           //Number of generations before alveoli
	unsigned Ngen2;          //Total number of generations in tree
	unsigned MinGenSize, MinAcinGenSize;
	double dt, dxmax;                //timestep
	double Viscosity;        //Air viscosity
	double Density;          //Air density
	double Diffusion;      //Diffusion constant
	double k0;          //bare uptake rate
	double LDratio, LDratio2;          //L0 length of trachea
	double VFRC;        //total lung resting volume
	double Tin;         //period first inhalation
	double P0;          //Pressure on first inhalation (always negative)
	double VT, VD, VDM, Vductvol;          //Tidal volume
	double E, Rb, Rmouth, V0;           //Elastance of bag, (Total) Resistance of bag, volume of bag
	double RunTime;     //Total run time of simulation in breaths
	double PrerunTime;
	double lambda, lambda2;      //Geometric ratio of tree generations
	double StimTime;    //Time stimulation kept at inlet
	double printerval;  //Time between data outputs
	double MaxPeclet;   //Maximum element peclet number allowed
	double AcinAreaFactor;
	vector<vector<Defect>> def;     //Vector of large magnitude perturbations to symmetric tree
	//vector<Defect> pert;      //Vector of small magnitude perturbations to consider individually
	//vector<vector<char>> *tree_pert;
	unordered_map<unsigned long, vector<Defect>> def_map;   //map from position in tree to defects at that location
	unsigned long kmtot;   //total number of finite volumes

	/***Determined in code***/
	chrono::time_point<std::chrono::system_clock> tstart, tend;
	unsigned Ntrees;               //Number of subtrees - identifier for this run
	string SimID;
	double Peclet;        //Peclet number for branch 0
	/***Constructor - Default Values***/
	Options()
	{
		TreeOp = NOPERT;
		BcOp = NOFLUX;
		UpwindOp = FIRST_ORDER_UPWIND;
		PressOp = SINUSOIDAL;
		RespFunc = LINEAR_RESP;
		FlowType = POISEUILLE;
		ShapeOp = GEOMETRIC;
		SolverOp = ITERATIVE;
		InitOp = FULL;
		InputOp = VOLUME;
		TaylorDisp = SCHERER;
		output_perts = true;
		output_lung_volumes = true;
		OutputOp = CSV;
		Ngen = NGENDEF;
		Ngen2 = NGENTOT;
		MinGenSize = DEFMINGENSIZE;
		MinAcinGenSize = MINGENSIZE_ACIN;
		Viscosity = VISCDEF;
		Density = DENSITYDEF;
		E = EDEF;
		Rb = RBDEF;
		Rmouth = RMDEF;
		Diffusion = DIFFUSIONDEF;
		MaxPeclet = MAXPECDEF;
		k0 = K0DEF;
		LDratio = LDDEF;
		LDratio2 = LD2DEF;
		VFRC = VFRCDEF;
		VD = VDDEF;
		VDM = VDMDEF;
		Vductvol = VDUCTDEF;
		Tin = TINDEF;
		P0 = P0DEF;
		VT = VTDEF;
		RunTime = RUNDEF;
		PrerunTime = DEFSTARTDELAY;
		lambda = LAMBDADEF;
		lambda2 = LAMBDA2DEF;
		StimTime = STDEF;
		printerval = DEFPRINTERVAL;
		dt = DEF_TS;
		dxmax = DEF_DX;
		AcinAreaFactor = DFACTOR;
		def.resize(13);
	}
	//functions to return text for options
	string read_tree_option();
	string read_bc_option();
	string read_flow_option();
	string read_resp_option();
	string read_pressure_option();
	string read_shape_option();
	string read_upwind_option();
	string read_perttype();
	string read_taylor_option();
	string read_solver_option();
	string read_init_option();
	string read_input_option();
	string read_output_option();
	string read_leak_type();
};

struct node     //stores info at a single finite volume node
{
	double c, cold, Aold, Anew, DA, x, uc, y0;        //values of conc, previous conc, totarea, position, vel at element centre
	double ul, ur, al, ar, Dl, Dr;                             //velocity, diffusion and area on left and right of element
	unsigned long km;                                       //corresponding entrance in matrix
	vector<unsigned> iup[3], jup[3], kup[3];        //stores indices for neighbours n-1, n and n+1 respectively
	vector<double> dxup[3], dcfr[3], dcfl[3], ucfrpos[3], ucflpos[3], ucfrneg[3], ucflneg[3], sr, sul;   //vector of dx at n-1, n  and n+1 and cross-sections
	double sl, sdr;   //cross section left sl and right (for node to left) sdr
	node()   //constructor
	{
		c = 0;
		cold = 0;
		Aold = 0;
		Anew = 0;
		DA = 0;
		x = 0;
		uc = 0;
		ul = 0;
		ur = 0;
		al = 0;
		ar = 0;
		Dl = 0;
		Dr = 0;
		km = 0;
		sl = 0;
		sdr = 0;
	}
	void set_quantities_zero()   //rest relevant quantities to zeros
	{
		c = 0;
		cold = 0;
		Aold = 0;
		Anew = 0;
		DA = 0;
		x = 0;
		uc = 0;
		ul = 0;
		ur = 0;
		al = 0;
		ar = 0;
		Dl = 0;
		Dr = 0;
		sl = 0;
		sdr = 0;
		km = 0;
		for (unsigned counter = 0; counter < 3; counter++)
		{
			for (unsigned counter2 = 0; counter2 < dxup[counter].size(); counter2++)
			{
				dxup[counter][counter2] = 0;
				dcfr[counter][counter2] = 0;
				dcfl[counter][counter2] = 0;
				ucfrpos[counter][counter2] = 0;
				ucflpos[counter][counter2] = 0;
				ucfrneg[counter][counter2] = 0;
				ucflneg[counter][counter2] = 0;
			}

		}
		for (unsigned counter2 = 0; counter2 < sr.size(); counter2++) sr[counter2] = 0;
		for (unsigned counter2 = 0; counter2 < sul.size(); counter2++) sul[counter2] = 0;
	}
};

struct gen    //stores a generation of finite volume nodes
{
	double dx;              //element length
	unsigned long Nb;        //No of branches in this gen
	double x0;         //start position
	vector<node> p;    //vector of nodes
	double Ap, Lp;     //defects/perturbations
	gen()    //Constructor
	{
		dx = 0;
		Nb = 0;
		x0 = 0;
	}
	void set_quantities_zero()
	{
		dx = 0;
		x0 = 0;
		Ap = 0;
		Lp = 0;
	}
};

struct subtree    //stores a subtree consisting of generations connected in series
{
	unsigned StartGen;           //generation where subtree starts
	unsigned long StartBranch;        //branch no. of tree base
	unsigned EndGen;             //generation where subtree ends
	unsigned Ncond, Ntot;         //Gen at which conducting airways terminate and alv airways terminate
	unsigned imeanpath;         //number of mean path
	double A0, L0, A1, L1;
	double Vold;            //equivalent total volume of bags at end at time t
	double Vnew;            //equivalent total volume of bags at end at time t+dt
	double Valv0, sValv0, V0, Vacinduct;           //length of all pipes in alveoli region
	double qend;
	vector<gen> gn;                 //generation
	double fluxin, fluxout; //instantantaneous flux at inlet and outlet
	double masstot, totIGvol;         //total mass in system
	double totIGvolold;         //total mass in system
	int treein;             //number of the tree feeding this one (-1 if root)
	int treeout[2];         //numbers of tree fed by this one (both -1 if base)
	double E, Ep, Rb, Rbp;              //tree elastance and bag resistance
	double dR;              //effective resistance change at start gen of tree
	double Rj;                  //effective resistance of subtree
	double Rtree;                 //resistance of this tree only
	double dVacinus;           //stores the volume change of the associated acinus (or acini)
	double z0;   //coordinate for plotting
	vector<unsigned> EndSubtrees;              //list of terminating subtrees connected to this subtree
	vector<unsigned> isub;                     //list of all subtrees
	bool blocked;     //true if blocked off, false if not
	unsigned i_stree;   //stores index of corresponding subtree in stree
	subtree()   //constructor
	{
		StartGen = 0;
		EndGen = NGENTOT;
		Vold = 0;
		Vnew = 0;
		Valv0 = 0;
		sValv0 = 0;
		Vacinduct = 0;
		L0 = 0;
		A0 = 0;
		L1 = 0;
		A1 = 0;
		V0 = 0;
		qend = 0;
		fluxin = 0;
		fluxout = 0;
		masstot = 0;
		totIGvol = 0;
		totIGvolold = 0;
		treein = -1;
		treeout[0] = -1;
		treeout[1] = -1;
		E = EDEF;
		Ep = 0;
		Rb = RBDEF;
		Rbp = 0;
		dR = 0;
		Rj = 0;
		Rtree = 0;
		z0 = 0;
		blocked = false;
		i_stree = 0;
		dVacinus = 0;
	}

	~subtree()
	{
		gn.clear();
	}
	void set_quantities_zero() //set relevant quantities to zero for perturbative model
	{
		Vold = 0;
		Vnew = 0;
		Valv0 = 0;
		sValv0 = 0;
		V0 = 0;
		qend = 0;
		fluxin = 0;
		fluxout = 0;
		masstot = 0;
		totIGvol = 0;
		totIGvolold = 0;
		EndSubtrees.clear();
		Ep = 0;
		Rbp = 0;
		dR = 0;
		Rj = 0;
	}
	void allocate_gn(unsigned size)
	{
		gn.resize(size);
	}
	void deallocate_gn()
	{
		gn.clear();
	}
	double length_function(unsigned j, Options &o);    //returns gen j length
	double area_function(double dx, unsigned j, Options &o);       //returns dx along gen j cross section
	double alveolar_density(unsigned j, unsigned k, Options &o);    //returns alveolar density function
	void subtree_resistance(void);                               //computes subtree resistance
	void tot_area_function(double Vtot);                         //distributes sac volume to cross section A
	void tot_lp_area_function(subtree &sst, double DVtot);        //same as abouve for perturbed tree
	void velocity_calc(double qend, Options &o);                //calculates flow velocity from A
	void velocity_lp_calc(subtree &sst, double qend, Options &o);          //same as above for perturbed tree
};

class Tree                  //for storing whole airway tree data
{
public:
	vector<subtree> st;
	double masstot, masstotold, fluxin;            //total mass in system
	double Ppl, Pplold;                //pleural pressure
	unsigned pert_ij[2];          //stores st and generation of perturbation
	char pert_type;               //'K', 'A' or 'L' depending on which per
	//Eigen matrices and functions fro ventialtion and transport problems
	Eigen::HouseholderQR<Eigen::MatrixXd> *AmatQR;
	Eigen::PartialPivLU<Eigen::MatrixXd> *AmatLU;
	Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::IncompleteLUT<double>> *solver_iter;
	Eigen::SparseLU<Eigen::SparseMatrix<double>> *solver_dir;
	Eigen::MatrixXd Amat, Bmat, Kmat;
	Eigen::VectorXd KVs, Vs;
	double VDfront, VDfrontold, VDfrontoldold, cmouth, cmouthold, Vlung, Vairways;  //volume of mouth dead space filled this breath
	double Vlungold;
	vector<double> ctop, Vtot;
	vector<unsigned> EndSubtrees, meanpath;
	unordered_map<unsigned, unsigned> STno_to_ESTno;   //subtree number to end subtree number
	vector<vector<unsigned>> mpsubtrees;  //stores subtrees that are in a given mean path
	double R0;
	unsigned long kmtot;  //total number of points for transport equations
	Tree() //constructor
	{
		masstot = 0;
		fluxin = 0;
		Ppl = 0;
		Pplold = 0;
		pert_ij[0] = 0;
		pert_ij[1] = 0;
		pert_type = 'N';  //N for none
		cmouth = 1;
		cmouthold = 1;
		VDfront = 0;
		VDfrontold = 0;
		VDfrontoldold = 0;
		kmtot = 0;
		ctop.clear();
		Vtot.clear();
	}
	~Tree()
	{
		ctop.clear();
		Vtot.clear();
	}
	//output functions
	int printfunc_tree(string filename, Conversions cons);
	int printfunc_lp_tree(string filename, Tree &stree, Conversions cons);
	int printfunc_tree_csv(string filename, Conversions cons);
	int printfunc_lp_tree_csv(string filename, Tree &stree, Conversions cons);
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

//read and set simulation options
int parse_options(string infile, Options &o);
void check_option_consistency(Options &o);
int convert_units(Tree &stree, Conversions &cons, double LL, Options &o);

//initialisation
int initialise_system(Tree &stree, Conversions &cons, Options &o);
int initialise_defects(Tree &stree, Options &o);
int initialise_tree(Tree &stree, Options &o);
int pre_run_breaths(Tree &stree, vector<Tree> &ltree, double &time, Options &o);
int initialise_lung_volumes(Tree &stree, vector<Tree> &ltree, Options &o);

//tree building
double build_alternative_lobe_branches(Tree &stree, Options &o);
double build_symmetric_branches(Tree &stree, Options &o);
string read_lobe_no(unsigned i, Options &o);

//build perturbed trees
void setup_pert_trees(vector<Tree> &ltree, Tree &stree, Options &o);
void setup_lp_tree(vector<Tree> &ltree, Tree &stree, Options &o);
void calc_perts(vector<Tree> &ltree, Tree &stree);
void apply_pert(Tree &ltree, Tree &stree);

//build and solve ventilation problem
int update_flux(Tree &stree, vector<Tree> &ltree, double time, Options &o);
int tree_flux(Tree &stree, double time, Options &o);
int tree_lp_flux(Tree &ltree, Tree &stree, Options &o);

//build and solve transport problem
int update_conc(Tree &stree, vector<Tree> &ltree, double time, Options &o);
int update_c_stree(Tree &stree, double time, Options &o);
int update_c_ltree(Tree &ltree, Tree &stree, double time, Options &o);
void calc_gas_concs(Tree &stree, Eigen::VectorXd &XC, Options &o);
void fill_Ab(Tree &stree, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &XC, Eigen::VectorXd &BC, unsigned long Ntot, double time, Options &o);
void calc_gas_concs_lp(Tree &ltree, Tree &stree, Eigen::VectorXd &XC, Options &o);
void fill_Ab_lp(Tree &ltree, Tree &stree, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &XC, Eigen::VectorXd &BC, unsigned long Ntot, Options &o);

//output functions
int printfunc(Tree &stree, vector<Tree> &ltree, Options &o, Conversions cons, double t);
int append_masterout(string filename, double time, Tree &tree, Options &o, Conversions cons);
int print_simops(ofstream &summary_file, Options &o);
int print_params(ofstream &summary_file, Options &o);

//various input functions determining parameters
double pressure_func(double t, Options &o);
double c_stim(double t, Options &o);

double total_run_time(Options &o);
unsigned long calc_ts_number(Options &o);
unsigned lobe_gens(unsigned nlobe, Options &o);  //returns gen number of final branch
unsigned long ijk_index(unsigned i, unsigned j, unsigned long k, Options &o);
double weibel_length(unsigned j);
double weibel_area(unsigned j);

//calculating resistance matrices for ventilation problem
void subtree_resistance(Tree &tree, unsigned i, Options &o);
void subtree_lp_symm_resistance(subtree &st, subtree &symm, Options &o);
int full_tree_fluxcalc(Tree &tree, double time, Options &o);
int full_lp_tree_fluxcalc(Tree &ltree, Tree &stree, Options &o);
int build_resistance_matrix(Tree &tree, Options &o);
bool isparent(Tree &tree, unsigned ip, unsigned id);

//finite difference functions and solving routines
void upwind_coeffs(vector<double> ucfl[3], vector<double> ucfr[3], vector<double> dxh[3], double u0, Options &o);
void lp_upwind_coeffs(vector<double> ducfl[3], vector<double> ducfr[3], vector<double> dx0[3], vector<double> dxh[3], double u0, Options &o);
void fd_coeffs(vector<double> dcfl[3], vector<double> dcfr[3], vector<double> dxh[3]);
void lp_fd_coeffs(vector<double> ddcfl[3], vector<double> ddcfr[3], vector<double> dx0[3], vector<double> dxh[3]);
void tree_point_pos(Tree &tree, unsigned i, unsigned j, unsigned k, int n, vector<unsigned> &in, vector<unsigned> &jn, vector<unsigned> &kn);
int iterative_solver(Eigen::BiCGSTAB<Eigen::SparseMatrix<double, Eigen::RowMajor>, Eigen::IncompleteLUT<double>> *solver, Eigen::SparseMatrix<double, Eigen::RowMajor> &AC, Eigen::VectorXd &x, Eigen::VectorXd &b);
int LU_solver(Eigen::SparseLU<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> *solver, Eigen::SparseMatrix<double> &A, Eigen::VectorXd &x, Eigen::VectorXd &b);

inline unsigned bitflip(unsigned m)
{
	if (m == 0) return 1;
	else return 0;
}

inline bool jcomp(Defect d1, Defect d2)  //compare defect by i, then j, then k, then type, then magntiude
{
	return ((d1.jn < d2.jn) || ((d1.jn == d2.jn) && (d1.kstart < d2.kstart)) || ((d1.jn == d2.jn) && (d1.kstart == d2.kstart) && (d1.type < d2.type)));
}


#ifndef GIFS_LINKATOMS_H
#define GIFS_LINKATOMS_H

#include <armadillo>

class LinkAtoms
{
public:

    // you own it
    static LinkAtoms* with_const_factors(const arma::umat global_idx, arma::vec factors);
    //
    static LinkAtoms* with_const_length(const arma::umat global_idx, arma::vec dist);
    //
    arma::mat& get_frc() { return frc; }
    arma::mat& get_crd() { return crd; }
    //
    inline void set_local_idx(const size_t* indices) {
        auto itr = local_idx.begin();
        for (size_t idx=0; idx<nlink; ++idx) {
            *itr++ = indices[idx*2];
            *itr++ = indices[idx*2+1];
        }
    };
    // assume global coords
    template<typename T>
    void update_crd(T* syscrd);
    // assume local coords!
    template<typename T>
    void update_crd(T* qmcrd, T* mmcrd);
    //
public:
    arma::uword nlink{0};
private:
    LinkAtoms(arma::umat in_idxs, arma::vec in_factors, arma::vec in_dist):
      nlink{in_factors.size()}, factors{in_factors},  dist{in_dist},
      crd(3, nlink), frc(3, nlink), global_idx{in_idxs}, local_idx(2, nlink) {}
    //
    template<typename T>
    inline double
    get_la_crd(const double& x, const T& qmcrd, const T& mmcrd) {
        return (1.0-x) * qmcrd + x * mmcrd;
    };
    //
    template<typename T>
    void update_factors(T* qmcrd, T* mmcrd);
    //
    template<typename T>
    void update_factors(T* syscrd);
    //
protected:
     //
    arma::vec factors{};
    arma::vec dist{};
    //
    arma::mat crd{};
    arma::mat frc{};
    // global idx
    arma::umat global_idx{};
    // local index
    arma::umat local_idx{};
};
// assume global coords
template<typename T>
void
LinkAtoms::update_crd(T* syscrd) {
    if (!dist.empty()) {
        update_factors(syscrd);
    }
    auto itr = crd.begin();
    for (size_t idx=0; idx<nlink; ++idx) {
        auto col = global_idx.col(idx);
        arma::uword qmid = col[0]*3;
        arma::uword mmid = col[1]*3;
        double fac = factors[idx];

        for(size_t ixyz=0; ixyz<3; ++ixyz) {
            *itr++ = get_la_crd(fac, syscrd[qmid + ixyz], syscrd[mmid + ixyz]);
        }
    }
};
// assume local coords, qm and mm section
template<typename T>
void
LinkAtoms::update_crd(T* qmcrd, T* mmcrd) {
    if (!dist.empty()) {
        update_factors(qmcrd, mmcrd);
    }

    auto itr = crd.begin();
    for (size_t idx=0; idx<nlink; ++idx) {
        auto col = local_idx.col(idx);
        arma::uword qmid = col[0]*3;
        arma::uword mmid = col[1]*3;
        double fac = factors[idx];

        for(size_t ixyz=0; ixyz<3; ++ixyz) {
            *itr++ = get_la_crd(fac, qmcrd[qmid + ixyz], mmcrd[mmid + ixyz]);
        }
    }
};

template<typename T>
void
LinkAtoms::update_factors(T* syscrd) {
    auto fac_itr = factors.begin();
    auto dist_itr = dist.begin();
    for (size_t idx=0; idx<nlink; ++idx) {
        auto col = local_idx.col(idx);
        arma::uword qmid = col[0]*3;
        arma::uword mmid = col[1]*3;
        double R = 0;
        for(size_t ixyz=0; ixyz<3; ++ixyz) {
            R += pow(syscrd[qmid + ixyz] - syscrd[mmid + ixyz], 2);
        }
            *fac_itr++ = *dist_itr++/std::sqrt(R);
    }
};
// assume local coords, qm and mm section
template<typename T>
void
LinkAtoms::update_factors(T* qmcrd, T* mmcrd) {
    auto fac_itr = factors.begin();
    auto dist_itr = dist.begin();
    for (size_t idx=0; idx<nlink; ++idx) {
        auto col = local_idx.col(idx);
        arma::uword qmid = col[0]*3;
        arma::uword mmid = col[1]*3;
        double R = 0;
        for(size_t ixyz=0; ixyz<3; ++ixyz) {
            R += pow(qmcrd[qmid + ixyz] - mmcrd[mmid + ixyz], 2);
        }
        *fac_itr++ = *dist_itr++/std::sqrt(R);
    }
};

#endif // GIFS_LINKATOMS_H


/*
 * Copyright [2015] [Ke Sun <sunke.polyu@gmail.com>]
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <ros/ros.h>
#include <Eigen/SVD>
#include <mocap_base/KalmanFilter.h>

using namespace std;
using namespace Eigen;

namespace mocap {

KalmanFilter::KalmanFilter():
  attitude         (Quaterniond::Identity()),
  position         (Vector3d::Zero()),
  angular_vel      (Vector3d::Zero()),
  linear_vel       (Vector3d::Zero()),
  state_cov        (Matrix12d::Identity()),
  input_cov        (Matrix12d::Identity()),
  measurement_cov  (Matrix6d::Identity()),
  filter_status    (INIT_POSE),
  last_time_stamp  (0.0),
  msg_interval     (0.01),
  proc_jacob       (Matrix12d::Zero()),
  meas_jacob       (Matrix6_12d::Zero()),
  proc_noise_jacob (Matrix12d::Zero()),
  meas_noise_jacob (Matrix6d::Zero()) {

  // Needs to be changed based on angular velocity
  // and time interval
  proc_jacob = Matrix12d::Identity();
  proc_noise_jacob = Matrix12d::Identity();

  // Do not need to be changed
  meas_jacob.leftCols<6>() = Matrix6d::Identity();
  meas_noise_jacob = Matrix6d::Identity();

  return;
}

bool KalmanFilter::init(const Matrix12d& u_cov,
    const Matrix6d& m_cov, const int& freq) {
  bool is_valid = true;
  JacobiSVD<Matrix12d> u_svd(u_cov);
  JacobiSVD<Matrix6d> m_svd(m_cov);

  Vector12d u_sigmas = u_svd.singularValues();
  Vector6d m_sigmas = m_svd.singularValues();

  if (u_sigmas(11) < 1e-10) {
    is_valid = false;
    ROS_ERROR("Input Cov is close to singular (least singlar value:%f < 1e-7)",
        u_sigmas(11));
  } else {
    input_cov = u_cov;
  }

  if (m_sigmas(5) < 1e-10) {
    is_valid = false;
    ROS_ERROR("Measurement Cov is close to singular (least singlar value:%f < 1e-7)",
        m_sigmas(5));
  } else {
    measurement_cov = m_cov;
  }

  if (freq < 0) {
    is_valid = false;
    ROS_ERROR("Invalid frequency for filter (%d < 0)", freq);
  } else {
    msg_interval = 1.0 / static_cast<double>(freq);
  }

  return is_valid;
}

bool KalmanFilter::prepareInitialCondition(
    const double& curr_time_stamp,
    const Eigen::Quaterniond& m_attitude,
    const Eigen::Vector3d& m_position) {

  switch (filter_status) {
    case INIT_POSE: {
      // Set the current pose
      last_time_stamp = curr_time_stamp;
      attitude = m_attitude;
      position = m_position;
      filter_status = INIT_TWIST;
      // Set the uncertainty of the state
      state_cov = Matrix12d::Identity();
      return true;
    }
    case INIT_TWIST: {
      // Compute the difference between the current
      // pose and last pose
      Quaterniond dq = m_attitude*attitude.inverse();
      AngleAxisd daa(dq);
      Vector3d dr = m_position-position;
      double dt = curr_time_stamp-last_time_stamp;
      //dt = dt > 0 ? dt : msg_interval;
      dt = dt*0.9 + msg_interval*0.1;
      // Set current pose and velocity
      last_time_stamp += dt;
      attitude = m_attitude;
      position = m_position;
      angular_vel = daa.axis()*daa.angle()/dt;
      linear_vel = dr/dt;
      // Set the uncertainty of the state
      state_cov = Matrix12d::Identity();
      filter_status = READY;
      return true;
    }
    case READY:
      return false;
    default:
      return false;
  }
  return false;
}

bool KalmanFilter::isReady() {
  if (filter_status != READY)
    return false;
  else
    return true;
}

void KalmanFilter::reset() {
  filter_status = INIT_POSE;
  return;
}

void KalmanFilter::prediction(const double& curr_time_stamp) {
  // Propogate the actual state
  double dt = curr_time_stamp - last_time_stamp;
  //dt = dt > 0 ? dt : msg_interval;
  dt = dt*0.9 + msg_interval*0.1;
  Vector3d dw = angular_vel * dt;
  Vector3d dr = linear_vel * dt;

  double dangle = dw.norm();
  Vector3d axis = dw / dangle;
  AngleAxisd daa(dangle, axis);
  Quaterniond dq(daa);
  // Velocities are modeled as constants
  last_time_stamp += dt;
  attitude = dq * attitude;
  position = dr + position;

  // Propogate the uncertainty of the estimation error
  // TODO: Optimize this part
  proc_jacob(0,  1) =  dw(2);
  proc_jacob(0,  2) = -dw(1);
  proc_jacob(1,  0) = -dw(2);
  proc_jacob(1,  2) =  dw(0);
  proc_jacob(2,  0) =  dw(1);
  proc_jacob(2,  1) = -dw(0);
  proc_jacob(0,  6) =  dt;
  proc_jacob(1,  7) =  dt;
  proc_jacob(2,  8) =  dt;
  proc_jacob(3,  9) =  dt;
  proc_jacob(4, 10) =  dt;
  proc_jacob(5, 11) =  dt;

  //state_cov = proc_jacob*state_cov*proc_jacob.transpose() +
  //  proc_noise_jacob*input_cov*proc_noise_jacob.transpose();
  state_cov = proc_jacob*state_cov*proc_jacob.transpose() + input_cov;

  //cout << "Process: " << endl;
  //cout << "att: " <<
  //  Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl;
  //cout << "pos: " << position.transpose() << endl;
  //cout << "ang: " << angular_vel.transpose() << endl;
  //cout << "lin: " << linear_vel.transpose() << endl;
  //cout << "input noise:\n" << input_cov << endl;
  //cout << "proc_jacob:\n" << proc_jacob << endl;
  //cout << "Pk+1|k:\n" << state_cov << endl;

  return;
}

void KalmanFilter::update(const Eigen::Quaterniond& m_attitude,
    const Eigen::Vector3d& m_position) {
  // TODO: Optimize the whole function
  // Compute the residual of the measurement
  Quaterniond re_q = m_attitude * attitude.inverse();
  AngleAxisd re_aa(re_q);
  if (std::abs(re_aa.angle()) > std::abs(2*M_PI-re_aa.angle())) {
    re_aa.angle() = 2*M_PI - re_aa.angle();
    re_aa.axis() = -re_aa.axis();
  }
  Quaterniond re_qs(re_aa);
  Vector3d re_th(re_qs.x()*2.0, re_qs.y()*2.0, re_qs.z()*2.0);
  Vector3d re_r = m_position - position;
  Vector6d re;
  re.head<3>() = re_th;
  re.tail<3>() = re_r;

  // Compute the covariance of the residual
  //Matrix6d S = measurement_cov +
  //  meas_jacob*state_cov*meas_jacob.transpose();
  Matrix6d S = measurement_cov + state_cov.topLeftCorner<6, 6>();

  // Compute the Kalman gain
  //Matrix12_6d K = state_cov*meas_jacob.transpose()*S.inverse();
  Matrix12_6d K = state_cov.leftCols<6>()*S.inverse();

  // Compute the correction of the state
  Vector12d dx = K * re;

  // Update the state based on the correction
  Vector3d dq_vec3 = dx.head<3>() / 2.0;
  Vector4d dq_vec4 = 1.0/sqrt(1.0+dq_vec3.squaredNorm())*
    Vector4d(dq_vec3(0), dq_vec3(1), dq_vec3(2), 1.0);
  Quaterniond dq(dq_vec4(3), dq_vec4(0), dq_vec4(1), dq_vec4(2));

  attitude = dq * attitude;
  position += dx.segment<3>(3);
  angular_vel += dx.segment<3>(6);
  linear_vel += dx.segment<3>(9);

  // Update the uncertainty of the state/error
  state_cov = (Matrix12d::Identity()-K*meas_jacob) * state_cov;

  //cout << "Error: " << endl;
  //cout << "dat: " << Vector4d(re_q.w(), re_q.x(), re_q.y(), re_q.z()).transpose() << endl;
  //cout << "dpo: " << re_r.transpose() << endl;

  //cout << "Innovation: " << endl;
  //cout << "dat: " << Vector4d(dq.w(), dq.x(), dq.y(), dq.z()).transpose() << endl;
  //cout << "dpo: " << dx.segment<3>(3).transpose() << endl;
  //cout << "dan: " << dx.segment<3>(6).transpose() << endl;
  //cout << "dli: " << dx.segment<3>(9).transpose() << endl;

  //cout << "Update: " << endl;
  //cout << "att: " <<
  //  Vector4d(attitude.w(), attitude.x(), attitude.y(), attitude.z()).transpose() << endl;
  //cout << "pos: " << position.transpose() << endl;
  //cout << "ang: " << angular_vel.transpose() << endl;
  //cout << "lin: " << linear_vel.transpose() << endl;
  //cout << "means_jacob:\n" << meas_jacob << endl;
  //cout << "Pk+1|k:\n" << state_cov << endl;
  //cout << "meas_noise:\n" << measurement_cov << endl;
  //cout << endl;

  return;
}
}


/*

PICCANTE
The hottest HDR imaging library!
http://vcg.isti.cnr.it/piccante

Copyright (C) 2014
Visual Computing Laboratory - ISTI CNR
http://vcg.isti.cnr.it
First author: Francesco Banterle

This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this
file, You can obtain one at http://mozilla.org/MPL/2.0/.

*/

#ifndef PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP
#define PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP

#include "../util/matrix_3_x_3.hpp"
#include "../util/nelder_mead_opt_base.hpp"
#include "../computer_vision/camera_matrix.hpp"

#ifndef PIC_DISABLE_EIGEN
#ifndef PIC_EIGEN_NOT_BUNDLED
   #include "../externals/Eigen/Dense"
#else
    #include <Eigen/Dense>
#endif
#endif

namespace pic {

#ifndef PIC_DISABLE_EIGEN

#define GL_PACKED_CAMERA_SIZE 11
#define GL_3D_POINT_SIZE 3

class NelderMeadOptGordonLowe: public NelderMeadOptBase<double>
{
public:
    std::vector< std::vector< Vec<2, float> > > m;

    /**
     * @brief NelderMeadOptGordonLowe
     * @param m0
     * @param m1
     */
    NelderMeadOptGordonLowe(std::vector< Vec<2, float> > m0, std::vector< Vec<2, float> > m1) : NelderMeadOptBase()
    {
        this->m.push_back(m0);
        this->m.push_back(m1);
    }

    static Eigen::Matrix34d parseCameraMatrix(float *x, unsigned int index)
    {
        Eigen::Matrix3d K, R;
        Eigen::Vector3d t;

        //offset of the matrix
        unsigned int c = index * 11;

        Eigen::Quaternion<double> reg;
        reg.x() = x[c]; c++;
        reg.y() = x[c]; c++;
        reg.z() = x[c]; c++;
        reg.w() = x[c]; c++;
        R = reg.toRotationMatrix();

        t[0] = x[c]; c++;
        t[1] = x[c]; c++;
        t[2] = x[c]; c++;

        K.setZero();
        K(0, 0) = x[c]; c++;
        K(1, 1) = x[c]; c++;
        K(0, 2) = x[c]; c++;
        K(1, 2) = x[c];
        K(2, 2) = 1.0;

        return getCameraMatrix(K, R, t);
    }

    /**
     * @brief ProjectionError
     * @param x
     * @param index
     * @return
     */
    double ProjectionError(float *x, unsigned int index) {

       double err = 0.0;

       Eigen::Matrix34d P = parseCameraMatrix(x, index);

       //offset of vertices
       int c = GL_PACKED_CAMERA_SIZE * int(m.size());

       Eigen::Vector4d point;
       for(int i = 0; i < m[index].size(); i++) {
           point = Eigen::Vector4d(x[c], x[c + 1], x[c + 2], 1.0);

           Eigen::Vector3d point_proj = P * point;
           point_proj /= point_proj[2];

           double dx = point_proj[0] - m[index][i][0];
           double dy = point_proj[1] - m[index][i][1];

           err += dx * dx + dy * dy;

           c += 3;
       }

       return err;
    }

    /**
     * @brief function
     * @param x
     * @param n
     * @return
     */
    float function(float *x, unsigned int n)
    {
        int n2 = int(m.size() * m[0].size());
        double err = sqrt((ProjectionError(x, 0) + ProjectionError(x, 1)) / double(n2));

        return float(err);
    }

    /**
     * @brief init3DPoints
     * @param K
     * @param m
     * @param x
     * @param distance
     */
    static void init3DPoints(Eigen::Matrix3d K, std::vector< Vec<2, float> > &m, std::vector< Eigen::Vector3d > &x, float distance = 20.0f)
    {
        Eigen::Matrix3d K_inv = K.inverse();
    \
        printf("Points: %zd\n", m.size());

        for(unsigned int i = 0; i < m.size(); i++) {
            Eigen::Vector3d point = Eigen::Vector3d (m[i][0], m[i][1], 1.0);

            point = K_inv * point;

            point *= distance;

            x.push_back(point);
        }
    }

    /**
     * @brief prepareInputData
     * @param K
     * @param R
     * @param t
     * @param x
     * @param ret_size
     * @return
     */
    static double *prepareInputData(std::vector< Eigen::Matrix3d > &K, std::vector< Eigen::Matrix3d > &R, std::vector< Eigen::Vector3d > &t, std::vector< Eigen::Vector3d > &x, unsigned int &ret_size)
    {
        if(R.size() != t.size()) {
            return NULL;
        }

        if(x.empty()) {
            return NULL;
        }

        int n = int (R.size());
        ret_size = GL_PACKED_CAMERA_SIZE * n + GL_3D_POINT_SIZE * int(x.size());
        double *ret = new double[ret_size];

        int c = 0;
        for(int i = 0; i < n; i++) {

            Eigen::Quaternion<double> reg(R[i]);

            ret[c] = reg.x(); c++;
            ret[c] = reg.y(); c++;
            ret[c] = reg.z(); c++;
            ret[c] = reg.w(); c++;

            ret[c] = t[i][0]; c++;
            ret[c] = t[i][1]; c++;
            ret[c] = t[i][2]; c++;

            ret[c] = K[i](0, 0); c++;
            ret[c] = K[i](1, 1); c++;
            ret[c] = K[i](0, 2); c++;
            ret[c] = K[i](1, 2); c++;
        }

        for(size_t i = 0; i < x.size(); i++) {
            ret[c] = x[i][0]; c++;
            ret[c] = x[i][1]; c++;
            ret[c] = x[i][2]; c++;
        }

        return ret;
    }
};

#endif

}

#endif // PIC_COMPUTER_VISION_NELDER_MEAD_OPT_GORDON_LOWE_HPP


// https://qiita.com/lucidfrontier45/items/5048ef74fbf32eeb9f08

#include <iostream>
#include <pybind11/eigen.h>
#include <Eigen/Core>
namespace py = pybind11;

template <typename T>
using RMatrix = Eigen::Matrix<T, -1, -1, Eigen::RowMajor>;

template <typename T>
void print_array(RMatrix<T> m){
    py::print(m);
}
template <typename T>
RMatrix<T> modify_array(RMatrix<T> m, T a){
    return m * a;
}
template <typename T>
void modify_array_inplace(Eigen::Ref<RMatrix<T>> m, T a){ //中身参照
    m = m * a;
}
template<typename T>
void modify_new_array(Eigen::Ref<RMatrix<T>> m){
    m = RMatrix<T>::Ones(3,10); // not work

}
template<typename T>
RMatrix<T> get_new_array(){
    RMatrix<T> m = RMatrix<T>::Ones(10,3);
	for(int i=0; i<m.rows(); ++i){
		for(int j=0; j<m.cols(); ++j){
			std::cout << m(i,j) << std::endl;
		}
	}
	RMatrix<T> mm(1,3);
	mm << 100,200,300;
	m.block(0,0,1,3) = mm;
	return m;
}

PYBIND11_MODULE(MY_MODULE_NAME, m){
    m.def("print_array", &print_array<double>, "");
    m.def("modify_array", &modify_array<double>, "");
    m.def("modify_array_inplace", &modify_array_inplace<double>, "");
	m.def("modify_new_array", &modify_new_array<double>, "");
	m.def("get_new_array", &get_new_array<double>, "");
}


#include <k52/optimization/hooke_jeeves_method.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <iostream>
#include <cmath>
#include <algorithm>
#include <stdexcept>

#include <k52/common/floating_point.h>
#include <k52/optimization/params/i_continuous_parameters.h>

using ::std::vector;

namespace k52
{
namespace optimization
{

HookeJeevesMethod::HookeJeevesMethod(
    double acceleration,
    double init_step,
    size_t max_iteration_number,
    double precision,
    double step_divider)
    : acceleration_(acceleration)
    , init_step_(init_step)
    , max_iteration_number_(max_iteration_number)
    , precision_(precision)
    , step_divider_(step_divider)
{
}

HookeJeevesMethod* HookeJeevesMethod::Clone() const
{
    return new HookeJeevesMethod(
        acceleration_,
        init_step_,
        max_iteration_number_,
        precision_,
        step_divider_);
}

std::string HookeJeevesMethod::get_name() const
{
    return "Hooke-Jeeves Method";
}

#ifdef BUILD_WITH_MPI
void HookeJeevesMethod::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, acceleration_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, init_step_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, step_divider_);
}

void HookeJeevesMethod::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, acceleration_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, max_iteration_number_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, init_step_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, step_divider_);
}
#endif

vector<double> HookeJeevesMethod::FindOptimalParameters(const vector<double>& initial_parameters)
{
    vector<double> arguments(initial_parameters);
    vector<double> arguments_after_search(initial_parameters.size());
    vector<double> steps_array = vector<double>(arguments.size(), init_step_);
    size_t iteration = 0;

    do
    {
        arguments_after_search = CoordinatewiseSearch(arguments, steps_array);
        if (common::FloatingPoint::AreEqual(arguments, arguments_after_search))
        {
            for (size_t i = 0; i < steps_array.size(); ++i)
            {
                steps_array[i] /= step_divider_;
            }
            continue;
        }

        vector<double> next_step_arguments(arguments.size());
        vector<double> next_step_arguments_after_search(arguments.size());
        while (true)
        {
            for (size_t i = 0; i<arguments.size(); ++i)
            {
                next_step_arguments[i] = arguments[i] + acceleration_
                        * (arguments_after_search[i] - arguments[i]);
            }
            next_step_arguments_after_search = CoordinatewiseSearch(next_step_arguments, steps_array);

            arguments = arguments_after_search;
            if (CountObjectiveFunctionValueToMinimize(arguments_after_search) <=
                    CountObjectiveFunctionValueToMinimize(next_step_arguments_after_search))
            {
                break;
            }
            arguments_after_search = next_step_arguments_after_search;

            iteration++;
            // Check number of iterations
            if (iteration == max_iteration_number_)
            {
                std::cout << "Solution not found" << std::endl;
                return arguments_after_search;
            }
        }
    } while (!IsExitCriteriaFulfilled(steps_array));
    return arguments_after_search;
}

bool HookeJeevesMethod::IsExitCriteriaFulfilled(
    const vector<double>& steps_array) const
{
    double temp = 0;
    // Check if step is small enough
    for (size_t j = 0; j<steps_array.size(); ++j)
    {
        temp += steps_array[j] * steps_array[j];
    }
    if (std::sqrt(temp) <= precision_)
    {
        return true;
    }
    return false;
}

vector<double> HookeJeevesMethod::CoordinatewiseSearch(
    const vector<double>& arguments,
    const vector<double>& steps_array) const
{
    vector<double> new_arguments(arguments);
    for (size_t i = 0; i < arguments.size(); ++i)
    {
        double new_function_value = CountObjectiveFunctionValueToMinimize(new_arguments);
        vector<double> temp(new_arguments);

        temp[i] = new_arguments[i] + steps_array[i];
        if (CountObjectiveFunctionValueToMinimize(temp) < new_function_value)
        {
            new_arguments = temp;
            continue;
        }

        temp[i] = new_arguments[i] - steps_array[i];
        if (CountObjectiveFunctionValueToMinimize(temp) < new_function_value)
        {
            new_arguments = temp;
        }
    }
    return new_arguments;
}

}/* namespace optimization */
}/* namespace k52 */


#include "gtest/gtest.h"
#include <string>
#include <boost/algorithm/string.hpp>
#include "Graph.h"

TEST(graph_check, test_directed) {
    //this adjancency matrix is directed
    vector<vector<int>> am = {
            {0, 1, 0, 0},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isDirected(), false);

}

TEST(graph_check, test_not_directed) {
    //this adjancency matrix is directed
    vector<vector<int>> am = {
            {0, 1, 0, 1},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isDirected(), true);
}

TEST(graph_check, test_nr_edges_1) {
    //this adjancency matrix is directed
    vector<vector<int>> am = {
            {0, 1, 0, 0},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->getNumberOfEdges(), 3);
}

TEST(graph_check, test_nr_edges_2) {
    //this adjancency matrix is not directed
    vector<vector<int>> am = {
            {0, 1, 0, 1},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->getNumberOfEdges(), 7);
}

TEST(graph_check, test_has_cycle_1) {
    //this adjancency matrix has a cycle
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->hasCycle(), true);
}

TEST(graph_check, test_has_cycle_2) {
    //this adjancency matrix has no cycle
    vector<vector<int>> am = {
            {0, 1, 0, 0},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->hasCycle(), true);
}

TEST(graph_check, test_nr_of_nodes) {
    vector<vector<int>> am = {
            {0, 1, 0, 0},
            {1, 0, 1, 0},
            {0, 1, 0, 1},
            {0, 0, 1, 0}
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->getNumberOfNodes(), 4);
}

TEST(graph_check, test_regular_1) {
    vector<vector<int>> am = {
            {0, 1, 1},
            {1, 0, 1},
            {1, 1, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isRegular(), true);
}

TEST(graph_check, test_regular_2) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {1, 1, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isRegular(), false);
}

TEST(graph_check, test_complete_1) {
    vector<vector<int>> am = {
            {0, 1, 1},
            {1, 0, 1},
            {1, 1, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isComplete(), true);
}

TEST(graph_check, test_complete_2) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {1, 1, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->isComplete(), false);
}

TEST(graph_check, test_indegree) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->getInDeg(0), 2);
    EXPECT_EQ(g->getInDeg(1), 2);
    EXPECT_EQ(g->getInDeg(2), 2);
    EXPECT_EQ(g->getInDeg(3), 0);
}

TEST(graph_check, test_outdegree) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_EQ(g->getOutDeg(0), 2);
    EXPECT_EQ(g->getOutDeg(1), 2);
    EXPECT_EQ(g->getOutDeg(2), 2);
    EXPECT_EQ(g->getOutDeg(3), 0);
}

TEST(graph_check, test_free_of_loops) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_TRUE(g->isFreeOfLoops());
}

TEST(graph_check, test_free_of_loops_2) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 1, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_FALSE(g->isFreeOfLoops());
}

TEST(graph_check, test_are_neighbours_1) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 1, 1, 0},
            {1, 1, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_FALSE(g->areNeighbours(0, 0));
    EXPECT_FALSE(g->areNeighbours(0, 3));
    EXPECT_FALSE(g->areNeighbours(1, 3));
    EXPECT_FALSE(g->areNeighbours(2, 2));
    EXPECT_FALSE(g->areNeighbours(2, 3));
    EXPECT_FALSE(g->areNeighbours(3, 0));
    EXPECT_FALSE(g->areNeighbours(3, 1));
    EXPECT_FALSE(g->areNeighbours(3, 2));
    EXPECT_FALSE(g->areNeighbours(3, 3));

    EXPECT_TRUE(g->areNeighbours(0, 1));
    EXPECT_TRUE(g->areNeighbours(0, 2));
    EXPECT_TRUE(g->areNeighbours(1, 0));
    EXPECT_TRUE(g->areNeighbours(1, 1));
    EXPECT_TRUE(g->areNeighbours(1, 2));
    EXPECT_TRUE(g->areNeighbours(2, 0));
    EXPECT_TRUE(g->areNeighbours(2, 1));
}

TEST(graph_check, test_are_neighbours_directed) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {0, 0, 0, 0},
            {0, 0, 0, 0},
            {0, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_FALSE(g->areNeighbours(0, 0));
    EXPECT_FALSE(g->areNeighbours(1, 0));
    EXPECT_FALSE(g->areNeighbours(2, 0));
    EXPECT_FALSE(g->areNeighbours(3, 0));

    EXPECT_TRUE(g->areNeighbours(0, 1));
    EXPECT_TRUE(g->areNeighbours(0, 2));
    EXPECT_TRUE(g->areNeighbours(0, 3));
}

TEST(graph_check, test_are_neighbours_undirected) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
    };
    Graph *g = new Graph(am);
    EXPECT_TRUE(g->areNeighbours(1, 0));
    EXPECT_TRUE(g->areNeighbours(2, 0));
    EXPECT_TRUE(g->areNeighbours(3, 0));

    EXPECT_TRUE(g->areNeighbours(0, 1));
    EXPECT_TRUE(g->areNeighbours(0, 2));
    EXPECT_TRUE(g->areNeighbours(0, 3));
}

TEST(graph_check, test_are_neighbours_range_1) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
    };
    Graph *g = new Graph(am);

    // throw out_of_range("from/to must be in [0, " + getNumberOfNodes() + string("[."));
    try {
        g->areNeighbours(-1, 3);
        FAIL();
    } catch (const out_of_range &e) {
        cout << e.what() << endl;
        ASSERT_STREQ("from and to have to be in the range. ", e.what());
    }
}

TEST(graph_check, test_are_neighbours_range_2) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
    };
    Graph *g = new Graph(am);

    // throw out_of_range("from/to must be in [0, " + getNumberOfNodes() + string("[."));
    try {
        g->areNeighbours(2, -2);
        FAIL();
    } catch (const out_of_range &e) {
        cout << e.what() << endl;
        ASSERT_STREQ("from and to have to be in the range. ", e.what());
    }
}

TEST(graph_check, test_are_neighbours_range_3) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
    };
    Graph *g = new Graph(am);

    // throw out_of_range("from/to must be in [0, " + getNumberOfNodes() + string("[."));
    try {
        g->areNeighbours(0, 4);
        FAIL();
    } catch (const out_of_range &e) {
        cout << e.what() << endl;
        ASSERT_STREQ("from and to have to be in the range. ", e.what());
    }
}

TEST(graph_check, test_are_neighbours_range_4) {
    vector<vector<int>> am = {
            {0, 1, 1, 1},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
            {1, 0, 0, 0},
    };
    Graph *g = new Graph(am);

    // throw out_of_range("from/to must be in [0, " + getNumberOfNodes() + string("[."));
    try {
        g->areNeighbours(4, 0);
        FAIL();
    } catch (const out_of_range &e) {
        cout << e.what() << endl;
        ASSERT_STREQ("from and to have to be in the range. ", e.what());
    }
}

TEST(graph_check, test_matlab_correct_notation) {

    try {
        // These graphs should not cause an error.
        Graph *a = new Graph("[1 0 1; 1 1 1; 0 0 1]");
        Graph *b = new Graph("  \t [1 0  1;1 \t\t 1 1\t\t\t    ;     0  0  1 ] ");
        Graph *c = new Graph("[1\t0\t1;1\t1\t1;0\t0\t1]");
        Graph *d = new Graph(" [ 1   0   1 ;   1   1   1 ;   0   0   1 ] ");
        Graph *e = new Graph("[1]");

    } catch (const invalid_argument &e) {
        cout << e.what();
        FAIL();
    }

    SUCCEED();
}

TEST(graph_check, test_matlab_wrong_notation_1) {

    try {
        Graph *g = new Graph("[1 0 1; 1 1 1 1; 0 0 1]"); // one column too much
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("There must be a syntax error, because the number of columns are different.", e.what());
    }
}

TEST(graph_check, test_matlab_wrong_notation_2) {

    try {
        Graph *g = new Graph("[1 0 1 0; 1 1 1 1; 0 0 1 1]"); // is not square
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("adjacency matrix has to be symmetrical", e.what());
    }
}

TEST(graph_check, test_matlab_wrong_notation_3) {

    try {
        Graph *g = new Graph("[0 1 0; 1 1 1; 0 1 1; 1 1 1]"); // is not square
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("adjacency matrix has to be symmetrical", e.what());
    }

}

TEST(graph_check, test_matlab_wrong_notation_4) {

    try {
        Graph *g = new Graph("1 0 1; 1 1 1; 0 0 1");
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("A valid matlab matrix notation must start with '[' and end up with ']'.", e.what());
    }
}

TEST(graph_check, test_matlab_wrong_notation_5) {

    try {
        Graph *g = new Graph("[]");
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("matrix entries must be and contain at least one integer.", e.what());
    }
}

TEST(graph_check, test_matlab_wrong_notation_6) {

    try {
        Graph *g = new Graph("[ a ]");
        FAIL();
    } catch (const invalid_argument &e) {
        ASSERT_STREQ("matrix entries must be and contain at least one integer.", e.what());
    }
}

TEST(graph_check, test_constructor_names) {
    vector<vector<int>> am = {
            {0, 1, 1, 0},
            {1, 0, 1, 0},
            {1, 1, 0, 0},
            {1, 1, 0, 0},
    };
    vector<string> nodes = {"test", "node2", "node3", "node4"};
    Graph *g = new Graph(am, nodes);
    string dot = g->exportDot();
    cout << dot << endl;
    EXPECT_NE(dot.find("test -> node2"), string::npos);
    EXPECT_NE(dot.find("test -> node3"), string::npos);
    EXPECT_NE(dot.find("node2 -> test"), string::npos);
    EXPECT_NE(dot.find("node2 -> node3"), string::npos);
    EXPECT_NE(dot.find("node3 -> test"), string::npos);
    EXPECT_NE(dot.find("node3 -> node2"), string::npos);
    EXPECT_NE(dot.find("node4 -> test"), string::npos);
    EXPECT_NE(dot.find("node4 -> node2"), string::npos);
}

TEST(graph_check, test_adjazenzmatrix_string) {
    Graph *g = new Graph("[1 0 1; 1 1 1; 0 0 1]");
    string mat = g->getAdjacencyMatrixString();

    std::vector<std::string> strs;
    boost::split(strs, mat, boost::is_any_of("\n"));

    ASSERT_EQ(strs[0], "1,0,1");
    ASSERT_EQ(strs[1], "1,1,1");
    ASSERT_EQ(strs[2], "0,0,1");
}

TEST(graph_check, test_forest_true) {
    Graph *g = new Graph("[0 1 1; 0 0 0; 0 0 0]");
    bool forest = g->isForest();

    ASSERT_TRUE(forest);
}

TEST(graph_check, test_forest_false) {
    //cycle -> false
    Graph *g = new Graph("[0 1 1; 0 0 0; 1 0 0]");
    bool forest = g->isForest();

    ASSERT_FALSE(forest);
}

TEST(graph_check, test_forest_undirected) {
    //undirected -> false
    Graph *g = new Graph("[0 1 1; 1 0 0; 1 0 0]");
    bool forest = g->isForest();

    ASSERT_FALSE(forest);
}


/**
 * @file mesh.cc
 * @brief NPDE homework ElementMatrixComputation code
 * @author Janik Schüttler, edited by Oliver Rietmann
 * @date 03.03.2019
 * @copyright Developed at ETH Zurich
 */

#include "mesh.h"

#include <lf/base/base.h>
#include <lf/geometry/geometry.h>
#include <lf/mesh/hybrid2d/hybrid2d.h>

#include <Eigen/Core>
#include <array>
#include <memory>

std::shared_ptr<lf::mesh::Mesh> Generate2DTestMesh() {
  using size_type = lf::mesh::Mesh::size_type;

  // Obtain mesh factory
  std::shared_ptr<lf::mesh::hybrid2d::MeshFactory> mesh_factory_ptr =
      std::make_shared<lf::mesh::hybrid2d::MeshFactory>(2);

  double scale = 1.0;

  // Hybrid mesh of square [0,3]^2 with only triangles and rectangles
  // Set coordinates of nodes
  // clang-format off
  std::array<std::array<double, 2>, 10> node_coord{
      std::array<double, 2>({0, 0 }),
      std::array<double, 2>({1, 0 }),
      std::array<double, 2>({2, 0 }),
      std::array<double, 2>({3, 0 }),
      std::array<double, 2>({0 ,2 }),
      std::array<double, 2>({1 ,2 }),
      std::array<double, 2>({3 ,2 }),
      std::array<double, 2>({0 ,3 }),
      std::array<double, 2>({1 ,3 }),
      std::array<double, 2>({3 ,3 })};
  // clang-format on

  // Specify triangles (five)
  std::array<std::array<size_type, 3>, 5> tria_nodes{
      std::array<size_type, 3>({1, 2, 5}), std::array<size_type, 3>({2, 3, 6}),
      std::array<size_type, 3>({5, 2, 6}), std::array<size_type, 3>({4, 5, 7}),
      std::array<size_type, 3>({5, 8, 7})};

  // Specify parallelograms (two)
  std::array<std::array<size_type, 4>, 2> parg_nodes{
      std::array<size_type, 4>({0, 1, 5, 4}),
      std::array<size_type, 4>({5, 6, 9, 8})};

  // Create nodes
  for (const auto &node : node_coord) {
    mesh_factory_ptr->AddPoint(
        Eigen::Vector2d({node[0] * scale, node[1] * scale}));
  }

  // generate triangles
  for (const auto &node : tria_nodes) {
    mesh_factory_ptr->AddEntity(
        lf::base::RefEl::kTria(),
        nonstd::span<const size_type>({node[0], node[1], node[2]}),
        std::unique_ptr<lf::geometry::Geometry>(nullptr));
  }

  // generate Parallelograms
  for (const auto &node : parg_nodes) {
    Eigen::MatrixXd quad_coord(2, 4);
    for (int n_pt = 0; n_pt < 4; ++n_pt) {
      quad_coord(0, n_pt) = node_coord[node[n_pt]][0];
      quad_coord(1, n_pt) = node_coord[node[n_pt]][1];
    }
    mesh_factory_ptr->AddEntity(
        lf::base::RefEl::kQuad(),
        nonstd::span<const size_type>({node[0], node[1], node[2], node[3]}),
        std::make_unique<lf::geometry::Parallelogram>(quad_coord));
  }

  // Optional: Inspect data
  // mesh_factory_ptr->PrintLists(std::cout);
  return mesh_factory_ptr->Build();
}


// -----------------------------------------------------------------------------
// Fern © Geoneric
//
// This file is part of Geoneric Fern which is available under the terms of
// the GNU General Public License (GPL), version 2. If you do not want to
// be bound by the terms of the GPL, you may purchase a proprietary license
// from Geoneric (http://www.geoneric.eu/contact).
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE fern algorithm trigonometry sin
#include <boost/test/unit_test.hpp>
#include "fern/core/data_customization_point/scalar.h"
#include "fern/core/type_traits.h"
#include "fern/algorithm/trigonometry/sin.h"

namespace fa = fern::algorithm;

template<
    class Value>
using OutOfDomainPolicy = fa::sin::OutOfDomainPolicy<Value>;

BOOST_AUTO_TEST_CASE(out_of_domain_policy)
{
    {
        OutOfDomainPolicy<double> policy;
        BOOST_CHECK(policy.within_domain(5));
        BOOST_CHECK(policy.within_domain(-5));
        BOOST_CHECK(policy.within_domain(0));
        BOOST_CHECK(!policy.within_domain(fern::infinity<double>()));
        BOOST_CHECK(!policy.within_domain(-fern::infinity<double>()));
    }
}

template<
    class Value,
    class Result>
void verify_value(
    Value const& value,
    Result const& result_we_want)
{
    fa::SequentialExecutionPolicy sequential;

    Result result_we_get;
    fa::trigonometry::sin(sequential, value, result_we_get);
    BOOST_CHECK_CLOSE(1.0 + result_we_get, 1.0 + result_we_want, 1e-10);
}

BOOST_AUTO_TEST_CASE(algorithm)
{
    verify_value<double, double>(0.0, 0.0);
    verify_value<double, double>(-0.0, -0.0);
    verify_value<double, double>(fern::half_pi<double>(), 1.0);
    verify_value<double, double>(fern::pi<double>(), 0.0);
    verify_value<double, double>(-fern::half_pi<double>(), -1.0);
    verify_value<double, double>(-fern::pi<double>(), 0.0);
}


//---------------------------------------------------------------------------
//    $Id: compressed_block_sparsity_pattern.cc 28377 2013-02-13 15:22:32Z heister $
//
//    Copyright (C) 2006-2007 by the deal.II authors
//
//    This file is subject to QPL and may not be  distributed
//    without copyright and license information. Please refer
//    to the file deal.II/doc/license.html for the  text  and
//    further information on this license.
//
//---------------------------------------------------------------------------

// See documentation of BlockCompressedSparsityPattern for documentation of this example

#include <deal.II/lac/block_sparsity_pattern.h>
#include <deal.II/lac/constraint_matrix.h>
#include <deal.II/grid/tria.h>
#include <deal.II/grid/tria_accessor.h>
#include <deal.II/grid/tria_iterator.h>
#include <deal.II/grid/grid_generator.h>
#include <deal.II/fe/fe_q.h>
#include <deal.II/fe/fe_system.h>
#include <deal.II/dofs/dof_handler.h>
#include <deal.II/dofs/dof_renumbering.h>
#include <deal.II/dofs/dof_tools.h>

#include <iostream>

using namespace dealii;

int main()
{
  Triangulation<2> tr;
  GridGenerator::subdivided_hyper_cube(tr, 3);
  tr.begin_active()->set_refine_flag();
  tr.execute_coarsening_and_refinement();

  FE_Q<2> fe1(1);
  FE_Q<2> fe2(2);
  FESystem<2> fe(fe1, 2, fe2, 1);

  DoFHandler<2> dof(tr);
  dof.distribute_dofs(fe);
  DoFRenumbering::Cuthill_McKee(dof);
  DoFRenumbering::component_wise(dof);

  ConstraintMatrix constraints;
  DoFTools::make_hanging_node_constraints(dof, constraints);
  constraints.close();

  std::vector<unsigned int> dofs_per_block(fe.n_blocks());
  DoFTools::count_dofs_per_block(dof, dofs_per_block);

  BlockCompressedSparsityPattern c_sparsity(fe.n_blocks(), fe.n_blocks());
  for (unsigned int i=0; i<fe.n_blocks(); ++i)
    for (unsigned int j=0; j<fe.n_blocks(); ++j)
      c_sparsity.block(i,j).reinit(dofs_per_block[i],dofs_per_block[j]);
  c_sparsity.collect_sizes();

  DoFTools::make_sparsity_pattern(dof, c_sparsity);
  constraints.condense(c_sparsity);

  BlockSparsityPattern sparsity;
  sparsity.copy_from(c_sparsity);

  unsigned int ig = 0;
  for (unsigned int ib=0; ib<fe.n_blocks(); ++ib)
    for (unsigned int i=0; i<dofs_per_block[ib]; ++i,++ig)
      {
        unsigned int jg = 0;
        for (unsigned int jb=0; jb<fe.n_blocks(); ++jb)
          for (unsigned int j=0; j<dofs_per_block[jb]; ++j,++jg)
            {
              if (sparsity.exists(ig,jg))
                std::cout << ig << ' ' << jg
                          << '\t' << ib << jb << std::endl;
            }
      }
}


#include <math.h>

#include <boost/foreach.hpp>

#include <ros/ros.h>

// #include <tf/transform_datatypes.h>
#include <eigen_conversions/eigen_msg.h>

#include <pcl_ros/point_cloud.h>

#include <pcl/common/common.h>
#include <pcl/point_cloud.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/extract_polygonal_prism_data.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/pcl_config.h>

#include "sensor_msgs/JointState.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Vector3.h"
#include "geometry_msgs/PointStamped.h"
#include "visualization_msgs/Marker.h"
#include "simple_grasping/shape_extraction.h"
#include "shape_msgs/SolidPrimitive.h"

#include <fetch_delivery_system/BoxTarget.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

// not good practice, might want to get rid of globals in future
ros::Publisher pub_object;
ros::Publisher pub_plane;
ros::Publisher box_marker;
ros::Publisher box_pose_pub;
ros::Publisher box_target;
ros::Publisher plane_centroid;
double head_tilt = 0;

void update_head_angle(const sensor_msgs::JointStateConstPtr &msg)
{
  // get the head tilt angle not sure if the index is fixed?
  head_tilt = msg->position[5];
}

void callback(const PointCloud::ConstPtr &cloud)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);

  // filtering
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(cloud);
  pass.setFilterFieldName("z");
  pass.setFilterLimits(0.0, 1.5);
  //pass.setFilterLimitsNegative (true);
  pass.filter(*cloud_filtered);

  // Plane extraction
  pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // seg.setOptimizeCoefficients (true); // optional, refines estimated plane coeffecients, takes more time
  // Mandatory
  seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setDistanceThreshold(0.02);
  double y = 1.0 * cos(head_tilt);
  double z = 1.0 * sin(head_tilt);
  Eigen::Vector3f axis = Eigen::Vector3f(0.0, y, z);
  seg.setAxis(axis);
  seg.setEpsAngle(5.0 * (M_PI / 180.0));

  seg.setInputCloud(cloud_filtered);
  seg.segment(*inliers, *coefficients);

  if (inliers->indices.size() == 0)
  {
    PCL_ERROR("Could not estimate a planar model for the given dataset.");
    return;
  }

  std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::copyPointCloud(*cloud_filtered, *inliers, *plane_cloud);

  std::cout << "PointCloud representing the Cluster: " << plane_cloud->size() << " data points." << std::endl;

  // publish the plane cloud as a ROS message
  sensor_msgs::PointCloud2 plane_output;
  pcl::toROSMsg(*plane_cloud, plane_output);
  plane_output.header.frame_id = "head_camera_rgb_optical_frame";
  pub_plane.publish(plane_output);

  // find the centroid of the plane
  Eigen::Vector4d plane_centroid4;
  Eigen::Vector3d plane_centroid3;
  geometry_msgs::PointStamped plane_point_stamped;
  pcl::compute3DCentroid(*plane_cloud, plane_centroid4);
  plane_centroid3 = plane_centroid4.head<3>();
  tf::pointEigenToMsg(plane_centroid3, plane_point_stamped.point);
  plane_point_stamped.header.frame_id = "head_camera_rgb_optical_frame";
  // publish centroid of plane
  plane_centroid.publish(plane_point_stamped);

  // Segment object
  pcl::PointIndices::Ptr object_indices(new pcl::PointIndices);
  double z_min = 0.05, z_max = 0.5; // we want the points above the plane, between 5 cm and 50 cm from the surface
  pcl::PointCloud<pcl::PointXYZ>::Ptr hull_points(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::ConvexHull<pcl::PointXYZ> hull;
  // hull.setDimension (2); // not necessarily needed, but we need to check the dimensionality of the output
  hull.setInputCloud(plane_cloud);
  hull.reconstruct(*hull_points);
  if (hull.getDimension() == 2)
  {
    pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism;
    prism.setInputCloud(cloud_filtered);
    prism.setInputPlanarHull(hull_points);
    prism.setHeightLimits(z_min, z_max);
    prism.segment(*object_indices);
  }
  else
  {
    PCL_ERROR("The input cloud does not represent a planar surface.\n");
  }

  // a point cloud of possible objects
  pcl::PointCloud<pcl::PointXYZ>::Ptr object_cloud(new pcl::PointCloud<pcl::PointXYZ>);

  pcl::copyPointCloud(*cloud_filtered, *object_indices, *object_cloud);

  std::cout << "Object cloud: " << object_cloud->size() << " data points." << std::endl;

  if (object_cloud->size() == 0)
  {
    return;
  }

  // Clustering
  // find the clusters
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> euclid;
  euclid.setInputCloud(object_cloud);
  euclid.setClusterTolerance(0.02);
  euclid.setMinClusterSize(100);
  euclid.setMaxClusterSize(25000);
  euclid.extract(cluster_indices);

  // find the closest cluster
  pcl::PointCloud<pcl::PointXYZ>::Ptr nearest_cluster_cloud(new pcl::PointCloud<pcl::PointXYZ>);
  double shortest_distance = INFINITY;
  double current_distance;
  Eigen::Vector4d centroid4;
  Eigen::Vector3d centroid3;
  pcl::PointIndices nearest_cluster_indices;
  for (pcl::PointIndices it : cluster_indices)
  {
    // find centroid of cluster
    pcl::compute3DCentroid(*object_cloud, it, centroid4);
    // std::cerr << "centroid: " << centroid4 << std::endl;
    // compute distance from centroid to head camera
    centroid3 = centroid4.head<3>();
    current_distance = centroid3.norm();
    if (current_distance < shortest_distance)
    {
      // reset shortest distance
      shortest_distance = current_distance;
      // store nearest cluster indicies
      nearest_cluster_indices = it;
    }

  }
  if (nearest_cluster_indices.indices.size() == 0)
  {
    return;
  }
  else
  {
    // create nearest cluster cloud
    pcl::copyPointCloud(*object_cloud, nearest_cluster_indices, *nearest_cluster_cloud);
    std::cerr << "distance: " << shortest_distance << std::endl;
  }

  sensor_msgs::PointCloud2 object_output;
  pcl::toROSMsg(*nearest_cluster_cloud, object_output);
  object_output.header.frame_id = "head_camera_rgb_optical_frame";
  pub_object.publish(object_output);

  // Bounding box
  pcl::PointCloud<pcl::PointXYZRGB> object_cloud_xyzrgb;
  pcl::copyPointCloud(*nearest_cluster_cloud, object_cloud_xyzrgb);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr extract_out(new pcl::PointCloud<pcl::PointXYZRGB>);
  shape_msgs::SolidPrimitive shape;
  geometry_msgs::Pose box_pose;
  // for(auto myc : coefficients->values)
  // {
  //   std::cout << myc << ", ";
  // }
  // std::cout << std::endl;

  // very bad code
  // fixing the normal vector of the plane segmentation
  if (coefficients->values[3] < 0)
  {
    coefficients->values[0] = -coefficients->values[0];
    coefficients->values[1] = -coefficients->values[1];
    coefficients->values[2] = -coefficients->values[2];
    coefficients->values[3] = -coefficients->values[3];
  }

  simple_grasping::extractShape(object_cloud_xyzrgb, coefficients, *extract_out, shape,
                                box_pose);

  if (shape.type == shape_msgs::SolidPrimitive::BOX)
  {
    pcl::PointXYZRGB min;
    pcl::PointXYZRGB max;
    pcl::getMinMax3D<pcl::PointXYZRGB>(*extract_out, min, max);

    visualization_msgs::Marker object_marker;
    object_marker.ns = "objects";
    object_marker.id = 0;
    object_marker.header.frame_id = "head_camera_rgb_optical_frame";
    object_marker.type = visualization_msgs::Marker::CUBE;
    object_marker.color.g = 1;
    object_marker.color.a = 0.3;

    object_marker.pose = box_pose;

    object_marker.scale.x = shape.dimensions[0];
    object_marker.scale.y = shape.dimensions[1];
    object_marker.scale.z = shape.dimensions[2];
    box_marker.publish(object_marker);
    box_pose_pub.publish(box_pose);
    fetch_delivery_system::BoxTarget box_target_msg;
    box_target_msg.box_scale = object_marker;
    box_target_msg.box_pose = box_pose;
    box_target.publish(box_target_msg);
  }
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  pub_object = nh.advertise<PointCloud>("points2_object", 1);
  pub_plane = nh.advertise<PointCloud>("points2_plane", 1);
  box_marker = nh.advertise<visualization_msgs::Marker>("box_marker", 1);
  box_pose_pub = nh.advertise<geometry_msgs::Pose>("box_target_pose", 1);
  box_target = nh.advertise<fetch_delivery_system::BoxTarget>("box_target", 1);
  plane_centroid = nh.advertise<geometry_msgs::PointStamped>("plane_centroid", 1);
  ros::Subscriber sub = nh.subscribe<PointCloud>("/head_camera/depth_downsample/points", 1, callback);
  ros::Subscriber joint_sub = nh.subscribe<sensor_msgs::JointState>("joint_states", 1, update_head_angle);
  ros::spin();
}


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

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

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

#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/mhalf.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/oneplus.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0,
               class style,
               class base_A0 = bd::scalar_of_t<A0>
             >
    struct trig_evaluation {};

    // This class exposes the public static members:
    // sin_eval
    // cos_eval
    // tan_eval
    // which evaluate a polynomial approximation of each standard trigonometric
    // functions in the range [pi/4, -pi/4]

    template < class A0> struct trig_evaluation < A0,  tag::not_simd_type, float>
    {
      static BOOST_FORCEINLINE A0 cos_eval(A0 z) BOOST_NOEXCEPT
      {
        const A0 y = horn<A0
          , 0x3d2aaaa5
          , 0xbab60619
          , 0x37ccf5ce
          > (z);
        return oneplus( fma(z,Mhalf<A0>(), y* sqr(z)));
      }

      static BOOST_FORCEINLINE A0 sin_eval(A0 z, A0 x) BOOST_NOEXCEPT
      {
        const A0 y1 = horn<A0
          , 0xbe2aaaa2
          , 0x3c08839d
          , 0xb94ca1f9
          > (z);
        return fma(y1*z,x,x);
      }

      static BOOST_FORCEINLINE A0 base_tan_eval(A0 z) BOOST_NOEXCEPT
      {
        const A0 zz = sqr(z);
        A0 y = horn<A0,
          0x3eaaaa6f,
          0x3e0896dd,
          0x3d5ac5c9,
          0x3cc821b5,
          0x3b4c779c,
          0x3c19c53b
          >(zz)*zz*z+z;
        return y;
      }

      static BOOST_FORCEINLINE A0 tan_eval(A0 z, const int n) BOOST_NOEXCEPT
      {
        const A0 y = base_tan_eval(z);
        if (n == 1) return y;  else return -rec(y);
      }
      static BOOST_FORCEINLINE A0 cot_eval(A0 z, const int n) BOOST_NOEXCEPT
      {
        const A0 y = base_tan_eval(z);
        if (n == 1) return rec(y);  else return -y;
      }
    };
  }
} }
#endif


/* ----------------------------------------------------------------------
 *
 *                    *** Smooth Mach Dynamics ***
 *
 * This file is part of the USER-SMD package for LAMMPS.
 * Copyright (2014) Georg C. Ganzenmueller, georg.ganzenmueller@emi.fhg.de
 * Fraunhofer Ernst-Mach Institute for High-Speed Dynamics, EMI,
 * Eckerstrasse 4, D-79104 Freiburg i.Br, Germany.
 *
 * ----------------------------------------------------------------------- */

/* ----------------------------------------------------------------------
 LAMMPS - Large-scale Atomic/Molecular Massively Parallel Simulator
 http://lammps.sandia.gov, Sandia National Laboratories
 Steve Plimpton, sjplimp@sandia.gov

 Copyright (2003) Sandia Corporation.  Under the terms of Contract
 DE-AC04-94AL85000 with Sandia Corporation, the U.S. Government retains
 certain rights in this software.  This software is distributed under
 the GNU General Public License.

 See the README file in the top-level LAMMPS directory.
 ------------------------------------------------------------------------- */
#include <iostream>
#include "math_special.h"
#include <stdio.h>

#include <Eigen/Eigen>

using namespace LAMMPS_NS::MathSpecial;
using namespace std;
using namespace Eigen;

#define MIN(A,B) ((A) < (B) ? (A) : (B))
#define MAX(A,B) ((A) > (B) ? (A) : (B))

/* ----------------------------------------------------------------------
 linear EOS for use with linear elasticity
 input: initial pressure pInitial, isotropic part of the strain rate d, time-step dt
 output: final pressure pFinal, pressure rate p_rate
 ------------------------------------------------------------------------- */
void LinearEOS(double lambda, double pInitial, double d, double dt, double &pFinal, double &p_rate) {

	/*
	 * pressure rate
	 */
	p_rate = lambda * d;

	pFinal = pInitial + dt * p_rate; // increment pressure using pressure rate
	//cout << "hurz" << endl;

}

/* ----------------------------------------------------------------------
 shock EOS
 input:
 current density rho
 reference density rho0
 current energy density e
 reference energy density e0
 reference speed of sound c0
 shock Hugoniot parameter S
 Grueneisen parameter Gamma
 initial pressure pInitial
 time step dt

 output:
 pressure rate p_rate
 final pressure pFinal

 ------------------------------------------------------------------------- */
void ShockEOS(double rho, double rho0, double e, double e0, double c0, double S, double Gamma, double pInitial, double dt,
		double &pFinal, double &p_rate) {

	double mu = rho / rho0 - 1.0;
	double pH = rho0 * square(c0) * mu * (1.0 + mu) / square(1.0 - (S - 1.0) * mu);

	pFinal = (pH + rho * Gamma * (e - e0));

	//printf("shock EOS: rho = %g, rho0 = %g, Gamma=%f, c0=%f, S=%f, e=%f, e0=%f\n", rho, rho0, Gamma, c0, S, e, e0);
	//printf("pFinal = %f\n", pFinal);
	p_rate = (pFinal - pInitial) / dt;

}

/* ----------------------------------------------------------------------
 polynomial EOS
 input:
 current density rho
 reference density rho0
 coefficients 0 .. 6
 initial pressure pInitial
 time step dt

 output:
 pressure rate p_rate
 final pressure pFinal

 ------------------------------------------------------------------------- */
void polynomialEOS(double rho, double rho0, double e, double C0, double C1, double C2, double C3, double C4, double C5, double C6,
		double pInitial, double dt, double &pFinal, double &p_rate) {

	double mu = rho / rho0 - 1.0;

	if (mu > 0.0) {
		pFinal = C0 + C1 * mu + C2 * mu * mu + C3 * mu * mu * mu; // + (C4 + C5 * mu + C6 * mu * mu) * e;
	} else {
		pFinal = C0 + C1 * mu + C3 * mu * mu * mu; //  + (C4 + C5 * mu) * e;
	}
	pFinal = -pFinal; // we want the mean stress, not the pressure.

	//printf("pFinal = %f\n", pFinal);
	p_rate = (pFinal - pInitial) / dt;

}

/* ----------------------------------------------------------------------
 Tait EOS based on current density vs. reference density.

 input: (1) reference sound speed
 (2) equilibrium mass density
 (3) current mass density

 output:(1) pressure
 (2) current speed of sound
 ------------------------------------------------------------------------- */
void TaitEOS_density(const double exponent, const double c0_reference, const double rho_reference, const double rho_current,
		double &pressure, double &sound_speed) {

	double B = rho_reference * c0_reference * c0_reference / exponent;
	double tmp = pow(rho_current / rho_reference, exponent);
	pressure = B * (tmp - 1.0);
	double bulk_modulus = B * tmp * exponent; // computed as rho * d(pressure)/d(rho)
	sound_speed = sqrt(bulk_modulus / rho_current);

//	if (fabs(pressure) > 0.01) {
//		printf("tmp = %f, press=%f, K=%f\n", tmp, pressure, bulk_modulus);
//	}

}

/* ----------------------------------------------------------------------
 perfect gas EOS
 input: gamma -- adiabatic index (ratio of specific heats)
 J -- determinant of deformation gradient
 volume0 -- reference configuration volume of particle
 energy -- energy of particle
 pInitial -- initial pressure of the particle
 d -- isotropic part of the strain rate tensor,
 dt -- time-step size

 output: final pressure pFinal, pressure rate p_rate
 ------------------------------------------------------------------------- */
void PerfectGasEOS(const double gamma, const double vol, const double mass, const double energy, double &pFinal, double &c0) {

	/*
	 * perfect gas EOS is p = (gamma - 1) rho e
	 */

	if (energy > 0.0) {

		pFinal = (1.0 - gamma) * energy / vol;
//printf("gamma = %f, vol%f, e=%g ==> p=%g\n", gamma, vol, energy, *pFinal__/1.0e-9);

		c0 = sqrt((gamma - 1.0) * energy / mass);

	} else {
		pFinal = c0 = 0.0;
	}

}

/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void LinearStrength(const double mu, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev, const double dt,
		Matrix3d &sigmaFinal_dev__, Matrix3d &sigma_dev_rate__) {

	/*
	 * deviatoric rate of unrotated stress
	 */
	sigma_dev_rate__ = 2.0 * mu * d_dev;

	/*
	 * elastic update to the deviatoric stress
	 */
	sigmaFinal_dev__ = sigmaInitial_dev + dt * sigma_dev_rate__;
}

/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: F: deformation gradient
 output:  total stress tensor, deviator + pressure
 ------------------------------------------------------------------------- */
//void PairTlsph::LinearStrengthDefgrad(double lambda, double mu, Matrix3d F, Matrix3d *T) {
//	Matrix3d E, PK2, eye, sigma, S, tau;
//
//	eye.setIdentity();
//
//	E = 0.5 * (F * F.transpose() - eye); // strain measure E = 0.5 * (B - I) = 0.5 * (F * F^T - I)
//	tau = lambda * E.trace() * eye + 2.0 * mu * E; // Kirchhoff stress, work conjugate to above strain
//	sigma = tau / F.determinant(); // convert Kirchhoff stress to Cauchy stress
//
////printf("l=%f, mu=%f, sigma xy = %f\n", lambda, mu, sigma(0,1));
//
////    E = 0.5 * (F.transpose() * F - eye); // Green-Lagrange Strain E = 0.5 * (C - I)
////    S = lambda * E.trace() * eye + 2.0 * mu * Deviator(E); // PK2 stress
////    tau = F * S * F.transpose(); // convert PK2 to Kirchhoff stress
////    sigma = tau / F.determinant();
//
//	//*T = sigma;
//
//	/*
//	 * neo-hookean model due to Bonet
//	 */
////    lambda = mu = 100.0;
////    // left Cauchy-Green Tensor, b = F.F^T
//	double J = F.determinant();
//	double logJ = log(J);
//	Matrix3d b;
//	b = F * F.transpose();
//
//	sigma = (mu / J) * (b - eye) + (lambda / J) * logJ * eye;
//	*T = sigma;
//}
/* ----------------------------------------------------------------------
 linear strength model for use with linear elasticity
 input: lambda, mu : Lame parameters
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void LinearPlasticStrength(const double G, const double yieldStress, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev,
		const double dt, Matrix3d &sigmaFinal_dev__, Matrix3d &sigma_dev_rate__, double &plastic_strain_increment) {

	Matrix3d sigmaTrial_dev, dev_rate;
	double J2;

	/*
	 * deviatoric rate of unrotated stress
	 */
	dev_rate = 2.0 * G * d_dev;

	/*
	 * perform a trial elastic update to the deviatoric stress
	 */
	sigmaTrial_dev = sigmaInitial_dev + dt * dev_rate; // increment stress deviator using deviatoric rate

	/*
	 * check yield condition
	 */
	J2 = sqrt(3. / 2.) * sigmaTrial_dev.norm();

	if (J2 < yieldStress) {
		/*
		 * no yielding has occured.
		 * final deviatoric stress is trial deviatoric stress
		 */
		sigma_dev_rate__ = dev_rate;
		sigmaFinal_dev__ = sigmaTrial_dev;
		plastic_strain_increment = 0.0;
		//printf("no yield\n");

	} else {
		//printf("yiedl\n");
		/*
		 * yielding has occured
		 */
		plastic_strain_increment = (J2 - yieldStress) / (3.0 * G);

		/*
		 * new deviatoric stress:
		 * obtain by scaling the trial stress deviator
		 */
		sigmaFinal_dev__ = (yieldStress / J2) * sigmaTrial_dev;

		/*
		 * new deviatoric stress rate
		 */
		sigma_dev_rate__ = sigmaFinal_dev__ - sigmaInitial_dev;
		//printf("yielding has occured.\n");
	}
}

/* ----------------------------------------------------------------------
 Johnson Cook Material Strength model
 input:
 G : shear modulus
 cp : heat capacity
 espec : energy / mass
 A : initial yield stress under quasi-static / room temperature conditions
 B : proportionality factor for plastic strain dependency
 a : exponent for plastic strain dpendency
 C : proportionality factor for logarithmic plastic strain rate dependency
 epdot0 : dimensionality factor for plastic strain rate dependency
 T : current temperature
 T0 : reference (room) temperature
 Tmelt : melting temperature
 input: sigmaInitial_dev, d_dev: initial stress deviator, deviatoric part of the strain rate tensor
 input: dt: time-step
 output:  sigmaFinal_dev, sigmaFinal_dev_rate__: final stress deviator and its rate.
 ------------------------------------------------------------------------- */
void JohnsonCookStrength(const double G, const double cp, const double espec, const double A, const double B, const double a,
		const double C, const double epdot0, const double T0, const double Tmelt, const double M, const double dt, const double ep,
		const double epdot, const Matrix3d sigmaInitial_dev, const Matrix3d d_dev, Matrix3d &sigmaFinal_dev__,
		Matrix3d &sigma_dev_rate__, double &plastic_strain_increment) {

	Matrix3d sigmaTrial_dev, dev_rate;
	double J2, yieldStress;

	double deltaT = espec / cp;
	double TH = deltaT / (Tmelt - T0);
	TH = MAX(TH, 0.0);
	double epdot_ratio = epdot / epdot0;
	epdot_ratio = MAX(epdot_ratio, 1.0);
	//printf("current temperature delta is %f, TH=%f\n", deltaT, TH);

	yieldStress = (A + B * pow(ep, a)) * (1.0 + C * log(epdot_ratio)); // * (1.0 - pow(TH, M));

	/*
	 * deviatoric rate of unrotated stress
	 */
	dev_rate = 2.0 * G * d_dev;

	/*
	 * perform a trial elastic update to the deviatoric stress
	 */
	sigmaTrial_dev = sigmaInitial_dev + dt * dev_rate; // increment stress deviator using deviatoric rate

	/*
	 * check yield condition
	 */
	J2 = sqrt(3. / 2.) * sigmaTrial_dev.norm();

	if (J2 < yieldStress) {
		/*
		 * no yielding has occured.
		 * final deviatoric stress is trial deviatoric stress
		 */
		sigma_dev_rate__ = dev_rate;
		sigmaFinal_dev__ = sigmaTrial_dev;
		plastic_strain_increment = 0.0;
		//printf("no yield\n");

	} else {
		//printf("yiedl\n");
		/*
		 * yielding has occured
		 */
		plastic_strain_increment = (J2 - yieldStress) / (3.0 * G);

		/*
		 * new deviatoric stress:
		 * obtain by scaling the trial stress deviator
		 */
		sigmaFinal_dev__ = (yieldStress / J2) * sigmaTrial_dev;

		/*
		 * new deviatoric stress rate
		 */
		sigma_dev_rate__ = sigmaFinal_dev__ - sigmaInitial_dev;
		//printf("yielding has occured.\n");
	}
}

/* ----------------------------------------------------------------------
 isotropic maximum strain damage model
 input:
 current strain
 maximum value of allowed principal strain

 output:
 return value is true if any eigenvalue of the current strain exceeds the allowed principal strain

 ------------------------------------------------------------------------- */

bool IsotropicMaxStrainDamage(const Matrix3d E, const double maxStrain) {

	/*
	 * compute Eigenvalues of strain matrix
	 */
	SelfAdjointEigenSolver < Matrix3d > es;
	es.compute(E); // compute eigenvalue and eigenvectors of strain

	double max_eigenvalue = es.eigenvalues().maxCoeff();

	if (max_eigenvalue > maxStrain) {
		return true;
	} else {
		return false;
	}
}

/* ----------------------------------------------------------------------
 isotropic maximum stress damage model
 input:
 current stress
 maximum value of allowed principal stress

 output:
 return value is true if any eigenvalue of the current stress exceeds the allowed principal stress

 ------------------------------------------------------------------------- */

bool IsotropicMaxStressDamage(const Matrix3d S, const double maxStress) {

	/*
	 * compute Eigenvalues of strain matrix
	 */
	SelfAdjointEigenSolver < Matrix3d > es;
	es.compute(S); // compute eigenvalue and eigenvectors of strain

	double max_eigenvalue = es.eigenvalues().maxCoeff();

	if (max_eigenvalue > maxStress) {
		return true;
	} else {
		return false;
	}
}

/* ----------------------------------------------------------------------
 Johnson-Cook failure model
 input:

 output:

 ------------------------------------------------------------------------- */

double JohnsonCookFailureStrain(const double p, const Matrix3d Sdev, const double d1, const double d2, const double d3,
		const double d4, const double epdot0, const double epdot) {

	double vm = sqrt(3. / 2.) * Sdev.norm(); // von-Mises equivalent stress
	if (vm < 0.0) {
		cout << "this is sdev " << endl << Sdev << endl;
		printf("vm=%f < 0.0, surely must be an error\n", vm);
		exit(1);
	}

	// determine stress triaxiality
	double triax = p / (vm + 0.01 * fabs(p)); // have softening in denominator to avoid divison by zero
	if (triax < 0.0) {
		triax = 0.0;
	} else if (triax > 3.0) {
		triax = 3.0;
	}

	// Johnson-Cook failure strain, dependence on stress triaxiality
	double jc_failure_strain = d1 + d2 * exp(d3 * triax);

	// include strain rate dependency if parameter d4 is defined and current plastic strain rate exceeds reference strain rate
	if (d4 > 0.0) { //
		if (epdot > epdot0) {
			double epdot_ratio = epdot / epdot0;
			jc_failure_strain *= (1.0 + d4 * log(epdot_ratio));
			//printf("epsdot=%f, epsdot0=%f, factor = %f\n", epdot, epdot0, (1.0 + d4 * log(epdot_ratio)));
			//exit(1);

		}
	}

	return jc_failure_strain;

}


  //==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_SIMD_VMX_ALTIVEC_FAST_RSQRT_HPP_INCLUDED
#define BOOST_SIMD_ARITHMETIC_FUNCTIONS_SIMD_VMX_ALTIVEC_FAST_RSQRT_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/arithmetic/functions/rsqrt.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/fma.hpp>
#include <boost/simd/include/functions/simd/sqr.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( fast_rsqrt_
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((simd_ < single_<A0>
                                              , boost::simd::tag::vmx_
                                              >
                                      ))
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL(1)
    {
      result_type hf = Half<result_type>();
      result_type o  = One<result_type>();

      result_type estimate = vec_rsqrte( a0() );
      result_type se = sqr(estimate);
      result_type he = estimate*hf;
      result_type st = vec_nmsub(a0(),se(),o());

      return fma( st, he, estimate);
    }
  };
} } }

#endif

#endif


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

    @copyright 2016 NumScale SAS

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

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

  /*!
    @ingroup group-trigonometric
    This function object returns the inverse sine.

    @par Header <boost/simd/function/asin.hpp>

    @par Note

      For every parameter of floating type `asin(x)`
      returns the arc @c r in the interval  \f$[-\pi/2, \pi/2]\f$ such that
      <tt>sin(r) == x</tt>.  If @c x is outside \f$[-1, 1]\f$ the result is Nan.

    @par Decorators

      - std_ for floating entries provides access to std::asin

    @see asind, asinpi, sin

    @par Example:

      @snippet asin.cpp asin

    @par Possible output:

      @snippet asin.txt asin

  **/
  IEEEValue asin(IEEEValue const & x);
} }
#endif

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

#endif