//=======================================================================
// Copyright 2007 Aaron Windsor
//
// 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/graph/adjacency_list.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/biconnected_components.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/property_map.hpp>
#include <boost/vector_property_map.hpp>
#include <boost/test/minimal.hpp>

using namespace boost;

template <typename Graph>
void reset_edge_index(Graph& g)
{
  typename property_map<Graph, edge_index_t>::type index = get(edge_index, g);
  typename graph_traits<Graph>::edge_iterator ei, ei_end;
  typename graph_traits<Graph>::edges_size_type cnt = 0;
  for(tie(ei,ei_end) = edges(g); ei != ei_end; ++ei)
    put(index, *ei, cnt++);
}

template <typename Graph>
void make_line_graph(Graph& g, int size)
{
  typedef typename graph_traits<Graph>::vertex_descriptor vertex_t;

  vertex_t prev_vertex = add_vertex(g);

  for(int i = 1; i < size; ++i)
    {
      vertex_t curr_vertex = add_vertex(g);
      add_edge(curr_vertex, prev_vertex, g);
      prev_vertex = curr_vertex;
    }
}

struct UpdateVertexIndex
{
  template <typename Graph>
  void update(Graph& g)
  {
    typename property_map<Graph, vertex_index_t>::type index = get(vertex_index, g);
    typename graph_traits<Graph>::vertex_iterator vi, vi_end;
    typename graph_traits<Graph>::vertices_size_type cnt = 0;
    for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
      put(index, *vi, cnt++);
  }
};

struct NoVertexIndexUpdater
{
  template <typename Graph> void update(Graph& g) {}
};

template <typename Graph, typename VertexIndexUpdater>
void test_line_graph(VertexIndexUpdater vertex_index_updater, int size)
{

  Graph g;
  make_line_graph(g, size);
  vertex_index_updater.update(g);
  reset_edge_index(g);

  typedef std::vector< typename graph_traits<Graph>::edge_descriptor > edge_vector_t;
  typedef std::vector< edge_vector_t > embedding_storage_t;
  typedef iterator_property_map
    < typename embedding_storage_t::iterator,
      typename property_map<Graph, vertex_index_t>::type
    > embedding_t;

  embedding_storage_t embedding_storage(num_vertices(g));
  embedding_t embedding(embedding_storage.begin(), get(vertex_index, g));

  typename graph_traits<Graph>::vertex_iterator vi, vi_end;
  for(tie(vi,vi_end) = vertices(g); vi != vi_end; ++vi)
    std::copy(out_edges(*vi,g).first, out_edges(*vi,g).second, std::back_inserter(embedding[*vi]));

  BOOST_CHECK(biconnected_components(g, make_vector_property_map<int>(get(edge_index,g))) > 1);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));
  make_biconnected_planar(g, embedding);
  reset_edge_index(g);
  BOOST_CHECK(biconnected_components(g, make_vector_property_map<int>(get(edge_index,g))) == 1);
  BOOST_CHECK(boyer_myrvold_planarity_test(g));

}

int test_main(int, char* [])
{
  typedef adjacency_list
    <vecS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    VVgraph_t;

  typedef adjacency_list
    <vecS,
    listS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    VLgraph_t;

  typedef adjacency_list
    <listS,
    vecS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    LVgraph_t;

  typedef adjacency_list
    <listS,
    listS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    LLgraph_t;

  typedef adjacency_list
    <setS,
    setS,
    undirectedS,
    property<vertex_index_t, int>,
    property<edge_index_t, int>
    >
    SSgraph_t;

  test_line_graph<VVgraph_t>(NoVertexIndexUpdater(), 10);
  test_line_graph<VVgraph_t>(NoVertexIndexUpdater(), 50);

  test_line_graph<VLgraph_t>(UpdateVertexIndex(), 3);
  test_line_graph<VLgraph_t>(UpdateVertexIndex(), 30);

  test_line_graph<LVgraph_t>(NoVertexIndexUpdater(), 15);
  test_line_graph<LVgraph_t>(NoVertexIndexUpdater(), 45);

  test_line_graph<LLgraph_t>(UpdateVertexIndex(), 8);
  test_line_graph<LLgraph_t>(UpdateVertexIndex(), 19);

  test_line_graph<SSgraph_t>(UpdateVertexIndex(), 13);
  test_line_graph<SSgraph_t>(UpdateVertexIndex(), 20);

  return 0;
}


#include <scitbx/array_family/boost_python/flex_fwd.h>

#include <scitbx/array_family/boost_python/flex_wrapper.h>
#include <scitbx/serialization/single_buffered.h>
#include <scitbx/matrix/transpose_multiply.h>
#include <scitbx/math/utils.h>
#include <scitbx/vec2.h>
#include <scitbx/mat2.h>
#include <boost/python/make_constructor.hpp>
#include <boost/python/args.hpp>
#include <boost/python/return_arg.hpp>
#include "flex_helpers.h"

namespace scitbx { namespace serialization { namespace single_buffered {

  inline
  char* to_string(char* start, vec2<double> const& value)
  {
    return
      to_string(to_string(start, value[0]), value[1]);
  }

  template <>
  struct from_string<vec2<double> >
  {
    from_string(const char* start)
    {
      end = start;
      for(std::size_t i=0;i<2;i++) {
        from_string<double> proxy(end);
        value[i] = proxy.value;
        end = proxy.end;
      }
    }

    vec2<double> value;
    const char* end;
  };

}}} // namespace scitbx::serialization::single_buffered

#include <scitbx/array_family/boost_python/flex_pickle_single_buffered.h>

namespace scitbx { namespace af {
namespace {

  boost::python::tuple
  parts(
    versa<vec2<double>, flex_grid<> > const& O)
  {
    tiny<versa<double, flex_grid<> >, 2> result;
    std::size_t n = O.size();
    for(std::size_t i=0;i<2;i++) {
      result[i].resize(O.accessor());
      for(std::size_t j=0;j<n;j++) {
        result[i][j] = O[j][i];
      }
    }
    return boost::python::make_tuple(result[0], result[1]);
  }

  flex<vec2<double> >::type*
  join(
    af::const_ref<double> const& x,
    af::const_ref<double> const& y)
  {
    SCITBX_ASSERT(y.size() == x.size());
    af::shared<vec2<double> > result((af::reserve(x.size())));
    for(std::size_t i=0;i<x.size();i++) {
      result.push_back(vec2<double>(x[i],y[i]));
    }
    return new flex<vec2<double> >::type(result, result.size());
  }

  flex<vec2<double> >::type*
  from_double(
    af::const_ref<double> const& x)
  {
    SCITBX_ASSERT(x.size() % 2 == 0);
    std::size_t result_size = x.size() / 2;
    af::shared<vec2<double> > result((af::reserve(result_size)));
    const double* d = x.begin();
    for(std::size_t i=0;i<result_size;i++) {
      result.push_back(vec2<double>(d));
      d += 2;
    }
    return new flex<vec2<double> >::type(result, result.size());
  }

  flex<vec2<double> >::type*
  array_indices_as_double_from_array_focus_dimensions(
    vec2<int> const& ij)
  {
    std::size_t result_size = ij[0] * ij[1];
    af::shared<vec2<double> > result((af::reserve(result_size)));
    for(std::size_t i=0;i<ij[0];i++) { //slow
      for(std::size_t j=0;j<ij[1];j++) { //fast
        result.push_back(vec2<double>(i,j));
      }
    }
    return new flex<vec2<double> >::type(result, result.size());
  }

  flex_double
  as_double(flex<vec2<double> >::type const& a)
  {
    SCITBX_ASSERT(a.accessor().is_trivial_1d());
    flex_double result(a.size()*2, init_functor_null<double>());
    double* r = result.begin();
    const_ref<vec2<double> > a_ref = a.const_ref().as_1d();
    for(std::size_t i=0;i<a_ref.size();i++) {
      for(std::size_t j=0;j<2;j++) {
        *r++ = a_ref[i][j];
      }
    }
    return result;
  }

  vec2<double>
  vec2_min(flex<vec2<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec2<double> result(0,0);
    af::const_ref<vec2<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_min(a_ref[i]);
      }
    }
    return result;
  }

  vec2<double>
  vec2_max(flex<vec2<double> >::type const& a)
  {
    SCITBX_ASSERT(!a.accessor().is_padded());
    vec2<double> result(0,0);
    af::const_ref<vec2<double>, af::flex_grid<> > a_ref = a.const_ref();
    if (a_ref.size() > 0) {
      result = a_ref[0];
      for(std::size_t i=1;i<a_ref.size();i++) {
        result.each_update_max(a_ref[i]);
      }
    }
    return result;
  }

  vec2<double>
  mean_weighted_a_a(
    af::const_ref<vec2<double> > const& self,
    af::const_ref<double> const& weights)
  {
    return af::mean_weighted(self, weights);
  }

