//
// Created by Masahiro Tanaka on 2019-03-15.
//
#include <cassert>

#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>

#include <graph/ir.h>
#include <Config.h>
#include "ProfiledWeightDecomposer.h"
#include "Decomposition.h"

namespace rannc {
    long block_size(const std::vector<size_t>& split, size_t idx) {
        if (idx == 0) {
            return split.at(idx);
        }
        return split.at(idx) - split.at(idx - 1);
    }

    std::vector<size_t> all_block_sizes(const std::vector<size_t>& split) {
        std::vector<size_t> sizes;
        for (size_t i=0; i<split.size(); i++) {
            sizes.push_back(block_size(split, i));
        }
        return sizes;
    }

    long left_block_size(const std::vector<size_t>& split, size_t idx) {
        long sum = 0;
        for (size_t i=0; i<idx; i++) {
            sum += block_size(split, i);
        }
        return sum;
    }

    long right_block_size(const std::vector<size_t>& split, size_t idx) {
        long sum = 0;
        for (size_t i=idx+1; i<split.size(); i++) {
            sum += block_size(split, i);
        }
        return sum;
    }

    long ave_block_vals(const std::vector<long>& vals) {
        return average(vals);
    }

    long block_val(const std::vector<long>& vals, size_t idx) {
        return vals.at(idx);
    }

    long left_blocks_val(const std::vector<long>& vals, size_t idx) {
        long sum = 0;
        for (size_t i=0; i<idx; i++) {
            sum += block_val(vals, i);
        }
        return sum;
    }

    long right_blocks_val(const std::vector<long>& vals, size_t idx) {
        long sum = 0;
        for (size_t i=idx+1; i<vals.size(); i++) {
            sum += block_val(vals, i);
        }
        return sum;
    }

    size_t max_val_diff_block_idx(const std::vector<long>& vals) {
        long val_ave = ave_block_vals(vals);

        long max_val_diff = 0; // needs pos/neg sign
        size_t max_val_diff_idx = 0;
        for (size_t sg_idx=0; sg_idx<vals.size(); sg_idx++) {
            long diff = val_ave - vals.at(sg_idx);
            if (std::abs(diff) > std::abs(max_val_diff)) {
                max_val_diff = diff;
                max_val_diff_idx = sg_idx;
            }
        }
        return max_val_diff_idx;
    }

    long block_diff_to_ave(const std::vector<long>& vals, size_t idx) {
        long val_ave = ave_block_vals(vals);
        return val_ave - vals.at(idx);
    }

    long sum_abs_diff_to_ave(const std::vector<long>& vals) {
        long sum = 0;
        for (size_t i=0; i<vals.size(); i++) {
            sum += std::abs(block_diff_to_ave(vals, i));
        }
        return sum;
    }

    double scale_to_ave(const std::vector<long>& vals, size_t idx, double fix_rate) {
        long val = vals.at(idx);
        long diff = block_diff_to_ave(vals, idx); // if positive, need to extend
        long scaled_val = val + diff * fix_rate;
        return scaled_val / (double) val;
    }

    long blocks_to_move_by_scale(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx, double scale) {
        long n_blocks = block_size(split, idx);
        long n_block_scaled = n_blocks * scale;
        return n_block_scaled - n_blocks;
    }

    long blocks_to_move(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx, double fix_rate) {
        double scale = scale_to_ave(vals, idx, fix_rate);
        return blocks_to_move_by_scale(split, vals, idx, scale);
    }

    double _scale_by_block_num(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                               long n_move_block, const std::function<long(const std::vector<size_t>&, size_t)> f) {
        long n_block = f(split, idx);
        if (n_block == 0) {
            return 1.0;
        }
        // if n_move_block is positive, the block at i increases and other blocks decrease
        long n_new_block = n_block - n_move_block;
        return n_new_block / (double) n_block;
    }

    double block_scale(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx, long n_move_block) {
        return _scale_by_block_num(split, vals, idx, n_move_block, block_size);
    }

    double left_scale(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx, long n_move_block) {
        return _scale_by_block_num(split, vals, idx, n_move_block, left_block_size);
    }

    double right_scale(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx, long n_move_block) {
        return _scale_by_block_num(split, vals, idx, n_move_block, right_block_size);
    }

    std::vector<size_t> scale_left_block(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                         long n_move_block) {
        // unable to move block to the left
        if (idx == 0) {
            return split;
        }

        std::vector<size_t> new_split = split;
        size_t offset = 0;
        double scale = left_scale(split, vals, idx, n_move_block);
        for (size_t i=0; i<idx; i++) {
            size_t size = block_size(split, i) * scale;
            offset += size;
            new_split[i] = offset;
        }
        if (idx != vals.size()-1) {
            long size = block_size(split, idx) + n_move_block;
            offset += size;
            new_split[idx] = offset;
        }
        return new_split;
    }

    std::vector<size_t> scale_right_block(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                          long n_move_block) {
        // unable to move block to the right
        if (idx == vals.size()-1) {
            return split;
        }

        size_t offset = 0;
        if (idx > 0) {
            offset = split[idx - 1];
        }
        offset += block_size(split, idx) + n_move_block;

        std::vector<size_t> new_split = split;
        new_split[idx] = offset;

        double scale = right_scale(split, vals, idx, n_move_block);
        for (size_t i=idx+1; i<split.size()-1; i++) {
            size_t size = block_size(split, i) * scale;
            offset += size;
            new_split[i] = offset;
        }
        return new_split;
    }

    std::vector<long> _est_vals_scale_block(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                            long n_move_block,
                                            const std::function<std::vector<size_t>(const std::vector<size_t>&, const std::vector<long>&, size_t, long)>& f) {
        const auto& new_split = f(split, vals, idx, n_move_block);
        std::vector<long> est_values;
        for (size_t i=0; i<split.size(); i++) {
            double block_scale = block_size(new_split, i) / (double) block_size(split, i);
            est_values.push_back(vals.at(i) * block_scale);
        }
        return est_values;
    }

    std::vector<long> est_vals_scale_left_block(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                                long n_move_block) {
        return _est_vals_scale_block(split, vals, idx, n_move_block, scale_left_block);
    }

    std::vector<long> est_vals_scale_right_block(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                                 long n_move_block) {
        return _est_vals_scale_block(split, vals, idx, n_move_block, scale_right_block);
    }

    std::vector<size_t> balance_blocks(const std::vector<size_t>& split, const std::vector<long>& vals, size_t idx,
                                       long n_move_node) {

        std::vector<long> est_vals_left = est_vals_scale_left_block(split, vals, idx, n_move_node);
        const auto split_left = scale_left_block(split, vals, idx, n_move_node);
        long est_max_val_left = max(est_vals_left);

        std::vector<long> est_vals_right = est_vals_scale_right_block(split, vals, idx, n_move_node);
        const auto split_right = scale_right_block(split, vals, idx, n_move_node);
        long est_max_val_right = max(est_vals_right);

        bool balance_left = est_max_val_right - est_max_val_left > 0;

        if (balance_left) {
            return split_left;
        }
        return split_right;
    }

    long evalSubgraph(const GraphProfile& prof) {
        return prof.fwd_time * 2;
    }

    long estimate(const std::vector<long>& vals, int pipeline_num) {
//        spdlog::info("@estimate vals={} pl={} pipeline_num+vals.size()-1={}",
//                     join_as_str(vals), pipeline_num, pipeline_num+vals.size()-1);
        long est = 0;
        for (size_t t=0; t<pipeline_num+vals.size()-1; t++) {
            long est_step = 0;
            size_t init_sg_idx = std::max((long)0, ((long) t - pipeline_num + 1));
            size_t fin_sg_idx = std::min(vals.size(), t+1);

//            spdlog::info("t={} init_sg_idx={} fin_sg_idx={}", t, init_sg_idx, fin_sg_idx);
            for (size_t sg_idx=init_sg_idx; sg_idx<fin_sg_idx; sg_idx++) {
                est_step = std::max(est_step, vals.at(sg_idx));
            }
            est += est_step;
        }
        return est;
    }

    std::pair<long, std::vector<size_t>> ProfiledWeightDecomposer::doBalance(const std::vector<size_t>& initial_split, int replica_num,
                                             int pipeline_num, BGraph& g) {

        const int MAX_BALANCE_ITER = 10;
//        const double mem_exceed_scale = 0.9;

        std::vector<size_t> split = initial_split;
        std::vector<size_t> prev_split;

        long est_val = LONG_MAX;
        for (int iter_idx=0; iter_idx < MAX_BALANCE_ITER; iter_idx++) {
            setRanksOnGraph(g, split);
            fixNonBatchRanks(g);

            Partition partition = createPartition(g);
            std::unordered_map<std::string, std::shared_ptr<IRGraph>> scaled_graphs;
            std::stringstream ss;
            for (const auto &it: partition.subgraphs) {
                scaled_graphs[it.first] = scaleGraph(it.second, replica_num*pipeline_num, batch_size_);
            }

            int opt_param_factor = config::Config::get().getVal<int>(config::OPT_PARAM_FACTOR);
            std::unordered_map<std::string, long> mem_limits;
            for (const auto& it: scaled_graphs) {
                const auto& sg = it.second;
                long opt_mem = sg->getParamSizeInByte() * opt_param_factor;
                mem_limits[it.first] = dev_mem_ - opt_mem;

//                spdlog::info("mem_limits[{}]={} param_size={} opt_param_factor={}", it.first, mem_limits[it.first],
//                             sg->getParamSizeInByte(), opt_param_factor);
            }

            ProfilingResult profiles;
            try {
                bool chk_pt = config::Config::get().getVal<bool>(config::CHECKPOINTING);
                int profiling_iter = config::Config::get().getVal<int>(config::PROFILING_ITER);
                profiles = sg_prof_->profile(scaled_graphs, profiling_iter);
            } catch (...) {
                spdlog::info("Encountered an error while profiling.");
                sg_prof_->clear();
                split = prev_split;
                break;
            }

            std::vector<long> subgraph_vals;
            for (const auto &id: partition.order) {
                const auto &prof = profiles.node_profiles.at(id);
                logger->trace("{} fwd_time={} bwd_time={} mem={}", id, prof.fwd_time, prof.bwd_time,
                              prof.max_allocated_mem);
                subgraph_vals.push_back(evalSubgraph(prof));
            }
            est_val = estimate(subgraph_vals, pipeline_num);

            long time_ar = 0;
            if (replica_num > 1) {
                long param_size_sum = 0;
                for (const auto &it: scaled_graphs) {
                    const auto &sg = it.second;
                    param_size_sum += sg->getParamSizeInByte();
                    spdlog::info("{} param size={}", it.first, sg->getParamSizeInByte());
                }
                time_ar = param_size_sum / (200 * 1000);
            }

            spdlog::info("vals={} pl={} est(comp)={} time_ar={}", join_as_str(subgraph_vals), pipeline_num,
                    est_val, time_ar);
            est_val += time_ar;

            // check memory error
            bool mem_exceed = false;
            for (size_t sg_idx=0; sg_idx<partition.order.size(); sg_idx++) {
                const auto& id = partition.order.at(sg_idx);
                const auto& prof = profiles.node_profiles.at(id);

//                spdlog::info("mem_limits[{}]={} alloc_mem={}", id, mem_limits[id], prof.max_allocated_mem);
                if (prof.max_allocated_mem > mem_limits[id]) {
                    mem_exceed = true;
                    break;
                }
            }
            if (mem_exceed) {
                sg_prof_->clear();
                split = prev_split;
                break;
            }

            prev_split = split;
//            if (doBalanceSplitAdjacent(split, subgraph_vals)) {
            if (doBalanceSplitVariance(split, subgraph_vals)) {
                break;
            }
        }

        setRanksOnGraph(g, split);
        fixNonBatchRanks(g);

        return {est_val, split};
    }

