#include <iostream>
#include <assert.h>

//pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// pcl
#include <pcl/common/transforms.h>
#include <pcl/point_types.h>

//octomap
#include <octomap/octomap.h>
#include <octomap/ColorOcTree.h>
#include "opencv2/opencv.hpp"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>

// boost.format 字符串处理
#include <boost/format.hpp>

// 使用Eigen的Geometry模块处理3d运动
#include <Eigen/Core>
#include <Eigen/Geometry>
using namespace std;
using namespace Eigen;
using namespace cv;

float camera_scale  = 1000;
float cx     = 620.5;
float cy     = 367.5;
float fx     = 481.0;
float fy     = 479.0;

Vec3b get_color(cv::Mat image,Eigen::Vector2d pixel){

return image.at<Vec3b>(Point(int(pixel[0]),int(pixel[1])));

// for better color, maybe interpolation?

}

int main( int argc, char** argv )
{
    if (argc != 4)
    {
        cout<<"Usage: map_pcd_left_img <file.pcd> <file.png>  xyzq.txt"<<endl;
        return -1;
    }

    string pcd_path = argv[1], img_path = argv[2],transform_path = argv[3];
    pcl::PointCloud<pcl::PointXYZI> cloud;
    pcl::io::loadPCDFile<pcl::PointXYZI> ( pcd_path, cloud );

    cout<<"point cloud loaded, piont size = "<<cloud.points.size()<<endl;

    cv::Mat img = cv::imread(img_path);
    cv::Mat img_fuse = img.clone();
   // cv::imshow("",img);
   // cv::waitKey(0);

//  create the transformation -0.02 -0.17 0.1 0 0 0 1
    ifstream fin;
    fin.open( transform_path );
   // Eigen::Quaterniond q;
   // Eigen::Isometry3d t;
    vector< Eigen::Isometry3d > poses;
   // while( fin.peek() != EOF )
    {
        int index_keyframe;
        float data[7]; // 三个位置加一个姿态四元数x,y,z, w,ux,uy,uz
        fin>>index_keyframe;
        for ( int i=0; i<7; i++ )
        {
            fin>>data[i];
            cout<<data[i]<<" ";
        }
        cout<<endl;
        //if (fin.fail()) break;
        // 注意这里的顺序。g2o文件四元数按 qx, qy, qz, qw来存
        // 但Eigen初始化按照qw, qx, qy, qz来做
        Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
        Eigen::Isometry3d t(q);
        t(0,3) = data[0]; t(1,3) = data[1]; t(2,3) = data[2];
        poses.push_back(t);
	poses[0] = poses[0];

    }

//transform the point cloud so as to it is in the left cam frame

cout<<poses[0].matrix() <<endl;

    pcl::PointCloud<pcl::PointXYZI>::Ptr trans_cloud_ptr( new     pcl::PointCloud<pcl::PointXYZI>() );
    pcl::transformPointCloud( cloud, *trans_cloud_ptr, poses[0].matrix());

    //声明octomap变量
    cout<<"copy data into octomap..."<<endl;
    // 创建带颜色的八叉树对象，参数为分辨率，这里设成了0.05
    octomap::ColorOcTree tree( 0.005 );

    for (auto p:trans_cloud_ptr->points)
    {

// construct octomap when there are visual content.

		//
		   double x = fx*p.x/p.z+cx;
		   double y = fy*p.y/p.z+cy;
		   Eigen::Vector2d pixel(x,y); //1280 720

			if(pixel[0]>0 && pixel[1]>0 && pixel[0]<1280 && pixel[1]<720){

		   	cout<<"pcd: "<<p.x<<" "<<p.y<<" "<<p.z<<" pixel: "<<pixel[0]<<" "<<pixel[1]<<endl;
			//cout<<p.intensity<<endl;

			img_fuse.at<Vec3b>(Point(int(pixel[0]),int(pixel[1]))) = 250,0,0;//p.intensity,p.intensity,p.intensity;

			// 将点云里的点插入到octomap中
			tree.updateNode( octomap::point3d(p.x, p.y, p.z), true );
			Vec3b color = get_color(img,pixel);
			tree.integrateNodeColor( p.x, p.y, p.z, color[0],color[1],color[2] );

			}

    }

    cv::imshow("",img_fuse);
    cv::waitKey();
    cv::imwrite("fused_img.png",img_fuse);

    // 更新octomap
    tree.updateInnerOccupancy();
    // 存储octomap, 注意要存成.ot文件而非.bt文件
    tree.write( "color_coded.ot" );
    cout<<"done."<<endl;

    return 0;
}


#define BOOST_TEST_MODULE TestSolvers
#include <boost/test/unit_test.hpp>
#include <amgcl/backend/builtin.hpp>
#include <amgcl/value_type/eigen.hpp>

#include "test_solver.hpp"

BOOST_AUTO_TEST_SUITE( test_solvers )

BOOST_AUTO_TEST_CASE(test_nonscalar_backend)
{
    test_backend< amgcl::backend::builtin< Eigen::Matrix<double, 2, 2> > >();
}

BOOST_AUTO_TEST_SUITE_END()


//
//  paramils_conf.cpp
//  grammar2code
//
//  Created by Franco Mascia on 26/07/13.
//  Copyright (c) 2013 Franco Mascia. All rights reserved.
//
//  This file is distributed under the BSD 2-Clause License. See LICENSE.TXT
//  for details.
//

#include "grammar.hpp"
#include "error.hpp"

#include <boost/algorithm/string/join.hpp>
#include <boost/algorithm/string/erase.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <boost/algorithm/string/predicate.hpp>

#include <string>
#include <vector>

std::string grammar::paramils_conf::fmt_rule_name(const std::string& path)
{
    auto param_name = rule_name(path);
    return param_name.second;
}

std::string grammar::paramils_conf::fmt_rule_cond(const std::string& path, const std::string& node_name, int rec_index)
{
    auto param_name = rule_name(path);
    auto cond = rule_cond(path, node_name, rec_index);
    auto cond_name = rule_name(cond.first);

    if (cond.first.empty() || cond.second.empty()) {
        return "";
    } else {
        return param_name.second + " | " + cond_name.second + " in {" + cond.second + "}";
    }
}

void grammar::paramils_conf::fmt_parameter(const std::string& rule_name, const std::string& rule_type, const std::vector<std::string>& values, std::string default_value, bool log_scale, std::string rule_cond)
{
    std::string alternatives = boost::join(values, ", ");
    parameters_.push_back(rule_name + " {" + alternatives + "}[" + default_value + "]");
    conditionals_.push_back(rule_cond);
}

void grammar::paramils_conf::callback_range(const pugi::xml_node& node, std::string path, int depth)
{
    std::string rule = fmt_rule_name(path);
    std::string cond = fmt_rule_cond(path, node.name());
    std::vector<std::string> choices;
    std::string type = node.attribute("type").value();
    // default value
    std::string default_value;
    if (strcmp(node.attribute("default").value(), "")) {
        default_value = node.attribute("default").value();
    } else {
        default_value = node.attribute("min").value();
    }
    // log scale sampling
    std::string attribute = node.attribute("log-scale").value();
    bool log_scale = false;
    if (boost::iequals(attribute, "true") || boost::iequals(attribute, "yes")) {
        log_scale = true;
    }
    if (type == "int") {
        for (int i = std::stoi(node.attribute("min").value());
             i <= std::stoi(node.attribute("max").value());
             i += std::stoi(node.attribute("stepIfEnumerated").value())) {
            choices.push_back(std::to_string(i));
        }
        fmt_parameter(rule, type, choices, default_value, log_scale, cond);
    }
    if (type == "real") {
        for (float i = std::stof(node.attribute("min").value());
             i <= std::stof(node.attribute("max").value());
             i += std::stof(node.attribute("stepIfEnumerated").value())) {
            choices.push_back(std::to_string(i));
        }
        fmt_parameter(rule, type, choices, default_value, log_scale, cond);
    }
}

void grammar::paramils_conf::print(std::ostream& stream)
{
    conditionals_.clear();

    grammar::configuration::print(stream);

    stream << "\nConditionals:" << std::endl;
    for (auto& conditional : conditionals_) {
        if (!conditional.empty()) {
            stream << conditional << std::endl;
        }
    }
}


//---------------------------------------------------------------------------//
// Copyright (c) 2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2019 Alexey Moskvin
// Copyright (c) 2020 Ilias Khairullin <ilias@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef BOOST_MULTIPRECISION_MODULAR_ADAPTOR_HPP
#define BOOST_MULTIPRECISION_MODULAR_ADAPTOR_HPP

#include <boost/cstdint.hpp>
#include <boost/functional/hash_fwd.hpp>

#include <nil/crypto3/multiprecision/detail/digits.hpp>
#include <nil/crypto3/multiprecision/number.hpp>

