/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * (c) 2013 Quantitative Engineering Design (http://qed.ai)
 * Authors: William Wu, Jiehua Chen, Zhang Zhiming, Michał Łazowik
 * Primary Contact: William Wu (w@qed.ai)
 *
 * THE BSD LICENSE
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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.
 *************************************************************************/

/** @mainpage Masked Cross-Correlation in the Fourier Domain

@section purpose Purpose/Overview

This program efficiently computes the cross-correlation between two images,
each of which can be independently masked, using fast Fourier techniques. Our
procedure follows the algorithms described in the following paper:

Dirk Padfield. "Masked Object Registration in the Fourier Domain". IEEE Transactions on Image Processing, vol.21(5), pp. 2706-2718, 2012.

@section reqs Requirements

The default input image files are fixedImage.jpg and movingImage.jpg.

The default input masked image files are fixedMask.png and movingMask.png.

The output image is xcorr_image.jpg which shows the relative intensity of one channel's masked correlation.

The application shall display the xcorr_image.jpg on screen.

@section globals Global Data/Functions

There is no global data in this application.  All the data is contained within objects.

@section objects Objects

See the individual object documentation pages for more info on each object.

Xcorr_opencv - A class calculating the masked correlation in the fourier domain

@section arch High-level architecture

The general workflow of the code is:

1. Read input files.
2. Pass files to class Xcorr_opencv.
3. For each channel, calculate masked correlation in Fourier domain.
4. Get the relative intensity of masked correlation on one channel.
5. Display the relative matching intensity on the screen.

@section functions Function Descriptions

All functions are described in detail on the documentation pages for the classes
that either contain them or are friends of them.

@section ui User Interface

The only user interface is that the user will be prompted a image for showing the relative matching intensity on one channel. The user need to switch focus to the display window and press any key to continue.

@section testing Testing

 A shining point shall be shown on the top middle of the image, which means that movingImage.jpg matches the head of fixedImage.jpg.
 */
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <iostream>
#include <vector>
#include <numeric>
#include <fstream>
#include <functional>

#include <boost/bind.hpp>
#include <boost/filesystem.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/program_options.hpp>

namespace fs = boost::filesystem;

#include "normxcorr2_masked.hpp"

using namespace cv;

struct Mat_elt {
    int i;
    int j;
    double value;
};
bool Mat_elt_cmp(const Mat_elt& x, const Mat_elt& y) { return (x.value > y.value); }

int
is_file_openable (const std::string& filename) {
    ifstream my_file(filename.c_str(), ios::in);
    return my_file.good();
}

/*
Write an image of the cross-correlation matrix.
The Mat::convertTo() method is restricted to mappings of the type: x --> a*x + b, where we can specify a and b.
For maximum dynamic range:
    min --> 0
    max --> 255
Linear equations:
    (1): a(min) + b = 0
    (2): a(max) + b = 255
Solving:
    (1): b  = -a*min
    (2): a(max) - a(min) = 255
Therefore,
    a = 255 / (max - min)
    b = - 255 * min / (max - min)
Note 1: If we do not wish to achieve maximum dynamic range, and simply want to map -1 --> 0 and +1 --> 255, then a = b = 127.5.
Note 2: The resulting image does NOT match the output of the interactive window. I don't understand yet what kind of autoscaling imshow is doing.
*/
void
write_scaled_image(cv::Mat mat, const char* filename) {
    double minVal, maxVal;
    Point minPoint, maxPoint;
    minMaxLoc(mat, &minVal, &maxVal, &minPoint, &maxPoint, noArray());
    cv::Mat mat_scaled;
    double a = 255 / (maxVal - minVal);
    double b = -255 * minVal / (maxVal - minVal);
    mat.convertTo(mat_scaled, CV_8UC1, a, b);
    imwrite(filename, mat_scaled);
}

void
statistics(cv::Mat CC, int topK) {
    std::vector<Mat_elt> data;
    for (int i=0; i<CC.rows; i++) {
        for (int j=0; j<CC.cols; j++) {
            Mat_elt e;
            e.i = i;
            e.j = j;
            e.value = CC.at<double>(i,j);
            data.push_back(e);
        }
    }
    /* find top K values, and minimum */
    std::sort(data.begin(), data.end(), Mat_elt_cmp);
    for (int k=0; k<topK; k++) {
        printf("#%d: %f (%d,%d)\n", k+1, data[k].value, data[k].i, data[k].j);
    }
    double maxVal = data.front().value;
    double minVal = data.back().value;
    printf("maximum value: %f @ (y,x)=(%d,%d)\n", maxVal, data.front().j, data.front().i);
    printf("minimum value: %f @ (y,x)=(%d,%d)\n", minVal, data.back().j, data.back().i);
}

template <typename T>
void
validateRange(T value, T min, T max, const std::string &option_name = "") {
    if (value < min || value > max) {
        throw boost::program_options::validation_error(
            boost::program_options::validation_error::invalid_option_value,
            option_name
        );
    }
}

/**
  Main method

  Get image file names and pass them to the member function of Xcorr_opencv.
  After the relative intensities of masked correlation are calculated,
  main function shall get the result of one channel and display the result in
  one image.

  @return The exit status of the program as an integer
*/
int
main (int argc, char **argv)
{
    namespace po = boost::program_options;

    po::options_description desc(
        "Fast Masked Cross-Correlation Using Fourier Domain Methods\n\n"
        "Remarks:\n"
        "    Pixels in fixedImage that do not fall under the fixedMask "
        "are ignored in all correlation computations.\n"
        "    Pixels in movingImage that do not fall under the movingMask "
        "are ignored in all correlation computations.\n"
        "    If both overlap-fraction and overlap-pixels are present a "
        "greater resulting number is used"
    );

    std::string fixedImageName;
    std::string fixedMaskName;
    std::string movingImageName;
    std::string movingMaskName;
    std::string outputImageName;
    double requiredFractionOfOverlappingPixels;
    double requiredNumberOfOverlappingPixels;
    int topK;

    desc.add_options()
        ("help,h", "show help message")
        (
            "fixed-image,F",
            po::value<std::string>(&fixedImageName)->
                required(),
            "scene that we wish to search for the template"
        )
        (
            "fixed-mask,f",
            po::value<std::string>(&fixedMaskName)->
                required(),
            "binary mask specifying the search region in the scene, "
            "having the same dimensions as fixedImage"
        )
        (
            "moving-image,M",
            po::value<std::string>(&movingImageName)->
                required(),
            "template that we 'slide' throughout the scene"
        )
        (
            "moving-mask,m",
            po::value<std::string>(&movingMaskName)->
                required(),
            "a binary mask specifying the region of the template not "
            "to be ignored, having the same dimensions as movingImage"
        )
        (
            "overlap-fraction,o",
            po::value<double>(&requiredFractionOfOverlappingPixels)->
                default_value(0.3, "0.3")->notifier(
                    boost::bind(&validateRange<double>, _1, 0.0, 1.0, "\u2012O [ overlap\u2012fraction ]")
                ),
            "required fraction of maximum possible number of common pixels of masked images"
        )
        (
            "overlap-pixels,p",
            po::value<double>(&requiredNumberOfOverlappingPixels)->
                default_value(0),
            "required number of common pixels of masked images"
        )
        (
            "output,O",
            po::value<std::string>(&outputImageName)->
                default_value("xcorr.jpg"),
            "output image"
        )
        (
            "top-k,k",
            po::value<int>(&topK)->default_value(5),
            "print locations of the top k highest normalized "
            "cross-correlations and their values"
        )
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);

    if (vm.count("help")) {
        std::cout << desc << std::endl;
        return EXIT_SUCCESS;
    }

    try {
        po::notify(vm);
    } catch (boost::program_options::error &e) {
        std::cerr << "ERROR: " << e.what() << std::endl;
        return EXIT_FAILURE;
    }

    if (!is_file_openable(fixedImageName)) {
        std::cerr << "ERROR: Cannot open file " << fixedImageName << std::endl;
        return EXIT_FAILURE;
    }

    if (!is_file_openable(fixedMaskName)) {
        std::cerr << "ERROR: Cannot open file " << fixedMaskName << std::endl;
        return EXIT_FAILURE;
    }

    if (!is_file_openable(movingMaskName)) {
        std::cerr << "ERROR: Cannot open file " << movingMaskName << std::endl;
        return EXIT_FAILURE;
    }

    if (!is_file_openable(movingImageName)) {
        std::cerr << "ERROR: Cannot open file " << movingImageName << std::endl;
        return EXIT_FAILURE;
    }

    fs::path pathname(outputImageName);
    std::string dirname  = pathname.parent_path().string();
    std::string basename = pathname.stem().string();
    std::string extension = pathname.extension().string();
    if (extension.length() == 0) {
        extension = ".jpg"; // default
    }

    Xcorr_opencv *p_xcorr = new Xcorr_opencv;
    p_xcorr->Initialization(
        string(fixedImageName),
        string(fixedMaskName),
        string(movingImageName),
        string(movingMaskName),
        requiredFractionOfOverlappingPixels,
        requiredNumberOfOverlappingPixels
    );

    double t = (double)getTickCount();
    p_xcorr->CalXcorr();
    t = (double)getTickCount() - t;
    printf("Total cross-correlation time for all channels: %g ms\n\n", t*1000/getTickFrequency());
    t = (double)getTickCount();

    vector<cv::Mat> CC_vec;
    vector<cv::Mat> overlap_vec;
    cv::Mat CC;
    cv::Mat overlap;
    cv::Mat CC_blend;

    // iterate through channels
    for (int ch=0; ch<3; ch++) {
        printf("Results for channel %d ...\n",ch);
        p_xcorr->GetResult(CC,overlap,ch);
        CC_vec.push_back(CC);
        overlap_vec.push_back(overlap);

        // // Display xcorr matrix using an interactive window
        // cv::namedWindow("xcorr matrix", CV_WINDOW_AUTOSIZE);
        // cv::imshow("xcorr matrix", CC);
        // std::cout << "Displaying xcorr matrix. Switch focus to the display window and press any key to continue ..." << std::endl;
        // cv::waitKey();

        // find top matches
        statistics(CC, topK);

        // write xcorr image to disk
        std::string outputChannelImageBasename;
        if (dirname.length() > 0) {
            outputChannelImageBasename = dirname + "/" + basename + "_" + boost::lexical_cast<string>(ch);
        } else {
            outputChannelImageBasename = basename + "_" + boost::lexical_cast<string>(ch);
        }
        cout << "Writing image " << outputChannelImageBasename + extension << " ... " << endl;
        write_scaled_image(CC, (outputChannelImageBasename + extension).c_str());

        // writing xcorr matrices to disk
        std::string xcorrMatrixChannelName = (outputChannelImageBasename + "_correlation_matrix.tsv");
        std::string overlapMatrixChannelName = (outputChannelImageBasename + "_overlap_matrix.tsv");
        FILE* ccfile = fopen(xcorrMatrixChannelName.c_str(),"w+t"); // overwrite mode
        FILE* overlapfile = fopen(overlapMatrixChannelName.c_str(),"w+t");
        cout << "Writing numerical matrices " << xcorrMatrixChannelName << " and " << overlapMatrixChannelName << " ... " << endl;
        for (int i = 0; i < CC.rows; i++)
        {
            for (int j = 0; j < CC.cols; j++)
            {
                fprintf(ccfile,"%0.6f\t",CC.at<double>(i,j));
                fprintf(overlapfile,"%0.0f\t",overlap.at<double>(i,j));
            }
            fprintf(ccfile,"\n");
            fprintf(overlapfile,"\n");
        }
        fclose(ccfile);
        fclose(overlapfile);

        printf("\n");
    }
    std::string outputBlendedImageName;
    if (dirname.length() > 0) {
        outputBlendedImageName = dirname + "/" + basename + extension;
    } else {
        outputBlendedImageName = basename + extension;
    }
    cout << "Writing blended correlation matrix " << outputBlendedImageName << " ..." << endl;
    addWeighted(CC_vec[0],1.0/3,CC_vec[1],1.0/3,0,CC_blend);
    addWeighted(CC_blend,1.0,CC_vec[2],1.0/3,0,CC_blend);
    write_scaled_image(CC_blend, outputBlendedImageName.c_str());
    statistics(CC_blend, topK);

    return 0;
}


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

