/* boost random/mersenne_twister.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * 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)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: mersenne_twister.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_MERSENNE_TWISTER_HPP
#define BOOST_RANDOM_MERSENNE_TWISTER_HPP

#include <iostream>
#include <algorithm>     // std::copy
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/integer_traits.hpp>
#include <boost/cstdint.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/detail/workaround.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/ptr_helper.hpp>
#include <boost/random/detail/seed.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template mersenne_twister model a
 * \pseudo_random_number_generator. It uses the algorithm described in
 *
 *  @blockquote
 *  "Mersenne Twister: A 623-dimensionally equidistributed uniform
 *  pseudo-random number generator", Makoto Matsumoto and Takuji Nishimura,
 *  ACM Transactions on Modeling and Computer Simulation: Special Issue on
 *  Uniform Random Number Generation, Vol. 8, No. 1, January 1998, pp. 3-30.
 *  @endblockquote
 *
 * @xmlnote
 * The boost variant has been implemented from scratch and does not
 * derive from or use mt19937.c provided on the above WWW site. However, it
 * was verified that both produce identical output.
 * @endxmlnote
 *
 * The seeding from an integer was changed in April 2005 to address a
 * weakness.
 *
 * The quality of the generator crucially depends on the choice of the
 * parameters.  User code should employ one of the sensibly parameterized
 * generators such as \mt19937 instead.
 *
 * The generator requires considerable amounts of memory for the storage of
 * its state array. For example, \mt11213b requires about 1408 bytes and
 * \mt19937 requires about 2496 bytes.
 */
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
class mersenne_twister
{
public:
  typedef UIntType result_type;
  BOOST_STATIC_CONSTANT(int, word_size = w);
  BOOST_STATIC_CONSTANT(int, state_size = n);
  BOOST_STATIC_CONSTANT(int, shift_size = m);
  BOOST_STATIC_CONSTANT(int, mask_bits = r);
  BOOST_STATIC_CONSTANT(UIntType, parameter_a = a);
  BOOST_STATIC_CONSTANT(int, output_u = u);
  BOOST_STATIC_CONSTANT(int, output_s = s);
  BOOST_STATIC_CONSTANT(UIntType, output_b = b);
  BOOST_STATIC_CONSTANT(int, output_t = t);
  BOOST_STATIC_CONSTANT(UIntType, output_c = c);
  BOOST_STATIC_CONSTANT(int, output_l = l);

  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);

  /**
   * Constructs a @c mersenne_twister and calls @c seed().
   */
  mersenne_twister() { seed(); }

  /**
   * Constructs a @c mersenne_twister and calls @c seed(value).
   */
  BOOST_RANDOM_DETAIL_ARITHMETIC_CONSTRUCTOR(mersenne_twister, UIntType, value)
  { seed(value); }
  template<class It> mersenne_twister(It& first, It last) { seed(first,last); }

  /**
   * Constructs a mersenne_twister and calls @c seed(gen).
   *
   * @xmlnote
   * The copy constructor will always be preferred over
   * the templated constructor.
   * @endxmlnote
   */
  BOOST_RANDOM_DETAIL_GENERATOR_CONSTRUCTOR(mersenne_twister, Generator, gen)
  { seed(gen); }

  // compiler-generated copy ctor and assignment operator are fine

  /** Calls @c seed(result_type(5489)). */
  void seed() { seed(UIntType(5489)); }

  /**
   * Sets the state x(0) to v mod 2w. Then, iteratively,
   * sets x(i) to (i + 1812433253 * (x(i-1) xor (x(i-1) rshift w-2))) mod 2w
   * for i = 1 .. n-1. x(n) is the first value to be returned by operator().
   */
  BOOST_RANDOM_DETAIL_ARITHMETIC_SEED(mersenne_twister, UIntType, value)
  {
    // New seeding algorithm from
    // http://www.math.sci.hiroshima-u.ac.jp/~m-mat/MT/MT2002/emt19937ar.html
    // In the previous versions, MSBs of the seed affected only MSBs of the
    // state x[].
    const UIntType mask = ~0u;
    x[0] = value & mask;
    for (i = 1; i < n; i++) {
      // See Knuth "The Art of Computer Programming" Vol. 2, 3rd ed., page 106
      x[i] = (1812433253UL * (x[i-1] ^ (x[i-1] >> (w-2))) + i) & mask;
    }
  }

  /**
   * Sets the state of this mersenne_twister to the values
   * returned by n invocations of gen.
   *
   * Complexity: Exactly n invocations of gen.
   */
  BOOST_RANDOM_DETAIL_GENERATOR_SEED(mersenne_twister, Generator, gen)
  {
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(!std::numeric_limits<result_type>::is_signed);
#endif
    // I could have used std::generate_n, but it takes "gen" by value
    for(int j = 0; j < n; j++)
      x[j] = gen();
    i = n;
  }

  template<class It>
  void seed(It& first, It last)
  {
    int j;
    for(j = 0; j < n && first != last; ++j, ++first)
      x[j] = *first;
    i = n;
    if(first == last && j < n)
      throw std::invalid_argument("mersenne_twister::seed");
  }

  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const
  {
    // avoid "left shift count >= with of type" warning
    result_type res = 0;
    for(int j = 0; j < w; ++j)
      res |= (1u << j);
    return res;
  }

  result_type operator()();
  static bool validation(result_type v) { return val == v; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const mersenne_twister& mt)
  {
    for(int j = 0; j < mt.state_size; ++j)
      os << mt.compute(j) << " ";
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, mersenne_twister& mt)
  {
    for(int j = 0; j < mt.state_size; ++j)
      is >> mt.x[j] >> std::ws;
    // MSVC (up to 7.1) and Borland (up to 5.64) don't handle the template
    // value parameter "n" available from the class template scope, so use
    // the static constant with the same value
    mt.i = mt.state_size;
    return is;
  }
#endif

  friend bool operator==(const mersenne_twister& x, const mersenne_twister& y)
  {
    for(int j = 0; j < state_size; ++j)
      if(x.compute(j) != y.compute(j))
        return false;
    return true;
  }

  friend bool operator!=(const mersenne_twister& x, const mersenne_twister& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const mersenne_twister& rhs) const
  {
    for(int j = 0; j < state_size; ++j)
      if(compute(j) != rhs.compute(j))
        return false;
    return true;
  }

  bool operator!=(const mersenne_twister& rhs) const
  { return !(*this == rhs); }
#endif

private:
  /// \cond hide_private_members
  // returns x(i-n+index), where index is in 0..n-1
  UIntType compute(unsigned int index) const
  {
    // equivalent to (i-n+index) % 2n, but doesn't produce negative numbers
    return x[ (i + n + index) % (2*n) ];
  }
  void twist(int block);
  /// \endcond

  // state representation: next output is o(x(i))
  //   x[0]  ... x[k] x[k+1] ... x[n-1]     x[n]     ... x[2*n-1]   represents
  //  x(i-k) ... x(i) x(i+1) ... x(i-k+n-1) x(i-k-n) ... x[i(i-k-1)]
  // The goal is to always have x(i-n) ... x(i-1) available for
  // operator== and save/restore.

  UIntType x[2*n];
  int i;
};

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const bool mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::has_fixed_range;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::state_size;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::shift_size;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::mask_bits;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::parameter_a;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_u;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_s;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_b;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_t;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const UIntType mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_c;
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
const int mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::output_l;
#endif

/// \cond hide_private_members
template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
void mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::twist(int block)
{
  const UIntType upper_mask = (~0u) << r;
  const UIntType lower_mask = ~upper_mask;

  if(block == 0) {
    for(int j = n; j < 2*n; j++) {
      UIntType y = (x[j-n] & upper_mask) | (x[j-(n-1)] & lower_mask);
      x[j] = x[j-(n-m)] ^ (y >> 1) ^ (y&1 ? a : 0);
    }
  } else if (block == 1) {
    // split loop to avoid costly modulo operations
    {  // extra scope for MSVC brokenness w.r.t. for scope
      for(int j = 0; j < n-m; j++) {
        UIntType y = (x[j+n] & upper_mask) | (x[j+n+1] & lower_mask);
        x[j] = x[j+n+m] ^ (y >> 1) ^ (y&1 ? a : 0);
      }
    }

    for(int j = n-m; j < n-1; j++) {
      UIntType y = (x[j+n] & upper_mask) | (x[j+n+1] & lower_mask);
      x[j] = x[j-(n-m)] ^ (y >> 1) ^ (y&1 ? a : 0);
    }
    // last iteration
    UIntType y = (x[2*n-1] & upper_mask) | (x[0] & lower_mask);
    x[n-1] = x[m-1] ^ (y >> 1) ^ (y&1 ? a : 0);
    i = 0;
  }
}
/// \endcond

template<class UIntType, int w, int n, int m, int r, UIntType a, int u,
  int s, UIntType b, int t, UIntType c, int l, UIntType val>
inline typename mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::result_type
mersenne_twister<UIntType,w,n,m,r,a,u,s,b,t,c,l,val>::operator()()
{
  if(i == n)
    twist(0);
  else if(i >= 2*n)
    twist(1);
  // Step 4
  UIntType z = x[i];
  ++i;
  z ^= (z >> u);
  z ^= ((z << s) & b);
  z ^= ((z << t) & c);
  z ^= (z >> l);
  return z;
}

} // namespace random

/**
 * The specializations \mt11213b and \mt19937 are from
 *
 *  @blockquote
 *  "Mersenne Twister: A 623-dimensionally equidistributed
 *  uniform pseudo-random number generator", Makoto Matsumoto
 *  and Takuji Nishimura, ACM Transactions on Modeling and
 *  Computer Simulation: Special Issue on Uniform Random Number
 *  Generation, Vol. 8, No. 1, January 1998, pp. 3-30.
 *  @endblockquote
 */
typedef random::mersenne_twister<uint32_t,32,351,175,19,0xccab8ee7,11,
  7,0x31b6ab00,15,0xffe50000,17, 0xa37d3c92> mt11213b;

/**
 * The specializations \mt11213b and \mt19937 are from
 *
 *  @blockquote
 *  "Mersenne Twister: A 623-dimensionally equidistributed
 *  uniform pseudo-random number generator", Makoto Matsumoto
 *  and Takuji Nishimura, ACM Transactions on Modeling and
 *  Computer Simulation: Special Issue on Uniform Random Number
 *  Generation, Vol. 8, No. 1, January 1998, pp. 3-30.
 *  @endblockquote
 */
typedef random::mersenne_twister<uint32_t,32,624,397,31,0x9908b0df,11,
  7,0x9d2c5680,15,0xefc60000,18, 3346425566U> mt19937;

} // namespace boost

BOOST_RANDOM_PTR_HELPER_SPEC(boost::mt19937)

#endif // BOOST_RANDOM_MERSENNE_TWISTER_HPP


//
//  Distance.hpp
//  Distance functions
//
//  Updated by Andrew Long on 08/22/17.
//  Copyright (c) 2016-2017 Andrew Long. All rights reserved.
//
//  Utilizes Armadillo (http://arma.sourceforge.net/) C++ LinAlg Library
//  Sanderson, C. Armadillo: An Open Source C++ Linear Algebra Library
//  for Fast Prototyping and Computationally Intensive Experiments.
//  Technical Report, NICTA, 2010.

#pragma once
#include <armadillo>
#include "DFSAlign.hpp"
#include "pyarma.h"
#include "Heuristic.h"
#include <pybind11/stl.h>
#define BRANCHES 12

double
dist(pyarr_d a, pyarr_d b, MatchHeuristic heur = MatchHeuristic::ISORANK)
{
	arma::mat oa = py_to_mat(a);
	arma::mat ob = py_to_mat(b);
	return dist_dfs(oa, ob, BRANCHES, heur);
}