  af::shared<vec2<double> >
  mul_a_scalar(
    af::const_ref<vec2<double> > const& a,
    double f)
  {
    af::shared<vec2<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * f);
    }
    return result;
  }

  void
  imul_a_scalar(
    af::ref<vec2<double> > const& a,
    double f)
  {
    for(std::size_t i=0;i<a.size();i++) a[i] *= f;
  }

  af::shared<vec2<double> >
  div_a_as(
    af::ref<vec2<double> > const& lhs,
    af::ref<double> const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<vec2<double> > result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      SCITBX_ASSERT(rhs[i] != 0);
      result.push_back(lhs[i] / rhs[i]);
    }
    return result;
  }

  af::shared<vec2<double> >
  mul_a_mat2(
    af::const_ref<vec2<double> > const& a,
    mat2<double> const& m)
  {
    af::shared<vec2<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m);
    }
    return result;
  }

  af::shared<vec2<double> >
  rmul_a_mat2(
    af::const_ref<vec2<double> > const& a,
    mat2<double> const& m)
  {
    mat2<double> m_transposed = m.transpose();
    af::shared<vec2<double> > result((af::reserve(a.size())));
    for(std::size_t i=0;i<a.size();i++) {
      result.push_back(a[i] * m_transposed);
    }
    return result;
  }

  af::shared<double>
  dot_a_a(
    af::const_ref<vec2<double> > const& lhs,
    af::const_ref<vec2<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * rhs[i]);
    }
    return result;
  }

  af::shared<double>
  dot_a(
    af::const_ref<vec2<double> > const& lhs)
  {
    af::shared<double> result((af::reserve(lhs.size())));
    for(std::size_t i=0;i<lhs.size();i++) {
      result.push_back(lhs[i] * lhs[i]);
    }
    return result;
  }

  double
  sum_sq_(
    af::const_ref<vec2<double> > const& self)
  {
    double result = 0;
    for(std::size_t i=0;i<self.size();i++) {
      result += self[i] * self[i];
    }
    return result;
  }

  double
  norm_(
    af::const_ref<vec2<double> > const& self)
  {
    return std::sqrt(sum_sq_(self));
  }

  double
  min_distance_between_any_pair(
    af::const_ref<vec2<double> > const& lhs,
    af::const_ref<vec2<double> > const& rhs)
  {
    if (lhs.size() == 0) return 0;
    if (rhs.size() == 0) return 0;
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        math::update_min(min_length_sq, (lhs[i]-rhs[j]).length_sq());
      }
    }
    return std::sqrt(min_length_sq);
  }

  boost::python::tuple
  min_distance_between_any_pair_with_id(
    af::const_ref<vec2<double> > const& lhs,
    af::const_ref<vec2<double> > const& rhs)
  {
    if (lhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    if (rhs.size() == 0) return boost::python::make_tuple(0, 0, 0);
    double min_length_sq = (lhs[0]-rhs[0]).length_sq();
    double working_length_sq = 0;
    int best_i=0;
    int best_j=0;
    for(std::size_t i=0;i<lhs.size();i++) {
      for(std::size_t j=0;j<rhs.size();j++) {
        working_length_sq=(lhs[i]-rhs[j]).length_sq();
        if (working_length_sq < min_length_sq) {
          best_i=i;
          best_j=j;
          min_length_sq=working_length_sq;
        }
      }
    }
    return boost::python::make_tuple(std::sqrt(min_length_sq), best_i, best_j);
  }

  double
  max_distance(
    af::const_ref<vec2<double> > const& lhs,
    af::const_ref<vec2<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double max_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      math::update_max(max_length_sq, (lhs[i]-rhs[i]).length_sq());
    }
    return std::sqrt(max_length_sq);
  }

  double
  rms_difference(
    af::const_ref<vec2<double> > const& lhs,
    af::const_ref<vec2<double> > const& rhs)
  {
    SCITBX_ASSERT(lhs.size() == rhs.size());
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += (lhs[i]-rhs[i]).length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

  double
  rms_length(
    af::const_ref<vec2<double> > const& lhs)
  {
    if (lhs.size() == 0) return 0;
    double sum_length_sq = 0;
    for(std::size_t i=0;i<lhs.size();i++) {
      sum_length_sq += lhs[i].length_sq();
    }
    return std::sqrt(sum_length_sq / lhs.size());
  }

} // namespace <anonymous>

namespace boost_python {

  template <>
  struct flex_default_element<vec2<double> >
  {
    static vec2<double>
    get() { return vec2<double>(0,0); }
  };

  void wrap_flex_vec2_double()
  {
    using namespace boost::python;
    using boost::python::arg;
    typedef flex_wrapper<vec2<double> > f_w;
    f_w::plain("vec2_double")
      .def_pickle(flex_pickle_single_buffered<vec2<double>,
        2*pickle_size_per_element<double>::value>())
      .def("__init__", make_constructor(join))
      .def("__init__", make_constructor(from_double))
      .def("__init__", make_constructor(array_indices_as_double_from_array_focus_dimensions))
      .def("as_double", as_double)
      .def("add_selected",
        (object(*)(
          object const&,
          af::const_ref<std::size_t> const&,
          af::const_ref<vec2<double> > const&)) add_selected_unsigned_a,
        (arg("self"), arg("indices"), arg("values")))
      .def("min", vec2_min)
      .def("max", vec2_max)
      .def("sum", f_w::sum_a)
      .def("mean", f_w::mean_a)
      .def("mean_weighted", mean_weighted_a_a, (arg("self"), arg("weights")))
      .def("__add__", f_w::add_a_s)
      .def("__add__", f_w::add_a_a)
      .def("__iadd__", f_w::iadd_a_s)
      .def("__iadd__", f_w::iadd_a_a)
      .def("__sub__", f_w::sub_a_s)
      .def("__sub__", f_w::sub_a_a)
      .def("__isub__", f_w::isub_a_s)
      .def("__mul__", mul_a_scalar)
      .def("__rmul__", mul_a_scalar)
      .def("__imul__", imul_a_scalar, return_self<>())
      .def("__div__", div_a_as)
      .def("__truediv__", div_a_as)
      .def("__mul__", mul_a_mat2)
      .def("__rmul__", rmul_a_mat2)
      .def("dot", dot_a_a)
      .def("dot", dot_a)
      .def("transpose_multiply",
        (mat2<double>(*)(
          af::const_ref<vec2<double> > const&,
          af::const_ref<vec2<double> > const&)) matrix::transpose_multiply)
      .def("sum_sq", sum_sq_)
      .def("norm", norm_)
      .def("min_distance_between_any_pair", min_distance_between_any_pair)
      .def("min_distance_between_any_pair_with_id",
           min_distance_between_any_pair_with_id)
      .def("max_distance", max_distance)
      .def("rms_difference", rms_difference)
      .def("rms_length", rms_length)
      .def("parts", parts)
    ;
  }

}}} // namespace scitbx::af::boost_python


/**
 *  Project: The Stock Libraries
 *
 *  File: utilities.cpp
 *  Created: Jun 25, 2012
 *
 *  Author: Abhinav Sarje <abhinav.sarje@gmail.com>
 *
 *  Copyright (c) 2012-2017 Abhinav Sarje
 *  Distributed under the Boost Software License.
 *  See accompanying LICENSE file.
 */

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

#include "utilities.hpp"

namespace stock {

	/**
	 * specialization for vector3_t
	 */
	template <>
	vector3_t min <vector3_t> (vector3_t a, vector3_t b) {
		return vector3_t((a[0] < b[0] ? a[0] : b[0]), (a[1] < b[1] ? a[1] : b[1]),
						(a[2] < b[2] ? a[2] : b[2]));
	} // min <vector3_t>

	/**
	 * specialization for vector3_t
	 */
	template <>
	vector3_t max <vector3_t> (vector3_t a, vector3_t b) {
		return vector3_t((a[0] > b[0] ? a[0] : b[0]), (a[1] > b[1] ? a[1] : b[1]),
						(a[2] > b[2] ? a[2] : b[2]));
	} // max <vector3_t>

	/**
	 * apply log10 to all elements of the 2D matrix
	 */
	bool mat_log10_2d(unsigned int x_size, unsigned int y_size, real_t* &data) {
		if(data == NULL) {
			std::cerr << "error: data is null while calculating log10" << std::endl;
			return false;
		} // if
		for(unsigned int i = 0; i < x_size * y_size; ++ i) {
			if(data[i] <= 0) {
				if(data[i] == 0) {
					data[i] = 0.0;
					continue;
				} else {
					std::cerr << "error: matrix has a negative value. cannot calculate logarithm"
							<< std::endl;
					return false;
				} // if-else
			} else
				data[i] = log10(data[i]);
		} // for
		return true;
	} // mat_log10()

	/**
	 * compute the transpose of a matrix
	 * use boost libs ...
	 */
	bool transpose(unsigned int x_size, unsigned int y_size, const real_t *matrix, real_t* &transp) {
		if(matrix == NULL) {
			std::cerr << "error: matrix is NULL while tranposing" << std::endl;
			return false;
		} // if
		transp = new (std::nothrow) real_t[x_size * y_size];
		for(unsigned int y = 0; y < y_size; ++ y) {
			for(unsigned int x = 0; x < x_size; ++ x) {
				transp[y_size * x + y] = matrix[x_size * y + x];
			} // for x
		} // for y

		return true;
	} // transpose()

	/**
	 * matrix multiplication for two 3x3 matrices
	 * operation is:
	 * x1 x2 x3   a1 a2 a3   d1 d2 d3
	 * y1 y2 y3 = b1 b2 b3 x e1 e2 e3
	 * z1 z2 z3   c1 c2 c3   f1 f2 f3
	 * use boost libs ... and make it general ...
	 */
	bool mat_mul_3x3(vector3_t a, vector3_t b, vector3_t c,
					vector3_t d, vector3_t e, vector3_t f,
					vector3_t& x, vector3_t& y, vector3_t& z) {
		real_t *A = new (std::nothrow) real_t[9];
		real_t *B = new (std::nothrow) real_t[9];
		real_t *C = new (std::nothrow) real_t[9];

		A[0] = a[0]; A[1] = a[1]; A[2] = a[2];
		A[3] = b[0]; A[4] = b[1]; A[5] = b[2];
		A[6] = c[0]; A[7] = c[1]; A[8] = c[2];
		B[0] = d[0]; B[1] = d[1]; B[2] = d[2];
		B[3] = e[0]; B[4] = e[1]; B[5] = e[2];
		B[6] = f[0]; B[7] = f[1]; B[8] = f[2];

		for(int i = 0; i < 3; i ++) {
			for(int j = 0; j < 3; j ++) {
				C[3 * i + j] = 0.0;
				for(int k = 0; k < 3; k ++) {
					C[3 * i + j] += A[3 * i + k] * B[3 * k + j];
				} // for k
			} // for j
		} // for i

		x[0] = C[0]; x[1] = C[1]; x[2] = C[2];
		y[0] = C[3]; y[1] = C[4]; y[2] = C[5];
		z[0] = C[6]; z[1] = C[7]; z[2] = C[8];

		delete[] C;
		delete[] B;
		delete[] A;
		return true;
	} // mat_mul_3x3()