#include <boost/simd/sdk/constant/constant.hpp>
#include <boost/simd/sdk/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
    BOOST_SIMD_CONSTANT_REGISTER( Fastgammalargelim, double
                                , 0, 0x41200000         // 10.f
                                , 0x4040800000000000ll  // 30.
                                );
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Fastgammalargelim, Fastgammalargelim);
}

#endif


#include <algorithm>
#include <numeric>
#include <iostream>
#include <set>
#include <queue>
#include <utility>
#include <unordered_map>
#include <unordered_set>
#include <limits>
#include <cmath>
#include <ctime>
#include <cassert>

#include <boost/random.hpp>
#include <boost/functional/hash.hpp>

#include "Factor.h"
#include "DisjointSet.h"
#include "DisjointSetBT.h"
#include "RandomSource.h"
#include "VAcyclicDecomposition.h"

namespace Grante {

const unsigned int VAcyclicDecomposition::sa_steps = 20;
const double VAcyclicDecomposition::sa_t0 = 5.0;
const double VAcyclicDecomposition::sa_tfinal = 0.1;

VAcyclicDecomposition::VAcyclicDecomposition(const FactorGraph* fg)
	: fg(fg), fgu(fg) {
}

double VAcyclicDecomposition::ComputeDecompositionGreedy(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	return (ComputeDecomposition(factor_weights, factor_is_removed,
		1, 1.0e-6, 1.0e-6));
}

double VAcyclicDecomposition::ComputeDecompositionSA(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	return (ComputeDecomposition(factor_weights, factor_is_removed,
		sa_steps, sa_t0, sa_tfinal));
}

// Compute solution based on iterative set packing heuristic
double VAcyclicDecomposition::ComputeDecompositionSP(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed) {
	// Throughout the algorithm, we keep a partition of the factor graph
	// variables
	const std::vector<Factor*>& factors = fg->Factors();
	DisjointSet wset(fg->Cardinalities().size());
	std::set<unsigned int> E;
	for (unsigned int iter = 1; true; ++iter) {
		// Go through all the factors of the model and decide whether they are
		// still active.  A factor is inactive if any of the following
		// conditions are met:
		//
		// 1. All its adjacent variables are already mapped to one component,
		// 2. There are two or more factors joining the same components.
		//    If we would allow these factors to be active, the components
		//    could be merged, violating v-acyclicity.
		//
		// If 1. and 2. are false, the factor is 'active'.
		std::unordered_map<std::set<unsigned int>, unsigned int,
			boost::hash<std::set<unsigned int> > > EF;
		std::vector<bool> factor_active(factors.size(), true);
		for (unsigned int fi = 0; fi < factors.size(); ++fi) {
			const Factor* fac = factors[fi];
			const std::vector<unsigned int>& fac_vars = fac->Variables();

			// Build mapped edge set
			E.clear();
			for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
				E.insert(wset.FindSet(fac_vars[fvi]));
			// -> All mapped to one component?
			if (E.size() == 1) {
				factor_active[fi] = false;
				continue;
			}

			// Check whether the same component link exist already -> delete
			std::unordered_map<std::set<unsigned int>,
				unsigned int>::const_iterator efi = EF.find(E);
			if (efi != EF.end()) {
				factor_active[fi] = false;
				factor_active[efi->second] = false;
				continue;
			}

			// Is active, add to dupe checking list
			EF[E] = fi;
		}

		// Solve set packing problem
		std::vector<std::unordered_set<unsigned int> > S;
		std::vector<double> S_weights;
		std::vector<unsigned int> S_fi;
		std::vector<bool> S_is_selected;
		for (std::unordered_map<std::set<unsigned int>,
			unsigned int>::const_iterator efi = EF.begin(); efi != EF.end();
			++efi) {
			if (factor_active[efi->second] == false)
				continue;

#if 0
			std::cout << "VASP iter " << iter << ", adding:";
			for (std::set<unsigned int>::const_iterator
				esi = efi->first.begin(); esi != efi->first.end(); ++esi)
				std::cout << " " << *esi;
			std::cout << std::endl;
#endif

			// Add, negative cost to benefit
			S.push_back(std::unordered_set<unsigned int>(
				efi->first.begin(), efi->first.end()));
			S_weights.push_back(factor_weights[efi->second]);
			S_fi.push_back(efi->second);
		}
		// No further merging possible -> break
		if (S.empty())
			break;
		Grante::VAcyclicDecomposition::ComputeSetPacking(
			S, S_weights, S_is_selected, 5);	// TODO: make 5 configurable

		// Perform merging
		unsigned int merged = 0;
		for (unsigned int si = 0; si < S.size(); ++si) {
			if (S_is_selected[si] == false)
				continue;

			const std::vector<unsigned int>& fac_vars =
				factors[S_fi[si]]->Variables();
			assert(fac_vars.size() >= 2);
			for (unsigned int fvi = 1; fvi < fac_vars.size(); ++fvi) {
				wset.Link(wset.FindSet(fac_vars[0]),
					wset.FindSet(fac_vars[fvi]));
			}
			merged += 1;
		}
		// This can happen due to symmetry, where the Lagrangian relaxation
		// method fails to identify a solution
		if (merged == 0)
			break;
	}

	// Reconstruct final solution: every factor not mapped to one component is
	// removed.
	factor_is_removed.resize(factors.size());
	std::fill(factor_is_removed.begin(), factor_is_removed.end(), true);
	double obj = 0.0;
	for (unsigned int fi = 0; fi < factors.size(); ++fi) {
		const Factor* fac = factors[fi];
		const std::vector<unsigned int>& fac_vars = fac->Variables();

		// Build mapped edge set
		E.clear();
		for (unsigned int fvi = 0; fvi < fac_vars.size(); ++fvi)
			E.insert(wset.FindSet(fac_vars[fvi]));

		// All mapped to one component? -> factor is kept
		if (E.size() == 1) {
			factor_is_removed[fi] = false;
			obj += factor_weights[fi];
		}
	}
	return (obj);
}

double VAcyclicDecomposition::ComputeSetPacking(
	const std::vector<std::unordered_set<unsigned int> >& S,
	const std::vector<double>& S_weights,
	std::vector<bool>& S_is_selected, unsigned int lr_max_iter) {
	assert(S.size() == S_weights.size());

	// Random number generation
	boost::mt19937 rgen(RandomSource::GetGlobalRandomSeed());
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	// Fast vertex->edgeset map, VE[v]: list of set indices
	std::unordered_map<unsigned int, std::set<unsigned int> > VE;
	// Lagrange multipliers for constraints: \sum_{e, v \in e} x(e) <= 1.
	std::unordered_map<unsigned int, double> V_mu;
	std::unordered_map<unsigned int, double> V_mu_subg;
	for (unsigned int si = 0; si < S.size(); ++si) {
		for (std::unordered_set<unsigned int>::const_iterator
			ssi = S[si].begin(); ssi != S[si].end(); ++ssi) {
			VE[*ssi].insert(si);

			// Minimally random initialization to prevent symmetry problems:
			// if many costs have the same weight, and we initialize all
			// multipliers to zero, then we are at a dual degenerate ridge of
			// the cost function, where the subgradient is symmetric: we never
			// manage to break the symmetry, producing trivial solutions.
			V_mu[*ssi] = 1.0e-3*randu();
			V_mu_subg[*ssi] = 0.0;
		}
	}

	// Current, possibly infeasible solution
	std::vector<bool> cur_lr_solution(S.size(), false);
	std::vector<bool> cur_feas_solution(S.size(), false);
	double upper_bound = std::numeric_limits<double>::infinity();
	double lower_bound = -std::numeric_limits<double>::infinity();

	// Perform Lagrangian relaxation iterations
	for (unsigned int lr_iter = 1; lr_iter < lr_max_iter; ++lr_iter) {
		// Solve for x_e
		double cur_obj = 0.0;
		double feas_obj = 0.0;
		for (unsigned int si = 0; si < S.size(); ++si) {
			double si_obj = S_weights[si];
			for (std::unordered_set<unsigned int>::const_iterator
				ssi = S[si].begin(); ssi != S[si].end(); ++ssi) {
				si_obj += V_mu[*ssi];
			}
			cur_lr_solution[si] = (si_obj > 0.0) ? true : false;
			cur_obj += cur_lr_solution[si] ? si_obj : 0.0;

			cur_feas_solution[si] = cur_lr_solution[si];
			feas_obj += cur_feas_solution[si] ? S_weights[si] : 0.0;
		}
		// - \sum_v \mu_v
		for (std::unordered_map<unsigned int, double>::const_iterator
			vmi = V_mu.begin(); vmi != V_mu.end(); ++vmi) {
			cur_obj -= vmi->second;
		}

		// cur_obj provides a global upper bound on the solution
		if (cur_obj < upper_bound)
			upper_bound = cur_obj;

		// Check feasibility.  If solution is infeasible, produce a primal
		// feasible solution
		double cslackness = 0.0;
		double feasible = true;
		double subg_norm = 0.0;
		for (std::unordered_map<unsigned int, double>::iterator
			vmi = V_mu.begin(); vmi != V_mu.end(); ++vmi) {
			double mu_v_sd = -1.0;
			const std::set<unsigned int>& eset = VE[vmi->first];
			for (std::set<unsigned int>::const_iterator ei = eset.begin();
				ei != eset.end(); ++ei) {
				mu_v_sd += cur_lr_solution[*ei] ? 1.0 : 0.0;
			}
			cslackness += vmi->second * mu_v_sd;

			// Constraint is violated, adjust multiplier
			if (mu_v_sd > 0.0) {
				feasible = false;

				// Remove sets until feasible
				double infeas_m = mu_v_sd;
				for (std::set<unsigned int>::const_iterator ei = eset.begin();
					ei != eset.end(); ++ei) {
					// Sufficient number of edges removed? -> done
					if (infeas_m <= 1.0e-8)
						break;

					if (cur_lr_solution[*ei] == false)
						continue;

					infeas_m -= 1.0;

					// Was the set removed already? -> nothing to do
					if (cur_feas_solution[*ei] == false)
						continue;

					// Remove edge
					cur_feas_solution[*ei] = false;
					feas_obj -= S_weights[*ei];
				}
				assert(infeas_m <= 1.0e-8);
			}

			// Adjust Lagrange multiplier by projected gradient method
			subg_norm += mu_v_sd * mu_v_sd;
			V_mu_subg[vmi->first] = mu_v_sd;
		}

		// Compute step size: Polyak
		double beta_m = 1.0;
		double beta = (1.0 + beta_m) /
			(beta_m + static_cast<double>(lr_iter));
		double alpha = 0.0;
		if (subg_norm >= 1.0e-8) {
			alpha = (beta * (cur_obj - feas_obj)) / subg_norm;
		}
		for (std::unordered_map<unsigned int, double>::iterator
			vmi = V_mu_subg.begin(); vmi != V_mu_subg.end(); ++vmi) {
			// TODO: use subg_norm for LR mult update
			V_mu[vmi->first] -= alpha * vmi->second;
			V_mu[vmi->first] = std::min(0.0, V_mu[vmi->first]);
		}

		// Update lower bound and solution
		if (feas_obj > lower_bound) {
			lower_bound = feas_obj;
			S_is_selected = cur_feas_solution;
		}
#if 0
		std::cout << "set packing LR iter " << lr_iter
			<< ", lb " << lower_bound << ", ub " << upper_bound
			<< ", |subg| " << std::sqrt(subg_norm)
			<< std::endl;
#endif

		// Sufficient optimality condition:
		// primal feasible and \mu'v(y(\mu)) = cslackness = 0.
		if (feasible && std::fabs(cslackness) <= 1.0e-8) {
			S_is_selected = cur_lr_solution;
			return (cur_obj);
		}
	}
	return (lower_bound);
}

double VAcyclicDecomposition::ComputeDecomposition(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed,
	unsigned int csa_steps, double csa_t0, double csa_tfinal) {
	const std::vector<Factor*>& factors = fg->Factors();
	assert(factor_weights.size() == factors.size());

	// Initialization: all factors removed
	std::unordered_set<unsigned int> removed_factors(factors.size());
	for (unsigned int fi = 0; fi < factors.size(); ++fi)
		removed_factors.insert(fi);

	// Components
	std::vector<unsigned int> node_to_comp;
	const std::vector<unsigned int>& card = fg->Cardinalities();
	node_to_comp.reserve(card.size());
	std::vector<std::unordered_set<unsigned int> > comps(card.size());
	for (unsigned int ni = 0; ni < card.size(); ++ni) {
		node_to_comp[ni] = ni;
		comps[ni].insert(ni);
	}

	// Objective: weights of all included factors (zero)
	double obj = 0.0;

	// Best solution so far
	std::unordered_set<unsigned int>
		best_removed_factors(removed_factors);
	double best_obj = obj;

	// Random number generators: factor index
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(
		reinterpret_cast<size_t>(fg) ^ std::time(0))+14);
	boost::uniform_int<unsigned int> rdestd(0,
		static_cast<boost::uint32_t>(factors.size()-1));
	boost::variate_generator<boost::mt19937,
		boost::uniform_int<unsigned int> > rand_fi(rgen, rdestd);