pyarr_d
pdist(std::vector<pyarr_d> pyobjs, MatchHeuristic heur = MatchHeuristic::ISORANK)
{
	size_t N = pyobjs.size();
	arma::mat dists(N,N,arma::fill::zeros);
	std::vector<arma::mat> objs(N);
	for(size_t i = 0; i < N; ++i)
		objs.at(i) = py_to_mat(pyobjs.at(i));
// load balances by computing subdiagonal elements from the bottom of the matrix
#if defined(ENABLE_OPENMP)
		size_t n_rows = (N-1)/2;
	#pragma omp parallel for shared(dists)
		for(size_t i = 0; i < n_rows; ++i)
		{
			for(size_t j = 0; j < N; ++j)
			{
				size_t idx_i = i, idx_j = j;
				if(idx_i >= idx_j)
				{
					idx_i = N - 2 - idx_i;
					idx_j = N - 1 - idx_j;
				}
				double d = dist_dfs(objs.at(idx_i), objs.at(idx_j), BRANCHES, heur);
				dists(idx_i, idx_j) = d;
				dists(idx_j, idx_i) = d;
			}
		}

		if(N % 2 == 0)
		{
		#pragma omp parallel for shared(dists)
			for(size_t j = n_rows+1; j < N; ++j)
			{
				double d = dist_dfs(objs.at(n_rows), objs.at(j), BRANCHES, heur);
				dists(n_rows, j) = d;
				dists(j, n_rows) = d;
			}
		}
#else
	for(size_t i = 0; i < N; ++i)
	{
		for(size_t j=i+1; j < N; ++j)
		{
			dists(i,j) = dist_dfs(objs.at(i), objs.at(j), BRANCHES, heur);
			dists(j,i) = dists(i,j);
		}
	}
#endif
	return mat_to_py(dists);
}

pyarr_d
pdist2(std::vector<pyarr_d> p1, std::vector<pyarr_d> p2, MatchHeuristic heur = MatchHeuristic::ISORANK)
{
    std::vector<arma::mat> o1 = convert_matlist(p1);
    std::vector<arma::mat> o2 = convert_matlist(p2);
	size_t N1 = o1.size();
	size_t N2 = o2.size();
    arma::mat dists(N1,N2);
    // ensuring good parallelization
    if(N1 == 1)
    {
        #if defined(ENABLE_OPENMP)
        #pragma omp parallel for shared(dists)
        #endif
        for(size_t j = 0; j < N2; ++j)
			dists(0,j) = dist_dfs(o1.at(0), o2.at(j), BRANCHES, heur);
    }
    else if(N2 == 1)
    {
        #if defined(ENABLE_OPENMP)
        #pragma omp parallel for shared(dists)
        #endif
        for(size_t i = 0; i < N1; ++i)
			dists(i,0) = dist_dfs(o1.at(i), o2.at(0), BRANCHES, heur);

    }
    else
    {
        #if defined(ENABLE_OPENMP)
        #pragma omp parallel for shared(dists)
        #endif
	    for(size_t i = 0; i < N1; ++i)
		    for(size_t j = 0; j < N2; ++j)
			    dists(i,j) = dist_dfs(o1.at(i), o2.at(j), BRANCHES, heur);
	}
	return mat_to_py(dists);
}


#include <voxelized_geometry_tools/mesh_rasterizer.hpp>

#include <algorithm>
#include <limits>
#include <stdexcept>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <common_robotics_utilities/voxel_grid.hpp>
#if defined(_OPENMP)
#include <omp.h>
#endif
#include <voxelized_geometry_tools/collision_map.hpp>

namespace voxelized_geometry_tools
{
namespace
{
bool PointProjectsInsideTriangle(
    const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2,
    const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& p_MQ)
{
  const auto same_side = [] (
      const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB,
      const Eigen::Vector3d& p_Mp1, const Eigen::Vector3d& p_Mp2) -> bool
  {
    const Eigen::Vector3d v_AB = p_MB - p_MA;
    const Eigen::Vector3d cross1 = v_AB.cross(p_Mp1 - p_MA);
    const Eigen::Vector3d cross2 = v_AB.cross(p_Mp2 - p_MA);
    return (cross1.dot(cross2) >= 0.0);
  };

  return (same_side(p_Mv1, p_Mv2, p_Mv3, p_MQ) &&
          same_side(p_Mv2, p_Mv3, p_Mv1, p_MQ) &&
          same_side(p_Mv3, p_Mv1, p_Mv2, p_MQ));
}

Eigen::Vector3d ClosestPointOnLineSegment(
    const Eigen::Vector3d& p_MA, const Eigen::Vector3d& p_MB,
    const Eigen::Vector3d& p_MQ)
{
  const Eigen::Vector3d v_AB = p_MB - p_MA;
  const Eigen::Vector3d v_AQ = p_MQ - p_MA;

  const double ratio = v_AB.dot(v_AQ) / v_AB.squaredNorm();
  const double clamped =
      common_robotics_utilities::utility::ClampValue(ratio, 0.0, 1.0);

  return p_MA + (v_AB * clamped);
}

Eigen::Vector3d CalcClosestPointOnTriangle(
    const Eigen::Vector3d& p_Mv1, const Eigen::Vector3d& p_Mv2,
    const Eigen::Vector3d& p_Mv3, const Eigen::Vector3d& normal,
    const Eigen::Vector3d& p_MQ)
{
  Eigen::Vector3d p_MQclosest;

  if (PointProjectsInsideTriangle(p_Mv1, p_Mv2, p_Mv3, p_MQ))
  {
    const Eigen::Vector3d v_v1Q = p_MQ - p_Mv1;
    // Project query point to triangle plane.
    const Eigen::Vector3d p_MQprojected =
        p_Mv1 + common_robotics_utilities::math::VectorRejection(normal, v_v1Q);
    p_MQclosest = p_MQprojected;
  }
  else
  {
    const Eigen::Vector3d p_MQclosest12 =
        ClosestPointOnLineSegment(p_Mv1, p_Mv2, p_MQ);
    const Eigen::Vector3d p_MQclosest23 =
        ClosestPointOnLineSegment(p_Mv2, p_Mv3, p_MQ);
    const Eigen::Vector3d p_MQclosest31 =
        ClosestPointOnLineSegment(p_Mv3, p_Mv1, p_MQ);
    const double d_closest12_squared = p_MQclosest12.squaredNorm();
    const double d_closest23_squared = p_MQclosest23.squaredNorm();
    const double d_closest31_squared = p_MQclosest31.squaredNorm();
    if (d_closest12_squared <= d_closest23_squared &&
        d_closest12_squared <= d_closest31_squared)
    {
      p_MQclosest = p_MQclosest12;
    }
    else if (d_closest23_squared <= d_closest12_squared &&
             d_closest23_squared <= d_closest31_squared)
    {
      p_MQclosest = p_MQclosest23;
    }
    else
    {
      p_MQclosest = p_MQclosest31;
    }
  }

  return p_MQclosest;
}
}  // namespace

void RasterizeTriangle(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    const size_t triangle_index,
    voxelized_geometry_tools::CollisionMap& collision_map,
    const bool enforce_collision_map_contains_triangle)
{
  if (!collision_map.IsInitialized())
  {
    throw std::invalid_argument("collision_map must be initialized");
  }

  const double min_check_radius = collision_map.GetResolution() * 0.5;
  const double max_check_radius = min_check_radius * std::sqrt(3.0);
  const double max_check_radius_squared = std::pow(max_check_radius, 2.0);

  const Eigen::Vector3i& triangle = triangles.at(triangle_index);
  const Eigen::Vector3d& p_Mv1 = vertices.at(triangle(0));
  const Eigen::Vector3d& p_Mv2 = vertices.at(triangle(1));
  const Eigen::Vector3d& p_Mv3 = vertices.at(triangle(2));

  const Eigen::Vector3d v1v2 = p_Mv2 - p_Mv1;
  const Eigen::Vector3d v1v3 = p_Mv3 - p_Mv1;

  // This assumes that the triangle is well-conditioned. Long and skinny
  // triangles may lead to problems.
  const Eigen::Vector3d normal = v1v2.cross(v1v3);

  const double x_min = std::min({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()});
  const double y_min = std::min({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()});
  const double z_min = std::min({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()});

  const double x_max = std::max({p_Mv1.x(), p_Mv2.x(), p_Mv3.x()});
  const double y_max = std::max({p_Mv1.y(), p_Mv2.y(), p_Mv3.y()});
  const double z_max = std::max({p_Mv1.z(), p_Mv2.z(), p_Mv3.z()});

  const auto min_index =
      collision_map.LocationToGridIndex(x_min, y_min, z_min);
  const auto max_index =
      collision_map.LocationToGridIndex(x_max, y_max, z_max);

  for (int64_t x_index = min_index.X(); x_index <= max_index.X();
       x_index++)
  {
    for (int64_t y_index = min_index.Y(); y_index <= max_index.Y();
         y_index++)
    {
      for (int64_t z_index = min_index.Z(); z_index <= max_index.Z();
           z_index++)
      {
        const common_robotics_utilities::voxel_grid::GridIndex
            current_index(x_index, y_index, z_index);
        const Eigen::Vector3d p_MQ =
            collision_map.GridIndexToLocation(current_index).head<3>();

        const Eigen::Vector3d p_MQclosest =
            CalcClosestPointOnTriangle(p_Mv1, p_Mv2, p_Mv3, normal, p_MQ);

        const double distance_squared = (p_MQclosest - p_MQ).squaredNorm();

        /// TODO(calderpg) Improve this with more a accurate triangle-box check.
        /// This is a coarse approximation of true triangle-box collision.
        /// The right solution is something along the lines of:
        ///
        /// if distance_squared > max_check_radius_squared:
        ///   triangle_intersects = false
        /// else if distance_squared < min_check_radius_squared:
        ///   triangle_intersects = true
        /// else:
        ///   check_index = collision_map.LocationToGridIndex3d(p_MQclosest)
        ///   if check_index == current_index:
        ///     triangle_intersects = true;
        ///   else:
        ///     Fall back to a better triangle-box check
        ///
        /// For now the following approximation is enough.
        const bool triangle_intersects =
            (distance_squared <= max_check_radius_squared);

        if (triangle_intersects)
        {
          auto query = collision_map.GetMutable(current_index);
          if (query)
          {
            query.Value().Occupancy() = 1.0f;
          }
          else if (enforce_collision_map_contains_triangle)
          {
            throw std::runtime_error(
                "Triangle is not contained by collision map");
          }
        }
      }
    }
  }
}

void RasterizeMesh(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    voxelized_geometry_tools::CollisionMap& collision_map,
    const bool enforce_collision_map_contains_mesh)
{
  if (!collision_map.IsInitialized())
  {
    throw std::invalid_argument("collision_map must be initialized");
  }

#if defined(_OPENMP)
#pragma omp parallel for
#endif
  for (size_t idx = 0; idx < triangles.size(); idx++)
  {
    RasterizeTriangle(
        vertices, triangles, idx, collision_map,
        enforce_collision_map_contains_mesh);
  }
}

voxelized_geometry_tools::CollisionMap RasterizeMeshIntoCollisionMap(
    const std::vector<Eigen::Vector3d>& vertices,
    const std::vector<Eigen::Vector3i>& triangles,
    const double resolution)
{
  if (resolution <= 0.0)
  {
    throw std::invalid_argument("resolution must be greater than zero");
  }

  Eigen::Vector3d lower_corner =
      Eigen::Vector3d::Constant(std::numeric_limits<double>::infinity());
  Eigen::Vector3d upper_corner =
      Eigen::Vector3d::Constant(-std::numeric_limits<double>::infinity());

  for (size_t idx = 0; idx < vertices.size(); idx++)
  {
    const Eigen::Vector3d& vertex = vertices.at(idx);
    lower_corner = lower_corner.cwiseMin(vertex);
    upper_corner = upper_corner.cwiseMax(vertex);
  }

  const Eigen::Vector3d object_size = upper_corner - lower_corner;

  const double buffer_size = resolution * 2.0;
  const common_robotics_utilities::voxel_grid::GridSizes filter_grid_sizes(
      resolution, object_size.x() + buffer_size, object_size.y() + buffer_size,
      object_size.z() + buffer_size);

  const Eigen::Isometry3d X_OG(Eigen::Translation3d(
      lower_corner.x() - resolution,
      lower_corner.y() - resolution,
      lower_corner.z() - resolution));

  const voxelized_geometry_tools::CollisionCell empty_cell(0.0f);
  voxelized_geometry_tools::CollisionMap collision_map(
      X_OG, "mesh", filter_grid_sizes, empty_cell);

  RasterizeMesh(vertices, triangles, collision_map);

  return collision_map;
}
}  // namespace voxelized_geometry_tools


