#include "mesh_components.hpp"
#include "plane3d.hpp"
#include <vector>
#include <functional>
#include <array>
#include <iostream>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

Tetrahedron::Tetrahedron(const int _id, const vector<reference_wrapper<Vertex3d>> _vertices, const double _weight, const int _label){
    id = _id;
    vertices = _vertices;
    weight = _weight;
    label = _label;
    //cout << vertices[0].get().Vec() << endl;

    Vector3d v0 = vertices[0].get().Vec();
    Vector3d v1 = vertices[1].get().Vec();
    Vector3d v2 = vertices[2].get().Vec();
    Vector3d v3 = vertices[3].get().Vec();

    //array<double, 3> faceCentroid = {(v0[0] + v1[0] + v2[0]) / 3, (v0[1] + v1[1] + v2[1]) / 3, (v0[2] + v1[2] + v2[2]) / 3};

    //Vector3f pt3 = vertices[3]->Vec();

    sphereCenter = { (v0[0] + v1[0] + v2[0] + v3[0])/4, (v0[1] + v1[1] + v2[1] + v3[1])/4, (v0[2] + v1[2] + v2[2] + v3[2])/4};

    double r0 = sqrt( pow(sphereCenter[0]-v0[0], 2) + pow(sphereCenter[1]-v0[1], 2) + pow(sphereCenter[2]-v0[2], 2));

    double r1 = sqrt( pow(sphereCenter[0]-v1[0], 2) + pow(sphereCenter[1]-v1[1], 2) + pow(sphereCenter[2]-v1[2], 2));

    double r2 = sqrt( pow(sphereCenter[0]-v2[0], 2) + pow(sphereCenter[1]-v2[1], 2) + pow(sphereCenter[2]-v2[2], 2));

    double r3 = sqrt( pow(sphereCenter[0]-v3[0], 2) + pow(sphereCenter[1]-v3[1], 2) + pow(sphereCenter[2]-v3[2], 2));

    double radii [4] = {r0, r1, r2, r3};

    double r = r0;

    for(int i=1; i<4; ++i){
	//float tmp_r = ;
	if (radii[i] > r){
	    r = radii[i];
	}
    }

    sphereRadius = r;
}

void Tetrahedron::addNeighbor(unsigned long int neighborId){
    neighbors.push_back(neighborId);
}

Face::Face(const vector<reference_wrapper<Vertex3d>> _vertices, unsigned long int _tetId){
    vertices = _vertices;
    tetId=_tetId;
}