#include <nil/crypto3/multiprecision/modular/modular_params.hpp>
#include <nil/crypto3/multiprecision/modular/modular_adaptor_fixed.hpp>

#include <boost/container/small_vector.hpp>

#include <algorithm>
#include <cmath>
#include <vector>

namespace nil {
    namespace crypto3 {
        namespace multiprecision {
            namespace backends {

                template<typename Backend>
                struct modular_adaptor {
                    typedef modular_params<Backend> modulus_type;
                    typedef Backend backend_type;

                protected:
                    Backend m_base;
                    modulus_type m_mod;

                public:
                    inline Backend& base_data() {
                        return m_base;
                    }
                    inline const Backend& base_data() const {
                        return m_base;
                    }
                    inline modulus_type& mod_data() {
                        return m_mod;
                    }
                    inline const modulus_type& mod_data() const {
                        return m_mod;
                    }

                    typedef typename Backend::signed_types signed_types;
                    typedef typename Backend::unsigned_types unsigned_types;

                    modular_adaptor() {
                    }

                    modular_adaptor(const modular_adaptor& o) : m_base(o.base_data()), m_mod(o.mod_data()) {
                    }

#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES

                    modular_adaptor(modular_adaptor&& o) :
                        m_base(std::move(o.base_data())), m_mod(std::move(o.mod_data())) {
                    }

#endif
                    // TODO: implement create_internal_representation
                    // modular_adaptor(const Backend& val, const modular_params<Backend>& mod) : m_mod(mod),
                    // m_base(mod.create_internal_representation(val))
                    // {
                    //
                    // }

                    modular_adaptor(const Backend& val, const Backend& mod) : m_base(val), m_mod(mod) {
                    }

                    modular_adaptor(Backend& val, Backend& mod) : m_base(val), m_mod(mod) {
                    }

                    // TODO: maybe initialize modulus rather than base
                    modular_adaptor(const Backend& val) :
                        m_base(val), m_mod(typename boost::mpl::front<unsigned_types>::type(0u)) {
                    }

                    modular_adaptor(const modular_params<Backend>& mod) :
                        m_base(typename boost::mpl::front<unsigned_types>::type(0u)), m_mod(mod) {
                    }

                    modular_adaptor& operator=(const modular_adaptor& o) {
                        m_base = o.base_data();
                        m_mod = o.mod_data();
                        return *this;
                    }

#ifndef BOOST_NO_CXX11_RVALUE_REFERENCES

                    modular_adaptor& operator=(modular_adaptor&& o)

                        BOOST_NOEXCEPT {
                        m_base = std::move(o.base_data());
                        m_mod = std::move(o.mod_data());
                        return *this;
                    }
#endif

                    modular_adaptor& operator=(const char* s) {
                        typedef typename boost::mpl::front<unsigned_types>::type ui_type;
                        ui_type zero = 0u;

                        using default_ops::eval_fpclassify;

                        if (s && (*s == '(')) {
                            std::string part;
                            const char* p = ++s;
                            while (*p && (*p != ',') && (*p != ')'))
                                ++p;
                            part.assign(s, p);
                            if (!part.empty())
                                m_base() = part.c_str();
                            else
                                m_base() = zero;
                            s = p;
                            if (*p && (*p != ')')) {
                                ++p;
                                while (*p && (*p != ')'))
                                    ++p;
                                part.assign(s + 1, p);
                            } else
                                part.erase();
                            if (!part.empty())
                                m_mod() = part.c_str();
                            else
                                m_mod() = zero;
                        } else {
                            base_data() = s;
                            m_mod() = zero;
                        }
                        return *this;
                    }

                    // TODO: maybe change behaviour to check a congruence relation
                    int compare(const modular_adaptor& o) const {
                        // They are either equal or not:<
                        if (m_mod.compare(o.mod_data()) != 0) {
                            BOOST_THROW_EXCEPTION(
                                std::runtime_error("Could not compare modular number with different mod."));
                        }
                        Backend tmp1, tmp2;
                        mod_data().adjust_regular(tmp1, base_data());
                        mod_data().adjust_regular(tmp2, o.base_data());
                        return tmp1.compare(tmp2);
                    }

                    // TODO: maybe change behaviour to check a congruence relation
                    template<class T>
                    int compare(const T& val) const {
                        using default_ops::eval_lt;
                        if (!eval_lt(m_mod, val)) {
                            BOOST_THROW_EXCEPTION(std::runtime_error(
                                "Could not compare modular number with mod bigger than compared number."));
                        }
                        Backend tmp;
                        mod_data().adjust_regular(tmp, base_data());
                        return tmp.compare(val);
                    }

                    inline void swap(modular_adaptor& o) {
                        base_data().swap(o.base_data());
                        std::swap(mod_data(), o.mod_data());
                    }

                    inline std::string str(std::streamsize dig, std::ios_base::fmtflags f) const {
                        Backend tmp;
                        mod_data().adjust_regular(tmp, base_data());
                        return tmp.str(dig, f);
                    }

                    inline void negate() {
                        base_data().negate();
                        eval_add(base_data(), mod_data().get_mod().backend());
                    }

                    template<typename BackendT, expression_template_option ExpressionTemplates>
                    operator number<BackendT, ExpressionTemplates>() {
                        return base_data();
                    };
                };

                template<class Result, class Backend>
                constexpr void eval_convert_to(Result* result, const modular_adaptor<Backend>& val) {
                    using default_ops::eval_convert_to;
                    eval_convert_to(result, val.base_data());
                }

                template<class Backend, class T>
                constexpr typename boost::enable_if<boost::is_arithmetic<T>, bool>::type eval_eq(const modular_adaptor<Backend>& a,
                                                                                const T& b) {
                    return a.compare(b) == 0;
                }

                template<class Backend1, class Backend2>
                constexpr void eval_redc(Backend1& result, const modular_params<Backend2>& mod) {
                    mod.reduce(result);
                    eval_modulus(result, mod.get_mod().backend());
                }

                template<class Backend>
                constexpr void eval_add(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& o) {
                    BOOST_ASSERT(result.mod_data().get_mod() == o.mod_data().get_mod());
                    using default_ops::eval_lt;

                    eval_add(result.base_data(), o.base_data());
                    if (!eval_lt(result.base_data(), result.mod_data().get_mod())) {
                        eval_subtract(result.base_data(), result.mod_data().get_mod().backend());
                    }
                }

                template<class Backend>
                constexpr void eval_subtract(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& o) {
                    BOOST_ASSERT(result.mod_data().get_mod() == o.mod_data().get_mod());
                    typedef typename boost::mpl::front<typename Backend::unsigned_types>::type ui_type;
                    using default_ops::eval_lt;
                    eval_subtract(result.base_data(), o.base_data());
                    if (eval_lt(result.base_data(), ui_type(0u))) {
                        eval_add(result.base_data(), result.mod_data().get_mod().backend());
                    }
                }

                template<class Backend>
                constexpr void eval_multiply(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& o) {
                    BOOST_ASSERT(result.mod_data().get_mod() == o.mod_data().get_mod());
                    eval_multiply(result.base_data(), o.base_data());
                    eval_redc(result.base_data(), result.mod_data());
                }

                template<class Backend>
                constexpr void eval_divide(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& o) {
                    BOOST_ASSERT(result.mod_data().get_mod() == o.mod_data().get_mod());
                    Backend tmp1, tmp2;
                    result.mod_data().adjust_regular(tmp1, result.base_data());
                    result.mod_data().adjust_regular(tmp2, o.base_data());
                    eval_divide(tmp1, tmp2);
                    result.base_data() = tmp1;
                    result.mod_data().adjust_modular(result.base_data());
                    result.mod_data().adjust_regular(tmp2, result.base_data());
                }

                template<class Backend>
                constexpr void eval_modulus(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& o) {
                    BOOST_ASSERT(result.mod_data().get_mod() == o.mod_data().get_mod());
                    Backend tmp1, tmp2;
                    result.mod_data().adjust_regular(tmp1, result.base_data());
                    result.mod_data().adjust_regular(tmp2, o.base_data());
                    eval_modulus(tmp1, tmp2);
                    result.base_data() = tmp1;
                    result.mod_data().adjust_modular(result.base_data());
                    result.mod_data().adjust_regular(tmp2, result.base_data());
                }

                template<class Backend>
                constexpr bool eval_is_zero(const modular_adaptor<Backend>& val)

                    BOOST_NOEXCEPT {
                    using default_ops::eval_is_zero;
                    return eval_is_zero(val.base_data());
                }

                // TODO: check returned value
                template<class Backend>
                constexpr int eval_get_sign(const modular_adaptor<Backend>&) {
                    return 1;
                }

                // TODO: is the function required
                // template <class Result, class Backend>
                // constexpr typename boost::disable_if_c<boost::is_complex<Result>::value>::type
                // eval_convert_to(Result* result, const modular_adaptor<Backend>& val)
                // {
                //    using default_ops::eval_convert_to;
                //    eval_convert_to(result, val.base_data());
                // }

