#ifndef CX_INTERACTIONS_HPP
#define CX_INTERACTIONS_HPP

#include "parameters.hpp"
#include "cx_workspace.hpp"
#include "la.hpp"

#include <boost/multi_array.hpp>

#include <cmath>
#include <iomanip>

typedef boost::multi_array<double, 1> spin_t;

namespace cx_dqmc {
    namespace interaction {

	inline void onsite_left(dqmc::parameters * p, cx_mat_t& M,
				cx_vec_t& int_vec,
				const spin_t& spins,
				double inv, int replica) {
	    using namespace std;
	    if (M.rows() == p->N) replica = 0;

	    int_vec.setOnes();

	    for (int i = 0; i < p->N; ++i) {
		for (int j = 0; j < M.cols(); ++j) {
		    if (i < p->n_A) {
			int_vec(i) = std::exp(p->mu_factor * inv)
			    * std::exp(p->cx_osi_lambda * spins[i] * inv);
		    } else {
			int_vec(i + replica * p->n_B) =
			    std::exp(p->mu_factor * inv)
			    * std::exp(p->cx_osi_lambda * spins[i] * inv);
		    }
		}
	    }
	    M = int_vec.asDiagonal() * M;
	}

	inline void onsite_right(dqmc::parameters * p, cx_mat_t& M,
				 cx_vec_t& int_vec,
				 const spin_t& spins,
				 double inv, int replica) {
	    if (M.cols() == p->N) replica = 0;
	    int_vec.setOnes();

	    for (int i = 0; i < p->N; ++i) {
		for (int j = 0; j < M.rows(); ++j) {
		    if (i < p->n_A) {
			int_vec(i) =
			    std::exp(p->mu_factor * inv)
			    * std::exp(p->cx_osi_lambda * spins[i] * inv);

		    } else {
			int_vec(i + replica * p->n_B) =
			    std::exp(p->mu_factor * inv)
			    * std::exp(p->cx_osi_lambda * spins[i] * inv);
		    }
		}
	    }
	    M = M * int_vec.asDiagonal();
	}

	inline void onsite_alternative_left(dqmc::parameters * p, cx_mat_t& M,
					    cx_vec_t& int_vec,
					    const spin_t& spins,
					    double inv, int replica) {
	    using namespace std;
	    if (M.rows() == p->N) replica = 0;

	    int_vec.setOnes();

	    // cout << "Onsite left\t"
	    // 	 << p->cx_interaction_vals[0] << " "
	    // 	 << p->cx_interaction_vals[1] << " "
	    // 	 << p->cx_interaction_vals[2] << " " << endl;

	    for (int i = 0; i < p->n_A; ++i) {
		for (int j = 0; j < M.cols(); ++j) {
		    int_vec(i) =
			p->mu_site_factor[int(inv + 1)]
			* p->cx_interaction_vals[int(-1. * spins[i] * inv + 1)];
		    // * std::exp(p->cx_osi_lambda * spins[i] * inv);
		}
	    }

	    for (int i = p->n_A; i < p->N; ++i) {
		for (int j = 0; j < M.cols(); ++j) {
		    int_vec(i + replica * p->n_B) =
			p->mu_site_factor[int(inv + 1)]
			* p->cx_interaction_vals[int(-1. * spins[i] * inv + 1)];
		    // * std::exp(p->cx_osi_lambda * spins[i] * inv);
		}
	    }
	    M = int_vec.asDiagonal() * M;
	}

	inline void onsite_alternative_right(dqmc::parameters * p, cx_mat& M,
					     cx_vec_t& int_vec,
					     const spin_t& spins,
					     double inv, int replica) {
	    using namespace std;
	    if (M.cols() == p->N) replica = 0;
	    int_vec.setOnes();

	    // cout << "Onsite right\t"
	    // 	 << p->cx_interaction_vals[0] << " "
	    // 	 << p->cx_interaction_vals[1] << " "
	    // 	 << p->cx_interaction_vals[2] << " " << endl;

	    for (int i = 0; i < p->n_A; ++i) {
		for (int j = 0; j < M.rows(); ++j) {
		    int_vec(i) =
			p->mu_site_factor[int(inv + 1)]
			* p->cx_interaction_vals[int(-1 * spins[i] * inv + 1)];
		    // * std::exp(p->cx_osi_lambda * spins[i] * inv);
		}
	    }

	    for (int i = p->n_A; i < p->N; ++i) {
		for (int j = 0; j < M.rows(); ++j) {
		    int_vec(i + replica * p->n_B) =
			p->mu_site_factor[int(inv + 1)]
			* p->cx_interaction_vals[int(-1. * spins[i] * inv + 1)];
		    // * std::exp(p->cx_osi_lambda * spins[i] * inv);
		}
	    }
	    M = M * int_vec.asDiagonal();
	}

	//============================================================
	// alternative next-nearest neighbor interaction for spinless
	// fermions
	//============================================================

	inline void nn_left_alternative(dqmc::parameters * p, cx_dqmc::workspace * ws,
					cx_mat_t& M, const spin_t& spins,
					double inv, int replica) {
	    using namespace std;
	    if (M.rows() == p->N) replica = 0;

	    dqmc::la::ones(ws->la_vec_1);
	    for (int s = 0; s < p->N; ++s) {
		if (s < p->n_A) {
		    ws->la_vec_1(s) = p->mu_site_factor[int(inv + 1)];
		} else {
		    ws->la_vec_1(s + replica * p->n_B) = p->mu_site_factor[int(inv + 1)];
		}

	    }

	    for (int s = 0; s < p->N; ++s) {
		// cout << "Site "  << s << endl;
		for (int b = 0; b < 40; ++b) {
		    // cout << "Bond: " << b << endl;
		    if (ws->site_signs[s][b] == 0) break;
		    if (s < p->n_A) {
			ws->la_vec_1(s) *=
			    p->cx_bond_vals[ws->bond_types[ ws->site_bonds[s][b] ] ][ws->site_signs[s][b] *
											 spins[ws->site_bonds[s][b]] * inv + 1];
			// * p->mu_bond_factor[int(inv + 1)];
		    } else {
			ws->la_vec_1(s + replica * p->n_B) *=
			    p->cx_bond_vals[ws->bond_types[ws->site_bonds[s][b]]][ws->site_signs[s][b] *
						   spins[ws->site_bonds[s][b]] * inv + 1];
			    // * p->mu_bond_factor[int(inv + 1)];
		    }
	    	}
	    }
	    M = ws->la_vec_1.asDiagonal() * M;
	}

	inline void nn_right_alternative(dqmc::parameters * p, cx_dqmc::workspace * ws,
					 cx_mat_t& M, const spin_t& spins,
					 double inv, int replica) {
	    // std::cout << "NNNNNN" << std::endl;
	    if (M.cols() == p->N) replica = 0;

	    dqmc::la::ones(ws->la_vec_1);
	    for (int s = 0; s < p->N; ++s) {
		if (s < p->n_A) {
		    ws->la_vec_1(s) = p->mu_site_factor[int(inv + 1)];
		} else {
		    ws->la_vec_1(s + replica * p->n_B) = p->mu_site_factor[int(inv + 1)];
		}

	    }
	    for (int s = 0; s < p->N; ++s) {
		for (int b = 0; b < 40; ++b) {
		    if (ws->site_signs[s][b] == 0) break;
		    if (s < p->n_A) {
			ws->la_vec_1(s) *=
			    p->cx_bond_vals[ws->bond_types[ws->site_bonds[s][b]]][ws->site_signs[s][b] *
										  spins[ws->site_bonds[s][b]] * inv + 1];
			    // * p->mu_bond_factor[int(inv + 1)];
		    } else {
			ws->la_vec_1(s + replica * p->n_B) *=
			    p->cx_bond_vals[ws->bond_types[ws->site_bonds[s][b]]][ws->site_signs[s][b] *
										  spins[ws->site_bonds[s][b]] * inv + 1];
			    // * p->mu_bond_factor[int(inv + 1)];
		    }
	    	}
	    }

	    M = M * ws->la_vec_1.asDiagonal();
	    return;
	}

	/////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////
	/////////////////////////////////////////////////////////////////

	//////////////////////////////////////////////////////////////
	// Next-nearest neighbor interaction for spinless fermions
	//////////////////////////////////////////////////////////////
 	inline void nn_left(dqmc::parameters * p, cx_dqmc::workspace * ws,
			    cx_mat_t& M, const spin_t& spins,
			    double inv, int replica) {
	    dqmc::tools::abort ("nn left is too simple");
	    if (M.rows() == p->N) replica = 0;

	    dqmc::la::ones(ws->la_vec_1);
	    for (int s = 0; s < p->N; ++s) {
		for (int b = 0; b < 4; ++b) {
		    if (ws->site_signs[s][b] == 0) break;
		    if (s < p->n_A) {
			ws->la_vec_1(s) *=
			    p->cx_interaction_vals[ws->site_signs[s][b] *
						   int(spins[ws->site_bonds[s][b]] * inv) + 1]
			    * cx_double(p->mu_vals[int(inv + 1)], 0);
		    } else {
			ws->la_vec_1(s + replica * p->n_B) *=
			    p->cx_interaction_vals[ws->site_signs[s][b] *
						   int(spins[ws->site_bonds[s][b]] * inv) + 1]
			    * p->mu_vals[int(inv + 1)];
		    }
	    	}
	    }
	    // M = (ws->la_vec_1.asDiagonal()) * M;
	    M = (ws->la_vec_1.asDiagonal() * p->mu_site_factor[int(inv + 1)]) * M;
	    return;
	}

	inline void nn_right(dqmc::parameters * p, cx_dqmc::workspace * ws,
			     cx_mat_t& M, const spin_t& spins,
			     double inv, int replica) {
	    dqmc::tools::abort ("nn right is too simple");
	    // std::cout << "NNNNNN" << std::endl;
	    if (M.cols() == p->N) replica = 0;

	    dqmc::la::ones(ws->la_vec_1);
	    for (int s = 0; s < p->N; ++s) {
		for (int b = 0; b < 4; ++b) {
		    if (ws->site_signs[s][b] == 0) break;
		    if (s < p->n_A) {
			ws->la_vec_1(s) *=
			    p->cx_interaction_vals[ws->site_signs[s][b] *
						   spins[ws->site_bonds[s][b]] * inv + 1]
			* p->mu_vals[int(inv + 1)];
		    } else {
			ws->la_vec_1(s + replica * p->n_B) *=
			    p->cx_interaction_vals[ws->site_signs[s][b] *
						   spins[ws->site_bonds[s][b]] * inv + 1]
			    * p->mu_vals[int(inv + 1)];
		    }
	    	}
	    }
	    //M = M * ws->la_vec_1.asDiagonal();
	    M = M * (ws->la_vec_1.asDiagonal() * p->mu_site_factor[int(inv + 1)]);
	}

	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////
	//////////////////////////////////////////////////////////////

	inline void interaction_left(dqmc::parameters * p, cx_dqmc::workspace * ws,
				     cx_mat_t& M, cx_vec_t& int_vec,
				     const spin_t& spins,
				     double inv, int replica) {
	    if (p->model_id == p->CX_SPINFUL_HUBBARD)
		cx_dqmc::interaction::onsite_left(p, M, int_vec,
						  spins, inv, replica);
	    else if (p->model_id == p->CX_SPINFUL_HUBBARD_ALTERNATIVE)
		cx_dqmc::interaction::onsite_alternative_left(p, M, int_vec,
							      spins, inv, replica);
	    else if (p->model_id == p->CX_SPINLESS_HUBBARD)
		cx_dqmc::interaction::nn_left(p, ws, M,
					      spins, inv, replica);
	    else if (p->model_id == p->CX_SPINLESS_HUBBARD_ALTERNATIVE)
		cx_dqmc::interaction::nn_left_alternative(p, ws, M,
							  spins, inv, replica);

	}