	// Random number generator: Metropolis chain
	boost::mt19937 rgen2(static_cast<const boost::uint32_t>(
		reinterpret_cast<size_t>(fg) ^ std::time(0))+13);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > rand_m(rgen2, rdestu);

	// Calculate exponential multiplier alpha such that T(sa_steps)=Tfinal.
	double alpha = std::exp(std::log(csa_tfinal / csa_t0) /
		static_cast<double>(csa_steps));
	for (unsigned int k = 1; k <= csa_steps; ++k) {
		// Exponential schedule
		double temperature = csa_t0 * std::pow(alpha, static_cast<double>(k));
#if 0
		std::cout << "iter " << k << ", temp " << temperature
			<< ", best obj " << best_obj << std::endl;
#endif

		// One epoch
		for (unsigned int fi_d = 0; fi_d < factors.size(); ++fi_d) {
			unsigned int fi = rand_fi();
			// Special case of one pass: linear
			if (csa_steps == 1)
				fi = fi_d;

			double delta_E = std::numeric_limits<double>::signaling_NaN();
			bool is_in_G = (removed_factors.count(fi) == 0);
			if (is_in_G) {
				// Factor is currently in G, can be removed.
				delta_E = -factor_weights[fi];
			} else {
				// Factor is currently removed, check whether it can be added.
				if (IsComponentBridge(node_to_comp, comps, fi) == false)
					continue;	// not possible, reject

				delta_E = factor_weights[fi];
			}
			// Reject
			if (delta_E <= 0.0 && rand_m() >= std::exp(delta_E/temperature))
				continue;

			// Accept
			if (is_in_G) {
				// Split components adjacent to the factor
				SplitComponents(removed_factors, node_to_comp, comps, fi);
			} else {
				// Merge components adjacent to the factor
				MergeComponents(node_to_comp, comps, factors[fi]);
				removed_factors.erase(fi);	// its in the graph now
			}
			obj += delta_E;

			// Keep track of best solution
			if (obj > best_obj) {
				best_obj = obj;
				best_removed_factors = removed_factors;
			}
		}
	}

	// Return list of removed factors
	factor_is_removed.resize(factors.size());
	std::fill(factor_is_removed.begin(), factor_is_removed.end(), false);
	for (std::unordered_set<unsigned int>::const_iterator
		ri = best_removed_factors.begin(); ri != best_removed_factors.end();
		++ri) {
		factor_is_removed[*ri] = true;
	}
	return (best_obj);
}

bool VAcyclicDecomposition::IsComponentBridge(
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	unsigned int factor_index) const {
	const Factor* fac = fg->Factors()[factor_index];
	const std::vector<unsigned int>& fvars = fac->Variables();

	// 1. Collect adjacent factor sets for each component
	std::vector<std::unordered_set<unsigned int> >
		comp_facset(fvars.size());;
	for (unsigned int fvi = 0; fvi < fvars.size(); ++fvi) {
		const std::unordered_set<unsigned int>& cur_vars =
			comps[node_to_comp[fvars[fvi]]];
		assert(cur_vars.empty() == false);
		for (std::unordered_set<unsigned int>::const_iterator
			cvi = cur_vars.begin(); cvi != cur_vars.end(); ++cvi) {
			const std::set<unsigned int>& cur_facset =
				fgu.AdjacentFactors(*cvi);
			comp_facset[fvi].insert(cur_facset.begin(), cur_facset.end());
		}
		// Do not consider the factor of interest
		comp_facset[fvi].erase(factor_index);
	}

	// 2. Find factors that would link the components
	for (unsigned int c1 = 0; c1 < comp_facset.size(); ++c1) {
		const std::unordered_set<unsigned int>& c1_fset = comp_facset[c1];
		for (unsigned int c2 = c1 + 1; c2 < comp_facset.size(); ++c2) {
			for (std::unordered_set<unsigned int>::const_iterator
				c2i = comp_facset[c2].begin(); c2i != comp_facset[c2].end();
				++c2i) {
				// Check whether another factor between the components exists.
				// If so, factor_index is no bridge.
				if (c1_fset.count(*c2i) > 0)
					return (false);
			}
		}
	}
	return (true);
}

void VAcyclicDecomposition::SplitComponents(
	std::unordered_set<unsigned int>& removed_factors,
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	unsigned int fac_index) const {
	// Obtain variable indices (these are all in the same component)
	const Factor* fac = fg->Factors()[fac_index];
	const std::vector<unsigned int>& fvar = fac->Variables();
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi) {
		assert(node_to_comp[fvar[fvi]] == node_to_comp[fvar[0]]);
	}

	// Save joined component, remove factor from graph
#if 0
	std::unordered_map<unsigned int> comp_old;
	comp_old.swap(comps[node_to_comp[fvar[0]]]);
#endif
	comps[node_to_comp[fvar[0]]].clear();
	removed_factors.insert(fac_index);

	// Relabel tree rooted in factor-adjacent node to a new component
	// FIXME: this seems to be very slow
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi) {
		unsigned int vi = fvar[fvi];	// var and component index
		std::unordered_set<unsigned int>& comp_vi = comps[vi];
		assert(comp_vi.empty());

		// store (var_idx, came_from_factor_index) in queue
		std::queue<std::pair<unsigned int, unsigned int> > var_q;
		var_q.push(std::pair<unsigned int, unsigned int>(vi, fac_index));

		// Recurse on tree-structured partial component, relabeling in the
		// process
		while (var_q.empty() == false) {
			const std::pair<unsigned int, unsigned int>& cur = var_q.front();

			// Relabel this node
			comp_vi.insert(cur.first);
			node_to_comp[cur.first] = vi;

			// Insert adjacent nodes
			const std::set<unsigned int>& cur_facset =
				fgu.AdjacentFactors(cur.first);
			for (std::set<unsigned int>::const_iterator
				fi = cur_facset.begin(); fi != cur_facset.end(); ++fi) {
				if (*fi == cur.second)
					continue;	// this is the factor we came from

				// If factor is removed, skip
				if (removed_factors.count(*fi) > 0)
					continue;

				// Factor is valid, add all its variables except this one
				const Factor* cur_factor = fg->Factors()[*fi];
				const std::vector<unsigned int>& cur_facvar =
					cur_factor->Variables();
				for (std::vector<unsigned int>::const_iterator
					cfvi = cur_facvar.begin(); cfvi != cur_facvar.end();
					++cfvi) {
					if (*cfvi == cur.first)
						continue;	// do not add ourselves again

					// Put variable into the queue
					var_q.push(std::pair<unsigned int, unsigned int>(
						*cfvi, *fi));
				}
			}
			var_q.pop();
		}
	}
}

void VAcyclicDecomposition::MergeComponents(
	std::vector<unsigned int>& node_to_comp,
	std::vector<std::unordered_set<unsigned int> >& comps,
	const Factor* fac) const {
	// Obtain component indices (these are all be disjoint)
	const std::vector<unsigned int>& fvar = fac->Variables();
	std::vector<unsigned int> comp_indices(fvar.size());
	for (unsigned int fvi = 0; fvi < fvar.size(); ++fvi)
		comp_indices[fvi] = node_to_comp[fvar[fvi]];

	// Map them all to the first component
	unsigned int target = comp_indices[0];
	for (unsigned int ci = 1; ci < comp_indices.size(); ++ci) {
		// 1. relabel all nodes to the new component
		for (std::unordered_set<unsigned int>::const_iterator
			si = comps[comp_indices[ci]].begin();
			si != comps[comp_indices[ci]].end(); ++si) {
			node_to_comp[*si] = target;
		}
		// 2. merge component sets
		comps[target].insert(comps[comp_indices[ci]].begin(),
			comps[comp_indices[ci]].end());
		comps[comp_indices[ci]].clear();
	}
}

double VAcyclicDecomposition::ComputeDecompositionExact(
	const std::vector<double>& factor_weights,
	std::vector<bool>& factor_is_removed, double opt_eps) {
	// Call reverse search enumeration function
	ReverseSearch rsearch(this, factor_weights, opt_eps);
	double obj = rsearch.Search(factor_is_removed);

	return (obj);
}

VAcyclicDecomposition::ReverseSearch::ReverseSearch(VAcyclicDecomposition* vac,
	const std::vector<double>& factor_weights, double opt_eps)
	: factor_count(vac->fg->Factors().size()), factor_weights(factor_weights),
		vac(vac), best_global(0.0),
		best_factor_is_removed(vac->fg->Factors().size(), true),
		opt_eps(opt_eps) {
}