                template<class Backend, class T, class V>
                constexpr void assign_components(modular_adaptor<Backend>& result, const T& a, const V& b) {
                    result.base_data() = a;
                    result.mod_data() = b;
                    result.mod_data().adjust_modular(result.base_data());
                }

                template<class Backend>
                constexpr void eval_sqrt(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& val) {
                    eval_sqrt(result.base_data(), val.base_data());
                }

                template<class Backend>
                constexpr void eval_abs(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& val) {
                    result = val;
                }

                size_t window_bits(size_t exp_bits) {
                    BOOST_STATIC_CONSTEXPR size_t wsize_count = 6;
                    BOOST_STATIC_CONSTEXPR size_t wsize[wsize_count][2] = {{1434, 7}, {539, 6}, {197, 4},
                                                                           {70, 3},   {17, 2},  {0, 0}};

                    size_t window_bits = 1;

                    size_t j = wsize_count - 1;
                    while (wsize[j][0] > exp_bits) {
                        --j;
                    }
                    window_bits += wsize[j][1];

                    return window_bits;
                };

                template<class Backend>
                inline void find_modular_pow(modular_adaptor<Backend>& result,
                                             const modular_adaptor<Backend>& b,
                                             const Backend& exp) {
                    using default_ops::eval_bit_set;
                    using default_ops::eval_convert_to;
                    using default_ops::eval_decrement;
                    using default_ops::eval_multiply;

                    typedef number<modular_adaptor<Backend>> modular_type;
                    modular_params<Backend> mod = b.mod_data();
                    size_t m_window_bits;
                    unsigned long cur_exp_index;
                    size_t exp_bits = eval_msb(exp);
                    m_window_bits = window_bits(exp_bits + 1);

                    std::vector<modular_type> m_g(1U << m_window_bits);
                    modular_type* p_g = m_g.data();
                    modular_type x(1, mod);
                    Backend nibble = exp;
                    Backend mask;
                    eval_bit_set(mask, m_window_bits);
                    eval_decrement(mask);
                    *p_g = x;
                    ++p_g;
                    *p_g = b;
                    ++p_g;
                    for (size_t i = 2; i < (1U << m_window_bits); i++) {
                        eval_multiply((*p_g).backend(), m_g[i - 1].backend(), b);
                        ++p_g;
                    }
                    size_t exp_nibbles = (exp_bits + 1 + m_window_bits - 1) / m_window_bits;
                    std::vector<size_t> exp_index;

                    for (size_t i = 0; i < exp_nibbles; ++i) {
                        Backend tmp = nibble;
                        eval_bitwise_and(tmp, mask);
                        eval_convert_to(&cur_exp_index, tmp);
                        eval_right_shift(nibble, m_window_bits);
                        exp_index.push_back(cur_exp_index);
                    }

                    x = x * m_g[exp_index[exp_nibbles - 1]];
                    for (size_t i = exp_nibbles - 1; i > 0; --i) {

                        for (size_t j = 0; j != m_window_bits; ++j) {
                            x = x * x;
                        }

                        x = x * m_g[exp_index[i - 1]];
                    }
                    result = x.backend();
                }

                template<class Backend, typename T>
                constexpr void eval_pow(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& b,
                                        const T& e) {
                    find_modular_pow(result, b, e);
                }

                template<class Backend>
                constexpr void eval_pow(modular_adaptor<Backend>& result,
                                        const modular_adaptor<Backend>& b,
                                        const modular_adaptor<Backend>& e) {
                    Backend exp;
                    e.mod_data().adjust_regular(exp, e.base_data());
                    find_modular_pow(result, b, exp);
                }

                template<typename Backend1, typename Backend2, typename T>
                constexpr void eval_powm(modular_adaptor<Backend1>& result, const modular_adaptor<Backend2>& b,
                                         const T& e) {
                    eval_pow(result, b, e);
                }

                template<typename Backend1, typename Backend2, typename Backend3>
                constexpr void eval_powm(modular_adaptor<Backend1>& result,
                                         const modular_adaptor<Backend2>& b,
                                         const modular_adaptor<Backend3>& e) {
                    eval_pow(result, b, e);
                }

                template<class Backend, class UI>
                constexpr void eval_left_shift(modular_adaptor<Backend>& t, UI i) {
                    using default_ops::eval_left_shift;
                    Backend tmp;
                    t.mod_data().adjust_regular(tmp, t.base_data());
                    eval_left_shift(tmp, i);
                    t.base_data() = tmp;
                    t.mod_data().adjust_modular(t.base_data());
                }

                template<class Backend, class UI>
                constexpr void eval_right_shift(modular_adaptor<Backend>& t, UI i) {
                    using default_ops::eval_right_shift;
                    Backend tmp;
                    t.mod_data().adjust_regular(tmp, t.base_data());
                    eval_right_shift(tmp, i);
                    t.base_data() = tmp;
                    t.mod_data().adjust_modular(t.base_data());
                }

                template<class Backend, class UI>
                constexpr void eval_left_shift(modular_adaptor<Backend>& t, const modular_adaptor<Backend>& v, UI i) {
                    using default_ops::eval_left_shift;
                    Backend tmp1, tmp2;
                    t.mod_data().adjust_regular(tmp1, t.base_data());
                    t.mod_data().adjust_regular(tmp2, v.base_data());
                    eval_left_shift(tmp1, tmp2, static_cast<unsigned long>(i));
                    t.base_data() = tmp1;
                    t.mod_data().adjust_modular(t.base_data());
                }

                template<class Backend, class UI>
                constexpr void eval_right_shift(modular_adaptor<Backend>& t, const modular_adaptor<Backend>& v, UI i) {
                    using default_ops::eval_right_shift;
                    Backend tmp1, tmp2;
                    t.mod_data().adjust_regular(tmp1, t.base_data());
                    t.mod_data().adjust_regular(tmp2, v.base_data());
                    eval_right_shift(tmp1, tmp2, static_cast<unsigned long>(i));
                    t.base_data() = tmp1;
                    t.mod_data().adjust_modular(t.base_data());
                }

                template<class Backend>
                constexpr void eval_bitwise_and(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& v) {
                    using default_ops::eval_bitwise_and;
                    BOOST_ASSERT(result.mod_data().get_mod() == v.mod_data().get_mod());

                    Backend tmp1, tmp2;
                    result.mod_data().adjust_regular(tmp1, result.base_data());
                    v.mod_data().adjust_regular(tmp2, v.base_data());
                    eval_bitwise_and(tmp1, tmp1, tmp2);
                    result.base_data() = tmp1;
                    result.mod_data().adjust_modular(result.base_data());
                }

                template<class Backend>
                constexpr void eval_bitwise_or(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& v) {
                    using default_ops::eval_bitwise_or;
                    BOOST_ASSERT(result.mod_data().get_mod() == v.mod_data().get_mod());

                    Backend tmp1, tmp2;
                    result.mod_data().adjust_regular(tmp1, result.base_data());
                    v.mod_data().adjust_regular(tmp2, v.base_data());
                    eval_bitwise_or(tmp1, tmp1, tmp2);
                    result.base_data() = tmp1;
                    result.mod_data().adjust_modular(result.base_data());
                }

                template<class Backend>
                constexpr void eval_bitwise_xor(modular_adaptor<Backend>& result, const modular_adaptor<Backend>& v) {
                    using default_ops::eval_bitwise_xor;
                    BOOST_ASSERT(result.mod_data().get_mod() == v.mod_data().get_mod());

                    Backend tmp1, tmp2;
                    result.mod_data().adjust_regular(tmp1, result.base_data());
                    v.mod_data().adjust_regular(tmp2, v.base_data());
                    eval_bitwise_xor(tmp1, tmp1, tmp2);
                    result.base_data() = tmp1;
                    result.mod_data().adjust_modular(result.base_data());
                }

                template<typename Backend>
                constexpr int eval_msb(const modular_adaptor<Backend>& m) {
                    using default_ops::eval_msb;
                    Backend tmp;
                    m.mod_data().adjust_regular(tmp, m.base_data());
                    return eval_msb(tmp);
                }

                template<typename Backend>
                constexpr unsigned eval_lsb(const modular_adaptor<Backend>& m) {
                    using default_ops::eval_lsb;
                    Backend tmp;
                    m.mod_data().adjust_regular(tmp, m.base_data());
                    return eval_lsb(tmp);
                }

                template<typename Backend>
                constexpr bool eval_bit_test(const modular_adaptor<Backend>& m, unsigned index) {
                    using default_ops::eval_bit_test;
                    Backend tmp;
                    m.mod_data().adjust_regular(tmp, m.base_data());
                    return eval_bit_test(tmp, index);
                }