	/**
	 * matrix vector product for matrix of size 3x3 and vector of size 1x3
	 * operation is:
	 * x1   a1 a2 a3   d1
	 * x2 = b1 b2 b3 x d2
	 * x3   c1 c2 c3   d3
	 * note: transpose of d is used
	 * use boost libs ...
	 */
	bool mat_mul_3x1(vector3_t a, vector3_t b, vector3_t c, vector3_t d, vector3_t& x) {
		x[0] = a[0] * d[0] + a[1] * d[1] + a[2] * d[2];
		x[1] = b[0] * d[0] + b[1] * d[1] + b[2] * d[2];
		x[2] = c[0] * d[0] + c[1] * d[1] + c[2] * d[2];

		return true;
	} // mat_mul_3x1()

	/**
	 * specialized floor function
	 */
	vector3_t floor(vector3_t a) {
		return vector3_t(std::floor(a[0]), std::floor(a[1]), std::floor(a[2]));
	} // floor()

	/**
	 * comparison of complex numbers
	 */
	bool operator<(complex_t a, complex_t b) {
		if(std::real(a) < std::real(b)) return true;
		if(std::real(a) == std::real(b) && std::imag(a) < std::imag(b)) return true;
		//if(a.x < b.x) return true;
		//if(a.x == b.x && a.y < b.y) return true;
		return false;
	} // operator<()

	/**
	 * arithmetic operators for complex types
	 */

	complex_t operator*(complex_t c, complex_t s) {
		return complex_t(c.real() * s.real() - c.imag() * s.imag(),
						 c.real() * s.imag() + c.imag() * s.real());
	} // operator*()

	complex_t operator*(complex_t c, real_t s) {
		return complex_t(c.real() * s, c.imag() * s);
	} // operator*()

	complex_t operator*(real_t s, complex_t c) {
		return complex_t(c.real() * s, c.imag() * s);
	} // operator*()

	std::complex<long double> operator*(std::complex<long double> c, long double s) {
		return std::complex<long double>(c.real() * s, c.imag() * s);
	} // operator*()

	std::complex<long double> operator/(std::complex<long double> c, long double s) {
		return std::complex<long double>(c.real() / s, c.imag() / s);
	} // operator*()

	/**
	 * constructs element-by-element sum of two matrices into result
	 */
	bool mat_add(unsigned int x1_size, unsigned int y1_size, unsigned int z1_size,
					const std::vector<complex_t>& matrix1,
					unsigned int x2_size, unsigned int y2_size, unsigned int z2_size,
					const std::vector<complex_t>& matrix2,
					std::vector<complex_t>& result) {
		if(x1_size != x2_size || y1_size != y2_size || z1_size != z2_size
				|| matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for addition operation" << std::endl;
			return false;
		} // if
		result.clear();
		std::vector<complex_t>::const_iterator i1 = matrix1.begin();
		std::vector<complex_t>::const_iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			result.push_back((*i1) + (*i2));
		} // for
		return true;
	} // mat_add()

	/**
	 * performs in-place element-by-element sum of two matrices into first matrix
	 */
	bool mat_add_in(unsigned int x1_size, unsigned int y1_size, unsigned int z1_size,
					std::vector<complex_t>& matrix1,
					unsigned int x2_size, unsigned int y2_size, unsigned int z2_size,
					std::vector<complex_t>& matrix2) {
		if(x1_size != x2_size || y1_size != y2_size || z1_size != z2_size
				|| matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for addition operation" << std::endl;
			return false;
		} // if
		std::vector<complex_t>::iterator i1 = matrix1.begin();
		std::vector<complex_t>::iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			*i1 = (*i1) + (*i2);
		} // for
		return true;
	} // mat_add()

	/**
	 * scalar-matrix multiplication into result
	 */
	bool mat_mul(real_t scalar, const complex_vec_t& matrix, complex_vec_t& result) {
		result.clear();
		for(std::vector<complex_t>::const_iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			result.push_back((*i) * scalar);
		} // for
		return true;
	} // mat_mul()

	bool mat_mul(complex_t scalar, const complex_vec_t& matrix, complex_vec_t& result) {
		result.clear();
		for(complex_vec_t::const_iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			result.push_back((*i) * scalar);
		} // for
		return true;
	} // mat_mul()

	bool mat_mul(const complex_vec_t& matrix, real_t scalar, complex_vec_t& result) {
		return mat_mul(scalar, matrix, result);
	} // mat_mul()

	bool mat_mul(const complex_vec_t& matrix, complex_t scalar, complex_vec_t& result) {
		return mat_mul(scalar, matrix, result);
	} // mat_mul()

	/**
	 * in-place scalar-matrix multiplication
	 */
	bool mat_mul_in(real_t scalar,	complex_vec_t& matrix) {
		for(complex_vec_t::iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			*i = (*i) * scalar;
		} // for
		return true;
	} // mat_mul()

	bool mat_mul_in(complex_t scalar, complex_vec_t& matrix) {
		for(complex_vec_t::iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			*i = (*i) * scalar;
		} // for
		return true;
	} // mat_mul()

	bool mat_mul_in(complex_vec_t& matrix, real_t scalar) {
		return mat_mul_in(scalar, matrix);
	} // mat_mul()

	bool mat_mul_in(complex_vec_t& matrix, complex_t scalar) {
		return mat_mul_in(scalar, matrix);
	} // mat_mul()

	/**
	 * computes element-by-element product of two matrices into result
	 */
	bool mat_dot_prod(unsigned int x1_size, unsigned int y1_size, unsigned int z1_size,
						const complex_vec_t& matrix1,
						unsigned int x2_size, unsigned int y2_size, unsigned int z2_size,
						const complex_vec_t& matrix2,
						complex_vec_t& result) {
		if(x1_size != x2_size || y1_size != y2_size || z1_size != z2_size
				|| matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for dot product operation" << std::endl;
			return false;
		} // if
		result.clear();
		complex_vec_t::const_iterator i1 = matrix1.begin();
		complex_vec_t::const_iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			result.push_back((*i1) * (*i2));
		} // for
		return true;
	} // mat_dot_prod()

	/**
	 * performs in-place element-by-element product of two matrices
	 */
	bool mat_dot_prod_in(unsigned int x1_size, unsigned int y1_size, unsigned int z1_size,
							std::vector<complex_t>& matrix1,
							unsigned int x2_size, unsigned int y2_size, unsigned int z2_size,
							std::vector<complex_t>& matrix2) {
		if(x1_size != x2_size || y1_size != y2_size || z1_size != z2_size
				|| matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for dot product operation" << std::endl;
			return false;
		} // if
		std::vector<complex_t>::iterator i1 = matrix1.begin();
		std::vector<complex_t>::iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			*i1 = (*i1) * (*i2);
		} // for
		return true;
	} // mat_dot_prod()

	/**
	 * computes element-by-element division of two matrices (matrix1 / matrix2)	into result
	 */
	bool mat_dot_div(unsigned int nx1, unsigned int ny1, unsigned int nz1,
						const complex_vec_t& matrix1,
						unsigned int nx2, unsigned int ny2, unsigned int nz2,
						const complex_vec_t& matrix2,
						complex_vec_t& result) {
		if(nx1 != nx2 || ny1 != ny2 || nz1 != nz2 || matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for dot division operation"
						<< std::endl;
			return false;
		} // if
		result.clear();
		complex_vec_t::const_iterator i1 = matrix1.begin();
		complex_vec_t::const_iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			result.push_back((*i1) / (*i2));
		} // for
		return true;
	} // mat_dot_div()

	/**
	 * performs in-place element-by-element division of two matrices (matrix1/matrix2) into matrix1
	 */
	bool mat_dot_div_in(unsigned int nx1, unsigned int ny1, unsigned int nz1,
						std::vector<complex_t>& matrix1,
						unsigned int nx2, unsigned int ny2, unsigned int nz2,
						std::vector<complex_t>& matrix2) {
		if(nx1 != nx2 || ny1 != ny2 || nz1 != nz2 || matrix1.size() != matrix2.size()) {
			std::cerr << "error: matrix sizes are not the same for dot division operation"
						<< std::endl;
			return false;
		} // if
		std::vector<complex_t>::iterator i1 = matrix1.begin();
		std::vector<complex_t>::iterator i2 = matrix2.begin();
		for(; i1 != matrix1.end(); ++ i1, ++ i2) {
			*i1 = (*i1) / (*i2);
		} // for
		return true;
	} // mat_dot_div()

	bool mat_sqr(const complex_vec_t& matrix, complex_vec_t& result) {
		result.clear();
		for(complex_vec_t::const_iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			result.push_back((*i) * (*i));
		} // for
		return true;
	} // mat_sqr()

	bool mat_sqr_in(//unsigned int nx, unsigned int ny, unsigned int nz,
						std::vector<complex_t>& matrix) {
		for(std::vector<complex_t>::iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			*i = (*i) * (*i);
		} // for
		return true;
	} // mat_sqr()

	bool mat_sqrt(const complex_vec_t& matrix, complex_vec_t& result) {
		result.clear();
		for(complex_vec_t::const_iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			result.push_back(sqrt(*i));
		} // for
		return true;
	} // mat_sqrt()

	bool mat_sqrt_in(//unsigned int nx, unsigned int ny, unsigned int nz,
						std::vector<complex_t>& matrix) {
		for(std::vector<complex_t>::iterator i = matrix.begin(); i != matrix.end(); ++ i) {
			*i = sqrt(*i);
		} // for
		return true;
	} // mat_sqrt()

	bool mat_exp(complex_vec_t& matrix, complex_vec_t& result) {
		result.clear();
		for(complex_vec_t::iterator i = matrix.begin(); i != matrix.end(); ++ i)
			result.push_back(exp(*i));
	} // mat_exp()

	bool mat_exp_in(complex_vec_t& matrix) {
		for(complex_vec_t::iterator i = matrix.begin(); i != matrix.end(); ++ i) *i = exp(*i);
	} // mat_exp_in()

	// compute integral of e^(ikx) between x1 and x2
	complex_t integral_e(real_t x1, real_t x2, complex_t k) {
		if(boost::math::fpclassify(k.real()) == FP_ZERO &&
				boost::math::fpclassify(k.imag()) == FP_ZERO) {
			return complex_t(x2 - x1, 0);
		} else {
			complex_t ik = complex_t(0.0, 1.0) * k;
			return (((real_t) 1.0 / ik) * (exp(ik * x2) - exp(ik * x1)));
		} // if-else
	} // integral_e

	// compute integral of (ax + b) e^(ikx) between x1 and x2
	complex_t integral_xe(real_t x1, real_t x2, real_t a, real_t b, complex_t k) {
		if(boost::math::fpclassify(k.real()) == FP_ZERO &&
				boost::math::fpclassify(k.imag()) == FP_ZERO) {
			return complex_t(a * (x2 * x2 - x1 * x1) / 2 + b * (x2 - x1), 0.0);
		} else {
			complex_t ik = complex_t(0.0, 1.0) * k;
			return (((real_t) 1.0 / ik) * ((a * x2 + b - a / ik) * exp(ik * x2) -
											(a * x1 + b - a / ik) * exp(ik * x1)));
		} // if-else
	} // integral_xe()

} // namespace stock