double VAcyclicDecomposition::ReverseSearch::Search(
	std::vector<bool>& factor_is_removed_out) {
	std::list<unsigned int> factor_in;	// start with empty set
	std::list<unsigned int> factor_cand;
	std::set<unsigned int> factor_out;
	for (unsigned int fi = 0; fi < factor_count; ++fi) {
		factor_cand.push_back(fi);
		factor_out.insert(fi);
	}

	examined = 0;
	std::cout << "SEARCH BEGIN" << std::endl;
	DisjointSetBT dset(factor_count);
	Recurse(0.0, dset, factor_in, factor_cand, factor_out);
	std::cout << "SEARCH END" << std::endl << std::endl;

	factor_is_removed_out = best_factor_is_removed;

	return (best_global);
}

// This has polynomial delay at O(F log F + F N(F)), where the output
// operation is a an enumeration of a vac structure.  So in effect if the
// maximum factor scope size N(F) is small, the runtime is an almost linear
// function in the number of factors of the graph.
std::set<unsigned int>::const_iterator
VAcyclicDecomposition::ReverseSearch::Recurse(double obj, DisjointSetBT& dset,
	std::list<unsigned int>& factor_in, std::list<unsigned int>& factor_cand,
	std::set<unsigned int>& factor_out)
{
	examined += 1;
	if (examined % 1000000 == 1) {
		std::cout << "Recurse(obj=" << obj << ", factor_in (" << factor_in.size()
			<< "), factor_cand (" << factor_cand.size() << "), "
			<< "factor_out (" << factor_out.size() << "))"
			<< " best: " << best_global << std::endl;
	}

	// 1. Determine whether this tree is still vac.
	// When we find that it is not vac, return.
	//
	// Complexity: O(F log log F)

	// We need to check all factors not in factor_in
	for (std::set<unsigned int>::const_iterator foi = factor_out.begin();
		foi != factor_out.end(); ++foi) {
		const Factor* fac = vac->fg->Factors()[*foi];
		const std::vector<unsigned int>& fvars = fac->Variables();

		for (unsigned int fvi1 = 0; fvi1 < fvars.size(); ++fvi1) {
			unsigned int root1 = dset.Find(fvars[fvi1]);
			for (unsigned int fvi2 = fvi1 + 1; fvi2 < fvars.size(); ++fvi2) {
				unsigned int root2 = dset.Find(fvars[fvi2]);
#if 0
				std::cout << "   fi " << *foi << ", " << fvars[fvi1]
					<< "--" << fvars[fvi2] << " map to roots "
					<< root1 << "--" << root2 << std::endl;
#endif

				if (root1 == root2) {
#if 0
					std::cout << "   * not vac, returning" << std::endl;
#endif
					return (foi);
				}
			}
		}
	}

	// 2. Current set is vac, so update objective
	//
	// Complexity: O(1)
	if (obj > best_global) {
		best_global = obj;
		best_factor_is_removed.resize(factor_count);
		std::fill(best_factor_is_removed.begin(),
			best_factor_is_removed.end(), true);
		for (std::list<unsigned int>::const_iterator fil = factor_in.begin();
			fil != factor_in.end(); ++fil) {
			best_factor_is_removed[*fil] = false;
		}
		std::cout << "   found new best vac with obj " << obj << std::endl;
	}

	// 3. Compute bound and if it turns out that we cannot beat the best
	// solution so far, stop recursion
	//
	// Complexity: O(F)
	double obj_upper_bound = obj;
	for (std::list<unsigned int>::const_iterator fci = factor_cand.begin();
		fci != factor_cand.end(); ++fci) {
		obj_upper_bound += std::max(0.0, factor_weights[*fci]);
	}
#if 0
	if (examined % 10000 == 1) {
		std::cout << "   upper bound at this node: " << obj_upper_bound << std::endl;
	}
#endif

	if (obj_upper_bound <= best_global + opt_eps)
		return (factor_out.end());	// cannot beat best global one

	// 4. For all remaining factors, add and recurse
	//
	// Complexity: O(F N(F) + F log F) + recursion
	//  i) Sort by weight
	factor_cand.sort(
		[this](unsigned int v1, unsigned int v2) -> bool {
			return (this->factor_weights[v1] > this->factor_weights[v2]);
		});

	// ii) Recurse for each, greedily
	std::list<unsigned int>::iterator fii = factor_cand.begin();
	while (fii != factor_cand.end()) {
		// Skip factors that would not contribute anything to the objective
		unsigned int fi = *fii;
		if (factor_weights[fi] <= 0.0) {
			++fii;
			continue;
		}

		const Factor* fac = vac->fg->Factors()[fi];
		unsigned int union_count = AddFactor(dset, fac);

		// Prepare recursion
		factor_in.push_back(fi);	// more factor into retained factor set
		factor_out.erase(fi);

		// Note that here we change the list factor_cand, so that we realize
		// the reverse search ordering and avoid enumerating duplicates
		fii = factor_cand.erase(fii);	// erase factor from candidate list
		std::list<unsigned int> remaining_cand(fii, factor_cand.end());

		// Recurse
		std::set<unsigned int>::iterator conflict_iter =
			Recurse(obj + factor_weights[fi], dset, factor_in,
				remaining_cand, factor_out);

		// Handling discovered vac conflicts in the remaining candidate set
		if (conflict_iter != factor_out.end()) {
			unsigned int conflict_fi = *conflict_iter;

#if 0
			if (std::find(factor_cand.begin(), factor_cand.end(), conflict_fi)
				!= factor_cand.end()) {
				std::cout << "   removed conflict" << std::endl;
			}
#endif
			factor_cand.remove_if([conflict_fi](unsigned int k) -> bool {
				return (conflict_fi == k);
			});
			fii = factor_cand.begin();
		}

		factor_out.insert(fi);
		factor_in.pop_back();

		// Undo
		for (unsigned int uc = 0; uc < union_count; ++uc)
			dset.Deunion();
	}
	return (factor_out.end());
}

unsigned int VAcyclicDecomposition::ReverseSearch::AddFactor(DisjointSetBT& dset,
	const Factor* fac) {
	const std::vector<unsigned int>& fvars = fac->Variables();
	unsigned int union_count = 0;

	// Union all variables in the factor scope N(F)
	unsigned int root = dset.Find(fvars[0]);
	for (unsigned int fvi = 1; fvi < fvars.size(); ++fvi) {
		unsigned int root2 = dset.Find(fvars[fvi]);
		assert(root != root2);
		root = dset.Union(root, root2);
		union_count += 1;
	}

	return (union_count);
}

}


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

  Defines the logical_ hierarchy

  @copyright 2016 NumScale SAS

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

**/
//==================================================================================================
#ifndef BOOST_SIMD_META_HIERARCHY_LOGICAL_HPP_INCLUDED
#define BOOST_SIMD_META_HIERARCHY_LOGICAL_HPP_INCLUDED

#include <boost/simd/detail/dispatch/meta/value_of.hpp>
#include <boost/simd/detail/dispatch/meta/model_of.hpp>
#include <boost/simd/meta/real_of.hpp>
#include <boost/simd/detail/dispatch/hierarchy/fundamental.hpp>
#include <boost/simd/detail/dispatch/property_of.hpp>

namespace boost { namespace simd
{
  /*!
    @ingroup group-sdk
    @brief Logical type hierarchy tag

    Types classified as logical_ have properties similar to logical<T>

    @tparam T Base hierarchy
  **/
  template<typename T> struct logical_ : boost::dispatch::fundamental_<T>
  {
    using parent = boost::dispatch::fundamental_<T>;
  };

  template<typename T>
  struct real_of< boost::simd::logical<T> >
  {
    using type = boost::simd::logical<T>;
  };
} }

namespace boost { namespace dispatch
{
  namespace ext
  {
    template<typename T> struct model_of<boost::simd::logical<T>>
    {
      template<typename X> struct apply { using type = boost::simd::logical<X>; };
    };

    template<typename T> struct value_of<boost::simd::logical<T>>
    {
      using type = T;
    };

    template<typename T, typename Origin>
    struct property_of<boost::simd::logical<T>,Origin>
    {
      using type = boost::simd::logical_<Origin>;
    };

    template<typename T>
    struct scalar_of< simd::logical<T> >
    {
      using type = boost::simd::logical<T>;
    };
} } }

#endif


/*
 * File:   AICpanda.cpp
 * Author: Corrado Pezzato, CoR, TU Deft
 *
 * Created on September 4th, 2019
 *
 * This node allows to control the 7DOF Franka Emika Panda robot arm through
 * Active Inference
 *
 * The control is in joint space, sending torque commands and using position and
 * velocity feedback. The robot moves to the desired goal from the initial pose.
 *
 * ---> Usage:: ./<folder_code>/AICpanda.cpp <fci-ip>
 *
 */

#include <iostream>
#include <array>
#include <franka/robot.h>
#include <franka/duration.h>
#include <franka/exception.h>
#include <Eigen/Core>
#include <Eigen/Dense>

#include "examples_common.h"