#ifndef CT_ICP_CT_ICP_H
#define CT_ICP_CT_ICP_H

#include <iostream>
#include <string>
#include <sstream>
#include <iomanip>
#include <list>

#define _USE_MATH_DEFINES

#include <fstream>
#include <vector>
#include <random>
#include <unordered_map>

#include <Eigen/Dense>

#include "types.hpp"
#include "cost_functions.h"

namespace ct_icp {

    // Subsample to keep one random point in every voxel of the current frame
    void sub_sample_frame(std::vector<Point3D> &frame, double size_voxel);

    // Samples Keypoints randomly in a voxel grid
    void
    grid_sampling(const std::vector<Point3D> &frame, std::vector<Point3D> &keypoints, double size_voxel_subsampling);

    enum CT_ICP_SOLVER {
        GN,
        CERES
    };

    enum LEAST_SQUARES {
        STANDARD,
        CAUCHY,
        HUBER,
        TOLERANT,
        TRUNCATED
    };

    enum VIZ_MODE {
        TIMESTAMP,
        WEIGHT,
        NORMAL
    };

    enum WEIGHTING_SCHEME {
        PLANARITY,      // Weighs residuals by their planarity coefficient
        NEIGHBORHOOD,   // Weighs residuals by the distance to their neares neighbors
        ALL             // Combines all weighting schemes with different coefficients
    };

    // Options for the Elastic_ICP
    struct CTICPOptions {

        // The threshold on the voxel occupancy
        // To be considered in the neighbor search, a voxel must have at least threshold_voxel_occupancy points
        int threshold_voxel_occupancy = 1;

        int init_num_frames = 20; // The number of frames defining the initialization of the map

        double size_voxel_map = 1.0; //Max Voxel : -32767 to 32767 then 32km map for SIZE_VOXEL_MAP = 1m

        int num_iters_icp = 5; // The Maximum number of ICP iterations performed

        int min_number_neighbors = 20;

        short voxel_neighborhood = 1; // Visits the (3 * voxel_neighborhood)^3 neighboring voxels

        double power_planarity = 2.0; // The power of planarity defined in the weighting scheme

        // Whether to estimate the normal of the key point or the closest neighbor
        bool estimate_normal_from_neighborhood = true;

        int max_number_neighbors = 20;

        double max_dist_to_plane_ct_icp = 0.3; // The maximum distance point-to-plane (OLD Version of ICP)

        double threshold_orientation_norm = 0.0001; // Threshold on rotation (deg) for ICP's stopping criterion

        double threshold_translation_norm = 0.001; // Threshold on translation (deg) for ICP's stopping criterion

        bool point_to_plane_with_distortion = true; // Whether to distort the frames at each ICP iteration

        int max_num_residuals = -1; // The maximum number of keypoints used

        int min_num_residuals = 100; // Below this number, CT_ICP will crash

        ICP_DISTANCE distance = CT_POINT_TO_PLANE;

        int num_closest_neighbors = 1; // The number of closest neighbors considered as residuals

        // TODO : Add Trajectory Constraints Options
        double beta_location_consistency = 0.001; // Constraints on location

        double beta_constant_velocity = 0.001; // Constraint on velocity

        double beta_small_velocity = 0.0; // Constraint on the relative motion

        double beta_orientation_consistency = 0.0; // Constraint on the orientation consistency

        WEIGHTING_SCHEME weighting_scheme = ALL;

        double weight_alpha = 0.9;

        double weight_neighborhood = 0.1;

        CT_ICP_SOLVER solver = GN;

        /* ---------------------------------------------------------------------------------------------------------- */
        /* LEAST SQUARE OPTIMIZATION PARAMETERS                                                                       */

        LEAST_SQUARES loss_function = CAUCHY;

        int ls_max_num_iters = 1;

        int ls_num_threads = 16;

        double ls_sigma = 0.1; // The robust parameter (for Cauchy, Huber or truncated least square)

        double ls_tolerant_min_threshold = 0.05; // The Tolerant

        // Debug params
        bool debug_print = true; // Whether to output debug information to std::cout

        bool debug_viz = false; // Whether to pass the key points to the ExplorationEngine

        VIZ_MODE viz_mode = TIMESTAMP;
    };

    struct ICPSummary {
        bool success = false; // Whether the registration succeeded

        int num_residuals_used = 0;

        std::string error_log;
    };

    // CT_ICP_CERES : Registers keypoints into the voxel_map taking into account the motion of the
    //               Sensor during the acquisition of the LiDAR Frame
    //
    // Refines the estimate of `trajectory[index_frame]` by registering the points of vector `keypoints`
    // Into the voxel map `voxels_map`. The points of the vector `keypoints` are also modified by interpolation
    // of the beginning and end pose of the associated trajectory frame, using the timestamp alpha_timestamp.
    //
    // For distance CT_POINT_TO_PLANE:
    //      Both the beginning and end pose of the trajectory are chosen as parameters
    //      Each residual is the point-to-plane residual of the point transformed using the interpolated pose
    //      Note:
    //          CT_POINT_TO_PLANE requires meaningful timestamps. When timestamps are not known, they should
    //          all be set to 1.0, and the distance POINT_TO_PLANE should be selected.
    //
    // For distance POINT_TO_PLANE:
    //      Only the end pose of the trajectory is optimized (the beginning of the trajectory is not refined).
    //      If `options.point_to_plane_with_distortion` is true, then at each step, the keypoints are distorted
    //      At each iteration, after refinement of the estimate of the end pose of the trajectory frame
    //
    // Note: CT_ICP_CERES will modify the last TrajectoryFrame of the trajectory vector
    ICPSummary CT_ICP_CERES(const CTICPOptions &options,
                            const VoxelHashMap &voxels_map, std::vector<Point3D> &keypoints,
                            std::vector<TrajectoryFrame> &trajectory, int index_frame);

    ICPSummary CT_ICP_GN(const CTICPOptions &options,
                         const VoxelHashMap &voxels_map, std::vector<Point3D> &keypoints,
                         std::vector<TrajectoryFrame> &trajectory, int index_frame);

} // namespace Elastic_ICP

#endif //CT_ICP_CT_ICP_H


/** \file mean_combined.hpp
    \brief Parametric functions that combine other functions */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _MEAN_COMBINED_HPP_
#define  _MEAN_COMBINED_HPP_

#include <boost/numeric/ublas/vector_proxy.hpp>
#include "mean_functors.hpp"

namespace bayesopt
{

  /**\addtogroup ParametricFunctions
   * @{
   */

  /** \brief Abstract class for combined functions.
   *  It allows combinations of other functions (addition, product, etc.)
   */
  class CombinedFunction : public ParametricFunction
  {
  public:
    virtual int init(size_t input_dim,
		     ParametricFunction* left,
		     ParametricFunction* right)
    {
      n_inputs = input_dim;
      this->left = left;
      this->right = right;
      return 0;
    };
    void setParameters(const vectord &theta)
    {
      using boost::numeric::ublas::subrange;

      size_t n_lhs = left->nParameters();
      size_t n_rhs = right->nParameters();
      if (theta.size() != n_lhs + n_rhs)
	{
	  throw std::invalid_argument("Wrong number of mean function parameters");
	}

      left->setParameters(subrange(theta,0,n_lhs));
      right->setParameters(subrange(theta,n_lhs,n_lhs+n_rhs));
    };

    vectord getParameters()
    {
      using boost::numeric::ublas::subrange;

      size_t n_lhs = left->nParameters();
      size_t n_rhs = right->nParameters();
      vectord par(n_lhs + n_rhs);
      subrange(par,0,n_lhs) = left->getParameters();
      subrange(par,n_lhs,n_lhs+n_rhs) = right->getParameters();
      return par;
    };

    size_t nParameters()
    {
      size_t n_lhs = left->nParameters();
      size_t n_rhs = right->nParameters();
      return n_lhs + n_rhs;
    };

    virtual ~CombinedFunction()
    {
      delete left;
      delete right;
    };

  protected:
    ParametricFunction* left;
    ParametricFunction* right;
  };

  /** \brief Sum of two kernels */
  class SumFunction: public CombinedFunction
  {
  public:
    double getMean(const vectord &x)
    {
      return left->getMean(x) + right->getMean(x);
    };

    vectord getFeatures(const vectord &x)
    {
      using boost::numeric::ublas::subrange;

      size_t n_lhf = left->nFeatures();
      size_t n_rhf = right->nFeatures();
      vectord feat(n_lhf + n_rhf);
      subrange(feat,0,n_lhf) = left->getFeatures(x);
      subrange(feat,n_lhf,n_lhf+n_rhf) = right->getFeatures(x);
      return feat;
    };

    size_t nFeatures()
    {
      size_t n_lhf = left->nFeatures();
      size_t n_rhf = right->nFeatures();
      return n_lhf + n_rhf;
    };

  };

  //@}

} //namespace bayesopt

#endif


#ifndef SEGMATCH_GRAPH_BASED_GEOMETRIC_CONSISTENCY_RECOGNIZER_HPP_
#define SEGMATCH_GRAPH_BASED_GEOMETRIC_CONSISTENCY_RECOGNIZER_HPP_

#include <boost/graph/adjacency_list.hpp>

#include "segmatch/parameters.hpp"
#include "segmatch/recognizers/correspondence_recognizer.hpp"

namespace segmatch {

/// \brief Recognizes a model in a scene using a graph-based approach. First a consistency graph
/// is constructed, where the nodes represent the matches and edges connect matches that are
/// pairwise consistent. Recognition finds a maximum clique matches that are pairwise consistent.
class GraphBasedGeometricConsistencyRecognizer : public CorrespondenceRecognizer {
 public:
  /// \brief Initializes a new instance of the GraphBasedGeometricConsistencyRecognizer class.
  /// \param params The parameters of the geometry consistency grouping.
  explicit GraphBasedGeometricConsistencyRecognizer(
      const GeometricConsistencyParams& params) noexcept;

  /// \brief Sets the current matches and tries to recognize the model.
  /// \param predicted_matches Vector of possible correspondences between model and scene.
  void recognize(const PairwiseMatches& predicted_matches) override;

  /// \brief Gets the candidate transformations between model and scene.
  /// \returns Vector containing the candidate transformations. Transformations are sorted in
  /// decreasing recognition quality order. If empty, the model was not recognized.
  const std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>&
  getCandidateTransformations() const override {
    return candidate_transfomations_;
  }

  /// \brief Gets the candidate clusters of matches between model and scene. Every cluster
  /// represents a possible recognition.
  /// \returns Vector containing the candidate clusters. Clusters are sorted in
  /// decreasing recognition quality order. If empty, the model was not recognized.
  const std::vector<PairwiseMatches>& getCandidateClusters() const override {
    return candidate_matches_;
  }

 protected:
  // Data types for the consistency graph.
  typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS> ConsistencyGraph;