bool Tetrahedron::contains(const array<double, 3> pt_target){
    if (sqrt( pow(pt_target[0] - sphereCenter[0], 2) + pow(pt_target[1] - sphereCenter[1], 2) + pow(pt_target[2] - sphereCenter[2], 2)) < sphereRadius){
        /*cout << pt_target[0] << endl;
        cout << pt_target[1] << endl;
        cout << pt_target[2] << endl;
        cout << "HERE" << endl;*/

	Vector3d pt0 = {vertices[0].get().Vec()};
	Vector3d pt1 = {vertices[1].get().Vec()};
	Vector3d pt2 = {vertices[2].get().Vec()};
	Vector3d pt3 = {vertices[3].get().Vec()};

	/*cout << "pt0" << endl;
	cout << pt0 << endl;
	cout << "pt1" << endl;
	cout << pt1 << endl;
	cout << "pt2" << endl;
	cout << pt2 << endl;
	cout << "pt3" << endl;
	cout << pt3 << endl;*/

	Matrix4d m0;
	Matrix4d m1;
	Matrix4d m2;
	Matrix4d m3;
	Matrix4d m4;

	m0 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d0 = m0.determinant();

	m1 << pt_target[0], pt_target[1], pt_target[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d1 = m1.determinant();

	m2 << pt0[0], pt0[1], pt0[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d2 = m2.determinant();

	m3 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1,
	      pt3[0], pt3[1], pt3[2], 1;

	double d3 = m3.determinant();

	m4 << pt0[0], pt0[1], pt0[2], 1,
	      pt1[0], pt1[1], pt1[2], 1,
	      pt2[0], pt2[1], pt2[2], 1,
	      pt_target[0], pt_target[1], pt_target[2], 1;

	double d4 = m4.determinant();

	/*cout << d0 << endl;
	cout << d1 << endl;
	cout << d2 << endl;
	cout << d3 << endl;
	cout << d4 << endl;*/

	return (d0<0 && d1<0 && d2<0 && d3<0 && d4<0) || (d0>0 && d1>0 && d2>0 && d3>0 && d4>0);
    }
    return false;
}

vector<array<double, 3>> Tetrahedron::intersectsPlane(Plane3d plane){
    vector<array<double, 3>> intersectionPoints;

    for(int i=0; i<4; ++i){
	Vector3d v0 = vertices[i].get().Vec();
	if(plane.containsPoint(v0)){
	    array<double, 3> v0Pt = {v0[0], v0[1], v0[2]};
	    intersectionPoints.push_back(v0Pt);
	}
	else {
	    for(int j=i+1; j<4; ++j){
		Vector3d v1 = vertices[j].get().Vec();
		if(plane.intersectsEdge(v0, v1)){
		    intersectionPoints.push_back(plane.findIntersection(v0,v1));
		}
	    }
	}
    }

    if(intersectionPoints.size()==4){
	intersectionPoints = {intersectionPoints[0], intersectionPoints[1], intersectionPoints[3], intersectionPoints[2]};
    }

    return intersectionPoints;

}

bool Face::intersectsPlane(Plane3d plane){
    Vector3d v0 = vertices[0].get().Vec();
    Vector3d v1 = vertices[1].get().Vec();
    Vector3d v2 = vertices[2].get().Vec();

    return (plane.intersectsEdge(v0, v1) || plane.intersectsEdge(v1, v2) || plane.intersectsEdge(v0, v2));
}

unsigned long int Tetrahedron::Id(){
    return id;
}

vector<unsigned long int> Tetrahedron::Neighbors(){
    return neighbors;
}

vector<reference_wrapper<Vertex3d>> Tetrahedron::Vertices(){
    return vertices;
}

double Tetrahedron::Weight(){
    return weight;
}

int Tetrahedron::Label(){
    return label;
}

unsigned long int Face::TetId(){
    return tetId;
}

vector<reference_wrapper<Vertex3d>> Face::Vertices(){
    return vertices;
}

Vertex3d::Vertex3d(array<double, 3> _vec) {
    //vec = _vec;
    vec << _vec[0], _vec[1], _vec[2];
}

Vector3d Vertex3d::Vec(){
    return vec;
}


#include <iostream>
#include <fstream>
#include <stdio.h>
#include <math.h>
#include <omp.h>
#include <chrono>

#include <random>
#include <map>
#include <string>
#include <iomanip>

#include <unistd.h>
#include <string>
#include <algorithm>
#include <random>

#include <Eigen/Core>

#include <boost/program_options.hpp>
#include <iterator>

#include "BayesC_distributions.h"
#include "Sampling_functions.hpp"

using namespace std;
using namespace Eigen;
namespace po = boost::program_options;

void ReadFromFile(std::vector<double> &x, const std::string &file_name)
{
	std::ifstream read_file(file_name);
	assert(read_file.is_open());

	std::copy(std::istream_iterator<double>(read_file), std::istream_iterator<double>(),
			std::back_inserter(x));

	read_file.close();
}

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

	po::options_description desc("Options");
	desc.add_options()
		("M", po::value<int>()->required(), "No. of simulated markers")
		("N", po::value<int>()->required(), "No. of simulated individuals")
		("iter", po::value<int>()->default_value(5000), "No. of Gibbs iterations")
		("pNZ", po::value<double>()->default_value(0.5), "Proportion nonzero (simulations)")
		("h2", po::value<double>()->default_value(0.6), "Heritability (simulations)")
		("input", po::value<std::string>()->default_value("none"),"Input filename")
		("out", po::value<std::string>()->default_value("BayesC_out"),"Output filename")
	;

	//clock starts
	std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();

	//map variables
	po::variables_map vm;
	po::store(po::parse_command_line(argc,argv,desc),vm);
	po::notify(vm);

	int M=vm["M"].as<int>();
	int N=vm["N"].as<int>();
	int iter=vm["iter"].as<int>();

	string input=vm["input"].as<string>();
	string output=vm["out"].as<string>();

	MatrixXd X(N,M);
	VectorXd Y(N);

	//beta coefficients
	VectorXd beta_true(M);
	beta_true.setZero();

	int i,j,k,l,m=0;

	//Was an input matrix given?

	if (input!="none"){ //Either read input tables for X and Y
		ifstream f1(input+".X");
		//f1 >> m >> n;
		for (int i = 0; i < N; i++)
		{
			for (int j = 0; j < M; j++)
			{
				f1 >> X(i,j);
				//cout<<X(i,j)<<endl;
			}
		}
		f1.close();
		cout<<"finished reading matrix X!"<<endl;

		std::vector<double> Y_in;
		ReadFromFile(Y_in, input+".Y");
		double* ptr_Y = &Y_in[0];
		Eigen::Map<Eigen::VectorXd> Y1(ptr_Y, Y_in.size());
		cout<<"finished reading vector Y!"<<endl;
		if(Y_in.size()!=N){cout<<"input Y vector size doesnt much the size indicated in the command line"<<endl;return 0;}
		Y=Y1;
		Y_in.clear();

	}else //or simulate
	{
		double h2=vm["h2"].as<double>();
		double pNZ=vm["pNZ"].as<double>();
		double sigmaY_true=1;

		int MT=pNZ*M;

		//Fill Genotype matrix
		for (i=0;i<N;i++){
			for (j=0;j<M;j++){
				X(i,j)=rnorm(0,1);
			}
		}
		for (i=0;i<MT;i++){
			beta_true[i]=rnorm(0,sqrt(h2/MT));
		}

		//error
		VectorXd error(N);
		for (i=0;i<N;i++){
			error[i]=rnorm(0,sqrt(1-h2));
		}

		//construct phenotypes
		Y=X*beta_true;
		Y+=error;
	}
	//standardize matrix X
	RowVectorXd mean = X.colwise().mean();
	RowVectorXd sd = ((X.rowwise() - mean).array().square().colwise().sum() / (X.rows() - 1)).sqrt();
	X = (X.rowwise() - mean).array().rowwise() / sd.array();

	//standardize vector Y
    Y = (Y.array() - Y.array().mean());
    Y /= sqrt(Y.squaredNorm() / (double(N - 1)));

    //Initialize variables
	double Emu=0;
	VectorXd vEmu(N);
	vEmu.setOnes();

	VectorXd Ebeta(M);
	Ebeta.setZero();
	VectorXd ny(M);
	ny.setZero();
	double Ew=0.5;
	//residual error
	VectorXd epsilon(N);

	epsilon=Y-X*Ebeta-vEmu*Emu;

	std::vector<int> markerI;
	for (int i=0; i<M; ++i) {
		markerI.push_back(i);
	}
	int marker=0;

	int NZ=0;

	double Esigma2=epsilon.squaredNorm()/(N*0.5);
	double Epsi2=rbeta(1,1);

	//Standard parameterization of hyperpriors for variances
	//double v0E=0.001,s0E=0.001,v0B=0.001,s0B=0.001;

	// Alternative parameterization of hyperpriors for variances
	double v0E=4,v0B=4;
	double s0B=((v0B-2)/v0B)*Epsi2;
	double s0E=((v0E-2)/v0E)*Esigma2;

	//pre-computed elements for calculations
	VectorXd el1(M);
	for (int i=0; i<M; ++i) {
		el1[i]=X.col(i).transpose()*X.col(i);
	}

	//open files for writing
	std::ofstream ofs;
	ofs.open(output+"_estimates.txt");
	for (int i=0; i<M; ++i) {
		ofs << "beta_" <<i<< ' ';
	}
	for (int i=0; i<M; ++i) {
		ofs << "incl_" <<i<< ' ';
	}
	ofs << "Ew" << " ";
	ofs << "Epsi2" << " ";
	ofs << "Esigma2" << " ";
	ofs << "\n";
	ofs.close();

	std::chrono::steady_clock::time_point end1= std::chrono::steady_clock::now();
	std::cout << "Time taken for Reading/generating data = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end1 - begin).count()*1e-9 <<" seconds"<<std::endl;

	//begin GIBBS sampling iterations

	ofs.open (output+"_estimates.txt", std::ios_base::app);
	for (i=0;i<iter;i++){

		Emu=sample_mu(N,Esigma2,Y,X,Ebeta);

		//sample effects and probabilities jointly
		std::random_shuffle(markerI.begin(), markerI.end());

		for (j=0;j<M;j++){
			marker=markerI[j];

			epsilon=epsilon+X.col(marker)*Ebeta[marker];

			double Cj=el1[marker]+Esigma2/Epsi2;
			double rj=X.col(marker).transpose()*epsilon;

			double ratio=(((exp(-(pow(rj,2))/(2*Cj*Esigma2))*sqrt((Epsi2*Cj)/Esigma2))));
			ratio=Ew/(Ew+ratio*(1-Ew));
			ny[marker]=rbernoulli(ratio);

			if (ny[marker]==0){
				Ebeta[marker]=0;
			}
			else if (ny[marker]==1){
				Ebeta[marker]=rnorm(rj/Cj,Esigma2/Cj);
			}

			epsilon=epsilon-X.col(marker)*Ebeta[marker];

		}
		for (j=0;j<M;j++){
			ofs << Ebeta[j] << " ";
		}
		for (j=0;j<M;j++){
			ofs << ny[j] << " ";
		}
		NZ=ny.sum();
		//cout<<NZ<<endl;

		Ew=sample_w(M,NZ);
		epsilon=Y-X*Ebeta-vEmu*Emu;

		Epsi2=sample_psi2_chisq(Ebeta,NZ,v0B,s0B);
		Esigma2=sample_sigma_chisq(N,epsilon,v0E,s0E);

		ofs << Ew << " ";
		ofs << Epsi2 << " ";
		ofs << Esigma2 << " ";
		ofs << "\n";

	}
	ofs.close();
//write out simulated data
if (input=="none"){
	//write to files
	ofstream myfile1;
	myfile1.open (output+"_simulated_Y.txt");
	for (i=0;i<N;i++){
		myfile1 << Y[i] << ' ';
	}
	myfile1 << endl;
	myfile1.close();
/*
	ofstream myfile2;
	myfile2.open (output+"_simulated_X.txt");
	for (i=0;i<N;i++){
		for (j=0;j<M;j++){
			myfile2<<X(i,j)<< ' ';
		}
		myfile2<<endl;
	}
	myfile2.close();
*/
	ofstream myfile3;
	myfile3.open (output+"_simulated_betatrue.txt");
	myfile3 << beta_true << ' ';
	myfile3.close();
}

std::chrono::steady_clock::time_point end2= std::chrono::steady_clock::now();
std::cout << "Time taken for full analysis = " << std::chrono::duration_cast<std::chrono::nanoseconds> (end2 - begin).count()*1e-9 <<" seconds"<<std::endl;

	return 0;
}


// ---------------------------------------------------------------------
//
// Copyright (c) 2017 - 2022 by the IBAMR developers
// All rights reserved.
//
// This file is part of IBAMR.
//
// IBAMR is free software and is distributed under the 3-clause BSD
// license. The full text of the license can be found in the file
// COPYRIGHT at the top level directory of IBAMR.
//
// ---------------------------------------------------------------------

// Config files
#include <SAMRAI_config.h>

// Headers for basic PETSc functions
#include <petscsys.h>

// Headers for basic SAMRAI objects
#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <LoadBalancer.h>
#include <StandardTagAndInitialize.h>

// Headers for basic libMesh objects
#include <libmesh/boundary_info.h>
#include <libmesh/equation_systems.h>
#include <libmesh/exodusII_io.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>
#include <libmesh/replicated_mesh.h>

// Headers for application-specific algorithm/data structure objects
#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFECentroidPostProcessor.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>

#include <ibtk/AppInitializer.h>
#include <ibtk/BoxPartitioner.h>
#include <ibtk/IBTKInit.h>
#include <ibtk/IBTK_MPI.h>
#include <ibtk/libmesh_utilities.h>
#include <ibtk/muParserCartGridFunction.h>
#include <ibtk/muParserRobinBcCoefs.h>

#include <boost/multi_array.hpp>

// Set up application namespace declarations
#include <ibamr/app_namespaces.h>

// Elasticity model data.
namespace ModelData
{
// Coordinate mapping function.
void
coordinate_mapping_function(libMesh::Point& X, const libMesh::Point& s, void* /*ctx*/)
{
    X(0) = s(0) + 0.6;
    X(1) = s(1) + 0.5;
#if (NDIM == 3)
    X(2) = s(2) + 0.5;
#endif
    return;
} // coordinate_mapping_function

// Stress tensor functions.
static double c1_s = 0.05;
static double p0_s = 0.0;
static double beta_s = 0.0;
void
PK1_dev_stress_function(TensorValue<double>& PP,
                        const TensorValue<double>& FF,
                        const libMesh::Point& /*X*/,
                        const libMesh::Point& /*s*/,
                        Elem* const /*elem*/,
                        const vector<const vector<double>*>& /*var_data*/,
                        const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                        double /*time*/,
                        void* /*ctx*/)
{
    PP = 2.0 * c1_s * FF;
    return;
} // PK1_dev_stress_function

void
PK1_dil_stress_function(TensorValue<double>& PP,
                        const TensorValue<double>& FF,
                        const libMesh::Point& /*X*/,
                        const libMesh::Point& /*s*/,
                        Elem* const /*elem*/,
                        const vector<const vector<double>*>& /*var_data*/,
                        const vector<const vector<VectorValue<double> >*>& /*grad_var_data*/,
                        double /*time*/,
                        void* /*ctx*/)
{
    PP = 2.0 * (-p0_s + beta_s * log(FF.det())) * tensor_inverse_transpose(FF, NDIM);
    return;
} // PK1_dil_stress_function
} // namespace ModelData
using namespace ModelData;

// Function prototypes
void output_data(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
                 Pointer<INSHierarchyIntegrator> navier_stokes_integrator,
                 MeshBase& mesh,
                 EquationSystems* equation_systems,
                 const int iteration_num,
                 const double loop_time,
                 const string& data_dump_dirname);

/*******************************************************************************
 * For each run, the input filename and restart information (if needed) must   *
 * be given on the command line.  For non-restarted case, command line is:     *
 *                                                                             *
 *    executable <input file name>                                             *
 *                                                                             *
 * For restarted run, command line is:                                         *
 *                                                                             *
 *    executable <input file name> <restart directory> <restart number>        *
 *                                                                             *
 *******************************************************************************/

int
main(int argc, char* argv[])
{
    // Initialize IBAMR and libraries. Deinitialization is handled by this object as well.
    IBTKInit ibtk_init(argc, argv, MPI_COMM_WORLD);
    const LibMeshInit& init = ibtk_init.getLibMeshInit();

    { // cleanup dynamically allocated objects prior to shutdown

        // Parse command line options, set some standard options from the input
        // file, initialize the restart database (if this is a restarted run),
        // and enable file logging.
        Pointer<AppInitializer> app_initializer = new AppInitializer(argc, argv, "IB.log");
        Pointer<Database> input_db = app_initializer->getInputDatabase();

        // Get various standard options set in the input file.
        const bool dump_viz_data = app_initializer->dumpVizData();
        const int viz_dump_interval = app_initializer->getVizDumpInterval();
        const bool uses_visit = dump_viz_data && app_initializer->getVisItDataWriter();
#ifdef LIBMESH_HAVE_EXODUS_API
        const bool uses_exodus = dump_viz_data && !app_initializer->getExodusIIFilename().empty();
#else
        const bool uses_exodus = false;
        if (!app_initializer->getExodusIIFilename().empty())
        {
            plog << "WARNING: libMesh was compiled without Exodus support, so no "
                 << "Exodus output will be written in this program.\n";
        }
#endif
        const string exodus_filename = app_initializer->getExodusIIFilename();

        const bool dump_restart_data = app_initializer->dumpRestartData();
        const int restart_dump_interval = app_initializer->getRestartDumpInterval();
        const string restart_dump_dirname = app_initializer->getRestartDumpDirectory();
        const string restart_read_dirname = app_initializer->getRestartReadDirectory();
        const int restart_restore_num = app_initializer->getRestartRestoreNumber();

        const bool dump_postproc_data = app_initializer->dumpPostProcessingData();
        const int postproc_data_dump_interval = app_initializer->getPostProcessingDataDumpInterval();
        const string postproc_data_dump_dirname = app_initializer->getPostProcessingDataDumpDirectory();
        if (dump_postproc_data && (postproc_data_dump_interval > 0) && !postproc_data_dump_dirname.empty())
        {
            Utilities::recursiveMkdir(postproc_data_dump_dirname);
        }

        const bool dump_timer_data = app_initializer->dumpTimerData();
        const int timer_dump_interval = app_initializer->getTimerDumpInterval();

        // Create a simple FE mesh.
        ReplicatedMesh mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        const double ds = input_db->getDouble("MFAC") * dx;
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.2;
        if (NDIM == 2 && (elem_type == "TRI3" || elem_type == "TRI6"))
        {
#ifdef LIBMESH_HAVE_TRIANGLE
            const int num_circum_nodes = ceil(2.0 * M_PI * R / ds);
            for (int k = 0; k < num_circum_nodes; ++k)
            {
                const double theta = 2.0 * M_PI * static_cast<double>(k) / static_cast<double>(num_circum_nodes);
                mesh.add_point(libMesh::Point(R * cos(theta), R * sin(theta)));
            }
            TriangleInterface triangle(mesh);
            triangle.triangulation_type() = TriangleInterface::GENERATE_CONVEX_HULL;
            triangle.elem_type() = Utility::string_to_enum<ElemType>(elem_type);
            triangle.desired_area() = 1.5 * sqrt(3.0) / 4.0 * ds * ds;
            triangle.insert_extra_points() = true;
            triangle.smooth_after_generating() = true;
            triangle.triangulate();
#else
            TBOX_ERROR("ERROR: libMesh appears to have been configured without support for Triangle,\n"
                       << "       but Triangle is required for TRI3 or TRI6 elements.\n");
#endif
        }
        else
        {
            // NOTE: number of segments along boundary is 4*2^r.
            const double num_circum_segments = 2.0 * M_PI * R / ds;
            const int r = log2(0.25 * num_circum_segments);
            MeshTools::Generation::build_sphere(mesh, R, r, Utility::string_to_enum<ElemType>(elem_type));
        }

        // Ensure nodes on the surface are on the analytic boundary.
        MeshBase::element_iterator el_end = mesh.elements_end();
        for (MeshBase::element_iterator el = mesh.elements_begin(); el != el_end; ++el)
        {
            Elem* const elem = *el;
            for (unsigned int side = 0; side < elem->n_sides(); ++side)
            {
                const bool at_mesh_bdry = !elem->neighbor_ptr(side);
                if (!at_mesh_bdry) continue;
                for (unsigned int k = 0; k < elem->n_nodes(); ++k)
                {
                    if (!elem->is_node_on_side(k, side)) continue;
                    Node& n = elem->node_ref(k);
                    n = R * n.unit();
                }
            }
        }
        mesh.prepare_for_use();

        c1_s = input_db->getDouble("C1_S");
        p0_s = input_db->getDouble("P0_S");
        beta_s = input_db->getDouble("BETA_S");

        // Create major algorithm and data objects that comprise the
        // application.  These objects are configured from the input database
        // and, if this is a restarted run, from the restart database.
        Pointer<CartesianGridGeometry<NDIM> > grid_geometry = new CartesianGridGeometry<NDIM>(
            "CartesianGeometry", app_initializer->getComponentDatabase("CartesianGeometry"));
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy = new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry);
        Pointer<LoadBalancer<NDIM> > load_balancer =
            new LoadBalancer<NDIM>("LoadBalancer", app_initializer->getComponentDatabase("LoadBalancer"));
        Pointer<BergerRigoutsos<NDIM> > box_generator = new BergerRigoutsos<NDIM>();

        Pointer<INSHierarchyIntegrator> navier_stokes_integrator;
        const string solver_type = app_initializer->getComponentDatabase("Main")->getString("solver_type");
        if (solver_type == "STAGGERED")
        {
            navier_stokes_integrator = new INSStaggeredHierarchyIntegrator(
                "INSStaggeredHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSStaggeredHierarchyIntegrator"));
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"));
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }
        Pointer<IBFEMethod> ib_method_ops =
            new IBFEMethod("IBFEMethod",
                           app_initializer->getComponentDatabase("IBFEMethod"),
                           &mesh,
                           app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                           /*register_for_restart*/ true,
                           restart_read_dirname,
                           restart_restore_num);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_method_ops,
                                              navier_stokes_integrator);
        time_integrator->registerLoadBalancer(load_balancer);

        Pointer<StandardTagAndInitialize<NDIM> > error_detector =
            new StandardTagAndInitialize<NDIM>("StandardTagAndInitialize",
                                               time_integrator,
                                               app_initializer->getComponentDatabase("StandardTagAndInitialize"));
        Pointer<GriddingAlgorithm<NDIM> > gridding_algorithm =
            new GriddingAlgorithm<NDIM>("GriddingAlgorithm",
                                        app_initializer->getComponentDatabase("GriddingAlgorithm"),
                                        error_detector,
                                        box_generator,
                                        load_balancer);

        // Configure the IBFE solver.
        ib_method_ops->registerInitialCoordinateMappingFunction(coordinate_mapping_function);
        IBFEMethod::PK1StressFcnData PK1_dev_stress_data(PK1_dev_stress_function);
        IBFEMethod::PK1StressFcnData PK1_dil_stress_data(PK1_dil_stress_function);
        PK1_dev_stress_data.quad_order =
            Utility::string_to_enum<libMesh::Order>(input_db->getStringWithDefault("PK1_DEV_QUAD_ORDER", "THIRD"));
        PK1_dil_stress_data.quad_order =
            Utility::string_to_enum<libMesh::Order>(input_db->getStringWithDefault("PK1_DIL_QUAD_ORDER", "FIRST"));
        ib_method_ops->registerPK1StressFunction(PK1_dev_stress_data);
        ib_method_ops->registerPK1StressFunction(PK1_dil_stress_data);
        if (input_db->getBoolWithDefault("ELIMINATE_PRESSURE_JUMPS", false))
        {
            ib_method_ops->registerStressNormalizationPart();
        }
        ib_method_ops->initializeFEEquationSystems();
        EquationSystems* equation_systems = ib_method_ops->getFEDataManager()->getEquationSystems();

        // Set up post processor to recover computed stresses.
        ib_method_ops->initializeFEEquationSystems();
        FEDataManager* fe_data_manager = ib_method_ops->getFEDataManager();

        Pointer<IBFEPostProcessor> ib_post_processor =
            new IBFECentroidPostProcessor("IBFEPostProcessor", fe_data_manager);

        ib_post_processor->registerTensorVariable("FF", MONOMIAL, CONSTANT, IBFEPostProcessor::FF_fcn);

        pair<IBTK::TensorMeshFcnPtr, void*> PK1_dev_stress_fcn_data(PK1_dev_stress_function, static_cast<void*>(NULL));
        ib_post_processor->registerTensorVariable("sigma_dev",
                                                  MONOMIAL,
                                                  CONSTANT,
                                                  IBFEPostProcessor::cauchy_stress_from_PK1_stress_fcn,
                                                  vector<SystemData>(),
                                                  &PK1_dev_stress_fcn_data);

        pair<IBTK::TensorMeshFcnPtr, void*> PK1_dil_stress_fcn_data(PK1_dil_stress_function, static_cast<void*>(NULL));
        ib_post_processor->registerTensorVariable("sigma_dil",
                                                  MONOMIAL,
                                                  CONSTANT,
                                                  IBFEPostProcessor::cauchy_stress_from_PK1_stress_fcn,
                                                  vector<SystemData>(),
                                                  &PK1_dil_stress_fcn_data);

        Pointer<hier::Variable<NDIM> > p_var = navier_stokes_integrator->getPressureVariable();
        Pointer<VariableContext> p_current_ctx = navier_stokes_integrator->getCurrentContext();
        HierarchyGhostCellInterpolation::InterpolationTransactionComponent p_ghostfill(
            /*data_idx*/ -1, "LINEAR_REFINE", /*use_cf_bdry_interpolation*/ false, "CONSERVATIVE_COARSEN", "LINEAR");
        FEDataManager::InterpSpec p_interp_spec("PIECEWISE_LINEAR",
                                                QGAUSS,
                                                FIFTH,
                                                /*use_adaptive_quadrature*/ false,
                                                /*point_density*/ 2.0,
                                                /*use_consistent_mass_matrix*/ true,
                                                /*use_nodal_quadrature*/ false,
                                                /*allow_rules_with_negative_weights*/ false);
        ib_post_processor->registerInterpolatedScalarEulerianVariable(
            "p_f", LAGRANGE, FIRST, p_var, p_current_ctx, p_ghostfill, p_interp_spec);

        // Create Eulerian initial condition specification objects.
        if (input_db->keyExists("VelocityInitialConditions"))
        {
            Pointer<CartGridFunction> u_init = new muParserCartGridFunction(
                "u_init", app_initializer->getComponentDatabase("VelocityInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerVelocityInitialConditions(u_init);
        }

        if (input_db->keyExists("PressureInitialConditions"))
        {
            Pointer<CartGridFunction> p_init = new muParserCartGridFunction(
                "p_init", app_initializer->getComponentDatabase("PressureInitialConditions"), grid_geometry);
            navier_stokes_integrator->registerPressureInitialConditions(p_init);
        }

        // Create Eulerian boundary condition specification objects (when necessary).
        const IntVector<NDIM>& periodic_shift = grid_geometry->getPeriodicShift();
        vector<RobinBcCoefStrategy<NDIM>*> u_bc_coefs(NDIM);
        if (periodic_shift.min() > 0)
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                u_bc_coefs[d] = NULL;
            }
        }
        else
        {
            for (unsigned int d = 0; d < NDIM; ++d)
            {
                const std::string bc_coefs_name = "u_bc_coefs_" + std::to_string(d);

                const std::string bc_coefs_db_name = "VelocityBcCoefs_" + std::to_string(d);

                u_bc_coefs[d] = new muParserRobinBcCoefs(
                    bc_coefs_name, app_initializer->getComponentDatabase(bc_coefs_db_name), grid_geometry);
            }
            navier_stokes_integrator->registerPhysicalBoundaryConditions(u_bc_coefs);
        }

        // Create Eulerian body force function specification objects.
        if (input_db->keyExists("ForcingFunction"))
        {
            Pointer<CartGridFunction> f_fcn = new muParserCartGridFunction(
                "f_fcn", app_initializer->getComponentDatabase("ForcingFunction"), grid_geometry);
            time_integrator->registerBodyForceFunction(f_fcn);
        }

        // Set up visualization plot file writers.
        Pointer<VisItDataWriter<NDIM> > visit_data_writer = app_initializer->getVisItDataWriter();
        if (uses_visit)
        {
            time_integrator->registerVisItDataWriter(visit_data_writer);
            visit_data_writer->registerPlotQuantity("workload", "SCALAR", time_integrator->getWorkloadDataIndex());
        }
        std::unique_ptr<ExodusII_IO> exodus_io(uses_exodus ? new ExodusII_IO(mesh) : NULL);

        // Check to see if this is a restarted run to append current exodus files
        if (uses_exodus)
        {
            const bool from_restart = RestartManager::getManager()->isFromRestart();
            exodus_io->append(from_restart);
        }

        // Initialize hierarchy configuration and data on all patches.
        ib_method_ops->initializeFEData();
        if (ib_post_processor) ib_post_processor->initializeFEData();
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);

        // Deallocate initialization objects.
        app_initializer.setNull();

        // Print the input database contents to the log file.
        plog << "Input database:\n";
        input_db->printClassData(plog);

        // Write out initial visualization data.
        int iteration_num = time_integrator->getIntegratorStep();
        double loop_time = time_integrator->getIntegratorTime();
        if (dump_viz_data)
        {
            pout << "\n\nWriting visualization files...\n\n";
            if (uses_visit)
            {
                const System& position_system = equation_systems->get_system(IBFEMethod::COORDS_SYSTEM_NAME);
                time_integrator->setupPlotData();
                visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                if (NDIM < 3)
                {
                    IBTK::BoxPartitioner partitioner(*patch_hierarchy, position_system);
                    partitioner.writePartitioning("patch-part-" + std::to_string(iteration_num) + ".txt");
                    // Write partitioning data from libMesh.
                    IBTK::write_node_partitioning("node-part-" + std::to_string(iteration_num) + ".txt",
                                                  position_system);
                }
            }
            if (uses_exodus)
            {
                if (ib_post_processor) ib_post_processor->postProcessData(loop_time);
                exodus_io->write_timestep(
                    exodus_filename, *equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
            }
        }

        // Open streams to save volume of structure.
        ofstream volume_stream;
        if (IBTK_MPI::getRank() == 0)
        {
            volume_stream.open("volume.curve", ios_base::out | ios_base::trunc);
        }

        // Main time step loop.
        double loop_time_end = time_integrator->getEndTime();
        double dt = 0.0;
        while (!IBTK::rel_equal_eps(loop_time, loop_time_end) && time_integrator->stepsRemaining())
        {
            iteration_num = time_integrator->getIntegratorStep();
            loop_time = time_integrator->getIntegratorTime();

            pout << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "At beginning of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";

            dt = time_integrator->getMaximumTimeStepSize();
            time_integrator->advanceHierarchy(dt);
            loop_time += dt;

            pout << "\n";
            pout << "At end       of timestep # " << iteration_num << "\n";
            pout << "Simulation time is " << loop_time << "\n";
            pout << "+++++++++++++++++++++++++++++++++++++++++++++++++++\n";
            pout << "\n";

            // At specified intervals, write visualization and restart files,
            // print out timer data, and store hierarchy data for post
            // processing.
            iteration_num += 1;
            const bool last_step = !time_integrator->stepsRemaining();
            if (dump_viz_data && (iteration_num % viz_dump_interval == 0 || last_step))
            {
                pout << "\nWriting visualization files...\n\n";
                if (uses_visit)
                {
                    const System& position_system = equation_systems->get_system(IBFEMethod::COORDS_SYSTEM_NAME);
                    time_integrator->setupPlotData();
                    visit_data_writer->writePlotData(patch_hierarchy, iteration_num, loop_time);
                    if (NDIM < 3)
                    {
                        IBTK::BoxPartitioner partitioner(*patch_hierarchy, position_system);
                        partitioner.writePartitioning("patch-part-" + std::to_string(iteration_num) + ".txt");
                        IBTK::write_node_partitioning("node-part-" + std::to_string(iteration_num) + ".txt",
                                                      position_system);
                    }
                }
                if (uses_exodus)
                {
                    if (ib_post_processor) ib_post_processor->postProcessData(loop_time);
                    exodus_io->write_timestep(
                        exodus_filename, *equation_systems, iteration_num / viz_dump_interval + 1, loop_time);
                }
            }
            if (dump_restart_data && (iteration_num % restart_dump_interval == 0 || last_step))
            {
                pout << "\nWriting restart files...\n\n";
                RestartManager::getManager()->writeRestartFile(restart_dump_dirname, iteration_num);
                ib_method_ops->writeFEDataToRestartFile(restart_dump_dirname, iteration_num);
            }
            if (dump_timer_data && (iteration_num % timer_dump_interval == 0 || last_step))
            {
                pout << "\nWriting timer data...\n\n";
                TimerManager::getManager()->print(plog);
            }
            if (dump_postproc_data && (iteration_num % postproc_data_dump_interval == 0 || last_step))
            {
                output_data(patch_hierarchy,
                            navier_stokes_integrator,
                            mesh,
                            equation_systems,
                            iteration_num,
                            loop_time,
                            postproc_data_dump_dirname);
            }

            // Compute the volume of the structure.
            double J_integral = 0.0;
            System& X_system = equation_systems->get_system<System>(IBFEMethod::COORDS_SYSTEM_NAME);
            NumericVector<double>* X_vec = X_system.solution.get();
            NumericVector<double>* X_ghost_vec = X_system.current_local_solution.get();
            copy_and_synch(*X_vec, *X_ghost_vec);
            DofMap& X_dof_map = X_system.get_dof_map();
            vector<vector<unsigned int> > X_dof_indices(NDIM);
            std::unique_ptr<FEBase> fe(FEBase::build(NDIM, X_dof_map.variable_type(0)));
            std::unique_ptr<QBase> qrule = QBase::build(QGAUSS, NDIM, FIFTH);
            fe->attach_quadrature_rule(qrule.get());
            const vector<double>& JxW = fe->get_JxW();
            const vector<vector<VectorValue<double> > >& dphi = fe->get_dphi();
            TensorValue<double> FF;
            boost::multi_array<double, 2> X_node;
            const auto el_begin = mesh.active_local_elements_begin();
            const auto el_end = mesh.active_local_elements_end();
            for (auto el_it = el_begin; el_it != el_end; ++el_it)
            {
                const auto elem = *el_it;
                fe->reinit(elem);
                for (unsigned int d = 0; d < NDIM; ++d)
                {
                    X_dof_map.dof_indices(elem, X_dof_indices[d], d);
                }
                const int n_qp = qrule->n_points();
                get_values_for_interpolation(X_node, *X_ghost_vec, X_dof_indices);
                for (int qp = 0; qp < n_qp; ++qp)
                {
                    jacobian(FF, qp, X_node, dphi);
                    J_integral += abs(FF.det()) * JxW[qp];
                }
            }
            J_integral = IBTK_MPI::sumReduction(J_integral);
            if (IBTK_MPI::getRank() == 0)
            {
                volume_stream.precision(12);
                volume_stream.setf(ios::fixed, ios::floatfield);
                volume_stream << loop_time << " " << J_integral << endl;
            }
        }

        // Close the logging streams.
        if (IBTK_MPI::getRank() == 0)
        {
            volume_stream.close();
        }

        // Cleanup Eulerian boundary condition specification objects (when
        // necessary).
        for (unsigned int d = 0; d < NDIM; ++d) delete u_bc_coefs[d];

    } // cleanup dynamically allocated objects prior to shutdown
} // main