	inline void interaction_right(dqmc::parameters * p, cx_dqmc::workspace * ws,
				      cx_mat_t& M, cx_vec_t& int_vec,
				      const spin_t& spins,
				      double inv, int replica) {
	    if (p->model_id == p->CX_SPINFUL_HUBBARD)
		cx_dqmc::interaction::onsite_right(p, M, int_vec,
						   spins, inv, replica);
	    else if (p->model_id == p->CX_SPINFUL_HUBBARD_ALTERNATIVE)
		cx_dqmc::interaction::onsite_alternative_right(p, M, int_vec,
							       spins, inv, replica);
	    else if (p->model_id == p->CX_SPINLESS_HUBBARD)
		cx_dqmc::interaction::nn_right(p, ws, M,
						   spins, inv, replica);
	    else if (p->model_id == p->CX_SPINLESS_HUBBARD_ALTERNATIVE)
		cx_dqmc::interaction::nn_right_alternative(p, ws, M,
							   spins, inv, replica);

	}
    }
}
#endif


#include <boost/graph/max_cardinality_matching.hpp>


#include "testsuite.h"
#include <blitz/array.h>

BZ_USING_NAMESPACE(blitz)

int zippy()
{ return 0; }

int main()
{
    if (zippy())
    {
#ifdef BZ_HAVE_COMPLEX
#ifdef BZ_HAVE_COMPLEX_MATH1

{
    Array<complex<float>,1> test(5);
    test = sqrt(test);

    Array<double,1> test2(5);
    test2 = sqrt(test2);
}

#endif  // BZ_HAVE_COMPLEX_MATH1

{
  Array<complex<float>,1> test(5),test2(5) ;
  Array<float,1> test3(5) ;
  test3 = 0.64f ;
  test2 = complex<float>(3,3) ;
  test = 2*test3*test2 ;
  test = test3*test2;
}

#endif  // BZ_HAVE_COMPLEX

    }

    return 0;
}


#ifndef BOOST_MP11_DETAIL_MP_PLUS_HPP_INCLUDED
#define BOOST_MP11_DETAIL_MP_PLUS_HPP_INCLUDED

//  Copyright 2015 Peter Dimov.
//
//  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/mp11/detail/config.hpp>
#include <boost/config/workaround.hpp>
#include <type_traits>

namespace boost
{
namespace mp11
{

// mp_plus
namespace detail
{

#if defined( BOOST_MP11_HAS_FOLD_EXPRESSIONS )

template<class... T> struct mp_plus_impl
{
    static const auto _v = (T::value + ... + 0);
    using type = std::integral_constant<typename std::remove_const<decltype(_v)>::type, _v>;
};

#else

template<class... T> struct mp_plus_impl;

template<> struct mp_plus_impl<>
{
    using type = std::integral_constant<int, 0>;
};

#if BOOST_WORKAROUND( BOOST_GCC, < 40800 )

template<class T1, class... T> struct mp_plus_impl<T1, T...>
{
    static const decltype(T1::value + mp_plus_impl<T...>::type::value) _v = T1::value + mp_plus_impl<T...>::type::value;
    using type = std::integral_constant<typename std::remove_const<decltype(_v)>::type, _v>;
};

template<class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class... T> struct mp_plus_impl<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T...>
{
    static const
        decltype(T1::value + T2::value + T3::value + T4::value + T5::value + T6::value + T7::value + T8::value + T9::value + T10::value + mp_plus_impl<T...>::type::value)
        _v = T1::value + T2::value + T3::value + T4::value + T5::value + T6::value + T7::value + T8::value + T9::value + T10::value + mp_plus_impl<T...>::type::value;
    using type = std::integral_constant<typename std::remove_const<decltype(_v)>::type, _v>;
};

#else

template<class T1, class... T> struct mp_plus_impl<T1, T...>
{
    static const auto _v = T1::value + mp_plus_impl<T...>::type::value;
    using type = std::integral_constant<typename std::remove_const<decltype(_v)>::type, _v>;
};

template<class T1, class T2, class T3, class T4, class T5, class T6, class T7, class T8, class T9, class T10, class... T> struct mp_plus_impl<T1, T2, T3, T4, T5, T6, T7, T8, T9, T10, T...>
{
    static const auto _v = T1::value + T2::value + T3::value + T4::value + T5::value + T6::value + T7::value + T8::value + T9::value + T10::value + mp_plus_impl<T...>::type::value;
    using type = std::integral_constant<typename std::remove_const<decltype(_v)>::type, _v>;
};

#endif

#endif

} // namespace detail

template<class... T> using mp_plus = typename detail::mp_plus_impl<T...>::type;

} // namespace mp11
} // namespace boost

#endif // #ifndef BOOST_MP11_DETAIL_MP_PLUS_HPP_INCLUDED


#include <boost/math/special_functions/fpclassify.hpp>


//=======================================================================
// Copyright (c) 2014 Piotr Smulewicz
//
// 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)
//=======================================================================
/**
 * @file k_center_long_test.cpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-01-29
 */

#include "test_utils/test_result_check.hpp"

#include "greedy/k_center/in_balls.hpp"

#include "paal/data_structures/metric/basic_metrics.hpp"
#include "paal/greedy/k_center/k_center.hpp"
#include "paal/utils/irange.hpp"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_CASE(KCenter) {
    // performance test
    std::size_t const NUM_CENTERS = 1000;
    const int NUM_ITEMS = 1000 * 1000;
    const int P1 = 15469313;
    const int P2 = 10554857;
    const int P3 = 4241099;
    const double APPROXIMATION_RATIO = 2;
    auto metric = [ = ](int a, int b) {
        if (a > b) std::swap(a, b);
        if (a == b) return 0;
        return ((a * a * P3 + b * P2) % P1 +
                P1 * (1 + 100 * (((b - a) % NUM_CENTERS) != 0)));
    };
    auto items = paal::irange(NUM_ITEMS);
    std::vector<int> centers;
    // solution
    int radius = paal::greedy::kCenter(metric, NUM_CENTERS, items.begin(),
                                       items.end(), back_inserter(centers));
    BOOST_CHECK_EQUAL(centers.size(), NUM_CENTERS);
    check_result(radius, P1, APPROXIMATION_RATIO, paal::utils::less_equal(), 0,
                 "lower bound ", "upper bound for approximation ratio ");
    paal::in_balls(items, centers, metric, radius);
}