////////////////////////////////////////////////////////////////////////////
// lazy prelude.hpp
//
// Build lazy operations for Phoenix equivalents for FC++
//
// These are equivalents of the Boost FC++ functoids in prelude.hpp
//
// Usage:  All of these are functors which need various numbers of arguments.
//         Those can be supplied as real arguments or as Phoenix arguments.
//         Execution will happen when all the arguments are supplied.
// e.g.
//         take(2,list)()  or  take(2,arg1)(list)
//
// Implemented so far:
//
// id (moved back to operators.hpp)
//
// A lot of what comes here uses the list type, so that will be needed first.
//
// Now that list<T> is available I can start to build things here.
//
//
// until(pred,f,start)         - if pred(start) is true, return start
//                               apply value = f(start)
//                               apply value = f(value)
//                               until pred(value) is true
//                               return value
//
// The predicate argument pred must be a lazy function taking one argument
// and returning bool.
// This can be a lazy function with an argument already.
// This has to be declared before the call to until.
// The type can be declated using Predicate as in this example:
//
//    Predicate<int>::type f(greater(arg1,10));
//    std::cout << until(f, inc, 1)() << std::endl;
//
// until2(pred,f,start,value2) - if pred(start,value2) is true, return start
//                               apply value1 = f(start)
//                               apply value1 = f(value1)
//                               until pred(value1,value2) is true
//                               return value1
//
// NOTE: until2 has been defined because this code does not support
//       FC++ currying, so that a partial function cannot be passed
//       as an argument. This provides a way of passing a second parameter.
//       There is now the option to use Predicate<T> as shown above.
//
// odd(n)     true if n is odd
// even(n)    true if n is even
//
// last(list)
// all_but_last(list)
// at(list,n)
// length(list)
// filter(pred,list)
// iterate(function,value)
// repeat(value)
// take(n,list)
// drop(n,list)
// enum_from(x)
// enum_from_to(x,y)
//
////////////////////////////////////////////////////////////////////////////
// Interdependence:
// The old Boost FC++ has a set of headers which interelate and call each
// other in a complicated way. I am going to document the interdependence
// of the files here. I will then make sure that they are called correctly
// starting from this file. John Fletcher. February 2015.
////////////////////////////////////////////////////////////////////////////
// BoostFC++ header sequence:
//
// prelude.hpp   -> list.hpp (optinally monad.hpp at end)
// list.hpp      -> reuse.hpp
// reuse.hpp     -> function.hpp
// function.hpp  -> ref_count.hpp operator.hpp
// ref_count.hpp -> config.hpp boost headers and RefCountType definition
// operator.hpp  -> lambda.hpp
// lambda.hpp    -> full.hpp (use of lambda internals is optional)
// full.hpp      -> smart.hpp curry.hpp pre_lambda.hpp (optionally full4.hpp)
// smart.hpp     -> signature.hpp
// curry.hpp     -> signature.hpp
// signature.hpp -> config.hpp
//
////////////////////////////////////////////////////////////////////////////
// Proposed order in lazy_prelude.hpp
// on the basis that files need what they call.
//
// lazy_config.hpp    (If needed)* probably not needed.
// lazy_signature.hpp (If needed)*
// lazy_smart.hpp     (If needed)*
// lazy_curry.hpp     (If needed)*
// lazy_full.hpp      (If needed)*
// lazy_operator.hpp  (absorb definition of RefCountType)
// lazy_function.hpp  (may not now be needed)
// lazy_reuse.hpp     (implemented without use of FC++ functions)
// lazy_list.hpp
//
// * file does not yet exist.
////////////////////////////////////////////////////////////////////////////
// This is implemented such that no other lazy_ file calls other lazy_ files.
// They do call their own external files, which may well be duplicates.
// That can be sorted out later.
////////////////////////////////////////////////////////////////////////////
// Notes: full and curry operations should be covered by Phoenix.
// The lambda operations are quite different from Phoenix lambda
// and will be omitted.
// The implementation monad can be postponed.
// Some of function and reuse are needed for the list type.
// I will review later whether they are part of the external interface.
//
// John Fletcher  February 2015.
////////////////////////////////////////////////////////////////////////////
/*=============================================================================
    Copyright (c) 2000-2003 Brian McNamara and Yannis Smaragdakis
    Copyright (c) 2001-2007 Joel de Guzman
    Copyright (c) 2015 John Fletcher

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

#ifndef BOOST_PHOENIX_FUNCTION_LAZY_PRELUDE
#define BOOST_PHOENIX_FUNCTION_LAZY_PRELUDE

#include <exception>
#include <vector>
#include <boost/phoenix/core.hpp>
#include <boost/phoenix/function.hpp>
#include <boost/phoenix/scope.hpp>
#include <boost/phoenix/operator.hpp>
#include <boost/phoenix/function/lazy_operator.hpp>
#include <boost/phoenix/function/lazy_reuse.hpp>
#include <boost/phoenix/function/lazy_list.hpp>

////////////////////////////////////////////////////////////////////////////
// To come here, the Haskell Prelude things which need list<T>.
// Things which do not need list<T> are in lazy_operator.hpp.
////////////////////////////////////////////////////////////////////////////

namespace boost {

  namespace phoenix {

    // These are in fcpp namespace as they introduce an FC++ style.
    namespace fcpp {

    template <typename T>
    struct Predicate {
        typedef typename boost::function1<bool,T> fun1_bool_T;
        typedef typename boost::phoenix::function<fun1_bool_T> bool_F_T;
        typedef bool_F_T type;
    };

    template <typename R>
    struct Function0 {
        typedef typename boost::function0<R> fun0_R;
        typedef typename boost::phoenix::function<fun0_R> R_F;
        typedef R_F type;
    };

    template <typename R,typename A0>
    struct Function1 {
        typedef typename boost::function1<R,A0> fun1_R_A0;
        typedef typename boost::phoenix::function<fun1_R_A0> R_F_A0;
        typedef R_F_A0 type;
    };

    template <typename R, typename A0, typename A1>
    struct Function2 {
      typedef typename boost::function2<R,A0,A1> fun2_R_A0_A1;
        typedef typename boost::phoenix::function<fun2_R_A0_A1> R_F_A0_A1;
        typedef R_F_A0_A1 type;
    };

    }

    namespace impl {
      using fcpp::INV;
      using fcpp::VAR;
      using fcpp::reuser1;
      using fcpp::reuser2;
      using fcpp::reuser3;
      using boost::phoenix::arg_names::arg1;

         struct Pow {

            template <typename Sig>
            struct result;

            template <typename This, typename N, typename A0>
            struct result<This(N,A0)>
               : boost::remove_reference<A0>
            {};

            template <typename N, typename A0>
            A0 operator()(N n, const A0 & a0,
            reuser2<INV,VAR,INV,Pow,N,A0> r = NIL ) const {
              if ( n <= 0 )
                 return A0(1);
              else if ( n==1 )
                 return a0;
              else {
                A0 a1 = r( Pow(), n-1, a0)();
                return a0*a1;
              }
            }

         };

         struct Apply {

            template <typename Sig>
            struct result;

            template <typename This, typename N, typename F,typename A0>
            struct result<This(N,F,A0)>
               : boost::remove_reference<A0>
            {};

            template <typename N, typename F, typename A0>
            A0 operator()(N n, const F &f, const A0 & a0,
            reuser3<INV,VAR,INV,INV,Apply,N,F,A0> r = NIL ) const {
              if ( n <= 0 )
                 return a0;
              else if ( n==1 )
                 return f(arg1)(a0);
              else {
                A0 a1 = r( Apply(), n-1, f, a0)();
                return f(a1)();
              }
            }

         };

         struct Odd {
            template <typename Sig>
            struct result;

            template <typename This, typename T>
            struct result<This(T)>
            {
              typedef bool type;
            };

            template <class T>
            typename result<Odd(T)>::type operator()( const T& x ) const {
               return x%2==1;
            }
         };

         struct Even {
            template <typename Sig>
            struct result;

            template <typename This, typename T>
            struct result<This(T)>
            {
              typedef bool type;
            };

            template <class T>
            typename result<Even(T)>::type operator()( const T& x ) const {
               return x%2==0;
            }
         };

    }
    typedef boost::phoenix::function<impl::Pow>   Pow;
    typedef boost::phoenix::function<impl::Apply> Apply;
    typedef boost::phoenix::function<impl::Odd>   Odd;
    typedef boost::phoenix::function<impl::Even>  Even;
    Pow   pow;
    Apply apply;
    Odd   odd;
    Even  even;

    namespace impl {
      using fcpp::INV;
      using fcpp::VAR;
      using fcpp::reuser1;
      using fcpp::reuser2;
      using fcpp::reuser3;
      using boost::phoenix::arg_names::arg1;

      // I cannot yet do currying to pass e.g. greater(9,arg1)
      // as a function. This can be done using Predicate<T>::type.
         struct Until {

             template <typename Sig> struct result;

             template <typename This, typename Pred, typename Unary, typename T>
             struct result<This(Pred,Unary,T)>
                : boost::remove_reference<T> {};

             template <class Pred, class Unary, class T>
             T operator()( const Pred& p,const Unary& op,const T &start) const
             {
               T tmp = start;
               while( !p(tmp)() ) {
                 tmp = apply(1,op,tmp)();
               }
                return tmp;
             }

          };

          struct Until2 {

             template <typename Sig> struct result;

             template <typename This, typename Binary, typename Unary,
                       typename T, typename X>
             struct result<This(Binary,Unary,T,X)>
                : boost::remove_reference<T> {};

             template <class Binary, class Unary, class T, class X>
             typename result<Until2(Binary,Unary,T,X)>::type
             operator()( const Binary& p, const Unary& op, const T & start,
                        const X & check ) const
             {
               T tmp1 = start;
               T tmp2;
               while( !p(tmp1,check)() ) {
                 tmp2 = apply(1,op,tmp1)();
                 tmp1 = tmp2;

               }
               return tmp1;
             }
          };

          struct Last {
             template <typename Sig> struct result;

             template <typename This, typename L>
             struct result<This(L)>
             {
               typedef typename result_of::ListType<L>::value_type type;
             };

             template <class L>
             typename result<Last(L)>::type
             operator()( const L& ll ) const {
               size_t x = 0;
               typename result_of::ListType<L>::delay_result_type l = delay(ll);
               while( !null( tail(l)() )() ) {
                 l = tail(l)();
                 ++x;
#ifndef BOOST_PHOENIX_NO_LAZY_EXCEPTIONS
                 if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                   break;
#endif
               }
#ifndef BOOST_PHOENIX_NO_LAZY_EXCEPTIONS
                 if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                     throw lazy_exception("Your list is too long!!");
#endif
                 return head(l)();
             }
          };

          struct Init {

             template <typename Sig> struct result;

             template <typename This, typename L>
             struct result<This(L)>
             {
               typedef typename result_of::ListType<L>::force_result_type type;
             };

             template <class L>
             typename result<Init(L)>::type
             operator()( const L& l,
                         reuser1<INV,VAR,Init,
                         typename result_of::ListType<L>::delay_result_type>
                         r = NIL ) const {
               if( null( tail( l )() )() )
                   return NIL;
               else
                   return cons( head(l)(), r( Init(), tail(l)() )() )();
               }
          };

          struct Length {
            template <typename Sig> struct result;

            template <typename This, typename L>
            struct result<This(L)>
            {
               typedef size_t type;
            };

            template <class L>
            size_t operator()( const L& ll ) const {
              typename L::delay_result_type l = delay(ll);
              size_t x = 0;
              while( !null(l)() ) {
                  l = tail(l);
                  ++x;
                  if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                     break;
              }
#ifndef BOOST_PHOENIX_NO_LAZY_EXCEPTIONS
              if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                   throw lazy_exception("Your list is too long!!");
#endif
              return x;
            }
          };

          // at is Haskell's operator (!!)
          // This is zero indexed.  at(l,0)() returns the first element.
          struct At {
            template <typename Sig> struct result;

            template <typename This, typename L, typename N>
            struct result<This(L,N)>
            {
               typedef typename result_of::ListType<L>::value_type type;
            };

              template <class L>
              typename result<At(L,size_t)>::type
              operator()( L l, size_t n ) const {
                  while( n!=0 ) {
                      l = tail(l);
                      --n;
                  }
                  return head(l)();
              }
          };

         template <class P,class L>
         struct FilterH
          {
              P p;
              L l;
              FilterH( const P& pp, const L& ll) : p(pp), l(ll) {}
              template <typename Sig> struct result;

              template <typename This, class PP, class LL>
              struct result<This(PP,LL)>
              {
                typedef typename boost::phoenix::result_of::
                        ListType<LL>::delay_result_type type;
              };
                typename result<FilterH(P,L)>::type operator()() const {
                typedef typename result_of::ListType<L>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> Fun2_R_P_L;
                typedef boost::phoenix::function<Fun2_R_P_L> FilterH_R_P_L;
                if (null(l)() )
                   return NIL;
                Fun2_R_P_L fun2_R_P_L = FilterH<P,L>(p,tail(l));
                FilterH_R_P_L filterh_R_P_L(fun2_R_P_L);
                if( p(head(l))() )
                   return cons( head(l)(), filterh_R_P_L() );
                else
                   return filterh_R_P_L();
              }
          };

          struct Filter {
            template <typename Sig> struct result;

                template <typename This, typename P, typename L>
                struct result<This(P,L)>
                {
                  typedef typename result_of::ListType<L>::delay_result_type
                          type;
                };

                template <class P, class L>
                typename result<Filter(P,L)>::type
                operator()( const P& p, const L& ll) const
                {
                     typename  result_of::ListType<L>::delay_result_type
                     l = delay(ll);
                     typedef typename result_of::ListType<L>::
                           delay_result_type result_type;
                     typedef boost::function0<result_type> Fun2_R_P_L;
                     typedef boost::phoenix::function<Fun2_R_P_L> FilterH_R_P_L;
                     Fun2_R_P_L fun2_R_P_L = FilterH<P,L>(p,l);
                     FilterH_R_P_L filterh_R_P_L(fun2_R_P_L);
                     return filterh_R_P_L();
                }
          };

         template <class F,class T>
         struct IterateH
          {
              F f;
              T t;
              IterateH( const F& ff, const T& tt) : f(ff), t(tt) {}
              template <typename Sig> struct result;

              template <typename This,class F2,class T2>
              struct result<This(F2,T2)>
              {
                typedef typename boost::remove_reference<T2>::type TT;
                typedef typename boost::remove_const<TT>::type TTT;
                typedef typename UseList::template List<TTT>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type type;
              };

                typename result<IterateH(F,T)>::type operator()() const {
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> Fun2_R_F_T;
                typedef boost::phoenix::function<Fun2_R_F_T> IterateH_R_F_T;
                Fun2_R_F_T fun2_R_F_T = IterateH<F,T>(f,f(t)());
                IterateH_R_F_T iterateh_R_F_T(fun2_R_F_T);
                   return cons( t, iterateh_R_F_T() );
              }
          };

          struct Iterate {
   // Note: this does always return an odd_list; iterate() takes no ListLike
   // parameter, and it requires that its result be lazy.
              template <typename Sig> struct result;

              template <typename This, typename F, typename T>
              struct result<This(F,T)>
              {
                typedef typename boost::remove_reference<T>::type TT;
                typedef typename boost::remove_const<TT>::type TTT;
                typedef typename UseList::template List<TTT>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type type;
              };

              template <class F, class T>
                typename result<Iterate(F,T)>::type operator()
                (const F& f, const T& t) const {
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> Fun2_R_F_T;
                typedef boost::phoenix::function<Fun2_R_F_T> IterateH_R_F_T;
                Fun2_R_F_T fun2_R_F_T = IterateH<F,T>(f,f(t)());
                IterateH_R_F_T iterateh_R_F_T(fun2_R_F_T);
                   return iterateh_R_F_T();
              }
          };

    }

    typedef boost::phoenix::function<impl::Until> Until;
    typedef boost::phoenix::function<impl::Until2> Until2;
    typedef boost::phoenix::function<impl::Last>  Last;
    typedef boost::phoenix::function<impl::Init>  Init;
    typedef boost::phoenix::function<impl::Length> Length;
    typedef boost::phoenix::function<impl::At>    At;
    typedef boost::phoenix::function<impl::Filter> Filter;
    typedef boost::phoenix::function<impl::Iterate> Iterate;
    Until until;
    Until2 until2;
    Last  last;
    Init  all_but_last;  // renamed from init which is not available.
    Length length;
    At at_;  //Renamed from at.
    Filter filter;
    Iterate iterate;

    namespace impl {

          struct Repeat {
         // See note for iterate()
              template <typename Sig> struct result;

              template <typename This, typename T>
              struct result<This(T)>
              {
                typedef typename boost::remove_reference<T>::type TT;
                typedef typename boost::remove_const<TT>::type TTT;
                typedef typename UseList::template List<TTT>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type type;
              };

              template <class T>
              typename result<Repeat(T)>::type operator()( const T& x) const
              {
                return iterate(id,x);
              }
          };

          struct Take {

             template <typename Sig> struct result;

             template <typename This, typename N, typename L>
             struct result<This(N,L)>
             {
               typedef typename result_of::ListType<L>::force_result_type type;
             };

             template <class N,class L>
             typename result<Take(N,L)>::type
             operator()( N n, const L& l,
               reuser2<INV,VAR,VAR,Take,N,
               typename result_of::ListType<L>::force_result_type>
               r = NIL
             ) const {
               if( n <= 0 || null(l)() )
                 return NIL;
               else {
                 return cons( head(l)(), r( Take(), n-1, tail(l)() )() )();
               }
             }
          };

          struct Drop {
             template <typename Sig> struct result;

             template <typename This, typename Dummy, typename L>
             struct result<This(Dummy,L)>
             {
               typedef typename result_of::ListType<L>::delay_result_type type;
             };

             template <class L>
             typename result<Drop(size_t,L)>::type
             operator()( size_t n, const L& ll ) const {
               typename L::delay_result_type l = delay(ll);
               while( n!=0 && !null(l)() ) {
                 --n;
                 l = tail(l)();
               }
               return l;
             }
          };

          template <class T>
          struct EFH
          {
              mutable T x;
              EFH( const T& xx) : x(xx) {}
              template <typename Sig> struct result;

              template <typename This, class TT>
              struct result<This(TT)>
              {
                typedef typename boost::phoenix::UseList::template
                        List<TT>::type LType;
                typedef typename boost::phoenix::result_of::
                        ListType<LType>::delay_result_type type;
              };
              typename result<EFH(T)>::type operator()() const {
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> fun1_R_TTT;
                //std::cout << "EFH (" << x << ")" << std::endl;
                ++x;
                fun1_R_TTT efh_R_TTT = EFH<T>(x);
                typedef boost::phoenix::function<fun1_R_TTT> EFH_R_T;
                EFH_R_T efh_R_T(efh_R_TTT);
#ifndef BOOST_PHOENIX_NO_LAZY_EXCEPTIONS
                if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                     throw lazy_exception("Running away in EFH!!");
#endif
                return cons( x-1, efh_R_T() );
              }
          };

          struct Enum_from {
             template <typename Sig> struct result;

             template <typename This, typename T>
             struct result<This(T)>
             {
               typedef typename boost::remove_reference<T>::type TT;
               typedef typename boost::remove_const<TT>::type TTT;
               typedef typename UseList::template List<TTT>::type LType;
               typedef typename result_of::ListType<LType>::
                       delay_result_type type;
             };

             template <class T>
             typename result<Enum_from(T)>::type operator()
                (const T & x) const
              {
                typedef typename boost::remove_reference<T>::type TT;
                typedef typename boost::remove_const<TT>::type TTT;
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> fun1_R_TTT;
                fun1_R_TTT efh_R_TTT = EFH<TTT>(x);
                typedef boost::phoenix::function<fun1_R_TTT> EFH_R_T;
                EFH_R_T efh_R_T(efh_R_TTT);
                //std::cout << "enum_from (" << x << ")" << std::endl;
                return efh_R_T();
              }
          };

       template <class T>
         struct EFTH
          {
              mutable T x;
              T y;
              EFTH( const T& xx, const T& yy) : x(xx), y(yy) {}
              template <typename Sig> struct result;

              template <typename This, class TT>
              struct result<This(TT)>
              {
                typedef typename boost::phoenix::UseList::template
                        List<TT>::type LType;
                typedef typename boost::phoenix::result_of::
                        ListType<LType>::delay_result_type type;
              };
              typename result<EFTH(T)>::type operator()() const {
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> fun1_R_TTT;
                //std::cout << "EFTH (" << x << ")" << std::endl;
                if (x > y ) return NIL;
                ++x;
                fun1_R_TTT efth_R_TTT = EFTH<T>(x,y);
                typedef boost::phoenix::function<fun1_R_TTT> EFTH_R_T;
                EFTH_R_T efth_R_T(efth_R_TTT);
#ifndef BOOST_PHOENIX_NO_LAZY_EXCEPTIONS
                if (x > BOOST_PHOENIX_FUNCTION_MAX_LAZY_LIST_LENGTH)
                     throw lazy_exception("Running away in EFTH!!");
#endif
                return cons( x-1, efth_R_T() );
              }
          };

          struct Enum_from_to {
             template <typename Sig> struct result;

             template <typename This, typename T>
             struct result<This(T,T)>
             {
               typedef typename boost::remove_reference<T>::type TT;
               typedef typename boost::remove_const<TT>::type TTT;
               typedef typename UseList::template List<TTT>::type LType;
               typedef typename result_of::ListType<LType>::
                       delay_result_type type;
             };

             template <class T>
             typename result<Enum_from(T,T)>::type operator()
             (const T & x, const T & y) const
              {
                typedef typename boost::remove_reference<T>::type TT;
                typedef typename boost::remove_const<TT>::type TTT;
                typedef typename UseList::template List<T>::type LType;
                typedef typename result_of::ListType<LType>::
                        delay_result_type result_type;
                typedef boost::function0<result_type> fun1_R_TTT;
                fun1_R_TTT efth_R_TTT = EFTH<TTT>(x,y);
                typedef boost::phoenix::function<fun1_R_TTT> EFTH_R_T;
                EFTH_R_T efth_R_T(efth_R_TTT);
                //std::cout << "enum_from (" << x << ")" << std::endl;
                return efth_R_T();
              }
          };

    }

    //BOOST_PHOENIX_ADAPT_CALLABLE(apply, impl::apply, 3)
    // Functors to be used in reuser will have to be defined
    // using boost::phoenix::function directly
    // in order to be able to be used as arguments.
    typedef boost::phoenix::function<impl::Repeat> Repeat;
    typedef boost::phoenix::function<impl::Take>  Take;
    typedef boost::phoenix::function<impl::Drop>  Drop;
    typedef boost::phoenix::function<impl::Enum_from>     Enum_from;
    typedef boost::phoenix::function<impl::Enum_from_to>  Enum_from_to;
    Repeat repeat;
    Take  take;
    Drop  drop;
    Enum_from enum_from;
    Enum_from_to enum_from_to;

    namespace fcpp {

    }

  }

}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   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
//==============================================================================
#include <boost/simd/operator/include/functions/multiplies.hpp>
#include <boost/simd/include/functions/enumerate.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/simd/include/functions/scalar/unary_minus.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/details/prng.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>
#include <boost/simd/sdk/simd/io.hpp>

#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/mzero.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/sqrtvalmax.hpp>

NT2_TEST_CASE_TPL ( multiplies_real,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::multiplies;
  using boost::simd::tag::multiplies_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<multiplies_(vT,vT)>::type r_t;

  // specific values tests
  NT2_TEST_EQUAL(multiplies(boost::simd::Inf<vT>(), boost::simd::Inf<vT>()), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Minf<vT>(), boost::simd::Minf<vT>()), boost::simd::Inf<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Nan<vT>(), boost::simd::Nan<vT>()), boost::simd::Nan<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::One<vT>(),boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Mzero<vT>(), boost::simd::One<vT>()), boost::simd::Mzero<r_t>());
} // end of test for floating_

NT2_TEST_CASE_TPL ( multiplies_integer, BOOST_SIMD_SIMD_INTEGRAL_TYPES)
{
  using boost::simd::Valmin;
  using boost::simd::Valmax;
  using boost::simd::Sqrtvalmax;
  using boost::simd::splat;
  using boost::simd::enumerate;
  using boost::simd::multiplies;
  using boost::simd::tag::multiplies_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<multiplies_(vT,vT)>::type r_t;

  vT v,w,ref;

  nt2::roll(v,T(0),Sqrtvalmax<T>());
  nt2::roll(w,T(0),Sqrtvalmax<T>());
  for(std::size_t i=0;i<vT::static_size;++i)
    v[i] = multiplies(v[i], i%2 ? T(-1) : T(1));

  for(std::size_t i=0;i<vT::static_size/2;++i)
    w[i] = multiplies(w[i],T(-1));

  for(std::size_t i=0;i<vT::static_size;++i)
    ref[i] = multiplies(v[i], w[i]);

  // specific values tests
  NT2_TEST_EQUAL(multiplies(boost::simd::One<vT>(), boost::simd::One<vT>()), boost::simd::One<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::One<vT>(),boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Zero<vT>(), boost::simd::Zero<vT>()), boost::simd::Zero<r_t>());
  NT2_TEST_EQUAL(multiplies(boost::simd::Mzero<vT>(), boost::simd::One<vT>()), boost::simd::Mzero<r_t>());
  NT2_TEST_EQUAL(multiplies(v,w), ref);
}


////////////////////////////////////////////////////////////////////////////////
//
// Reads the pendigits file
//
// Authors: Christoph Sterz, Patrick Lühne (2013)
//
////////////////////////////////////////////////////////////////////////////////

#include "DataSet.h"

#include <iostream>
#include <fstream>
#include <limits>

#include <boost/tokenizer.hpp>

float stringToFloat(std::string string) {
	float number;
	std::stringstream stringstream(string);
	stringstream >> number;
	return number;
}

void readDataSet(std::string fileName, int numberOfRows, cv::Mat &data, cv::Mat &labels) {

	data = cv::Mat(numberOfRows, 16, CV_32FC1);
	labels = cv::Mat(numberOfRows, 1, CV_32FC1);

	std::ifstream file(fileName.c_str());

	if (!file.is_open())
	{
		std::cerr << "ERROR: No such file." << std::endl;
		exit(EXIT_FAILURE);
	}

	// Setup boost's CSV parser
	typedef boost::tokenizer<boost::char_separator<char> > Tokenizer;

	std::vector<std::string> tokens;
	std::string currentLine;

	int row = 0;

	// Parse file
	while (getline(file, currentLine) && row < numberOfRows)
	{
		// Skip empty lines
		if (currentLine == "")
			continue;

		boost::char_separator<char> separators(",", "");
		Tokenizer tokenizer(currentLine, separators);
		tokens.assign(tokenizer.begin(), tokenizer.end());

		for (int i = 0; i < 16; i++)
			data.at<float>(row, i) = stringToFloat(tokens[i]) / 100.0f;

		labels.at<float>(row) = stringToFloat(tokens[16]);
		row++;
	}
}


/*=============================================================================
    Copyright (c) 2003 Vaclav Vesely
    http://spirit.sourceforge.net/

    Use, modification and distribution is subject to the Boost Software
    License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt)
=============================================================================*/
//
//  This example demonstrates the lazy_p parser. You should read
//  "The Lazy Parser" in the documentation.
//
//  We want to parse nested blocks of numbers like this:
//
//  dec {
//      1 2 3
//      bin {
//          1 10 11
//      }
//      4 5 6
//  }
//
//  where the numbers in the "dec" block are wrote in the decimal system and
//  the numbers in the "bin" block are wrote in the binary system. We want
//  parser to return the overall sum.
//
//  To achive this when base ("bin" or "dec") is parsed, in semantic action
//  we store a pointer to the appropriate numeric parser in the closure
//  variable block.int_rule. Than, when we need to parse a number we use lazy_p
//  parser to invoke the parser stored in the block.int_rule pointer.
//
//-----------------------------------------------------------------------------
#include <boost/assert.hpp>
#include <boost/cstdlib.hpp>
#include <boost/spirit/include/phoenix1.hpp>
#include <boost/spirit/include/classic_core.hpp>
#include <boost/spirit/include/classic_symbols.hpp>
#include <boost/spirit/include/classic_attribute.hpp>
#include <boost/spirit/include/classic_dynamic.hpp>