void
output_data(Pointer<PatchHierarchy<NDIM> > patch_hierarchy,
            Pointer<INSHierarchyIntegrator> navier_stokes_integrator,
            MeshBase& mesh,
            EquationSystems* equation_systems,
            const int iteration_num,
            const double loop_time,
            const string& data_dump_dirname)
{
    plog << "writing hierarchy data at iteration " << iteration_num << " to disk" << endl;
    plog << "simulation time is " << loop_time << endl;

    // Write Cartesian data.
    string file_name = data_dump_dirname + "/" + "hier_data.";
    char temp_buf[128];
    sprintf(temp_buf, "%05d.samrai.%05d", iteration_num, IBTK_MPI::getRank());
    file_name += temp_buf;
    Pointer<HDFDatabase> hier_db = new HDFDatabase("hier_db");
    hier_db->create(file_name);
    VariableDatabase<NDIM>* var_db = VariableDatabase<NDIM>::getDatabase();
    ComponentSelector hier_data;
    hier_data.setFlag(var_db->mapVariableAndContextToIndex(navier_stokes_integrator->getVelocityVariable(),
                                                           navier_stokes_integrator->getCurrentContext()));
    hier_data.setFlag(var_db->mapVariableAndContextToIndex(navier_stokes_integrator->getPressureVariable(),
                                                           navier_stokes_integrator->getCurrentContext()));
    patch_hierarchy->putToDatabase(hier_db->putDatabase("PatchHierarchy"), hier_data);
    hier_db->putDouble("loop_time", loop_time);
    hier_db->putInteger("iteration_num", iteration_num);
    hier_db->close();

    // Write Lagrangian data.
    file_name = data_dump_dirname + "/" + "fe_mesh.";
    sprintf(temp_buf, "%05d", iteration_num);
    file_name += temp_buf;
    file_name += ".xda";
    mesh.write(file_name);
    file_name = data_dump_dirname + "/" + "fe_equation_systems.";
    sprintf(temp_buf, "%05d", iteration_num);
    file_name += temp_buf;
    equation_systems->write(file_name, (EquationSystems::WRITE_DATA | EquationSystems::WRITE_ADDITIONAL_DATA));
    return;
} // output_data