/*
 *
 * Copyright (c) 2002, 2003 Kresimir Fresl, Toon Knapen and Karl Meerbergen
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_AS_MATRIX_H
#define BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_AS_MATRIX_H

#include <boost/numeric/bindings/traits/config.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#ifndef BOOST_UBLAS_HAVE_BINDINGS
#  include <boost/numeric/bindings/traits/ublas_vector.hpp>
#endif
#include <boost/numeric/bindings/traits/matrix_traits.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/same_traits.hpp>
#  include <boost/mpl/if.hpp>
#endif

namespace boost { namespace numeric { namespace bindings { namespace traits {

  // ublas::vector<> treated as matrix (nx1)
  template <typename T, typename ArrT, typename V>
  struct matrix_detail_traits<boost::numeric::ublas::vector<T, ArrT>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::vector<T, ArrT>,
         typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef boost::numeric::ublas::vector<T, ArrT> identifier_type;
    typedef V matrix_type;
    typedef general_t matrix_structure;
    typedef column_major_t ordering_type;

    typedef T value_type;
    typedef typename detail::generate_const<V,T>::type* pointer;

    static pointer storage (matrix_type& v) {
      typedef typename detail::generate_const<V,ArrT>::type array_type;
      return vector_traits<array_type>::storage (v.data());
    }
    static std::ptrdiff_t num_rows (matrix_type& v) { return v.size(); }
    static std::ptrdiff_t num_columns (matrix_type&) { return 1; }
//    static std::ptrdiff_t stride1 (matrix_type& v) { return vector_traits<V>::stride (v); }
//    static std::ptrdiff_t stride2 (matrix_type&) { return 1; }
    static std::ptrdiff_t leading_dimension (matrix_type& v) { return v.size(); }
  };

  // ublas::vector_range<> treated as matrix (nx1)
  template <typename T, typename V>
  struct matrix_detail_traits<boost::numeric::ublas::vector_range<T>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::vector_range<T>,
         typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef boost::numeric::ublas::vector_range<T> identifier_type;
    typedef V matrix_type;
    typedef general_t matrix_structure;
    typedef column_major_t ordering_type;

    typedef typename T::value_type value_type;
    typedef typename detail::generate_const<V,value_type>::type* pointer;

    static pointer storage (matrix_type& v) {
      return vector_traits<V>::storage (v);
    }
    static std::ptrdiff_t num_rows (matrix_type& v) { return v.size(); }
    static std::ptrdiff_t num_columns (matrix_type&) { return 1; }
//    static std::ptrdiff_t stride1 (matrix_type& v) { return vector_traits<V>::stride (v); }
//    static std::ptrdiff_t stride2 (matrix_type&) { return 1; }
    static std::ptrdiff_t leading_dimension (matrix_type& v) { return v.size(); }
  };

#ifndef BOOST_NUMERIC_BINDINGS_FORTRAN

  // (undocumented) ublas::c_vector<>
  template <typename T, std::size_t N, typename V>
  struct matrix_detail_traits<boost::numeric::ublas::c_vector<T,N>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::c_vector<T,N>,
         typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef boost::numeric::ublas::c_vector<T,N> identifier_type;
    typedef V matrix_type;
    typedef general_t matrix_structure;
    typedef row_major_t ordering_type; // consistent with c_matrix<>

    typedef T value_type;
    typedef typename detail::generate_const<V,T>::type* pointer;

    static pointer storage (matrix_type& v) { return v.data(); }
    static std::ptrdiff_t num_rows (matrix_type&) { return 1; }
    static std::ptrdiff_t num_columns (matrix_type& v) { return v.size(); }
    static std::ptrdiff_t leading_dimension (matrix_type&) { return N; }
  };

#endif // BOOST_NUMERIC_BINDINGS_FORTRAN

  // ublas::bounded_vector<> treated as matrix (nx1)
  template <typename T, std::size_t N, typename V>
  struct matrix_detail_traits<boost::numeric::ublas::bounded_vector<T, N>, V>
  {
#ifndef BOOST_NUMERIC_BINDINGS_NO_SANITY_CHECK
    BOOST_STATIC_ASSERT(
      (boost::is_same<
         boost::numeric::ublas::bounded_vector<T, N>,
         typename boost::remove_const<V>::type
       >::value) );
#endif

    typedef boost::numeric::ublas::bounded_vector<T, N> identifier_type;
    typedef V matrix_type;
    typedef general_t matrix_structure;
    typedef column_major_t ordering_type;

    typedef T value_type;
    typedef typename detail::generate_const<V,T>::type* pointer;

    static pointer storage (matrix_type& v) {
      typedef typename detail::generate_const<V,typename identifier_type::array_type>::type array_type;
      return vector_traits<array_type>::storage (v.data());
    }
    static std::ptrdiff_t num_rows (matrix_type& v) { return v.size(); }
    static std::ptrdiff_t num_columns (matrix_type&) { return 1; }
    static std::ptrdiff_t storage_size (matrix_type& v) { return v.size(); }
//    static std::ptrdiff_t stride1 (matrix_type& v) { return vector_traits<V>::stride (v); }
//    static std::ptrdiff_t stride2 (matrix_type&) { return 1; }
    static std::ptrdiff_t leading_dimension (matrix_type& v) { return v.size(); }
  };

}}}}

#else // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#error with your compiler ublas::vector<> cannot be used as matrix

#endif // BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS

#endif // BOOST_NUMERIC_BINDINGS_TRAITS_UBLAS_VECTOR_AS_MATRIX_H


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/constant/fourthrooteps.hpp>
#include <boost/simd/constant/eps.hpp>
#include <boost/simd/as.hpp>
#include <scalar_test.hpp>

STF_CASE_TPL( "Check fourthrooteps behavior for integral types"
            , (std::uint8_t)(std::uint16_t)(std::uint32_t)(std::uint64_t)
              (std::int8_t)(std::int16_t)(std::int32_t)(std::int64_t)
            )
{
  using boost::simd::as;
  using boost::simd::detail::fourthrooteps;
  using boost::simd::Fourthrooteps;

  STF_TYPE_IS(decltype(Fourthrooteps<T>()), T);
  STF_EQUAL(Fourthrooteps<T>(), T(1));
  STF_EQUAL(fourthrooteps( as(T{}) ),T(1));
}

STF_CASE_TPL( "Check fourthrooteps behavior for floating types"
            , (double)(float)
            )
{
  using boost::simd::as;
  using boost::simd::detail::fourthrooteps;
  using boost::simd::Fourthrooteps;
  using boost::simd::Eps;

  STF_TYPE_IS(decltype(Fourthrooteps<T>()), T);
  auto z1 = Fourthrooteps<T>();
  STF_ULP_EQUAL(z1*z1*z1*z1, Eps<T>(), 1);
  auto z2 = fourthrooteps( as(T{}) );
  STF_ULP_EQUAL(z2*z2*z2*z2, Eps<T>(), 2);
}


// loop100 generated by makeloops.py Thu Jun 30 16:44:56 2011

#include <blitz/vector2.h>
#include <blitz/array.h>
#include <random/uniform.h>
#include <blitz/benchext.h>

#ifdef BZ_HAVE_VALARRAY
 #define BENCHMARK_VALARRAY
#endif

#ifdef BENCHMARK_VALARRAY
#include <valarray>
#endif

BZ_NAMESPACE(blitz)
extern void sink();
BZ_NAMESPACE_END

BZ_USING_NAMESPACE(blitz)
BZ_USING_NAMESPACE(std)

#if defined(BZ_FORTRAN_SYMBOLS_WITH_TRAILING_UNDERSCORES)
 #define loop100_f77 loop100_f77_
 #define loop100_f77overhead loop100_f77overhead_
 #define loop100_f90 loop100_f90_
 #define loop100_f90overhead loop100_f90overhead_
#elif defined(BZ_FORTRAN_SYMBOLS_WITH_DOUBLE_TRAILING_UNDERSCORES)
 #define loop100_f77 loop100_f77__
 #define loop100_f77overhead loop100_f77overhead__
 #define loop100_f90 loop100_f90__
 #define loop100_f90overhead loop100_f90overhead__
#elif defined(BZ_FORTRAN_SYMBOLS_CAPS)
 #define loop100_f77 LOOP100_F77
 #define loop100_f77overhead LOOP100_F77OVERHEAD
 #define loop100_f90 LOOP100_F90
 #define loop100_f90overhead LOOP100_F90OVERHEAD
#endif

extern "C" {
  void loop100_f77(const int& N, double* a, double* b, double* c, double* d, double* x, const double& u, const double& v, const double& w, const double& z);
  void loop100_f77overhead(const int& N, double* a, double* b, double* c, double* d, double* x, const double& u, const double& v, const double& w, const double& z);
  void loop100_f90(const int& N, double* a, double* b, double* c, double* d, double* x, const double& u, const double& v, const double& w, const double& z);
  void loop100_f90overhead(const int& N, double* a, double* b, double* c, double* d, double* x, const double& u, const double& v, const double& w, const double& z);

}

void VectorVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void ArrayVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void ArrayVersion_unaligned(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void ArrayVersion_misaligned(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void ArrayVersion_index(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void doTinyVectorVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z);
void F77Version(BenchmarkExt<int>& bench, double u, double v, double w, double z);
#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench, double u, double v, double w, double z);
#endif
#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z);
#endif

const int numSizes = 80;
const bool runvector=false; // no point as long as Vector is Array<1>

int main()
{
    int numBenchmarks = 5;
    if (runvector) numBenchmarks++;
#ifdef BENCHMARK_VALARRAY
    numBenchmarks++;
#endif
#ifdef FORTRAN_90
    numBenchmarks++;
#endif

    BenchmarkExt<int> bench("loop100: $x=(1.0-$c*$c)/((4*w)*sin(1.0+$c*$c-2*v*$c))*$a*$b*u*exp(-z*$d)", numBenchmarks);

    bench.setNumParameters(numSizes);

    Array<int,1> parameters(numSizes);
    Array<long,1> iters(numSizes);
    Array<double,1> flops(numSizes);

    parameters=pow(pow(2.,0.25),tensor::i)+tensor::i;
    flops = 18 * parameters;
    iters = 100000000L / flops;
    iters = where(iters<2, 2, iters);
    cout << iters << endl;

    bench.setParameterVector(parameters);
    bench.setIterations(iters);
    bench.setOpsPerIteration(flops);
    bench.setDependentVariable("flops");
    bench.beginBenchmarking();

    double u = 0.39123982498157938742;
    double v = 0.39123982498157938742;
    double w = 0.39123982498157938742;
    double z = 0.39123982498157938742;

    ArrayVersion(bench, u, v, w, z);
    ArrayVersion_unaligned(bench, u, v, w, z);
    ArrayVersion_misaligned(bench, u, v, w, z);
    ArrayVersion_index(bench, u, v, w, z);
    //doTinyVectorVersion(bench, u, v, w, z);
    F77Version(bench, u, v, w, z);
#ifdef FORTRAN_90
    F90Version(bench, u, v, w, z);
#endif
#ifdef BENCHMARK_VALARRAY
    ValarrayVersion(bench, u, v, w, z);
#endif

    if(runvector)
      VectorVersion(bench, u, v, w, z);

    bench.endBenchmarking();

    bench.saveMatlabGraph("loop100.m");
    return 0;
}

template<class T>
void initializeRandomDouble(T* data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

template<class T>
void initializeRandomDouble(valarray<T>& data, int numElements, int stride = 1)
{
    ranlib::Uniform<T> rnd;

    for (int i=0; i < numElements; ++i)
        data[size_t(i*stride)] = rnd.random();
}

void VectorVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Vector<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Vector<double> a(N);
        initializeRandomDouble(a.data(), N);
        Vector<double> b(N);
        initializeRandomDouble(b.data(), N);
        Vector<double> c(N);
        initializeRandomDouble(c.data(), N);
        Vector<double> d(N);
        initializeRandomDouble(d.data(), N);
        Vector<double> x(N);
        initializeRandomDouble(x.data(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c*c)/((4*w)*sin(1.0+c*c-2*v*c))*a*b*u*exp(-z*d);
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Array<T,1>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<double,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<double,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<double,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);
        Array<double,1> d(N);
        initializeRandomDouble(d.dataFirst(), N);
        Array<double,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c*c)/((4*w)*sin(1.0+c*c-2*v*c))*a*b*u*exp(-z*d);
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_index(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Array<T,1> (indexexpr.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

        Array<double,1> a(N);
        initializeRandomDouble(a.dataFirst(), N);
        Array<double,1> b(N);
        initializeRandomDouble(b.dataFirst(), N);
        Array<double,1> c(N);
        initializeRandomDouble(c.dataFirst(), N);
        Array<double,1> d(N);
        initializeRandomDouble(d.dataFirst(), N);
        Array<double,1> x(N);
        initializeRandomDouble(x.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c(tensor::i)*c(tensor::i))/((4*w)*sin(1.0+c(tensor::i)*c(tensor::i)-2*v*c(tensor::i)))*a(tensor::i)*b(tensor::i)*u*exp(-z*d(tensor::i));;
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_unaligned(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Array<T,1> (unal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<double,1> afill(N+1);
    Array<double,1> a(afill(Range(1,N)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<double,1> bfill(N+1);
    Array<double,1> b(bfill(Range(1,N)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<double,1> cfill(N+1);
    Array<double,1> c(cfill(Range(1,N)));
    initializeRandomDouble(c.dataFirst(), N);

    Array<double,1> dfill(N+1);
    Array<double,1> d(dfill(Range(1,N)));
    initializeRandomDouble(d.dataFirst(), N);

    Array<double,1> xfill(N+1);
    Array<double,1> x(xfill(Range(1,N)));
    initializeRandomDouble(x.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c*c)/((4*w)*sin(1.0+c*c-2*v*c))*a*b*u*exp(-z*d);
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

  void ArrayVersion_misaligned(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Array<T,1> (misal.)");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        long iters = bench.getIterations();

        cout << bench.currentImplementation() << ": N = " << N << endl;

    Array<double,1> afill(N+5);
    Array<double,1> a(afill(Range(0,N+0-1)));
    initializeRandomDouble(a.dataFirst(), N);

    Array<double,1> bfill(N+5);
    Array<double,1> b(bfill(Range(1,N+1-1)));
    initializeRandomDouble(b.dataFirst(), N);

    Array<double,1> cfill(N+5);
    Array<double,1> c(cfill(Range(2,N+2-1)));
    initializeRandomDouble(c.dataFirst(), N);

    Array<double,1> dfill(N+5);
    Array<double,1> d(dfill(Range(3,N+3-1)));
    initializeRandomDouble(d.dataFirst(), N);

    Array<double,1> xfill(N+5);
    Array<double,1> x(xfill(Range(4,N+4-1)));
    initializeRandomDouble(x.dataFirst(), N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c*c)/((4*w)*sin(1.0+c*c-2*v*c))*a*b*u*exp(-z*d);
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
            sink();
	}

        bench.stopOverhead();
    }

    bench.endImplementation();
}

#ifdef BENCHMARK_VALARRAY
void ValarrayVersion(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("valarray<T>");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        long iters = bench.getIterations();

        valarray<double> a(N);
        initializeRandomDouble(a, N);
        valarray<double> b(N);
        initializeRandomDouble(b, N);
        valarray<double> c(N);
        initializeRandomDouble(c, N);
        valarray<double> d(N);
        initializeRandomDouble(d, N);
        valarray<double> x(N);
        initializeRandomDouble(x, N);

        bench.start();
        for (long i=0; i < iters; ++i)
        {
            x=(1.0-c*c)/((4*w)*sin(1.0+c*c-2*v*c))*a*b*u*exp(-z*d);
            sink();
        }
        bench.stop();

        bench.startOverhead();
        for (long i=0; i < iters; ++i) {
	  sink();
	}
        bench.stopOverhead();
    }

    bench.endImplementation();
}
#endif

void F77Version(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Fortran 77");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        double* a = new double[N];
        initializeRandomDouble(a, N);
        double* b = new double[N];
        initializeRandomDouble(b, N);
        double* c = new double[N];
        initializeRandomDouble(c, N);
        double* d = new double[N];
        initializeRandomDouble(d, N);
        double* x = new double[N];
        initializeRandomDouble(x, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            loop100_f77(N, a, b, c, d, x, u, v, w, z);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            loop100_f77overhead(N, a, b, c, d, x, u, v, w, z);

        bench.stopOverhead();

        delete [] a;
        delete [] b;
        delete [] c;
        delete [] d;
        delete [] x;

    }

    bench.endImplementation();
}

#ifdef FORTRAN_90
void F90Version(BenchmarkExt<int>& bench, double u, double v, double w, double z)
{
    bench.beginImplementation("Fortran 90");

    while (!bench.doneImplementationBenchmark())
    {
        int N = bench.getParameter();
        cout << bench.currentImplementation() << ": N = " << N << endl;

        int iters = bench.getIterations();

        double* a = new double[N];
        initializeRandomDouble(a, N);
        double* b = new double[N];
        initializeRandomDouble(b, N);
        double* c = new double[N];
        initializeRandomDouble(c, N);
        double* d = new double[N];
        initializeRandomDouble(d, N);
        double* x = new double[N];
        initializeRandomDouble(x, N);

        bench.start();
        for (int iter=0; iter < iters; ++iter)
            loop100_f90(N, a, b, c, d, x, u, v, w, z);
        bench.stop();

        bench.startOverhead();
        for (int iter=0; iter < iters; ++iter)
            loop100_f90overhead(N, a, b, c, d, x, u, v, w, z);

        bench.stopOverhead();
        delete [] a;
        delete [] b;
        delete [] c;
        delete [] d;
        delete [] x;

    }

    bench.endImplementation();
}
#endif


//=======================================================================
// Copyright (c)
//
// 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)
//=======================================================================
/**
 * @file ublas_traits_test.cpp
 * @brief
 * @author Tomasz Strozak
 * @version 1.0
 * @date 2015-06-21
 */