                template<typename Backend>
                constexpr void eval_bit_set(modular_adaptor<Backend>& result, unsigned index) {
                    using default_ops::eval_bit_set;
                    Backend tmp;
                    result.mod_data().adjust_regular(tmp, result.base_data());
                    eval_bit_set(tmp, index);
                    result.mod_data().adjust_modular(result.base_data(), tmp);
                }

                template<typename Backend>
                constexpr void eval_bit_unset(modular_adaptor<Backend>& result, unsigned index) {
                    using default_ops::eval_bit_unset;
                    Backend tmp;
                    result.mod_data().adjust_regular(tmp, result.base_data());
                    eval_bit_unset(tmp, index);
                    result.mod_data().adjust_modular(result.base_data(), tmp);
                }

                template<typename Backend>
                constexpr void eval_bit_flip(modular_adaptor<Backend>& result, unsigned index) {
                    using default_ops::eval_bit_flip;
                    Backend tmp;
                    result.mod_data().adjust_regular(tmp, result.base_data());
                    eval_bit_flip(tmp, index);
                    result.mod_data().adjust_modular(result.base_data(), tmp);
                }

            }    // namespace backends

            using nil::crypto3::multiprecision::backends::modular_adaptor;

            template<class Backend>
            struct number_category<modular_adaptor<Backend>>
                : public boost::mpl::int_<nil::crypto3::multiprecision::number_kind_modular> { };

            template<class Backend, expression_template_option ExpressionTemplates>
            struct component_type<number<modular_adaptor<Backend>, ExpressionTemplates>> {
                typedef number<Backend, ExpressionTemplates> type;
            };

        }    // namespace multiprecision
    }        // namespace crypto3
}    // namespace nil

#endif


//---------------------------------------------------------------------------//
//!
//! \file   MonteCarlo_ElectronMaterial.hpp
//! \author Luke Kersting
//! \brief  Electron material class declaration
//!
//---------------------------------------------------------------------------//

#ifndef MONTE_CARLO_ELECTRON_MATERIAL_HPP
#define MONTE_CARLO_ELECTRON_MATERIAL_HPP

// Boost Includes
#include <boost/unordered_map.hpp>

// Trilinos Includes
#include <Teuchos_Array.hpp>
#include <Teuchos_RCP.hpp>

// FRENSIE Includes
#include "MonteCarlo_ModuleTraits.hpp"
#include "MonteCarlo_Electroatom.hpp"
#include "Utility_Tuple.hpp"

namespace MonteCarlo{

//! The electron material class
class ElectronMaterial
{

private:

  // Typedef for Teuchos ScalarTraits
  typedef Teuchos::ScalarTraits<double> ST;

public:

  //! Typedef for electroatom name map
  typedef boost::unordered_map<std::string,Teuchos::RCP<Electroatom> >
  ElectroatomNameMap;

  //! Constructor
  ElectronMaterial( const ModuleTraits::InternalMaterialHandle id,
                    const double density,
                    const ElectroatomNameMap& electroatom_name_map,
                    const Teuchos::Array<double>& electroatom_fractions,
                    const Teuchos::Array<std::string>& electroatom_names );

  //! Destructor
  ~ElectronMaterial()
  { /* ... */ }

  //! Return the material id
  ModuleTraits::InternalMaterialHandle getId() const;

  //! Return the number density (atom/b-cm)
  double getNumberDensity() const;

  //! Return the macroscopic total cross section (1/cm)
  double getMacroscopicTotalCrossSection( const double energy ) const;

  //! Return the macroscopic absorption cross section (1/cm)
  double getMacroscopicAbsorptionCrossSection( const double energy ) const;

  //! Return the survival probability
  double getSurvivalProbability( const double energy ) const;

  //! Return the macroscopic cross section (1/cm) for a specific reaction
  double getMacroscopicReactionCrossSection(
				const double energy,
				const ElectroatomicReactionType reaction ) const;

  //! Collide with a electron
  void collideAnalogue( ElectronState& electron, ParticleBank& bank ) const;

  //! Collide with a electron and survival bias
  void collideSurvivalBias( ElectronState& electron, ParticleBank& bank ) const;

private:

  // Get the atomic weight from an atom pointer
  static double getAtomicWeight(
	    const Utility::Pair<double,Teuchos::RCP<const Electroatom> >& pair );

  // Sample the atom that is collided with
  unsigned sampleCollisionAtom( const double energy ) const;

  // The material id
  ModuleTraits::InternalMaterialHandle d_id;

  // The number density of the atoms of the material
  double d_number_density;

  // The atoms that make up the material
  Teuchos::Array<Utility::Pair<double,Teuchos::RCP<const Electroatom> > >
  d_atoms;
};

} // end MonteCarlo namespace

#endif // end MONTE_CARLO_ELECTRON_MATERIAL_HPP

//---------------------------------------------------------------------------//
// end MonteCarlo_ElectronMaterial.hpp
//---------------------------------------------------------------------------//


#include "itkTractsToVectorImageFilter.h"

// VTK
#include <vtkPolyLine.h>
#include <vtkCellArray.h>
#include <vtkCellData.h>

// ITK
#include <itkTimeProbe.h>
#include <itkImageRegionIterator.h>

// misc
#define _USE_MATH_DEFINES
#include <math.h>
#include <boost/progress.hpp>