using namespace boost;
using namespace BOOST_SPIRIT_CLASSIC_NS;
using namespace phoenix;

//-----------------------------------------------------------------------------
//  my grammar

struct my_grammar
    :   public grammar<my_grammar, parser_context<int> >
{
    // grammar definition
    template<typename ScannerT>
    struct definition
    {
        typedef rule<ScannerT> rule_t;
        typedef stored_rule<ScannerT, parser_context<int> > number_rule_t;

        struct block_closure;
        typedef boost::spirit::classic::closure<
            block_closure,
            int,
            typename number_rule_t::alias_t>
        closure_base_t;

        struct block_closure : closure_base_t
        {
            typename closure_base_t::member1 sum;
            typename closure_base_t::member2 int_rule;
        };

        // block rule type
        typedef rule<ScannerT, typename block_closure::context_t> block_rule_t;

        block_rule_t block;
        rule_t block_item;
        symbols<number_rule_t> base;

        definition(my_grammar const& self)
        {
            block =
                    base[
                        block.sum = 0,
                        // store a number rule in a closure member
                        block.int_rule = arg1
                    ]
                >>  "{"
                >>  *block_item
                >>  "}"
                ;

            block_item =
                    // use the stored rule
                    lazy_p(block.int_rule)[block.sum += arg1]
                |   block[block.sum += arg1]
                ;

            // bind base keywords and number parsers
            base.add
                ("bin", bin_p)
                ("dec", uint_p)
                ;
        }

        block_rule_t const& start() const
        {
            return block;
        }
    };
};