#include "paal/data_structures/ublas_traits.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_sparse.hpp>

#include <vector>

using coordinate_t = double;
using paal::data_structures::is_sparse_row;
namespace ublas = boost::numeric::ublas;

BOOST_AUTO_TEST_SUITE(sparse_row)

BOOST_AUTO_TEST_CASE(vector) {
    BOOST_CHECK(!is_sparse_row<std::vector<coordinate_t>>());
}

BOOST_AUTO_TEST_CASE(ublas_vector) {
    BOOST_CHECK(!is_sparse_row<ublas::vector<coordinate_t>>());
}

BOOST_AUTO_TEST_CASE(ublas_compressed_vector) {
    BOOST_CHECK(is_sparse_row<ublas::compressed_vector<coordinate_t>>());
}

BOOST_AUTO_TEST_CASE(ublas_matrix_row) {
    BOOST_CHECK(!is_sparse_row<ublas::matrix<coordinate_t>::iterator1>());
}

BOOST_AUTO_TEST_CASE(ublas_compressed_matrix_row) {
    BOOST_CHECK(is_sparse_row<ublas::compressed_matrix<coordinate_t>::iterator1>());
}

BOOST_AUTO_TEST_SUITE_END()


/******************************************************************************\
* Author: Matthew Beauregard Smith                                             *
* Affiliation: The University of Texas at Austin                               *
* Department: Oden Institute and Institute for Cellular and Molecular Biology  *
* PI: Edward Marcotte                                                          *
* Project: Protein Fluorosequencing                                            *
\******************************************************************************/

// Boost unit test framework (recommended to be the first include):
#include <boost/test/unit_test.hpp>

// File under test:
#include "stuck-dye-emission.h"

// Standard C++ library headers:
#include <functional>

// External headers:
#include "fakeit.hpp"

// Local project headers:
#include "hmm/fit/error-model-fitter.h"
#include "hmm/state-vector/stuck-dye-state-vector.h"
#include "test-util/fakeit.h"

namespace whatprot {

namespace {
using boost::unit_test::tolerance;
using fakeit::Fake;
using fakeit::Mock;
using fakeit::Verify;
using fakeit::VerifyNoOtherInvocations;
using std::function;
using whatprot::test_util::Close;
const double TOL = 0.000000001;
}  // namespace

BOOST_AUTO_TEST_SUITE(hmm_suite)
BOOST_AUTO_TEST_SUITE(step_suite)
BOOST_AUTO_TEST_SUITE(stuck_dye_emission_suite)

BOOST_AUTO_TEST_CASE(constructor_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return 0.5;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int expected_size = num_timesteps * num_channels * 2;
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.num_channels == num_channels);
    BOOST_TEST(e.channel == channel);
    BOOST_TEST(e.values[0] == 0.5);
}

BOOST_AUTO_TEST_CASE(prob_multiple_timesteps_test, *tolerance(TOL)) {
    int num_timesteps = 3;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(1, 0) = 1.0;
    rad(2, 0) = 2.0;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int expected_size = num_timesteps * num_channels * 2;
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 1));
    BOOST_TEST(e.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(e.prob(1, 0, 0) == pdf(1.0, 1));
    BOOST_TEST(e.prob(2, 0, 0) == pdf(2.0, 0));
    BOOST_TEST(e.prob(2, 0, 0) == pdf(2.0, 1));
}

BOOST_AUTO_TEST_CASE(prob_multiple_timesteps_const_test, *tolerance(TOL)) {
    int num_timesteps = 3;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(1, 0) = 1.0;
    rad(2, 0) = 2.0;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int expected_size = num_timesteps * num_channels * 2;
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    const StuckDyeEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 1));
    BOOST_TEST(ce.prob(1, 0, 0) == pdf(1.0, 0));
    BOOST_TEST(ce.prob(1, 0, 0) == pdf(1.0, 1));
    BOOST_TEST(ce.prob(2, 0, 0) == pdf(2.0, 0));
    BOOST_TEST(ce.prob(2, 0, 0) == pdf(2.0, 1));
}

BOOST_AUTO_TEST_CASE(prob_multiple_channels_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 3;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(0, 2) = 0.2;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int expected_size = num_timesteps * num_channels * 2;
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(e.prob(0, 0, 0) == pdf(0.0, 1));
    BOOST_TEST(e.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(e.prob(0, 1, 0) == pdf(0.1, 1));
}

BOOST_AUTO_TEST_CASE(prob_multiple_channels_const_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 3;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 0.0;
    rad(0, 1) = 0.1;
    rad(0, 2) = 0.2;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.042;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int expected_size = num_timesteps * num_channels * 2;
    BOOST_TEST(e.values.size() == expected_size);
    BOOST_TEST(e.num_timesteps == num_timesteps);
    const StuckDyeEmission& ce = e;
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 0));
    BOOST_TEST(ce.prob(0, 0, 0) == pdf(0.0, 1));
    BOOST_TEST(ce.prob(0, 1, 0) == pdf(0.1, 0));
    BOOST_TEST(ce.prob(0, 1, 0) == pdf(0.1, 1));
}

BOOST_AUTO_TEST_CASE(forward_basic_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector sdsv;
    sdsv.dye = 0.7;
    sdsv.no_dye = 0.3;
    e.forward(&num_edmans, &sdsv);
    BOOST_TEST(sdsv.no_dye == 0.3 * pdf(1.0, 0));
    BOOST_TEST(sdsv.dye == 0.7 * pdf(1.0, 1));
}

