//  Boost Sort library float_sort_test.cpp file  -----------------------------//

//  Copyright Steven Ross 2014. 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)

//  See http://www.boost.org/libs/sort for library home page.

#include <boost/sort/spreadsort/spreadsort.hpp>
// Include unit test framework
#include <boost/test/included/test_exec_monitor.hpp>
#include <boost/test/test_tools.hpp>
#include <vector>

using namespace std;
using namespace boost::sort::spreadsort;

//Casting to an integer before bitshifting
struct rightshift {
  int operator()(const float &x, const unsigned offset) const {
    return float_mem_cast<float, int>(x) >> offset;
  }
};

struct rightshift_64 {
  boost::int64_t operator()(const double &x, const boost::uint64_t offset) const
  {
    return float_mem_cast<double, boost::int64_t>(x) >> offset;
  }
};

boost::int32_t
rand_32(bool sign = true) {
   boost::int32_t result = rand() | (rand()<< 16);
   if (rand() % 2)
     result |= 1 << 15;
   //Adding the sign bit
   if (sign && (rand() % 2))
     result *= -1;
   return result;
}

static const unsigned input_count = 1000000;

// Helper class to run tests across all float_sort interface variants.
template<class FloatType, class RightShift>
void test_vector(vector<FloatType> base_vec, RightShift shifter) {
  vector<FloatType> sorted_vec = base_vec;
  vector<FloatType> test_vec = base_vec;
  std::sort(sorted_vec.begin(), sorted_vec.end());
  //Testing boost::sort::spreadsort version
  test_vec = base_vec;
  boost::sort::spreadsort::spreadsort(test_vec.begin(), test_vec.end());
  BOOST_CHECK(test_vec == sorted_vec);
  //One functor
  test_vec = base_vec;
  float_sort(test_vec.begin(), test_vec.end(), shifter);
  BOOST_CHECK(test_vec == sorted_vec);
  //Both functors
  test_vec = base_vec;
  float_sort(test_vec.begin(), test_vec.end(), shifter, less<FloatType>());
  BOOST_CHECK(test_vec == sorted_vec);
}

void float_test()
{
  // Prepare inputs
  vector<float> base_vec;

  //Generating semirandom numbers that will work for basic testing
  for (unsigned u = 0; u < input_count; ++u) {
    float val = float(rand_32());
    //As std::sort gives arbitrary results for NaNs and 0.0 vs. -0.0, treat all
    //those as just 0.0 for testing
    if (!(val < 0.0) && !(0.0 < val))
      base_vec.push_back(0.0);
    else
      base_vec.push_back(val);
  }
  test_vector(base_vec, rightshift());

  // Trying both positive and negative sorted and reverse sorted data.
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(-i);
  test_vector(base_vec, rightshift());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i - input_count);
  test_vector(base_vec, rightshift());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(input_count - i);
  test_vector(base_vec, rightshift());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i);
  test_vector(base_vec, rightshift());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i);
  for (size_t i = 0; i < input_count; i += 2) base_vec[i] *= -1;
  test_vector(base_vec, rightshift());
}

void double_test() {
  vector<double> base_vec;
  for (unsigned u = 0; u < input_count; ++u) {
    double val = double
    ((((boost::int64_t)rand_32()) << ((8 * sizeof(int)) -1)) + rand_32(false));
    //As std::sort gives arbitrary results for NaNs and 0.0 vs. -0.0,
    //treat all those as just 0.0 for testing
    if (!(val < 0.0) && !(0.0 < val))
      base_vec.push_back(0.0);
    else
      base_vec.push_back(val);
  }
  test_vector(base_vec, rightshift_64());

  // Trying both positive and negative sorted and reverse sorted data.
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(-i);
  test_vector(base_vec, rightshift_64());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i - input_count);
  test_vector(base_vec, rightshift_64());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(input_count - i);
  test_vector(base_vec, rightshift_64());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i);
  test_vector(base_vec, rightshift_64());
  base_vec.clear();
  for (size_t i = 0; i < input_count; ++i) base_vec.push_back(i);
  for (size_t i = 0; i < input_count; i += 2) base_vec[i] *= -1;
  test_vector(base_vec, rightshift_64());
}

// Verify that 0 and 1 elements work correctly.
void corner_test() {
  vector<float> test_vec;
  boost::sort::spreadsort::spreadsort(test_vec.begin(), test_vec.end());
  const float test_value = -0.0;
  test_vec.push_back(test_value);
  boost::sort::spreadsort::spreadsort(test_vec.begin(), test_vec.end());
  BOOST_CHECK(test_vec.size() == 1);
  BOOST_CHECK(test_vec[0] == test_value);
}

// test main
int test_main( int, char*[] )
{
  srand(1);
  float_test();
  double_test();
  corner_test();
  return 0;
}


/*

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

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

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

*/

#ifndef PIC_UTIL_coeffNOMIAL_HPP
#define PIC_UTIL_coeffNOMIAL_HPP

#include <vector>

#include "../base.hpp"

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

namespace pic {

class Polynomial
{
public:
    std::vector<float> coeff;
    bool  all_coeff_positive;

    /**
     * @brief Polynomial
     */
    Polynomial()
    {
        all_coeff_positive = true;
    }

    /**
     * @brief Polynomial
     * @param nCoeff
     */
    Polynomial(int nCoeff)
    {
        for(int i = 0; i < nCoeff; i++) {
            coeff.push_back(0.0f);
        }

        all_coeff_positive = true;
    }

    /**
     * @brief coeffnomial
     * @param coeff
     * @param nCoeff
     */
    Polynomial(float *coeff, int nCoeff)
    {
        if(nCoeff < 1 || coeff == NULL) {
            return;
        }

        this->coeff.assign(coeff, coeff + nCoeff);

        computeAllCoeffPositive();
    }

    ~Polynomial()
    {

    }

    /**
     * @brief computeAllCoeffPositive
     */
    void computeAllCoeffPositive()
    {
        int counter = 0;
        for (const float &c : coeff) {
            if(c < 0.0f) {
                counter++;
            }
        }

        all_coeff_positive = counter < 1;
    }

    /**
     * @brief print
     */
    void print()
    {
        printf("%s\n", toString().c_str());
    }

    std::string toString()
    {
        std::string ret = "";
        if(coeff.empty()) {
            return ret;
        }

        auto nCoeff = coeff.size();

        if(nCoeff > 1) {

            std::string sep = "+ ";
            if(coeff[1] < 0.0f) {
                sep = " ";
            }

            ret = fromNumberToString(coeff[0]) + sep;

            for(auto i = 1; i < (nCoeff - 1); i++) {

                if(coeff[i + 1] > 0.0f) {
                    sep = "+ ";
                } else {
                    sep = " ";
                }

                ret += fromNumberToString(coeff[i]) + " * x^" + fromNumberToString(i) + sep;
            }

            ret += fromNumberToString(coeff[nCoeff - 1]) + " * x^" + fromNumberToString(nCoeff - 1);
        } else {
            ret = fromNumberToString(coeff[0]);
        }
        return ret;
    }

    /**
     * @brief getArray
     * @return
     */
    float *getArray(float *ret)
    {
        if(ret == NULL) {
            ret = new float[coeff.size()];
        }

        for(int i = 0; i < coeff.size(); i++) {
            ret[i] = coeff[i];
        }

        return ret;
    }

    /**
     * @brief eval
     * @param x
     * @return
     */
    float eval(float x)
    {
        float val = 0.f;
        float M = 1.f;
        for (const float &c : coeff) {
            val += c * M;
            M *= x;
        }
        return val;
    }

    /**
     * @brief dEval
     * @param t
     * @return
     */
    float dEval(float x)
    {
        auto nCoeff = coeff.size();

        if(nCoeff < 2) {
            return 0.0f;
        }

        float ret = coeff[nCoeff - 1] * float(nCoeff - 1);

        for(auto i = (nCoeff - 2); i > 0 ; i--) {
            ret = (ret * x) + coeff[i] * float(i);
        }
        return ret;
    }

    /**
     * @brief fit
     * @param x
     * @param y
     * @param n
     */
    void fit(std::vector<float> &x, std::vector<float> &y, int n)
    {
#ifndef PIC_DISABLE_EIGEN
        if(n < 1 || (x.size() != y.size())) {
            return;
        }

        coeff.clear();

        int np1 = n + 1;

        int s = int(x.size());
        Eigen::MatrixXf A(s, np1);
        Eigen::VectorXf b(s);

        for(int i = 0; i < s; i++) {
            b(i) = y[i];
            A(i, n) = 1.0f;
        }

        for(int j = (n - 1); j >= 0; j--) {
            for(int i = 0; i < s; i++) {
                A(i, j) = x[i] * A(i, j + 1);
            }
        }

        Eigen::VectorXf _x = A.colPivHouseholderQr().solve(b);

        for(int i = n; i >= 0; i--) {
            coeff.push_back(_x(i));
        }
#endif
    }

    /**
     * @brief normalForm
     */
    void normalForm()
    {
        auto last = coeff.size() - 1;

        if(fabsf(coeff[last]) > 0.0f) {

            for(int i = 0; i < last; i++) {
                coeff[i] /= coeff[last];
            }

            coeff[last] = 1.0f;
        }
    }

    /**
     * @brief horner
     * @param d
     * @param remainder
     * @return
     */
    Polynomial horner(float d, float &remainder)
    {
        int nCoeff = int(coeff.size());
        Polynomial p(nCoeff - 1);

        p.coeff[nCoeff - 2] = coeff[nCoeff - 1];

        for(int i = (nCoeff - 3); i >= 0 ; i--) {
            p.coeff[i] = (p.coeff[i + 1] * d + coeff[i + 1]);
        }

        p.computeAllCoeffPositive();

        remainder = coeff[0] + p.coeff[0] * d;

        return p;
    }

    /**
     * @brief getRoots
     * @param x
     * @return
     */
    bool getRoots(float *x)
    {
        auto nCoeff = coeff.size();

        if(nCoeff < 2) {
            return false;
        }

        if(nCoeff == 2) {
            //this coefficient may be not positive
            if(coeff[1] > 0.0f) {
                x[0] = -coeff[0] / coeff[1];
                return true;
            } else {
                return false;
            }
        }

        if(nCoeff == 3) {
            //these coefficients may be not positive
            return getSecondOrderRoots(coeff[2], coeff[1], coeff[0], &x[0], &x[1]);
        }

        if(all_coeff_positive) {
            return false;
        }

        float max_coeff0 = -1.0f;

        for(int i = 1; i < coeff.size(); i++) {
            float tmp = fabsf(coeff[i]);
            if(tmp > max_coeff0) {
                max_coeff0 = tmp;
            }
        }

        float tmp = fabsf(coeff[0]);
        float max_coeff;
        if(max_coeff0 < tmp) {
            max_coeff = tmp;
        } else {
            max_coeff = max_coeff0;
        }

        float lower_bound = fabsf(coeff[0]) / (fabsf(coeff[0]) + max_coeff0);

#ifdef PIC_DEBUG
        float upper_bound = 1.0f + max_coeff / fabsf(coeff[0]);
        printf("Upper bound: %f\n", upper_bound);
        printf("Lower bound: %f\n", lower_bound);
#endif

        float x_p = lower_bound;
        float x_n;
        bool notConverged = true;
        int counter = 0;
        float E_x_p;
        while(notConverged) {
            E_x_p = eval(x_p);
            x_n = x_p - E_x_p / dEval(x_p);
            x_p = x_n;
            counter++;

            notConverged = (fabsf(E_x_p) > 1e-6f) && (counter < 500);
        }

        if(counter >= 500) {
            return false;
        } else {
            *x = x_n;
            return true;
        }
    }