/*
 * Copyright (c) 2009 Carnegie Mellon University.
 *     All rights reserved.
 *
 *  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.
 *
 * For more about this software visit:
 *
 *      http://www.graphlab.ml.cmu.edu
 *
 */

/**
 *
 * \brief This application is almost identical to LBP structured
 * prediction except that it generates an artificial field for every
 * vertex enabling the studying of distributed LBP on various graph
 * structures without the need for actual data.
 *
 *
 * Technical Explanation
 * ========================
 *
 * This application creates a pair-wise Markov Random Field with
 * Ising-Potts edge factors and then uses residual loopy belief
 * propagation to compute posterior belief estimates for each vertex.
 *
 *
 *  \author Joseph Gonzalez
 */

#include <boost/config/warning_disable.hpp>
#include <boost/spirit/include/qi.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_operator.hpp>
#include <boost/spirit/include/phoenix_stl.hpp>

#include <Eigen/Dense>
#include "eigen_serialization.hpp"

#include <graphlab.hpp>
#include <graphlab/macros_def.hpp>

/**
 * \brief Eigen library vectors are used to store factor in _LOG
 * SPACE_.
 */
typedef Eigen::VectorXd factor_type;

/**
 * \brief The Ising smoothing parameter which controls the coupling
 * between adjacent predictions in the graph.  Larger values imply
 * greater smoothing (stronger coupling).
 *
 * \code
 * edge_factor(xi, xj) = exp( (xi == xj)? 0 : -SMOOTHING * edge_weight );
 * \endcode
 *
 * Not that the default edge weight is 1 however the graph file can
 * contain an additional edge weight column which allows per edge
 * control of the smoothing parameter.
 *
 * This parameter is set as a command line argument.
 */
double SMOOTHING = 2;

double FIELD = 2;
size_t NSTATES = 5;

/**
 * \brief The Damping parameter which helps ensure stable convergence.
 * Larger damping values lead to slower but more stable convergence.
 *
 * Currently damping is implemented in log-space in the following
 * equation:
 *
 * \code
 * log(new_message) = DAMPING * log(old_message) +
 *                         (1-DAMPING) * log(new_message);
 * \endcode
 *
 * This parameter is set as a command line argument.
 */
double DAMPING = 0.1;

/**
 * \brief The convergence threshold for each message.  Smaller values
 * imply tighter convergence but slower execution.
 *
 *
 * The algorithm convergence when:
 *
 * \code
 * sum(abs(log(old_message) - log(new_message))) < TOLERANCE
 * \endcode
 *
 * The parameter is set as a command line argument
 */
double TOLERANCE = 0.01;

bool USE_CACHE = false;

/**
 * Make a synthetic node potential
 */
factor_type make_node_potential(size_t vid) {
  // const size_t obs = vid % NSTATES;
  const size_t obs = 0;
  factor_type factor;
  factor.setZero(NSTATES);
  if(vid % 101 < 1) {
    for(size_t i = 0; i <  NSTATES; ++i) {
      factor(i) = (i == obs)? 0 : -FIELD;
    }
  }
  return factor;
}

/**
 * \brief The vertex data contains the vertex potential as well as the
 * current belief estimate and represents a random variable in the
 * Markov Random Field.
 *
 * The vertex potential represents the prior and is obtained from the
 * vertex prior file (stored in log form).
 *
 * The belief represents the current posterior estimate.
 */
struct vertex_data {
  factor_type belief;
  void load(graphlab::iarchive& arc) { arc >> belief; }
  void save(graphlab::oarchive& arc) const { arc << belief; }
}; // end of vertex_data

/**
 * \brief The edge data represents an edge in the Markov Random Field
 * and contains the loopy belief propagation message in both
 * directions along that edge as well as the old message in each
 * direction.  In addition each edge contains the weight parameter
 * used to set edge specific smoothing (default value is 1).
 */
class edge_data {
  /**
   * \brief We store old and new messages in both directions as an
   * array of messages.  The particular message index is then computed
   * using the \ref message_idx function.
   */
  factor_type messages_[4];
  /**
   * \brief The weight associated with the edge (used to scale the
   * smoothing parameter)
   */
  double weight_;
  /**
   * \brief The function used to compute the message index in the edge
   * message array.
   */
  size_t message_idx(size_t source_id, size_t target_id, bool is_new) {
    return size_t(source_id < target_id)  + 2 * size_t(is_new);
  }

public:

  edge_data(const double w = 1) : weight_(w) { }
  const double& weight() const { return weight_; }

  /**
   * \brief Get the new message value from source_id to target_id
   */
  factor_type& message(size_t source_id, size_t target_id) {
    return messages_[message_idx(source_id, target_id, true)];
  }
  /**
   * \brief Get the old message value from source_id to target_id
   */
  factor_type& old_message(size_t source_id, size_t target_id) {
     return messages_[message_idx(source_id, target_id, false)];
  }

  /**
   * \brief Set the old message value equal to the new message value
   */
  void update_old(size_t source_id, size_t target_id) {
    old_message(source_id, target_id) = message(source_id, target_id);
  }

  /**
   * \brief Initialize the edge data with source and target having the
   * appropriate number of states.
   *
   * \param source_id the vertex id of the source
   * \param nsource the number of states the source vertex takes
   * \param target_id the vertex id of the target
   * \param ntarget the number of states the target vertex takes
   */
  void initialize(size_t source_id, size_t nsource, size_t target_id, size_t ntarget) {
    ASSERT_GT(nsource, 0); ASSERT_GT(ntarget, 0);
    message(source_id, target_id).setZero(ntarget);
    old_message(source_id, target_id).setZero(ntarget);
    message(target_id, source_id).setZero(nsource);
    old_message(target_id, source_id).setZero(nsource);
  }
  void save(graphlab::oarchive& arc) const {
    for(size_t i = 0; i < 4; ++i) arc << messages_[i];
    arc << weight_;
  }
  void load(graphlab::iarchive& arc) {
    for(size_t i = 0; i < 4; ++i) arc >> messages_[i];
    arc >> weight_;
  }
}; // End of edge data