int main(int argc, char** argv) {
 // Check if the user provided the hostname or IP
 if (argc != 2) {
   std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
   return -1;
 }
 // If hostname or IP provided, try to connect to the robot
 try {

   // Define an object for the Panda to handle the control signals and read
   // the sensors. Using the method robot.control() you read the
   // joint values at 1kHz
   franka::Robot robot(argv[1]);
   setDefaultBehavior(robot);

   // Set collision behavior
   robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                              {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                              {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}},
                              {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}});

   // Define controller
   /////////// Controller parameters /////////////
   // Auxiliary variables
   double time = 0.0;
   double maxTime = 100.0;
   // Variances for internal belief and sensory input, learning rates, integration step
   double var_q, var_qdot, var_mu, var_muprime, k_mu, k_a, h;
   // Precision matrices, diagonal matrices with the inverce of the variance
   Eigen::Matrix<double, 7, 7> SigmaP_yq0, SigmaP_yq1, SigmaP_mu, SigmaP_muprime;
   // Belief about the states and their derivatives mu, mu', mu'', joint states, desired value, control
   Eigen::Matrix<double, 7, 1> mu, mu_p, mu_pp, mu_dot, mu_dot_p, mu_dot_pp, jointPos, jointVel, mu_d, u;

   // Set desired goal
   // Center
   mu_d << 0.0345169,-0.167844,-0.000332711,-1.74526,0.00905814,1.56589,0.824567;

   // Right
   //mu_d << 0.15086,0.235858,-0.458875,-2.44786,0.0599695,2.6241,0.450365;

   //////////// Controller tuning //////////////
   // Variances associated with the beliefs and the sensory inputs
   var_mu = 5.0;
   var_muprime = 10.0;
   var_q = 1;
   var_qdot = 1;

   // Learning rates for the gradient descent
   k_mu = 11.67;
   k_a = 200;

   // Integration step
   h = 0.001;

   // Precision matrices (first set them to zero then populate the diagonal)
   SigmaP_yq0 = Eigen::Matrix<double, 7, 7>::Zero();
   SigmaP_yq1 = Eigen::Matrix<double, 7, 7>::Zero();
   SigmaP_mu = Eigen::Matrix<double, 7, 7>::Zero();
   SigmaP_muprime = Eigen::Matrix<double, 7, 7>::Zero();
   //////////////////////////////////////////////

   // Read robot initial state to set the initial belief
   franka::RobotState initial_state = robot.readOnce();
   // Initialization
   for( int i = 0; i < 7; i = i + 1 ) {
     SigmaP_yq0(i,i) = 1/var_q;
     SigmaP_yq1(i,i) = 1/var_qdot;
     SigmaP_mu(i,i) = 1/var_mu;
     SigmaP_muprime(i,i) = 1/var_muprime;
     // Internal belief starting from initial pose
     jointPos(i) = initial_state.q[i];
     jointVel(i) = initial_state.dq[i];
   }
   // Initial belief
   mu = jointPos;
   mu_p = jointVel;
   mu_pp << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
   // Initial control actions
   u << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;

   // Controller callback for RT loop
   auto my_AIC_controller_callback = [&](const franka::RobotState& robot_state,
                                     franka::Duration period) -> franka::Torques {
     time += period.toSec();
     // Save the robot state for algebric manipulation
     for( int i = 0; i < 7; i = i + 1 ) {
       // Set current sensory input
       jointPos(i) = robot_state.q[i];
       jointVel(i) = robot_state.dq[i];
     }

     /////////// Active inference ///////////
     // Belief update
     mu_dot = mu_p - k_mu*(-SigmaP_yq0*(jointPos-mu)+SigmaP_mu*(mu_p+mu-mu_d));
     mu_dot_p = mu_pp - k_mu*(-SigmaP_yq1*(jointVel-mu_p)+SigmaP_mu*(mu_p+mu-mu_d)+SigmaP_muprime*(mu_pp+mu_p));
     mu_dot_pp = - k_mu*(SigmaP_muprime*(mu_pp+mu_p));

     mu = mu + h*mu_dot;             // Belief about the position
     mu_p = mu_p + h*mu_dot_p;       // Belief about motion of mu
     mu_pp = mu_pp + h*mu_dot_pp;    // Belief about motion of mu'

     // Compute control actions
     u = u-h*k_a*(SigmaP_yq1*(jointVel-mu_p)+SigmaP_yq0*(jointPos-mu));

    // Build control signal and send it to the robot
    franka::Torques tau_cmd = {{u(0), u(1), u(2), u(3), u(4), u(5), u(6)}};
    // Set torques to zero for testing
    // franka::Torques tau_cmd = {{0, 0, 0, 0, 0, 0, 0}};

    if (time >= maxTime) {
      std::cout << std::endl << "Finished motion, shutting down demo" << std::endl;
      return franka::MotionFinished(tau_cmd);
    }
    return tau_cmd;
   };
   std::cout << "WARNING: Make sure sure that the robot has enough space to move. Keep in mind that "
                "collision thresholds are set to high values."
             << std::endl
             << "Press Enter to start the demo..." << std::endl;
   std::cin.ignore();
   // start real-time control loop
   robot.control(my_AIC_controller_callback);
   // Catch the exemptions if any connection errors occurred
 } catch (franka::Exception const& e) {
  std::cout << e.what() << std::endl;
     return -1;
   }
  return 0;
}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE PipelineNoMultiSourceTest0

#include <memory>

#include <boost/test/unit_test.hpp>
#include <filereader.hpp>
#include <popart/builder.hpp>
#include <popart/dataflow.hpp>
#include <popart/ir.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/ipucopy.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/nll.hpp>
#include <popart/op/restore.hpp>
#include <popart/op/stash.hpp>
#include <popart/optimizer.hpp>
#include <popart/sgd.hpp>
#include <popart/tensor.hpp>
#include <popart/tensordata.hpp>
#include <popart/testdevice.hpp>

// We confirm that a model which would normally have an IpuCopyOp
// with multiple sources, can be pipelined as multi-source IpuCopyOps
// are not created when pipelining is enabled.