    size_t calcCutSize(const Partition& p) {
        size_t size_sum = 0;
        const auto& g = p.graph;
        for (const auto& con: p.connections) {
            const auto& val = g->getValue(con.value);
            size_sum += val.getSizeInByte();
        }
        return size_sum;
    }

    struct CutSearchState {
        bool operator==(const CutSearchState &rhs) const {
            return cut_size == rhs.cut_size &&
                   split == rhs.split;
        }

        bool operator!=(const CutSearchState &rhs) const {
            return !(rhs == *this);
        }

        size_t cut_size;
        std::vector<size_t> split;
    };

    struct CutSearchStateHash {
        std::size_t operator()(const CutSearchState &state) const {
            std::stringstream ss;
            ss << join_as_str(state.split) << "=" << state.cut_size;
            return std::hash<std::string>()(ss.str());
        };
    };

    CutSearchState chooseBestState(const std::unordered_set<CutSearchState, CutSearchStateHash>& open) {
        CutSearchState best_state{SIZE_MAX, {}};
        for (const auto& it: open) {
            if (it.cut_size < best_state.cut_size) {
                best_state = it;
            }
        }
        return best_state;
    }

    CutSearchState doSearchCuts(const std::vector<size_t>& split, BGraph& g) {
        setRanksOnGraph(g, split);
        fixNonBatchRanks(g);
        size_t cut_size = calcCutSize(createPartition(g));

        int MAX_SEARCH_ITER = 10;

        CutSearchState init_state{cut_size, split};
        spdlog::info("initial cut={} split={}", cut_size,join_as_str(split));

        std::unordered_set<CutSearchState, CutSearchStateHash> open;
        std::unordered_set<CutSearchState, CutSearchStateHash> closed;
        std::unordered_set<std::vector<size_t>, SizeVectorHash> visited;
        open.insert(init_state);

        for (int iter_idx = 0; iter_idx < MAX_SEARCH_ITER; iter_idx++) {
            spdlog::info("iter={} open size={}", iter_idx, open.size());

            if (open.empty()) {
                break;
            }
            CutSearchState state = chooseBestState(open);

            for (size_t idx = 0; idx < state.split.size() - 1; idx++) {
                std::vector<int> steps = {-1, +1};
                for (int s: steps) {
                    auto test_split = state.split;
                    test_split[idx] += s;

                    if (contains(visited, test_split)) {
                        continue;
                    }

                    setRanksOnGraph(g, test_split);
                    fixNonBatchRanks(g);
                    size_t test_cut_size = calcCutSize(createPartition(g));

                    CutSearchState test_state{test_cut_size, test_split};
                    spdlog::info("iter={} current={} test_cut={} test_split={}", iter_idx,
                            state.cut_size,
                            test_cut_size,
                            join_as_str(test_split));

                    open.insert(test_state);
                    visited.insert(test_split);
                }
            }
            open.erase(state);
        }

        std::unordered_set<CutSearchState, CutSearchStateHash> all;
        for (const auto& it: open) {
            all.insert(it);
        }
        for (const auto& it: closed) {
            all.insert(it);
        }

        return chooseBestState(all);
    }

    ParallelConf ProfiledWeightDecomposer::balance(BGraph& g) {

        bool disable_balancing = config::Config::get().getVal<bool>(config::DISABLE_BALANCING);
        int cfg_part_num = config::Config::get().getVal<int>(config::PARTITION_NUM);
        int cfg_replica_num = config::Config::get().getVal<int>(config::REPLICA_NUM);
        int cfg_pipeline_num = config::Config::get().getVal<int>(config::PIPELINE_NUM);
        bool cfg_auto_parallel = config::Config::get().getVal<bool>(config::AUTO_PARALLEL);

        ParallelConf pconf;
        if (disable_balancing && !cfg_auto_parallel) {
            pconf.part = cfg_part_num;
            pconf.repl = cfg_replica_num;
            pconf.pipe = cfg_pipeline_num;

            const auto split = splitByValueSizes(g, pconf.part);
            setRanksOnGraph(g, split);
            fixNonBatchRanks(g);
            return pconf;
        }

        long best_est = LONG_MAX;
        std::vector<size_t> best_split;

        if (cfg_auto_parallel) {
            for (size_t part_num = 1; part_num <= mpi::getSize(); part_num *= 2) {
                const auto initial_split = splitByValueSizes(g, part_num);
                int replica_num = mpi::getSize() / part_num;

//        if (initial_split.size() == 1 || disable_balancing) {
//            setRanksOnGraph(g, initial_split);
//            fixNonBatchRanks(g);
//            return pipeline_num;
//        }

//        logger->trace("balance starting. initial_split={} replica_num={} pipeline_num={}",
//                join_as_str(initial_split), replica_num, pipeline_num);

                int max_pipeline_num = std::min((int) 32, (int) batch_size_ / replica_num);
                for (size_t pl = 1; pl <= max_pipeline_num; pl *= 2) {
                    std::pair<long, std::vector<size_t>> balance_est = doBalance(initial_split, replica_num, pl, g);
                    long est = balance_est.first;
                    spdlog::info("est={} P={} R={} L={}", est, part_num, replica_num, pl);
                    if (best_est > est) {
                        best_est = est;
                        pconf.part = part_num;
                        pconf.repl = replica_num;
                        pconf.pipe = pl;
                        best_split = balance_est.second;
                        spdlog::info("BEST est={} P={} R={} L={} split={}", est, part_num, replica_num, pl,
                                     join_as_str(initial_split));
                    }
                }
            }
        } else {
            pconf.part = cfg_part_num;
            pconf.repl = cfg_replica_num;
            pconf.pipe = cfg_pipeline_num;

            const auto split = splitByValueSizes(g, pconf.part);
            std::pair<long, std::vector<size_t>> balance_est = doBalance(split, cfg_replica_num,
                    cfg_pipeline_num, g);

            CutSearchState st = doSearchCuts(balance_est.second, g);
            best_split = st.split;
        }

        setRanksOnGraph(g, best_split);
        fixNonBatchRanks(g);

        return pconf;
    }

    Deployment ProfiledWeightDecomposer::decompose(const std::shared_ptr<IRGraph>& irGraph) {

        int conf_n_partition = config::Config::get().getVal<int>(config::PARTITION_NUM);

        logger->trace("ProfiledWeightDecomposer::decompose starting");

        if (dev_mem_ > 0) {
            dev_mem_ -= 1024L * 1024L * 1024L;
        } else {
            logger->warn("No CUDA device found on workers. Assuming (almost) unlimited host memory when assigning subgraphs.");
            dev_mem_ = 2 * 1024L * 1024L * 1024L * 1024L; // 2TB
        }

        int n_partition = conf_n_partition;
        if (n_partition <= 0) {
            size_t graph_size = irGraph->getSizeInByte();
            n_partition = (int) (graph_size / dev_mem_) + 1;
            logger->info("The partition num was automatically set to {}. To manually set this value, "
                         "add 'partition_num' to ~/.pyrannc/rannc_conf.toml", n_partition);
        }
        assert(n_partition != 0);

        int replica_num = config::Config::get().getVal<int>(config::REPLICA_NUM);

        BGraph g = toBGL(irGraph);

        ParallelConf pconf = balance(g);
        replica_num = pconf.repl;
//
//        setRanksOnGraph(g, split);
//        fixNonBatchRanks(g);

        Partition partition = createPartition(g);

        // Use IRGraph in the argument because createPartition loses the order of inputs/outputs
        partition.graph = irGraph;
        logger->trace("ProfiledWeightDecomposer::decompose created partition. id={}", partition.id);

        logger->trace("ProfiledWeightDecomposer::decompose creating replications. id={} repl_num={}", partition.id, replica_num);

        std::unordered_map<std::string, int> repl_nums;
        for (const auto& it: partition.subgraphs) {
            repl_nums[it.first] = replica_num;
        }
        PartitionDP partitionDp = replicate(partition, repl_nums, batch_size_);
        logger->trace("ProfiledWeightDecomposer::decompose created PartitionDP. id={}", partitionDp.id);

        logger->info("Assigning {} subgraphs to {} device(s) ... (mem: {} per device)",
                     partitionDp.subgraphs.size(), mpi::getSize(), dev_mem_);

        std::unordered_map<std::string, std::unordered_set<int>> alloc = searchAllocation(partitionDp, mpi::getSize(), dev_mem_*2);
        if (alloc.empty()) {
            throw std::runtime_error("Failed to allocate gpus to subgraphs.");
        }

        for (const auto& it: alloc) {
            logger->info(" Assigned subgraph {} to rank{}", it.first, join_as_str(it.second));
        }
        Deployment deployment = createDeployment(partitionDp, alloc);
        deployment.pipeline_num = pconf.pipe;
        logger->trace("ProfiledWeightDecomposer::decompose finished");

        return deployment;
    }
}


// practice 5-6

#include <iostream>
#include <boost/type_traits/is_reference.hpp>
#include <boost/type_traits/is_same.hpp>
#include <boost/type_traits/is_pointer.hpp>
#include <boost/type_traits/is_float.hpp>
#include <boost/type_traits/alignment_of.hpp>
#include <boost/type_traits/remove_reference.hpp>
#include <boost/type_traits/remove_pointer.hpp>
#include <boost/type_traits/add_pointer.hpp>
#include <boost/type_traits/is_function.hpp>
#include <boost/type_traits/is_const.hpp>
#include <boost/static_assert.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/apply.hpp>

#include <boost/mpl/vector_c.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/mpl/transform.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/not_equal_to.hpp>
#include <boost/mpl/greater.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/or.hpp>
#include <boost/mpl/and.hpp>
#include <boost/mpl/multiplies.hpp>

#include <boost/mpl/at.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/begin.hpp>
#include <boost/mpl/end.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/push_front.hpp>
#include <boost/mpl/pop_front.hpp>
#include <boost/mpl/insert.hpp>
#include <boost/mpl/size.hpp>

using namespace boost::mpl::placeholders;
#include <iterator>
#include <utility>
#include <list>
#include <vector>
#include <string>
#include <assert.h>

namespace mpl = boost::mpl;

template<class T> struct dim
{
	BOOST_STATIC_ASSERT(0);
};

template<class A, int N1, int N2, int N3>
struct dim< A[N1][N2][N3] > :
	mpl::vector_c<int, N1, N2, N3>
{
	typedef typename A value_type;
};

template < class T >
struct dimensions :
	dim<T>::type
{};

struct test :
	dimensions< float[10][5][3] >
{

};

// dimensions seq<array>
//size<seq>
//at_c<seq,0>::type::value
typedef dimensions< char[10][9][8] > seq;
BOOST_STATIC_ASSERT( mpl::size<seq>::value == 3 );

//BOOST_STATIC_ASSERT( mpl::at<seq, mpl::int_<0> >::type::value );
// BOOST_STATIC_ASSERT(
// 		mpl::at<
// 		mpl::vector_c<int, 10,1,2>
// 		, mpl::int_<0>
// 		>::type::value == 10 );

struct none{};

template<class T=int>
struct wrapAssert : T
{
	typedef typename T type;
	BOOST_STATIC_ASSERT(0);
};

struct test2 :
	mpl::eval_if<
		mpl::equal<
			mpl::at<seq, mpl::int_<0> >::type
			, mpl::int_<10>
		>
		, none
		, wrapAssert<>
	>
{};

struct test3 :
	mpl::eval_if<
	mpl::equal<
	mpl::at<seq, mpl::int_<1> >::type
	, mpl::int_<5>
	>
	, none
	, wrapAssert<>
	>
{};