/**
 * \brief The graph type used to store the Markov Random Field with
 * vertex data containing node potentials and beliefs and edge data
 * containing messages and weights.
 */
typedef graphlab::distributed_graph<vertex_data, edge_data> graph_type;

/**
 * \brief The Loopy Belief Propagation Vertex Program which computes
 * the product of the inbound messages during the gather phase,
 * updates the belief during the apply phase, and then computes the
 * new out-bound messages during the scatter phase.
 *
 * Since the gather phase is computing the product of the inbound
 * messages and the messages are stored in log form the resulting sum
 * operation is actually a vector sum and so the gather type is simply
 * the factor type and the operator+= operation for the factor type is
 * sufficient.
 *
 */
struct bp_vertex_program :
  public graphlab::ivertex_program< graph_type, factor_type,
                                    graphlab::messages::sum_priority >,
  public graphlab::IS_POD_TYPE {

  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type gather_edges(icontext_type& context,
                             const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of gather_edges

  /**
   * \brief Update the old message to be the new message and collect the
   * message value.
   */
  factor_type gather(icontext_type& context, const vertex_type& vertex,
                     edge_type& edge) const {
    const vertex_type other_vertex = get_other_vertex(edge, vertex);
    edge_data& edata = edge.data();
    // Update the old message with the value of the new Message.  We
    // then receive the old message during gather and then compute the
    // "cavity" during scatter (again using the old message).
    edata.update_old(other_vertex.id(), vertex.id());
    const factor_type& recv_message =
      edata.old_message(other_vertex.id(), vertex.id());
    return recv_message;

  }; // end of gather function

  /**
   * \brief Multiply message product by node potential and update the
   * belief.
   */
  void apply(icontext_type& context, vertex_type& vertex,
             const factor_type& total) {
    // If we have no neighbors than the belief is equal to the
    // potential so simply update the belief
    if(vertex.num_in_edges() + vertex.num_out_edges() == 0) {
      vertex.data().belief = make_node_potential(vertex.id());
    } else {
      vertex_data& vdata = vertex.data();
      // Multiply (add in log space) the potential to compute the belief
      vdata.belief = make_node_potential(vertex.id()) + total;
      ASSERT_GT(vdata.belief.size(), 0);
      // Rescale the belief to ensure numerical stability.  (This is
      // essentially normalization in log-space.)
      vdata.belief.array() -= vdata.belief.maxCoeff();
    }
  }; // end of apply

  /**
   * \brief Since the MRF is undirected we will use all edges for gather and
   * scatter
   */
  edge_dir_type scatter_edges(icontext_type& context,
                              const vertex_type& vertex) const {
    return graphlab::ALL_EDGES;
  }; // end of scatter edges

  /**
   * \brief Compute new message value for each edge.
   */
  void scatter(icontext_type& context, const vertex_type& vertex,
               edge_type& edge) const {
    const vertex_type other_vertex = get_other_vertex(edge, vertex);
    edge_data& edata = edge.data();
    // Divide (subtract in log space) out of the belief the old in
    // message to construct the cavity
    const factor_type& old_in_message =
      edata.old_message(other_vertex.id(), vertex.id());
    ASSERT_EQ(old_in_message.size(), vertex.data().belief.size());
    factor_type cavity = vertex.data().belief - old_in_message;
    // compute the new message by convolving with the Ising-Potts Edge
    // factor.
    factor_type& new_out_message =
      edata.message(vertex.id(), other_vertex.id());
    // Make a backup of the last sent message which we will use to
    // maintain the cache
    const factor_type last_sent_message = new_out_message;
    const factor_type& last_recv_message =
      edata.old_message(vertex.id(), other_vertex.id());
    convolve(cavity, edata.weight(), new_out_message);
    // Renormalize (done in log space)
    new_out_message.array() -= new_out_message.maxCoeff();
    // // Apply damping to the message to stabilize convergence.
    new_out_message = DAMPING * last_sent_message +
      (1-DAMPING) * new_out_message;
    // Compute message residual
    const double residual =
      (new_out_message - last_recv_message).cwiseAbs().sum();
    if(USE_CACHE) {
      // context.clear_gather_cache(other_vertex);
      context.post_delta(other_vertex, new_out_message - last_sent_message);
      edata.update_old(vertex.id(), other_vertex.id());
    }
    // Schedule the adjacent vertex
    if(residual > TOLERANCE) context.signal(other_vertex, residual);
 }; // end of scatter

private:

  /**
   * \brief Compute the convolution of the cavity with the Ising-Potts
   * edge potential and store the result in the message
   *
   * \param cavity the belief minus the in-bound message
   * \param weight the edge weight used to scale the smoothing parameter
   * \param [out] message The message in which to store the result of
   * the convolution.
   */
  inline void convolve(const factor_type& cavity, const double& weight,
                       factor_type& message) const {
    for(int i = 0; i < message.size(); ++i) {
      double sum = 0;
      for(int j = 0; j < cavity.size(); ++j) {
        sum += std::exp( cavity(j)  + ( i == j? 0 : -(SMOOTHING*weight) ) );
      }
      // To try and ensure numerical stability we do not allow
      // messages to underflow in log-space
      message(i) = (sum > 0)? std::log(sum) :
        std::numeric_limits<double>::min();
    }
  } // end of convolve

  /**
   * \brief Given an edge and a vertex return the other vertex along
   * that edge.
   */
  inline vertex_type get_other_vertex(edge_type& edge,
                                      const vertex_type& vertex) const {
    return vertex.id() == edge.source().id()? edge.target() : edge.source();
  }; // end of other_vertex

}; // end of class bp_vertex_program

/**
 * \brief The edge data loader is used by the GraphLab graph loading
 * API to parse lines in the edge data file.
 */
bool edge_loader(graph_type& graph, const std::string& fname,
                 const std::string& line) {
  ASSERT_FALSE(line.empty());
  namespace qi = boost::spirit::qi;
  namespace ascii = boost::spirit::ascii;
  namespace phoenix = boost::phoenix;
  graphlab::vertex_id_type source(-1), target(-1);
  double weight = 1;
  const bool success = qi::phrase_parse
    (line.begin(), line.end(),
     //  Begin grammar
     (
      qi::ulong_[phoenix::ref(source) = qi::_1] >>  -qi::char_(',')
      >> qi::ulong_[phoenix::ref(target) = qi::_1] >>
      -(-qi::char_(',') >> qi::double_[phoenix::ref(weight) = qi::_1])
      )
     ,
     //  End grammar
     ascii::space);
  if(!success) return false;
  if(source == target) return true;
  else {
    graph.add_edge(source, target, edge_data(weight));
    return true;
  }
} // end of edge loader

/**
 * \brief The edge initializer is used to allocate the messages along
 * each edge based on the number of states of the source and target
 * vertex.
 */
void edge_initializer(graph_type::edge_type& edge) {
  edge_data& edata = edge.data();
  const graphlab::vertex_id_type source_id = edge.source().id();
  const size_t nsource = NSTATES;
  const graphlab::vertex_id_type target_id = edge.target().id();
  const size_t ntarget = NSTATES;
  edata.initialize(source_id, nsource, target_id, ntarget);
} // end of edge initializer

/**
 * \brief The belief prediction saver is used to save the belief
 * predictions for each vertex.
 */
struct belief_prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    std::stringstream strm;
    strm << vertex.id() << '\t';
    factor_type pred = vertex.data().belief;
    double sum = 0;
    for(int i = 0; i < pred.size(); ++i)
      sum += (pred(i) = std::exp(pred(i)));
    pred.array() /= sum;
    for(int i = 0; i < pred.size(); ++i)
      strm << pred(i) << (i+1 < pred.size()? '\t' : '\n');
    return strm.str();
  }
  std::string save_edge(const edge_type& edge) const {
    return ""; // nop
  }
}; // end of belief_prediction_saver

/**
 * \brief The MAP prediction saver is used to save the map estimated
 * for each vertex.  The MAP estimate is the most likely assignment
 */
struct map_prediction_saver {
  typedef graph_type::vertex_type vertex_type;
  typedef graph_type::edge_type   edge_type;
  std::string save_vertex(const vertex_type& vertex) const {
    std::stringstream strm;
    size_t prediction = 0;
    vertex.data().belief.maxCoeff(&prediction);
    strm << vertex.id() << '\t' << prediction << '\n';
    return strm.str();
  }
  std::string save_edge(const edge_type& edge) const {
    return ""; // nop
  }
}; // end of map prediction_saver

int main(int argc, char** argv) {
  global_logger().set_log_level(LOG_INFO);
  global_logger().set_log_to_console(true);
  ///! Initialize control plain using mpi
  graphlab::mpi_tools::init(argc, argv);
  graphlab::distributed_control dc;
  // Parse command line options -----------------------------------------------
  // \todo update description string
  const std::string description = "Structure prediction solver";
  graphlab::command_line_options clopts(description);

  std::string graph_dir;
  std::string output_dir = "pred";
  std::string exec_type = "async";
  std::string format = "tsv";
  bool map = false;
  clopts.attach_option("graph", graph_dir,
                       "The directory containing the adjacency graph");
  clopts.add_positional("graph");
  clopts.attach_option("field", FIELD,
                       "The background field used to construct the node potentials");
  clopts.attach_option("nstates", NSTATES,
                       "The number of states for each variable");
  clopts.attach_option("cache", USE_CACHE, "use gather caching");
  clopts.attach_option("output", output_dir,
                       "The directory in which to save the predictions");
  clopts.attach_option("format", format, "The graph file format.");
  clopts.add_positional("output");
  clopts.attach_option("smoothing", SMOOTHING,
                       "The amount of smoothing (larger = more)");
  clopts.attach_option("damping", DAMPING,
                       "The amount of damping (0 -> no damping and 1 -> no progress)");
  clopts.attach_option("tol", TOLERANCE,
                       "The tolerance level for convergence.");
  clopts.attach_option("map", map,
                       "Return maximizing assignment instead of the posterior distribution.");
  clopts.attach_option("engine", exec_type,
                       "The type of engine to use {async, sync}.");
  if(!clopts.parse(argc, argv)) {
    graphlab::mpi_tools::finalize();
    return clopts.is_set("help")? EXIT_SUCCESS : EXIT_FAILURE;
  }

  clopts.get_engine_args().set_option("use_cache", USE_CACHE);

  if(graph_dir.empty()) {
    logstream(LOG_ERROR) << "No graph was provided." << std::endl;
    return EXIT_FAILURE;
  }

  // Start the webserver
  graphlab::launch_metric_server();

  ///! load the graph
  graph_type graph(dc, clopts);

  ///! load the graph
  graph.load_format(graph_dir, format);
  graph.finalize();
  dc.cout() << "Initializing edge data" << std::endl;
  graph.transform_edges(edge_initializer);

  typedef graphlab::omni_engine<bp_vertex_program> engine_type;
  engine_type engine(dc, graph, exec_type, clopts);
  engine.signal_all();
  graphlab::timer timer;
  dc.cout() << "Running engine" << std::endl;
  engine.start();
  const double runtime = timer.current_time();
    dc.cout()
    << "----------------------------------------------------------" << std::endl
    << "Final Runtime (seconds):   " << runtime
    << std::endl
    << "Updates executed: " << engine.num_updates() << std::endl
    << "Update Rate (updates/second): "
    << engine.num_updates() / runtime << std::endl;

  std::cout << "Saving predictions" << std::endl;
  const bool gzip_output = false;
  const bool save_vertices = true;
  const bool save_edges = false;
  const size_t threads_per_machine = 2;
  if(map) {
    graph.save(output_dir, map_prediction_saver(),
               gzip_output, save_vertices,
               save_edges, threads_per_machine);
  } else {
    graph.save(output_dir, belief_prediction_saver(),
               gzip_output, save_vertices,
               save_edges, threads_per_machine);
  }

  //  graphlab::stop_metric_server_on_eof();
  graphlab::stop_metric_server();
  graphlab::mpi_tools::finalize();
  return EXIT_SUCCESS;

} // end of main