    /**
     * @brief getAllRoots
     * @param x
     * @return
     */
    bool getAllRoots(float *x)
    {
        for(int i = 0; i < coeff.size() - 1; i++) {
            x[i] = FLT_MAX;
        }

        bool bOut = getRoots(&x[0]);

        if(!bOut) {
            return false;
        }

        float r;
        Polynomial p = horner(x[0], r);
        p.normalForm();

        int nCoeff = int(coeff.size());
        for(int i = 1; i < (nCoeff - 2); i++) {
            bool bOut = p.getRoots(&x[i]);

            if(!bOut) {
                return true;
            }

            p = p.horner(x[i], r);
            p.normalForm();
        }

        return true;
    }

    /**
     * @brief getQuarticRoots --> MANDATORY p[5] == 1.0
     * @param p
     * @param x
     * @return
     */
    static bool getQuarticRoots(float *p, float *x)
    {
#ifndef PIC_DISABLE_EIGEN
        Eigen::Matrix4d m;

        m << -p[3], -p[2], -p[1], -p[0],
             1.0f, 0.0f, 0.0f, 0.0f,
             0.0f, 1.0f, 0.0f, 0.0f,
             0.0f, 0.0f, 1.0f, 0.0f;

        Eigen::EigenSolver<Eigen::Matrix4d> es(m);
        Eigen::Vector4cd e = es.eigenvalues();

        bool bOut = false;

        for (int i = 0; i < 4; i++) {
            if (fabs(e(i).imag()) <= 0.0) {
                x[i] = float(e(i).real());
                bOut = true;
            }
        }

        return bOut;
#else
        return true;
#endif
    }

    /**
     * @brief getSecondOrderRoots solves second order equations, ax^2 + b x + c = 0
     * @param a is the a coefficient
     * @param b is the b coefficient
     * @param c is the c coefficient
     * @param x0 is the first zero
     * @param x1 is the second zero
     * @return It returns true, if x0 and x1 have a real value, false otherwise
     */
    static bool getSecondOrderRoots(float a, float b, float c, float *x0, float *x1)
    {
        float delta = b * b - 4.0f * a * c;

        if(delta >= 0.0f) {
            float dnum = 2.0f * a;
            delta = sqrtf(delta);
            *x0 = (-b + delta) / dnum;
            *x1 = (-b - delta) / dnum;
            return true;
        } else {
            return false;
        }
    }

    /**
     * @brief getSecondOrderRootsS solves second order equations, ax^2 + 2 b x + c = 0; i.e., 2 b is even!
     * @param a is the a coefficient
     * @param b is the b coefficient
     * @param c is the c coefficient
     * @param x0 is the first zero
     * @param x1 is the second zero
     * @return It returns true, if x0 and x1 have a real value, false otherwise
     */
    static bool getSecondOrderRootsS(float a, float b, float c, float *x0, float *x1)
    {
        float delta = b * b - a * c;
        if(delta >= 0.0f) {
            delta = sqrtf(delta);
            *x0 = (-b + delta) / a;
            *x1 = (-b - delta) / a;
            return true;
        } else {
            return false;
        }
    }

};

} // end namespace pic

#endif //PIC_UTIL_coeffNOMIAL_HPP


// Copyright 2008 Google Inc. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License")
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//      http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an AS IS BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Author: master___chen@163.com (Chen Shuo)

// Main file for building-block hillclimber (BBHC)

#include <fstream>
#include <iostream>
#include <algorithm>
#include <cmath>
#include <string>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>

#include "../utils/io_util.h"
#include "../utils/bool_util.h"

#include "../modeling/BDe.h"
#include "../alignment/scoring.h"
#include "../modeling/dtree_learn.h"

#include "../modeling/BB.h"

using namespace boost;
using namespace std;
using namespace modeling;

typedef pair<double,Assignment> fpair;

struct opt_base
{
	opt_base(): _evals(0) {};
	//virtual double operator()(Assigenment& a);
	void inline addEvals() { ++_evals; }
	int evals() const { return _evals; }
	int _evals;
};

struct testBool : public opt_base
{
	testBool(): opt_base() {};
	double operator()(Assignment& a)
	  { opt_base::addEvals();
		return count(a.begin(), a.end(), Value(true)); }
};

struct testInt : public opt_base
{
	testInt(): opt_base() {};
	double operator()(Assignment& a)
	  { opt_base::addEvals();
	    int sum=0;
	    for(Assignment::iterator i=a.begin(); i!=a.end(); ++i) sum+=todisc(*i);
	    return sum; }
    string name() { return "test int"; }
};

struct hiff : public opt_base
{
	hiff(int hLevel): opt_base(), _hLevel(hLevel), _length(exp2(hLevel)){};
	int _hLevel, _length;

	double operator()(Assignment& a)
	  { assert(a.size()==_length);
	    opt_base::addEvals();
	    return H(a.begin(), a.end()); }
	double H(Assignment::iterator first, Assignment::iterator last) const
	  { if ( first+1==last ) return h(first,last);
	    Assignment::iterator mid=first+(last-first)/2;
		return H(first, mid) + H(mid, last) // Hiff(L)+Hiff(R)
		       +h(first, last)*(last-first); }// hiff(x) * length
	double h(Assignment::iterator first, Assignment::iterator last) const
	  { if ( first+1==last ) return 1;
	    Assignment::iterator mid=first+(last-first)/2;
		if ( std::mismatch(first, mid, mid).first==mid
				&& h(first, mid)==1 && h(mid, last)==1) return 1;
		return 0; }
	bool isExp2(int length) const
	  { while (length>1 && length%2==0) length=length/2;
	    if (length==1) return true; else return false; }
	int exp2(int t) const
	  { int n=1; for (int i=0; i<t; ++i) n*=2;
	    return n; }
	string name() { return "HIFF"; }
};

struct hxor : public opt_base
{
	hxor(int hLevel): opt_base(), _hLevel(hLevel), _length(exp2(hLevel)){};
	  int _hLevel, _length;

	double operator()(Assignment& a)
	  { assert(a.size()==_length);
	    opt_base::addEvals();
	    return H(a.begin(), a.end()); }
	double H(Assignment::iterator first, Assignment::iterator last) const
	  { if ( first+1==last ) return h(first,last);
	    Assignment::iterator mid=first+(last-first)/2;
		return H(first, mid) + H(mid, last) // Hiff(L)+Hiff(R)
		       +h(first, last)*(last-first); }// hiff(x) * length
	template <typename valueType>
	struct _xor
	{ bool operator()(valueType a, valueType b) const
		{return (boost::get<bool>(a)==!boost::get<bool>(b));} };
	double h(Assignment::iterator first, Assignment::iterator last) const
	  { if ( first+1==last ) return 1;
	    Assignment::iterator mid=first+(last-first)/2;
		if ( std::mismatch(first, mid, mid, _xor<Value>()).first==mid
		    	&& h(first, mid)==1 && h(mid, last)==1) return 1;
		return 0;
	  }
	int exp2(int t) const
	  { int n=1; for (int i=0; i<t; ++i) n*=2;
	    return n; }
	string name() { return "HXOR"; }
};

struct htrap3 : public opt_base
{
	typedef pair<int, int> node_type; //
	typedef vector<node_type> level_type; //
	htrap3(int hLevel)
	  : _hLevel(hLevel), _length(exp3(hLevel)), _structure(0)
	  { for(int p=0; p<=_hLevel; ++p) _structure.push_back(level_type(exp3(_hLevel-p)) ); }
	int _hLevel, _length;
	vector<level_type > _structure;

	double operator()(Assignment& a)
	  { opt_base::addEvals();
		makeStruct(a);
		return HT();
	  }
	void makeStruct(Assignment& a)
	  { assert(a.size()==_length);
		for (int i=0; i<_length; ++i) _structure[0][i].first=int(tobool(a[i]));
        for (int p=1; p<=_hLevel; ++p) for (int j=0; j<_structure[p].size(); ++j)
          _structure[p][j]=h(_structure[p-1].begin()+j*3);
	  }
	double HT()
	  { double res=0;
		for (int p=1; p<=_hLevel; ++p)
	      { int len = exp3(p);
		    for (level_type::iterator it=_structure[p].begin(); it!=_structure[p].end(); ++it)
			  res += len * trap(it->second, 1.0, p==_hLevel?0.9:1.0);
	      }
		return res;
	  }
	struct value_equal
	  { int _value;
	    value_equal(int value): _value(value) {}
		bool operator()(node_type n){return n.first==_value;} };
	node_type h(level_type::iterator first1)
	  { if (first1->first==-1 || (first1+1)->first==-1 || (first1+2)->first==-1)
		  return make_pair(-1, -1);
		if (first1->first==(first1+1)->first && first1->first==(first1+2)->first)
		  return make_pair(first1->first, count_if(first1, first1+3, value_equal(1)) );
	    else return make_pair(-1, count_if(first1, first1+3, value_equal(1)) ); }
	double trap(int u, double fHigh, double fLow)
	  { assert(u<4);
	    if (u<0) return 0;
		if (u==3) return fHigh;
	    if (u>=0) return fLow*(3-1-u)/(3-1); }
	int exp3(int t)
	  { int res=1; for(int i=0; i<t; ++i) res*=3; return res; }
	string name() { return "HTRAP"; }
};

template <class eval_type>
struct noise : eval_type
{
	noise(const eval_type& ev, double nr)
	  :eval_type(ev), _nrate(nr) {}
	double _nrate;

	bool byChance(double rate) const { return randdouble()<rate;}
	Assignment makeNoise(Assignment a) const
	  { for (Assignment::iterator i=a.begin(); i!=a.end(); ++i)
		  if (byChance(_nrate)) *i=!tobool(*i);
		return a; }
	double operator() (Assignment& a)
	  { Assignment tmp=makeNoise(a);
		return eval_type::operator()(tmp); }
};

//typedef pair<double,Assignment> fpair;

const Assignment* t_selection(const vector<fpair>& v) {
  return &(tournament_select(v.begin(),v.end(),2,less<fpair>())->second);
}

vector<fpair>::iterator rtr_select(const Assignment& a,double score,
                                   vector<fpair>& pop,int tsize) {
  assert(tsize>0);
  vector<fpair>::iterator closest;
  if (tsize==1) { //simply do size-2 tournament selection
    closest=pop.begin()+randint(pop.size());
  } else {
    Selector select(pop.size());

    int closestSim=NEG_INFINITY;
    for (int i=0;i<tsize;++i) {
      vector<fpair>::iterator tmp=pop.begin()+select();
      int sim=(-hamming_distance(a.begin(),a.end(),tmp->second.begin()));
      if (sim>closestSim) {
        sim=closestSim;
        closest=tmp;
      }
    }
  }
  return (closest->first<score) ? closest : pop.end();
}