struct test4 :
	mpl::eval_if<
	mpl::equal<
	mpl::at<seq, mpl::int_<2> >::type
	, mpl::int_<3>
	>
	, none
	, wrapAssert<>
	>
{};

struct test5 :
	mpl::push_back< seq >::type
{};

void main()
{
// 	int n1 = test::type::n1;
// 	int n2 = test::type::n2;
//	test::type::value_type a;

}


//==================================================================================================
/**
  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)
**/
//==================================================================================================
#include <boost/simd/range/segmented_input_range.hpp>
#include <boost/simd/memory/allocator.hpp>
#include <boost/simd/function/sum.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include <vector>

template <typename T, typename Env>
bool is_not_unalignable(Env& $)
{
  // If alignment == sizeof(T) then moving data() by 1 won't change memory alignment, then if the
  // memory passed to segmented_*_range is already aligned to pack<T>::alignment, prologue will
  // be skiped.
  if (boost::simd::pack<T>::alignment == sizeof(T)) {
    STF_PASS("Data is not unalignable");
    return true;
  }
  return false;
}

STF_CASE_TPL("perfect iteration", STF_NUMERIC_TYPES)
{
  using boost::simd::pack;
  using boost::simd::segmented_input_range;

  std::vector<T,boost::simd::allocator<T>>  ref(pack<T>::static_size*3);

  for(std::size_t i=0;i<ref.size();i++) ref[i] = T(1);

  auto pr = segmented_input_range(ref);
  T value = 0;
  for( auto& pe : std::get<0>(pr) ) value += pe;
  for( auto& me : std::get<1>(pr) ) value += boost::simd::sum(me);
  for( auto& ee : std::get<2>(pr) ) value += ee;

  STF_EQUAL(std::distance(std::begin(std::get<0>(pr)),std::end(std::get<0>(pr))), 0 );
  STF_EQUAL(std::distance(std::begin(std::get<2>(pr)),std::end(std::get<2>(pr))), 0 );

  STF_EQUAL(value, T(ref.size()) );
}

STF_CASE_TPL("iteration with prologue", STF_NUMERIC_TYPES)
{
  using boost::simd::pack;
  using boost::simd::segmented_input_range;

  std::vector<T,boost::simd::allocator<T>> ref(pack<T>::static_size*3+pack<T>::alignment/sizeof(T));

  if (is_not_unalignable<T>($)) return;

  for(std::size_t i=0;i<ref.size();i++) ref[i] = T(1);

  auto data = boost::make_iterator_range(ref.data()+1, ref.data()+ref.size());
  auto pr = segmented_input_range(data);

  T value = 0;
  for( auto& pe : std::get<0>(pr) ) value += pe;
  for( auto& me : std::get<1>(pr) ) value += boost::simd::sum(me);
  for( auto& ee : std::get<2>(pr) ) value += ee;

  STF_EQUAL(std::distance(std::begin(std::get<2>(pr)),std::end(std::get<2>(pr))), 0 );
  STF_EQUAL(value, T(ref.size()-1) );
}

STF_CASE_TPL("iteration with epilogue", STF_NUMERIC_TYPES)
{
  using boost::simd::pack;
  using boost::simd::segmented_input_range;

  std::vector<T,boost::simd::allocator<T>>  ref(pack<T>::static_size*3);

  for(std::size_t i=0;i<ref.size();i++) ref[i] = T(1);

  auto data = boost::make_iterator_range(ref.data(), ref.data()+ref.size()-1);
  auto pr = segmented_input_range(data);

  T value = 0;
  for( auto& pe : std::get<0>(pr) ) value += pe;
  for( auto& me : std::get<1>(pr) ) value += boost::simd::sum(me);
  for( auto& ee : std::get<2>(pr) ) value += ee;

  STF_EQUAL(std::distance(std::begin(std::get<0>(pr)),std::end(std::get<0>(pr))), 0 );
  STF_EQUAL(value, T(ref.size()-1) );
}

STF_CASE_TPL("iteration with epilogue & prologue", STF_NUMERIC_TYPES)
{
  using boost::simd::pack;
  using boost::simd::segmented_input_range;

  std::vector<T,boost::simd::allocator<T>>  ref(pack<T>::static_size*3);

  for(std::size_t i=0;i<ref.size();i++) ref[i] = T(1);

  auto data = boost::make_iterator_range(ref.data()+1, ref.data()+ref.size()-1);
  auto pr = segmented_input_range(data);

  T value = 0;
  for( auto& pe : std::get<0>(pr) ) value += pe;
  for( auto& me : std::get<1>(pr) ) value += boost::simd::sum(me);
  for( auto& ee : std::get<2>(pr) ) value += ee;

  STF_EQUAL(value, T(ref.size()-2) );
}

STF_CASE_TPL("iteration with only epilogue & prologue", STF_NUMERIC_TYPES)
{
  using boost::simd::pack;
  using boost::simd::segmented_input_range;

  std::vector<T,boost::simd::allocator<T>>  ref(pack<T>::static_size*2);

  for(std::size_t i=0;i<ref.size();i++) ref[i] = T(1);

  auto data = boost::make_iterator_range(ref.data()+1, ref.data()+pack<T>::static_size-1);
  auto pr = segmented_input_range(data);

  T value = 0;
  for( auto& pe : std::get<0>(pr) ) value += pe;
  for( auto& ee : std::get<2>(pr) ) value += ee;

  STF_EQUAL(std::distance(std::begin(std::get<1>(pr)),std::end(std::get<1>(pr))), 0);
  STF_EQUAL(value, T(std::distance(std::begin(data),std::end(data))) );
}


//
// Created by wei on 5/22/18.
//

#include "common/common_utils.h"
#include "common/ConfigParser.h"
#include "core/SurfelWarpSerial.h"
#include <boost/filesystem.hpp>

std::vector<surfelwarp::Matrix4f> readMatrix(const std::string& filename, int num_frames)
{
    std::vector<Eigen::Matrix4f> poses;
//    poses.reserve(num_frames);
    std::ifstream file(filename.c_str());
    float number;
    if (file.is_open()) {
        std::string line;
        for (int i = 0; i < num_frames && std::getline(file, line); i++) {
            std::vector<float> myNumbers;
            std::stringstream iss( line );
            while ( iss >> number )
                myNumbers.push_back( number );
            poses.push_back(Eigen::Map<Eigen::Matrix<float,4,4,Eigen::RowMajor>>(myNumbers.data()));
        }
        file.close();
    }

    return poses;
}

int main(int argc, char** argv) {
    using namespace surfelwarp;

    //Get the config path
    std::string config_path, poses_path;
    if (argc <= 1) {
#if defined(WIN32)
        config_path = "C:/Users/wei/Documents/Visual Studio 2015/Projects/surfelwarp/test_data/boxing_config.json";
#else
        config_path = "/home/wei/Documents/programs/surfelwarp/test_data/boxing_config.json";
#endif
    } else {
        config_path = std::string(argv[1]);
        if (argc == 3)
            poses_path = std::string(argv[2]);
    }

    //Parse it
    auto& config = ConfigParser::Instance();
    config.ParseConfig(config_path);

    //The context
//    auto context = initCudaContext();

    std::vector<surfelwarp::Matrix4f> poses;
    if(!poses_path.empty()){
        poses = readMatrix(poses_path, config.num_frames());
        LOG(INFO) << "Read " << poses.size() << " poses";
    }

    auto fetcher = std::make_shared<surfelwarp::VolumeDeformFileFetch>(config.data_path());
    std::cout<<"Data path:" << config.data_path()<<std::endl;

    cv::Mat m_depth_img, m_rgb_img;
    m_depth_img = cv::Mat(cv::Size(config.raw_image_cols(), config.raw_image_rows()), CV_16FC1);
    m_rgb_img = cv::Mat(cv::Size(config.raw_image_cols(), config.raw_image_rows()), CV_8UC4);

    //The processing loop
    SurfelWarpSerial fusion;

    for(auto i = config.start_frame_idx(); i < config.num_frames(); i++){
        if(config.frame_skip() > 1 && i % config.frame_skip())
            continue;

        if(poses_path.empty())
        {
            fusion.Process(nullptr, nullptr, true);
        }
        else
        {
            fetcher->FetchDepthImage(i, m_depth_img);
            fetcher->FetchRGBImage(i, m_rgb_img);
            fusion.SetPose(poses[i]);
            fusion.Process(&m_rgb_img, &m_depth_img, false);
        }
    }

    //destroyCudaContext(context);
}


#ifndef CORE_ALGEBRA_ENCMAT_HPP
#define CORE_ALGEBRA_ENCMAT_HPP
#include "core/coda.hpp"
#include <vector>
#include <memory>
#include <iosfwd>
#include <NTL/mat_ZZ.h>
typedef NTL::mat_ZZ Matrix;
namespace core {
class EncVec;
class EncMat {
public:
    EncMat(core::pk_ptr pk);

    ~EncMat() {}

    EncMat(const EncMat &oth);

    EncMat& operator=(const EncMat &oth);

    long rowNums() const;

    long colNums() const;

    EncMat& pack(const Matrix &mat);

	bool unpack(Matrix &result, core::sk_ptr sk, bool negate = false) const;

    const EncVec& rowAt(int r) const;

    EncMat& add(const EncMat &oth);

    EncMat& add(const Matrix &c);

    EncMat& sub(const EncMat &oth);

    EncMat& sub(const Matrix &c);
    /// this * oth
    EncMat& dot(const EncMat &oth);
    /// this * oth, but this is symmetric matrix
    /// faster when oth is with the auxilary made.
    /// this * oth equals to oth * this since this is symmetric
    EncMat& sym_dot(const EncMat &oth);

    bool make_auxilary();

    EncVec sym_dot(const EncVec &oth) const;

    EncMat& mul(const Matrix &oth);

    EncMat& mul(const NTL::ZZ &c);

    EncMat& mul(const long &c);

    EncMat& negate();

	bool dump(std::ostream &out) const;

	bool restore(std::istream &in);
private:
	class Imp;
	std::shared_ptr<Imp> imp_;
};
} // namespace core
#endif // CORE_ALGEBRA_ENCMAT_HPP


/*
* Copyright (c) 2014, Autonomous Systems Lab
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of the Autonomous Systems Lab, ETH Zurich nor the
* names of its contributors may be used to endorse or promote products
* derived from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#include <Eigen/Dense>
#include <glog/logging.h>
#include <gtest/gtest.h>

#include "rovio/FilterStates.hpp"
#include "rovio/LocalizationLandmarkUpdate.hpp"
#include "rovio/MultiCamera.hpp"

typedef rovio::FilterState</*nMax=*/1,/*nLevels=*/2,/*patchSize=*/20,
    /*nCam=*/2,/*nPose=*/1,/*enableMapLocalization=*/true> FilterState;
typedef rovio::LocalizationLandmarkUpdate<FilterState>
LocalizationLandmarkUpdate;
typedef typename FilterState::mtState State;
typedef typename FilterState::mtState Noise;
typedef typename LocalizationLandmarkUpdate::mtMeas Measurement;
typedef typename LocalizationLandmarkUpdate::mtInnovation Innovation;