namespace itk{

static bool CompareVectorLengths(const vnl_vector_fixed< double, 3 >& v1, const vnl_vector_fixed< double, 3 >& v2)
{
    return (v1.magnitude()>v2.magnitude());
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::TractsToVectorImageFilter():
    m_AngularThreshold(0.7),
    m_Epsilon(0.999),
    m_MaskImage(NULL),
    m_NormalizeVectors(false),
    m_UseWorkingCopy(true),
    m_UseTrilinearInterpolation(false),
    m_MaxNumDirections(3),
    m_Thres(0.5),
    m_NumDirectionsImage(NULL)
{
    this->SetNumberOfRequiredOutputs(1);
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::~TractsToVectorImageFilter()
{
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToVectorImageFilter< PixelType >::GetVnlVector(double point[])
{
    vnl_vector_fixed<double, 3> vnlVector;
    vnlVector[0] = point[0];
    vnlVector[1] = point[1];
    vnlVector[2] = point[2];
    return vnlVector;
}

template< class PixelType >
itk::Point<double, 3> TractsToVectorImageFilter< PixelType >::GetItkPoint(double point[])
{
    itk::Point<double, 3> itkPoint;
    itkPoint[0] = point[0];
    itkPoint[1] = point[1];
    itkPoint[2] = point[2];
    return itkPoint;
}

template< class PixelType >
void TractsToVectorImageFilter< PixelType >::GenerateData()
{
  mitk::BaseGeometry::Pointer geometry = m_FiberBundle->GetGeometry();

    // calculate new image parameters
    itk::Vector<double> spacing;
    itk::Point<double> origin;
    itk::Matrix<double, 3, 3> direction;
    ImageRegion<3> imageRegion;
    if (!m_MaskImage.IsNull())
    {
        spacing = m_MaskImage->GetSpacing();
        imageRegion = m_MaskImage->GetLargestPossibleRegion();
        origin = m_MaskImage->GetOrigin();
        direction = m_MaskImage->GetDirection();
    }
    else
    {
        spacing = geometry->GetSpacing();
        origin = geometry->GetOrigin();
        mitk::BaseGeometry::BoundsArrayType bounds = geometry->GetBounds();
        origin[0] += bounds.GetElement(0);
        origin[1] += bounds.GetElement(2);
        origin[2] += bounds.GetElement(4);

        for (int i=0; i<3; i++)
            for (int j=0; j<3; j++)
                direction[j][i] = geometry->GetMatrixColumn(i)[j];
        imageRegion.SetSize(0, geometry->GetExtent(0));
        imageRegion.SetSize(1, geometry->GetExtent(1));
        imageRegion.SetSize(2, geometry->GetExtent(2));

        m_MaskImage = ItkUcharImgType::New();
        m_MaskImage->SetSpacing( spacing );
        m_MaskImage->SetOrigin( origin );
        m_MaskImage->SetDirection( direction );
        m_MaskImage->SetRegions( imageRegion );
        m_MaskImage->Allocate();
        m_MaskImage->FillBuffer(1);
    }
    OutputImageType::RegionType::SizeType outImageSize = imageRegion.GetSize();
    m_OutImageSpacing = m_MaskImage->GetSpacing();
    m_ClusteredDirectionsContainer = ContainerType::New();

    // initialize crossings image
    m_CrossingsImage = ItkUcharImgType::New();
    m_CrossingsImage->SetSpacing( spacing );
    m_CrossingsImage->SetOrigin( origin );
    m_CrossingsImage->SetDirection( direction );
    m_CrossingsImage->SetRegions( imageRegion );
    m_CrossingsImage->Allocate();
    m_CrossingsImage->FillBuffer(0);

    // initialize num directions image
    m_NumDirectionsImage = ItkUcharImgType::New();
    m_NumDirectionsImage->SetSpacing( spacing );
    m_NumDirectionsImage->SetOrigin( origin );
    m_NumDirectionsImage->SetDirection( direction );
    m_NumDirectionsImage->SetRegions( imageRegion );
    m_NumDirectionsImage->Allocate();
    m_NumDirectionsImage->FillBuffer(0);

    // resample fiber bundle
    double minSpacing = 1;
    if(m_OutImageSpacing[0]<m_OutImageSpacing[1] && m_OutImageSpacing[0]<m_OutImageSpacing[2])
        minSpacing = m_OutImageSpacing[0];
    else if (m_OutImageSpacing[1] < m_OutImageSpacing[2])
        minSpacing = m_OutImageSpacing[1];
    else
        minSpacing = m_OutImageSpacing[2];

    if (m_UseWorkingCopy)
        m_FiberBundle = m_FiberBundle->GetDeepCopy();

    // resample fiber bundle for sufficient voxel coverage
    m_FiberBundle->ResampleFibers(minSpacing/3);

    // iterate over all fibers
    vtkSmartPointer<vtkPolyData> fiberPolyData = m_FiberBundle->GetFiberPolyData();
    vtkSmartPointer<vtkCellArray> vLines = fiberPolyData->GetLines();
    vLines->InitTraversal();
    int numFibers = m_FiberBundle->GetNumFibers();
    itk::TimeProbe clock;
    m_DirectionsContainer = ContainerType::New();

    if (m_UseTrilinearInterpolation)
        MITK_INFO << "Generating directions from tractogram (trilinear interpolation)";
    else
        MITK_INFO << "Generating directions from tractogram";

    boost::progress_display disp(numFibers);
    for( int i=0; i<numFibers; i++ )
    {
        ++disp;
        clock.Start();
        vtkIdType   numPoints(0);
        vtkIdType*  points(NULL);
        vLines->GetNextCell ( numPoints, points );
        if (numPoints<2)
            continue;

        itk::Index<3> index; index.Fill(0);
        itk::ContinuousIndex<double, 3> contIndex;
        vnl_vector_fixed<double, 3> dir, wDir;
        itk::Point<double, 3> worldPos;
        vnl_vector<double> v;
        for( int j=0; j<numPoints-1; j++)
        {
            double* temp = fiberPolyData->GetPoint(points[j]);
            worldPos = GetItkPoint(temp);
            v = GetVnlVector(temp);

            dir = GetVnlVector(fiberPolyData->GetPoint(points[j+1]))-v;
            dir.normalize();

            m_MaskImage->TransformPhysicalPointToIndex(worldPos, index);
            m_MaskImage->TransformPhysicalPointToContinuousIndex(worldPos, contIndex);

            if (m_MaskImage->GetPixel(index)==0)
                continue;

            if (!m_UseTrilinearInterpolation)
            {
                if (index[0] < 0 || (unsigned long)index[0] >= outImageSize[0])
                    continue;
                if (index[1] < 0 || (unsigned long)index[1] >= outImageSize[1])
                    continue;
                if (index[2] < 0 || (unsigned long)index[2] >= outImageSize[2])
                    continue;

                unsigned int idx = index[0] + outImageSize[0]*(index[1] + outImageSize[1]*index[2]);
                DirectionContainerType::Pointer dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, dir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), dir);
                }
                else
                {
                    dirCont->InsertElement(0, dir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }

                continue;
            }

            double frac_x = contIndex[0] - index[0];
            double frac_y = contIndex[1] - index[1];
            double frac_z = contIndex[2] - index[2];

            if (frac_x<0)
            {
                index[0] -= 1;
                frac_x += 1;
            }
            if (frac_y<0)
            {
                index[1] -= 1;
                frac_y += 1;
            }
            if (frac_z<0)
            {
                index[2] -= 1;
                frac_z += 1;
            }

            frac_x = 1-frac_x;
            frac_y = 1-frac_y;
            frac_z = 1-frac_z;

            // int coordinates inside image?
            if (index[0] < 0 || (unsigned long)index[0] >= outImageSize[0]-1)
                continue;
            if (index[1] < 0 || (unsigned long)index[1] >= outImageSize[1]-1)
                continue;
            if (index[2] < 0 || (unsigned long)index[2] >= outImageSize[2]-1)
                continue;

            DirectionContainerType::Pointer dirCont;
            int idx;
            wDir = dir;
            double weight = (  frac_x)*(  frac_y)*(  frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]   + outImageSize[0]*(index[1]  + outImageSize[1]*index[2]  );
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (  frac_x)*(1-frac_y)*(  frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]   + outImageSize[0]*(index[1]+1+ outImageSize[1]*index[2]  );
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (  frac_x)*(  frac_y)*(1-frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]   + outImageSize[0]*(index[1]  + outImageSize[1]*index[2]+outImageSize[1]);
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (  frac_x)*(1-frac_y)*(1-frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]   + outImageSize[0]*(index[1]+1+ outImageSize[1]*index[2]+outImageSize[1]);
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (1-frac_x)*(  frac_y)*(  frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]+1 + outImageSize[0]*(index[1]  + outImageSize[1]*index[2]  );
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (1-frac_x)*(  frac_y)*(1-frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]+1 + outImageSize[0]*(index[1]  + outImageSize[1]*index[2]+outImageSize[1]);
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (1-frac_x)*(1-frac_y)*(  frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]+1 + outImageSize[0]*(index[1]+1+ outImageSize[1]*index[2]  );
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }

            wDir = dir;
            weight = (1-frac_x)*(1-frac_y)*(1-frac_z);
            if (weight>m_Thres)
            {
                wDir *= weight;
                idx = index[0]+1 + outImageSize[0]*(index[1]+1+ outImageSize[1]*index[2]+outImageSize[1]);
                dirCont = DirectionContainerType::New();
                if (m_DirectionsContainer->IndexExists(idx))
                {
                    dirCont = m_DirectionsContainer->GetElement(idx);
                    if (dirCont.IsNull())
                    {
                        dirCont = DirectionContainerType::New();
                        dirCont->InsertElement(0, wDir);
                        m_DirectionsContainer->InsertElement(idx, dirCont);
                    }
                    else
                        dirCont->InsertElement(dirCont->Size(), wDir);
                }
                else
                {
                    dirCont->InsertElement(0, wDir);
                    m_DirectionsContainer->InsertElement(idx, dirCont);
                }
            }
        }
        clock.Stop();
    }

    vtkSmartPointer<vtkCellArray> m_VtkCellArray = vtkSmartPointer<vtkCellArray>::New();
    vtkSmartPointer<vtkPoints>    m_VtkPoints = vtkSmartPointer<vtkPoints>::New();

    itk::ImageRegionIterator<ItkUcharImgType> dirIt(m_NumDirectionsImage, m_NumDirectionsImage->GetLargestPossibleRegion());
    itk::ImageRegionIterator<ItkUcharImgType> crossIt(m_CrossingsImage, m_CrossingsImage->GetLargestPossibleRegion());

    m_DirectionImageContainer = DirectionImageContainerType::New();
    unsigned int maxNumDirections = 0;

    MITK_INFO << "Clustering directions";
    boost::progress_display disp2(outImageSize[0]*outImageSize[1]*outImageSize[2]);
    for(crossIt.GoToBegin(); !crossIt.IsAtEnd(); ++crossIt)
    {
        ++disp2;
        OutputImageType::IndexType index = crossIt.GetIndex();
        int idx = index[0]+(index[1]+index[2]*outImageSize[1])*outImageSize[0];

        if (!m_DirectionsContainer->IndexExists(idx))
        {
            ++dirIt;
            continue;
        }
        DirectionContainerType::Pointer dirCont = m_DirectionsContainer->GetElement(idx);
        if (dirCont.IsNull() || index[0] < 0 || (unsigned long)index[0] >= outImageSize[0] || index[1] < 0 || (unsigned long)index[1] >= outImageSize[1] || index[2] < 0 || (unsigned long)index[2] >= outImageSize[2])
        {
            ++dirIt;
            continue;
        }

        std::vector< DirectionType > directions;

        for (unsigned int i=0; i<dirCont->Size(); i++)
            if (dirCont->ElementAt(i).magnitude()>0.0001)
                directions.push_back(dirCont->ElementAt(i));

        if (!directions.empty())
            directions = FastClustering(directions);

        std::sort( directions.begin(), directions.end(), CompareVectorLengths );

        if ( directions.size() > maxNumDirections )
        {
            for (unsigned int i=maxNumDirections; i<std::min<unsigned int>(directions.size(), m_MaxNumDirections); i++)
            {
                ItkDirectionImageType::Pointer directionImage = ItkDirectionImageType::New();
                directionImage->SetSpacing( spacing );
                directionImage->SetOrigin( origin );
                directionImage->SetDirection( direction );
                directionImage->SetRegions( imageRegion );
                directionImage->Allocate();
                Vector< float, 3 > nullVec; nullVec.Fill(0.0);
                directionImage->FillBuffer(nullVec);
                m_DirectionImageContainer->InsertElement(i, directionImage);
            }
            maxNumDirections = std::min<unsigned int>(directions.size(), m_MaxNumDirections);
        }

        unsigned int numDir = directions.size();
        if (numDir>m_MaxNumDirections)
            numDir = m_MaxNumDirections;

        for (unsigned int i=0; i<numDir; i++)
        {
            vtkSmartPointer<vtkPolyLine> container = vtkSmartPointer<vtkPolyLine>::New();
            itk::ContinuousIndex<double, 3> center;
            center[0] = index[0];
            center[1] = index[1];
            center[2] = index[2];
            itk::Point<double> worldCenter;
            m_MaskImage->TransformContinuousIndexToPhysicalPoint( center, worldCenter );
            DirectionType dir = directions.at(i);

            // set direction image pixel
            ItkDirectionImageType::Pointer directionImage = m_DirectionImageContainer->GetElement(i);
            Vector< float, 3 > pixel;
            pixel.SetElement(0, dir[0]);
            pixel.SetElement(1, dir[1]);
            pixel.SetElement(2, dir[2]);
            directionImage->SetPixel(index, pixel);

            // add direction to vector field (with spacing compensation)
            itk::Point<double> worldStart;
            worldStart[0] = worldCenter[0]-dir[0]/2*minSpacing;
            worldStart[1] = worldCenter[1]-dir[1]/2*minSpacing;
            worldStart[2] = worldCenter[2]-dir[2]/2*minSpacing;
            vtkIdType id = m_VtkPoints->InsertNextPoint(worldStart.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            itk::Point<double> worldEnd;
            worldEnd[0] = worldCenter[0]+dir[0]/2*minSpacing;
            worldEnd[1] = worldCenter[1]+dir[1]/2*minSpacing;
            worldEnd[2] = worldCenter[2]+dir[2]/2*minSpacing;
            id = m_VtkPoints->InsertNextPoint(worldEnd.GetDataPointer());
            container->GetPointIds()->InsertNextId(id);
            m_VtkCellArray->InsertNextCell(container);
        }
        dirIt.Set(numDir);
        ++dirIt;
    }

    vtkSmartPointer<vtkPolyData> directionsPolyData = vtkSmartPointer<vtkPolyData>::New();
    directionsPolyData->SetPoints(m_VtkPoints);
    directionsPolyData->SetLines(m_VtkCellArray);
    m_OutputFiberBundle = mitk::FiberBundleX::New(directionsPolyData);
}

template< class PixelType >
std::vector< vnl_vector_fixed< double, 3 > > TractsToVectorImageFilter< PixelType >::FastClustering(std::vector< vnl_vector_fixed< double, 3 > >& inDirs)
{
    std::vector< vnl_vector_fixed< double, 3 > > outDirs;
    if (inDirs.empty())
        return outDirs;
    vnl_vector_fixed< double, 3 > oldMean, currentMean, workingMean;

    std::vector< vnl_vector_fixed< double, 3 > > normalizedDirs;
    std::vector< int > touched;
    for (unsigned int i=0; i<inDirs.size(); i++)
    {
        normalizedDirs.push_back(inDirs[i]);
        normalizedDirs.back().normalize();
    }

    // initialize
    double max = 0.0;
    touched.resize(inDirs.size(), 0);
    bool free = true;
    currentMean = inDirs[0];  // initialize first seed
    while (free)
    {
        oldMean.fill(0.0);

        // start mean-shift clustering
        double angle = 0.0;
        int counter = 0;
        while ((currentMean-oldMean).magnitude()>0.0001)
        {
            counter = 0;
            oldMean = currentMean;
            workingMean = oldMean;
            workingMean.normalize();
            currentMean.fill(0.0);
            for (unsigned int i=0; i<normalizedDirs.size(); i++)
            {
                angle = dot_product(workingMean, normalizedDirs[i]);
                if (angle>=m_AngularThreshold)
                {
                    currentMean += inDirs[i];
                    touched[i] = 1;
                    counter++;
                }
                else if (-angle>=m_AngularThreshold)
                {
                    currentMean -= inDirs[i];
                    touched[i] = 1;
                    counter++;
                }
            }
        }

        // found stable mean
        if (counter>0)
        {
            currentMean /= counter;
            double mag = currentMean.magnitude();

            if (mag>0)
            {
                if (mag>max)
                    max = mag;

                outDirs.push_back(currentMean);
            }
        }

        // find next unused seed
        free = false;
        for (unsigned int i=0; i<touched.size(); i++)
            if (touched[i]==0)
            {
                currentMean = inDirs[i];
                free = true;
            }
    }

    if (m_NormalizeVectors)
        for (unsigned int i=0; i<outDirs.size(); i++)
            outDirs[i].normalize();
    else if (max>0)
        for (unsigned int i=0; i<outDirs.size(); i++)
            outDirs[i] /= max;

    if (inDirs.size()==outDirs.size())
        return outDirs;
    else
        return FastClustering(outDirs);
}

template< class PixelType >
std::vector< vnl_vector_fixed< double, 3 > > TractsToVectorImageFilter< PixelType >::Clustering(std::vector< vnl_vector_fixed< double, 3 > >& inDirs)
{
    std::vector< vnl_vector_fixed< double, 3 > > outDirs;
    if (inDirs.empty())
        return outDirs;
    vnl_vector_fixed< double, 3 > oldMean, currentMean, workingMean;

    std::vector< vnl_vector_fixed< double, 3 > > normalizedDirs;
    std::vector< int > touched;
    for (std::size_t i=0; i<inDirs.size(); i++)
    {
        normalizedDirs.push_back(inDirs[i]);
        normalizedDirs.back().normalize();
    }

    // initialize
    double max = 0.0;
    touched.resize(inDirs.size(), 0);
    for (std::size_t j=0; j<inDirs.size(); j++)
    {
        currentMean = inDirs[j];
        oldMean.fill(0.0);

        // start mean-shift clustering
        double angle = 0.0;
        int counter = 0;
        while ((currentMean-oldMean).magnitude()>0.0001)
        {
            counter = 0;
            oldMean = currentMean;
            workingMean = oldMean;
            workingMean.normalize();
            currentMean.fill(0.0);
            for (std::size_t i=0; i<normalizedDirs.size(); i++)
            {
                angle = dot_product(workingMean, normalizedDirs[i]);
                if (angle>=m_AngularThreshold)
                {
                    currentMean += inDirs[i];
                    counter++;
                }
                else if (-angle>=m_AngularThreshold)
                {
                    currentMean -= inDirs[i];
                    counter++;
                }
            }
        }

        // found stable mean
        if (counter>0)
        {
            bool add = true;
            vnl_vector_fixed< double, 3 > normMean = currentMean;
            normMean.normalize();
            for (std::size_t i=0; i<outDirs.size(); i++)
            {
                vnl_vector_fixed< double, 3 > dir = outDirs[i];
                dir.normalize();
                if ((normMean-dir).magnitude()<=0.0001)
                {
                    add = false;
                    break;
                }
            }

            currentMean /= counter;
            if (add)
            {
                double mag = currentMean.magnitude();
                if (mag>0)
                {
                    if (mag>max)
                        max = mag;

                    outDirs.push_back(currentMean);
                }
            }
        }
    }

    if (m_NormalizeVectors)
        for (std::size_t i=0; i<outDirs.size(); i++)
            outDirs[i].normalize();
    else if (max>0)
        for (std::size_t i=0; i<outDirs.size(); i++)
            outDirs[i] /= max;

    if (inDirs.size()==outDirs.size())
        return outDirs;
    else
        return FastClustering(outDirs);
}

template< class PixelType >
TractsToVectorImageFilter< PixelType >::DirectionContainerType::Pointer TractsToVectorImageFilter< PixelType >::MeanShiftClustering(DirectionContainerType::Pointer dirCont)
{
    DirectionContainerType::Pointer container = DirectionContainerType::New();

    double max = 0;
    for (DirectionContainerType::ConstIterator it = dirCont->Begin(); it!=dirCont->End(); ++it)
    {
        vnl_vector_fixed<double, 3> mean = ClusterStep(dirCont, it.Value());

        if (mean.is_zero())
            continue;
        bool addMean = true;

        for (DirectionContainerType::ConstIterator it2 = container->Begin(); it2!=container->End(); ++it2)
        {
            vnl_vector_fixed<double, 3> dir = it2.Value();
            double angle = fabs(dot_product(mean, dir)/(mean.magnitude()*dir.magnitude()));
            if (angle>=m_Epsilon)
            {
                addMean = false;
                break;
            }
        }

        if (addMean)
        {
            if (m_NormalizeVectors)
                mean.normalize();
            else if (mean.magnitude()>max)
                max = mean.magnitude();
            container->InsertElement(container->Size(), mean);
        }
    }

    // max normalize voxel directions
    if (max>0 && !m_NormalizeVectors)
        for (std::size_t i=0; i<container->Size(); i++)
            container->ElementAt(i) /= max;

    if (container->Size()<dirCont->Size())
        return MeanShiftClustering(container);
    else
        return container;
}

template< class PixelType >
vnl_vector_fixed<double, 3> TractsToVectorImageFilter< PixelType >::ClusterStep(DirectionContainerType::Pointer dirCont, vnl_vector_fixed<double, 3> currentMean)
{
    vnl_vector_fixed<double, 3> newMean; newMean.fill(0);

    for (DirectionContainerType::ConstIterator it = dirCont->Begin(); it!=dirCont->End(); ++it)
    {
        vnl_vector_fixed<double, 3> dir = it.Value();
        double angle = dot_product(currentMean, dir)/(currentMean.magnitude()*dir.magnitude());
        if (angle>=m_AngularThreshold)
            newMean += dir;
        else if (-angle>=m_AngularThreshold)
            newMean -= dir;
    }

    if (fabs(dot_product(currentMean, newMean)/(currentMean.magnitude()*newMean.magnitude()))>=m_Epsilon || newMean.is_zero())
        return newMean;
    else
        return ClusterStep(dirCont, newMean);
}
}


//
// Created by christoph on 09.02.19.
//

#include <chrono>
#include <boost/algorithm/string/predicate.hpp>

#include <Utils/File/FileUtils.hpp>
#include <Utils/File/Logfile.hpp>
#include <Math/Geometry/MatrixUtil.hpp>

#include "../VoxelRaytracing/VoxelData.hpp"
#include "../VoxelRaytracing/VoxelCurveDiscretizer.hpp"

#include "VoxelAO.hpp"

void VoxelAOHelper::loadAOFactorsFromVoxelFile(const std::string &filename, TrajectoryType trajectoryType)
{
    // Check if voxel grid is already created
    // Pure filename without extension (to create compressed .voxel filename)
    std::string modelFilenamePure = sgl::FileUtils::get()->removeExtension(filename);

    std::string modelFilenameVoxelGrid = modelFilenamePure + ".voxel";

    // Can be either hair dataset or trajectory dataset
    bool isHairDataset = boost::starts_with(modelFilenamePure, "Data/Hair");
    bool isRings = boost::starts_with(modelFilenamePure, "Data/Rings");
    bool isConvectionRolls = boost::starts_with(modelFilenamePure, "Data/ConvectionRolls");
    bool isAneurysm = boost::starts_with(modelFilenamePure, "Data/Trajectories");

    auto start = std::chrono::system_clock::now();

    uint16_t voxelRes = 128;
    if (isRings) {
        voxelRes = 32;
    }
    if (isAneurysm)
    {
        voxelRes = 128;
    }

    VoxelGridDataCompressed compressedData;
    if (!sgl::FileUtils::get()->exists(modelFilenameVoxelGrid)) {
        VoxelCurveDiscretizer discretizer(glm::ivec3(voxelRes), glm::ivec3(64));

        int maxNumLinesPerVoxel = 32;
        if (boost::starts_with(filename, "Data/WCB")) {
            maxNumLinesPerVoxel = 128;
        } else if (boost::starts_with(filename, "Data/ConvectionRolls/turbulence20000")){
            maxNumLinesPerVoxel = 64;
        }

        if (isHairDataset) {
            std::string modelFilenameHair = modelFilenamePure + ".hair";
            float lineRadius;
            glm::vec4 hairStrandColor;
            compressedData = discretizer.createFromHairDataset(modelFilenameHair, lineRadius, hairStrandColor,
                    maxNumLinesPerVoxel);
        } else {
            std::string modelFilenameObj = modelFilenamePure + ".obj";
            std::vector<float> attributes;
            float maxVorticity;
            compressedData = discretizer.createFromTrajectoryDataset(modelFilenameObj, trajectoryType,
                    attributes, maxVorticity, maxNumLinesPerVoxel);
        }

        auto end = std::chrono::system_clock::now();
        auto elapsed =
                std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
        sgl::Logfile::get()->writeInfo(std::string() + "Computational time to create voxel density for AO: "
                                       + std::to_string(elapsed.count()));

        saveToFile(modelFilenameVoxelGrid, compressedData);
    } else {
        loadFromFile(modelFilenameVoxelGrid, compressedData);
    }

    aoTexture = generateDensityTexture(compressedData.voxelAOFactors, compressedData.gridResolution);
    worldToVoxelGridMatrix = compressedData.worldToVoxelGridMatrix;
    gridResolution = compressedData.gridResolution;
}

void VoxelAOHelper::setUniformValues(sgl::ShaderProgramPtr transparencyShader)
{
    sgl::TexturePtr aoTexture = getAOTexture();
    transparencyShader->setUniform("aoTexture", aoTexture, 4);
    glm::mat4 worldSpaceToVoxelTextureSpace =
            sgl::matrixScaling(1.0f / glm::vec3(gridResolution)) * worldToVoxelGridMatrix;
    transparencyShader->setUniform("worldSpaceToVoxelTextureSpace", worldSpaceToVoxelTextureSpace);
}


#include "precompiled.h"
#include "finite_difference_weights.h"

#ifndef AUTOMATIC_PRECOMPILATION
#include <utility>
#include <tbb/tick_count.h>
#include <tbb/partitioner.h>
#include <boost/throw_exception.hpp>
#endif

#include "Eigen.h"
#include "utility.h"
#include "Math.h"

#ifndef AUTOMATIC_PRECOMPILATION
#include <pretty_printer.h>
#endif

#include "finite_difference_stencils.h"
#include "logging.h"
#include "finite_difference_weights_algo.h"
#include "grid_striding_info_inline.h"
#include "global_config.h"
#include "exceptions.h"
#include "PDE.h"

namespace fipster { namespace finite_difference_weights {