/*
 *  Copyright (c) 2008--2011, Universitaet Bremen
 *  All rights reserved.
 *
 *  Author: Christoph Hertzberg <chtz@informatik.uni-bremen.de>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Universitaet Bremen nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
/**
 * @file mtk/types/SOn.hpp
 * @brief Standard Orthogonal Groups i.e.\ rotatation groups.
 */
#ifndef SON_H_
#define SON_H_

#include "vect.hpp"
#include "../src/mtkmath.hpp"

#include <Eigen/Geometry>

namespace MTK {

/**
 * Two-dimensional orientations represented as scalar.
 * There is no guarantee that the representing scalar is within any interval,
 * but the result of boxminus will always have magnitude @f$\le\pi @f$.
 */
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO2 : public Eigen::Rotation2D<_scalar> {
	enum {DOF = 1, DIM = 2};

	typedef _scalar scalar;
	typedef Eigen::Rotation2D<scalar> base;
	typedef vect<DIM, scalar, Options> vect_type;

	//using base::operator=;

	//! Construct from angle
	SO2(const scalar& angle = 0) : base(angle) {	}

	//! Construct from Eigen::Rotation2D
	SO2(const base& src) : base(src) {}

	/**
	 * Construct from 2D vector.
	 * Resulting orientation will rotate the first unit vector to point to vec.
	 */
	SO2(const vect_type &vec) : base(atan2(vec[1], vec[0])) {};

	//! Calculate @c this->inverse() * @c r
	SO2 operator%(const base &r) const {
		return base::inverse() * r;
	}

	//! Calculate @c this->inverse() * @c r
	template<class Derived>
	vect_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
		return base::inverse() * vec;
	}

	//! Calculate @c *this * @c r.inverse()
	SO2 operator/(const SO2 &r) const {
		return *this * r.inverse();
	}

	//! Gets the angle as scalar.
	operator scalar() const {
		return base::angle();
	}

	//! @name Manifold requirements
	//{
	void boxplus(MTK::vectview<const scalar, DOF> vec, scalar scale=1) {
		base::angle() += scale * vec[0];
	}
	void boxminus(MTK::vectview<scalar, DOF> res, const SO2<scalar>& other) const {
		res[0] = MTK::normalize(base::angle() - other.angle(), scalar(MTK::pi));
	}
	//}

	friend std::istream& operator>>(std::istream &is, SO2<scalar>& ang){
		return is >> ang.angle();
	}

};

/**
 * Three-dimensional orientations represented as Quaternion.
 * It is assumed that the internal Quaternion always stays normalized,
 * should this not be the case, call inherited member function @c normalize().
 */
template<class _scalar = double, int Options = Eigen::AutoAlign>
struct SO3 : public Eigen::Quaternion<_scalar, Options> {
	enum {DOF = 3, DIM = 3};
	typedef _scalar scalar_type;
	typedef Eigen::Quaternion<scalar_type, Options> base;
	typedef Eigen::Quaternion<scalar_type> Quaternion;
	typedef vect<DIM, scalar_type, Options> vectorized_type;

	//using base::operator=;

	//! Calculate @c this->inverse() * @c r
	template<class OtherDerived> EIGEN_STRONG_INLINE
	Quaternion operator%(const Eigen::QuaternionBase<OtherDerived> &r) const {
		return base::conjugate() * r;
	}

	//! Calculate @c this->inverse() * @c r
	template<class Derived>
	vectorized_type operator%(const Eigen::MatrixBase<Derived> &vec) const {
		return base::conjugate() * vec;
	}

	//! Calculate @c this * @c r.conjugate()
	template<class OtherDerived> EIGEN_STRONG_INLINE
	Quaternion operator/(const Eigen::QuaternionBase<OtherDerived> &r) const {
		return *this * r.conjugate();
	}

	/**
	 * Construct from real part and three imaginary parts.
	 * Quaternion is normalized after construction.
	 */
	SO3(const scalar_type& w, const scalar_type& x, const scalar_type& y, const scalar_type& z) : base(w, x, y, z) {
		base::normalize();
	}

	/**
	 * Construct from Eigen::Quaternion.
	 * @note Non-normalized input may result result in spurious behavior.
	 */
	SO3(const base& src = base::Identity()) : base(src) {}

	/**
	 * Construct from rotation matrix.
	 * @note Invalid rotation matrices may lead to spurious behavior.
	 */
	template<class Derived>
	SO3(const Eigen::MatrixBase<Derived>& matrix) : base(matrix) {}

	/**
	 * Construct from arbitrary rotation type.
	 * @note Invalid rotation matrices may lead to spurious behavior.
	 */
	template<class Derived>
	SO3(const Eigen::RotationBase<Derived, 3>& rotation) : base(rotation.derived()) {}

	//! @name Manifold requirements
	//{
	SO3 operator+(const vectorized_type& vec) const {
		SO3 delta = exp(vec);
		return delta * *this;
	}

    void operator+=(const vectorized_type& vec) {
        *this = *this + vec;
    }

	auto operator-(const SO3<scalar_type>& other) const {
		return SO3::log(*this * other.conjugate());
	}
	//}

	friend std::ostream& operator<<(std::ostream &os, const SO3<scalar_type, Options>& q){
		return os << q.coeffs().transpose() << " ";
	}
	friend std::istream& operator>>(std::istream &is, SO3<scalar_type, Options>& q){
		vect<4,scalar_type> coeffs;
		is >> coeffs;
		q.coeffs() = coeffs.normalized();
		return is;
	}

	//! @name Helper functions
	//{
	/**
	 * Calculate the exponential map. In matrix terms this would correspond
	 * to the Rodrigues formula.
	 */
	// FIXME vectview<> can't be constructed from every MatrixBase<>, use const Vector3x& as workaround
//	static SO3 exp(MTK::vectview<const scalar, 3> dvec, scalar scale = 1){
	static SO3 exp(const Eigen::Matrix<scalar_type, 3, 1>& dvec, scalar_type scale = 1){
		SO3 res;
		res.w() = MTK::exp<scalar_type, 3>(res.vec(), dvec, scalar_type(scale / 2));
		return res;
	}
	/**
	 * Calculate the inverse of @c exp.
	 * Only guarantees that <code>exp(log(x)) == x </code>
	 */
	static typename base::Vector3 log(const SO3 &orient){
		typename base::Vector3 res;
		MTK::log<scalar_type, 3>(res, orient.w(), orient.vec(), scalar_type(2), true);
		return res;
	}
	//}
};

namespace internal {
template<class Scalar, int Options>
struct UnalignedType<SO2<Scalar, Options > >{
	typedef SO2<Scalar, Options | Eigen::DontAlign> type;
};

template<class Scalar, int Options>
struct UnalignedType<SO3<Scalar, Options > >{
	typedef SO3<Scalar, Options | Eigen::DontAlign> type;
};

}  // namespace internal

}  // namespace MTK

#endif /*SON_H_*/


#include <Eigen/Core>
#include <iostream>

using namespace Eigen;

// [circulant_func]
template<class ArgType>
class circulant_functor {
  const ArgType &m_vec;
public:
  circulant_functor(const ArgType& arg) : m_vec(arg) {}

  const typename ArgType::Scalar& operator() (Index row, Index col) const {
    Index index = row - col;
    if (index < 0) index += m_vec.size();
    return m_vec(index);
  }
};
// [circulant_func]

// [square]
template<class ArgType>
struct circulant_helper {
  typedef Matrix<typename ArgType::Scalar,
                 ArgType::SizeAtCompileTime,
                 ArgType::SizeAtCompileTime,
                 ColMajor,
                 ArgType::MaxSizeAtCompileTime,
                 ArgType::MaxSizeAtCompileTime> MatrixType;
};
// [square]

// [makeCirculant]
template <class ArgType>
CwiseNullaryOp<circulant_functor<ArgType>, typename circulant_helper<ArgType>::MatrixType>
makeCirculant(const Eigen::MatrixBase<ArgType>& arg)
{
  typedef typename circulant_helper<ArgType>::MatrixType MatrixType;
  return MatrixType::NullaryExpr(arg.size(), arg.size(), circulant_functor<ArgType>(arg.derived()));
}
// [makeCirculant]

// [main]
int main()
{
  Eigen::VectorXd vec(4);
  vec << 1, 2, 4, 8;
  Eigen::MatrixXd mat;
  mat = makeCirculant(vec);
  std::cout << mat << std::endl;
}
// [main]


#include <boost/numeric/conversion/cast.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include "generalprimary.h"
#include "primarynumber.h"
#include "primaryvariable.h"
#include "servicecontainer.h"
#include "primaryfunction.h"
#include "primaryexpression.h"

double compile_time_expression_structure::generalPrimary::value(double prime) const
{
    if (isFactorial)
    {
        if (prime<0.0)
            throw std::logic_error("factorial applied to neg. value");
        using boost::numeric_cast;
        unsigned int uif = numeric_cast<unsigned int>(prime);
        double duif = numeric_cast<double>(uif);
        if (prime-duif > std::numeric_limits<double>::min())
            throw std::logic_error("factorial applied to not integer value");
        return boost::math::factorial<double>(uif);
    }
    else
    {
        return prime;
    }
}

std::unique_ptr<compile_time_expression_structure::generalPrimary>
compile_time_expression_structure::generalPrimary::createPrimary
(compile_time_expression_structure::serviceContainer& sc)
{
    Token t {sc.pop()};
    std::unique_ptr<generalPrimary> gp{nullptr};
    switch (t.kind) {
        case TokenKind::number:
        {
            gp = std::make_unique<primaryNumber>(t.value);
            break;
        }
        case TokenKind::variable:
        {
            // first find in constants
            auto gcit {global_constants_id.find(t.name)};
            if (gcit == cend(global_constants_id))
            {
                auto vit {global_variables_id.find(t.name)};
                if (vit == cend(global_variables_id))
                    throw std::logic_error("undefined variable literal " + t.name);
                // not constant
                gp = std::make_unique<primaryVariable>(vit->second, sc);
                break;
            }
            auto cit = global_constants.find(gcit->second);
            if (cit == cend(global_constants))
                throw std::logic_error("undefined symbolic literal");
            const double Value = cit->second;
            gp = std::make_unique<primaryNumber>(Value);
            break;
        }
        case TokenKind::function:
        {
            auto gfidIt {global_functions_id.find(t.name)};
            if (gfidIt == cend(global_functions_id))
                throw std::logic_error("unknown function name");
            gp = std::make_unique<primaryFunction>(gfidIt->second, sc);
            break;
        }
        case TokenKind::bracket_left0:
//        case TokenKind::bracket_left1:
        {
            gp = std::make_unique<primaryExpression>(sc);
            //double d {expression(ist)};
            Token t2 {sc.pop()};
            if (t2.kind != TokenKind::bracket_right0)
                throw std::logic_error("bracket_right expected");
            break;
        }
//        case TokenKind::bracket_left1:
//        {
//            double d {expression()};
//            t = ts.pop();
//            if (t.kind != TokenKind::bracket_right1)
//                throw std::logic_error("bracket_right expected");
//            result = d;
//            break;
//        }
        default:
            throw std::logic_error("primary is expected");
    }

    Token tp {sc.pop()};
    if (tp.kind == TokenKind::factorial)
    {
        gp->setFactorial(true);
    }
    else {
        sc.push(tp);
    }
    return gp;
}