TEST(LocalizationLandmarkUpdate, Jacobian) {
  rovio::MultiCamera<State::nCam_> multi_cameras;
  for (int cam_idx = 0u; cam_idx < State::nCam_; ++cam_idx) {
    multi_cameras.setExtrinsics(cam_idx, V3D::Random(), QPD().setRandom());
  }

  LocalizationLandmarkUpdate update;
  update.setCamera(&multi_cameras);

  unsigned int seed = 1;
  State state;
  state.setRandom(seed);
  state.updateMultiCameraExtrinsics(&multi_cameras);

  // Test the update function for each camera separately.
  for (int cam_idx = 0u; cam_idx < State::nCam_; ++cam_idx) {
    // Generate a measurement that is in front of the camera.
    const cv::Point2f keypoint_distorted(1.0, 2.0);
    Eigen::Vector3d C_l;
    CHECK(multi_cameras.cameras_[cam_idx].pixelToBearing(
        keypoint_distorted, C_l));
    C_l *= 10.0;

    const Eigen::Vector3d C_landmark(1.0,2.0,1.0);
    const Eigen::Vector3d M_landmark =
        state.qCM(cam_idx).inverseRotate(C_landmark) + state.MrMC(cam_idx);
    const Eigen::Vector3d W_landmark =
        state.qWM().rotate(M_landmark) + state.WrWM();
    const Eigen::Vector3d G_landmark =
        state.qWG().inverseRotate(V3D(W_landmark - state.WrWG()));

    // The keypoint measurement is disturbed to test the computation of the
    // innovation.

    const Eigen::Vector2d kKeypointDisturbance(2.5, 4.5);

    Measurement measurement;
    measurement.G_landmark() = G_landmark;
    measurement.keypoint() << keypoint_distorted.x, keypoint_distorted.y;
    measurement.keypoint() += kKeypointDisturbance;
    measurement.set_camera_index(cam_idx);
    update.setMeasurement(measurement);

    // The innovation evaluation should yield the disturbance.
    Innovation y;
    update.evalInnovationShort(y, state);
    EXPECT_TRUE(y.pix().isApprox(-kKeypointDisturbance));

    static constexpr unsigned int kDimState = State::D_;
    static constexpr unsigned int kDimInnovation = Innovation::D_;

    // Calculate the Jacobian analytically and using finite differences.
    Eigen::MatrixXd J(kDimInnovation, kDimState);
    update.jacState(J, state);

    double epsilon = 1e-4;
    Eigen::MatrixXd J_FD(kDimInnovation, kDimState);
    update.jacStateFD(J_FD, state, epsilon);

    // Compare the two Jacobians.
    constexpr double kThreshold = 0.1;

    std::vector<std::pair<std::string, size_t>> blockname_index_list{
      {"index_WrWG", State::template getId<State::_pmp>()},
      {"index_qWG", State::template getId<State::_pma>()},
      {"index_WrWM", State::template getId<State::_pos>()},
      {"index_qWM", State::template getId<State::_att>()},
    };

    for (int cam_idx = 0u; cam_idx < State::nCam_; ++cam_idx) {
      const size_t index_MrMC = State::template getId<State::_vep>() +
          3u * cam_idx;
      const size_t index_qCM = State::template getId<State::_vea>() +
           3u * cam_idx;
      std::string block_name_MrMC = "index_MrMC_" + std::to_string(cam_idx);
      std::string block_name_q_CM = "index_qCM_" + std::to_string(cam_idx);
      blockname_index_list.emplace_back(block_name_MrMC, index_MrMC);
      blockname_index_list.emplace_back(block_name_q_CM, index_qCM);
    }

    for (const auto& blockname_index : blockname_index_list) {
      typedef Eigen::Ref<Eigen::Matrix<double, 2, 3>> JacobianBlockRef;
      const std::string& block_name = blockname_index.first;
      const size_t start_index = blockname_index.second;

      CHECK_LT(start_index, J.cols());
      JacobianBlockRef Jblock = J.block<2,3>(0, start_index);
      JacobianBlockRef Jblock_FD = J_FD.block<2,3>(0, start_index);

      const bool is_same_approx = Jblock.isApprox(Jblock_FD, kThreshold);
      if (!is_same_approx) {
        LOG(ERROR) << "JACOBIAN MISMATCH:";
        LOG(INFO) << block_name << " (analytical):\n" << Jblock;
        LOG(INFO) << block_name << " (finite-diff):\n" << Jblock_FD << std::endl;
      }
      EXPECT_TRUE(is_same_approx);
    }

    // Set all block that are expected to be nonzero to zero and make sure the
    // nothing remains non-zero.
    for (const auto& blockname_index : blockname_index_list) {
      J.block<2,3>(0, blockname_index.second).setZero();
    }
    EXPECT_TRUE(J.isApproxToConstant(0.0, kThreshold));
  }
}

int main(int argc, char* argv[]) {
  google::InitGoogleLogging(*argv);
  ::testing::InitGoogleTest(&argc, argv);
  FLAGS_alsologtostderr = true;
  FLAGS_colorlogtostderr = true;
  return RUN_ALL_TESTS();
}


#ifndef PYTHONIC_INCLUDE_NUMPY_SQRT_HPP
#define PYTHONIC_INCLUDE_NUMPY_SQRT_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/sqrt.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
#define NUMPY_NARY_FUNC_NAME sqrt
#define NUMPY_NARY_FUNC_SYM boost::simd::sqrt
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


/*
 * Copyright (c) 2018 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include <boost/test/unit_test.hpp>

#include "itkRGBToLabColorSpacePixelAccessor.h"

#include <itkCastImageFilter.h>
#include <itkGradientAnisotropicDiffusionImageFilter.h>
#include <itkImageAdaptor.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkRescaleIntensityImageFilter.h>
#include <itkSLICImageFilter.h>

#include <itkImageIOFactory.h>
#include <itkMetaImageIOFactory.h>

BOOST_AUTO_TEST_SUITE(iSeg_suite);
BOOST_AUTO_TEST_SUITE(Data_suite);

BOOST_AUTO_TEST_CASE(SLIC)
{
	itk::MetaImageIOFactory::RegisterOneFactory();

	std::string inFileName = "/Users/lloyd/Models/iSeg_TestData/mr_T1/slice_004.mhd";
	std::string outFileName = "/Users/lloyd/Models/iSeg_TestData/mr_T1/slice_004_labels.mhd";

	using RGBPixelType = itk::RGBPixel<unsigned char>;
	using RGBImageType = itk::Image<RGBPixelType, 2>;
	using RGBToLabType = itk::Accessor::RGBToLabColorSpacePixelAccessor<unsigned char, float>;
	using RGBToLabAdaptorType = itk::ImageAdaptor<RGBImageType, RGBToLabType>;

	//using ImageType = itk::Image<itk::Vector<float,3>, 2>;	//itk::VectorImage<float, 2>;
	using ImageType = itk::Image<float, 2>;							//itk::VectorImage<float, 2>;
	using LabelImageType = itk::Image<unsigned int, 2>; // int not supported by PNG writer
	using OutputImageType = itk::Image<float, 2>;				//itk::VectorImage<float, 2>;
	using SLICFilterType = itk::SLICImageFilter<ImageType, LabelImageType>;

	//using ReaderType = itk::ImageFileReader<RGBImageType>;
	using ReaderType = itk::ImageFileReader<ImageType>;
	using WriterType = itk::ImageFileWriter<OutputImageType>;

	auto reader = ReaderType::New();
	reader->SetFileName(inFileName);
	reader->Update();

	auto aniso = itk::GradientAnisotropicDiffusionImageFilter<ImageType, ImageType>::New();
	aniso->SetInput(reader->GetOutput());
	aniso->SetNumberOfIterations(5);
	aniso->SetTimeStep(0.125);
	aniso->SetConductanceParameter(1.0);

	auto rescale = itk::RescaleIntensityImageFilter<ImageType>::New();
	rescale->SetInput(reader->GetOutput());
	rescale->SetOutputMinimum(0);
	rescale->SetOutputMaximum(255.0);

	/*
	auto rgbToLabAdaptor = RGBToLabAdaptorType::New();
	rgbToLabAdaptor->SetImage(reader->GetOutput());

	using CastType = itk::CastImageFilter<RGBToLabAdaptorType, ImageType>;
	auto cast = CastType::New();
	cast->SetInput(rgbToLabAdaptor);
	*/
	auto filter = SLICFilterType::New();
	//filter->SetNumberOfWorkUnits(1);
	filter->SetSpatialProximityWeight(15.0);
	filter->SetMaximumNumberOfIterations(600);
	filter->SetSuperGridSize(30);
	filter->SetEnforceConnectivity(true);
	filter->SetInitializationPerturbation(true);
	filter->SetInput(rescale->GetOutput());
	filter->Update();

	using CastType = itk::CastImageFilter<LabelImageType, OutputImageType>;
	auto cast = CastType::New();
	cast->SetInput(filter->GetOutput());

	auto writer = WriterType::New();
	writer->SetFileName(outFileName);
	writer->SetInput(cast->GetOutput());
	writer->Update();
}

BOOST_AUTO_TEST_SUITE_END();
BOOST_AUTO_TEST_SUITE_END();


#include <boost/graph/use_mpi.hpp>
#include <boost/config.hpp>
#include <boost/throw_exception.hpp>
#include <boost/graph/distributed/adjacency_list.hpp>
#include <boost/property_map/parallel/distributed_property_map.hpp>
#include <boost/graph/distributed/mpi_process_group.hpp>
#include <boost/graph/distributed/concepts.hpp>
#include <boost/graph/distributed/connected_components_parallel_search.hpp>
#include <boost/graph/distributed/connected_components.hpp>
#include <boost/graph/rmat_graph_generator.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/property_map/vector_property_map.hpp>

#include <iostream>
#include <cstdlib>
#include <iomanip>

#ifdef BOOST_NO_EXCEPTIONS
void
boost::throw_exception(std::exception const& ex)
{
  std::cout << ex.what() << std::endl;
  abort();
}
#endif

using namespace boost;
using boost::graph::distributed::mpi_process_group;

typedef double time_type;

inline time_type get_time()
{
  return MPI_Wtime();
}

std::string print_time(time_type t)
{
  std::ostringstream out;
  out << std::setiosflags(std::ios::fixed) << std::setprecision(2) << t;
  return out.str();
}

void
test_filtered_rmat_cc(char* graphfilename ,int num_ofVertex, int num_ofEdges)
{

  int N = num_ofVertex;
  mpi_process_group pg;
  mpi_process_group::process_id_type id = process_id(pg);

  //if (id == 0) printf("INFO: Params: n=%d, m=%d, a=%f, b=%f, c=%f, d=%f.\n", n, m, a, b, c, d);

  parallel::variant_distribution<mpi_process_group> distrib
    = parallel::block(pg, N);

  typedef adjacency_list<vecS,
        distributedS<mpi_process_group, vecS>,
        undirectedS> Graph;

  typedef keep_local_edges<parallel::variant_distribution<mpi_process_group>,
                           mpi_process_group::process_id_type>
    EdgeFilter;

  //typedef unique_rmat_iterator<rand48, Graph, EdgeFilter>
    //RMATIter;

  //if (id == 0) printf("INFO: Generating graph.\n");

  //rand48 gen;
  //Graph g(RMATIter(gen, n, m, a, b, c, d, true, EdgeFilter(distrib, id)),
    //      RMATIter(), n, pg, distrib);

typedef std::pair<int, int> Edge;
  //init Graph
  //std::ifstream graphfile("./amazon_gen_15.txt");
  std::ifstream graphfile(graphfilename);
  std::string line;
  //Edge edge_array[] = { Edge(A,B), Edge(A,C), Edge(B,C), Edge(C,A), Edge(D,C) };
  Edge edge_array[num_ofEdges];
  memset(edge_array,0,sizeof(int));
  //

  //Graph g;
  //std::map<int,int> edge_array;
  int i=0;

  //std::cout<<"!!!!!"<<std::endl;
  while(getline(graphfile,line)!=NULL){

      std::string::size_type index = line.find_first_of("\t",0);
      std::string::size_type end = line.find_first_of("\n",0);
      std::string inedge_s = line.substr(0,index);
      int inedge_num = std::atoi(inedge_s.c_str());
      std::string outedge_s = line.substr(index+1,end);
      int outedge_num = std::atoi(outedge_s.c_str());

      //add_edge(inedge_num, outedge_num, g);
      //edge_array.insert(inedge_num,outedge_num);

      edge_array[i].first=inedge_num;
      edge_array[i].second=outedge_num;
      //std::cout << edge_array[i].first <<" "<<edge_array[i].second<< std::endl;
      i++;
  }
  const int num_edges = sizeof(edge_array)/sizeof(edge_array[0]);

  // declare a graph object
  Graph g(edge_array, edge_array + num_edges, N);

  //std::vector<double> ranks(num_vertices(g));

  //page_rank(g,
  //          make_iterator_property_map(ranks.begin(),
   //                                    get(boost::vertex_index, g)),
   //         n_iterations(iterations), 0.85, N);

  synchronize(g);
  if (id == 0) {
      printf("INFO: Starting connected components.\n");
      printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d\n", graphfilename , num_ofVertex, num_ofEdges );
  }

  std::vector<int> local_components_vec(num_vertices(g));
  typedef iterator_property_map<std::vector<int>::iterator, property_map<Graph, vertex_index_t>::type> ComponentMap;
  ComponentMap component(local_components_vec.begin(), get(vertex_index, g));

  time_type start = get_time();
  int num_components = connected_components(g, component);
  time_type end = get_time();

  if (process_id(g.process_group()) == 0) {
      std::cout << "INFO: Test Complete. components found = " << num_components
                << ", time = " << print_time(end - start) << "s." << std::endl;
  }

}