	// #################################################
	// ##############   PHASE 2:			############
	// ##############   Node Factories      ############
	// #################################################

	fdweights::sender_ptr fdweights::setup_node( const arg_t& arg )
	{
		auto PDE=arg.first.PDE;

		auto pde = dynamic_pointer_cast<const BSiso>(PDE);
		if(pde) return create_node(body_t<const BSiso>(arg,pde));

		// check for different PDEs
		// auto pde = dynamic_cast<const DifferenPDE* >(PDE);
		// if(pde) return create_node(body_t<const ...>(arg,pde));

		FIPSTER_THROW_EXCEPTION(runtime_error("PDE type "+PDE->type+" not implemented!"));
		return 0;
	}

	// #################################################
	// ##############   PHASE 3:			############
	// ##############   Node Bodies         ############
	// #################################################

	using namespace Eigen;

	template<class PDE_core>
	body_t<PDE_core>::body_t( const fd_weights_arg_t& args
														, shared_ptr<const PDE_core> pde_ptr )
		:	pde(pde_ptr),args(args),D(args.second->D)
	{}

	//############################################################################
	//Constructs the multi_indices corresponding to partial derivatives
	int references::generatePDindices(uint order)
	{
		uint i__,i__1,i__2,i__3,k,j;
		bool more;
		/* Calculate number of partial derivatives to include */
		int numpds = 0;
		i__1 = order;
		for (i__ = 1; i__ <= i__1; ++i__) {
			i__2 = i__ + D - 1;
			i__3 = D - 1;
			numpds += (int) binom(i__2, i__3);
		}

		multi_indices.resize(numpds,D);
		position_t ind(D);
		more=0;
		j = 0;
		/* Calculate multi indices for partial derivatives */
		i__1 = numpds;
		int h,t;
		for (i__ = 0; i__ < i__1; ++i__) {
			if (! more) {
				++j;
				if (j > order)
					BOOST_THROW_EXCEPTION(invalid_argument(
					"Number of Partial derivatives is to high. Specified order exceeded"));
			}
			comp_next(j, D, &ind[0], &more, &h, &t);
			for (k = 0; k < D; ++k) {
				multi_indices(i__,k) = ind[k];
			}
		}

		return numpds;
	}