  /// \brief Builds a consistency graph of the provided matches.
  /// \param predicted_matches Vector of possible correspondences between model and scene.
  /// \returns Graph encoding pairwise consistencies. Match \c predicted_matches[i] is represented
  /// by node \c i .
  virtual ConsistencyGraph buildConsistencyGraph(const PairwiseMatches& predicted_matches) = 0;

  // The parameters of the geometry consistency grouping.
  GeometricConsistencyParams params_;

 private:
  // Estimate 3D transform between model and scene.
  Eigen::Matrix4f estimateRigidTransformation(const PairwiseMatches& true_matches);

  // Candidate transformations and matches between model and scene.
  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f>>
  candidate_transfomations_;
  std::vector<PairwiseMatches> candidate_matches_;
}; // class GraphBasedGeometricConsistencyRecognizer

} // namespace segmatch

#endif // SEGMATCH_GRAPH_BASED_GEOMETRIC_CONSISTENCY_RECOGNIZER_HPP_


#include "reviewsreader.h"

#include <Eigen/Dense>

#include <experimental/filesystem>
#include <iostream>
#include <string>

namespace fs = std::experimental::filesystem;

int main(int argc, char** argv) {
  if (argc > 1) {
    auto file_path = fs::path(argv[1]);
    if (fs::exists(file_path)) {
      auto papers = ReadPapersReviews(file_path);
      // create matrices
      Eigen::MatrixXi x_data(papers.size(), 3);
      Eigen::MatrixXi y_data(papers.size(), 1);
      Eigen::Index i = 0;
      for (const auto& p : papers) {
        if (p.preliminary_decision == "accept")
          y_data(i, 0) = 1;
        else
          y_data(i, 0) = 0;

        if (!p.reviews.empty()) {
          int64_t confidence_avg = 0;
          int64_t evaluation_avg = 0;
          int64_t orientation_avg = 0;
          for (const auto& r : p.reviews) {
            confidence_avg += std::stoi(r.confidence);
            evaluation_avg += std::stoi(r.evaluation);
            orientation_avg += std::stoi(r.orientation);
          }
          int64_t reviews_num = static_cast<int64_t>(p.reviews.size());
          x_data(i, 0) = static_cast<int>(confidence_avg / reviews_num);
          x_data(i, 1) = static_cast<int>(evaluation_avg / reviews_num);
          x_data(i, 2) = static_cast<int>(orientation_avg / reviews_num);
        }
        ++i;
      }
      std::cout << x_data << std::endl;
      std::cout << y_data << std::endl;

    } else {
      std::cout << "File path is incorrect " << file_path << "\n";
    }
  } else {
    std::cout << "Please provide a path to a dataset file\n";
  }

  return 0;
}


#pragma once

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

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include "cholmod.h"
#include <memory>

#include <nlohmann/json.hpp>
using json = nlohmann::json;

namespace polysolve
{
    typedef Eigen::SparseMatrix<double, Eigen::ColMajor, long int> StiffnessMatrixL;
    /**
     @brief      Base class for cholmod solver.
 */
    class CHOLMODSolver
    {

    protected:

        cholmod_common *cm;
        cholmod_sparse A;
        cholmod_dense *x, b;
        cholmod_factor *L;

    public:
        CHOLMODSolver();
        ~CHOLMODSolver();

        // Set solver parameters
        // virtual void setParameters(const json &params) {}

        // Get info on the last solve step
        void getInfo(json &params) const;

        // Analyze sparsity pattern
        void analyzePattern(StiffnessMatrixL &Ain);

        // Factorize system matrix
        void factorize(StiffnessMatrixL &Ain);

        //
        // @brief         { Solve the linear system Ax = b }
        //
        // @param[in]     b     { Right-hand side. }
        // @param[in,out] x     { Unknown to compute. When using an iterative
        //                      solver, the input unknown vector is used as an
        //                      initial guess, and must thus be properly allocated
        //                      and initialized. }
        //
        void solve(Eigen::VectorXd &rhs, Eigen::VectorXd &result);
    };

} // namespace polysolve


#include <iostream>
#include <fstream>
#include <chrono>

#include <opencv2/core.hpp>

#include <dlib/opencv.h>
#include <dlib/data_io.h>
#include <dlib/dnn.h>
#include <dlib/array2d.h>
#include <dlib/image_transforms.h>

#include "ReadMNISTDatas.hpp"
#include "TrainFilePath.hpp"
#include "../TestDNN/DLIBInput.hpp"
#include "NetworkStringTraits.hpp"

namespace enabler { extern void* enabler; }

template <class T>
using UnqualifiedType = std::remove_pointer_t<std::remove_cv_t<std::remove_reference_t<T>>>;

template <class Network>
void saveWeights(std::ostream& output, Network&& net);

template <class Network, class T>
void saveWeightsImpl(std::ostream& output, Network&& net, dlib::input<T> const& input);

template <class Network, class Subnet>
void saveWeightsImpl(std::ostream& output, Network&& net, Subnet&& subnet);

int main()
{
  using net_type = dlib::loss_multiclass_log<
    dlib::fc<10,
    dlib::relu<dlib::fc<84,
    dlib::relu<dlib::fc<120,
    dlib::max_pool<2, 2, 2, 2, dlib::relu<dlib::con<16, 5, 5, 1, 1,
    dlib::max_pool<2, 2, 2, 2, dlib::relu<dlib::con<6, 5, 5, 1, 1,
    dlib::input<cv::Mat>
    >>>>>>>>>>>>;

  net_type net;
  dlib::deserialize("mnist_network.dat") >> net;

  std::fstream output("weights.csv", std::ios::out | std::ios::trunc);
  saveWeights(output, net);

  return 0;
}

template <class Network>
void saveWeights(std::ostream& output, Network&& net)
{
  auto&& subnet = net.subnet();
  saveWeightsImpl(output, subnet, subnet.subnet());
}

using InputLayer = dlib::input<cv::Mat>;
/*
template <
  class Network,
  class Subnet,
  std::enable_if_t<
    std::is_same<UnqualifiedType<Subnet>, dlib::input<cv::Mat>>::value
  >*& = enabler::enabler
>
*/
template <class Network>
void saveWeightsImpl(std::ostream& output, Network&& net, InputLayer input)
{
  return;
}

template <class Network, class Subnet>
void saveWeightsImpl(std::ostream& output, Network&& net, Subnet&& subnet)
{
  using NetworkType = std::remove_reference_t<Network>;
  using SubnetType = std::remove_reference_t<Subnet>;
  saveWeightsImpl(
    output,
    std::forward<NetworkType::subnet_type>(net.subnet()),
    std::forward<SubnetType::subnet_type>(subnet.subnet())
  );

  output << NetworkStringTraits<NetworkType>::STRING << "\n";
  dlib::tensor const& tensor = net.layer_details().get_layer_params();
  output << "Layer:" << NetworkType::num_layers << ","
         << "Row:" << tensor.nr() << ","
         << "Column:" << tensor.nc() << ","
         << "Channel:" << tensor.k() << "\n";

  for (long k = 0; k < tensor.k(); ++k)
  {
    auto ptr = tensor.host();
    output << "Node" << k << "\n";
    for (long r = 0; r < tensor.nr(); ++r)
    {
      for (long c = 0; c < tensor.nc(); ++c)
      {
        output << ptr[c * tensor.k() + r * tensor.nc() + k] << ",";
      }
      output.seekp(std::ios::cur, -1) << "\n";
    }
    output << "\n";
  }

  output << "\n";
}


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at https://github.com/jjackowski/duds/blob/master/LICENSE.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2018  Jeff Jackowski
 */
/**
 * @file
 * Test of the duds::general::DataSize template.
 */

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include <duds/general/DataSize.hpp>

// operator << must be in the same namespace as what it outputs
namespace duds { namespace general {
template <std::size_t Bits>
std::ostream &operator << (std::ostream &os, const DataSize<Bits> &ds) {
	return os << '(' << ds.blocks() << ',' << Bits << ')';
}
} }

BOOST_AUTO_TEST_SUITE(DataSize)

BOOST_AUTO_TEST_CASE(DataSize) {
	BOOST_CHECK_EQUAL(std::is_trivial<duds::general::Bits>::value, true);
	BOOST_CHECK_EQUAL(std::is_trivially_copyable<duds::general::Bits>::value, true);
	// operations on bit sizes
	constexpr duds::general::Bits oneb(1), eightb(8);
	BOOST_CHECK_EQUAL(oneb.blocks(), 1);
	BOOST_CHECK_EQUAL(eightb.blocks(), 8);
	BOOST_CHECK(oneb != eightb);
	BOOST_CHECK_EQUAL(eightb.size<duds::general::Bytes>().blocks(), 1);

	// this doesn't, and shouldn't, compile
	//constexpr duds::general::Bytes badbyte(oneb);
	// this dies at run-time
	BOOST_CHECK_THROW(
		const duds::general::Bytes badbyte(oneb),
		duds::general::DataSizeConversionError
	);
	// wish this would fail to compile
	BOOST_CHECK_THROW(
		duds::general::Kilobytes(duds::general::Bytes(256)),
		duds::general::DataSizeConversionError
	);
	BOOST_CHECK_NO_THROW(
		duds::general::Kilobytes(duds::general::Bytes(256).sizeRounded<
			duds::general::Kilobytes
		>())
	);

	// try out Bytes type
	constexpr duds::general::Bytes oneB(eightb);
	BOOST_CHECK_EQUAL(oneB.blocks(), 1);
	BOOST_CHECK_EQUAL(oneB.bytes(), 1);
	BOOST_CHECK_EQUAL(oneB.bits(), 8);
	BOOST_CHECK_EQUAL(eightb, oneB);
	constexpr duds::general::Bits anotherEight(oneB);
	BOOST_CHECK_EQUAL(anotherEight.blocks(), 8);
	constexpr duds::general::Bytes foobyte(
		eightb.size<duds::general::Bytes>()
	);

	// nibbles & some conversions
	constexpr duds::general::Nibbles twoNa(oneB), twoNb(eightb);
	BOOST_CHECK_EQUAL(twoNa.blocks(), 2);
	BOOST_CHECK_EQUAL(twoNb.blocks(), 2);
	BOOST_CHECK_EQUAL(twoNa, eightb);
	BOOST_CHECK_EQUAL(
		twoNa.sizeRounded<duds::general::Kilobytes>(),
		duds::general::Kilobytes(1)
	);

	// operator tests
	constexpr duds::general::Bytes sum0(twoNa + duds::general::Kilobytes(1));
	BOOST_CHECK_EQUAL(sum0, duds::general::Bytes(1025));
	BOOST_CHECK_EQUAL(sum0, oneB + duds::general::Kilobytes(1));
	BOOST_CHECK(sum0 > duds::general::Kilobytes(1));
	constexpr duds::general::Bytes halfK(512);
	BOOST_CHECK_EQUAL(halfK / 2, duds::general::Bytes(256));
	BOOST_CHECK_EQUAL(halfK * 4, duds::general::Kilobytes(2));
	// wish this would fail to compile
	BOOST_CHECK_THROW(
		duds::general::Kilobytes kb(halfK),
		duds::general::DataSizeConversionError
	);
	duds::general::Kilobytes halfK2(halfK * 2);
	BOOST_CHECK_EQUAL(halfK * 2, halfK2);
	duds::general::Kilobytes kw(halfK2);
	BOOST_CHECK(kw > halfK);
	BOOST_CHECK_EQUAL(kw, halfK2);
	BOOST_CHECK_EQUAL(kw, duds::general::Kilobytes(1));

	// some more math
	duds::general::Bytes sum;
	BOOST_CHECK_NO_THROW(sum = halfK + duds::general::Kilobytes(1));
	BOOST_CHECK_EQUAL(sum.bytes(), 1536);
	BOOST_CHECK_EQUAL(sum, duds::general::DataSize<1536*8>(1));
	BOOST_CHECK(sum > duds::general::Kilobytes(1));
	BOOST_CHECK(sum < duds::general::Kilobytes(2));
	BOOST_CHECK_THROW(
		duds::general::Kilobytes(1) + sum,
		duds::general::DataSizeConversionError
	);
	BOOST_CHECK_NO_THROW(sum += halfK);
	BOOST_CHECK_EQUAL(sum.bytes(), 2048);
	BOOST_CHECK_EQUAL(sum, duds::general::Kilobytes(2));
	BOOST_CHECK_NO_THROW(sum + duds::general::Kilobytes(1));
	BOOST_CHECK_NO_THROW(duds::general::Kilobytes(1) + sum);
	BOOST_CHECK_NO_THROW(sum /= 2);
	BOOST_CHECK_EQUAL(sum, duds::general::Kilobytes(1));
	BOOST_CHECK_NO_THROW(sum *= 5);
	BOOST_CHECK_EQUAL(sum, duds::general::Kilobytes(5));
}