int
main(int argc, char* argv[])
{
  mpi::environment env(argc, argv);

  if (argc < 5){
      //printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d, iterations=%d\n", "amazon_gen_15.txt",32768 , 131655, 50);
      test_filtered_rmat_cc("amazon_gen_15.txt",32768, 131655);
  }
  else{
     // printf("INFO: Params: InputGraphfile=%s, num_ofVertex=%d, num_ofEdges=%d, iterations=%d\n", argv[1], atoi(argv[2]), atoi(argv[3]), atoi(argv[4]) );
      test_filtered_rmat_cc(argv[1], atoi(argv[2]), atoi(argv[3]));
  }

  //test_distributed_page_rank(num_ofVertex,num_ofEdges,iterations);

  return 0;
}


#include "engine/douglas_peucker.hpp"
#include "util/coordinate_calculation.hpp"

#include <boost/test/test_case_template.hpp>
#include <boost/test/unit_test.hpp>

#include <osrm/coordinate.hpp>

#include <vector>

BOOST_AUTO_TEST_SUITE(douglas_peucker_simplification)

using namespace osrm;
using namespace osrm::engine;

BOOST_AUTO_TEST_CASE(calibrate_thresholds)
{
    auto thresholds = detail::generateThreshold(5, 19);
    auto z = 0;
    for (auto t : thresholds)
    {
        BOOST_TEST_MESSAGE(t << ", // z" << z++);
    }
}

BOOST_AUTO_TEST_CASE(removed_middle_test)
{
    /*
            x
           / \
          x   \
         /     \
        x       x
    */
    std::vector<util::Coordinate> coordinates = {
        util::Coordinate{util::FloatLongitude{5}, util::FloatLatitude{5}},
        util::Coordinate{util::FloatLongitude{12.5}, util::FloatLatitude{12.6096298302}},
        util::Coordinate{util::FloatLongitude{20}, util::FloatLatitude{20}},
        util::Coordinate{util::FloatLongitude{25}, util::FloatLatitude{5}}};

    for (unsigned z = 0; z < detail::DOUGLAS_PEUCKER_THRESHOLDS_SIZE; z++)
    {
        auto result = douglasPeucker(coordinates, z);
        BOOST_CHECK_EQUAL(result.size(), 3);
        BOOST_CHECK_EQUAL(result[0], coordinates[0]);
        BOOST_CHECK_EQUAL(result[1], coordinates[2]);
        BOOST_CHECK_EQUAL(result[2], coordinates[3]);
    }
}

BOOST_AUTO_TEST_CASE(removed_middle_test_zoom_sensitive)
{
    /*
            x
           / \
          x   \
         /     \
        x       x
    */
    std::vector<util::Coordinate> coordinates = {
        util::Coordinate{util::FloatLongitude{5}, util::FloatLatitude{5}},
        util::Coordinate{util::FloatLongitude{6}, util::FloatLatitude{6}},
        util::Coordinate{util::FloatLongitude{20}, util::FloatLatitude{20}},
        util::Coordinate{util::FloatLongitude{25}, util::FloatLatitude{5}}};

    // Coordinate 6,6 should start getting included at Z9 and higher
    // Note that 5,5->6,6->10,10 is *not* a straight line on the surface
    // of the earth
    for (unsigned z = 0; z < 9; z++)
    {
        auto result = douglasPeucker(coordinates, z);
        BOOST_CHECK_EQUAL(result.size(), 3);
    }
    // From 9 to max zoom, we should get all coordinates
    for (unsigned z = 9; z < detail::DOUGLAS_PEUCKER_THRESHOLDS_SIZE; z++)
    {
        auto result = douglasPeucker(coordinates, z);
        BOOST_CHECK_EQUAL(result.size(), 4);
    }
}