//-----------------------------------------------------------------------------

int main()
{
    my_grammar gram;
    parse_info<> info;

    int result;
    info = parse("bin{1 dec{1 2 3} 10}", gram[var(result) = arg1], space_p);
    BOOST_ASSERT(info.full);
    BOOST_ASSERT(result == 9);

    return exit_success;
}

//-----------------------------------------------------------------------------


/* Copyright (c) 2014, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <Eigen/Dense>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>

#include <string>

using namespace Eigen;
using std::string;

bool updateCosy(const boost::shared_ptr<pcl::visualization::PCLVisualizer>& viewer
    ,const Matrix3f& R, string prefix="cosy", float scale=1.0f);

void addCosy(const boost::shared_ptr<pcl::visualization::PCLVisualizer>& viewer,
    const Matrix3f& R, string prefix="cosy", float scale=1.0f, int viewport=0);


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

#define BOOST_TEST_MODULE test_hierarchical_optimizer

// standard library

// libraries
#include <boost/test/unit_test.hpp>
#include <boost/python.hpp>
#include <Eigen/Eigen>
#include <unsupported/Eigen/CXX11/Tensor>

// test data
#include "data/test_data_hierarchical_optimizer.hpp"

// test targets
#include "../src/math/typedefs.hpp"
#include "../src/math/almost_equal.hpp"
#include "../src/nonrigid_optimization/hierarchical/pyramid.hpp"
#include "../src/nonrigid_optimization/hierarchical/optimizer.hpp"
#include "../src/nonrigid_optimization/hierarchical/optimizer_with_telemetry.hpp"
#include "../src/telemetry/optimization_iteration_data.hpp"
#include "../src/nonrigid_optimization/field_warping.hpp"

namespace eig = Eigen;

namespace nro_h = nonrigid_optimization::hierarchical;
namespace nro = nonrigid_optimization;

BOOST_AUTO_TEST_CASE(pyramid2d_test01) {
	// corresponds to test_contstruct_scalar_pyramid for Python
	eig::MatrixXf tile(8, 8);
	tile <<
			1.0f, 2.0f, 5.0f, 6.0f, -1.0f, -2.0f, -5.0f, -6.0f,
			3.0f, 4.0f, 7.0f, 8.0f, -3.0f, -4.0f, -7.0f, -8.0f,
			-1.0f, -2.0f, -5.0f, -6.0f, 1.0f, 2.0f, 5.0f, 6.0f,
			-3.0f, -4.0f, -7.0f, -8.0f, 3.0f, 4.0f, 7.0f, 8.0f,
			1.0f, 2.0f, 5.0f, 6.0f, 5.0f, 5.0f, 5.0f, 5.0f,
			3.0f, 4.0f, 7.0f, 8.0f, 5.0f, 5.0f, 5.0f, 5.0f,
			-1.0f, -2.0f, -5.0f, -6.0f, 5.0f, 5.0f, 5.0f, 5.0f,
			-3.0f, -4.0f, -7.0f, -8.0f, 5.0f, 5.0f, 5.0f, 5.0f;

	// results in shape 128 x 128
	eig::MatrixXf field = tile.replicate(16, 16);

	nro_h::Pyramid<eig::MatrixXf> pyramid(field);
	BOOST_REQUIRE_EQUAL(pyramid.get_level_count(), (unsigned)4);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0).rows(), 16);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0).cols(), 16);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(1).rows(), 32);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2).rows(), 64);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(3).rows(), 128);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(3).cols(), 128);

	float l2_00 = tile.block<2, 2>(0, 0).mean();
	float l2_10 = tile.block<2, 2>(2, 0).mean();
	float l2_01 = tile.block<2, 2>(0, 2).mean();
	float l2_11 = tile.block<2, 2>(2, 2).mean();
	float l2_02 = -l2_00;
	float l2_12 = -l2_10;
	float l2_03 = -l2_01;
	float l2_13 = -l2_11;

	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 0), l2_00);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 0), l2_10);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 1), l2_01);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 1), l2_11);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 0 + 2), l2_02);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 0 + 2), l2_12);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 1 + 2), l2_03);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 1 + 2), l2_13);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0 + 4, 0), l2_00);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1 + 4, 0), l2_10);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0 + 4, 1), l2_01);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1 + 4, 1), l2_11);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 0 + 4), l2_00);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 0 + 4), l2_10);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0, 1 + 4), l2_01);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(1, 1 + 4), l2_11);

	float l1_00 = (l2_00 + l2_10 + l2_01 + l2_11) / 4.0f;
	float l1_01 = (l2_02 + l2_12 + l2_03 + l2_13) / 4.0f;
	float l1_10 = l1_00;
	float l1_11 = 5.0;

	BOOST_REQUIRE_EQUAL(pyramid.get_level(1)(0, 0), l1_00);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(1)(1, 0), l1_10);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(1)(0, 1), l1_01);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(1)(1, 1), l1_11);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0)(0, 0), 5.0f/4.0f);
}

BOOST_AUTO_TEST_CASE(pyramid2d_test02) {
	eig::MatrixXf tile(8, 8);
		tile <<
				1.0f, 2.0f, 5.0f, 6.0f, -1.0f, -2.0f, -5.0f, -6.0f,
				3.0f, 4.0f, 7.0f, 8.0f, -3.0f, -4.0f, -7.0f, -8.0f,
				-1.0f, -2.0f, -5.0f, -6.0f, 1.0f, 2.0f, 5.0f, 6.0f,
				-3.0f, -4.0f, -7.0f, -8.0f, 3.0f, 4.0f, 7.0f, 8.0f,
				1.0f, 2.0f, 5.0f, 6.0f, 5.0f, 5.0f, 5.0f, 5.0f,
				3.0f, 4.0f, 7.0f, 8.0f, 5.0f, 5.0f, 5.0f, 5.0f,
				-1.0f, -2.0f, -5.0f, -6.0f, 5.0f, 5.0f, 5.0f, 5.0f,
				-3.0f, -4.0f, -7.0f, -8.0f, 5.0f, 5.0f, 5.0f, 5.0f;
	nro_h::Pyramid<eig::MatrixXf> pyramid(tile, 1);
	BOOST_REQUIRE_EQUAL(pyramid.get_level_count(), (unsigned)1);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0)(0,0), 1.0f);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0)(0,1), 2.0f);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0)(5,2), 7.0f);

}

BOOST_AUTO_TEST_CASE(pyramid3d_test01) {
	eig::Tensor<float,3> field = test_data::pyramid3d_argument_field;
	int field_dim0 = test_data::pyramid3d_argument_field.dimension(0);
	int field_dim1 = test_data::pyramid3d_argument_field.dimension(1);
	int field_dim2 = test_data::pyramid3d_argument_field.dimension(2);
	nro_h::Pyramid<math::Tensor3f> pyramid(field, 8);
	BOOST_REQUIRE_EQUAL(pyramid.get_level_count(), (unsigned)4);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0).dimension(0),1);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0).dimension(1),1);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0).dimension(2),1);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(0)(0),255.5f);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2).dimension(0),field_dim0/2);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2).dimension(1),field_dim1/2);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2).dimension(2),field_dim2/2);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(0),36.5);
	BOOST_REQUIRE_EQUAL(pyramid.get_level(2)(pyramid.get_level(2).size()-1),474.5);
}

BOOST_AUTO_TEST_CASE(warp_field_test01){
	//corresponds to test_resample_field01 in python code
	eig::MatrixXf resampled_field = nro::warp(test_data::field_A_16x16,test_data::warp_field_A_16x16);
	BOOST_REQUIRE(resampled_field.isApprox(test_data::fA_resampled_with_wfA));
}

BOOST_AUTO_TEST_CASE(warp_field_test02){
	//corresponds to test_resample_field_replacement01 in python code
	eig::MatrixXf resampled_field = nro::warp_with_replacement(test_data::field_B_16x16,test_data::warp_field_B_16x16,0.0f);
	BOOST_REQUIRE(resampled_field.isApprox(test_data::fB_resampled_with_wfB_replacement));
}

BOOST_AUTO_TEST_CASE(test_hierarchical_optimizer01){
	// corresponds to test_construction_and_operation in python code (test_hns_optimizer2d.py)
	nro_h::Optimizer2d optimizer(
			false, false,
			8,
			0.2,
			100, //max iteration count
			0.001, //max warp update threshold
			1.0,
			0.2,
			eig::VectorXf(0)
			);
	math::MatrixXv2f warp_field_out = optimizer.optimize(test_data::canonical_field, test_data::live_field);
	eig::MatrixXf final_live_resampled = nro::warp(test_data::live_field,warp_field_out);

	BOOST_REQUIRE(math::almost_equal_verbose(warp_field_out,test_data::warp_field,10e-6));
	BOOST_REQUIRE(final_live_resampled.isApprox(test_data::final_live_field));
}

BOOST_AUTO_TEST_CASE(test_hierarchical_optimizer_iteration_data){
	nro_h::OptimizerWithTelemetry2d optimizer(
			false, false,
			8,
			0.2,
			100, //max iteration count
			0.001, //max warp update threshold
			1.0,
			0.2,
			eig::VectorXf(0),
			nro_h::Optimizer2d::NEAREST_AND_AVERAGE,
			nro_h::OptimizerWithTelemetry2d::VerbosityParameters(),
			nro_h::OptimizerWithTelemetry2d::LoggingParameters(true,true)
			);
	math::MatrixXv2f warp_field_out = optimizer.optimize(test_data::canonical_field, test_data::live_field);
	eig::MatrixXf final_live_resampled = nro::warp(test_data::live_field,warp_field_out);

	std::vector<telemetry::OptimizationIterationData2d> data = optimizer.get_per_level_iteration_data();

	const std::vector<math::MatrixXv2f> vec = data[3].get_warp_fields();

	BOOST_REQUIRE(math::almost_equal_verbose(vec[50],test_data::iteration50_warp_field,10e-6));

	BOOST_REQUIRE(math::almost_equal_verbose(warp_field_out,test_data::warp_field,10e-6));
	BOOST_REQUIRE(final_live_resampled.isApprox(test_data::final_live_field));
}


#include "truncated_sb_mixing.h"

#include <google/protobuf/stubs/casts.h>

#include <Eigen/Dense>
#include <cassert>
#include <memory>
#include <numeric>
#include <stan/math/prim.hpp>
#include <vector>

#include "logit_sb_mixing.h"
#include "mixing_prior.pb.h"
#include "mixing_state.pb.h"
#include "src/hierarchies/abstract_hierarchy.h"
#include "src/utils/proto_utils.h"
#include "src/utils/rng.h"

void TruncatedSBMixing::initialize() {
  if (prior == nullptr) {
    throw std::invalid_argument("Mixing prior was not provided");
  }
  auto priorcast = cast_prior();
  num_components = priorcast->num_components();
  initialize_state();
}

void TruncatedSBMixing::update_state(
    const std::vector<std::shared_ptr<AbstractHierarchy>> &unique_values,
    const std::vector<unsigned int> &allocations) {
  assert(unique_values.size() == num_components);
  // Initialize relevant objects
  auto &rng = bayesmix::Rng::Instance().get();
  auto priorcast = cast_prior();
  // Recover cluster cardinalities
  std::vector<unsigned int> cards(num_components);
  for (int i = 0; i < num_components; i++) {
    cards[i] = unique_values[i]->get_card();
  }
  // Get prior parameters
  Eigen::MatrixXd shapes = get_prior_shape_parameters();
  // Loop over components
  for (int i = 0; i < num_components - 1; i++) {
    // Count data points in subsequent clusters than the i-th one
    unsigned int subseq_count = 0;
    for (int j = i + 1; j < num_components; j++) {
      subseq_count += cards[j];
    }
    // Draw new value for i-th stick
    state.sticks(i) = stan::math::beta_rng(shapes(0, i) + cards[i],
                                           shapes(1, i) + subseq_count, rng);
  }
  state.sticks(num_components - 1) = 1.0;
  // Update logweights
  state.logweights = logweights_from_sticks();
}

Eigen::VectorXd TruncatedSBMixing::get_weights(
    const bool log, const bool propto,
    const Eigen::RowVectorXd &covariate /*= Eigen::RowVectorXd(0)*/) const {
  if (log) {
    return state.logweights;
  } else {
    return state.logweights.array().exp();
  }
}