/***************hBOA***************/

typedef vector<fpair>::iterator popIter;

popIter rtr_select(const Assignment& a,double score,
		           popIter first, popIter last, int tsize) {
  assert(tsize>0);
  int pop_size=last-first;
  vector<fpair>::iterator closest;
  if (tsize==1) { //simply do size-2 tournament selection
    closest=first+randint(pop_size);
  } else {
    Selector select(pop_size);

    int closestSim=NEG_INFINITY;
    for (int i=0;i<tsize;++i) {
      vector<fpair>::iterator tmp=first+select();
      int sim=(-hamming_distance(a.begin(), a.end(), tmp->second.begin()));
      if (sim>closestSim) {
        sim=closestSim;
        closest=tmp;
      }
    }
  }
  return (closest->first<score) ? closest : last;
}

template<typename Opt>
void hBOA_once(popIter first, popIter last, Opt& scoring, int length)
{ int pop_size=last-first;
  Graph g(pop_size);
  g.reserve(length);
  for (int i=0;i<length;++i) {
	DiscMarginal& d=g.create_disc_marginal();
    assert(d.size()==pop_size);
    DiscMarginal::iterator j=d.begin();
    /**/
    for(popIter a=first; a!=last; ++a)
      { *j=todisc((a->second)[i]);
    	++j;
      } // get a Marginal distribution
    g.push_back(DiscDTree(d.begin(),d.end())); //
  }

  //update the gain offset
  BDe::gainOffset=(log2(gain_t(distance(first,last)))/2.0);

  //learn dependencies
  dtree_learn(g); //

  //sample new instances and RTR them into the population
  assert(g.ordering().size()==length);
  for (int i=0;i<pop_size;++i) {
    Assignment a(length,0);
    generate_assignment(g.ordering().begin(),g.ordering().end(),g,a);
    int rtr_tsize=min(length,int(0.05*double(pop_size)+1));
    double s=scoring(a);
    vector<fpair>::iterator j=rtr_select(a,s,first,last,rtr_tsize);
    if (j!=last) {
      j->first=s;
      j->second=a;
    }
  }
}

/***************HHBC***************/

template<typename Opt>
struct disc_bool_opt_wrapper
{ disc_bool_opt_wrapper(Opt* bool_opt, BBs bbs)
    : _opt(bool_opt), _bbs(bbs) {}
  Opt* _opt; BBs _bbs;

  double operator()(Assignment& a)
    { Assignment tmp=toBoolType(a);
      return _opt->operator()(tmp); }
  Assignment toBoolType(Assignment& a)
    { return _bbs.toBoolType(a); }
};

template<typename Opt>
void BBHC(Opt& scoring,int length,int pop_size,int n_gen,double maxscore)
{ BBs bbs(length);			     // model of building blocks
  vector<fpair> to_model(0); // promosing solutions
  vector<fpair> disc_model(0); // transformed to discrete type
  double best=0;
  fpair newState(0.0,Assignment(length,false));

  cout << "BBHC, " << scoring.name() << "~"
       << "  chro length: " << length
       << "  population size: " << pop_size
       << "  n generations: " << n_gen << endl;

  for (int gen=1;gen<n_gen;++gen) {

	to_model.clear();
	disc_model.clear();

	while (to_model.size()<pop_size ) //&& best<maxscore)  // not full && destination not reached
	  {
		newState.second=bbs.getRandomState();
		bbs.hillClimb(newState.second, scoring);
		newState.first=scoring(newState.second);
		to_model.push_back(newState);
		best=newState.first;
	  }

#define _DISABLE_HBOA
#ifndef _DISABLE_HBOA
	// hBOA

	// transform all to disc type
	for (int i=0; i<pop_size; ++i)
	  disc_model.push_back(make_pair(to_model[i].first,
			                           bbs.toDiscType(to_model[i].second)) );

	disc_bool_opt_wrapper<Opt> disc_scoring(&scoring, bbs);
	hBOA_once(disc_model.begin(), disc_model.end(),
			  disc_scoring, bbs.blocks());

	// tranform back
	for(int i=0; i<cSize; ++i)
	  { to_model[i].second=bbs.toBoolType(disc_model[i].second);
	    to_model[i].first=scoring(to_model[i].second);
	    if (to_model[i].first>best) best=to_model[i].first;
	  }
#endif

	// update BBHC model
	//bbs.updateBBsWithConf(to_model.begin(), to_model.end(), 0.9);
	bbs.updateBBs(to_model.begin(), to_model.end()); // to_model.begin()+cSize);
	bbs.updateBBConfigs(to_model.begin(), to_model.end()); // to_model.begin()+cSize);

	cout << "genration # " << gen
	     << ", # evals " << scoring.evals()
         << ", best fitness " << max_element(to_model.begin(),to_model.end())->first
         << ", BBs " << bbs.blocks()
         << ", configs " << bbs.bbconfigs()
         << endl ;

	best = max_element(to_model.begin(),to_model.end())->first;
    if (best>=maxscore) break;
  }

  cout << endl
       << "max score: "
       << best << endl
       << "best solution: "
       << max_element(to_model.begin(),to_model.end())->second << endl;

}

/******************END OF HHBC****************/

int main(int argc,char** argv) {
  assert(argc==5);
  srand(lexical_cast<int>(argv[1]));
  int level=lexical_cast<int>(argv[2]);
  int pop_size=lexical_cast<int>(argv[3]);
  int n_gen=lexical_cast<int>(argv[4]);

  hiff h(level);
  hxor hx(level);
  htrap3 ht3(level);

  /* 16-80; 32-192 ; 64-448 ; 128-1024; 256-2304; 512- 5120; 1024-11264*/

  // HIFF
  BBHC(h, h.exp2(level), pop_size, n_gen, h.exp2(level)*(level+1));

  // HXOR
  //BBHC(hx, hx.exp2(level), pop_size, n_gen, hx.exp2(level)*(level+1));

  // HTRAP
  //BBHC(ht3, ht3.exp3(level), pop_size, n_gen, ht3.exp3(level)*level);
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <fstream>
#include <string>
#include <boost/tuple/tuple.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/visitors.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <map>
#include <boost/graph/adj_list_serialize.hpp>
#include <boost/archive/text_iarchive.hpp>

struct vertex_properties {
  std::string name;

  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & name;
  }
};

struct edge_properties {
  std::string name;

  template<class Archive>
  void serialize(Archive & ar, const unsigned int version) {
    ar & name;
  }
};

using namespace boost;

typedef adjacency_list<vecS, vecS, undirectedS,
               vertex_properties, edge_properties> Graph;
typedef graph_traits<Graph>::vertex_descriptor Vertex;
typedef graph_traits<Graph>::edge_descriptor Edge;

class bacon_number_recorder : public default_bfs_visitor
{
public:
  bacon_number_recorder(int* dist) : d(dist) { }

  void tree_edge(Edge e, const Graph& g) const {
    Vertex u = source(e, g), v = target(e, g);
    d[v] = d[u] + 1;
  }
private:
  int* d;
};

int main()
{
  std::ifstream ifs("./kevin-bacon2.dat");
  if (!ifs) {
    std::cerr << "No ./kevin-bacon2.dat file" << std::endl;
    return EXIT_FAILURE;
  }
  archive::text_iarchive ia(ifs);
  Graph g;
  ia >> g;

  std::vector<int> bacon_number(num_vertices(g));

  // Get the vertex for Kevin Bacon
  Vertex src;
  graph_traits<Graph>::vertex_iterator i, end;
  for (boost::tie(i, end) = vertices(g); i != end; ++i)
    if (g[*i].name == "Kevin Bacon")
      src = *i;

  // Set Kevin's number to zero
  bacon_number[src] = 0;

  // Perform a breadth first search to compute everyone' Bacon number.
  breadth_first_search(g, src,
                       visitor(bacon_number_recorder(&bacon_number[0])));

  for (boost::tie(i, end) = vertices(g); i != end; ++i)
    std::cout << g[*i].name << " has a Bacon number of "
          << bacon_number[*i] << std::endl;

  return 0;
}


//#include <boost/test/unit_test.hpp>
//#include <iostream>
//
//#include "ssmkit/model/linear_gaussian.hpp"
//#include "ssmkit/distribution/gaussian.hpp"
//#include "ssmkit/distribution/conditional_distribution.hpp"
//#include "ssmkit/process/markov.hpp"
//#include "ssmkit/process/measurement.hpp"
//
//using namespace PROJECT_NAME;
//
//BOOST_AUTO_TEST_SUITE(process_measurement_test);
//
//BOOST_AUTO_TEST_CASE(test1) {
//  model::LinearGaussian<2, 2> f{{{1, 1}, {0, 1}}, {{0.0001, 0}, {0, 0.0001}}};
//  model::LinearGaussian<1, 2> h{{1, 0}, {0.0001}};
//  auto dynamic_cpdf =
//      distribution::makeParametericConditional(distribution::Gaussian<2>(), f);
//  auto measurement_cpdf =
//      distribution::makeParametericConditional(distribution::Gaussian<1>(), h);
//
//  auto markov_p = process::makeMarkov(dynamic_cpdf);
//
//  auto measurement_p = process::makeMeasurement(markov_p, measurement_cpdf);
//
//  std::cout << "a \n " << measurement_p.initialize(distribution::Gaussian<2>()) <<
//  std::endl;
//
//auto twin =  measurement_p.random();
//
//}
//
//BOOST_AUTO_TEST_SUITE_END();


// All content Copyright (C) 2018 Genomics plc
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include "caller/haplotypeLikelihoods.hpp"
#include "alignment/aligner.hpp"
#include "utils/matrix.hpp"

namespace wecall
{
namespace caller
{
    std::vector< double > computeHaplotypeFrequencies( const utils::matrix_t & haplotypeLikelihoods )
    {
        std::vector< double > haplotypeFrequencies( haplotypeLikelihoods.size2(), 0.0 );
        for ( std::size_t haplotypeIndex = 0; haplotypeIndex != haplotypeLikelihoods.size2(); ++haplotypeIndex )
        {
            const auto column = utils::matrixColumn_t( haplotypeLikelihoods, haplotypeIndex );
            const auto hapFreq = std::accumulate( column.begin(), column.end(), 0.0 );
            haplotypeFrequencies[haplotypeIndex] = hapFreq;
        }
        return haplotypeFrequencies;
    }