BOOST_AUTO_TEST_CASE(PipelineNoMultiSourceTest0) {

  using namespace popart;

  auto test = [](bool withPipelining) {
    auto builder     = Builder::create();
    auto aiOnnx      = builder->aiOnnxOpset9();
    auto aiGraphcore = builder->aiGraphcoreOpset1();
    TensorInfo info{"FLOAT", std::vector<int64_t>{4, 6}};
    std::vector<float> wVals(4 * 6, 1.0f);
    ConstVoidData wData = {wVals.data(), info};

    auto input1 = builder->addInputTensor(info);
    auto w1     = builder->addInitializedInputTensor(wData);

    auto input5 = builder->addInputTensor(info);
    auto w5     = builder->addInitializedInputTensor(wData);

    auto input2 = builder->addInputTensor(info);
    auto w2     = builder->addInitializedInputTensor(wData);

    auto input4 = builder->addInputTensor(info);
    auto w4     = builder->addInitializedInputTensor(wData);

    auto input3 = builder->addInputTensor(info);
    auto w3     = builder->addInitializedInputTensor(wData);

    auto getPipe = [&aiOnnx, &builder, &aiGraphcore](
                       TensorId id1, TensorId id2, std::string num, int vgid) {
      auto act = aiOnnx.add({id1, id2}, "add1-" + num);
      builder->virtualGraph(act, vgid);
      act = aiOnnx.sigmoid({act}, "sigmoid-" + num);
      builder->virtualGraph(act, vgid);
      act = aiOnnx.cos({act}, "cos-" + num);
      builder->virtualGraph(act, vgid);
      act = aiOnnx.mul({act, id1}, "mul-" + num);
      builder->virtualGraph(act, vgid);

      act = aiOnnx.add({id2, act}, "add2-" + num);
      builder->virtualGraph(act, vgid);

      act = aiOnnx.relu({act}, "relu-" + num);
      builder->virtualGraph(act, vgid);

      act = aiOnnx.sigmoid({act}, "sigmoid-" + num);
      builder->virtualGraph(act, vgid);

      act = aiGraphcore.scale({act}, 5, "scale-" + num);
      builder->virtualGraph(act, vgid);

      return act;
    };

    auto act1 = getPipe(input1, w1, "1", 0);
    auto act2 = getPipe(input2, w2, "2", 1);
    auto act3 = getPipe(input3, w3, "3", 2);
    auto act4 = getPipe(input4, w4, "4", 3);
    auto act5 = getPipe(input5, w5, "5", 4);

    auto act = aiOnnx.add({act1, act2}, "add1-final");
    builder->virtualGraph(act, 5);
    act = aiOnnx.sub({act, act3}, "sub-final");
    builder->virtualGraph(act, 5);
    act = aiOnnx.mul({act, act4}, "mul-final");
    builder->virtualGraph(act, 5);
    act = aiOnnx.add({act, act5}, "add2-final");
    builder->virtualGraph(act, 5);
    act = aiOnnx.sigmoid({act}, "sigmoid-final");
    builder->virtualGraph(act, 5);
    act = aiOnnx.relu({act}, "relu-final");
    builder->virtualGraph(act, 5);
    act = builder->aiGraphcoreOpset1().l1loss({act}, 0.1);
    builder->virtualGraph(act, 5);

    // in1, w1 in2,w2  in3, w3  in4, w4   in5, w5
    // |       |       |        |         |
    // |P      |       |        |         |
    // |I      |       |        |         |
    // |P      |       |        |         |
    // |E      |       |        |         |
    // |1      |       |        |         |P
    // |       |       |        |         |I
    // |       |       |        |         |P
    // ---------       |        |         |E
    //    |            |        |         |5
    //   add           |        |         |
    //    |            |        |         |
    //    -------------|        |         |
    //         |                |         |
    //        sub               |         |
    //         |                |         |
    //         -----------------|         |
    //                 |                  |
    //                mul                 |
    //                 |                  |
    //                 --------------------
    //                        |
    //                       add
    //                        |
    //                      sigmoid
    //                        |
    //                       relu
    //                        |
    //

    auto proto      = builder->getModelProto();
    auto modelProto = io::getModelFromString(proto);
    auto dataFlow   = DataFlow(20, {{act1, AnchorReturnType("All")}});

    SessionOptions userOptions;
    userOptions.virtualGraphMode = VirtualGraphMode::Manual;
    userOptions.enablePipelining = withPipelining;

    constexpr int64_t nIpus{6};
    auto optimizer = ConstSGD(0.01);

    auto device = createTestDevice(TEST_TARGET, nIpus);
    Ir ir;
    ir.prepare({modelProto,
                InputShapeInfo(),
                dataFlow,
                act,
                &optimizer,
                *device,
                userOptions,
                Patterns(PatternsLevel::Default)});

    int64_t nMultiSource = 0;

    // prepare the final log:
    std::array<std::stringstream, nIpus> sss;

    auto sched = ir.getOpSchedule({}, RequireOptimalSchedule::Yes);
    for (auto op : sched) {
      auto ipuCopyOp = dynamic_cast<IpuCopyOp *>(op);
      if (!ipuCopyOp) {
        auto vgid = op->getVirtualGraphId();
        sss[vgid] << op->wrtLossStr() << "  " << op->getName() << "     "
                  << op->str() << "\n";
      } else {
        int64_t vgid = 0;
        if (ipuCopyOp->getMinSourceIpu() == ipuCopyOp->getMaxSourceIpu()) {
          vgid = ipuCopyOp->getSourceIpu();
        } else {
          nMultiSource += 1;
        }

        sss[vgid] << op->wrtLossStr() << "  " << op->getName() << "     "
                  << op->str() << "      " << ipuCopyOp->getFromToStr() << "\n";
      }
    }

    for (int64_t ipu = 0; ipu < nIpus; ++ipu) {
      std::cout << "On IPU " << ipu << std::endl;
      std::cout << sss[ipu].str() << "\n\n" << std::endl;
    }

    if (withPipelining) {
      BOOST_CHECK(nMultiSource == 0);
    } else {
      std::cout << "Verifying that without pipelining there is a multi-source "
                   "ipu copy. This is not strictly necessary, and future "
                   "changes to sharding might change this"
                << std::endl;
      BOOST_CHECK(nMultiSource > 0);
    }
  };
  test(false);
  test(true);
}


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

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

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

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

 /*!
  @ingroup group-ieee
    Function object implementing fast_frexp capabilities

    Computes fast_frexp value of its parameter.

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

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

#endif


#include <pcl/io/pcd_io.h>
#include <pcl/features/integral_image_normal.h>
#include <boost/thread/thread.hpp>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/common/transforms.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/impl/passthrough.hpp>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <iostream>
#include <unordered_map>
#include <algorithm>
#include<pcl/filters/voxel_grid.h>

using namespace std;
const float PI = 3.1415926;
template<typename T> string toString(const T& t) {
	ostringstream oss;
	oss << t;
	return oss.str();
}
static int64_t gtm() {
	struct timeval tm;
	gettimeofday(&tm, 0);
	int64_t re = (((int64_t) tm.tv_sec) * 1000 * 1000 + tm.tv_usec);
	return re;
}

float Polar_angle_cal(float x, float y){
  float temp_tangle = 0;
  if(x== 0 && y ==0){
     temp_tangle = 0;
  }else if(y>=0){
     temp_tangle = (float)atan2(y,x);
  }else if(y<=0){
     temp_tangle = (float)atan2(y,x)+2*PI;
  }
 return temp_tangle;
}

struct PointAPR{
   float azimuth;
   float polar_angle;
   float range;
};

struct Voxel{
   bool haspoint = false;
   int cluster = -1;
   vector<int> index;
};

float minrange = 3;
float maxrange = 3;
float minazimuth = 0;
float maxazimuth = 0;
float deltaA = 2;
float deltaR = 0.35;
float deltaP = 1.2;
int length = 0;
int width = 0;
int height = 0;

bool compare_cluster(pair<int,int> a,pair<int,int> b){
    return a.second>b.second;
}//升序

template <typename PointT>
void calculateAPR(const pcl::PointCloud<PointT>& cloud_IN, vector<PointAPR>& vapr){
     for (int i =0; i<cloud_IN.points.size(); ++i){
           PointAPR par;
           par.polar_angle = Polar_angle_cal(cloud_IN.points[i].x, cloud_IN.points[i].y);
           par.range = sqrt(cloud_IN.points[i].x*cloud_IN.points[i].x+cloud_IN.points[i].y*cloud_IN.points[i].y);
           par.azimuth =(float) atan2(cloud_IN.points[i].z,par.range);
           if(par.azimuth < minazimuth){
                 minazimuth = par.azimuth;
           }
           if(par.azimuth > maxazimuth){
                 maxazimuth = par.azimuth;
           }
           if(par.range < minrange){
                 minrange = par.range;
           }
           if(par.range > maxrange){
                 maxrange = par.range;
           }
           vapr.push_back(par);
    }

  length = round((maxrange - minrange)/deltaR);
  width = 301;
  height = round((maxazimuth - minazimuth)/deltaA);

}

void build_hash_table(const vector<PointAPR>& vapr, unordered_map<int, Voxel> &map_out){
     vector<int> ri;
     vector<int> pi;
     vector<int> ai;
     for(int i =0; i< vapr.size(); ++i){
           int azimuth_index = round(((vapr[i].azimuth-minazimuth)*180/PI)/deltaA);
           int polar_index = round(vapr[i].polar_angle*180/PI/deltaP);
           int range_index = round((vapr[i].range-minrange)/deltaR);
           int voxel_index = (polar_index*(length+1)+range_index)+azimuth_index*(length+1)*(width+1);
           ri.push_back(range_index);
           pi.push_back(polar_index);
           ai.push_back(azimuth_index);
           unordered_map<int, Voxel>::iterator it_find;
           it_find = map_out.find(voxel_index);
           if (it_find != map_out.end()){
                it_find->second.index.push_back(i);

           }else{
                Voxel vox;
                vox.haspoint =true;
                vox.index.push_back(i);
                vox.index.swap(vox.index);
                map_out.insert(make_pair(voxel_index,vox));
           }

    }
    auto maxPosition = max_element(ai.begin(), ai.end());
    auto maxPosition1 = max_element(ri.begin(), ri.end());
    auto maxPosition2 = max_element(pi.begin(), pi.end());
    cout<<*maxPosition<<" "<<*maxPosition1<<" "<<*maxPosition2<<endl;

}

void find_neighbors(int polar, int range, int azimuth, vector<int>& neighborindex){
	for (int z = azimuth - 1; z <= azimuth + 1; z++){
		if (z < 0 || z >round((maxazimuth-minazimuth)*180/PI/deltaA)){
			continue;
		}

		for (int y = range - 1; y <= range + 1; y++){
			if (y < 0 || y >round((50-minrange)/deltaR)){
				continue;
			}

			for (int x = polar - 1; x <= polar + 1; x++){
                                int px = x;
				if (x < 0 ){
					px=300;
				}
                                if(x>300){
                                        px=0;

                                }

				neighborindex.push_back((px*(length+1)+y)+z*(length+1)*(width+1));
                        }
               }
        }
}

bool most_frequent_value(vector<int> values, vector<int> &cluster_index) {
	unordered_map<int, int> histcounts;
	for (int i = 0; i < values.size(); i++) {
		if (histcounts.find(values[i]) == histcounts.end()) {
			histcounts[values[i]] = 1;
		}
		else {
			histcounts[values[i]] += 1;
		}
	}

	int max = 0, maxi;
	vector<pair<int, int>> tr(histcounts.begin(), histcounts.end());
        sort(tr.begin(),tr.end(),compare_cluster);
        for(int i = 0 ; i< tr.size(); ++i){
             if(tr[i].second>10){
             cluster_index.push_back(tr[i].first);
             }
        }

	return true;
}

void mergeClusters(vector<int>& cluster_indices, int idx1, int idx2) {
	for (int i = 0; i < cluster_indices.size(); i++) {
		if (cluster_indices[i] == idx1) {
			cluster_indices[i] = idx2;
		}
	}
}

vector<int>  CVC(unordered_map<int, Voxel> &map_in,const vector<PointAPR>& vapr){
     int current_cluster = 0;
     cout<<"CVC"<<endl;
     vector<int> cluster_indices = vector<int>(vapr.size(), -1);

     for(int i = 0; i< vapr.size(); ++i){

           if (cluster_indices[i] != -1)
			continue;
           int azimuth_index = round((vapr[i].azimuth+fabs(minazimuth))*180/PI/deltaA);
           int polar_index = round(vapr[i].polar_angle*180/PI/deltaP);
           int range_index = round((vapr[i].range-minrange)/deltaR);
           int voxel_index = (polar_index*(length+1)+range_index)+azimuth_index*(length+1)*(width+1);

           unordered_map<int, Voxel>::iterator it_find;
           unordered_map<int, Voxel>::iterator it_find2;

           it_find = map_in.find(voxel_index);
           vector<int> neightbors;

           if (it_find != map_in.end()){

               vector<int> neighborid;
               find_neighbors(polar_index, range_index, azimuth_index, neighborid);
               for (int k =0; k<neighborid.size(); ++k){

                  it_find2 = map_in.find(neighborid[k]);

                  if (it_find2 != map_in.end()){

                     for(int j =0 ; j<it_find2->second.index.size(); ++j){
                        neightbors.push_back(it_find2->second.index[j]);
                      }
                   }
                }
            }

            neightbors.swap(neightbors);

            if(neightbors.size()>0){
                   for(int j =0 ; j<neightbors.size(); ++j){
                      int oc = cluster_indices[i] ;
                      int nc = cluster_indices[neightbors[j]];
		      if (oc != -1 && nc != -1) {
				if (oc != nc)
					mergeClusters(cluster_indices, oc, nc);
				}
		      else {
				if (nc != -1) {
					cluster_indices[i] = nc;
				}
				else {
					if (oc != -1) {
						cluster_indices[neightbors[j]] = oc;
					}
				}
			}

                   }
                }

 		if (cluster_indices[i] == -1) {
			current_cluster++;
			cluster_indices[i] = current_cluster;
                   for(int s =0 ; s<neightbors.size(); ++s){
                        cluster_indices[neightbors[s]] = current_cluster;
		   }
               }

          }
	return cluster_indices;
}

vector<float> hsv2rgb(vector<float>& hsv){
vector<float> rgb(3);
float R,G,B,H,S,V;
H = hsv[0];
S = hsv[1];
V = hsv[2];
if(S == 0){
rgb[0]=rgb[1]=rgb[2]=V;
}else{

int i = int(H*6);
float f = (H*6) - i;
float a = V * ( 1 - S );
float b = V * ( 1 - S * f );
float c = V * ( 1 - S * (1 - f ) );
i = i%6;
switch(i){
   case 0: {rgb[0] = V; rgb[1]= c; rgb[2] = a; break;}
   case 1: {rgb[0] = b; rgb[1] = V; rgb[2] = a;break;}
   case 2: {rgb[0] = a; rgb[1] = V; rgb[2] = c;break;}
   case 3: {rgb[0] = a; rgb[1] = b; rgb[2] = V;break;}
   case 4: {rgb[0] = c; rgb[1] = a; rgb[2] = V;break;}
   case 5: {rgb[0] = V; rgb[1] = a; rgb[2] = b;break;}
  }
}

return rgb;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::detail::fusion::example::at_key_iterator.cpp                  //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <vector>
#include <boost/mpl/int.hpp>
#include <boost/utility/result_of.hpp>
#include <boost/assert.hpp>
#include <boost/range.hpp>
#include <boost/fusion/sequence/intrinsic/at_key.hpp>
#include <boost/fusion/include/at_key.hpp>
#include <boost/fusion/container/map.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/map_fwd.hpp>
#include <boost/statistics/detail/fusion/at_key/functor.hpp>
#include <boost/statistics/detail/fusion/at_key/iterator.hpp>

#include <boost/iterator/iterator_traits.hpp>

#include <libs/statistics/detail/fusion/example/at_key.h>

void example_at_key_iterator(std::ostream& os){

    os << "example_at_key_iterator -> ";

    using namespace boost;
    namespace stat = boost::statistics::detail;

    typedef mpl::int_<0> key_0_;
    typedef mpl::int_<1> key_1_;

    typedef fusion::pair<key_0_,int>                    p_0_;
    typedef fusion::pair<key_1_,int>                    p_1_;
    typedef fusion::map<p_0_,p_1_>                      map_;
    typedef stat::fusion::at_key::functor<key_0_>       get_0_;
    typedef stat::fusion::at_key::functor<key_1_>       get_1_;
    typedef std::vector<map_>                           vec_map_;
    typedef range_iterator<const vec_map_>::type        it_cvec_map_;
    typedef stat::fusion::at_key::meta_iterator<it_cvec_map_,key_0_> m_0_;
    typedef m_0_::type it_0_;
    typedef stat::fusion::at_key::meta_iterator<it_cvec_map_,key_1_> m_1_;
    typedef m_1_::type it_1_;

    const unsigned n = 10;

    vec_map_ vec_map; vec_map.reserve(n);
    for(unsigned i = 0; i<n; i++){
        map_ map(
            fusion::make_pair<key_0_>( i ),
            fusion::make_pair<key_1_>( i+1 )
        );
        vec_map.push_back(map);
    }

    {
        typedef it_0_   it_;
        typedef key_0_  key_;

        it_ b = stat::fusion::at_key::make_iterator<key_>(
            boost::begin(vec_map)
        );
        it_ e = stat::fusion::at_key::make_iterator<key_>(
            boost::end(vec_map)
        );
        unsigned j = 0;
        b<e;
        for(it_ i = b; i<e; i++,j++)
        {
            BOOST_ASSERT(
                *i == fusion::at_key<key_>(
                    vec_map[j]
                )
           );
        }
    }
    {
        typedef it_1_   it_;
        typedef key_1_  key_;

        it_ b = stat::fusion::at_key::make_iterator<key_>(
            boost::begin(vec_map)
        );
        it_ e = stat::fusion::at_key::make_iterator<key_>(
            boost::end(vec_map)
        );
        unsigned j = 0;
        for(it_ i = b; i<e; i++,j++)
        {
            BOOST_ASSERT(
                *i == fusion::at_key<key_>(
                    vec_map[j]
                )
           );
        }
    }

    os << "<-" << std::endl;

}


/*
 * Copyright (c) 2018 Eliane Briand
 *
 * This file is part of SmolDock.
 *
 * SmolDock is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * SmolDock is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with SmolDock.  If not, see <https://www.gnu.org/licenses/>.
 *
 */

#include "L_BFGS.h"

#include <ensmallen.hpp>

#undef BOOST_LOG

#include <boost/log/trivial.hpp>

namespace SmolDock::Optimizer {

    L_BFGS::L_BFGS(Score::ScoringFunction* scoringFunc_, unsigned int maxIteration_) :
            scoringFunction(scoringFunc_), maxIteration(maxIteration_),
            result(scoringFunc_->getParamVectorDimension(), 1, arma::fill::zeros),
            score(0.0) {
    }