void TruncatedSBMixing::set_state_from_proto(
    const google::protobuf::Message &state_) {
  auto &statecast =
      google::protobuf::internal::down_cast<const bayesmix::MixingState &>(
          state_);
  state.sticks = bayesmix::to_eigen(statecast.trunc_sb_state().sticks());
  state.logweights = logweights_from_sticks();
}

void TruncatedSBMixing::write_state_to_proto(
    google::protobuf::Message *out) const {
  bayesmix::TruncSBState state_;
  bayesmix::to_proto(state.sticks, state_.mutable_sticks());
  bayesmix::to_proto(state.sticks, state_.mutable_logweights());
  google::protobuf::internal::down_cast<bayesmix::MixingState *>(out)
      ->mutable_trunc_sb_state()
      ->CopyFrom(state_);
}

void TruncatedSBMixing::initialize_state() {
  auto priorcast = cast_prior();
  state.sticks = Eigen::VectorXd(num_components);
  if (priorcast->has_beta_priors()) {
    if (priorcast->beta_priors().beta_distributions_size() != num_components) {
      throw std::invalid_argument(
          "Hyperparameters dimensions are not consistent");
    }
    for (int i = 0; i < num_components - 1; i++) {
      if (priorcast->beta_priors().beta_distributions(i).shape_a() <= 0 or
          priorcast->beta_priors().beta_distributions(i).shape_b() <= 0) {
        throw std::invalid_argument("Beta shape parameters must be > 0");
      }
    }
  } else if (priorcast->has_dp_prior()) {
    if (priorcast->dp_prior().totalmass() <= 0) {
      throw std::invalid_argument("Total mass parameter must be > 0");
    }
  } else if (priorcast->has_py_prior()) {
    if (priorcast->py_prior().strength() <=
        -priorcast->py_prior().discount()) {
      throw std::invalid_argument("Prior parameters are not valid");
    }
    if (priorcast->py_prior().discount() < 0 or
        priorcast->py_prior().discount() >= 1) {
      throw std::invalid_argument("Discount parameter must be in [0,1)");
    }
  } else {
    throw std::invalid_argument("Unrecognized mixing prior");
  }
  // Initialize sticks
  Eigen::MatrixXd shapes = get_prior_shape_parameters();
  for (int i = 0; i < num_components - 1; i++) {
    state.sticks(i) = shapes(0, i) / (shapes(0, i) + shapes(1, i));
  }
  state.sticks(num_components - 1) = 1.0;
  // Initialize logweights
  state.logweights = logweights_from_sticks();
}