BOOST_AUTO_TEST_SUITE_END()


/**
 * @file   map_sim.cpp
 * @author Changsheng Jiang <jiangzuoyan@gmail.com>
 * @date   Fri Oct 21 17:07:48 2011
 *
 * @brief map similarity
 *
 *
 */
#include "douban/map_utility.hpp"
#include <boost/python.hpp>
#include <boost/python/stl_iterator.hpp>
#include <douban/fmtstr.hpp>

namespace bpy = boost::python;

namespace  {

template <class K, class Func>
void input_map(
    K /**/,
    bpy::object &A, Func func) {
    bpy::stl_input_iterator<bpy::object> first(A), last;
    while (first != last) {
      K k = bpy::extract<K>(*first);
      bpy::object v = A[*first];
      func(k, v);
      ++first;
    }
}

} // namespace anonymous

// return (A * B' ktop by row)
bpy::object map_sim(
    bpy::object R, bpy::object A, bpy::object B, size_t ktop) {
  douban::map_type<size_t, size_t, double>::type a;
  douban::map_type<size_t, size_t, double>::type b;
  douban::map_type<size_t, std::vector< std::pair<size_t, double> > >::type sims;

  auto input_map2 = [&](
      bpy::object &A,
      douban::map_type<size_t, size_t, double>::type &a) {
    input_map(
        (size_t)0, A,
        [&](size_t k, bpy::object v) {
          auto &d = a[k];
          input_map(k, v, [&](size_t i, bpy::object vv) {
              double x = bpy::extract<double>(vv);
            d[i] = x;
            });
        });
  };
  input_map2(A, a);
  input_map2(B, b);
  b = douban::map_reverse2(b);

  auto sim_cmp = [](const std::pair<size_t, double> &a,
                    const std::pair<size_t, double> &b) {
    return douban::cmp(a.second, b.second);
  };

  auto insert_sim = [&](
      size_t f, std::vector<std::pair<size_t, double> > &vsim, size_t t, double s) {
    if (f == t) return; // ignore self sim.
    auto elm = std::make_pair(t, s);
    if (vsim.size() < (size_t)ktop) {
      vsim.push_back(elm);
      if (vsim.size() == (size_t)ktop) {
        douban::heapify(vsim.begin(), vsim.end(), sim_cmp);
      }
    } else {
      if (sim_cmp(vsim[0], elm) < 0) {
        vsim[0] = elm;
        douban::heap_shiftdown(vsim.begin(), vsim.end(), vsim.begin(), sim_cmp);
      }
    }
  };

  input_map(
      (size_t)0, R,
      [&](size_t n, bpy::object v) {
        bpy::stl_input_iterator<bpy::object> first(v), last;
        auto &d = sims[n];
        while (first != last) {
          bpy::object elm = *first;
          size_t j = bpy::extract<size_t>(elm[0]);
          double s = bpy::extract<double>(elm[1]);
          insert_sim(n, d, j, s);
          ++first;
        }
      });

  BOOST_FOREACH (auto &irow, a) {
    auto i = irow.first;
    douban::map_type<size_t, double>::type isim;
    BOOST_FOREACH (auto &kv, irow.second) {
      auto k = kv.first;
      auto av = kv.second;
      auto it = b.find(k);
      if (it == b.end()) continue;
      BOOST_FOREACH (auto &jv, it->second) {
        auto j = jv.first;
        auto bv = jv.second;
        douban::set_default(isim, j, 0.) += av * bv;
      }
    }
    auto &vsim = sims[i];
    BOOST_FOREACH(auto &js, isim) {
      insert_sim(i, vsim, js.first, js.second);
    }
  }

  bpy::dict ret;
  BOOST_FOREACH (auto &ns, sims) {
    auto n = ns.first;
    bpy::list l;
    BOOST_FOREACH (auto &js, ns.second) {
      l.append(bpy::make_tuple(js.first, js.second));
    }
    ret[n] = l;
  }
  // douban::fmterrlnt("A=(#%s, nnz=%s), B=(#%s, nnz=%s), ktop=%s",
  //                   a.size(), douban::map_size2(a),
  //                   b.size(), douban::map_size2(b),
  //                   ktop);
  return ret;
}

BOOST_PYTHON_MODULE(map_sim) {
  bpy::def("map_sim", &map_sim, "map_sim(A, B, ktop)");
};


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

#include "precomp.hpp"

// Eigen
#include <Eigen/Core>

// OpenCV
#include <opencv2/core/eigen.hpp>
#include <opencv2/sfm/conditioning.hpp>

// libmv headers
#include "libmv/multiview/conditioning.h"

namespace cv
{
namespace sfm
{

template<typename T>
void
preconditionerFromPoints( const Mat_<T> &_points,
                          Mat_<T> _Tr )
{
  libmv::Mat points;
  libmv::Mat3 Tr;

  cv2eigen( _points, points );

  libmv::PreconditionerFromPoints( points, &Tr );

  eigen2cv( Tr, _Tr );
}

void
preconditionerFromPoints( InputArray _points,
                          OutputArray _T )
{
  const Mat points = _points.getMat();
  const int depth = points.depth();
  CV_Assert((points.dims == 2 || points.dims == 3) && (depth == CV_32F || depth == CV_64F));

  _T.create(3, 3, depth);

  Mat T = _T.getMat();

  if ( depth == CV_32F )
  {
    preconditionerFromPoints<float>(points, T);
  }
  else
  {
    preconditionerFromPoints<double>(points, T);
  }
}

template<typename T>
void
isotropicPreconditionerFromPoints( const Mat_<T> &_points,
                                   Mat_<T> _T )
{
  libmv::Mat points;
  libmv::Mat3 Tr;

  cv2eigen( _points, points );

  libmv::IsotropicPreconditionerFromPoints( points, &Tr );

  eigen2cv( Tr, _T );
}

void
isotropicPreconditionerFromPoints( InputArray _points,
                                   OutputArray _T )
{
  const Mat points = _points.getMat();
  const int depth = points.depth();
  CV_Assert((points.dims == 2 || points.dims == 3) && (depth == CV_32F || depth == CV_64F));

  _T.create(3, 3, depth);

  Mat T = _T.getMat();

  if ( depth == CV_32F )
  {
    isotropicPreconditionerFromPoints<float>(points, T);
  }
  else
  {
    isotropicPreconditionerFromPoints<double>(points, T);
  }
}

template<typename T>
void
applyTransformationToPoints( const Mat_<T> &_points,
                             const Mat_<T> &_T,
                             Mat_<T> _transformed_points )
{
  libmv::Mat points, transformed_points;
  libmv::Mat3 Tr;

  cv2eigen( _points, points );
  cv2eigen( _T, Tr );

  libmv::ApplyTransformationToPoints( points, Tr, &transformed_points );

  eigen2cv( transformed_points, _transformed_points );
}

void
applyTransformationToPoints( InputArray _points,
                             InputArray _T,
                             OutputArray _transformed_points )
{
  const Mat points = _points.getMat(), T = _T.getMat();
  const int depth = points.depth();
  CV_Assert((points.dims == 2 || points.dims == 3) && T.size() == Size(3,3) && (depth == CV_32F || depth == CV_64F));

  _transformed_points.create(points.size(), depth);

  Mat transformed_points = _transformed_points.getMat();

  if ( depth == CV_32F )
  {
    applyTransformationToPoints<float>(points, T, transformed_points);
  }
  else
  {
    applyTransformationToPoints<double>(points, T, transformed_points);
  }
}

void
normalizePoints( InputArray points,
                 OutputArray normalized_points,
                 OutputArray T )
{
  preconditionerFromPoints(points, T);
  applyTransformationToPoints(points, T, normalized_points);
}

void
normalizeIsotropicPoints( InputArray points,
                          OutputArray normalized_points,
                          OutputArray T )
{
  isotropicPreconditionerFromPoints(points, T);
  applyTransformationToPoints(points, T, normalized_points);
}

} /* namespace sfm */
} /* namespace cv */


/* Copyright (c) 2012, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See LICENSE.txt or
 * http://www.opensource.org/licenses/mit-license.php */

#include <baseMeasure.hpp>
#include <numpyArma.hpp>

#include <armadillo>

#include <boost/python.hpp>
#include <boost/python/wrapper.hpp>
#include <numpy/ndarrayobject.h> // for PyArrayObject

#ifdef PYTHON_2_6
  #include <python2.6/object.h> // for PyArray_FROM_O
#endif
#ifdef PYTHON_2_7
  #include <python2.7/object.h> // for PyArray_FROM_O
#endif

using namespace boost::python;

class Dir_py : public Dir
{
public:
  Dir_py(const numeric::array& alphas) :
	  Dir(np2row<double>(alphas))
  {
	  //cout<<"alphas: "<<mAlphas<<endl;
  };
  Dir_py(const Dir_py& dir) :
	  Dir(dir)
  {
	  //cout<<"alphas: "<<mAlphas<<endl;
  };

  void asRow(const numeric::array& row)
  {
    Row<double> row_arma = Dir::asRow();
    Row<double> row_wrap = np2row<double>(row);
    row_wrap = row_arma;
  };

};

class NIW_py : public NIW
{
public:
	NIW_py(const numeric::array& vtheta, double kappa,
			const numeric::array& Delta, double nu) :
        NIW(np2col<double>(vtheta),kappa,np2mat<double>(Delta),nu)
    //        cpVtheta(np2col<double>(vtheta)), cpDelta(np2mat<double>(Delta)),
//  NIW(cpVtheta,kappa,cpDelta,nu)
	{
	  //cout<<"Creating "<<typeid(this).name()<<endl;
	};
	NIW_py(const NIW_py& inw) :
		NIW(inw)
	{};

  void asRow(const numeric::array& row)
  {
    Row<double> row_arma = NIW::asRow();
    Row<double> row_wrap = np2row<double>(row);
    row_wrap = row_arma;
  };

};


using namespace std;
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE hello
#include <iostream>
#include <boost/test/unit_test.hpp>
#include <memory>
#include <glm/glm.hpp>
#include <glm/ext.hpp>
#include <glm/mat4x4.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include "BoundingBox.h"
#include "BoundingBox.cc"

BOOST_AUTO_TEST_SUITE( hello );

int add(int i, int j)
{
    return i + j;
}

BOOST_AUTO_TEST_CASE(Add)
{
    BOOST_CHECK(add(2, 2) == 4);
}

BOOST_AUTO_TEST_CASE(CollidesTrue)
{
	auto b1 = make_shared<BoundingBox>(BoundingBox(Vector3(0.0f, 0.0f, 0.0f), 0.5f, 0.5f, 0.5f));
	auto b2 = make_shared<BoundingBox>(BoundingBox(Vector3(0.40f, 0.40f, 0.40f), 0.5f, 0.5f, 0.5f));
	BOOST_CHECK(b1->CollidesWith(b2));
	BOOST_CHECK(b2->CollidesWith(b1));
}