BOOST_AUTO_TEST_CASE(remove_second_node_test)
{
    // derived from the degreeToPixel function
    const auto delta_pixel_to_delta_degree = [](const int pixel, const unsigned zoom) {
        const double shift = (1u << zoom) * 256;
        const double b = shift / 2.0;
        return pixel * 180. / b;
    };
    // For zoom level 0 all gets reduced to a line
    for (unsigned z = 1; z < detail::DOUGLAS_PEUCKER_THRESHOLDS_SIZE; z++)
    {
        /*
             x
             | \
           x-x  x
                |
                x
        */
        std::vector<util::Coordinate> input = {
            util::Coordinate{util::FloatLongitude{5}, util::FloatLatitude{5}},
            util::Coordinate{util::FloatLongitude{5 + delta_pixel_to_delta_degree(2, z)},
                             util::FloatLatitude{5}},
            util::Coordinate{util::FloatLongitude{10}, util::FloatLatitude{10}},
            util::Coordinate{util::FloatLongitude{5}, util::FloatLatitude{15}},
            util::Coordinate{util::FloatLongitude{5},
                             util::FloatLatitude{15 + delta_pixel_to_delta_degree(2, z)}}};
        BOOST_TEST_MESSAGE("Delta (" << z << "): " << delta_pixel_to_delta_degree(2, z));
        auto result = douglasPeucker(input, z);
        BOOST_CHECK_EQUAL(result.size(), 3);
        BOOST_CHECK_EQUAL(input[0], result[0]);
        BOOST_CHECK_EQUAL(input[2], result[1]);
        BOOST_CHECK_EQUAL(input[4], result[2]);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef LWTNN_EIGEN_TYPEDEFS_HH
#define LWTNN_EIGEN_TYPEDEFS_HH

#include <Eigen/Dense>

namespace lwt {

  template<typename T>
  using VectorX = Eigen::Matrix<T, Eigen::Dynamic, 1>;

  template<typename T>
  using MatrixX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;

  template<typename T>
  using ArrayX = Eigen::Array<T, Eigen::Dynamic, Eigen::Dynamic>;

}

#endif // LWTNN_EIGEN_TYPEDEFS_HH


#include <boost/program_options.hpp>
#include <string>
#include <d3d_base/grid.h>
#include <iostream>
#include <d3d_base/configFile.h>
#include <d3d_fusion/volumetricFusionTools.h>
#include <algorithm>

void projectToHalspaceSpereIntersection(float& x, float& y, float& z, float nx, float ny, float nz, float dist, float radius)
{
    // check if point inside halfspace
    float pd = nx*x + ny*y + nz*z;
    if (pd < dist) {
        // project to sphere
        float norm = std::sqrt(x*x + y*y + z*z);
        float denom = std::max(1.f, norm/radius);
        x /= denom;
        y /= denom;
        z /= denom;
    } else {
        // project on halfplane
        float distDiff = pd-dist;
        x -= distDiff*nx;
        y -= distDiff*ny;
        z -= distDiff*nz;

        // check if inside sphere, if so we are done, otherwise project on circle (assuming radius > dist)
        float norm = std::sqrt(x*x + y*y + z*z);
        if (norm > radius) {
            // precompute for more efficient implemenetation
            float cicleRadius = std::sqrt(radius*radius - dist*dist);

            // shift to origin
            x -= dist*nx;
            y -= dist*ny;
            z -= dist*nz;

            // rescale such that norm = circleRadius
            norm = std::sqrt(x*x + y*y + z*z);
            x *= cicleRadius/norm;
            y *= cicleRadius/norm;
            z *= cicleRadius/norm;

            // shift to plane
            x += dist*nx;
            y += dist*ny;
            z += dist*nz;
        }
    }
}

struct Hist {
    float counts[8];
};

struct NormalInfo {
    bool isIsotropic;
    Eigen::Vector3f n1;
    Eigen::Vector3f n2;
    Eigen::Vector3f n3;
};

D3D::Grid<float> runTVHist(D3D::Grid<Hist>& dataCost, D3D::Grid<NormalInfo>& normals, Eigen::Vector3f& minCorner, Eigen::Vector3f& size,
                           Eigen::Matrix4f boxToGlobal, Eigen::Vector3f color, std::string vrmlOutputFile,
                           const int numIter, const float lambda, const float theta)
{
    D3D::Grid<float> u(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth());
    for (unsigned z = 0; z < u.getDepth(); z++)
        for (unsigned y = 0; y < u.getHeight(); y++)
            for (unsigned x = 0; x < u.getWidth(); x++)
            {
                u(x,y,z) = 0.f;
            }

    D3D::Grid<float> uBar = u.clone();

    D3D::Grid<float> lambda11(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda12(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda13(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda21(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda22(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> lambda23(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);

    D3D::Grid<float> lambdaBar11 = lambda11.clone();
    D3D::Grid<float> lambdaBar12 = lambda12.clone();
    D3D::Grid<float> lambdaBar13 = lambda13.clone();
    D3D::Grid<float> lambdaBar21 = lambda21.clone();
    D3D::Grid<float> lambdaBar22 = lambda22.clone();
    D3D::Grid<float> lambdaBar23 = lambda23.clone();

    // dual variables
    D3D::Grid<float> p11(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p12(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p13(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p21(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p22(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p23(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p31(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p32(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);
    D3D::Grid<float> p33(dataCost.getWidth(), dataCost.getHeight(), dataCost.getDepth(), 0);

    const int xDim = u.getWidth();
    const int yDim = u.getHeight();
    const int zDim = u.getDepth();

    // uses diagonal preconditioned first order pirmal dual

    const float sigma = 0.99f/3.f;
    const float tau = 0.99f/12.f;
    const float centers[8] = {-1.f, -5.f/7.f, -3.f/7.f, -1.f/7.f, 1.f/7.f, 3.f/7.f, 5.f/7.f, 1.f};

    for (int c = 0; c < numIter; c++)
    {
        std::cout << ".";
        std::cout.flush();

        // update dual variables
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z1 = (z < zDim-1) ? (z+1) : z;
            for (int y = 0; y < yDim; y++)
            {
                int const Y1 = (y < yDim-1) ? (y+1) : y;
                for (int x = 0; x < xDim; x++)
                {
                    float const u0 = uBar(x,y,z);

                    int const X1 = (x < xDim -1) ? (x+1) : x;

                    float const u_x = (uBar(X1, y, z) - u0);
                    float const u_y = (uBar(x, Y1, z) - u0);
                    float const u_z = (uBar(x, y, Z1) - u0);

                    float& p1w = p11(x,y,z);
                    float& p1h = p12(x,y,z);
                    float& p1d = p13(x,y,z);
                    float& p2w = p21(x,y,z);
                    float& p2h = p22(x,y,z);
                    float& p2d = p23(x,y,z);
                    float& p3w = p31(x,y,z);
                    float& p3h = p32(x,y,z);
                    float& p3d = p33(x,y,z);

                    p1w += sigma*(u_x + lambdaBar11(x,y,z));
                    p1h += sigma*(u_y + lambdaBar12(x,y,z));
                    p1d += sigma*(u_z + lambdaBar13(x,y,z));

                    p2w += sigma*(-lambdaBar11(x,y,z) + lambdaBar21(x,y,z));
                    p2h += sigma*(-lambdaBar12(x,y,z) + lambdaBar22(x,y,z));
                    p2d += sigma*(-lambdaBar13(x,y,z) + lambdaBar23(x,y,z));

                    p3w += sigma*(-lambdaBar21(x,y,z));
                    p3h += sigma*(-lambdaBar22(x,y,z));
                    p3d += sigma*(-lambdaBar23(x,y,z));

                    if (normals(x,y,z).isIsotropic) {
                        float tv = sqrtf(p1w*p1w + p1h*p1h + p1d*p1d);
                        float denom = std::max(1.f, tv/2.f);
                        p1w /= denom;
                        p1h /= denom;
                        p1d /= denom;

                        tv = sqrtf(p2w*p2w + p2h*p2h + p2d*p2d);
                        denom = std::max(1.f, tv/2.f);
                        p2w /= denom;
                        p2h /= denom;
                        p2d /= denom;

                        tv = sqrtf(p3w*p3w + p3h*p3h + p3d*p3d);
                        denom = std::max(1.f, tv/2.f);
                        p3w /= denom;
                        p3h /= denom;
                        p3d /= denom;
                    } else {
                        Eigen::Vector3f n1 = -normals(x,y,z).n1;
                        Eigen::Vector3f n2 = -normals(x,y,z).n2;
                        Eigen::Vector3f n3 = -normals(x,y,z).n3;
                        projectToHalspaceSpereIntersection(p1w, p1h, p1d, n1(0), n1(1), n1(2), 0.5, 2.0);
                        projectToHalspaceSpereIntersection(p2w, p2h, p2d, n2(0), n2(1), n2(2), 0.5, 2.0);
                        projectToHalspaceSpereIntersection(p3w, p3h, p3d, n3(0), n3(1), n3(2), 0.5, 2.0);
                    }
                }
            }
        }

        // update primal
#pragma omp parallel for
        for (int z = 0; z < zDim; z++)
        {
            int const Z0 = z-1;
            for (int y = 0; y < yDim; y++)
            {
                int const Y0 = y-1;
                for (int x = 0; x < xDim; x++)
                {
                    int const X0 = x-1;

                    float const div = (X0 < xDim -1 ? p11(x,y,z) : 0.0f) - (X0 >= 0 ? p11(X0, y, z) : 0.0f) +
                                      (Y0 < yDim -1 ? p12(x,y,z) : 0.0f) - (Y0 >= 0 ? p12(x, Y0, z) : 0.0f) +
                                      (Z0 < zDim -1 ? p13(x,y,z) : 0.0f) - (Z0 >= 0 ? p13(x, y, Z0) : 0.0f);

                    float const u0 = u(x,y,z);
                    float U = u(x,y,z) + tau*div;

//                    // check most likely bin, is in [1, 8] as u0 in [-1, 1]
//                    int mlBin = int(7.f * (u0 + 1.f) / 2.f + 1.f);

//                    float step = 0.f;
//                    for (int b = 0; b <= mlBin; ++b) {
//                        step -= float(dataCost(x, y, z).counts[b]);
//                    }
//                    for (int b = mlBin+1; b < 10; ++b) {
//                        step += float(dataCost(x, y, z).counts[b]);
//                    }

//                    float proxMin = u0 + tau*lambda*step;

                    float proxMin, step;

//                    if (proxMin <= centers[mlBin] || centers[mlBin+1] <= proxMin) {
                        // outside of range
                        bool found = false;
                        for (int bin = 0; bin < 7; bin++) {
//                            if (bin == mlBin)
//                                continue;
                            step = 0.f;
                            for (int b = 0; b <= bin; ++b) {
                                step -= dataCost(x, y, z).counts[b];
                            }
                            for (int b = bin+1; b < 8; ++b) {
                                step += dataCost(x, y, z).counts[b];
                            }

                            proxMin = U + tau*lambda*step;

                            if (centers[bin] < proxMin && proxMin < centers[bin+1]) {
                                // inside of range, val is the minimum
                                found = true;
                                break;
                            }
                        }
                        if (!found) {
                            // minimum must be one of the bin centers
                            float proxMinEnergy = std::numeric_limits<float>::max();
                            for (int bin = 0; bin < 8; bin++) {
                                float dataterm = 0.f;
                                for (int i = 0; i < 8; ++i) {
                                    dataterm += dataCost(x, y, z).counts[i] * fabs(centers[bin]-centers[i]);
                                }
                                float proxEnergy = (U-centers[bin])*(U-centers[bin])/(2.f*tau) + lambda*dataterm;
                                if (proxEnergy < proxMinEnergy) {
                                    proxMinEnergy = proxEnergy;
                                    proxMin = centers[bin];
                                }
                            }
                        }
//                    }

                    U = proxMin;

                    u(x,y,z) = U;
                    uBar(x,y,z) = U + theta*(U - u0);

                    float const L11 = lambda11(x,y,z) - tau*(p11(x,y,z) - p21(x,y,z));
                    float const L12 = lambda12(x,y,z) - tau*(p12(x,y,z) - p22(x,y,z));
                    float const L13 = lambda13(x,y,z) - tau*(p13(x,y,z) - p23(x,y,z));

                    lambdaBar11(x,y,z) = L11 + theta*(L11 - lambda11(x,y,z));
                    lambdaBar12(x,y,z) = L12 + theta*(L12 - lambda12(x,y,z));
                    lambdaBar13(x,y,z) = L13 + theta*(L13 - lambda13(x,y,z));

                    lambda11(x,y,z) = L11;
                    lambda12(x,y,z) = L12;
                    lambda13(x,y,z) = L13;

                    float const L21 = lambda21(x,y,z) - tau*(p21(x,y,z) - p31(x,y,z));
                    float const L22 = lambda22(x,y,z) - tau*(p22(x,y,z) - p32(x,y,z));
                    float const L23 = lambda23(x,y,z) - tau*(p23(x,y,z) - p33(x,y,z));

                    lambdaBar21(x,y,z) = L21 + theta*(L21 - lambda21(x,y,z));
                    lambdaBar22(x,y,z) = L22 + theta*(L22 - lambda22(x,y,z));
                    lambdaBar23(x,y,z) = L23 + theta*(L23 - lambda23(x,y,z));

                    lambda21(x,y,z) = L21;
                    lambda22(x,y,z) = L22;
                    lambda23(x,y,z) = L23;
                }
            }
        }

        if ((c+1) % 50 == 0) {
            D3D::saveVolumeAsVRMLMesh(u, 0.0f, minCorner, size, boxToGlobal, color, vrmlOutputFile, true);
            u.saveAsDataFile("tvHistNormalFusionU.dat");
            std::cout << std::endl;

        }
    }
    return u;
}

int main(int argc, char* argv[])
{
    std::string configFile;
    std::string intDepthGridFile;
    std::string clusteredNormalsFile;
    std::string vrmlOutputFile;

    boost::program_options::options_description desc("Allowed options");
    desc.add_options()
            ("help", "Produce help message")
            ("intDepthGridFile", boost::program_options::value<std::string>(&intDepthGridFile)->default_value("intCSDFGrid.dat"), "Depth map list file")
            ("clusteredNormalsFile", boost::program_options::value<std::string>(&clusteredNormalsFile)->default_value("clusteredNormals.dat"), "Clustered normals file")
            ("configFile", boost::program_options::value<std::string>(&configFile)->default_value("conf.txt"), "Config file")
            ("vrmlOutputFile", boost::program_options::value<std::string>(&vrmlOutputFile)->default_value("tvHistNormalModel.wrl"), "Output data file with integrated depth data.")
            ;

    boost::program_options::variables_map vm;
    boost::program_options::store(boost::program_options::command_line_parser(argc, argv).options(desc).run(), vm);
    boost::program_options::notify(vm);

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

    // config file
    D3D::ConfigFile conf(configFile);

    if (!conf.isFileRead())
    {
        D3D_THROW_EXCEPTION("Could not open config file.")
    }

    // load the box parameters
    std::string bpMinXStr = conf.get("BP_MIN_X");
    if (bpMinXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_X not specified in the config file.")
    }
    float bpMinX = atof(bpMinXStr.c_str());

    std::string bpSizeXStr = conf.get("BP_SIZE_X");
    if (bpSizeXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_X not specified in the config file.")
    }
    float bpSizeX = atof(bpSizeXStr.c_str());

    std::string bpMinYStr = conf.get("BP_MIN_Y");
    if (bpMinYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Y not specified in the config file.")
    }
    float bpMinY = atof(bpMinYStr.c_str());

    std::string bpSizeYStr = conf.get("BP_SIZE_Y");
    if (bpSizeYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Y not specified in the config file.")
    }
    float bpSizeY = atof(bpSizeYStr.c_str());

    std::string bpMinZStr = conf.get("BP_MIN_Z");
    if (bpMinZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_MIN_Z not specified in the config file.")
    }
    float bpMinZ = atof(bpMinZStr.c_str());

    std::string bpSizeZStr = conf.get("BP_SIZE_Z");
    if (bpSizeZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_SIZE_Z not specified in the config file.")
    }
    float bpSizeZ = atof(bpSizeZStr.c_str());

    std::string bpResXStr = conf.get("BP_RES_X");
    if (bpResXStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_X not specified in the config file.")
    }
    int bpResX = atoi(bpResXStr.c_str());

    std::string bpResYStr = conf.get("BP_RES_Y");
    if (bpResYStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Y not specified in the config file.")
    }
    int bpResY = atoi(bpResYStr.c_str());

    std::string bpResZStr = conf.get("BP_RES_Z");
    if (bpResZStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter BP_RES_Z not specified in the config file.")
    }
    int bpResZ = atoi(bpResZStr.c_str());

    std::string bpOriginStr = conf.get("BP_ORIGIN");
    Eigen::Vector3f bpOrigin;
    if (bpOriginStr.empty())
    {
        bpOrigin = Eigen::Vector3f::Zero();
    }
    else
    {
        std::stringstream bpOriginStream(bpOriginStr);
        bpOriginStream >> bpOrigin(0);
        bpOriginStream >> bpOrigin(1);
        bpOriginStream >> bpOrigin(2);

        if (bpOriginStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_ORIGIN");
        }
    }

    std::string bpXAxisStr = conf.get("BP_X_AXIS");
    Eigen::Vector3f bpXAxis;
    bool xAxisSet = false;
    if (!bpXAxisStr.empty())
    {
        xAxisSet = true;

        std::stringstream bpXAxisStream(bpXAxisStr);
        bpXAxisStream >> bpXAxis(0);
        bpXAxisStream >> bpXAxis(1);
        bpXAxisStream >> bpXAxis(2);

        if (bpXAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_X_AXIS");
        }
    }

    std::string bpYAxisStr = conf.get("BP_Y_AXIS");
    Eigen::Vector3f bpYAxis;
    bool yAxisSet = false;
    if (!bpYAxisStr.empty())
    {
        yAxisSet = true;

        std::stringstream bpYAxisStream(bpYAxisStr);
        bpYAxisStream >> bpYAxis(0);
        bpYAxisStream >> bpYAxis(1);
        bpYAxisStream >> bpYAxis(2);

        if (bpYAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Y_AXIS");
        }
    }

    std::string bpZAxisStr = conf.get("BP_Z_AXIS");
    Eigen::Vector3f bpZAxis;
    bool zAxisSet = false;
    if (!bpZAxisStr.empty())
    {
        zAxisSet = true;

        std::stringstream bpZAxisStream(bpZAxisStr);
        bpZAxisStream >> bpZAxis(0);
        bpZAxisStream >> bpZAxis(1);
        bpZAxisStream >> bpZAxis(2);

        if (bpZAxisStream.fail())
        {
            D3D_THROW_EXCEPTION("Error reading BP_Z_AXIS");
        }
    }

    if (xAxisSet == false && yAxisSet == false && zAxisSet == false)
    {
        bpXAxis(0) = 1; bpXAxis(1) = 0; bpXAxis(2) = 0;
        bpYAxis(0) = 0; bpYAxis(1) = 1; bpYAxis(2) = 0;
        bpZAxis(0) = 0; bpZAxis(1) = 0; bpZAxis(2) = 1;
    }
    else if (!(xAxisSet && yAxisSet && zAxisSet))
    {
        D3D_THROW_EXCEPTION("Either all or none out of BP_X_AXIS, BP_Y_AXIS and BP_Z_AXIS have to be specified.")
    }

    // rectify the axes
    // normlaize
    bpXAxis /= bpXAxis.norm();
    bpYAxis /= bpYAxis.norm();
    bpZAxis /= bpZAxis.norm();

    Eigen::Vector3f rightAngleZAxis = bpXAxis.cross(bpYAxis);
    if (rightAngleZAxis.dot(bpZAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.")
    }

    bpZAxis = rightAngleZAxis;

    Eigen::Vector3f rightAngleYAxis = bpZAxis.cross(bpXAxis);
    if (rightAngleYAxis.dot(bpYAxis) < 0.95)
    {
        D3D_THROW_EXCEPTION("The given axes are not orthogonal to each other.");
    }

    bpYAxis = rightAngleYAxis;

    Eigen::Matrix4f boxToGlobal = Eigen::Matrix4f::Identity();
    boxToGlobal.topLeftCorner(3,3).col(0) = bpXAxis;
    boxToGlobal.topLeftCorner(3,3).col(1) = bpYAxis;
    boxToGlobal.topLeftCorner(3,3).col(2) = bpZAxis;
    boxToGlobal.topRightCorner(3,1).col(0) = bpOrigin;

    std::string hfNumIterStr = conf.get("HF_NUM_ITER");
    if (hfNumIterStr.empty())
    {
        D3D_THROW_EXCEPTION("Paramter HF_NUM_ITER is not specified in the config file.")
    }
    int ffNumIter = atoi(hfNumIterStr.c_str());

    std::string hfLambdaStr = conf.get("HF_LAMBDA");
    if (hfLambdaStr.empty())
    {
        D3D_THROW_EXCEPTION("Parameter HF_LAMBDA is not specified in the config file.")
    }
    float hfLambda = atof(hfLambdaStr.c_str());

    // load grid
    D3D::Grid<Hist> dataCost;
    dataCost.loadFromDataFile(intDepthGridFile);

    std::cout << "Data Cost Loaded: width = " << dataCost.getWidth() << ", height = " << dataCost.getHeight() << ", depth = " << dataCost.getDepth() << std::endl;

    if (bpResX != (int) dataCost.getWidth() || bpResY != (int) dataCost.getHeight() || bpResZ != (int) dataCost.getDepth())
    {
        D3D_THROW_EXCEPTION("Resolution specified in the config file does not match the dimension of the grid with integrated depth maps.")
    }

    // load clustered normals
    D3D::Grid<NormalInfo> clusteredNormals;
    clusteredNormals.loadFromDataFile(clusteredNormalsFile);

    std::cout << "Clustered Normals Loaded: width = " << clusteredNormals.getWidth() << ", height = " << clusteredNormals.getHeight() << ", depth = " << clusteredNormals.getDepth() << std::endl;

    if (bpResX != (int) clusteredNormals.getWidth() || bpResY != (int) clusteredNormals.getHeight() || bpResZ != (int) clusteredNormals.getDepth())
    {
        D3D_THROW_EXCEPTION("Resolution specified in the config file does not match the dimension of the grid with the clustered normals.")
    }

    Eigen::Vector3f minCorner;
    minCorner(0) = bpMinX;
    minCorner(1) = bpMinY;
    minCorner(2) = bpMinZ;
    Eigen::Vector3f size;
    size(0) = bpSizeX;
    size(1) = bpSizeY;
    size(2) = bpSizeZ;
    Eigen::Vector3f color;
    color(0) = 0.5;
    color(1) = 0.5;
    color(2) = 0.5;

    runTVHist(dataCost, clusteredNormals, minCorner, size, boxToGlobal, color, vrmlOutputFile, ffNumIter, hfLambda, 1);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

// Copyright (c) 2016 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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 <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/algorithms/detail/overlay/debug_turn_info.hpp>
#include <boost/geometry/geometries/geometries.hpp>

#include "multi_overlay_cases.hpp"

static const std::size_t default_result = 9999;

// Adapted copy of handle_colocations::gather_cluster_properties
template
<
    bool Reverse1, bool Reverse2,
    typename Turns,
    typename Clusters,
    typename Geometry1,
    typename Geometry2
>
std::size_t test_gather_cluster_properties(std::string const& case_id,
        Clusters& clusters, Turns& turns,
        bg::detail::overlay::operation_type for_operation,
        Geometry1 const& geometry1, Geometry2 const& geometry2)
{
    using namespace boost::geometry;
    using namespace boost::geometry::detail::overlay;

    std::size_t result = default_result;

    typedef typename boost::range_value<Turns>::type turn_type;
    typedef typename turn_type::point_type point_type;
    typedef typename turn_type::turn_operation_type turn_operation_type;

    // Define sorter, sorting counter-clockwise such that polygons are on the
    // right side
    typedef sort_by_side::side_sorter
        <
            Reverse1, Reverse2, point_type, std::less<int>
        > sbs_type;

    // Only test the FIRST cluster (for now)
    typename Clusters::iterator mit = clusters.begin();
    if (mit == clusters.end())
    {
        return result;
    }

    cluster_info& cinfo = mit->second;
    std::set<signed_size_type> const& ids = cinfo.turn_indices;
    if (ids.empty())
    {
        return result;
    }

    sbs_type sbs;
    point_type turn_point; // should be all the same for all turns in cluster

    bool first = true;
    for (typename std::set<signed_size_type>::const_iterator sit = ids.begin();
         sit != ids.end(); ++sit)
    {
        signed_size_type turn_index = *sit;
        turn_type const& turn = turns[turn_index];
        if (first)
        {
            turn_point = turn.point;
        }
        for (int i = 0; i < 2; i++)
        {
            turn_operation_type const& op = turn.operations[i];
            sbs.add(op, turn_index, i, geometry1, geometry2, first);
            first = false;
        }
    }
    sbs.apply(turn_point);

    sbs.find_open();
    sbs.assign_zones(for_operation);

    cinfo.open_count = sbs.open_count(for_operation);

    return cinfo.open_count;
}

// Adapted copy of overlay::apply
template
<
    bg::overlay_type OverlayType,
    bool Reverse1, bool Reverse2, bool ReverseOut,
    typename GeometryOut,
    typename Geometry1, typename Geometry2,
    typename RobustPolicy, typename Strategy
>
std::size_t apply_overlay(std::string const& case_id,
            Geometry1 const& geometry1, Geometry2 const& geometry2,
            RobustPolicy const& robust_policy,
            Strategy const& )
{
    using namespace boost::geometry;

    typedef typename bg::point_type<GeometryOut>::type point_type;
    typedef bg::detail::overlay::traversal_turn_info
    <
        point_type,
        typename bg::segment_ratio_type<point_type, RobustPolicy>::type
    > turn_info;
    typedef std::deque<turn_info> turn_container_type;

    typedef std::deque
        <
            typename bg::ring_type<GeometryOut>::type
        > ring_container_type;

    // Define the clusters, mapping cluster_id -> turns
    typedef std::map
        <
            signed_size_type,
            bg::detail::overlay::cluster_info
        > cluster_type;

    turn_container_type turns;

    detail::get_turns::no_interrupt_policy policy;
    bg::get_turns
        <
            Reverse1, Reverse2,
            detail::overlay::assign_null_policy
        >(geometry1, geometry2, robust_policy, turns, policy);

    typename Strategy::side_strategy_type side_strategy;
    cluster_type clusters;

    bg::enrich_intersection_points<Reverse1, Reverse2, OverlayType>(turns,
            clusters, geometry1, geometry2,
                robust_policy,
                side_strategy);

    if (clusters.size() == 1)
    {
        // Now gather cluster properties again, with test option
        return test_gather_cluster_properties<Reverse1, Reverse2>(case_id,
                clusters, turns, bg::detail::overlay::operation_from_overlay<OverlayType>::value,
                    geometry1, geometry2);
    }

    return default_result;
}

template <typename Geometry, bg::overlay_type OverlayType>
void test_sort_by_side(std::string const& case_id,
        std::string const& wkt1, std::string const& wkt2,
        std::size_t expected_open_count)
{
//    std::cout << case_id << std::endl;

    Geometry g1;
    bg::read_wkt(wkt1, g1);

    Geometry g2;
    bg::read_wkt(wkt2, g2);

    // Reverse if necessary
    bg::correct(g1);
    bg::correct(g2);

    typedef typename boost::range_value<Geometry>::type geometry_out;

    typedef typename bg::rescale_overlay_policy_type
    <
        Geometry,
        Geometry
    >::type rescale_policy_type;

    rescale_policy_type robust_policy
        = bg::get_rescale_policy<rescale_policy_type>(g1, g2);

    typedef bg::intersection_strategies
    <
        typename bg::cs_tag<Geometry>::type,
        Geometry,
        Geometry,
        typename bg::point_type<Geometry>::type,
        rescale_policy_type
    > strategy;

    std::size_t open_count = apply_overlay<OverlayType, false, false, false, geometry_out>(case_id, g1, g2,
                    robust_policy,
                   strategy());

    BOOST_CHECK_MESSAGE(open_count == expected_open_count,
                        "  caseid="  << case_id
                        << " open count: expected="  << expected_open_count
                        << " detected="  << open_count);
}

// Define two small macro's to avoid repetitions of testcases/names etc
#define TEST_INT(caseid, exp) { (test_sort_by_side<multi_polygon, bg::overlay_intersection>) \
    ( #caseid "_int", caseid[0], caseid[1], exp); }

#define TEST_UNION(caseid, exp) { (test_sort_by_side<multi_polygon, bg::overlay_union>) \
    ( #caseid "_union", caseid[0], caseid[1], exp); }

template <typename T>
void test_all()
{
    typedef bg::model::point<T, 2, bg::cs::cartesian> point_type;
    typedef bg::model::polygon<point_type> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    // Selection of test cases having only one cluster

    TEST_INT(case_62_multi, 1);
    TEST_INT(case_63_multi, 1);
    TEST_INT(case_64_multi, 1);
    TEST_INT(case_72_multi, 3);
    TEST_INT(case_107_multi, 2);
    TEST_INT(case_123_multi, 3);
    TEST_INT(case_124_multi, 3);
    TEST_INT(case_recursive_boxes_1, 2);
    TEST_INT(case_recursive_boxes_10, 2);
    TEST_INT(case_recursive_boxes_18, 0);
    TEST_INT(case_recursive_boxes_19, 0);
    TEST_INT(case_recursive_boxes_20, 2);
    TEST_INT(case_recursive_boxes_21, 1);
    TEST_INT(case_recursive_boxes_22, 0);
    TEST_INT(case_recursive_boxes_23, 1);

    TEST_UNION(case_62_multi, 2);
    TEST_UNION(case_63_multi, 2);
    TEST_UNION(case_64_multi, 1);
    TEST_UNION(case_72_multi, 0);
    TEST_UNION(case_107_multi, 1);
    TEST_UNION(case_123_multi, 1);
    TEST_UNION(case_124_multi, 1);
    TEST_UNION(case_recursive_boxes_1, 1);
    TEST_UNION(case_recursive_boxes_10, 1);
    TEST_UNION(case_recursive_boxes_18, 3);
    TEST_UNION(case_recursive_boxes_19, 3);
    TEST_UNION(case_recursive_boxes_20, 2);
    TEST_UNION(case_recursive_boxes_21, 1);
    TEST_UNION(case_recursive_boxes_22, 1);
    TEST_UNION(case_recursive_boxes_23, 3);
}

int test_main(int, char* [])
{
    test_all<double>();
    return 0;
 }


#ifndef HOPS_DATA_HPP
#define HOPS_DATA_HPP

#include <hops/FileWriter/FileWriter.hpp>
#include <hops/FileWriter/FileWriterFactory.hpp>
#include <hops/FileWriter/FileWriterType.hpp>
#include <hops/MarkovChain/MarkovChain.hpp>
#include <hops/Statistics/ExpectedSquaredJumpDistance.hpp>
#include <hops/Statistics/EffectiveSampleSize.hpp>
#include <hops/Statistics/PotentialScaleReductionFactor.hpp>
#include <hops/Utility/ChainData.hpp>

#include <Eigen/Core>

#include <vector>
#include <memory>

namespace hops {
    class Data {
    public:
        Data(long dimension = 0) : dimension(dimension) {
            //
        }

        Data(const std::vector<std::shared_ptr<MarkovChain>>& markovChains, long dimension = 0) : dimension(dimension) {
            linkWithChains(markovChains);
        }

        void setDimension(long dimension) {
            this->dimension = dimension;
        }

        void linkWithChains(const std::vector<std::shared_ptr<MarkovChain>>& markovChains) {
            chains.resize(markovChains.size());
            for (size_t i = 0; i < markovChains.size(); ++i) {
                markovChains[i]->installDataObject(chains[i]);
            }
        }

        std::vector<const std::vector<double>*> getAcceptanceRates() {
            std::vector<const std::vector<double>*> acceptanceRates(chains.size());
            for (size_t i = 0; i < acceptanceRates.size(); ++i) {
                acceptanceRates[i] = chains[i].acceptanceRates.get();
            }
            return acceptanceRates;
        }

        std::vector<const std::vector<double>*> getNegativeLogLikelihood() {
            std::vector<const std::vector<double>*> negativeLogLikelihood(chains.size());
            for (size_t i = 0; i < negativeLogLikelihood.size(); ++i) {
                negativeLogLikelihood[i] = chains[i].negativeLogLikelihood.get();
            }
            return negativeLogLikelihood;
        }

        std::vector<const std::vector<Eigen::VectorXd>*> getStates() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
            }
            return states;
        }

        std::vector<const std::vector<long>*> getTimestamps() {
            std::vector<const std::vector<long>*> timestamps(chains.size());
            for (size_t i = 0; i < timestamps.size(); ++i) {
                timestamps[i] = chains[i].timestamps.get();
            }
            return timestamps;
        }

        void computeTotalNumberOfSamples() {
            totalNumberOfSamples = 0;
            for (size_t i = 0; i < chains.size(); ++i) {
                totalNumberOfSamples += chains[i].getStates().size();
            }
        }

        void computeAcceptanceRate() {
            acceptanceRate = Eigen::VectorXd(chains.size());
            for (size_t i = 0; i < chains.size(); ++i) {
                acceptanceRate(i) = chains[i].getAcceptanceRates().back();
            }
        }

        void computeEffectiveSampleSize() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> effectiveSampleSize = ::hops::computeEffectiveSampleSize(states);
            this->effectiveSampleSize = Eigen::Map<Eigen::VectorXd>(effectiveSampleSize.data(), dimension);
        }

        void computeExpectedSquaredJumpDistance() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> expectedSquaredJumpDistance = ::hops::computeExpectedSquaredJumpDistance<Eigen::VectorXd, Eigen::MatrixXd>(states);
            this->expectedSquaredJumpDistance = Eigen::Map<Eigen::VectorXd>(expectedSquaredJumpDistance.data(), chains.size());
        }

        void computePotentialScaleReductionFactor() {
            std::vector<const std::vector<Eigen::VectorXd>*> states(chains.size());
			if (!chains.size()) {
				throw EmptyChainDataException();
			}

            for (size_t i = 0; i < states.size(); ++i) {
                states[i] = chains[i].states.get();
				if (!states[i]) {
					throw EmptyChainDataException();
				}
            }
            std::vector<double> potentialScaleReductionFactor = ::hops::computePotentialScaleReductionFactor(states);
            this->potentialScaleReductionFactor = Eigen::Map<Eigen::VectorXd>(potentialScaleReductionFactor.data(), dimension);
        }

        void computeTotalTimeTaken() {
            totalTimeTaken = Eigen::VectorXd(chains.size());
            for (size_t i = 0; i < chains.size(); ++i) {
                totalTimeTaken(i) = chains[i].getTimestamps().back() - chains[i].getTimestamps().front();
            }
        }

        void reset() {
            for (size_t i = 0; i < chains.size(); ++i) {
                chains[i].reset();
            }
        }

        void write(const std::string& outputDirectory, bool discardRawData = false) const {
            if (!discardRawData) {
                for (size_t i = 0; i < chains.size(); ++i) {
                    auto fileWriter = FileWriterFactory::createFileWriter(outputDirectory + "/chain" + std::to_string(i), FileWriterType::CSV);
                    chains[i].write(fileWriter.get());
                }
            }

			auto statisticsWriter = FileWriterFactory::createFileWriter(outputDirectory + "/statistics", FileWriterType::CSV);

            if (acceptanceRate.size() > 0) {
                statisticsWriter->write("acceptanceRate", Eigen::MatrixXd(acceptanceRate.transpose()));
            }

            if (expectedSquaredJumpDistance.size() > 0) {
                statisticsWriter->write("expectedSquaredJumpDistance", Eigen::MatrixXd(expectedSquaredJumpDistance.transpose()));
            }

            if (effectiveSampleSize.size() > 0) {
                statisticsWriter->write("effectiveSampleSize", Eigen::MatrixXd(effectiveSampleSize.transpose()));
            }

            if (potentialScaleReductionFactor.size() > 0) {
                statisticsWriter->write("potentialScaleReductionFactor", Eigen::MatrixXd(potentialScaleReductionFactor.transpose()));
            }

            if (totalNumberOfSamples > 0) {
                statisticsWriter->write("totalNumberOfSamples", Eigen::MatrixXd(totalNumberOfSamples * Eigen::MatrixXd::Identity(1,1)));
            }

            if (totalTimeTaken.size() > 0) {
                statisticsWriter->write("totalTimeTaken", Eigen::MatrixXd(totalTimeTaken.transpose()));
            }

            if (totalNumberOfTuningSamples > 0) {
                auto tuningWriter = FileWriterFactory::createFileWriter(outputDirectory + "/tuning", FileWriterType::CSV);
                tuningWriter->write("totalNumberOfTuningSamples", std::vector<long>{static_cast<long>(totalNumberOfTuningSamples)});
                tuningWriter->write("stepSize", std::vector<double>{tunedStepSize});
                tuningWriter->write("objectiveValue", std::vector<double>{tunedObjectiveValue});
                tuningWriter->write("totalTimeTaken", std::vector<double>{totalTuningTimeTaken});

                if (tuningData.size() > 0) {
                    tuningWriter->write("data", tuningData);
                }

                if (tuningPosterior.size() > 0) {
                    tuningWriter->write("posterior", tuningPosterior);
                }
            }
        }

        void setTuningMethod(const std::string& tuningMethod) {
            this->tuningMethod = tuningMethod;
        }

        void setTotalNumberOfTuningSamples(unsigned long totalNumberOfTuningSamples) {
            this->totalNumberOfTuningSamples = totalNumberOfTuningSamples;
        }

        void setTunedStepSize(double tunedStepSize) {
            this->tunedStepSize = tunedStepSize;
        }

        void setTunedObjectiveValue(double tunedObjectiveValue) {
            this->tunedObjectiveValue = tunedObjectiveValue;
        }

        void setTotalTuningTimeTaken(double totalTuningTimeTaken) {
            this->totalTuningTimeTaken = totalTuningTimeTaken;
        }

        void setTuningData(const Eigen::MatrixXd& tuningData) {
            this->tuningData = tuningData;
        }

        void setTuningPosterior(const Eigen::MatrixXd& tuningPosterior) {
            this->tuningPosterior = tuningPosterior;
        }

    private:
        std::vector<ChainData> chains;

        double totalNumberOfSamples;
        Eigen::VectorXd acceptanceRate;
        Eigen::VectorXd expectedSquaredJumpDistance;
        Eigen::VectorXd effectiveSampleSize;
        Eigen::VectorXd potentialScaleReductionFactor;
        Eigen::VectorXd totalTimeTaken;

        // tuning data
        std::string tuningMethod;
        unsigned long totalNumberOfTuningSamples = 0;
        double tunedStepSize;
        double tunedObjectiveValue;
        double totalTuningTimeTaken;

        Eigen::MatrixXd tuningData;
        Eigen::MatrixXd tuningPosterior;

        std::vector<std::vector<double>> sampleVariances;
        std::vector<std::vector<double>> intraChainExpectations;
        std::vector<double> interChainExpectation;
        unsigned long numSeen = 0;

        long dimension = 0;
        friend Eigen::VectorXd computeAcceptanceRate(Data& data);
        friend Eigen::VectorXd computeExpectedSquaredJumpDistance(Data& data);
        friend Eigen::VectorXd computeEffectiveSampleSize(Data& data);
        friend Eigen::VectorXd computePotentialScaleReductionFactor(Data& data);
        friend double computeTotalNumberOfSamples(Data& data);
        friend Eigen::VectorXd computeTotalTimeTaken(Data& data);
    };

    inline Eigen::VectorXd computeAcceptanceRate(Data& data) {
        data.computeAcceptanceRate();
        return data.acceptanceRate;
    }

    inline Eigen::VectorXd computeExpectedSquaredJumpDistance(Data& data) {
        data.computeExpectedSquaredJumpDistance();
        return data.expectedSquaredJumpDistance;
    }

    inline Eigen::VectorXd computeEffectiveSampleSize(Data& data) {
        data.computeEffectiveSampleSize();
        return data.effectiveSampleSize;
    }

    inline Eigen::VectorXd computePotentialScaleReductionFactor(Data& data) {
        data.computePotentialScaleReductionFactor();
        return data.potentialScaleReductionFactor;
    }

    inline double computeTotalNumberOfSamples(Data& data) {
        data.computeTotalNumberOfSamples();
        return data.totalNumberOfSamples;
    }

    inline Eigen::VectorXd computeTotalTimeTaken(Data& data) {
        data.computeTotalTimeTaken();
        return data.totalTimeTaken;
    }
}

#endif // HOPS_DATA_HPP