    utils::matrix_t computeHaplotypeLikelihoods( const variant::HaplotypeVector & haplotypes,
                                                 const io::RegionsReads & readRange )
    {
        const auto nReads = static_cast< std::size_t >( std::distance( readRange.begin(), readRange.end() ) );
        const auto haplotypeSeqStart = haplotypes.paddedReferenceSequence()->start();

        utils::matrix_t readScores( nReads, haplotypes.size() );

        for ( std::size_t haplotypeIndex = 0; haplotypeIndex < haplotypes.size(); ++haplotypeIndex )
        {
            const auto & paddedHaplotypeSequences = haplotypes[haplotypeIndex].paddedSequences();

            std::vector< mapping::HashMapper > hashMappers;
            std::vector< alignment::GAlign > aligners;
            for ( const auto & paddedHaplotypeSequence : paddedHaplotypeSequences )
            {
                hashMappers.emplace_back( paddedHaplotypeSequence, constants::needlemanWunschPadding,
                                          constants::needlemanWunschPadding );
                aligners.emplace_back(
                    paddedHaplotypeSequence, constants::gapExtendPenalty, constants::nucleotidePrior,
                    alignment::computeGapOpen( paddedHaplotypeSequence, errorModels::illuminaErrorModel ) );
            }

            std::size_t readIndex = 0;
            for ( const auto & read : readRange )
            {
                const auto hintPosition = read.getStartPos() - haplotypeSeqStart;
                double maxReadScore = 0.0;
                for ( std::size_t sequenceIndex = 0; sequenceIndex < paddedHaplotypeSequences.size(); ++sequenceIndex )
                {
                    maxReadScore = std::max(
                        maxReadScore, computeLikelihoodForReadAndHaplotype(
                                          read, hintPosition, hashMappers[sequenceIndex], aligners[sequenceIndex] ) );
                }

                readScores( readIndex, haplotypeIndex ) = maxReadScore;
                ++readIndex;
            }
        }

        int64_t maxMappingQual = 0L;
        for ( const auto & read : readRange )
        {
            maxMappingQual = std::max( maxMappingQual, read.getMappingQuality() );
        }

        const double maxDifference = stats::fromPhredQ( maxMappingQual );

        utils::smoothLowOutliers( readScores, maxDifference );

        if ( false )
        {
            const auto debugFunc = [&haplotypes, &readScores, &readRange]() -> std::string
            {
                std::stringstream debugMessage;
                debugMessage << "Haplotypes order:- ";
                for ( const auto & haplotype : haplotypes )
                {
                    debugMessage << std::endl
                                 << haplotype.toString();
                }

                debugMessage << std::endl;
                for ( std::size_t hap_index = 0; hap_index < readScores.size2(); ++hap_index )
                {
                    debugMessage << "\tHaplotype scores for : " << haplotypes[hap_index].toString() << std::endl;
                    for ( std::size_t read_index = 0; read_index < readScores.size1(); ++read_index )
                    {
                        debugMessage << readScores( read_index, hap_index ) << ", ";
                    }
                    debugMessage << std::endl;
                }
                return debugMessage.str();
            };
            WECALL_LOG( SUPER_DEBUG, debugFunc() );
        }

        return readScores;
    }
}
}


#include <boost/mpl/int.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/mpl/assert.hpp>
#include <boost/simd/sdk/memory/is_aligned.hpp>

using boost::simd::meta::is_aligned_c;

int main()
{
  BOOST_MPL_ASSERT(( is_aligned_c<0,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<2,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<4,2>::type ));
  BOOST_MPL_ASSERT(( is_aligned_c<8,2>::type ));
  BOOST_MPL_ASSERT_NOT(( is_aligned_c<17,2>::type ));
}


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2014 Benoit Steiner <benoit.steiner.goog@gmail.com>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#define EIGEN_USE_THREADS

#include "main.h"
#include <iostream>
#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

class TestAllocator : public Allocator {
 public:
  ~TestAllocator() override {}
  EIGEN_DEVICE_FUNC void* allocate(size_t num_bytes) const override {
    const_cast<TestAllocator*>(this)->alloc_count_++;
    return internal::aligned_malloc(num_bytes);
  }
  EIGEN_DEVICE_FUNC void deallocate(void* buffer) const override {
    const_cast<TestAllocator*>(this)->dealloc_count_++;
    internal::aligned_free(buffer);
  }

  int alloc_count() const { return alloc_count_; }
  int dealloc_count() const { return dealloc_count_; }

 private:
  int alloc_count_ = 0;
  int dealloc_count_ = 0;
};

void test_multithread_elementwise()
{
  Tensor<float, 3> in1(2,3,7);
  Tensor<float, 3> in2(2,3,7);
  Tensor<float, 3> out(2,3,7);

  in1.setRandom();
  in2.setRandom();

  Eigen::ThreadPool tp(internal::random<int>(3, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(3, 11));
  out.device(thread_pool_device) = in1 + in2 * 3.14f;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_APPROX(out(i,j,k), in1(i,j,k) + in2(i,j,k) * 3.14f);
      }
    }
  }
}

void test_multithread_compound_assignment()
{
  Tensor<float, 3> in1(2,3,7);
  Tensor<float, 3> in2(2,3,7);
  Tensor<float, 3> out(2,3,7);

  in1.setRandom();
  in2.setRandom();

  Eigen::ThreadPool tp(internal::random<int>(3, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(3, 11));
  out.device(thread_pool_device) = in1;
  out.device(thread_pool_device) += in2 * 3.14f;

  for (int i = 0; i < 2; ++i) {
    for (int j = 0; j < 3; ++j) {
      for (int k = 0; k < 7; ++k) {
        VERIFY_IS_APPROX(out(i,j,k), in1(i,j,k) + in2(i,j,k) * 3.14f);
      }
    }
  }
}

template<int DataLayout>
void test_multithread_contraction()
{
  Tensor<float, 4, DataLayout> t_left(30, 50, 37, 31);
  Tensor<float, 5, DataLayout> t_right(37, 31, 70, 2, 10);
  Tensor<float, 5, DataLayout> t_result(30, 50, 70, 2, 10);

  t_left.setRandom();
  t_right.setRandom();

  // this contraction should be equivalent to a single matrix multiplication
  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 2> dims({{DimPair(2, 0), DimPair(3, 1)}});

  typedef Map<Matrix<float, Dynamic, Dynamic, DataLayout>> MapXf;
  MapXf m_left(t_left.data(), 1500, 1147);
  MapXf m_right(t_right.data(), 1147, 1400);
  Matrix<float, Dynamic, Dynamic, DataLayout> m_result(1500, 1400);

  Eigen::ThreadPool tp(4);
  Eigen::ThreadPoolDevice thread_pool_device(&tp, 4);

  // compute results by separate methods
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  m_result = m_left * m_right;

 for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    VERIFY(&t_result.data()[i] != &m_result.data()[i]);
    if (fabsf(t_result(i) - m_result(i)) < 1e-4f) {
      continue;
    }
    if (Eigen::internal::isApprox(t_result(i), m_result(i), 1e-4f)) {
      continue;
    }
    std::cout << "mismatch detected at index " << i << ": " << t_result(i)
              << " vs " <<  m_result(i) << std::endl;
    assert(false);
  }
}

template<int DataLayout>
void test_contraction_corner_cases()
{
  Tensor<float, 2, DataLayout> t_left(32, 500);
  Tensor<float, 2, DataLayout> t_right(32, 28*28);
  Tensor<float, 2, DataLayout> t_result(500, 28*28);

  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result = t_result.constant(NAN);

  // this contraction should be equivalent to a single matrix multiplication
  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 1> dims{{DimPair(0, 0)}};

  typedef Map<Matrix<float, Dynamic, Dynamic, DataLayout>> MapXf;
  MapXf m_left(t_left.data(), 32, 500);
  MapXf m_right(t_right.data(), 32, 28*28);
  Matrix<float, Dynamic, Dynamic, DataLayout> m_result(500, 28*28);

  Eigen::ThreadPool tp(12);
  Eigen::ThreadPoolDevice thread_pool_device(&tp, 12);

  // compute results by separate methods
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  m_result = m_left.transpose() * m_right;

  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!(numext::isnan)(t_result.data()[i]));
    if (fabsf(t_result.data()[i] - m_result.data()[i]) >= 1e-4f) {
      std::cout << "mismatch detected at index " << i << " : " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 1);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_result.resize (1, 28*28);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 1);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!(numext::isnan)(t_result.data()[i]));
    if (fabsf(t_result.data()[i] - m_result.data()[i]) >= 1e-4f) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 500);
  t_right.resize(32, 4);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result.resize (500, 4);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 500);
  new(&m_right) MapXf(t_right.data(), 32, 4);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!(numext::isnan)(t_result.data()[i]));
    if (fabsf(t_result.data()[i] - m_result.data()[i]) >= 1e-4f) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }

  t_left.resize(32, 1);
  t_right.resize(32, 4);
  t_left = (t_left.constant(-0.5f) + t_left.random()) * 2.0f;
  t_right = (t_right.constant(-0.6f) + t_right.random()) * 2.0f;
  t_result.resize (1, 4);
  t_result = t_result.constant(NAN);
  t_result.device(thread_pool_device) = t_left.contract(t_right, dims);
  new(&m_left) MapXf(t_left.data(), 32, 1);
  new(&m_right) MapXf(t_right.data(), 32, 4);
  m_result = m_left.transpose() * m_right;
  for (ptrdiff_t i = 0; i < t_result.size(); i++) {
    assert(!(numext::isnan)(t_result.data()[i]));
    if (fabsf(t_result.data()[i] - m_result.data()[i]) >= 1e-4f) {
      std::cout << "mismatch detected: " << t_result.data()[i] << " vs " <<  m_result.data()[i] << std::endl;
      assert(false);
    }
  }
}