BOOST_AUTO_TEST_CASE(CollidesFalse)
{
	auto b1 = make_shared<BoundingBox>(BoundingBox(Vector3(0.0f, 0.0f, 0.0f), 0.5f, 0.5f, 0.5f));
	auto b2 = make_shared<BoundingBox>(BoundingBox(Vector3(0.50f, 0.50f, 0.50f), 0.5f, 0.5f, 0.5f));
	BOOST_CHECK(!b1->CollidesWith(b2));
	BOOST_CHECK(!b2->CollidesWith(b1));
}

BOOST_AUTO_TEST_SUITE_END()


#include <opencv2/opencv.hpp>
#include <boost/foreach.hpp>
#include <boost/filesystem.hpp>
#include <unistd.h>
#include "calotypes/CalibrationLog.h"
#include "calotypes/PlanarGridStructure.h"

using namespace calotypes;
namespace bfs = boost::filesystem;

void PrintHelp()
{
	std::cout << "process_images: Generate detection file from images" << std::endl;
	std::cout << "Arguments: " << std::endl;
	std::cout << "[-r rows] Rows in the pattern" << std::endl;
	std::cout << "[-c cols] Columns in the pattern" << std::endl;
	std::cout << "[-s size] Pattern grid dimension" << std::endl;
	std::cout << "[-d dir] Directory of images" << std::endl;
	std::cout << "[-o output] Output file name" << std::endl;
	std::cout << "[-v vis_dir] Directory to output visualized detections" << std::endl;
	std::cout << "[-t mthresh] Movement rejection threshold" << std::endl;
}

double ComputeMovement( const ImagePoints& a, const ImagePoints& b )
{
	if( a.size() != b.size() )
	{
		throw std::runtime_error( "Cannot compare image points of different sizes." );
	}

	double acc = 0;
	double dx, dy;
	for( unsigned int i = 0; i < a.size(); i++ )
	{
		dx = a[i].x - b[i].x;
		dy = a[i].y - b[i].y;
		acc += std::sqrt( dx*dx + dy*dy );
	}
	return acc / a.size();
}

struct Entry
{
	std::string path;
	std::pair<std::string,std::string> stem;
};

bool CompareNames( const Entry& a, const Entry& b )
{
	if( a.stem.first.size() == b.stem.first.size() )
	{
		return a.stem < b.stem;
	}
	return a.stem.first.size() < b.stem.first.size();
}

int main( int argc, char** argv )
{

	unsigned int cbRows = 6, cbCols = 7;
	double cbDim = 1.0;
	char c;
	std::string imageDir( "./" );
	std::string outputPath( "detections.txt" );
	bool enableVis = false;
	std::string visDir;
	double movementThreshold = 1.0;

	while( (c = getopt( argc, argv, "hr:c:s:d:o:v:t:")) != -1 )
	{
		switch( c )
		{
			case 'h':
				PrintHelp();
				return 0;
			case 'r':
				cbRows = strtol( optarg, NULL, 10 );
				break;
			case 'c':
				cbCols = strtol( optarg, NULL, 10 );
				break;
			case 's':
				cbDim = strtod( optarg, NULL );
				break;
			case 'd':
				imageDir.assign( optarg );
				break;
			case 'o':
				outputPath.assign( optarg );
				break;
			case 'v':
				enableVis = true;
				visDir.assign( optarg );
				break;
			case 't':
				movementThreshold = strtod( optarg, NULL );
				break;
			case '?':
                return -1;
            default:
				std::cerr << "Unrecognized option." << std::endl;
                return -1;
		}
	}

	PlanarGridStructure patternGrid( cbRows, cbCols, cbDim );

	if( imageDir.back() != '/' ) { imageDir += "/"; }
	if( visDir.back() != '/' ) { visDir += "/"; }

	if( !bfs::exists( imageDir ) || !bfs::is_directory( imageDir ) )
	{
		throw std::runtime_error( "Invalid directory " + imageDir );
	}

	std::vector< Entry > entries;
// 	std::vector< std::string > imagePaths;
// 	std::vector< std::pair<std::string, std::string> > fileStems;
	bfs::directory_iterator iter( imageDir ), endIter;
	while( iter != endIter )
	{
		bfs::directory_entry entry = *iter;
		iter++;
		if( !is_regular_file( entry.path() ) ) { continue; }
		Entry e;
		e.path = entry.path().string();
// 		imagePaths.push_back( entry.path().string() );
		std::pair<std::string,std::string> stem( entry.path().stem().string(),
												 entry.path().extension().string() );
// 		fileStems.push_back( stem );
		e.stem = stem;
		entries.push_back( e );
	}
	std::sort( entries.begin(), entries.end(), CompareNames );

	CalibrationLogWriter writer( outputPath );

	cv::namedWindow( "images" );

	ImagePoints corners, prevCorners;
	unsigned int accepted = 0;
	for( unsigned int i = 0; i < entries.size(); i++ )
	{
		std::string path = entries[i].path;
		std::cout << "Reading image " << path << std::endl;
		cv::Mat image = cv::imread( path );
		if( image.empty() )
		{
			std::cerr << "Could not read image at " << path << std::endl;
			return -1;
		}
		cv::Mat gray;
		cv::cvtColor( image, gray, cv::COLOR_BGR2GRAY);

		if( !cv::findChessboardCorners( gray, patternGrid.GetSize(), corners,
										cv::CALIB_CB_ADAPTIVE_THRESH |
										cv::CALIB_CB_NORMALIZE_IMAGE |
										cv::CALIB_CB_FAST_CHECK ) )
		{
			std::cerr << "Could not find chessboard in " << path << std::endl;
			continue;
		}
		cv::TermCriteria cspCrit( cv::TermCriteria::EPS | cv::TermCriteria::COUNT, 30, 0.1 );
		cv::cornerSubPix( gray, corners, cv::Size(11,11), cv::Size(-1,-1), cspCrit );

		if( prevCorners.empty() )
		{
			prevCorners = corners;
			continue;
		}

		double movement = ComputeMovement( prevCorners, corners );
		prevCorners = corners;
		if( movement > movementThreshold )
		{
			std::cout << "Movement of " << movement << " exceeds threshold. Continuing..." << std::endl;
			continue;
		}
		else
		{
			std::cout << "Movement of " << movement << " accepted!" << std::endl;
			accepted++;
		}

		CameraTrainingData datum;
		datum.name = path;
		datum.imageSize = image.size();
		datum.imagePoints = corners;
		datum.objectPoints = patternGrid.GetPoints();

		writer.WriteNext( datum );

		cv::Mat drawn;
		cv::drawChessboardCorners( image, patternGrid.GetSize(), corners, true );
		cv::imshow( "images", image );
		cv::waitKey( 1 );

		if( enableVis )
		{
			std::stringstream ss;
			ss << visDir << entries[i].stem.first << "_detection" << entries[i].stem.second;
			imwrite( ss.str(), image );
		}
	}

	cv::destroyWindow( "images" );

	std::cout << "Accepted " << accepted << " images." << std::endl;
}


/**
 * @file tests/decision_stump_test.cpp
 * @author Udit Saxena
 *
 * Tests for DecisionStump class.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */
#include <mlpack/core.hpp>
#include <mlpack/methods/decision_stump/decision_stump.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"

using namespace mlpack;
using namespace mlpack::decision_stump;
using namespace arma;
using namespace mlpack::distribution;

BOOST_AUTO_TEST_SUITE(DecisionStumpTest);

/**
 * This tests handles the case wherein only one class exists in the input
 * labels.  It checks whether the only class supplied was the only class
 * predicted.
 */
BOOST_AUTO_TEST_CASE(OneClass)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 6;

  mat trainingData;
  trainingData << 2.4 << 3.8 << 3.8 << endr
               << 1   << 1   << 2   << endr
               << 1.3 << 1.9 << 1.3 << endr;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 1 << 1 << 1;

  mat testingData;
  testingData << 2.4 << 2.5 << 2.6;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  for (size_t i = 0; i < predictedLabels.size(); i++)
    BOOST_CHECK_EQUAL(predictedLabels(i), 1);
}

/**
 * This tests whether the entropy is being correctly calculated by checking the
 * correct value of the splitting column value.  This test is for an
 * inpBucketSize of 4 and the correct value of the splitting dimension is 0.
 */
BOOST_AUTO_TEST_CASE(CorrectDimensionChosen)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 4;

  // This dataset comes from Chapter 6 of the book "Data Mining: Concepts,
  // Models, Methods, and Algorithms" (2nd Edition) by Mehmed Kantardzic.  It is
  // found on page 176 (and a description of the correct splitting dimension is
  // given below that).
  mat trainingData;
  trainingData << 0  << 0  << 0  << 0  << 0  << 1  << 1  << 1  << 1
               << 2  << 2  << 2  << 2  << 2  << endr
               << 70 << 90 << 85 << 95 << 70 << 90 << 78 << 65 << 75
               << 80 << 70 << 80 << 80 << 96 << endr
               << 1  << 1  << 0  << 0  << 0  << 1  << 0  << 1  << 0
               << 1  << 1  << 0  << 0  << 0  << endr;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 1 << 1 << 1 << 0 << 0 << 0 << 0
           << 0 << 1 << 1 << 0 << 0 << 0;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  // Only need to check the value of the splitting column, no need of
  // classification.
  BOOST_CHECK_EQUAL(ds.SplitDimension(), 0);
}

/**
 * This tests for the classification:
 *   if testinput < 0 - class 0
 *   if testinput > 0 - class 1
 * An almost perfect split on zero.
 */
BOOST_AUTO_TEST_CASE(PerfectSplitOnZero)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 2;

  mat trainingData;
  trainingData << -1 << 1 << -2 << 2 << -3 << 3;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 1 << 0 << 1 << 0 << 1;

  mat testingData;
  testingData << -4 << 7 << -7 << -5 << 6;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 4), 1);
}

/**
 * This tests the binning function for the case when a dataset with cardinality
 * of input < inpBucketSize is provided.
 */
BOOST_AUTO_TEST_CASE(BinningTesting)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 10;

  mat trainingData;
  trainingData << -1 << 1 << -2 << 2 << -3 << 3 << -4;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 1 << 0 << 1 << 0 << 1 << 0;

  mat testingData;
  testingData << 5;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
}

/**
 * This is a test for the case when non-overlapping, multiple classes are
 * provided. It tests for a perfect split due to the non-overlapping nature of
 * the input classes.
 */
BOOST_AUTO_TEST_CASE(PerfectMultiClassSplit)
{
  const size_t numClasses = 4;
  const size_t inpBucketSize = 3;

  mat trainingData;
  trainingData << -8 << -7 << -6 << -5 << -4 << -3 << -2 << -1
               << 0  << 1  << 2  << 3  << 4  << 5  << 6  << 7;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 0 << 0 << 0 << 1 << 1 << 1 << 1
           << 2 << 2 << 2 << 2 << 3 << 3 << 3 << 3;

  mat testingData;
  testingData << -6.1 << -2.1 << 1.1 << 5.1;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 2);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 3);
}

/**
 * This test is for the case when reasonably overlapping, multiple classes are
 * provided in the input label set. It tests whether classification takes place
 * with a reasonable amount of error due to the overlapping nature of input
 * classes.
 */