#include <boost/program_options.hpp>
#include <cmath>
#include <complex>
#include <cstdint>
#include <omp.h>
#include <opencv2/opencv.hpp>
#include <vector>

using complex = std::complex<double>;
constexpr double PI = 3.14159'26535'89793'23846'26433'83279'50288;

std::uint8_t colu(double t0) {
  double t = std::fmod(std::fmod(t0, 3.0) + 3.0, 3.0);
  if (t < 2) {
    return static_cast<std::uint8_t>(
        std::lround((1 - std::cos(t * PI)) * (255 / 2.0)));
  } else {
    return 0;
  }
}

cv::Vec3b color(double t0) {
  double t = t0;
  return {colu(t), colu(t + 1), colu(t + 2)};
}

double abslog(double x) {
  if (x == 0) {
    return 0;
  }
  auto l = std::log(std::abs(x) + 1);
  auto sign = x < 0 ? -1 : 1;
  return l * sign;
}

class mandel_maker {
  cv::Vec3d pix_at(double x, double y) {
    complex z{0.0, 0.0};
    auto c = complex{x, y};
    std::vector<cv::Point2f> points;
    points.reserve(rep);
    constexpr double LIMIT = 20;
    for (int ix = 0; ix < rep; ++ix) {
      z = z*z*z/(std::abs(z)+1e-10)+c;
      if (LIMIT < std::abs(z)) {
        return {static_cast<double>(ix), std::arg(z), std::abs(z)};
      }
      points.emplace_back(cv::Point2f(abslog(real(z)), abslog(imag(z))));
    }
    std::vector<cv::Point2f> hull;
    cv::convexHull(points, hull);
    auto area = cv::contourArea(hull);
    return {-1, std::arg(z), area};
  }
  int rep;
  int w, h;
  double x0, dx;
  double y0, dy;

public:
  explicit mandel_maker(int rep_, int w_, int h_, double x0_, double x1_,
                        double y0_)
      : rep(rep_), w(w_), h(h_), x0(x0_), dx((x1_ - x0_) / w_), y0(y0_),
        dy((x1_ - x0_) * h_ / w_ / h_) {}
  cv::Mat make() {
    cv::Mat im = cv::Mat::zeros(w, h, CV_64FC3);
#pragma omp parallel for
    for (int iy = 0; iy < h; ++iy) {
      double y = y0 + dy * iy;
      for (int ix = 0; ix < w; ++ix) {
        double x = x0 + ix * dx;
        auto col = pix_at(x, y);
        im.at<cv::Vec3d>(iy, ix) = col;
      }
    }
    return im;
  }
};

cv::Rect2d rect(char const *cmd) {
  double w0 = 1 << 5;
  cv::Rect2d r(-w0 / 2, -w0 / 2, w0, w0);
  for (; *cmd; ++cmd) {
    if (*cmd=='s'){
      auto w = r.width;
      auto h = r.height;
      auto dw = w/10;
      auto dh = h/10;
      r = cv::Rect2d( r.x+dw, r.y+dh, w-dw*2, h-dh*2 );
      continue;
    }
    double w = r.width / 2;
    double h = r.height / 2;
    double x, y;
    int c = *cmd - '0';
    switch (c % 3) {
    case 0:
      x = r.x;
      break;
    case 1:
      x = r.x + w / 2;
      break;
    case 2:
      x = r.x + w;
      break;
    }
    switch (c / 3) {
    case 0:
      y = r.y;
      break;
    case 1:
      y = r.y + h / 2;
      break;
    case 2:
      y = r.y + h;
      break;
    }
    r = cv::Rect2d(x, y, w, h);
  }
  return r;
}

void save_image(char const *filename, cv::Mat const &im) {
  cv::FileStorage fs(filename, cv::FileStorage::WRITE);
  fs << "image" << im;
}

std::pair<std::int32_t, std::int32_t> find_minmax(cv::Mat const &im) {
  int min = INT32_MAX;
  int max = INT32_MIN;
  auto end = im.end<std::int32_t>();
  for (auto it = im.begin<std::int32_t>(); it != end; ++it) {
    int32_t col = *it;
    if (col < 0) {
      continue;
    }
    if (col < min) {
      min = col;
    }
    if (max < col) {
      max = col;
    }
  }
  return {min, max};
}

cv::Mat colorize(cv::Mat const &src) {
  cv::Mat dest = cv::Mat::zeros(src.rows, src.cols, CV_8UC3);
#pragma omp parallel for
  for (int y = 0; y < src.rows; ++y) {
    for (int x = 0; x < src.cols; ++x) {
      auto col{src.at<cv::Vec3d>(y, x)};
      if (col[0] < 0) {
        // auto c0 = color((col[2]*0.1 + col[1] / (PI * 2) * 3 + 3) * 3) / 2;
        auto c0 = color(std::log(col[2]));
        dest.at<cv::Vec3b>(y, x) = c0 / 2;
      } else {
        auto c0 = cv::Vec3b{255, 255, 255} - color(col[1] / (PI * 2) * 3 + 3);
        auto c1 = cv::Vec3b{255, 255, 255} - color(col[0] * 0.006 + 1);
        constexpr double W = 0.2;
        dest.at<cv::Vec3b>(y, x) = c0 * W + c1 * (1 - W);
      }
    }
  }
  return dest;
}

namespace po = boost::program_options;

po::options_description calc_options() {
  po::options_description desc("calc options");
  desc.add_options()                                                     //
      ("help", "produce help message")                                   //
      ("size,s", po::value<double>()->default_value(60), "size in mm")   //
      ("dpi,d", po::value<double>()->default_value(96), "DPI")           //
      ("rep,r", po::value<int>()->default_value(1000), "repeat count")   //
      ("pos,p", po::value<std::string>()->default_value(""), "position") //
      ;
  return desc;
}

po::options_description col_options() {
  po::options_description desc("col options");
  desc.add_options()                   //
      ("help", "produce help message") //
      ("infile,i", po::value<std::string>()->default_value("data/hoge.yaml"),
       "input yaml file") //
      ("outfile,o", po::value<std::string>()->default_value("data/hoge.png"),
       "output image file") //
      ;
  return desc;
}

int calc(int argc, char const *argv[]) {
  auto desc = calc_options();
  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
    double realsize = vm["size"].as<double>();
    double dpi = vm["dpi"].as<double>();
    auto pix = lround(realsize / 25.4 * dpi);
    omp_set_num_threads(16);
    int rep = vm["rep"].as<int>();
    cv::Rect2d rc = rect(vm["pos"].as<std::string>().c_str());
    auto mm = mandel_maker(rep, pix, pix, rc.x, rc.br().x, rc.y);
    cv::Mat im = mm.make();
    cv::imwrite("data/hoge.png", colorize(im));
    save_image("data/hoge.yaml", im);
    return 0;
  } catch (po::error &e) {
    std::cout << e.what() << std::endl;
    return 0;
  }
}

int colorize(int argc, char const *argv[]) {
  auto desc = col_options();
  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    if (vm.count("help")) {
      std::cout << desc << "\n";
      return 0;
    }
    auto infile = vm["infile"].as<std::string>();
    auto outfile = vm["outfile"].as<std::string>();
    std::cout << infile << ", " << outfile << "\n";

    cv::FileStorage fs(infile, cv::FileStorage::READ);
    cv::Mat im;
    fs["image"] >> im;
    cv::imwrite(outfile, colorize(im));
    return 0;
  } catch (po::error &e) {
    std::cout << e.what() << std::endl;
    return 0;
  }
}
void show_help() {
  std::cout << calc_options() << "\n" << col_options() << "\n";
}

int main(int argc, char const *argv[]) {
  if (argc < 2) {
    show_help();
    return 0;
  }
  auto cmd = std::string(argv[1]);
  if (cmd == "calc") {
    return calc(argc - 1, argv + 1);
  } else if (cmd == "col") {
    return colorize(argc - 1, argv + 1);
  } else {
    show_help();
    return 0;
  }
}


#define NOMINMAX
#include "interactions.hpp"
#include "vsh_translation.hpp"
#include "indices.hpp"
#include <cmath>
#include <Eigen/IterativeLinearSolvers>
#include <omp.h>

using std::complex;
using namespace std::complex_literals;

using Eigen::Vector3d;

ComplexVector matrix_vector_product(const Ref<const ComplexMatrix>& A, const Ref<const ComplexVector>& x) {
    int size = x.size();
    ComplexVector ret(size);
#pragma omp parallel for
    for (int i = 0; i < size; i++) {
        ret(i) = A.row(i)*x;
    }

    return ret;
}

complex<double> dot_product(const Ref<const ComplexVector>& x, const Ref<const ComplexVector>& y) {
    int size = x.size();
    double real_part = 0;
    double imag_part = 0;
#pragma omp parallel for reduction(+:real_part,imag_part)
    for (int i = 0; i < size; i++) {
        real_part += x(i).real()*y(i).real() + x(i).imag()*y(i).imag();
        imag_part += x(i).real()*y(i).imag() - x(i).imag()*y(i).real();
    }

    return std::complex<double>(real_part, imag_part);
}

ComplexVector bicgstab(const Ref<const ComplexMatrix>& A, const Ref<const ComplexVector>& b,
        int maxiter, double tolerance) {

    int size = b.size();

    // step 1
    ComplexVector x_prev = b;
    ComplexVector r_prev = b - matrix_vector_product(A, x_prev);

    double error = (r_prev).norm();
    if (error < tolerance)
        return x_prev;

    // step 2
    ComplexVector r_hat = r_prev;

    // step 3
    complex<double> rho_prev = 1;
    complex<double> alpha    = 1;
    complex<double> w_prev   = 1;

    // step 4
    ComplexVector v_prev = ComplexVector::Zero(size);
    ComplexVector p_prev = ComplexVector::Zero(size);

    // step 5
    int current_iteration = 1;

    while (true) {
        //complex<double> rho_i = r_hat.dot(r_prev);
        complex<double> rho_i = dot_product(r_hat, r_prev);
        complex<double> beta = (rho_i/rho_prev)*(alpha/w_prev);
        ComplexVector pi = r_prev + beta*(p_prev - w_prev*v_prev);
        ComplexVector vi = matrix_vector_product(A, pi);

        //alpha = rho_i/r_hat.dot(vi);
        alpha = rho_i/dot_product(r_hat, vi);
        ComplexVector h = x_prev + alpha*pi;

        ComplexVector s = r_prev - alpha*vi;
        ComplexVector t = matrix_vector_product(A, s);
        //complex<double> w_i = t.dot(s)/t.dot(t);
        complex<double> w_i = dot_product(t,s)/dot_product(t,t);
        ComplexVector xi = h + w_i*s;
        ComplexVector ri = s - w_i*t;

        error = ri.norm();
        if (error < tolerance || current_iteration > maxiter)
            return xi;

        x_prev   = xi;
        r_prev   = ri;
        rho_prev = rho_i;
        v_prev   = vi;
        p_prev   = pi;
        w_prev   = w_i;

        current_iteration += 1;
    }
}