template<int DataLayout>
void test_multithread_contraction_agrees_with_singlethread() {
  int contract_size = internal::random<int>(1, 5000);

  Tensor<float, 3, DataLayout> left(internal::random<int>(1, 80),
                                    contract_size,
                                    internal::random<int>(1, 100));

  Tensor<float, 4, DataLayout> right(internal::random<int>(1, 25),
                                     internal::random<int>(1, 37),
                                     contract_size,
                                     internal::random<int>(1, 51));

  left.setRandom();
  right.setRandom();

  // add constants to shift values away from 0 for more precision
  left += left.constant(1.5f);
  right += right.constant(1.5f);

  typedef Tensor<float, 1>::DimensionPair DimPair;
  Eigen::array<DimPair, 1> dims({{DimPair(1, 2)}});

  Eigen::ThreadPool tp(internal::random<int>(2, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(2, 11));

  Tensor<float, 5, DataLayout> st_result;
  st_result = left.contract(right, dims);

  Tensor<float, 5, DataLayout> tp_result(st_result.dimensions());
  tp_result.device(thread_pool_device) = left.contract(right, dims);

  VERIFY(dimensions_match(st_result.dimensions(), tp_result.dimensions()));
  for (ptrdiff_t i = 0; i < st_result.size(); i++) {
    // if both of the values are very small, then do nothing (because the test will fail
    // due to numerical precision issues when values are small)
    if (numext::abs(st_result.data()[i] - tp_result.data()[i]) >= 1e-4f) {
      VERIFY_IS_APPROX(st_result.data()[i], tp_result.data()[i]);
    }
  }
}

// Apply Sqrt to all output elements.
struct SqrtOutputKernel {
  template <typename Index, typename Scalar>
  EIGEN_ALWAYS_INLINE void operator()(
      const internal::blas_data_mapper<Scalar, Index, ColMajor>& output_mapper,
      const TensorContractionParams&, Index, Index, Index num_rows,
      Index num_cols) const {
    for (int i = 0; i < num_rows; ++i) {
      for (int j = 0; j < num_cols; ++j) {
        output_mapper(i, j) = std::sqrt(output_mapper(i, j));
      }
    }
  }
};

template <int DataLayout>
static void test_multithread_contraction_with_output_kernel() {
  typedef Tensor<float, 1>::DimensionPair DimPair;

  const int num_threads = internal::random<int>(2, 11);
  ThreadPool threads(num_threads);
  Eigen::ThreadPoolDevice device(&threads, num_threads);

  Tensor<float, 4, DataLayout> t_left(30, 50, 8, 31);
  Tensor<float, 5, DataLayout> t_right(8, 31, 7, 20, 10);
  Tensor<float, 5, DataLayout> t_result(30, 50, 7, 20, 10);

  t_left.setRandom();
  t_right.setRandom();
  // Put trash in mat4 to verify contraction clears output memory.
  t_result.setRandom();

  // Add a little offset so that the results won't be close to zero.
  t_left += t_left.constant(1.0f);
  t_right += t_right.constant(1.0f);

  typedef Map<Eigen::Matrix<float, Dynamic, Dynamic, DataLayout>> MapXf;
  MapXf m_left(t_left.data(), 1500, 248);
  MapXf m_right(t_right.data(), 248, 1400);
  Eigen::Matrix<float, Dynamic, Dynamic, DataLayout> m_result(1500, 1400);

  // this contraction should be equivalent to a single matrix multiplication
  Eigen::array<DimPair, 2> dims({{DimPair(2, 0), DimPair(3, 1)}});

  // compute results by separate methods
  t_result.device(device) = t_left.contract(t_right, dims, SqrtOutputKernel());

  m_result = m_left * m_right;

  for (Index i = 0; i < t_result.dimensions().TotalSize(); i++) {
    VERIFY(&t_result.data()[i] != &m_result.data()[i]);
    VERIFY_IS_APPROX(t_result.data()[i], std::sqrt(m_result.data()[i]));
  }
}

template<int DataLayout>
void test_full_contraction() {
  int contract_size1 = internal::random<int>(1, 500);
  int contract_size2 = internal::random<int>(1, 500);

  Tensor<float, 2, DataLayout> left(contract_size1,
                                    contract_size2);
  Tensor<float, 2, DataLayout> right(contract_size1,
                                    contract_size2);
  left.setRandom();
  right.setRandom();

  // add constants to shift values away from 0 for more precision
  left += left.constant(1.5f);
  right += right.constant(1.5f);

  typedef Tensor<float, 2>::DimensionPair DimPair;
  Eigen::array<DimPair, 2> dims({{DimPair(0, 0), DimPair(1, 1)}});

  Eigen::ThreadPool tp(internal::random<int>(2, 11));
  Eigen::ThreadPoolDevice thread_pool_device(&tp, internal::random<int>(2, 11));

  Tensor<float, 0, DataLayout> st_result;
  st_result = left.contract(right, dims);

  Tensor<float, 0, DataLayout> tp_result;
  tp_result.device(thread_pool_device) = left.contract(right, dims);

  VERIFY(dimensions_match(st_result.dimensions(), tp_result.dimensions()));
  // if both of the values are very small, then do nothing (because the test will fail
  // due to numerical precision issues when values are small)
  if (numext::abs(st_result() - tp_result()) >= 1e-4f) {
    VERIFY_IS_APPROX(st_result(), tp_result());
  }
}

template<int DataLayout>
void test_multithreaded_reductions() {
  const int num_threads = internal::random<int>(3, 11);
  ThreadPool thread_pool(num_threads);
  Eigen::ThreadPoolDevice thread_pool_device(&thread_pool, num_threads);

  const int num_rows = internal::random<int>(13, 732);
  const int num_cols = internal::random<int>(13, 732);
  Tensor<float, 2, DataLayout> t1(num_rows, num_cols);
  t1.setRandom();

  Tensor<float, 0, DataLayout> full_redux;
  full_redux = t1.sum();

  Tensor<float, 0, DataLayout> full_redux_tp;
  full_redux_tp.device(thread_pool_device) = t1.sum();

  // Check that the single threaded and the multi threaded reductions return
  // the same result.
  VERIFY_IS_APPROX(full_redux(), full_redux_tp());
}

void test_memcpy() {

  for (int i = 0; i < 5; ++i) {
    const int num_threads = internal::random<int>(3, 11);
    Eigen::ThreadPool tp(num_threads);
    Eigen::ThreadPoolDevice thread_pool_device(&tp, num_threads);

    const int size = internal::random<int>(13, 7632);
    Tensor<float, 1> t1(size);
    t1.setRandom();
    std::vector<float> result(size);
    thread_pool_device.memcpy(&result[0], t1.data(), size*sizeof(float));
    for (int j = 0; j < size; j++) {
      VERIFY_IS_EQUAL(t1(j), result[j]);
    }
  }
}

void test_multithread_random()
{
  Eigen::ThreadPool tp(2);
  Eigen::ThreadPoolDevice device(&tp, 2);
  Tensor<float, 1> t(1 << 20);
  t.device(device) = t.random<Eigen::internal::NormalRandomGenerator<float>>();
}

template<int DataLayout>
void test_multithread_shuffle(Allocator* allocator)
{
  Tensor<float, 4, DataLayout> tensor(17,5,7,11);
  tensor.setRandom();

  const int num_threads = internal::random<int>(2, 11);
  ThreadPool threads(num_threads);
  Eigen::ThreadPoolDevice device(&threads, num_threads, allocator);

  Tensor<float, 4, DataLayout> shuffle(7,5,11,17);
  array<ptrdiff_t, 4> shuffles = {{2,1,3,0}};
  shuffle.device(device) = tensor.shuffle(shuffles);

  for (int i = 0; i < 17; ++i) {
    for (int j = 0; j < 5; ++j) {
      for (int k = 0; k < 7; ++k) {
        for (int l = 0; l < 11; ++l) {
          VERIFY_IS_EQUAL(tensor(i,j,k,l), shuffle(k,j,l,i));
        }
      }
    }
  }
}

void test_threadpool_allocate(TestAllocator* allocator)
{
  const int num_threads = internal::random<int>(2, 11);
  const int num_allocs = internal::random<int>(2, 11);
  ThreadPool threads(num_threads);
  Eigen::ThreadPoolDevice device(&threads, num_threads, allocator);

  for (int a = 0; a < num_allocs; ++a) {
    void* ptr = device.allocate(512);
    device.deallocate(ptr);
  }
  VERIFY(allocator != NULL);
  VERIFY_IS_EQUAL(allocator->alloc_count(), num_allocs);
  VERIFY_IS_EQUAL(allocator->dealloc_count(), num_allocs);
}

EIGEN_DECLARE_TEST(cxx11_tensor_thread_pool)
{
  CALL_SUBTEST_1(test_multithread_elementwise());
  CALL_SUBTEST_1(test_multithread_compound_assignment());

  CALL_SUBTEST_2(test_multithread_contraction<ColMajor>());
  CALL_SUBTEST_2(test_multithread_contraction<RowMajor>());

  CALL_SUBTEST_3(test_multithread_contraction_agrees_with_singlethread<ColMajor>());
  CALL_SUBTEST_3(test_multithread_contraction_agrees_with_singlethread<RowMajor>());
  CALL_SUBTEST_3(test_multithread_contraction_with_output_kernel<ColMajor>());
  CALL_SUBTEST_3(test_multithread_contraction_with_output_kernel<RowMajor>());

  // Exercise various cases that have been problematic in the past.
  CALL_SUBTEST_4(test_contraction_corner_cases<ColMajor>());
  CALL_SUBTEST_4(test_contraction_corner_cases<RowMajor>());

  CALL_SUBTEST_4(test_full_contraction<ColMajor>());
  CALL_SUBTEST_4(test_full_contraction<RowMajor>());

  CALL_SUBTEST_5(test_multithreaded_reductions<ColMajor>());
  CALL_SUBTEST_5(test_multithreaded_reductions<RowMajor>());

  CALL_SUBTEST_6(test_memcpy());
  CALL_SUBTEST_6(test_multithread_random());

  TestAllocator test_allocator;
  CALL_SUBTEST_6(test_multithread_shuffle<ColMajor>(NULL));
  CALL_SUBTEST_6(test_multithread_shuffle<RowMajor>(&test_allocator));
  CALL_SUBTEST_6(test_threadpool_allocate(&test_allocator));
}


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators/mean.hpp>
#include <boost/histogram/accumulators/ostream.hpp>
#include <boost/histogram/weight.hpp>
#include <sstream>
#include "is_close.hpp"
#include "throw_exception.hpp"
#include "utility_str.hpp"

using namespace boost::histogram;
using namespace std::literals;

int main() {
  using m_t = accumulators::mean<double>;
  m_t a;
  BOOST_TEST_EQ(a.count(), 0);
  BOOST_TEST_EQ(a, m_t{});

  a(4);
  a(7);
  a(13);
  a(16);

  BOOST_TEST_EQ(a.count(), 4);
  BOOST_TEST_EQ(a.value(), 10);
  BOOST_TEST_EQ(a.variance(), 30);

  BOOST_TEST_EQ(str(a), "mean(4, 10, 30)"s);
  BOOST_TEST_EQ(str(a, 20, false), "     mean(4, 10, 30)"s);
  BOOST_TEST_EQ(str(a, 20, true), "mean(4, 10, 30)     "s);

  m_t b;
  b(1e8 + 4);
  b(1e8 + 7);
  b(1e8 + 13);
  b(1e8 + 16);

  BOOST_TEST_EQ(b.count(), 4);
  BOOST_TEST_EQ(b.value(), 1e8 + 10);
  BOOST_TEST_EQ(b.variance(), 30);

  auto c = a;
  c += a; // same as feeding all samples twice

  BOOST_TEST_EQ(c.count(), 8);
  BOOST_TEST_EQ(c.value(), 10);
  BOOST_TEST_IS_CLOSE(c.variance(), 25.714, 1e-3);

  // also same as feeding all samples twice
  m_t d;
  d(weight(2), 4);
  d(weight(2), 7);
  d(weight(2), 13);
  d(weight(2), 16);

  BOOST_TEST_EQ(d, c);

  BOOST_TEST_EQ(m_t() += m_t(), m_t());
  BOOST_TEST_EQ(m_t(1, 2, 3) += m_t(), m_t(1, 2, 3));
  BOOST_TEST_EQ(m_t() += m_t(1, 2, 3), m_t(1, 2, 3));

  return boost::report_errors();
}


#include "utils.hpp"

#include <regex>

#include <fmt/format.h>

#include <boost/algorithm/string.hpp>

#include <render_pipeline/rpcore/rpobject.hpp>

static std::vector<std::string> split(const std::string& src, const std::regex& regex)
{
    return { std::sregex_token_iterator(src.begin(), src.end(), regex, -1), std::sregex_token_iterator() };
}

LMatrix4f parse_to_mat4(const std::string& str, const LMatrix4f& fallback)
{
    auto floats = split(boost::algorithm::trim_copy(str), std::regex(" "));
    if (floats.size() != LMatrix4f::num_components)
    {
        rpcore::RPObject::global_error("World", fmt::format("mat4 Size is not 16: {}", str));
        return fallback;
    }

    LMatrix4f result;
    try
    {
        for (size_t k = 0; k < LMatrix4f::num_components; ++k)
            (&result(0, 0))[k] = std::stof(floats[k]);
    }
    catch (const std::exception& err)
    {
        rpcore::RPObject::global_error("World", fmt::format("Failed to parse mat4: ", str));
        rpcore::RPObject::global_error("World", fmt::format("Error: ", err.what()));
        return fallback;
    }

    return result;
}


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

#ifndef UTILVEC_HPP
#define UTILVEC_HPP

#include <vector>

#include <Eigen/Core>

#include <ippp/types.h>

namespace ippp {
namespace util {

/*!
*  \brief      Check whether the vector is empty (contains a NaN value)
*  \author     Sascha Kaden
*  \param[in]  Vector
*  \param[out] result, true if vector is empty
*  \date       2017-04-04
*/
static bool empty(const Vector3 &vec) {
    bool status = false;
    for (unsigned int i = 0; i < 3; ++i) {
        if (std::isnan(vec[i])) {
            status = true;
            break;
        }
    }
    return status;
}

/*!
*  \brief      Check whether the vector is empty (contains a NaN value)
*  \author     Sascha Kaden
*  \param[in]  Vector
*  \param[out] result, true if vector is empty
*  \date       2017-04-04
*/
template <unsigned int dim>
bool empty(const Vector<dim> &vec) {
    bool status = false;
    for (unsigned int i = 0; i < dim; ++i) {
        if (std::isnan(vec[i])) {
            status = true;
            break;
        }
    }
    return status;
}

/*!
*  \brief      Appends the second Vector to the first one and returns the result Vector.
*  \author     Sascha Kaden
*  \param[in]  first Vector
*  \param[in]  second Vector
*  \param[out] result Vector
*  \date       2016-12-23
*/
template <unsigned int dim1, unsigned int dim2>
Vector<dim1 + dim2> append(const Vector<dim1> &first, const Vector<dim2> &second) {
    Vector<dim1 + dim2> vec;
    for (unsigned int i = 0; i < dim1; ++i)
        vec[i] = first[i];

    for (unsigned int i = 0; i < dim2; ++i)
        vec[dim1 + i] = second[i];

    return vec;
}

/*!
*  \brief      Appends the value to the first one and returns the result Vector.
*  \author     Sascha Kaden
*  \param[in]  first Vector
*  \param[in]  value
*  \param[out] result Vector
*  \date       2016-12-23
*/
template <unsigned int dim>
Vector<dim + 1> append(const Vector<dim> &source, const double add) {
    Vector<dim + 1> vec;
    for (unsigned int i = 0; i < dim; ++i)
        vec[i] = source[i];

    vec[dim] = add;
    return vec;
}

/*!
*  \brief      Multiplicate two vectors and return the result.
*  \author     Sascha Kaden
*  \param[in]  first Vector
*  \param[in]  second vector
*  \param[out] result Vector
*  \date       2017-06-20
*/
template <unsigned int dim>
Vector<dim> multiplyElementWise(const Vector<dim> &source, const Vector<dim> &mask) {
    Vector<dim> result(source);
    for (unsigned int i = 0; i < dim; ++i)
        result[i] *= mask[i];
    return result;
}

/*!
*  \brief      Splits the Vector in separate Vectors with the sizes of the passed list.
*  \author     Sascha Kaden
*  \param[in]  main Vector
*  \param[in]  splitting sequence
*  \param[out] separate result Vectors
*  \date       2017-06-20
*/
template <unsigned int dim>
std::vector<VectorX> splitVec(const Vector<dim> vec, const std::vector<unsigned int> &dofs) {
    std::vector<VectorX> vecs;
    vecs.reserve(dofs.size());
    unsigned int count = 0;
    for (auto &dof : dofs) {
        VectorX temp(dof);
        for (unsigned int i = 0; i < dof; ++i, ++count) {
            temp[i] = vec[count];
        }
        vecs.push_back(temp);
    }
    return vecs;
}

/*!
*  \brief      Creates a 5 dimensional Vector with passed values
*  \author     Sascha Kaden
*  \param[in]  x
*  \param[in]  y
*  \param[in]  z
*  \param[in]  rx
*  \param[in]  ry
*  \param[out] result Vector
*  \date       2016-12-23
*/
static Vector5 Vecd(double x, double y, double z, double rx, double ry) {
    Vector5 vec;
    vec << x, y, z, rx, ry;
    return vec;
}

/*!
*  \brief      Creates a 6 dimensional Vector with passed values
*  \author     Sascha Kaden
*  \param[in]  x
*  \param[in]  y
*  \param[in]  z
*  \param[in]  rx
*  \param[in]  ry
*  \param[in]  rz
*  \param[out] result Vector
*  \date       2016-12-23
*/
static Vector6 Vecd(double x, double y, double z, double rx, double ry, double rz) {
    Vector6 vec;
    vec << x, y, z, rx, ry, rz;
    return vec;
}

/*!
*  \brief      Creates a Vector with template dimension and all elements are set to the passed value
*  \author     Sascha Kaden
*  \param[in]  value
*  \param[out] result Vector
*  \date       2016-12-23
*/
template <unsigned int dim>
Vector<dim> Vecd(const double data) {
    Vector<dim> vec;
    for (unsigned int i = 0; i < dim; ++i) {
        vec[i] = data;
    }
    return vec;
}

/*!
*  \brief      Creates a Vector with passed array (dimension by template parameter)
*  \author     Sascha Kaden
*  \param[in]  array
*  \param[out] result Vector
*  \date       2016-12-23
*/
template <unsigned int dim>
Vector<dim> Vecd(const double data[]) {
    Vector<dim> vec;
    for (unsigned int i = 0; i < dim; ++i) {
        vec[i] = data[i];
    }
    return vec;
}

/*!
*  \brief      Creates a Vector with passed dimension and array
*  \author     Sascha Kaden
*  \param[in]  dim
*  \param[in]  array
*  \param[out] result Vector
*  \date       2016-12-23
*/
static VectorX Vecd(const unsigned int dim, const double data[]) {
    VectorX vec(dim);
    for (unsigned int i = 0; i < dim; ++i) {
        vec[i] = data[i];
    }
    return vec;
}

/*!
*  \brief      Creates a NaN Vector
*  \author     Sascha Kaden
*  \param[out] result NaN Vector
*  \date       2017-04-04
*/
template <unsigned int dim>
Vector<dim> NaNVector() {
    Vector<dim> vec;
    for (unsigned int i = 0; i < dim; ++i) {
        vec[i] = std::nanf("1");
    }
    return vec;
}

template<unsigned int N>
static bool equal(const Vector<N> &vec1, const Vector<N> &vec2) {
	return (vec1 - vec2).isZero(EPSILON);
}

} /* namespace util */
} /* namespace ippp */

#endif    // UTILVEC_HPP


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<boost::int64_t, 2, 4> test_long_2_4(const Eigen::Matrix<boost::int64_t, 2, 4> & M)
{
	return M;
}
void export_long_2_4()
{
	boost::python::def("test_long_2_4",test_long_2_4);
}


/*
  [auto_generated]
  boost/numeric/odeint/iterator/detail/adaptive_iterator_dense_output_impl.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

  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_NUMERIC_ODEINT_ITERATOR_DETAIL_ADAPTIVE_ITERATOR_DENSE_OUTPUT_IMPL_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_ITERATOR_DETAIL_ADAPTIVE_ITERATOR_DENSE_OUTPUT_IMPL_HPP_DEFINED

#include <boost/numeric/odeint/util/unit_helper.hpp>
#include <boost/numeric/odeint/iterator/detail/ode_iterator_base.hpp>

namespace boost {
namespace numeric {
namespace odeint {

    /*
     * Specilization for dense outputer steppers
     */
    /**
     * \brief ODE Iterator with adaptive step size control. The value type of this iterator is the state type of the stepper.
     *
     * Implements an ODE iterator with adaptive step size control. Uses dense-output steppers. adaptive_iterator is a model
     * of single-pass iterator.
     *
     * The value type of this iterator is the state type of the stepper. Hence one can only access the state and not the current time.
     *
     * \tparam Stepper The stepper type which should be used during the iteration.
     * \tparam System The type of the system function (ODE) which should be solved.
     */
    template< class Stepper , class System >
    class adaptive_iterator< Stepper , System , dense_output_stepper_tag > : public detail::ode_iterator_base
    <
        adaptive_iterator< Stepper , System , dense_output_stepper_tag > ,
        Stepper , System
    >
    {
    private:

        typedef Stepper stepper_type;
        typedef System system_type;
        typedef typename boost::numeric::odeint::unwrap_reference< stepper_type >::type unwrapped_stepper_type;
        typedef typename traits::state_type< stepper_type >::type state_type;
        typedef typename traits::time_type< stepper_type >::type time_type;
        typedef typename traits::value_type< stepper_type >::type ode_value_type;
        #ifndef DOXYGEN_SKIP
        typedef detail::ode_iterator_base<
            adaptive_iterator< Stepper , System , dense_output_stepper_tag > ,
            Stepper , System
            > base_type;
        #endif

   public:

        /**
         * \brief Constructs an adaptive_iterator. This constructor should be used to construct the begin iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state.
         * \param t The initial time.
         * \param t_end The end time, at which the iteration should stop.
         * \param dt The initial time step.
         */
        adaptive_iterator( stepper_type stepper , system_type sys , state_type &s , time_type t , time_type t_end , time_type dt )
            : base_type( stepper , sys , s , t , t_end , dt )
        {
            unwrapped_stepper_type &st = this->m_stepper;
            st.initialize( *( this->m_state ) , this->m_t , this->m_dt );
        }

        /**
         * \brief Constructs an adaptive_iterator. This constructor should be used to construct the end iterator.
         *
         * \param stepper The stepper to use during the iteration.
         * \param sys The system function (ODE) to solve.
         * \param s The initial state.
         */
        adaptive_iterator( stepper_type stepper , system_type sys , state_type &s )
            : base_type( stepper , sys , s ) { }

    protected:

        friend class boost::iterator_core_access;

        void increment()
        {
            unwrapped_stepper_type &stepper = this->m_stepper;
            stepper.do_step( this->m_system );
            this->m_t = stepper.current_time();
            this->check_end();
        }

        // overwrite dereference from ode_iterator_base
        const state_type& dereference() const
        {
            const unwrapped_stepper_type &stepper = this->m_stepper;
            return stepper.current_state();
        }

    };

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_ITERATOR_DETAIL_ADAPTIVE_ITERATOR_DENSE_OUTPUT_IMPL_HPP_DEFINED


/* vim: set tabstop=4 expandtab shiftwidth=4 softtabstop=4: */

/**
 * \file libs/numeric/ublasx/test/cat.cpp
 *
 * \brief Tests for the family of \c cat operations.
 *
 * \author Marco Guazzone (marco.guazzone@gmail.com)
 *
 * <hr/>
 *
 * Copyright (c) 2010, Marco Guazzone
 *
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompwhiching file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 */

#include <algorithm>
#include <boost/numeric/ublas/matrix_expression.hpp>
#include <boost/numeric/ublas/fwd.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublasx/operation/cat.hpp>
#include <boost/numeric/ublasx/operation/num_columns.hpp>
#include <boost/numeric/ublasx/operation/num_rows.hpp>
#include <cstddef>
#include "libs/numeric/ublasx/test/utils.hpp"
#include <iostream>

namespace ublas = boost::numeric::ublas;
namespace ublasx = boost::numeric::ublasx;

static const double tol = 1.0e-5;

BOOST_UBLASX_TEST_DEF( test_rows_dense_matrix_column_major_same_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Rows - Dense Matrix - Column Major - Same Rows Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::column_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nc = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(A_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10;
    B(1,0) = 11; B(1,1) = 12; B(1,2) = 13; B(1,3) = 14;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr, A_nc+B_nc);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) = B(0,0); expect_R(0,4) = B(0,1); expect_R(0,5) = B(0,2); expect_R(0,6) = B(0,3);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) = B(1,0); expect_R(1,4) = B(1,1); expect_R(1,5) = B(1,2); expect_R(1,6) = B(1,3);

    R = ublasx::cat_rows(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_rows(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_rows(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_rows_dense_matrix_row_major_same_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Rows - Dense Matrix - Row Major - Same Rows Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::row_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nc = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(A_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10;
    B(1,0) = 11; B(1,1) = 12; B(1,2) = 13; B(1,3) = 14;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr, A_nc+B_nc);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) = B(0,0); expect_R(0,4) = B(0,1); expect_R(0,5) = B(0,2); expect_R(0,6) = B(0,3);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) = B(1,0); expect_R(1,4) = B(1,1); expect_R(1,5) = B(1,2); expect_R(1,6) = B(1,3);

    R = ublasx::cat_rows(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_rows(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_rows(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_rows_dense_matrix_column_major_diff_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Rows - Dense Matrix - Column Major - Different Rows Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::column_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 3;
    const size_type B_nc = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10;
    B(1,0) = 11; B(1,1) = 12; B(1,2) = 13; B(1,3) = 14;
    B(2,0) = 15; B(2,1) = 16; B(2,2) = 17; B(2,3) = 18;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(std::max(A_nr,B_nr), A_nc+B_nc);

    // cat_columns(A,B)
    R = ublasx::cat_rows(A, B);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) = B(0,0); expect_R(0,4) = B(0,1); expect_R(0,5) = B(0,2); expect_R(0,6) = B(0,3);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) = B(1,0); expect_R(1,4) = B(1,1); expect_R(1,5) = B(1,2); expect_R(1,6) = B(1,3);
    expect_R(2,0) =      0; expect_R(2,1) =      0; expect_R(2,2) =      0; expect_R(2,3) = B(2,0); expect_R(2,4) = B(2,1); expect_R(2,5) = B(2,2); expect_R(2,6) = B(2,3);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_rows(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_rows(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_rows_dense_matrix_row_major_diff_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Rows - Dense Matrix - Row Major - Different Rows Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::row_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 3;
    const size_type B_nc = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10;
    B(1,0) = 11; B(1,1) = 12; B(1,2) = 13; B(1,3) = 14;
    B(2,0) = 15; B(2,1) = 16; B(2,2) = 17; B(2,3) = 18;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(std::max(A_nr,B_nr), A_nc+B_nc);

    // cat_columns(A,B)
    R = ublasx::cat_rows(A, B);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) = B(0,0); expect_R(0,4) = B(0,1); expect_R(0,5) = B(0,2); expect_R(0,6) = B(0,3);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) = B(1,0); expect_R(1,4) = B(1,1); expect_R(1,5) = B(1,2); expect_R(1,6) = B(1,3);
    expect_R(2,0) =      0; expect_R(2,1) =      0; expect_R(2,2) =      0; expect_R(2,3) = B(2,0); expect_R(2,4) = B(2,1); expect_R(2,5) = B(2,2); expect_R(2,6) = B(2,3);
    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_rows(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_rows(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_columns_dense_matrix_column_major_same_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Columns - Dense Matrix - Row Major - Same Columns Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::column_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,A_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9;
    B(1,0) = 10; B(1,1) = 11; B(1,2) = 12;
    B(2,0) = 13; B(2,1) = 14; B(2,2) = 15;
    B(3,0) = 16; B(3,1) = 17; B(3,2) = 18;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr+B_nr, A_nc);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2);
    expect_R(2,0) = B(0,0); expect_R(2,1) = B(0,1); expect_R(2,2) = B(0,2);
    expect_R(3,0) = B(1,0); expect_R(3,1) = B(1,1); expect_R(3,2) = B(1,2);
    expect_R(4,0) = B(2,0); expect_R(4,1) = B(2,1); expect_R(4,2) = B(2,2);
    expect_R(5,0) = B(3,0); expect_R(5,1) = B(3,1); expect_R(5,2) = B(3,2);

    R = ublasx::cat_columns(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_columns(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_columns(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_columns_dense_matrix_column_major_diff_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Columns - Dense Matrix - Row Major - Different Columns Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::column_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 4;
    const size_type B_nc = 5;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10; B(0,4) = 11;
    B(1,0) = 12; B(1,1) = 13; B(1,2) = 14; B(1,3) = 15; B(1,4) = 16;
    B(2,0) = 17; B(2,1) = 18; B(2,2) = 19; B(2,3) = 20; B(2,4) = 21;
    B(3,0) = 22; B(3,1) = 23; B(3,2) = 24; B(3,3) = 25; B(3,4) = 26;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr+B_nr, std::max(A_nc,B_nc));
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) =      0; expect_R(0,4) =      0;
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) =      0; expect_R(1,4) =      0;
    expect_R(2,0) = B(0,0); expect_R(2,1) = B(0,1); expect_R(2,2) = B(0,2); expect_R(2,3) = B(0,3); expect_R(2,4) = B(0,4);
    expect_R(3,0) = B(1,0); expect_R(3,1) = B(1,1); expect_R(3,2) = B(1,2); expect_R(3,3) = B(1,3); expect_R(3,4) = B(1,4);
    expect_R(4,0) = B(2,0); expect_R(4,1) = B(2,1); expect_R(4,2) = B(2,2); expect_R(4,3) = B(2,3); expect_R(4,4) = B(2,4);
    expect_R(5,0) = B(3,0); expect_R(5,1) = B(3,1); expect_R(5,2) = B(3,2); expect_R(5,3) = B(3,3); expect_R(5,4) = B(3,4);

    R = ublasx::cat_columns(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_columns(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_columns(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_columns_dense_matrix_row_major_same_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Columns - Dense Matrix - Row Major - Same Columns Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::row_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 4;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,A_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9;
    B(1,0) = 10; B(1,1) = 11; B(1,2) = 12;
    B(2,0) = 13; B(2,1) = 14; B(2,2) = 15;
    B(3,0) = 16; B(3,1) = 17; B(3,2) = 18;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr+B_nr, A_nc);
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2);
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2);
    expect_R(2,0) = B(0,0); expect_R(2,1) = B(0,1); expect_R(2,2) = B(0,2);
    expect_R(3,0) = B(1,0); expect_R(3,1) = B(1,1); expect_R(3,2) = B(1,2);
    expect_R(4,0) = B(2,0); expect_R(4,1) = B(2,1); expect_R(4,2) = B(2,2);
    expect_R(5,0) = B(3,0); expect_R(5,1) = B(3,1); expect_R(5,2) = B(3,2);

    R = ublasx::cat_columns(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_columns(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_columns(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

BOOST_UBLASX_TEST_DEF( test_columns_dense_matrix_row_major_diff_dim )
{
    BOOST_UBLASX_DEBUG_TRACE( "Test Case: Concatenate Columns - Dense Matrix - Row Major - Different Columns Number" );

    typedef double value_type;
    typedef std::size_t size_type;
    typedef ublas::matrix<value_type, ublas::row_major> matrix_type;

    const size_type A_nr = 2;
    const size_type A_nc = 3;
    const size_type B_nr = 4;
    const size_type B_nc = 5;

    matrix_type A(A_nr,A_nc);
    A(0,0) = 1; A(0,1) = 2; A(0,2) = 3;
    A(1,0) = 4; A(1,1) = 5; A(1,2) = 6;

    matrix_type B(B_nr,B_nc);
    B(0,0) =  7; B(0,1) =  8; B(0,2) =  9; B(0,3) = 10; B(0,4) = 11;
    B(1,0) = 12; B(1,1) = 13; B(1,2) = 14; B(1,3) = 15; B(1,4) = 16;
    B(2,0) = 18; B(2,1) = 19; B(2,2) = 20; B(2,3) = 21; B(2,4) = 22;
    B(3,0) = 23; B(3,1) = 24; B(3,2) = 25; B(3,3) = 26; B(3,4) = 27;

    matrix_type R;
    matrix_type expect_R;

    expect_R = matrix_type(A_nr+B_nr, std::max(A_nc,B_nc));
    expect_R(0,0) = A(0,0); expect_R(0,1) = A(0,1); expect_R(0,2) = A(0,2); expect_R(0,3) =      0; expect_R(0,4) =      0;
    expect_R(1,0) = A(1,0); expect_R(1,1) = A(1,1); expect_R(1,2) = A(1,2); expect_R(1,3) =      0; expect_R(1,4) =      0;
    expect_R(2,0) = B(0,0); expect_R(2,1) = B(0,1); expect_R(2,2) = B(0,2); expect_R(2,3) = B(0,3); expect_R(2,4) = B(0,4);
    expect_R(3,0) = B(1,0); expect_R(3,1) = B(1,1); expect_R(3,2) = B(1,2); expect_R(3,3) = B(1,3); expect_R(3,4) = B(1,4);
    expect_R(4,0) = B(2,0); expect_R(4,1) = B(2,1); expect_R(4,2) = B(2,2); expect_R(4,3) = B(2,3); expect_R(4,4) = B(2,4);
    expect_R(5,0) = B(3,0); expect_R(5,1) = B(3,1); expect_R(5,2) = B(3,2); expect_R(5,3) = B(3,3); expect_R(5,4) = B(3,4);

    R = ublasx::cat_columns(A, B);

    BOOST_UBLASX_DEBUG_TRACE( "A = " << A );
    BOOST_UBLASX_DEBUG_TRACE( "B = " << B );
    BOOST_UBLASX_DEBUG_TRACE( "cat_columns(A,B) = " << R );
    BOOST_UBLASX_DEBUG_TRACE( "expect cat_columns(A,B) = " << expect_R );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_rows(R) == ublasx::num_rows(expect_R) );
    BOOST_UBLASX_TEST_CHECK( ublasx::num_columns(R) == ublasx::num_columns(expect_R) );
    BOOST_UBLASX_TEST_CHECK_MATRIX_CLOSE( R, expect_R, ublasx::num_rows(expect_R), ublasx::num_columns(expect_R), tol );
}

int main()
{
    BOOST_UBLASX_DEBUG_TRACE("Test Suite: 'cat' operations");

    BOOST_UBLASX_TEST_BEGIN();

    BOOST_UBLASX_TEST_DO( test_columns_dense_matrix_column_major_same_dim );
    BOOST_UBLASX_TEST_DO( test_columns_dense_matrix_row_major_same_dim );
    BOOST_UBLASX_TEST_DO( test_columns_dense_matrix_column_major_diff_dim );
    BOOST_UBLASX_TEST_DO( test_columns_dense_matrix_row_major_diff_dim );
    BOOST_UBLASX_TEST_DO( test_rows_dense_matrix_column_major_same_dim );
    BOOST_UBLASX_TEST_DO( test_rows_dense_matrix_row_major_same_dim );
    BOOST_UBLASX_TEST_DO( test_rows_dense_matrix_column_major_diff_dim );
    BOOST_UBLASX_TEST_DO( test_rows_dense_matrix_row_major_diff_dim );

    BOOST_UBLASX_TEST_END();
}


#pragma once
#include "Core/Common.hpp"
#include "HashDAG.hpp"
#include "Color.hpp"

#include <openvdb/openvdb.h>
#include <Eigen/Dense>

static int RoundUpToPowerOf2(int val)
{
	--val;
	val |= val >> 1;
	val |= val >> 2;
	val |= val >> 4;
	val |= val >> 8;
	val |= val >> 16;
	return ++val;
}

struct AxisAlignedCubeI
{
	/// The position of the box's corner that has the least coordinate value in all axes.
	Eigen::Vector3i pos;
	/// The extents of the cube.
	Eigen::Vector3i span;

	static bool CubesIntersect(const AxisAlignedCubeI& cube1, const AxisAlignedCubeI& cube2)
	{
		bool xIntersects = !(cube2.pos.x() + cube2.span.x() <= cube1.pos.x() || cube1.pos.x() + cube1.span.x() <= cube2.pos.x());
		bool yIntersects = !(cube2.pos.y() + cube2.span.y() <= cube1.pos.y() || cube1.pos.y() + cube1.span.y() <= cube2.pos.y());
		bool zIntersects = !(cube2.pos.z() + cube2.span.z() <= cube1.pos.z() || cube1.pos.z() + cube1.span.z() <= cube2.pos.z());
		return xIntersects && yIntersects && zIntersects;
	}

	static void SplitCube(const AxisAlignedCubeI& cube, AxisAlignedCubeI children[8])
	{
		Eigen::Vector3i newSpan = cube.span / 2;

		children[0].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() };
		children[0].span = newSpan;

		children[1].pos = { cube.pos.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[1].span = newSpan;

		children[2].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[2].span = newSpan;

		children[3].pos = { cube.pos.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[3].span = newSpan;

		children[4].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() };
		children[4].span = newSpan;

		children[5].pos = { cube.pos.x() + newSpan.x(), cube.pos.y(), cube.pos.z() + newSpan.z() };
		children[5].span = newSpan;

		children[6].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() };
		children[6].span = newSpan;

		children[7].pos = { cube.pos.x() + newSpan.x(), cube.pos.y() + newSpan.y(), cube.pos.z() + newSpan.z() };
		children[7].span = newSpan;
	}

	friend std::ostream& operator<<(std::ostream& os, const AxisAlignedCubeI& cube)
	{
		os << '[' << cube.pos.x() << ", " << cube.pos.y() << ", " << cube.pos.z() << "] -> "
			<< '[' << cube.pos.x() + cube.span.x() - 1 << ", " << cube.pos.y() + cube.span.y() - 1 <<
			", " << cube.pos.z() + cube.span.z() - 1 << ']';
		return os;
	}
};

//#define DEBUG_CONVERTER

class Converter
{
public:
	static void OpenVDBToDAG(openvdb::SharedPtr<openvdb::Vec3SGrid> grid, HashDAG& hd);

private:
#ifdef DEBUG_CONVERTER
	static uint32_t ConstructHashDAG(const AxisAlignedCubeI& openvdbTrackingCube,
		void* nodeIn, HashDAG& hd, Color& hdColors, uint64_t& voxelIndex,
		int level, bool full, int depth, const AxisAlignedCubeI& treebbox);
#else
	static uint32_t ConstructHashDAG(const AxisAlignedCubeI& openvdbTrackingCube,
		void* nodeIn, HashDAG& hd, Color& hdColors, uint64_t& voxelIndex,
		int level, bool full, int depth);
#endif

#ifdef DEBUG_CONVERTER
	template <typename NodeType, int NODE_SIZE>
	static uint32_t HandleOpenvdbLevel(const AxisAlignedCubeI& openvdbTrackingCube,
		HashDAG& hd, Color& hdColors, uint64_t& voxelIndex,
		int level, bool full, NodeType* node, int depth, const AxisAlignedCubeI& treebbox)
#else
	template <typename NodeType, int NODE_SIZE>
	static uint32_t HandleOpenvdbLevel(const AxisAlignedCubeI& openvdbTrackingCube,
		HashDAG& hd, Color& hdColors, uint64_t& voxelIndex,
		int level, bool full, NodeType* node, int depth)
#endif
	{
		uint64_t parentIndex = voxelIndex;
		// Split the gridCube into children, recurse with the correct (according to the openvdb recursion rules) one.
		AxisAlignedCubeI trackingChildren[8];
		AxisAlignedCubeI::SplitCube(openvdbTrackingCube, trackingChildren);

#ifdef DEBUG_CONVERTER
		AxisAlignedCubeI treebboxChildren[8];
		AxisAlignedCubeI::SplitCube(treebbox, treebboxChildren);
#endif

		// When the cube has the correct extents, advance to the next gridIterator level before recursing.
		bool childOn[8];
		for (int i = 0; i < 8; ++i)
		{
			childOn[i] = true;
		}
		constexpr int NODE_SIZE_2 = NODE_SIZE * NODE_SIZE;
		if (openvdbTrackingCube.span.x() == 2 && !full)
		{
			int childCountOn = 8;
			for (int i = 0; i < 8; ++i)
			{
				childOn[i] = node->isChildMaskOn(
					NODE_SIZE_2 * trackingChildren[i].pos.x() +
					NODE_SIZE * trackingChildren[i].pos.y() +
					trackingChildren[i].pos.z());

				childCountOn -= childOn[i] ? 0 : 1;
			}
			if (childCountOn == 0)
			{
				return HTConstants::INVALID_POINTER;
			}
		}

		std::vector<uint32_t> nodeBytes;
		nodeBytes.push_back(0);
		int finalChildCount = 0;
		for (int i = 0; i < 8; ++i)
		{
			if (childOn[i])
			{
				uint32_t result = HTConstants::INVALID_POINTER;
				uint32_t previousVoxelIndex = uint32_t(voxelIndex - parentIndex);
				if (openvdbTrackingCube.span.x() == 2)
				{
					AxisAlignedCubeI trackingCube;
					trackingCube.pos = { 0, 0, 0 };
					constexpr int NODE_SIZE_HALF = NODE_SIZE / 2;
					trackingCube.span = { NODE_SIZE_HALF, NODE_SIZE_HALF, NODE_SIZE_HALF };

					typename NodeType::ChildNodeType* nextNode;
					openvdb::Vec3f dummy;
					int nodeIndex = NODE_SIZE_2 * trackingChildren[i].pos.x() +
						NODE_SIZE * trackingChildren[i].pos.y() +
						trackingChildren[i].pos.z();
					bool itemIn = node->beginChildAll().getItem(nodeIndex, nextNode, dummy);
					assert(itemIn);
#ifdef DEBUG_CONVERTER
					result = ConstructHashDAG(trackingCube, nextNode, hd, hdColors, voxelIndex, level + 1, full, depth + 1, treebboxChildren[i]);
#else
					result = ConstructHashDAG(trackingCube, nextNode, hd, hdColors, voxelIndex, level + 1, full, depth + 1);
#endif
				}
				else
				{
#ifdef DEBUG_CONVERTER
					result = ConstructHashDAG(trackingChildren[i], node, hd, hdColors, voxelIndex, level + 1, full, depth, treebboxChildren[i]);
#else
					result = ConstructHashDAG(trackingChildren[i], node, hd, hdColors, voxelIndex, level + 1, full, depth);
#endif
				}
				if (result != HTConstants::INVALID_POINTER)
				{
					nodeBytes[0] |= 1 << i;
					nodeBytes.push_back(result);
					nodeBytes.push_back(previousVoxelIndex);
					++finalChildCount;
				}
			}
		}

		if (finalChildCount == 0)
		{
			return HTConstants::INVALID_POINTER;
		}

		// Insert child results along with newly created node into the HashDAG.
		uint32_t nodePtr = hd.FindOrAddNode(level, nodeBytes.size(), nodeBytes.data());

		return nodePtr;
	}
};


#include <gtest/gtest.h>
#include <test/Helpers.h>

#include <mvp/Image/OrbitalImage.h>

#include <vw/Camera/PinholeModel.h>
#include <vw/Image/UtilityViews.h>
#include <vw/Image/Filter.h>
#include <vw/Image/MaskViews.h>
#include <vw/Image/Statistics.h>

#include <boost/random/linear_congruential.hpp>

using namespace std;
using namespace vw;
using namespace vw::test;
using namespace vw::camera;
using namespace mvp::image;

template <class Image1T, class Image2T>
bool is_image_valid_eq(ImageViewBase<Image1T> const& img1, ImageViewBase<Image2T> const& img2, double tol) {
  double diff = sum_of_channel_values(abs(img1.impl() - img2.impl()));
  return !is_transparent(img1) && !is_transparent(img2) && diff < tol;
}

OrbitalImage synth_orbit(Vector3 const& camera_center, Vector3 const& axis_angle,
                         Vector2 const& focus, Vector2i size)
{
  boost::rand48 gen;
  PinholeModel camera(camera_center, axis_angle_to_matrix(axis_angle),
                      focus.x(), focus.y(), size.x() / 2.0, size.y() / 2.0);

  ImageView<OrbitalImagePixel> img = pixel_cast<OrbitalImagePixel>(gaussian_filter(uniform_noise_view(gen, size.x(), size.y()), 1.5));
  OrbitalImage orbital_image(img, camera);
  return orbital_image;
}

TEST(OrbitalImage, back_project) {
  Vector2i size(64, 32);

  OrbitalImage orbital_image;
  ImageView<OrbitalImagePixel> patch;

  // Camera at z = 1, looking back at origin. Plane at z = 0
  orbital_image = synth_orbit(Vector3(0, 0, 1), Vector3(M_PI, 0, 0), Vector2(1, 1), size);
  patch = orbital_image.back_project(Vector3(0, 0, 0), Quat(1, 0, 0, 0), Vector2(1, 1), size);
  EXPECT_TRUE(is_image_valid_eq(orbital_image.image(), patch, 1e-6));

  // Camera at z = 1, looking back at origin. Plane at z = -1
  // Patch is 2f from plane, so to get the original image we make patch with scale 2m/px
  orbital_image = synth_orbit(Vector3(0, 0, 1), Vector3(M_PI, 0, 0), Vector2(1, 1), size);
  patch = orbital_image.back_project(Vector3(0, 0, -1), Quat(1, 0, 0, 0), Vector2(2, 2), size);
  EXPECT_TRUE(is_image_valid_eq(orbital_image.image(), patch, 1e-6));

  // Camera at z = -1, looking toward origin. Plane at z = 0, rotated 180deg about x axis (vertical flip)
  orbital_image = synth_orbit(Vector3(0, 0, -1), Vector3(0, 0, 0), Vector2(1, 1), size);
  patch = orbital_image.back_project(Vector3(0, 0, 0), Quat(0, 1, 0, 0), Vector2(1, 1), size);
  EXPECT_TRUE(is_image_valid_eq(orbital_image.image(), patch, 1e-6));
}