BOOST_AUTO_TEST_CASE(MultiClassSplit)
{
  const size_t numClasses = 3;
  const size_t inpBucketSize = 3;

  mat trainingData;
  trainingData << -7 << -6 << -5 << -4 << -3 << -2 << -1 << 0 << 1
               << 2  << 3  << 4  << 5  << 6  << 7  << 8  << 9 << 10;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 0 << 0 << 0 << 1 << 1 << 0 << 0
           << 1 << 1 << 1 << 2 << 1 << 2 << 2 << 2 << 2 << 2;

  mat testingData;
  testingData << -6.1 << -5.9 << -2.1 << -0.7 << 2.5 << 4.7 << 7.2 << 9.1;

  DecisionStump<> ds(trainingData, labelsIn.row(0), numClasses, inpBucketSize);

  Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 4), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 5), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 6), 2);
  BOOST_CHECK_EQUAL(predictedLabels(0, 7), 2);
}

/**
 * This tests that the decision stump can learn a good split on a dataset with
 * four dimensions that have progressing levels of separation.
 */
BOOST_AUTO_TEST_CASE(DimensionSelectionTest)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 2500;

  arma::mat dataset(4, 5000);

  // The most separable dimension.
  GaussianDistribution g1("-5", "1");
  GaussianDistribution g2("5", "1");

  for (size_t i = 0; i < 2500; ++i)
  {
    arma::vec tmp = g1.Random();
    dataset(1, i) = tmp[0];
  }
  for (size_t i = 2500; i < 5000; ++i)
  {
    arma::vec tmp = g2.Random();
    dataset(1, i) = tmp[0];
  }

  g1 = GaussianDistribution("-3", "1");
  g2 = GaussianDistribution("3", "1");

  for (size_t i = 0; i < 2500; ++i)
  {
    arma::vec tmp = g1.Random();
    dataset(3, i) = tmp[0];
  }
  for (size_t i = 2500; i < 5000; ++i)
  {
    arma::vec tmp = g2.Random();
    dataset(3, i) = tmp[0];
  }

  g1 = GaussianDistribution("-1", "1");
  g2 = GaussianDistribution("1", "1");

  for (size_t i = 0; i < 2500; ++i)
  {
    arma::vec tmp = g1.Random();
    dataset(0, i) = tmp[0];
  }
  for (size_t i = 2500; i < 5000; ++i)
  {
    arma::vec tmp = g2.Random();
    dataset(0, i) = tmp[0];
  }

  // Not separable at all.
  g1 = GaussianDistribution("0", "1");
  g2 = GaussianDistribution("0", "1");

  for (size_t i = 0; i < 2500; ++i)
  {
    arma::vec tmp = g1.Random();
    dataset(2, i) = tmp[0];
  }
  for (size_t i = 2500; i < 5000; ++i)
  {
    arma::vec tmp = g2.Random();
    dataset(2, i) = tmp[0];
  }

  // Generate the labels.
  arma::Row<size_t> labels(5000);
  for (size_t i = 0; i < 2500; ++i)
    labels[i] = 0;
  for (size_t i = 2500; i < 5000; ++i)
    labels[i] = 1;

  // Now create a decision stump.
  DecisionStump<> ds(dataset, labels, numClasses, inpBucketSize);

  // Make sure it split on the dimension that is most separable.
  BOOST_CHECK_EQUAL(ds.SplitDimension(), 1);

  // Make sure every bin below -1 classifies as label 0, and every bin above 1
  // classifies as label 1 (What happens in [-1, 1] isn't that big a deal.).
  for (size_t i = 0; i < ds.Split().n_elem; ++i)
  {
    if (ds.Split()[i] <= -3.0)
      BOOST_CHECK_EQUAL(ds.BinLabels()[i], 0);
    else if (ds.Split()[i] >= 3.0)
      BOOST_CHECK_EQUAL(ds.BinLabels()[i], 1);
  }
}

/**
 * Ensure that the default constructor works and that it classifies things as 0
 * always.
 */
BOOST_AUTO_TEST_CASE(EmptyConstructorTest)
{
  DecisionStump<> d;

  arma::mat data = arma::randu<arma::mat>(3, 10);
  arma::Row<size_t> labels;

  d.Classify(data, labels);

  for (size_t i = 0; i < 10; ++i)
    BOOST_REQUIRE_EQUAL(labels[i], 0);

  // Now train on another dataset and make sure something kind of makes sense.
  mat trainingData;
  trainingData << -7 << -6 << -5 << -4 << -3 << -2 << -1 << 0 << 1
               << 2  << 3  << 4  << 5  << 6  << 7  << 8  << 9 << 10;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 0 << 0 << 0 << 1 << 1 << 0 << 0
           << 1 << 1 << 1 << 2 << 1 << 2 << 2 << 2 << 2 << 2;

  mat testingData;
  testingData << -6.1 << -5.9 << -2.1 << -0.7 << 2.5 << 4.7 << 7.2 << 9.1;

  DecisionStump<> ds(trainingData, labelsIn.row(0), 4, 3);

  Row<size_t> predictedLabels(testingData.n_cols);
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 4), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 5), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 6), 2);
  BOOST_CHECK_EQUAL(predictedLabels(0, 7), 2);
}

/**
 * Ensure that a matrix holding ints can be trained.  The bigger issue here is
 * just compilation.
 */
BOOST_AUTO_TEST_CASE(IntTest)
{
  // Train on a dataset and make sure something kind of makes sense.
  imat trainingData;
  trainingData << -7 << -6 << -5 << -4 << -3 << -2 << -1 << 0 << 1
               << 2  << 3  << 4  << 5  << 6  << 7  << 8  << 9 << 10;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 0 << 0 << 0 << 1 << 1 << 0 << 0
           << 1 << 1 << 1 << 2 << 1 << 2 << 2 << 2 << 2 << 2;

  DecisionStump<arma::imat> ds(trainingData, labelsIn.row(0), 4, 3);

  imat testingData;
  testingData << -6 << -6 << -2 << -1 << 3 << 5 << 7 << 9;

  arma::Row<size_t> predictedLabels;
  ds.Classify(testingData, predictedLabels);

  BOOST_CHECK_EQUAL(predictedLabels(0, 0), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 1), 0);
  BOOST_CHECK_EQUAL(predictedLabels(0, 2), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 3), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 4), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 5), 1);
  BOOST_CHECK_EQUAL(predictedLabels(0, 6), 2);
  BOOST_CHECK_EQUAL(predictedLabels(0, 7), 2);
}

/**
 * Test that DecisionStump::Train() returns finite gain.
 */
BOOST_AUTO_TEST_CASE(DecisionStumpTrainReturnEntropy)
{
  const size_t numClasses = 2;
  const size_t inpBucketSize = 2;

  mat trainingData;
  trainingData << -1 << 1 << -2 << 2 << -3 << 3;

  // No need to normalize labels here.
  Mat<size_t> labelsIn;
  labelsIn << 0 << 1 << 0 << 1 << 0 << 1;

  arma::Row<double> weights = arma::ones<arma::Row<double>>(labelsIn.n_elem);

  // Train a simple decision stump without weights.
  DecisionStump<> ds;
  double gain = ds.Train(trainingData, labelsIn.row(0), numClasses,
      inpBucketSize);

  BOOST_REQUIRE_EQUAL(std::isfinite(gain), true);

  // Train decision stump with weights.
  DecisionStump<> wds;
  gain = wds.Train(trainingData, labelsIn.row(0), weights, numClasses,
      inpBucketSize);

  BOOST_REQUIRE_EQUAL(std::isfinite(gain), true);
}