Eigen::VectorXd TruncatedSBMixing::logweights_from_sticks() const {
  // Compute cumulative sums of logarithms
  std::vector<double> cumsum(num_components + 1, 0.0);
  for (int h = 1; h < num_components + 1; h++) {
    cumsum[h] = cumsum[h - 1] + std::log(1 - state.sticks(h - 1));
  }
  // Compute weights
  Eigen::VectorXd logweights(num_components);
  for (int h = 0; h < num_components; h++) {
    logweights(h) = std::log(state.sticks(h)) + cumsum[h];
  }
  return logweights;
}

Eigen::MatrixXd TruncatedSBMixing::get_prior_shape_parameters() const {
  Eigen::MatrixXd shapes(2, num_components);
  auto priorcast = cast_prior();
  if (priorcast->has_beta_priors()) {
    // Individual shape parameters for each component
    for (int i = 0; i < num_components; i++) {
      shapes(0, i) = priorcast->beta_priors().beta_distributions(i).shape_a();
      shapes(1, i) = priorcast->beta_priors().beta_distributions(i).shape_b();
    }
  } else if (priorcast->has_dp_prior()) {
    // Uniform shape parameters for all components, computed from DP parameters
    double totmass = priorcast->dp_prior().totalmass();
    shapes.row(0) = Eigen::VectorXd::Ones(num_components);
    shapes.row(1) = totmass * Eigen::VectorXd::Ones(num_components);
  } else if (priorcast->has_py_prior()) {
    // Uniform shape parameters for all components, computed from PY parameters
    double strength = priorcast->py_prior().strength();
    double disc = priorcast->py_prior().discount();
    for (int i = 0; i < num_components; i++) {
      shapes(0, i) = 1.0 - disc;
      shapes(1, i) = strength + i * disc;
    }
  } else {
    throw std::invalid_argument("Unrecognized mixing prior");
  }
  return shapes;
}


#pragma once
#include <tight_inclusion/Types.hpp>
#include <Eigen/Core>

namespace inclusion_ccd
{
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Eigen::ColMajor, 3, 1>
        VectorMax3d;
}