	//############################################################################
	// Compute line-wise constant factorials in denominator for weight optimization systems
	void references::generateFactorialDenoms(double HigherOrderWeight){

		factorialdenoms.resize(multi_indices.rows());

		uint j;

		for (int i = 0; i < factorialdenoms.size(); ++i) {
			uint sum=0;
			factorialdenoms[i]=1.0;
			for (j=0; j < D; ++j) {
				factorialdenoms[i] /= factorial(multi_indices(i,j));
				if(multi_indices(i,j)!=0) sum++;
			}
			if (sum>2)
				factorialdenoms[i] *= HigherOrderWeight;
		}
	}

	template<class F,class G, class H, class I >
	void test_A_o(const F& A_o, const G& A_io, const H& gsi, const I& stencil_points){
		auto& grid=*gsi.grid;
		auto end_b = grid.template b_end<J_SGS>();
		set<boundary_index<J_SGS-1>> obinds;
		for(auto bit = boundary_iterator<J_SGS>(grid); bit != end_b; ++bit)
		{
			obinds.clear();

			//loop over neighbors
			const auto s_end=end(gsi.sorted_neighs);
			for(auto s=begin(gsi.sorted_neighs);s!=s_end;++s){
				//get neighbor position
				auto pos=bit.pos()+stencil_points.at(s->second);
				//check, if points lies on outer boundary:
				boundary_index<J_SGS-1> obind;
				if(obind.make_ind_from_pos<true>(pos,grid)){
					FIPSTER_ASSERT(A_o.coeff(bit.b_ind,obind)
										==A_io.at(s->second).at(bit.g_ind));
					if(A_io.at(s->second).at(bit.g_ind)!=0)
						obinds.insert(obind);
				}
			}

			//compare number of nonzeros in this A_o row and the number of unique accesses
			FIPSTER_ASSERT(obinds.size()==
											(uint)A_o->innerVector(bit.b_ind.ind).nonZeros());
		}
	}