BOOST_AUTO_TEST_SUITE_END();


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011-2012 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[box_view
//` Shows usage of the Boost.Range compatible view on a box

#include <iostream>

#include <boost/geometry.hpp>

int main()
{
    typedef boost::geometry::model::box
        <
            boost::geometry::model::point<double, 2, boost::geometry::cs::cartesian>
        > box_type;

    // Define the Boost.Range compatible type:
    typedef boost::geometry::box_view<box_type> box_view;

    box_type box;
    boost::geometry::assign_values(box, 0, 0, 4, 4);

    box_view view(box);

    // Iterating in clockwise direction over the points of this box
    for (boost::range_iterator<box_view const>::type it = boost::begin(view);
        it != boost::end(view); ++it)
    {
        std::cout << " " << boost::geometry::dsv(*it);
    }
    std::cout << std::endl;

    // Note that a box_view is tagged as a ring, so supports area etc.
    std::cout << "Area: " << boost::geometry::area(view) << std::endl;

    return 0;
}

//]

//[box_view_output
/*`
Output:
[pre
 (0, 0) (0, 4) (4, 4) (4, 0) (0, 0)
Area: 16
]
*/
//]


//
//  Copyright Toon Knapen, 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)
//

#include "random.hpp"

#include <boost/numeric/bindings/traits/transpose.hpp>
#include <boost/numeric/bindings/traits/ublas_vector.hpp>
#include <boost/numeric/bindings/traits/ublas_matrix.hpp>
#include <boost/numeric/bindings/traits/ublas_symmetric.hpp>
#include <boost/numeric/bindings/traits/ublas_hermitian.hpp>
#include <boost/numeric/bindings/blas/blas3.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/hermitian.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <iostream>
#include <complex>

// Randomize a matrix
template <typename M>
void randomize(M& m) {
   typedef typename M::size_type  size_type ;
   typedef typename M::value_type value_type ;

   size_type size1 = m.size1() ;
   size_type size2 = m.size2() ;

   for (size_type i=0; i<size2; ++i) {
      for (size_type j=0; j<size1; ++j) {
         m(j,i) = random_value< value_type >() ;
      }
   }
} // randomize()

template <typename T>
struct RealDiagonal {
   template <typename M>
   void operator() (M& m) const {}
};

template <typename T>
struct RealDiagonal< std::complex<T> > {
   template <typename M>
   void operator() (M& m) const {
      for (typename M::size_type i=0; i<m.size1(); ++i) {
         m(i,i) = m(i,i).real();
      }
   }
};

template <typename M>
boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::unit_upper> unit_upper_part(const M& m) {
   return boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::unit_upper> (m);
}

template <typename M>
boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::upper> upper_part(const M& m) {
   return boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::upper> (m);
}

template <typename M>
boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::lower> lower_part(const M& m) {
   return boost::numeric::ublas::triangular_adaptor<const M, boost::numeric::ublas::lower> (m);
}

// Run over all matrix types and aply a functor to it.
template <typename F>
int do_matrix_types(const F& f) {
   using namespace boost::numeric::ublas ;

   typedef typename F::value_type value_type ;

   {
      matrix<value_type,column_major> m( f.size1(), f.size2() );
      if (f(m)) return 255;
   }
   {
      typedef matrix<value_type,column_major> matrix_type ;
      matrix_type m( 2*f.size1(), 2*f.size2() );
      matrix_range<matrix_type> m_r( m, range(1,1+f.size1()), range(2,2+f.size2()) );
      if (f(m_r)) return 255 ;
   }

   return 0;
} // do_matrix_types()

// Functor to select syrk/herk calls given the two matrix_types
template <typename M1>
struct Syrk2 {
   typedef typename M1::value_type                                                         value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Syrk2(const M1& a,
        const ref_matrix_type& a_ref)
   : a_( a ), a_ref_( a_ref )
   , c_ref_( a_ref_.size1(), a_ref_.size1() )
   {
      randomize( c_ref_ );
      RealDiagonal<value_type>() (c_ref_); // Must have real diagonal for herk.
   }

   template <typename M>
   int operator() (M& c) const {
      using namespace boost::numeric::ublas ;

      real_type alpha = 2.0 ;
      real_type beta = -3.0 ;

      c.assign( c_ref_ );
      boost::numeric::bindings::blas::syrk( 'U', 'N', value_type(alpha), a_, value_type(beta), c ) ;
      if ( norm_frobenius( upper_part( c - (beta*c_ref_ + alpha * prod( a_ref_, trans( a_ref_ ) ) ) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(c) ) ) return 255;

      c.assign( c_ref_ );
      symmetric_adaptor<M, upper> c_s( c );
      boost::numeric::bindings::blas::syrk( 'U', 'N', value_type(alpha), a_, value_type(beta), c_s ) ;
      if ( norm_frobenius( upper_part( c_s - (beta*c_ref_ + alpha * prod( a_ref_, trans( a_ref_ ) ) ) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(c_s) ) ) return 255;

      c.assign( c_ref_ );
      boost::numeric::bindings::blas::herk( 'U', 'N', alpha, a_, beta, c ) ;
      if ( norm_frobenius( upper_part( c - (beta*c_ref_ + alpha * prod( a_ref_, herm( a_ref_ ) ) ) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(c) ) ) return 255;

      c.assign( c_ref_ );
      hermitian_adaptor<M, upper> c_h( c );
      boost::numeric::bindings::blas::herk( 'U', 'N', alpha, a_, beta, c_h ) ;
      if ( norm_frobenius( upper_part( c_h - (beta*c_ref_ + alpha * prod( a_ref_, herm( a_ref_ ) ) ) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(c_h) ) ) return 255;

      return 0;
   }

   size_type size1() const {return c_ref_.size1() ;}
   size_type size2() const {return c_ref_.size2() ;}

   const M1& a_ ;
   const ref_matrix_type& a_ref_;
   ref_matrix_type  c_ref_;
}; // Syrk2

template <typename T>
struct Syrk1 {
   typedef T                                                                               value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Syrk1()
   : a_ref_( 4, 7 )
   {
      randomize( a_ref_ );
   }

   template <typename M>
   int operator() (M& a) const {
      typedef Syrk2<M> functor_type ;
      a.assign( a_ref_ );
      return do_matrix_types( functor_type(a, a_ref_) );
   }

   size_type size1() const {return a_ref_.size1();}
   size_type size2() const {return a_ref_.size2();}

   ref_matrix_type a_ref_;
}; // Syrk1

// Functor to select gemm calls given the three matrix_types
template <typename M1, typename M2>
struct Gemm3 {
   typedef typename M1::value_type                                                         value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Gemm3(const M1& a, const M2& b,
        const ref_matrix_type& a_ref, const ref_matrix_type& b_ref)
   : a_( a ), b_( b )
   , a_ref_( a_ref ), b_ref_( b_ref ), c_ref_( a_ref_.size1(), b_ref_.size2() )
   {
      randomize( c_ref_ );
   }

   template <typename M>
   int operator() (M& c) const {
      c.assign( c_ref_ );

      value_type alpha = 2.0 ;
      value_type beta = -3.0 ;

      boost::numeric::bindings::blas::gemm( 'N', 'N', alpha, a_, b_, beta, c ) ;
      double safety_factor (1.5);
      if ( norm_frobenius( c - (beta*c_ref_ + alpha * prod( a_ref_, b_ref_ ) ) )
          > safety_factor*std::numeric_limits< real_type >::epsilon() * norm_frobenius( c ) ) return 255;

      return 0;
   }

   size_type size1() const {return c_ref_.size1() ;}
   size_type size2() const {return c_ref_.size2() ;}

   const M1& a_ ;
   const M2& b_ ;
   const ref_matrix_type& a_ref_, b_ref_;
   ref_matrix_type  c_ref_;
}; // Gemm3

template <typename M1>
struct Gemm2 {
   typedef typename M1::value_type                                                         value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Gemm2(const M1& a, const ref_matrix_type& a_ref)
   : a_( a )
   , a_ref_( a_ref )
   , b_ref_( a_ref.size2(), 7 )
   {
      randomize( b_ref_ );
   }

   template <typename M>
   int operator() (M& b) const {
      typedef Gemm3<M1,M> functor_type ;
      b.assign( b_ref_ );
      return do_matrix_types( functor_type(a_, b, a_ref_, b_ref_) );
   }

   size_type size1() const {return b_ref_.size1();}
   size_type size2() const {return b_ref_.size2();}

   const M1&              a_ ;
   const ref_matrix_type& a_ref_;
   ref_matrix_type        b_ref_;
}; // Gemm2

template <typename T>
struct Gemm1 {
   typedef T                                                                               value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Gemm1()
   : a_ref_( 4, 7 )
   {
      randomize( a_ref_ );
   }

   template <typename M>
   int operator() (M& a) const {
      typedef Gemm2<M> functor_type ;
      a.assign( a_ref_ );
      return do_matrix_types( functor_type(a, a_ref_) );
   }

   size_type size1() const {return a_ref_.size1();}
   size_type size2() const {return a_ref_.size2();}

   ref_matrix_type a_ref_;
}; // Gemm1

// Functor to select syrk/herk calls given the two matrix_types
template <typename M1>
struct Trsm2 {
   typedef typename M1::value_type                                                         value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Trsm2(M1& c,
        const ref_matrix_type& b_ref, const ref_matrix_type& c_ref )
   : c_( c ), c_ref_( c_ref ), b_ref_( b_ref )
   , a_ref_( b_ref_.size1(), c_ref_.size2() )
   {
      randomize( a_ref_ ) ;
   }

   template <typename M>
   int operator() (M& a) const {
      using namespace boost::numeric::ublas ;

      real_type alpha = 2.0 ;

      a.assign( a_ref_ );
      boost::numeric::bindings::blas::trsm( 'L', 'U', 'N', 'N', value_type(alpha), b_ref_, a ) ;
      if ( norm_frobenius( alpha * a_ref_ - prod( upper_part(b_ref_), a ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(b_ref_) ) * norm_frobenius( a) ) return 255;

      a.assign( a_ref_ );
      boost::numeric::bindings::blas::trsm( 'R', 'U', 'N', 'N', value_type(alpha), c_, a ) ;
      if ( norm_frobenius( alpha * a_ref_ - prod( a, upper_part(c_) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(c_) ) * norm_frobenius( a ) ) return 255;

      a.assign( a_ref_ );
      boost::numeric::bindings::blas::trsm( 'R', 'L', 'N', 'N', value_type(alpha), c_, a ) ;
      if ( norm_frobenius( alpha * a_ref_ - prod( a, lower_part(c_) ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( lower_part(c_) ) * norm_frobenius( a ) ) return 255;

      a.assign( a_ref_ );
      boost::numeric::bindings::blas::trsm( 'L', 'U', 'T', 'N', value_type(alpha), b_ref_, a ) ;
      if ( norm_frobenius( alpha * a_ref_ - prod( trans(upper_part(b_ref_)), a ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( upper_part(b_ref_) ) * norm_frobenius( a) ) return 255;

      a.assign( a_ref_ );
      boost::numeric::bindings::blas::trsm( 'L', 'U', 'N', 'U', value_type(alpha), b_ref_, a ) ;
      if ( norm_frobenius( alpha * a_ref_ - prod( unit_upper_part(b_ref_), a ) )
          > std::numeric_limits< real_type >::epsilon() * norm_frobenius( unit_upper_part(b_ref_) ) * norm_frobenius( a) ) return 255;

      return 0;
   }

   size_type size1() const {return a_ref_.size1() ;}
   size_type size2() const {return a_ref_.size2() ;}

   const M1& c_ ;
   const ref_matrix_type& c_ref_;
   const ref_matrix_type& b_ref_;
   ref_matrix_type        a_ref_;
} ; // Trsm2

template <typename T>
struct Trsm1 {
   typedef T                                                                               value_type ;
   typedef boost::numeric::ublas::matrix<value_type, boost::numeric::ublas::column_major>  ref_matrix_type ;
   typedef typename boost::numeric::bindings::traits::type_traits< value_type >::real_type real_type ;
   typedef typename ref_matrix_type::size_type                                             size_type ;

   Trsm1()
   : b_( 4, 4 )
   , c_( 7, 7 )
   {
      randomize( b_ );
      randomize( c_ );
      for ( std::size_t i=0; i<b_.size1(); ++i ) if ( b_(i,i)==value_type(0.0) ) b_(i,i) = value_type(1.0) ;
      for ( std::size_t i=0; i<c_.size1(); ++i ) if ( c_(i,i)==value_type(0.0) ) c_(i,i) = value_type(1.0) ;
   }

   template <typename M>
   int operator() (M& c) const {
      typedef Trsm2<M> functor_type ;
      return do_matrix_types( functor_type(c, b_, c_) );
   }

   size_type size1() const {return c_.size1();}
   size_type size2() const {return c_.size2();}

   ref_matrix_type b_;
   ref_matrix_type c_;
} ; // Trsm1

template <typename T>
int do_value_type() {
   // Gemm test
   std::cout << "  gemm\n";
   if (do_matrix_types( Gemm1<T>() )) return 255 ;

   // Syrk and herk test
   std::cout << "  herk\n";
   if (do_matrix_types( Syrk1<T>() )) return 255 ;

   // Trsm test
   std::cout << "  trsm\n";
   if (do_matrix_types( Trsm1<T>() )) return 255 ;

   return 0 ;
} // do_value_type()

int main() {
   // Test for various value_types
   std::cout << "float\n";
   if (do_value_type<float>()) return 255 ;
   std::cout << "double\n";
   if (do_value_type<double>()) return 255 ;
   std::cout << "complex<float>\n";
   if (do_value_type<std::complex<float> >()) return 255 ;
   std::cout << "complex<double>\n";
   if (do_value_type<std::complex<double> >()) return 255 ;

   std::cout << "Regression test succeeded\n" ;

   return 0 ;
}


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#include <iostream>
#include <cmath>
#include <complex>
#include <vector>

#include <boost/numeric/mtl/mtl.hpp>

const unsigned sz= 5;

inline float f(float x) { return x; }
inline double f(double x) { return x; }

inline std::complex<double> f(std::complex<double> x)
{
    return std::complex<double>(real(x), real(x)+1.0);
}

template <typename Vector>
void test(Vector& v, const char* name)
{
    using std::abs; using std::cout;
    using mtl::irange; using mtl::imax;

    typedef typename mtl::Collection<Vector>::value_type T;
    std::cout << "\n" << name << "\n";

    for (unsigned i= 0; i < size(v); ++i)
	v[i]= f(T(i));

    Vector w(v[irange(2, 4)]);
    w[1]= f(T(8));
    cout << "w == " << w << "\n";

    MTL_THROW_IF(w[0] != f(T(2)), mtl::runtime_error("Wrong value in w"));
    MTL_THROW_IF(size(w) != 2, mtl::runtime_error("Wrong size of w"));

    MTL_THROW_IF(v[3] != f(T(8)), mtl::runtime_error("Cannot change v via w (correctly)"));

    Vector u( v[irange(2, imax)] );
    cout << "u == " << u << "\n";
    cout << "v == " << v << "\n";
    irange r(2 , 4);

    MTL_THROW_IF(u[0] != f(T(2)), mtl::runtime_error("Wrong value in u"));
    MTL_THROW_IF(size(u) != sz-2, mtl::runtime_error("Wrong size of u"));

    cout << "v[irange(2, 4)] == " << v[r] << "\n";
    --r ;
    cout << "v[irange(2, 3)] == " << v[r] << "\n";

#if 0 // only for column vectors
    T beta= trans(v[irange(2, 4)]) * w;
    cout << "trans(v) * w = " << beta << "\n";
#endif
}

int main(int, char**)
{
    using namespace mtl;
    dense_vector<float>                                                 cf(sz, 1.0);
    dense_vector<double>                                                cd(sz, 1.0);
    dense_vector<std::complex<double> >                                 cc(sz, 1.0);
    dense_vector<float, mtl::vec::parameters<row_major> >               rf(sz, 1.0);

    test(cf, "dense_vector<float>");
    test(cd, "dense_vector<double>");
    test(cc, "dense_vector<std::complex<double> >");
    test(rf, "dense_vector<float, parameters<row_major> >");

    return 0;
}