    bool L_BFGS::optimize(arma::mat startingPoint) {

        ens::L_BFGS lbfgs;
        lbfgs.MaxIterations() = this->maxIteration;
        lbfgs.MinGradientNorm() = 1e-4;
        lbfgs.Factr() = 1e-4;
        lbfgs.MinStep() = 1e-5;

        lbfgs.Optimize(*this->scoringFunction, startingPoint);

        this->result = startingPoint;
        this->score = this->scoringFunction->Evaluate(startingPoint);

        /*
        BOOST_LOG_TRIVIAL(debug) << "LBFGS: x =  " << startingPoint.t();
        BOOST_LOG_TRIVIAL(debug) << "LBFGS: f(x) =  " << this->score;
        //*/

        return true;

    }

    arma::mat L_BFGS::getRawResultMatrix() {
        return this->result;
    }

    double L_BFGS::getScore() {
        return this->score;
    }

};


/*
 * Copyright [2019] [Christopher Syben]
 *
 * 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.
 *
 * Links the cone-beam projector layer from python to the actual kernel implementation. Implemented according to Tensorflow API.
 * Implementation partially adapted from CONRAD
 * PYRO-NN is developed as an Open Source project under the Apache License, Version 2.0.
*/
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/shape_inference.h"
#include "helper_headers/helper_geometry_cpu.h"
#include "helper_headers/helper_eigen.h"
#include <Eigen/QR>
#include <typeinfo>
using namespace tensorflow; // NOLINT(build/namespaces)
using shape_inference::ShapeHandle;

#define CUDA_OPERATOR_KERNEL "ConeProjection3D"
REGISTER_OP(CUDA_OPERATOR_KERNEL)
    .Input("volume: float")
    .Attr("volume_shape: shape")
    .Attr("projection_shape: shape")
    .Attr("volume_origin : tensor")
    .Attr("volume_spacing : tensor")
    .Attr("projection_matrices : tensor")
    .Attr("hardware_interp : bool = false")
    .Attr("step_size: float = 1.0")
    .Attr("projection_multiplier : float")
    .Output("output: float")
    .SetShapeFn( []( ::tensorflow::shape_inference::InferenceContext* c )
    {
      TensorShapeProto sp;
      ShapeHandle sh;
      ShapeHandle batch;
      ShapeHandle out;
      auto status = c->GetAttr( "projection_shape", &sp );
      status.Update( c->MakeShapeFromShapeProto( sp, &sh ) );
      c->Subshape(c->input(0),0,1,&batch);
      c->Concatenate(batch, sh, &out);
      c->set_output( 0, out );
      return status;
    } )
    .Doc(R"doc(
Computes the 3D cone forward projection of the input based on the given the trajectory

output: A Tensor.
  output = A_cone * x
)doc");

void Cone_Projection_Kernel_Launcher(const float *volume_ptr, float *out, const float *inv_AR_matrix, const float *src_points, const int number_of_projections,
                                    const int volume_width, const int volume_height, const int volume_depth,
                                    const float volume_spacing_x, const float volume_spacing_y, const float volume_spacing_z,
                                    const int detector_width, const int detector_height, const float step_size);

void Cone_Projection_Kernel_Tex_Interp_Launcher(const float *volume_ptr, float *out, const float *inv_AR_matrix, const float *src_points, const int number_of_projections,
                                                const int volume_width, const int volume_height, const int volume_depth,
                                                const float volume_spacing_x, const float volume_spacing_y, const float volume_spacing_z,
                                                const int detector_width, const int detector_height, const float step_size);

class ConeProjection3DOp : public OpKernel
{
    TensorShape volume_shape;
    int volume_width, volume_height, volume_depth;

    TensorShape projection_shape;
    int detector_size_x, detector_size_y, number_of_projections;

    float volume_origin_x, volume_origin_y, volume_origin_z;

    float volume_spacing_x, volume_spacing_y, volume_spacing_z;

    float step_size;
    bool hardware_interp;

    float projection_multiplier;

    Eigen::Tensor<float, 3, Eigen::RowMajor> projection_matrices;
    //TensorShape projection_matrices_shape;

    Eigen::Tensor<float, 3, Eigen::RowMajor> inv_AR_matrix;
    Eigen::Tensor<float, 2, Eigen::RowMajor> src_points;

  public:

    explicit ConeProjection3DOp(OpKernelConstruction *context) : OpKernel(context)
    {
        //get volume shape from attributes
        OP_REQUIRES_OK(context, context->GetAttr("volume_shape", &volume_shape));
        volume_depth = volume_shape.dim_size(0);
        volume_height = volume_shape.dim_size(1);
        volume_width = volume_shape.dim_size(2);
        //get detector shape from attributes
        OP_REQUIRES_OK(context, context->GetAttr("projection_shape", &projection_shape));
        number_of_projections = projection_shape.dim_size(0);
        detector_size_y = projection_shape.dim_size(1);
        detector_size_x = projection_shape.dim_size(2);
        //get volume origin from attributes
        Tensor volume_origin_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("volume_origin", &volume_origin_tensor));
        auto volume_origin_eigen = volume_origin_tensor.tensor<float, 1>();
        volume_origin_z = volume_origin_eigen(0);
        volume_origin_y = volume_origin_eigen(1);
        volume_origin_x = volume_origin_eigen(2);

        //get volume spacing from attributes
        Tensor volume_spacing_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("volume_spacing", &volume_spacing_tensor));
        auto volume_spacing_eigen = volume_spacing_tensor.tensor<float, 1>();
        volume_spacing_z = volume_spacing_eigen(0);
        volume_spacing_y = volume_spacing_eigen(1);
        volume_spacing_x = volume_spacing_eigen(2);

        //get ray vectors from attributes
        Tensor projection_matrices_tensor;
        OP_REQUIRES_OK(context, context->GetAttr("projection_matrices", &projection_matrices_tensor));
        auto projection_matrices_eigen = projection_matrices_tensor.tensor<float, 3>();
        // projection_matrices_shape = projection_matrices_tensor.shape();
        //Init src_point and inv_ar_matrix tensors
        //get stepsize
        OP_REQUIRES_OK(context, context->GetAttr("step_size", &step_size));

        //get hardware interpolation flag
        OP_REQUIRES_OK(context, context->GetAttr("hardware_interp", &hardware_interp));

        //Projectionmultiplier for backprojection as gradient
        OP_REQUIRES_OK(context, context->GetAttr("projection_multiplier", &projection_multiplier));

        src_points = Eigen::Tensor<float, 2, Eigen::RowMajor>(number_of_projections,3);
        inv_AR_matrix = Eigen::Tensor<float, 3, Eigen::RowMajor>(number_of_projections,3,3);

        /*********************************************************************************************************************************************************************
         *
         *  P = [M | -MC]
         *  M = KR
         * 1. Extract Source Position (C) from P using SVD to calc right null space
         * 2. Calculate M^-1 and multiply with a 3x3 Matrix containing 1/voxel_spacing on the diagonal matrix
         * 3. Put src_points and inv_ar_matrix into the CUDA Kernel, like Cone-Projector from Conrad
         *
         * WARNING: The following code is not created under memory and runtime performance point of view.
         *          A better conversion from Tensorflow Tensor to Eigen::Tensor and Eigen::Matrizes are probably neccessary !!!!
         * ********************************************************************************************************************************************************************/

        Eigen::Matrix3f scaling_matrix(3,3);
        scaling_matrix.setZero();
        scaling_matrix(0,0) = 1.0/volume_spacing_x;
        scaling_matrix(1,1) = 1.0/volume_spacing_y;
        scaling_matrix(2,2) = 1.0/volume_spacing_z;
        src_points.setZero();
        inv_AR_matrix.setZero();

        //for each projection
        for (int n = 0; n < number_of_projections; n++)
        {
            Eigen::Matrix<float,3,4,Eigen::RowMajor> proj_mat(3,4);
            proj_mat << projection_matrices_eigen(n,0,0), projection_matrices_eigen(n,0,1), projection_matrices_eigen(n,0,2), projection_matrices_eigen(n,0,3),
                        projection_matrices_eigen(n,1,0), projection_matrices_eigen(n,1,1), projection_matrices_eigen(n,1,2), projection_matrices_eigen(n,1,3),
                        projection_matrices_eigen(n,2,0), projection_matrices_eigen(n,2,1) ,projection_matrices_eigen(n,2,2), projection_matrices_eigen(n,2,3);

            auto c = (Geometry::getCameraCenter(proj_mat) * -1).eval();

            src_points(n,0) = -((volume_origin_x * scaling_matrix(0,0)) + c(0) * scaling_matrix(0,0));
            src_points(n,1) = -((volume_origin_y * scaling_matrix(1,1)) + c(1) * scaling_matrix(1,1));
            src_points(n,2) = -((volume_origin_z * scaling_matrix(2,2)) + c(2) * scaling_matrix(2,2));

            Eigen::Matrix<float,3,3, Eigen::RowMajor> inverted_scaled_result = (scaling_matrix * proj_mat.block<3,3>(0,0).inverse()).eval();

            //TODO: dont copy element-wise use Eigen::Map to map eigen::matrix to eigen::tensor
            for(int j = 0; j < inverted_scaled_result.cols();++j){
                for(int i = 0; i < inverted_scaled_result.rows(); ++i){
                    inv_AR_matrix(n,j,i) = inverted_scaled_result(j,i);
                }
            }
        }
    }

    /*
    https://github.com/tensorflow/tensorflow/issues/5902
    tensorflow::GPUBFCAllocator* allocator = new tensorflow::GPUBFCAllocator(0, sizeof(float) * height * width * 3);
    tensorflow::Tensor input_tensor = tensorflow::Tensor(allocator, tensorflow::DataType::DT_FLOAT, tensorflow::TensorShape( { 1, height, width, 3 }));
    <copy output data from program A into the GPU memory allocated by input_tensor using a GPU->GPU copy>

    https://stackoverflow.com/questions/39797095/tensorflow-custom-allocator-and-accessing-data-from-tensor

    */

    void Compute(OpKernelContext *context) override
    {
        // Grab the input tensor
        const Tensor &input_tensor = context->input(0);
        auto input = input_tensor.flat<float>();
        // Create an output tensor
        TensorShape out_shape = TensorShape(
          {input_tensor.shape().dim_size(0), projection_shape.dim_size(0), projection_shape.dim_size(1), projection_shape.dim_size(2)});
        Tensor *output_tensor = nullptr;

        // Check Batch size. Batch > 1 is not supported currently.
        OP_REQUIRES(context, input_tensor.shape().dim_size(0) == 1,
                errors::InvalidArgument("Batch dimension is mandatory ! Batch size > 1 is not supported in the current PYRO-NN-layers."));

        OP_REQUIRES_OK(context, context->allocate_output(0, out_shape,
                                                         &output_tensor));

        auto output = output_tensor->template flat<float>();

        if(hardware_interp){
            Cone_Projection_Kernel_Tex_Interp_Launcher(input.data(), output.data(), inv_AR_matrix.data(), src_points.data(), number_of_projections,
                                        volume_width, volume_height, volume_depth, volume_spacing_x, volume_spacing_y, volume_spacing_z,
                                        detector_size_x, detector_size_y, step_size);
        }
        else{
            //TODO:
            // allocate inv_ar_matrix, src_points with tensorflow context as temp memory.
            // Call the cuda kernel launcher
            Cone_Projection_Kernel_Launcher(input.data(), output.data(), inv_AR_matrix.data(), src_points.data(), number_of_projections,
                                        volume_width, volume_height, volume_depth, volume_spacing_x, volume_spacing_y, volume_spacing_z,
                                        detector_size_x, detector_size_y,step_size);
        }
    }
};