ComplexVector solve_linear_system(const Ref<const ComplexMatrix>& agg_tmatrix,
        const Ref<const ComplexVector>& p_src, solver method) {

    ComplexMatrix interaction_matrix = agg_tmatrix;
    for (int i = 0; i < interaction_matrix.cols(); i++)
        interaction_matrix(i,i) += 1;

    switch (method) {
        case solver::exact:  // TODO: implment
        default:
        case solver::bicgstab:
            //Eigen::BiCGSTAB<ComplexMatrix> solver;
            //solver.setTolerance(1e-5);
            //solver.compute(interaction_matrix);
            //return solver.solveWithGuess(p_src, p_src);
            return bicgstab(interaction_matrix, p_src);
    }
}

ComplexMatrix sphere_aggregate_tmatrix(const Ref<const position_t>& positions,
        const Ref<const ComplexMatrix>& mie, double k) {

    int lmax = mie.cols()/2;
    int rmax = lmax_to_rmax(lmax);
    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix agg_tmatrix = ComplexMatrix::Zero(size, size);

    if (Nparticles == 1)
        return agg_tmatrix;

    int N = Nparticles*(Nparticles-1)/2;
    Array ivals(N);
    Array jvals(N);
    int counter = 0;

    for (int i = 0; i < Nparticles; i++) {
        for (int j = i+1; j < Nparticles; j++) {
            ivals(counter) = i;
            jvals(counter) = j;
            counter += 1;
        }
    }

    auto vsh_precompute = create_vsh_cache_map(lmax);

    #pragma omp parallel for
    for (int ij = 0; ij < N; ij++) {
        int i = ivals(ij);
        int j = jvals(ij);

        Vector3d dji = positions.row(i) - positions.row(j);

        double rad = dji.norm();
        double theta = acos(dji(2)/rad);
        double phi = atan2(dji(1), dji(0));

        vsh_translation_insert_pair(agg_tmatrix, mie, i, j, rad, theta, phi, k, vsh_precompute);
    }

    return agg_tmatrix;
}

ComplexMatrix particle_aggregate_tmatrix(const Ref<const position_t>& positions,
        const tmatrix_t& tmatrix, double k) {

    int rmax = tmatrix.dimensions()[1]/2;
    int lmax = rmax_to_lmax(rmax);

    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix agg_tmatrix = ComplexMatrix::Zero(size, size);

    if (Nparticles == 1)
        return agg_tmatrix;

    int N = Nparticles*(Nparticles-1)/2;
    Array ivals(N);
    Array jvals(N);
    int counter = 0;

    for (int i = 0; i < Nparticles; i++) {
        for (int j = i+1; j < Nparticles; j++) {
            ivals(counter) = i;
            jvals(counter) = j;
            counter += 1;
        }
    }

    auto vsh_precompute = create_vsh_cache_map(lmax);

    #pragma omp parallel for
    for (int ij = 0; ij < N; ij++) {
        int i = ivals(ij);
        int j = jvals(ij);

        Vector3d dji = positions.row(i) - positions.row(j);

        double rad = dji.norm();
        double theta = acos(dji(2)/rad);
        double phi = atan2(dji(1), dji(0));

        vsh_translation_insert_pair(agg_tmatrix, tmatrix, i, j, rad, theta, phi, k, vsh_precompute);
    }

    return agg_tmatrix;
}

ComplexMatrix reflection_matrix_nia(const Ref<const position_t>& positions,
        const Ref<const ComplexMatrix>& mie, double k, complex<double> reflection, double z) {

    int lmax = mie.cols()/2;
    int rmax = lmax_to_rmax(lmax);
    int Nparticles = positions.rows();
    int size = 2*rmax*Nparticles;

    ComplexMatrix R_matrix = ComplexMatrix::Zero(size, size);

    for (int i = 0; i < Nparticles; i++) {
        Vector3d pi = positions.row(i);
        pi(2) -= 2*(pi(2) - z);
        for (int j = 0; j < Nparticles; j++) {
            Vector3d pj = positions.row(j);
            Vector3d dji = pi - pj;

            double rad = dji.norm();
            double theta = acos(dji(2)/rad);
            double phi = atan2(dji(1), dji(0));

            for (int n = 1; n < lmax+1; n++) {
                for (int m = -n; m < n+1; m++) {
                    for (int v = 1; v < lmax+1; v++) {
                        for (int u = -v; u < v+1; u++) {
                            auto transfer = vsh_translation(m, n, u, v, rad, theta, phi, k, vsh_mode::outgoing);
                            for (int a = 0; a < 2; a++) {
                                for (int b = 0; b < 2; b++) {
                                    complex<double> factor = -reflection*pow(-1, m + n + a + 1);
                                    complex<double> val = transfer[(a+b)%2];
                                    int idx = i*(2*rmax) + a*(rmax) + n*(n+2) - n + m - 1;
                                    int idy = j*(2*rmax) + b*(rmax) + v*(v+2) - v + u - 1;
                                    R_matrix(idx, idy) = factor*val*mie(j, b*lmax + v-1);
                                }
                            }
                        }
                    }
                }
            }
        }
    }

    return R_matrix;
}


/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <stdint.h>
#include <algorithm>
#include <vector>
#include <Eigen/Dense>

#include <boost/random/mersenne_twister.hpp>

#include <dpMM/global.hpp>
#include <dpMM/normal.hpp>

using  namespace Eigen;
using std::min;
using std::max;
using std::cout;
using std::endl;

#ifdef BOOST_OLD
using boost::mt19937;
#else
using boost::mt19937;
#endif

// TODO Template needs fixing - probably just remove
template <typename T>
class Sphere
{
public:
  Sphere(uint32_t D) : D_(D), north_(Matrix<T,Dynamic,1>::Zero(D))
  {north_(D_-1) = 1.0;};

  //assumes north is size D and last entry is 1
  Sphere(Matrix<T,Dynamic,1> north) : D_(north.rows()), north_(north)
  {};
  virtual ~Sphere() {};

  /*
   * normal in tangent space around p rotate to the north pole
   * -> the last dimension will always be 0
   * -> return only first 2 dims
   */
  virtual Matrix<T,Dynamic,Dynamic> Log_p_north(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& q) const;
  /*
   * rotate points x in tangent plane around north pole down to p
   */
  Matrix<T,Dynamic,Dynamic> rotate_north2p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& xNorth) const;

  /* rotate points x in tangent plane around p to north pole and
   * return 2D coordinates */
  Matrix<T,Dynamic,Dynamic> rotate_p2north(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& x) const;
  void rotate_p2north( const Matrix<T,Dynamic,Dynamic>& ps,
    const Matrix<T,Dynamic,Dynamic>& x, Matrix<T,Dynamic,Dynamic>& xNorth,
    const VectorXu& z, uint32_t K) const;
  void rotate_p2north(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& x_p, Matrix<T,Dynamic,Dynamic>& xNorth,
    const VectorXu& z, uint32_t k) const;

  /* compute rotation from TpS^2 to north pole on sphere */
  Matrix<T,Dynamic,Dynamic> north_R_TpS2(const Matrix<T,Dynamic,1>& p) const;

  /* compute logarithm map from sphere to T_pS */
  Matrix<T,Dynamic,Dynamic> Log_p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& q) const;
  Matrix<T,Dynamic,1> Log_p_single(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,1>& q) const;
  /* compute logarithm map from sphere to T_pS skips points where w_i==0 */
  Matrix<T,Dynamic,Dynamic> Log_p(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const Matrix<T,Dynamic,1>& w) const;
  /* compute logmap for q where z_i ==k; leaves other x_i untouched */
  void Log_p(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const VectorXu& z, uint32_t k,
    Matrix<T,Dynamic,Dynamic>& x, uint32_t zDivider=1) const;
  /* compute logarithm map from sphere to T_pS around several points p
   * indicated by labels z; fill in matrix x */
  void Log_ps(const Matrix<T,Dynamic,Dynamic>& p,
    const Matrix<T,Dynamic,Dynamic>& q, const VectorXu& z,
    Matrix<T,Dynamic,Dynamic>& x) const;

  /* compute exponential map from T_pS to sphere */
  Matrix<T,Dynamic,Dynamic> Exp_p(const Matrix<T,Dynamic,1>& p,
      const Matrix<T,Dynamic,Dynamic>& x) const;
  Matrix<T,Dynamic,1> Exp_p_single(const Matrix<T,Dynamic,1>& p,
    const Matrix<T,Dynamic,1>& x) const;

  Matrix<T,Dynamic,1> sampleUnif(mt19937* pRndGen);

  // http://en.wikipedia.org/wiki/N-sphere
  // http://keisan.casio.com/exec/system/1223381019
  T logSurfaceArea() const {
    return (LOG_2+0.5*D_*LOG_PI - boost::math::lgamma(0.5*D_));};

  uint32_t D() const {return D_;};
  const Matrix<T,Dynamic,1>& north() const {return north_;};
protected:

  T invSincDot(T dot) const;

  //static const double MIN_DOT = -0.95;
  //static const double MAX_DOT = 0.95;
  static double const MIN_DOT;
  static double const MAX_DOT;
  uint32_t D_; // dimension of ambient space; sphere is D-1 dimensional.
  Matrix<T,Dynamic,1> north_;
};

/* rotation from point A to B; percentage specifies how far the rotation will
 * bring us towards B [0,1] */
template<typename T>
inline Matrix<T,Dynamic,Dynamic> rotationFromAtoB(const Matrix<T,Dynamic,1>& a,
    const Matrix<T,Dynamic,1>& b, T percentage=1.0)
{
  assert(b.size() == a.size());

  uint32_t D_ = b.size();
  Matrix<T,Dynamic,Dynamic> bRa(D_,D_);

  T dot = b.transpose()*a;
  ASSERT(fabs(dot) <=1.0, "a="<<a.transpose()<<" |.| "<<a.norm()
      <<" b="<<b.transpose()<<" |.| "<<b.norm()
      <<" -> "<<dot);
  dot = max(static_cast<T>(-1.0),min(static_cast<T>(1.0),dot));
//  cout << "dot="<<dot<<" | |"<<fabs(dot+1.)<<endl;
  if(fabs(dot -1.) < 1e-6)
  {
    // points are almost the same -> just put identity
    bRa =  Matrix<T,Dynamic,Dynamic>::Identity(D_,D_);
//    bRa(0,0) = cos(percentage*PI);
//    bRa(1,1) = cos(percentage*PI);
//    bRa(0,1) = -sin(percentage*PI);
//    bRa(1,0) = sin(percentage*PI);
  }else if(fabs(dot +1.) <1e-6)
  {
    // direction does not matter since points are on opposing sides of sphere
    // -> pick one and rotate by percentage;
    bRa = -Matrix<T,Dynamic,Dynamic>::Identity(D_,D_);
    bRa(0,0) = cos(percentage*PI*0.5);
    bRa(1,1) = cos(percentage*PI*0.5);
    bRa(0,1) = -sin(percentage*PI*0.5);
    bRa(1,0) = sin(percentage*PI*0.5);
  }else{
    T alpha = acos(dot) * percentage;
//    cout << "alpha="<<alpha<<endl;

    Matrix<T,Dynamic,1> c(D_);
    c = a - b*dot;
    ASSERT(c.norm() >1e-5, "c="<<c.transpose()<<" |.| "<<c.norm());
    c /= c.norm();
    Matrix<T,Dynamic,Dynamic> A = b*c.transpose() - c*b.transpose();

	Matrix<T,Dynamic,Dynamic> temp = b*b.transpose() + c*c.transpose();
	T temp2 = cos(alpha)-1.;
    bRa = Matrix<T,Dynamic,Dynamic>::Identity(D_,D_) + sin(alpha)*A +
      (temp2)*(temp);
  }
  return bRa;
}