	//############################################################################
	/** Generate the finite difference weights for the selected stencil,
		to get a finite difference approximation of the differential operator.

		It's exactly done as described in  the paper
		"Ito, K., & Toivanen, J. (2009). Lagrange Multiplier Approach with
		Optimized Finite Difference Stencils for Pricing American Options
		under Stochastic Volatility. SIAM Journal on Scientific Computing,
		31(4), 2646. doi:10.1137/07070574X"

		With the following additions:
		1. The sign choices specified in PDE.h dicate that the weights
			constitute a NEGATIVE M-MATRIX.
		2. This is achieved by using non-negative least squares for the off-diagonal weights,
			as opposed to the proposed solution of a quadratic programming problem.
		3. Extension to higher dimensional PDEs
		4. Eq. 40 contains (dx_i)^delta_i. I use 0^0 = 1 (Reason: If there is no
			derivative present in one direction (delta_i=0) then it is
			ignored (i.e. factor 1). In all other cases a stepsize of
			zero in one direction (dx_i=0) will yield a 0 in the matrix d.)

		 \todo verbesserung: wie vorgeschlagen, die partiellen Ableitungen
		 die man mit einem gröberen Gitter erhällt für die optimierung
		 benutzen...

	\return 1. A 2D array, where each row corresponds to one point in the multi-dim
			grid and the i-th column represent the weights of the i-th stencil point,
			if sorted lexicically by its direction vector. The last weight
			corresponds to the unfinished (see note) center weight.
			2. The so called "grid_striding_info"

	\note To get the final center weight all non-center weights have
			to be subtracted.

	*/
	template<class PDE_core>
	typename body_t<PDE_core>::result_sptr_t body_t<PDE_core>::operator()( const input_t& )
	{
		const grid_ptr& grid = args.second;

		//gcc: was editable_result(new result_t) (i.e. possibly without initizalitaion?)
		fd_weights_result_ptr_t result = make_shared<result_t>();

		//create object to hold stuff
		references r(D);

		//#### Create Stencil Points (excl. center point), sorted, such that:
		// [ points(i) , points(points.size()-i-1) ] is
		// the i-th direction

		try{

			r.stencil_points.construct(D,args.first.stencil_type);

		}catch(boost::exception& e){
			e<<boost::errinfo_file_name(("Stencil Type "+toS(args.first.stencil_type)).c_str());
			throw;
		}

		uint proper_neighs=r.stencil_points.proper_neighs();

		//#### Allocate result memory (+1 for center weights)
		auto end_ind = result->A_io.resize(*grid,r.stencil_points.nDirs());

		//#### Construct the multi_indices corresponding to partial derivatives
		uint numpds = r.generatePDindices(args.first.partial_derivative_order);

		thread_logger()<<"WEIGHTS| for "+toS(proper_neighs)+" neighbors and "+toS(numpds)+" of PDE terms"<<endl;

		if(0){cout<<"X"<<endl;
		cout<<"WEIGHTS| for "+toS(proper_neighs)+" neighbors and "+toS(numpds)+" of PDE terms"<<endl;
		cout<<args.first<<endl;
		}
		//#### Check, if weight (unconstrained) determination is not overdetermined:
		if (numpds == proper_neighs)
			thread_logger()<<"WEIGHTS| (unconstrained weights) are NOT overdetermined"<<endl;
		else if (numpds < proper_neighs)
			thread_logger()<<"WEIGHTS| (unconstrained weights) are UNDERDETERMINED and not unique!"<<endl;

		//####  Compute line wise constant factorials in denominator
		r.generateFactorialDenoms(args.first.higher_order_weight);

		//###### Optimize weights in parallel ( minimze norm: | D*w - b |)
		{	using namespace tbb;
			optimize_body<fd_weights_result_t::A_io_t,body_t<PDE_core>> body
				(r,result->A_io,pde,*grid);
			thread_logger()<<"WEIGHTS| nSites: "<<end_ind.ind<<endl;
			tick_count t0 = tick_count::now();
			parallel_reduce(
				blocked_range<grid_index<sg>>(grid_index<sg>(),end_ind,100),
				body/*,simple_partitioner()*/);
			tick_count t1 = tick_count::now();
			thread_logger()<<"WEIGHTS| DONE, maxNorm:"<<scientific <<setprecision(3)<<
				body.max_norm <<" in " <<(t1-t0).seconds()<<"s"<<endl;
		}

		//###### create information for decoupled tridiagonal systems in each splitting direction
		//###### AND A_o
		tick_count t0 = tick_count::now();

		auto b_end = grid->b_end<sg>();

		auto& gsi = *new grid_striding_info_collection<sg>(r.stencil_points.nDirs(),r.stencil_points,grid);
		thread_logger()<< "&gsi: " << &gsi << endl;
		result->grid_striding_infos.reset(&gsi);

		auto& A_io=result->A_io;
		auto& A_o=*result->A_o;
		A_o.resize(*grid);

		//test for uniqueness of the systems:
		//map<pair<int,int>,int> m;

		A_o->reserve(VectorXi::Constant(grid->Nbound[J_SGS],proper_neighs));

		//iterate over boundary and check for system beginnings
		for(connected_set_setup_iterator<sg> it(*grid);it!=b_end;++it){

			//cout<<"at "<<it.inner_start_g_ind().ind<<endl;

			auto i = r.stencil_points.dir_it();
			for(;i.valid();++i){//iterate through all directions
				auto& dir = r.stencil_points[i];
				if(it.apply_dir(dir)){//if a system in this direction exists

					//cout<<"dir: "<<dir.transpose()<<endl;

					/*
					//test for uniqueness of the system:
					//The naive test, that identifies a system
					//via its order start and end point on the inner boundary
					//fails in D>2, as a corner (of the inner boundary) can
					//belong to more than one system (in D>2)!
					//int a=it.inner_start_b_ind().ind;int b=it.inner_end_b_ind.ind;
					//use this instead:
					int a=it.outer_start_b_ind().ind;int b=it.outer_end_b_ind().ind;
					if(a==2 && b==2){
						cout<<"pos: "<<it.it.pos.transpose()<<endl;
						cout<<"dir: "<<dir.transpose()<<endl;
					}
					m[make_pair(min(a,b),max(a,b))]++;
					if(m[make_pair(min(a,b),max(a,b))]>1){
						cout<<m[make_pair(min(a,b),max(a,b))]<<" Systems found between "<<min(a,b)<<" and "<<max(a,b)<<endl;
					}
					*/

					//add subsystem
					gsi.at(i).pushback_connected_set(it);

					bool debug = false;///\todo: set false

					//add A_o corresponding to start of system
					A_o.insert(it.inner_start_b_ind(),it.outer_start_b_ind,
								(debug?1:A_io.at_opposite(i)[it.inner_start_g_ind()]));

					//cout<<it.inner_start_g_ind().ind<<": "<<A_io.at_opposite(i)[it.inner_start_g_ind()]<<endl;
					//cout<<it.inner_end_g_ind.ind<<": "<<A_io.at(i)[it.inner_end_g_ind]<<endl;

					//add A_o corresponding to end of system
					A_o.insert(it.inner_end_b_ind,it.outer_end_b_ind,
								(debug?1:A_io.at(i)[it.inner_end_g_ind]));

					///\todo: check for natural boundaries (i.e. all A_o=0 on bound)
				}
			}
		}

		A_o->makeCompressed();

		if(global_config::get().test_A_o){
			test_A_o(A_o,A_io,gsi,r.stencil_points);
		}

		//A_o.print();

		tick_count t1 = tick_count::now();
		thread_logger()<<"WEIGHTS| DONE systems information in "<<scientific <<setprecision(3)
			<<(t1-t0).seconds()<<"s"<<endl;

		return result;
	}

}}