REGISTER_KERNEL_BUILDER(Name(CUDA_OPERATOR_KERNEL).Device(DEVICE_GPU), ConeProjection3DOp);


// ---------------------------------------------------------------------
//
// Copyright (c) 2020 - 2020 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.
//
// ---------------------------------------------------------------------

#include <ibamr/IBExplicitHierarchyIntegrator.h>
#include <ibamr/IBFEMethod.h>
#include <ibamr/IBFESurfaceMethod.h>
#include <ibamr/INSCollocatedHierarchyIntegrator.h>
#include <ibamr/INSStaggeredHierarchyIntegrator.h>
#include <ibamr/app_namespaces.h>

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

#include <libmesh/boundary_info.h>
#include <libmesh/boundary_mesh.h>
#include <libmesh/centroid_partitioner.h>
#include <libmesh/equation_systems.h>
#include <libmesh/gmv_io.h>
#include <libmesh/mesh.h>
#include <libmesh/mesh_generation.h>
#include <libmesh/mesh_triangle_interface.h>

#include <petscsys.h>
#include <petscvec.h>

#include <boost/multi_array.hpp>

#include <BergerRigoutsos.h>
#include <CartesianGridGeometry.h>
#include <HierarchyDataOpsManager.h>
#include <LoadBalancer.h>
#include <SAMRAI_config.h>
#include <StandardTagAndInitialize.h>
#include <mpi.h>

#include <string>
#include <vector>

// This file is the main driver for parallel IB element partitioning for surface meshes.

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();

    // prevent a warning about timer initializations
    TimerManager::createManager(nullptr);
    {
        // 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();

        // Create a simple FE mesh.
        Mesh solid_mesh(init.comm(), NDIM);
        const double dx = input_db->getDouble("DX");
        string elem_type = input_db->getString("ELEM_TYPE");
        const double R = 0.3;
        // we want R/2^n_refinements = dx so that we have roughly equal spacing for both elements and cells
        const int n_refinements = int(std::log2(R / dx));
        const std::string shape = input_db->getStringWithDefault("SHAPE", "SPHERE");
        // no support for tetrahedral spheres in 3D yet
        if (shape == "CUBE")
        {
            MeshTools::Generation::build_cube(solid_mesh,
                                              n_refinements,
                                              n_refinements,
                                              n_refinements,
                                              -R,
                                              R,
                                              -R,
                                              R,
                                              -R,
                                              R,
                                              Utility::string_to_enum<ElemType>(elem_type),
                                              false);
        }
        else if (shape == "SQUARE")
        {
            MeshTools::Generation::build_square(solid_mesh,
                                                n_refinements,
                                                n_refinements,
                                                -R,
                                                R,
                                                -R,
                                                R,
                                                Utility::string_to_enum<ElemType>(elem_type),
                                                false);
        }
        else
        {
            TBOX_ASSERT(shape == "SPHERE");
            MeshTools::Generation::build_sphere(
                solid_mesh, R, n_refinements, Utility::string_to_enum<ElemType>(elem_type), 10);
        }
        solid_mesh.prepare_for_use();
        // metis does a good job partitioning, but the partitioning relies on
        // random numbers: the seed changed in libMesh commit
        // 98cede90ca8837688ee13aac5e299a3765f083da (between 1.3.1 and
        // 1.4.0). Hence, to achieve consistent partitioning, use a simpler partitioning scheme:
        IBTK::StableCentroidPartitioner partitioner;
        partitioner.partition(solid_mesh);

        for (auto node = solid_mesh.nodes_begin(); node != solid_mesh.nodes_end(); ++node)
        {
            (**node)(0) += 0.6;
            (**node)(1) += 0.5;
#if NDIM == 3
            (**node)(2) += 0.5;
#endif
        }

        BoundaryMesh boundary_mesh(solid_mesh.comm(), solid_mesh.mesh_dimension() - 1);
        solid_mesh.boundary_info->sync(boundary_mesh);
        boundary_mesh.prepare_for_use();

        bool use_boundary_mesh = input_db->getBoolWithDefault("USE_BOUNDARY_MESH", false);
        Mesh& mesh = use_boundary_mesh ? boundary_mesh : solid_mesh;

        GMVIO gmv_out(mesh);
        gmv_out.write("mesh.gmv");

        plog << "Number of elements: " << mesh.n_active_elem() << std::endl;

        // 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"), false);
        Pointer<PatchHierarchy<NDIM> > patch_hierarchy =
            new PatchHierarchy<NDIM>("PatchHierarchy", grid_geometry, false);
        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"),
                false);
        }
        else if (solver_type == "COLLOCATED")
        {
            navier_stokes_integrator = new INSCollocatedHierarchyIntegrator(
                "INSCollocatedHierarchyIntegrator",
                app_initializer->getComponentDatabase("INSCollocatedHierarchyIntegrator"),
                false);
        }
        else
        {
            TBOX_ERROR("Unsupported solver type: " << solver_type << "\n"
                                                   << "Valid options are: COLLOCATED, STAGGERED");
        }

        Pointer<IBStrategy> ib_ops;
        if (use_boundary_mesh)
            ib_ops = new IBFESurfaceMethod(
                "IBFEMethod",
                app_initializer->getComponentDatabase("IBFEMethod"),
                &mesh,
                app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                /*register_for_restart*/ false);
        else
            ib_ops =
                new IBFEMethod("IBFEMethod",
                               app_initializer->getComponentDatabase("IBFEMethod"),
                               &mesh,
                               app_initializer->getComponentDatabase("GriddingAlgorithm")->getInteger("max_levels"),
                               false);
        Pointer<IBHierarchyIntegrator> time_integrator =
            new IBExplicitHierarchyIntegrator("IBHierarchyIntegrator",
                                              app_initializer->getComponentDatabase("IBHierarchyIntegrator"),
                                              ib_ops,
                                              navier_stokes_integrator,
                                              false);

        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,
                                        false);

        // Configure the IBFE solver.
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            ibfe_ops->initializeFEData();
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            ibfe_ops->initializeFEEquationSystems();
            ibfe_ops->initializeFEData();
        }
        time_integrator->initializePatchHierarchy(patch_hierarchy, gridding_algorithm);
        time_integrator->regridHierarchy();

        FEDataManager* fe_data_manager = nullptr;
        if (use_boundary_mesh)
        {
            Pointer<IBFESurfaceMethod> ibfe_ops = ib_ops;
            fe_data_manager = ibfe_ops->getFEDataManager();
        }
        else
        {
            Pointer<IBFEMethod> ibfe_ops = ib_ops;
            fe_data_manager = ibfe_ops->getFEDataManager();
        }
        const std::string& displacement_name = IBFEMethod::COORDS_SYSTEM_NAME;
        std::unique_ptr<libMesh::PetscVector<double> > ib_vector =
            fe_data_manager->buildIBGhostedVector(displacement_name);
        Vec petsc_vec = ib_vector->vec();

        const int range_tag = 0;
        const int length_tag = 1;
        const int index_tag = 2;

        const int rank = IBTK_MPI::getRank();
        const int n_nodes = IBTK_MPI::getNodes();

        if (rank == 0)
        {
            for (int r = 0; r < n_nodes; ++r)
            {
                PetscInt bounds[2];
                std::vector<PetscInt> indices;

                if (r == 0)
                {
                    int ierr = VecGetOwnershipRange(petsc_vec, &bounds[0], &bounds[1]);
                    TBOX_ASSERT(ierr == 0);

                    ISLocalToGlobalMapping mapping;
                    ierr = VecGetLocalToGlobalMapping(petsc_vec, &mapping);
                    TBOX_ASSERT(ierr == 0);
                    TBOX_ASSERT(mapping);
                    PetscInt n;
                    ierr = ISLocalToGlobalMappingGetSize(mapping, &n);
                    TBOX_ASSERT(ierr == 0);
                    const PetscInt* petsc_indices;
                    ierr = ISLocalToGlobalMappingGetIndices(mapping, &petsc_indices);
                    TBOX_ASSERT(ierr == 0);

                    indices.insert(indices.begin(), petsc_indices, petsc_indices + n);
                }
                else
                {
                    int ierr =
                        MPI_Recv(bounds, 2, MPIU_INT, r, range_tag, IBTK_MPI::getCommunicator(), MPI_STATUS_IGNORE);
                    TBOX_ASSERT(ierr == 0);

                    PetscInt n = -1;
                    ierr = MPI_Recv(&n, 1, MPIU_INT, r, length_tag, IBTK_MPI::getCommunicator(), MPI_STATUS_IGNORE);
                    TBOX_ASSERT(ierr == 0);
                    indices.resize(n);
                    ierr = MPI_Recv(
                        indices.data(), n, MPIU_INT, r, index_tag, IBTK_MPI::getCommunicator(), MPI_STATUS_IGNORE);
                    TBOX_ASSERT(ierr == 0);
                }

                plog << "locally owned range on " << r << ": [" << bounds[0] << ", " << bounds[1] << ")\n";
                plog << "total length on " << r << ": " << indices.size() << '\n';
                plog << "global index ranges on " << r << ":\n";
                // Print out intervals instead of every single number
                for (PetscInt i = 0; i < long(indices.size()); ++i)
                {
                    if (i == 0)
                    {
                        plog << '[' << indices[i] << ", ";
                    }
                    else
                    {
                        // if we started a new range of consecutive integers
                        // then start a new interval on a new line
                        if (indices[i] != indices[i - 1] + 1)
                        {
                            plog << indices[i - 1] + 1 << ')' << '\n';
                            plog << '[' << indices[i] << ", ";
                        }
                    }
                }
                plog << indices.back() + 1 << ")\n";
            }
        }
        else
        {
            // send local range
            PetscInt bounds[2];
            int ierr = VecGetOwnershipRange(petsc_vec, &bounds[0], &bounds[1]);
            TBOX_ASSERT(ierr == 0);
            ierr = MPI_Send(bounds, 2, MPIU_INT, 0, range_tag, IBTK_MPI::getCommunicator());
            TBOX_ASSERT(ierr == 0);

            // send locally stored global indices
            ISLocalToGlobalMapping mapping;
            ierr = VecGetLocalToGlobalMapping(petsc_vec, &mapping);
            TBOX_ASSERT(ierr == 0);
            TBOX_ASSERT(mapping);
            PetscInt n;
            ierr = ISLocalToGlobalMappingGetSize(mapping, &n);
            TBOX_ASSERT(ierr == 0);
            const PetscInt* indices;
            ierr = ISLocalToGlobalMappingGetIndices(mapping, &indices);
            TBOX_ASSERT(ierr == 0);

            ierr = MPI_Send(&n, 1, MPIU_INT, 0, length_tag, IBTK_MPI::getCommunicator());
            TBOX_ASSERT(ierr == 0);

            ierr = MPI_Send(indices, n, MPIU_INT, 0, index_tag, IBTK_MPI::getCommunicator());
            TBOX_ASSERT(ierr == 0);
        }
    } // cleanup dynamically allocated objects prior to shutdown
} // main