BOOST_AUTO_TEST_CASE(forward_multiple_timestamps_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(1, 0) = 1.1;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 1;
    StuckDyeStateVector sdsv;
    sdsv.dye = 0.7;
    sdsv.no_dye = 0.3;
    e.forward(&num_edmans, &sdsv);
    BOOST_TEST(sdsv.no_dye == 0.3 * pdf(1.1, 0));
    BOOST_TEST(sdsv.dye == 0.7 * pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(forward_multiple_channels_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(0, 1) = 1.1;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector sdsv;
    sdsv.dye = 0.7;
    sdsv.no_dye = 0.3;
    e.forward(&num_edmans, &sdsv);
    BOOST_TEST(sdsv.no_dye == 0.3 * pdf(1.0, 0) * pdf(1.1, 0));
    BOOST_TEST(sdsv.dye == 0.7 * pdf(1.0, 1) * pdf(1.1, 0));
}

BOOST_AUTO_TEST_CASE(forward_multiple_channels_other_channel_test,
                     *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(0, 1) = 1.1;
    int channel = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector sdsv;
    sdsv.dye = 0.7;
    sdsv.no_dye = 0.3;
    e.forward(&num_edmans, &sdsv);
    BOOST_TEST(sdsv.no_dye == 0.3 * pdf(1.0, 0) * pdf(1.1, 0));
    BOOST_TEST(sdsv.dye == 0.7 * pdf(1.0, 0) * pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(backward_basic_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector input;
    input.dye = 0.7;
    input.no_dye = 0.3;
    StuckDyeStateVector output;
    e.backward(input, &num_edmans, &output);
    BOOST_TEST(output.no_dye == 0.3 * pdf(1.0, 0));
    BOOST_TEST(output.dye == 0.7 * pdf(1.0, 1));
}

BOOST_AUTO_TEST_CASE(backward_multiple_timestamps_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(1, 0) = 1.1;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 1;
    StuckDyeStateVector input;
    input.dye = 0.7;
    input.no_dye = 0.3;
    StuckDyeStateVector output;
    e.backward(input, &num_edmans, &output);
    BOOST_TEST(output.no_dye == 0.3 * pdf(1.1, 0));
    BOOST_TEST(output.dye == 0.7 * pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(backward_multiple_channels_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(0, 1) = 1.1;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector input;
    input.dye = 0.7;
    input.no_dye = 0.3;
    StuckDyeStateVector output;
    e.backward(input, &num_edmans, &output);
    BOOST_TEST(output.no_dye == 0.3 * pdf(1.0, 0) * pdf(1.1, 0));
    BOOST_TEST(output.dye == 0.7 * pdf(1.0, 1) * pdf(1.1, 0));
}

BOOST_AUTO_TEST_CASE(backward_multiple_channels_other_channel_test,
                     *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.0;
    rad(0, 1) = 1.1;
    int channel = 1;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    StuckDyeStateVector input;
    input.dye = 0.7;
    input.no_dye = 0.3;
    StuckDyeStateVector output;
    e.backward(input, &num_edmans, &output);
    BOOST_TEST(output.no_dye == 0.3 * pdf(1.0, 0) * pdf(1.1, 0));
    BOOST_TEST(output.dye == 0.7 * pdf(1.0, 0) * pdf(1.1, 1));
}

BOOST_AUTO_TEST_CASE(improve_fit_basic_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.2;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    double probability = 0.98765;
    StuckDyeStateVector forward_sdsv;
    forward_sdsv.dye = 0.7;
    forward_sdsv.no_dye = 0.3;
    StuckDyeStateVector backward_sdsv;
    backward_sdsv.dye = 0.6;
    backward_sdsv.no_dye = 0.4;
    StuckDyeStateVector next_backward_sdsv;
    next_backward_sdsv.dye = 0.8;
    next_backward_sdsv.no_dye = 0.2;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(forward_sdsv,
                  backward_sdsv,
                  next_backward_sdsv,
                  num_edmans,
                  probability,
                  &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.2, TOL), 0, Close(0.3 * 0.4 / 0.98765, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.2, TOL), 1, Close(0.7 * 0.6 / 0.98765, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_CASE(improve_fit_multiple_timesteps_test, *tolerance(TOL)) {
    int num_timesteps = 2;
    int num_channels = 1;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.2;
    rad(1, 0) = 1.3;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 1;
    double probability = 0.98765;
    StuckDyeStateVector forward_sdsv;
    forward_sdsv.dye = 0.7;
    forward_sdsv.no_dye = 0.3;
    StuckDyeStateVector backward_sdsv;
    backward_sdsv.dye = 0.6;
    backward_sdsv.no_dye = 0.4;
    StuckDyeStateVector next_backward_sdsv;
    next_backward_sdsv.dye = 0.8;
    next_backward_sdsv.no_dye = 0.2;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(forward_sdsv,
                  backward_sdsv,
                  next_backward_sdsv,
                  num_edmans,
                  probability,
                  &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.3, TOL), 0, Close(0.3 * 0.4 / 0.98765, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.3, TOL), 1, Close(0.7 * 0.6 / 0.98765, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_CASE(improve_fit_multiple_dye_colors_test, *tolerance(TOL)) {
    int num_timesteps = 1;
    int num_channels = 2;
    Radiometry rad(num_timesteps, num_channels);
    rad(0, 0) = 1.2;
    rad(0, 1) = 1.3;
    int channel = 0;
    function<double(double, int)> pdf = [](double observed,
                                           int state) -> double {
        return observed + 0.23 * (double)state;
    };
    StuckDyeEmission e(rad, channel, pdf);
    int num_edmans = 0;
    double probability = 0.98765;
    StuckDyeStateVector forward_sdsv;
    forward_sdsv.dye = 0.7;
    forward_sdsv.no_dye = 0.3;
    StuckDyeStateVector backward_sdsv;
    backward_sdsv.dye = 0.6;
    backward_sdsv.no_dye = 0.4;
    StuckDyeStateVector next_backward_sdsv;
    next_backward_sdsv.dye = 0.8;
    next_backward_sdsv.no_dye = 0.2;
    ErrorModelFitter emf;
    LogNormalDistributionFitter* original_dist_fit = emf.distribution_fit;
    Mock<LogNormalDistributionFitter> df_mock;
    Fake(Method(df_mock, add_sample));
    emf.distribution_fit = &df_mock.get();
    e.improve_fit(forward_sdsv,
                  backward_sdsv,
                  next_backward_sdsv,
                  num_edmans,
                  probability,
                  &emf);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.2, TOL), 0, Close(0.3 * 0.4 / 0.98765, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.2, TOL), 1, Close(0.7 * 0.6 / 0.98765, TOL)))
            .Exactly(1);
    Verify(Method(df_mock, add_sample)
                   .Using(Close(1.3, TOL),
                          0,
                          Close((0.3 * 0.4 + 0.7 * 0.6) / 0.98765, TOL)))
            .Exactly(1);
    VerifyNoOtherInvocations(df_mock);
    emf.distribution_fit = original_dist_fit;  // This avoids breaking cleanup.
}

BOOST_AUTO_TEST_SUITE_END()  // stuck_dye_emission_suite
BOOST_AUTO_TEST_SUITE_END()  // step_suite
BOOST_AUTO_TEST_SUITE_END()  // hmm_suite

}  // namespace whatprot


#ifndef YASMIC_UTIL_FILTERED_MATRIX
#define YASMIC_UTIL_FILTERED_MATRIX

#if _MSC_VER >= 1400
    // disable the warning for deprecated c++ commands
    #pragma warning( push )
	#pragma warning( disable : 4996 )
#endif // _MSC_VER >= 1400

#include <boost/iterator/filter_iterator.hpp>
#include <algorithm>
#include <functional>

#include <boost/type_traits.hpp>

namespace yasmic
{
	namespace impl
	{
		template <class Matrix>
		struct filtered_matrix_nonzero_helper
		{
			typedef typename smatrix_traits<Matrix>::size_type s;
			typedef typename smatrix_traits<Matrix>::value_type v;

			typedef boost::tuple<s,s,v> nz;
		};

		template <class Matrix, class FilterMap>
		class filtered_matrix_const_iterator
		: public boost::iterator_facade<
            filtered_matrix_const_iterator<Matrix, FilterMap>,
			typename filtered_matrix_nonzero_helper<Matrix>::nz const,
            boost::forward_traversal_tag,
            typename filtered_matrix_nonzero_helper<Matrix>::nz  const >
        {
        public:
			filtered_matrix_const_iterator()
				: _m(0), _fm(NULL)
			{}

            filtered_matrix_const_iterator(const Matrix& m, FilterMap fm,
				std::pair<typename smatrix_traits<Matrix>::nonzero_iterator,
						typename smatrix_traits<Matrix>::nonzero_iterator> i)
				: _m(&m), _fm(&fm), _i(i.first), _iend(i.second)
			{
				skip_nnzs();
			}

        private:
            friend class boost::iterator_core_access;

            void increment()
            {
				++_i;
				skip_nnzs();
            }

            bool equal(filtered_matrix_const_iterator const& other) const
            {
				return (_i == other._i);
            }

            typename filtered_matrix_nonzero_helper<Matrix>::nz
            dereference() const
            {
				typename impl::filtered_matrix_nonzero_helper<Matrix>::s r,c;

				r = (*_fm)[row(*_i,*_m)]-1;
				c = (*_fm)[column(*_i,*_m)]-1;

            	return boost::make_tuple(r, c, value(*_i,*_m));
            }

			const Matrix *_m;
			typename boost::add_pointer<FilterMap>::type _fm;
			typename smatrix_traits<Matrix>::nonzero_iterator _i;
			typename smatrix_traits<Matrix>::nonzero_iterator _iend;

			void skip_nnzs()
			{
				// advance to the next non-filtered nnz.
				while (_i != _iend && (!(*_fm)[row(*_i,*_m)] || !(*_fm)[column(*_i,*_m)]))
				{
					++_i;
				}
			}
        };
	}

	template <class Matrix, class RowColFilterMap>
	class row_column_filtered_matrix
	{
	public:
		// handle the matrix traits
		typedef typename smatrix_traits<Matrix>::index_type index_type;
		typedef typename smatrix_traits<Matrix>::value_type value_type;
		typedef typename smatrix_traits<Matrix>::size_type size_type;

		typedef boost::tuple<index_type, index_type, value_type> nonzero_descriptor;
		typedef impl::filtered_matrix_const_iterator<Matrix, RowColFilterMap> nonzero_iterator;

		typedef void row_iterator;

		typedef void row_nonzero_iterator;
		typedef void row_nonzero_descriptor;
		typedef void column_iterator;

		typedef typename smatrix_traits<Matrix>::properties properties;

		typedef typename smatrix_traits<Matrix>::nz_index_type nz_index_type;

		row_column_filtered_matrix(const Matrix &m, RowColFilterMap fm)
			: _nzi(m, fm, nonzeros(m)),
			  _nziend(m, fm, std::make_pair(nonzeros(m).second, nonzeros(m).second)),
			  _nrows(0), _ncols(0), _nnz(0)
		{
			nonzero_iterator i = _nzi;

			_nnz = 0;

			while (i != _nziend)
			{
				_nrows = std::max(row(*i,m),_nrows);
				_ncols = std::max(column(*i,m),_ncols);

				++i;
				++_nnz;
			}

			// we underestimate by one value
			++_nrows;
			++_ncols;
		}

 		row_column_filtered_matrix(const Matrix &m, RowColFilterMap fm,
			size_type nrows, size_type ncols)
			: _nzi(m, fm, nonzeros(m)),
			  _nziend(m, fm, std::make_pair(nonzeros(m).second, nonzeros(m).second)),
			  _nrows(nrows), _ncols(nrows), _nnz(0)
		{
			nonzero_iterator i = _nzi;

			_nnz = 0;

			while (i != _nziend)
			{
				++i;
				++_nnz;
			}
		}

		row_column_filtered_matrix(const Matrix &m, RowColFilterMap fm,
			size_type nrows, size_type ncols, size_type nnz)
			: _nzi(m, fm, nonzeros(m)),
			  _nziend(m, fm, std::make_pair(nonzeros(m).second, nonzeros(m).second)),
			  _nrows(nrows), _ncols(nrows), _nnz(nnz)
		{}

		std::pair<size_type, size_type> dimensions() const
		{
			return std::make_pair(_nrows, _ncols);
		}

		size_type nnz() const
		{
			return _nnz;
		}

		nonzero_iterator begin_nonzeros() const
		{
			return _nzi;
		}

		nonzero_iterator end_nonzeros() const
		{
			return _nziend;
		}

	private:
		size_type _nnz;
		size_type _nrows;
		size_type _ncols;

		nonzero_iterator _nzi;
		nonzero_iterator _nziend;
	};

	/*template <class Matrix, class RowColFilterMap>
	class row_column_filtered_matrix
	{
	private:
		typedef is_filtered_nonzero<nonzero_descriptor, Matrix, RowColFilterMap>
			filter_predicate;

	public:
		// handle the matrix traits
		typedef typename smatrix_traits<Matrix>::index_type index_type;
		typedef typename smatrix_traits<Matrix>::value_type value_type;

		typedef typename smatrix_traits<Matrix>::nonzero_descriptor nonzero_descriptor;

		typedef typename smatrix_traits<Matrix>::nonzero_iterator mat_nonzero_iterator;

		typedef typename smatrix_traits<Matrix>::row_iterator void;

		typedef typename smatrix_traits<Matrix>::row_nonzero_iterator void;
		typedef typename smatrix_traits<Matrix>::row_nonzero_descriptor void;

		typedef typename smatrix_traits<Matrix>::nz_index_type nz_index_type;

		typedef typename smatrix_traits<Matrix>::column_iterator void;

		row_column_filtered_matrix(const Matrix& im, RowColFilterMap fm)
			: m(im), rcfm(fm)
		{

		}

		std::pair<size_type, size_type> dimensions()
		{
			return make_pair(nrows, ncols);
		}

		size_type nnz()
		{
			return size_type;
		}

		nonzero_iterator begin_nonzeros()
		{
			filter_predicate p;
			p.m = m; p.fm = rcfm;

			mat_nonzero_iterator nzi, nziend
			boost::tie(nzi, nziend)  = nonzeros(m);
			return nonzero_iterator(p, nzi);
		}

		nonzero_iterator end_nonzeros()
		{
			filter_predicate p;
			p.m = m; p.fm = rcfm;

			mat_nonzero_iterator nzi, nziend
			boost::tie(nzi, nziend)  = nonzeros(m);
			return nonzero_iterator(p, nziend);
		}

	private:
		const Matrix& m;
		RowColFilterMap rcfm;

		size_type nnz;
		size_type nrows;
		size_type ncols;
	};*/

	template <class Iterator>
	void make_filter_map_from_indicator(Iterator start, Iterator end)
	{
		typedef typename std::iterator_traits<Iterator>::value_type t;

		std::partial_sum(
			boost::make_filter_iterator(
				std::bind2nd(std::greater<t>(), 0),
				start, end),
			boost::make_filter_iterator(
				std::bind2nd(std::greater<t>(), 0),
				end, end),
			boost::make_filter_iterator(
				std::bind2nd(std::greater<t>(), 0),
				start, end));

		//boost::filter_iterator<
		//	          std::bind2nd(std::greater<t>(), 0)
	}
};

#endif // YASMIC_UTIL_FILTERED_MATRIX


#include <pcl/point_types.h>
#include <pcl/features/boundary.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/kdtree.h>
#include <pcl/filters/extract_indices.h>

#include <Eigen/Dense>

#include "minipcl.h"

// set ksearch and radius to < 0 to disable
void mpcl_compute_normals(pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                          int ksearch,
                          double searchRadius,
                          pcl::PointCloud<pcl::Normal> &out)
{
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB> ());
    pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;

    ne.setSearchMethod (tree);
    ne.setInputCloud (cloud.makeShared());
    if (ksearch >= 0)
        ne.setKSearch (ksearch);
    if (searchRadius >= 0.0)
        ne.setRadiusSearch (searchRadius);
    ne.compute (out);
}

// Boundary estimation (uses normal estimation, hence the parameters).
void mpcl_estimate_boundaries(pcl::PointCloud<pcl::PointXYZRGB> &cloud,
                              float angle_threshold,
                              double search_radius,
                              int normal_ksearch,
                              double normal_search_radius,
                              char *out, size_t n)
{
    pcl::PointCloud<pcl::Normal> normals;
    mpcl_compute_normals(cloud, normal_ksearch, normal_search_radius, normals);
    pcl::PointCloud<pcl::Boundary> boundaries;
    pcl::BoundaryEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::Boundary> est;
    est.setInputCloud(cloud.makeShared());
    est.setInputNormals(normals.makeShared());

    if (search_radius >= 0) {
        est.setRadiusSearch(search_radius);
    }

    est.setSearchMethod(typename pcl::search::KdTree<pcl::PointXYZRGB>::Ptr(
                            new pcl::search::KdTree<pcl::PointXYZRGB>));
    est.compute(boundaries);

    for (size_t i = 0; i < n; i++) {
        out[i] = boundaries[i].boundary_point;
    }
}

void mpcl_sacnormal_set_axis(pcl::SACSegmentationFromNormals<pcl::PointXYZRGB, pcl::Normal> &sac,
                             double ax, double ay, double az)
{
    Eigen::Vector3f vect(ax,ay,az);
    sac.setAxis(vect);
}

void mpcl_extract(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &incloud,
                  pcl::PointCloud<pcl::PointXYZRGB> *outcloud,
                  pcl::PointIndices *indices,
                  bool negative)
{
    pcl::PointIndices::Ptr indicesptr (indices);
    pcl::ExtractIndices<pcl::PointXYZRGB> ext;
    ext.setInputCloud(incloud);
    ext.setIndices(indicesptr);
    ext.setNegative(negative);
    ext.filter(*outcloud);
}


// Copyright 2014, Max Planck Society.
// Distributed under the BSD 3-Clause license.
// (See accompanying file LICENSE.txt or copy at
// http://opensource.org/licenses/BSD-3-Clause)

/*!@file
 * This file contains an application of the Trimmed Grassmann Average to the frames of a movie, in order to compute
 * the meaningful first basis components in a robust manner. You may adapt the code to your needs/data by modifying
 * the function @c number2filename, which from the index of the frame returns a full path of the file of this frame.
 * To limit the memory footprint, the data is stored directly in the temporary memory of the algorithm as it is loaded
 * (see @c iterator_on_image_files). An observer flushes the results to the disk as they arrive from the algorithm (see
 * @c grassmann_pca_observer).
 */

#include <cstdio>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <include/grassmann_pca_with_trimming.hpp>
#include <include/private/boost_ublas_external_storage.hpp>
#include <include/private/boost_ublas_row_iterator.hpp>

#include <boost/program_options/cmdline.hpp>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/variables_map.hpp>
#include <boost/program_options/parsers.hpp>

#include <boost/iterator/iterator_facade.hpp>

#include <string>
#include <fstream>

#ifndef MAX_PATH
  // "funny" differences win32/posix
  #define MAX_PATH PATH_MAX
#endif

static std::string movielocation = "/is/ps/shared/users/jonas/movies/";
static std::string eigenvectorslocation = "./";

namespace grassmann_averages_pca
{
  namespace applications
  {
    void number2filename(size_t file_number, char *filename)
    {
      const size_t dir_num = file_number / 10000;
      sprintf(filename,
              (movielocation + "/starwars_%.3d/frame%.7d.png").c_str(),
              dir_num,
              file_number);
    }

    //! An iterator that will load the images on demand instead of storing everything on memory
    template <class T>
    class iterator_on_image_files :
      public boost::iterator_facade<
            iterator_on_image_files<T>
          , boost::numeric::ublas::vector<T>
          , std::random_access_iterator_tag
          , boost::numeric::ublas::vector<T> const& // const reference
        >
    {
    public:
      typedef iterator_on_image_files<T> this_type;
      typedef boost::numeric::ublas::vector<T> image_vector_type;
      iterator_on_image_files() : m_index(std::numeric_limits<size_t>::max())
      {}

      explicit iterator_on_image_files(size_t index)
        : m_index(index)
      {}

    private:
      friend class boost::iterator_core_access;

      typename this_type::difference_type distance_to(this_type const& r) const
      {
        return typename this_type::difference_type(r.m_index) - typename this_type::difference_type(m_index); // sign promotion
      }

      void increment()
      {
        m_index++;
        image_vector.resize(0, false);
      }

      bool equal(this_type const& other) const
      {
        return this->m_index == other.m_index;
      }

      image_vector_type const& dereference() const
      {
        if(image_vector.empty())
        {
          read_image();
        }
        return image_vector;
      }

      void read_image() const
      {
        char filename[MAX_PATH];
        number2filename(m_index, filename);

        if((m_index % 1000) == 0)
        {
          lock_t guard(internal_mutex);
          std::cout << "[THREAD " << boost::this_thread::get_id() << "] Reading " << filename << std::endl;
        }

        cv::Mat image = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
        if(!image.data)
        {
          std::ostringstream o;
          o << "error: could not load image '" << filename << "'";
          std::cerr << o.str() << std::endl;
          throw std::runtime_error(o.str());
        }

        const int w = image.size().width;
        const int h = image.size().height;

        image_vector.resize(w * h * 3);
        typename boost::numeric::ublas::vector<T>::iterator it = image_vector.begin();

        for(int y = 0; y < h; y++)
        {
          for(int x = 0; x < w; x++)
          {
            cv::Vec3b pixel = image.at<cv::Vec3b>(y, x);
            *it++ = pixel[0];
            *it++ = pixel[1];
            *it++ = pixel[2];
          }
        }
      }

      void advance(typename this_type::difference_type n)
      {
        if(n < 0)
        {
          assert((-n) <= static_cast<typename this_type::difference_type>(m_index));
          m_index -= n;
        }
        else
        {
          //assert(n + index <= matrix->size1());
          m_index += n;
        }
        if(n != 0)
        {
          image_vector.resize(0, false);
        }
      }

      size_t m_index;
      mutable boost::numeric::ublas::vector<T> image_vector;

      // for being able to log in a thread safe manner
      typedef boost::recursive_mutex mutex_t;
      typedef boost::lock_guard<mutex_t> lock_t;

      static mutex_t internal_mutex;

    };

    template <class T>
    typename iterator_on_image_files<T>::mutex_t iterator_on_image_files<T>::internal_mutex;

    template <class data_t>
    struct grassmann_pca_observer
    {
    private:
      size_t element_per_line_during_save;
      size_t last_nb_iteration;
      std::string filename_from_template(std::string template_, size_t i) const
      {
        char filename[MAX_PATH];
        sprintf(filename, template_.c_str(), i);
        return filename;
      }

      void save_vector(const data_t& v, std::string filename) const
      {
        std::ofstream f(filename);
        if(!f.is_open())
        {
          std::cerr << "[ERROR] Cannot open the file " << filename << " for writing" << std::endl;
          return;
        }

        std::cout << "-\tWriting file " << filename;

        typedef typename data_t::const_iterator element_iterator;

        element_iterator itelement(v.begin());
        for(int i = 0; i < v.size(); i++, ++itelement)
        {
          if((i + 1) % element_per_line_during_save == 0)
          {
            f << std::endl;
          }

          f << *itelement << " ";
        }

        f.close();
        std::cout << " -- done" << std::endl;
      }

    public:
      grassmann_pca_observer(size_t element_per_line_during_save_) :
        element_per_line_during_save(element_per_line_during_save_)
      {}

      void log_error_message(const char* message) const
      {
        std::cout << message << std::endl;
      }

      //! This is called after centering the data in order to keep track
      //! of the mean of the dataset
      void signal_mean(const data_t& mean) const
      {
        std::cout << "* Mean computed" << std::endl;
        save_vector(mean, "./mean_vector.txt");
      }

      //! Called after the computation of the PCA
      void signal_pca(const data_t& pca,
                      size_t current_eigenvector_dimension) const
      {
        std::cout << "* PCA subspace " << current_eigenvector_dimension << " computed" << std::endl;
        save_vector(pca, filename_from_template("./vector_pca_%.7d.txt", current_eigenvector_dimension));
      }

      //! Called each time a new eigenvector is computed
      void signal_eigenvector(const data_t& current_eigenvector,
                              size_t current_eigenvector_dimension) const
      {
        std::cout << "* Eigenvector subspace " << current_eigenvector_dimension << " computed in # " << last_nb_iteration << " iterations " << std::endl;
        save_vector(current_eigenvector, filename_from_template("./vector_subspace_%.7d.txt", current_eigenvector_dimension));
      }

      //! Called at every step of the algorithm, at the end of the step
      void signal_intermediate_result(
        const data_t& current_eigenvector_state,
        size_t current_eigenvector_dimension,
        size_t current_iteration_step)
      {
        last_nb_iteration = current_iteration_step;
        if((current_iteration_step % 100) == 0)
        {
          std::cout << "* Trimming subspace " << current_eigenvector_dimension << " @ iteration " << current_iteration_step << std::endl;
        }
      }

    };

  }
}

int main(int argc, char *argv[])
{
  namespace po = boost::program_options;

  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("nb-frames,f",       po::value<int>(),           "number of frames in the movie")
    ("movie,m",           po::value<std::string>(),   "full path to the movie location")
    ("max-dimensions,d",  po::value<int>(),           "requested number of components for the computation of the trimmed grassmann average")
    ("max-iterations",    po::value<int>(),           "maximum number of iterations (defaults to the number of frames)")
    ("nb-pca-steps",      po::value<int>(),           "number of pca steps")
    ("trimming-percentage",      po::value<float>(),  "percentage of trimming")
    ("nb-processors",      po::value<int>(),          "number of processors used (defaults to 1)")
  ;

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

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

  if(!vm.count("trimming-percentage"))
  {
    std::cerr << "the parameter 'trimming-percentage' should be set, now exiting...\n";
    return 1;
  }
  const float trimming_percentage = vm["trimming-percentage"].as<float>();

  size_t num_frames = 100; //179415;
  if(vm.count("nb-frames"))
  {
    std::cout << "The movie contains #" << vm["nb-frames"].as<int>() << " frames.\n";
    num_frames = vm["nb-frames"].as<int>();
  }
  else
  {
    std::cout << "Nb of frames not set, setting it to" << num_frames << "\n";
  }

  if(vm.count("movie"))
  {
    std::cout << "Directory of the movie #" << vm["movie"].as<std::string>() << "\n";
    movielocation = vm["movie"].as<std::string>();
  }

  size_t max_dimension = 30;
  if(vm.count("max-dimensions"))
  {
    std::cout << "Number of components requested #" << vm["max-dimensions"].as<int>() << "\n";
    max_dimension = vm["max-dimensions"].as<int>();
  }
  else
  {
    std::cout << "Nb of components not set, setting it to" << max_dimension << "\n";
  }

  size_t max_iterations = num_frames;
  if(vm.count("max-iterations"))
  {
    std::cout << "Maximum number of iterations #" << vm["max-iterations"].as<int>() << "\n";
    max_iterations = vm["max-iterations"].as<int>();
  }

  size_t nb_pca_steps = 3;
  if(vm.count("nb-pca-steps"))
  {
    std::cout << "Number of PCA steps #" << vm["nb-pca-steps"].as<int>() << "\n";
    nb_pca_steps = vm["nb-pca-steps"].as<int>();
  }
  else
  {
    std::cout << "Number of PCA steps not set, setting it to" << nb_pca_steps << "\n";
  }

  int nb_processors = 0;
  if(vm.count("nb-processors"))
  {
    std::cout << "Number of processors #" << vm["nb-processors"].as<int>() << "\n";
    nb_processors = vm["nb-processors"].as<int>();
  }

  namespace ub = boost::numeric::ublas;
  using namespace grassmann_averages_pca;
  using namespace grassmann_averages_pca::applications;
  using namespace grassmann_averages_pca::details::ublas_helpers;

  // Reading the first image to have the dimensions
  size_t rows(0);
  size_t cols(0);

  {
    char filename[MAX_PATH];
    number2filename(1, filename);

    cv::Mat image = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
    cv::Size image_size = image.size();
    rows = image_size.height;
    cols = image_size.width;
  }

  // type of the scalars manipulated by the algorithm
  typedef float input_array_type;

  // Allocate data
  std::cout << "=== Allocate ===" << std::endl;
  std::cout << "  Number of images: " << num_frames << std::endl;
  std::cout << "  Image size:       " << cols << "x" << rows << " (RGB)" << std::endl;
  std::cout << "  Data size:        " << (num_frames*rows*cols * 3 * sizeof(input_array_type)) / (1024 * 1024) << " MB" << std::endl;

  iterator_on_image_files<float> iterator_file_begin(1);
  iterator_on_image_files<float> iterator_file_end(num_frames + 1);

  // type of the data extracted from the input iterators
  typedef ub::vector<input_array_type> data_t;
  // type of the observer
  typedef grassmann_pca_observer<data_t> observer_t;
  // type of the trimmed grassmann algorithm
  typedef grassmann_pca_with_trimming< data_t, observer_t > grassmann_pca_with_trimming_t;

  // main instance
  grassmann_pca_with_trimming_t instance(trimming_percentage / 100);

  typedef std::vector<data_t> output_eigenvector_collection_t;
  output_eigenvector_collection_t v_output_eigenvectors(max_dimension);

  if(nb_processors > 0)
  {
    if(!instance.set_nb_processors(nb_processors))
    {
      std::cerr << "[configuration]" << "Incorrect number of processors. Please consult the documentation (was " << nb_processors << ")" << std::endl;
      return 1;
    }
  }

  if(!instance.set_nb_steps_pca(nb_pca_steps))
  {
    std::cerr << "[configuration]" << "Incorrect number of regular PCA steps. Please consult the documentation (was " << nb_pca_steps << ")" << std::endl;
    return 1;
  }

  // setting the observer
  observer_t my_simple_observer(cols);
  if(!instance.set_observer(&my_simple_observer))
  {
    std::cerr << "[configuration]" << "Error while setting the observer" << std::endl;
    return 1;
  }

  // requesting the centering of the data
  if(!instance.set_centering(true))
  {
    std::cerr << "[configuration]" << "Error while configuring the centering" << std::endl;
    return 1;
  }

  // running the computation
  bool ret = instance.batch_process(
    max_iterations,
    max_dimension,
    iterator_file_begin,
    iterator_file_end,
    v_output_eigenvectors.begin());

  // Results are saved in the observer instance

  if(!ret)
  {
    std::cerr << "The process returned an error" << std::endl;
    return 1;
  }

  return 0;
}


/*
 * 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/.
 */
/*******************************************************************************
 *  Builds the local orthogonal virtual orbitals (LOVOs)
 *
 *  Based on Fortran90 code by Jeremy Chwee, David Krisiloff,
 *          and Johannes M Dieterich
 *
 *  Author: Francis Ricci
 ******************************************************************************/

#include <iostream>
#include <armadillo>
#include <stdexcept>
#include <sstream>

#include <tiger_cpp/local_ortho_virt.h>
#include <tiger_cpp/lovo_math.h>
#include <tiger_cpp/PipekMezey.h>
#include <tiger_cpp/OrbitalParsers.h>
#include <tiger_cpp/Timer.h>
#include <tiger_cpp/TigerStructs.h>

using namespace::std;
using namespace::arma;

/******************************************************************************/

// static function declarations
static mat min_bas_space(size_t min_bas, size_t num_bas, size_t num_internal,
        vector<size_t>& bf_per_atom, mat& overlap, mat& mos);

static vector<vector<size_t>> separate(BasisData *basis);

static vector<size_t> funcs(BasisData *basis);

static void localize_inactive_space(mat& orbitals, BasisData *basis,
        vector<size_t>& bf_per_atom, size_t num_inactive, size_t num_frozen);

static mat proto(size_t num_bas, size_t natom, mat& overlap,
        vector<vector<size_t>>& ang_bf, vector<vector<size_t>>& min_ang_bf);

static mat hard(size_t num_bas, size_t num_internal, size_t min_bas,
        size_t natom, mat& mos, mat& overlap, mat& valence_virts, mat& protohard,
        vector<size_t>& bf_per_atom, vector<size_t>& min_bf_per_atom);

/******************************************************************************/

// Construct the local orthogonal virtual orbitals (LOVOs)
void ortho_virt(BasisData *basis){
    // THIS FUNCTION GENERATES ORTHONORMAL VIRTUAL ORBITALS
    // J. E. Subotnik, A. D. Dutoi, M. Head-Gordon, JCP, 123, 114108 (2005)
    //
    // Please note that if your AO space uses 5 d functions you need to make a
    // modification to the protohard virtuals

	if (basis->_globals->get_global_bool("skip_lovos")){
		cout << "WARNING: Skipping LOVO generation for a local calculation!" << endl;
		cout << "         If this is not desired, check your input file!" << endl;
		return;
	}

	Timer timer;

    // construct minimal basis
	BasisData *min_basis = new ErkaleBasis(*basis->_globals, tiger::minimal);

    cout << "Starting LOVO generation" << endl;

    // get some system information
    size_t natom = basis->get_nAtoms();
    size_t min_bas = min_basis->get_nBas();

    // get some global variables
    size_t num_internal = basis->_globals->get_global_int("num_internal");
    size_t num_bas = basis->_globals->get_global_int("number_bas");
    size_t num_inactive = basis->_globals->get_global_int("num_inactive");
    size_t num_frozen = basis->_globals->get_global_int("num_frozen");

    if (min_bas < num_internal){
        stringstream error;
        error << "LOVO ERROR" << '\n';
        error << "The size of the internal orbitals is greater than the";
        error << " size of the minimal basis space" << '\n';
        error << "Either decrease the number of references or increase";
        error << " the size of the minimal basis" << endl;
        throw runtime_error(error.str());
    }

    // preparation for minimal basis space calculation
    size_t tot_bas = min_bas + num_bas;
    mat overlap = zeros(tot_bas,tot_bas);
    overlap.submat(0,0,num_bas-1,num_bas-1) = basis->get_overlap();
    overlap.submat(num_bas,num_bas,tot_bas-1,tot_bas-1) = min_basis->get_overlap();
    overlap.submat(0,num_bas,num_bas-1,tot_bas-1) = basis->get_overlap(min_basis);
    overlap.submat(num_bas,0,tot_bas-1,num_bas-1) = basis->get_overlap(min_basis).t();

    mat mos = get_MOs();

    if (mos.n_cols != num_bas)
      throw runtime_error("Number of basis functions does not match MOs");

    vector<size_t> bf_per_atom = basis->funcs();
    vector<size_t> min_bf_per_atom = min_basis->funcs();
    vector<vector<size_t>> ang_bf = basis->separate();
    vector<vector<size_t>> min_ang_bf = min_basis->separate();

    mat valence_virts = min_bas_space(min_bas, num_bas, num_internal,
            bf_per_atom, overlap, mos);

    mat protohard = proto(num_bas, natom, overlap, ang_bf, min_ang_bf);

    mat hard_virts = hard(num_bas, num_internal, min_bas, natom, mos, overlap,
            valence_virts, protohard, bf_per_atom, min_bf_per_atom);

    if (basis->_globals->get_global_bool("localize_inactives"))
    	localize_inactive_space(mos, basis, bf_per_atom, num_inactive, num_frozen);
    else
    	cout << "WARNING: Inactive orbital localization has been turned off!" << endl;

    mos.cols(num_internal,min_bas-1) = valence_virts;
    mos.cols(min_bas,num_bas-1) = hard_virts;

    set_MOs(mos);

    delete min_basis;

    cout << endl;
    cout << "*******************************" << endl;
    cout << "* Orbital localization complete" << endl;
    cout << "*******************************" << endl;
    cout << endl;

    timer.print_elapsed_time("LOVO calculation    wall time:");
}

/******************************************************************************/

// Performs minimal basis space calculation
static mat min_bas_space(size_t min_bas, size_t num_bas, size_t num_internal,
        vector<size_t>& bf_per_atom, mat& overlap, mat& mos){

    size_t tot_bas = min_bas + num_bas;
    size_t red_bas = min_bas - num_internal;

    // Orthogonalize the current AO basis
    mat ortho_vecs = eye(num_bas, num_bas);
    mat over_sub = overlap.submat(0,0,num_bas-1,num_bas-1);

    gram_schmidt(num_bas, ortho_vecs, over_sub);

    // Project minimal basis onto orthonormal AO basis, forming new overlap matrix
    over_sub = overlap.submat(0,num_bas,num_bas-1,tot_bas-1);
    mat proj_min_ao = project(min_bas, num_bas, over_sub, ortho_vecs);

    mat mos_sub = mos.cols(0,num_internal-1);
    over_sub = overlap.submat(0,0,num_bas-1,num_bas-1);
    mat proj_occ = proj_min_ao - (mos_sub * mos_sub.t() * over_sub * proj_min_ao);

    mat proj_over = proj_occ.t() * over_sub * proj_occ;
    proj_over = proj_over.submat(0,0,min_bas-1,min_bas-1);

    // Diagonalize overlap matrix, convert into valence virtual orbitals
    mat eigenvecs(min_bas,min_bas);
    vec eigenvals(min_bas);

    if (!eig_sym(eigenvals, eigenvecs, proj_over))
        throw runtime_error("Armadillo diagonalization failed.");

    mat virts = zeros(num_bas, red_bas);
    for (size_t i = 0; i < red_bas; i++)
        virts.col(i) = proj_occ*eigenvecs.col(i+num_internal);

    // normalization of valence virtuals
    normalize(virts, over_sub);
    for (size_t i = 0; i < red_bas; i++)
        virts.col(i) /= sqrt(dot((over_sub * virts.col(i)), virts.col(i)));

    PipekMezey pipek(bf_per_atom, virts, over_sub, red_bas, 0);
    // localize the valence virtuals, runtime_error if no convergence
    try {
      pipek.localize();
    }
    catch (runtime_error& err){
        cout << err.what() << endl;
        throw runtime_error("Problem localizing in min_bas_space!");
    }

    return virts;
}

/******************************************************************************/

static void localize_inactive_space(mat& orbitals, BasisData *basis,
    vector<size_t>& bf_per_atom, size_t num_inactive, size_t num_frozen){
    mat overlap = basis->get_overlap();
    mat orb_sub = orbitals.cols(num_frozen, num_inactive-1);

    PipekMezey pipek(bf_per_atom, orb_sub, overlap, num_inactive, num_frozen);
    try {
      pipek.localize();
    }
    catch (runtime_error& err){
        cout << err.what() << endl;
        throw runtime_error("Problem localizing in min_bas_space!");
    }

    orbitals.cols(num_frozen, num_inactive-1) = orb_sub;
}

/******************************************************************************/

// generate protohard virtual orbitals
static mat proto(size_t num_bas, size_t natom, mat& overlap,
        vector<vector<size_t>>& ang_bf, vector<vector<size_t>>& min_ang_bf){

    mat protohard = zeros(num_bas, num_bas);
    size_t iBas  = 0;
    size_t iMin  = 0;
    size_t iProt = 0;

    // iterate over all nuclei
    size_t inuc = 0;
    for (vector<size_t> nuc_bf : ang_bf){
        // looping over sets of shells having the same angular momentum component
        // different from the Fortran looping (ie. looping over px,py,pz separately)
        vector<size_t> min_nuc_bf = min_ang_bf.at(inuc);
        size_t tot_am = 0;
        for (size_t shell_funcs : nuc_bf){
            // orthonormalize AOs
            size_t end_bas = iBas+shell_funcs;
            mat ortho_vecs = eye(shell_funcs, shell_funcs);
            mat over_sub = overlap.submat(iBas,iBas,end_bas-1,end_bas-1);
            gram_schmidt(shell_funcs, ortho_vecs, over_sub);

            // project out minimal basis functions
            if (min_nuc_bf.size() > tot_am){
                size_t min_funcs = min_nuc_bf.at(tot_am);
                size_t end_min = iMin+num_bas+min_funcs;
                size_t red_funcs = shell_funcs - min_funcs;

                // project minimal AOs onto current AOs
                over_sub = overlap.submat(iBas,iMin+num_bas,end_bas-1,end_min-1);
                mat proj = project(min_funcs, shell_funcs, over_sub, ortho_vecs);

                // orthonormalize these AOs
                over_sub = overlap.submat(iBas, iBas, end_bas-1, end_bas-1);
                gram_schmidt(min_funcs, proj, over_sub);

                // project out the minimal basis components from current AO space
                mat density = proj * proj.t();
                proj = project_out(shell_funcs, shell_funcs, over_sub, density);

                // form overlap matrix
                mat proj_over = proj.t() * over_sub * proj;

                // diagonalize the matrix
                mat eigenvecs(shell_funcs, shell_funcs);
                vec eigenvals(shell_funcs);
                if (!eig_sym(eigenvals, eigenvecs, proj_over))
                    throw runtime_error("Armadillo diagonalization failed.");

                // form the new vectors
                mat vecs = zeros(shell_funcs, red_funcs);
                for (size_t i = 0; i < red_funcs; i++)
		  vecs.col(i) = proj * eigenvecs.col(min_funcs+i);

                // normalize the protohards
                normalize(vecs, over_sub);

                protohard.submat(iBas,iProt,end_bas-1,iProt+red_funcs-1) = vecs;

                iBas  += shell_funcs;
                iMin  += min_funcs;
                iProt += red_funcs;
            }
            // no minimal basis functions to project out here
            else {
                size_t end_prot = iProt+shell_funcs;
		protohard.submat(iBas,iProt,end_bas-1,end_prot-1) = ortho_vecs;

                iBas  = end_bas;
                iProt = end_prot;
            }
            tot_am++;
        }
        inuc++;
    }

    return protohard;
}

/******************************************************************************/

// generate hard virtual orbitals
static mat hard(size_t num_bas, size_t num_internal, size_t min_bas,
        size_t natom, mat& mos, mat& overlap, mat& valence_virts, mat& protohard,
        vector<size_t>& bf_per_atom, vector<size_t>& min_bf_per_atom){

    // The hard virtuals are constructed for each atom individually
    size_t iBas = 0;
    size_t hv_count = 0;
    mat hard_virts = zeros(num_bas, num_bas);

    /* Project all atomic orbitals for each atom onto the
       orthogonal complement of the minimal basis space. */

    // Make the projection operator
    mat proj_tmp = zeros(num_bas, min_bas);
    proj_tmp.cols(0,num_internal-1) = mos.cols(0,num_internal-1);
    proj_tmp.cols(num_internal,min_bas-1) = valence_virts.cols(0,min_bas-num_internal-1);

    mat proj_op = proj_tmp * proj_tmp.t();

    // iterate over all nuclei
    for (size_t inuc = 0; inuc < natom; inuc++){
        size_t nuc_funcs = bf_per_atom.at(inuc);
        size_t min_funcs = min_bf_per_atom.at(inuc);
        size_t hv_funcs  = nuc_funcs-min_funcs;

        // project out the minimal basis space from the current AO space
        mat over_sub = overlap.submat(0,iBas,num_bas-1,iBas+nuc_funcs-1);
	mat proj = -1 * proj_op * over_sub;

	for (size_t i = iBas; i < iBas+nuc_funcs; i++)
	  proj(i,i-iBas) = proj(i,i-iBas) + 1;

        over_sub = overlap.submat(0,0,num_bas-1,num_bas-1);
        mat proj_over = proj.t() * over_sub * proj;

        // diagonalize the matrix
        mat eigenvecs(nuc_funcs, nuc_funcs);
        vec eigenvals(nuc_funcs);
        if (!eig_sym(eigenvals, eigenvecs, proj_over))
            throw runtime_error("Armadillo diagonalization failed.");

        // form and normalize initial hard virtuals
        for (size_t i = 0; i < hv_funcs; i++){
	  for (size_t j = 0; j < nuc_funcs; j++)
	    hard_virts.col(hv_count+i) += eigenvecs(j,i+min_funcs) * proj.col(j);
	  hard_virts.col(hv_count+i) /= sqrt(eigenvals(i+min_funcs));
        }

        iBas += nuc_funcs;
        hv_count += hv_funcs;
    }

    /* Now we want to form the transformation matrix, Z, which will orthogonalize
       and rotate the initial hard virtuals into maximum overlap with the
       protohard virtuals.
       Z = (R^-1 T)(T^T R^-1 T)^(-1/2)  */
    hv_count = 0;

    // Form the overlap matrix of the initial hard virtuals, R
    // Form the overlap matrix between the protohard and initial hard virtuals, T
    for (size_t inuc = 0; inuc < natom; inuc++){
        size_t nuc_funcs = bf_per_atom.at(inuc);
        size_t min_funcs = min_bf_per_atom.at(inuc);
        size_t hard_funcs = nuc_funcs - min_funcs;

        mat over_sub = overlap.submat(0,0,num_bas-1,num_bas-1);
        mat hard_sub = hard_virts.cols(hv_count,hv_count+hard_funcs-1);
        mat proto_sub = protohard.cols(hv_count,hv_count+hard_funcs-1);
        mat over_hard = over_sub * hard_sub;

        mat T = (over_hard).t() * proto_sub;
        mat R = (over_hard).t() * hard_sub;

        // find inverse of R
        mat R_inv = zeros(hard_funcs,hard_funcs);
        try{
            R_inv = inv(R, "std");
        }
        catch (runtime_error& err){
            cout << err.what() << endl;
            throw runtime_error("Armadillo failed to invert matrix R!");
        }

        // form T^T R^-1 T
        mat RT = R_inv * T;
        mat TRT = T.t() * RT;

        // take inverse sqrt
        vec eigenvals = zeros(hard_funcs);
        mat eigenvecs = zeros(hard_funcs,hard_funcs);
        diag_inv_sqrt(hard_funcs, eigenvals, eigenvecs, TRT);

        // form Z
        mat Z = RT * eigenvecs;

        // get the hard virtuals
        mat temp_hard = zeros(num_bas,hard_funcs);
        for (size_t i = 0; i < hard_funcs; i++){
            for (size_t j = 0; j < hard_funcs; j++)
                temp_hard.col(i) += Z(i,j) * hard_virts.col(hv_count+j);
        }

        hard_virts.cols(hv_count,hv_count+hard_funcs-1) = temp_hard.cols(0,hard_funcs-1);
	hv_count += hard_funcs;
    }

    // normalize the new hard virtuals
    mat over_sub = overlap.submat(0,0,num_bas-1,num_bas-1);
    hard_virts.resize(num_bas,hv_count);
    normalize(hard_virts, over_sub);

    // form the overlap matrix
    mat over_hard = hard_virts.t() * over_sub * hard_virts;

    vec eigenvals = zeros(hv_count);
    mat eigenvecs = zeros(hv_count, hv_count);
    diag_inv_sqrt(hv_count, eigenvals, eigenvecs, over_hard);

    // Form new orthogonalized hard virtuals
    hard_virts *= eigenvecs;

    return hard_virts;
}


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

#include <boost/simd/function/simd/minusone.hpp>
#include <boost/simd/function/simd/tofloat.hpp>
#include <boost/simd/function/simd/sqr.hpp>
#include <boost/simd/function/simd/unary_minus.hpp>
#include <boost/simd/function/simd/is_greater.hpp>
#include <boost/simd/function/simd/is_ltz.hpp>
#include <boost/simd/function/simd/is_equal.hpp>
#include <boost/simd/function/simd/fast_frexp.hpp>
#include <boost/simd/function/simd/seladd.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/multiplies.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/logical_or.hpp>
#include <boost/simd/function/simd/if_allbits_else.hpp>
#include <boost/simd/function/simd/if_else_zero.hpp>
#include <boost/simd/function/simd/is_eqz.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/invlog_10.hpp>
#include <boost/simd/constant/log_2olog_10.hpp>
#include <boost/simd/constant/log_2hi.hpp>
#include <boost/simd/constant/log_2lo.hpp>
#include <boost/simd/constant/half.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/arch/common/detail/tags.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/scalar_of.hpp>
#include <boost/config.hpp>

#ifndef BOOST_SIMD_NO_NANS
#include <boost/simd/function/simd/is_nan.hpp>
#endif
#ifndef BOOST_SIMD_NO_INFINITIES
#include <boost/simd/constant/inf.hpp>
#endif

namespace boost { namespace simd
{
  namespace detail
  {
    namespace bd =  boost::dispatch;
    namespace bs =  boost::simd;

    template < class A0 >
    struct logarithm< A0, tag::simd_type, double>
    {
      static BOOST_FORCEINLINE void kernel_log(const A0& a0,
                                    A0& dk,
                                    A0& hfsq,
                                    A0& s,
                                    A0& r,
                                    A0& f) BOOST_NOEXCEPT
    {
      using i_t = bd::as_integer_t<A0, signed>;
      i_t k;
      A0 x = frexp(a0, k); //, fast_);
      const i_t x_lt_sqrthf = if_else_zero(is_greater(Sqrt_2o_2<A0>(), x),Mone<i_t>());
      k = k+x_lt_sqrthf; //+=
      f = minusone(x+bitwise_and(x, x_lt_sqrthf));
      dk = tofloat(k);
      s = f/(Two<A0>()+f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0,
                    0x3fd999999997fa04ll,
                    0x3fcc71c51d8e78afll,
                    0x3fc39a09d078c69fll
                    > (w);
      A0 t2= z*horn<A0,
                    0x3fe5555555555593ll,
                    0x3fd2492494229359ll,
                    0x3fc7466496cb03dell,
                    0x3fc2f112df3e5244ll
                    > (w);
      r = t2+t1;
      hfsq = (Half<A0>()* sqr(f));
    }

    static BOOST_FORCEINLINE A0 log(const A0& a0) BOOST_NOEXCEPT
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y =  (dk* Log_2hi<A0>())-
        ((hfsq-(s*(hfsq+R)+(dk*Log_2lo<A0>())))-f);
      return finalize(a0, y);
    }

    static BOOST_FORCEINLINE A0 log2(const A0& a0) BOOST_NOEXCEPT
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y = -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;
      return finalize(a0, y);
    }

    static BOOST_FORCEINLINE A0 log10(const A0& a0) BOOST_NOEXCEPT
    {
      A0 dk, hfsq, s, R, f;
      kernel_log(a0, dk, hfsq, s, R, f);
      A0 y = -(hfsq-(s*(hfsq+R))-f)*Invlog_10<A0>()+dk*Log_2olog_10<A0>();
      return finalize(a0, y);
    }
  private:
    static BOOST_FORCEINLINE A0 finalize(const A0& a0, const A0& y) BOOST_NOEXCEPT
    {
    #ifdef BOOST_SIMD_NO_NANS
      auto test =  is_ltz(a0);
    #else
      auto test =  logical_or(is_ltz(a0), is_nan(a0));
    #endif
      A0 y1 = if_nan_else(test, y);
    #ifndef BOOST_SIMD_NO_INFINITIES
      y1 = if_else(eq(a0, Inf<A0>()), a0, y1);
    #endif
      return if_else(is_eqz(a0), Minf<A0>(), y1);
    }
  };
  }
} }

#endif