/*ckwg +29
 * Copyright 2013-2015 by Kitware, 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 name of Kitware, Inc. nor the names of any 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 AUTHORS 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.
 */

/**
 * \file
 * \brief Implementation of \link kwiver::vital::camera_intrinsics_
 *        camera_intrinsics_<T> \endlink class
 *        for \c T = { \c float, \c double }
 */

#include <vital/types/camera_intrinsics.h>
#include <vital/io/eigen_io.h>
#include <vital/math_constants.h>
#include <Eigen/Dense>

#include <cmath>
#include <iomanip>

namespace kwiver {
namespace vital {

/// Convert to a 3x3 calibration matrix
matrix_3x3d
camera_intrinsics
::as_matrix() const
{
  matrix_3x3d K;
  const double f = this->focal_length();
  const vector_2d pp = this->principal_point();
  K << f, this->skew(), pp.x(),
    0, f / this->aspect_ratio(), pp.y(),
    0, 0, 1;
  return K;
}

/// Map normalized image coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_2d& point ) const
{
  // apply radial and tangential distortion if coefficients are provided
  const vector_2d pt = this->distort( point );
  const vector_2d pp = this->principal_point();
  const double f = this->focal_length();

  return vector_2d( pt.x() * f + pt.y() * this->skew() + pp.x(),
                    pt.y() * f / this->aspect_ratio() + pp.y() );
}

/// Map a 3D point in camera coordinates into actual image coordinates
vector_2d
camera_intrinsics
::map( const vector_3d& norm_hpt ) const
{
  return this->map( vector_2d( norm_hpt[0] / norm_hpt[2],
                               norm_hpt[1] / norm_hpt[2] ) );
}

/// Unmap actual image coordinates back into normalized image coordinates
vector_2d
camera_intrinsics
::unmap( const vector_2d& pt ) const
{
  const double f = this->focal_length();
  const vector_2d p0 = pt - this->principal_point();
  const double y = p0.y() * this->aspect_ratio() / f;
  const double x = ( p0.x() - y * this->skew() ) / f;

  return this->undistort( vector_2d( x, y ) );
}

/// Check if a 3D point in camera coordinates can map into image coordinates
bool
camera_intrinsics
::is_map_valid(const vector_3d& norm_hpt) const
{
  return this->is_map_valid(vector_2d(norm_hpt[0] / norm_hpt[2],
                                      norm_hpt[1] / norm_hpt[2]));
}

namespace // anonymous namespace
{

/// Compute the radial distortion scaling
/** Distortion scaling is a function of the squared radius \p r2
 *  and the distortion parameters \p d
 */
template < typename T >
T
radial_distortion_scale( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T scale = T( 1 );

  if ( d.rows() > 0 )
  {
    scale += r2 * d[0];
    if ( d.rows() > 1 )
    {
      const T r4 = r2 * r2;
      scale += r4 * d[1];
      if ( d.rows() > 4 )
      {
        const T r6 = r2 * r4;
        scale += r6 * d[4];
        if ( d.rows() > 7 )
        {
          scale /= T( 1 ) + r2 * d[5] + r4 * d[6] + r6 * d[7];
        }
      }
    }
  }
  return scale;
}

/// Compute radial distortion as a scaling and offset
/** For a point \p pt and distortion coefficients \p d compute
 *  a scale and offset such that distortion can be applied as
 *  \code
 *    distorted_pt = pt * scale + offset;
 *  \endcode
 */
template < typename T >
void
distortion_scale_offset( const Eigen::Matrix< T, 2, 1 >& pt,
                         const Eigen::VectorXd& d,
                         T& scale, Eigen::Matrix< T, 2, 1 >& offset )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T r2 = x2 + y2;

  scale = radial_distortion_scale( r2, d );
  offset = Eigen::Matrix< T, 2, 1 > ( T( 0 ), T( 0 ) );
  if ( d.rows() > 3 )
  {
    const T two_xy = 2 * pt.x() * pt.y();
    offset = Eigen::Matrix< T, 2, 1 > ( d[2] * two_xy + d[3] * ( r2 + 2 * x2 ),
                                        d[3] * two_xy + d[2] * ( r2 + 2 * y2 ) );
  }
}

/// Compute the derivative of the radial distortion as a function of \p r2
template < typename T >
T
radial_distortion_deriv( const T                r2,
                         const Eigen::VectorXd&  d )
{
  T deriv = T( 0 );

  if ( d.rows() > 0 )
  {
    deriv += d[0];
    if ( d.rows() > 1 )
    {
      deriv += 2 * d[1] * r2;
      if ( d.rows() > 4 )
      {
        const T r4 = r2 * r2;
        deriv += 3 * d[4] * r4;
        if ( d.rows() > 7 )
        {
          const T r6 = r4 * r2;
          const T a1 = T( 1 ) / ( d[5] * r2 + d[6] * r4 + d[7] * r6 + T( 1 ) );
          const T a2 = d[5] + 2 * d[6] * r2 + 3 * d[7] * r4;
          deriv -= a2 * a1 * ( d[0] * r2 + d[1] * r4 + d[4] * r6 + T( 1 ) );
          deriv *= a1;
        }
      }
    }
  }
  return deriv;
}

/// Compute the Jacobian of the distortion at a point
template < typename T >
Eigen::Matrix< T, 2, 2 >
distortion_jacobian( const Eigen::Matrix< T, 2, 1 >& pt,
                     const Eigen::VectorXd& d )
{
  const T x2 = pt.x() * pt.x();
  const T y2 = pt.y() * pt.y();
  const T xy = pt.x() * pt.y();
  const T r2 = x2 + y2;
  const T d_scale = 2 * radial_distortion_deriv( r2, d );
  const T scale = radial_distortion_scale( r2, d );
  Eigen::Matrix< T, 2, 2 > J;

  J << d_scale * x2 + scale, d_scale * xy,
    d_scale * xy, d_scale * y2 + scale;
  // add tangential distortion jacobian
  if ( d.rows() > 3 )
  {
    const T axy = 2 * ( d[2] * pt.x() + d[3] * pt.y() );
    const T ay = 2 * d[2] * pt.y();
    const T ax = 2 * d[3] * pt.x();
    J( 0, 0 ) += ay + 3 * ax;
    J( 0, 1 ) += axy;
    J( 1, 0 ) += axy;
    J( 0, 0 ) += 3 * ay + ax;
  }
  return J;
}

} // end anonymous namespace

/// Constructor - from a calibration matrix
simple_camera_intrinsics
::simple_camera_intrinsics( const matrix_3x3d& K,
                            const vector_t& d )
  : focal_length_( K( 0, 0 ) ),
  principal_point_( K( 0, 2 ), K( 1, 2 ) ),
  aspect_ratio_( K( 0, 0 ) / K( 1, 1 ) ),
  skew_( K( 0, 1 ) ),
  dist_coeffs_( d ),
  max_distort_radius_sq_(compute_max_distort_radius_sq())
{
}

/// Map normalized image coordinates into distorted coordinates
vector_2d
simple_camera_intrinsics
::distort( const vector_2d& norm_pt ) const
{
  double scale;
  vector_2d offset;

  distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
  return scale * norm_pt + offset;
}

/// Unnap distorted normalized coordinates into normalized coordinates
vector_2d
simple_camera_intrinsics
::undistort( const vector_2d& dist_pt ) const
{
  double scale;
  vector_2d offset, residual;
  vector_2d norm_pt = dist_pt;

  // iteratively solve for the undistorted point
  for ( unsigned int i = 0; i < 5; ++i )
  {
    distortion_scale_offset( norm_pt, dist_coeffs_, scale, offset );
    // This is a Gauss-Newton update
    // an alternative is a fixed point iteration as used by OpenCV:
    //   norm_pt = (dist_pt - offset) / scale;
    // Gauss-Newton seems to have faster convergence
    matrix_2x2d J = distortion_jacobian( norm_pt, dist_coeffs_ );
    residual = norm_pt * scale + offset - dist_pt;
    // check the maximum absolution residual to test convergence
    if ( residual.cwiseAbs().maxCoeff() < 1e-12 )
    {
      break;
    }
    norm_pt -= J.ldlt().solve( residual );
  }
  return norm_pt;
}

/// Check if a normalized image coordinate can map into image coordinates
bool
simple_camera_intrinsics
::is_map_valid(const vector_2d& norm_pt) const
{
  return norm_pt.squaredNorm() < this->max_distort_radius_sq_;
}

/// Compute the maximum distortion radius from dist_coeffs_
double
simple_camera_intrinsics
::compute_max_distort_radius_sq() const
{
  double a = 0.0;
  double b = 0.0;
  double c = 0.0;
  if (dist_coeffs_.rows() > 0)
  {
    a = dist_coeffs_[0];
    if (dist_coeffs_.rows() > 1)
    {
      b = dist_coeffs_[1];
      if (dist_coeffs_.rows() > 4)
      {
        c = dist_coeffs_[4];
        if (dist_coeffs_.rows() > 5)
        {
          // the rational polynomial case is not yet handled
          // TODO: implement something or throw a warning
        }
      }
    }
  }
  return max_distort_radius_sq(a, b, c);
}

/// Compute the maximum radius for radial distortion given coefficients
double
simple_camera_intrinsics
::max_distort_radius_sq(double a, double b, double c)
{
  constexpr double inf = std::numeric_limits<double>::infinity();
  using kwiver::vital::pi;
  // this function finds the smallest positive root of
  //   (1 + a r^2 + b r^4 + c r^6) r
  // the derivative with respect to r is
  //   1 + 3 a r^2 + 5 b r^4 + 7 c r^6
  // substituting x = r^2 we can solve this cubic polynomial
  //   1 + 3 a x + 5 b x^2 + 7 c r^3
  // taking the square root of the smallest positive x gives r

  // fold the constants from the derivative into a, b, and c
  a *= 3;
  b *= 5;
  c *= 7;

  // if all non-negative there is no root
  if (a >= 0.0 && b >= 0.0 && c >= 0.0)
  {
    return inf;
  }
  // general solution for non-zero cubic term
  if (c != 0.0)
  {
    // an array for the three possible solutions
    double solns[3] = { inf, inf, inf };
    // precompute commonly used terms (boc is "b over c")
    double boc = b / c;
    double boc2 = boc * boc;
    double t1 = (9 * a * boc - 2 * b * boc2 - 27) / c;
    double t2 = 3 * a / c - boc2;
    double discrim = t1 * t1 + 4 * t2 * t2 * t2;
    if (discrim > 0.0)
    {
      discrim = std::cbrt((std::sqrt(discrim) + t1) / 2.0);
      solns[0] = (discrim - (t2 / discrim) - boc) / 3;
    }
    else
    {
      double theta = (std::atan2(std::sqrt(-discrim), t1)) / 3;
      constexpr double twothirdpi = 2.0 * pi / 3.0;
      // by construction, if discrim < 0 then t2 < 0, so the sqrt is safe
      double t3 = 2 * std::sqrt(-t2);
      solns[0] = (t3 * std::cos(theta) - boc) / 3;
      solns[1] = (t3 * std::cos(theta + twothirdpi) - boc) / 3;
      solns[2] = (t3 * std::cos(theta - twothirdpi) - boc) / 3;
    }
    // find the minimum positive solution
    double min_soln = inf;
    for (auto const& s : solns)
    {
      if (s > 0.0 && s < min_soln)
      {
        min_soln = s;
      }
    }
    return min_soln;
  }
  // simplified solution for the quadratic case
  else if (b != 0.0)
  {
    double discrim = a * a - 4 * b;
    // if less than zero both solutions are not real
    if (discrim >= 0.0)
    {
      // we only need the smaller of the two possible solutions
      // solutions must be either both positive or both negative
      discrim = std::sqrt(discrim) - a;
      // if less than zero both solutions are negative
      if (discrim > 0.0)
      {
        return 2.0 / discrim;
      }
    }
  }
  // simple linear case for b = c = 0
  else if (a < 0.0)
  {
    return 1.0 / -a;
  }
  return inf;
}

/// output stream operator for a base class camera_intrinsics
std::ostream&
operator<<( std::ostream& s, const camera_intrinsics& k )
{
  using std::setprecision;
  std::vector<double> d = k.dist_coeffs();
  // if no distortion coefficients, create a zero entry as a place holder
  if ( d.empty() )
  {
    d.push_back(0.0);
  }
  s << setprecision( 12 ) << k.as_matrix() << "\n\n";
  for(unsigned i=0; i<d.size(); ++i)
  {
    s << setprecision( 12 ) << d[i] << " ";
  }
  s << "\n";

  return s;
}

/// input stream operator for a camera intrinsics
std::istream&
operator>>( std::istream& s, simple_camera_intrinsics& k )
{
  matrix_3x3d K;
  Eigen::VectorXd d;

  s >> K >> d;
  // a single 0 in d is used as a place holder,
  // if a single 0 was loaded then clear d
  if ( ( d.rows() == 1 ) && ( d[0] == 0.0 ) )
  {
    d.resize( 0 );
  }
  k = simple_camera_intrinsics( K, d );
  return s;
}

} } // end namespace


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

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>
namespace nt2
{
  namespace tag
  {
   /*!
     @brief Pio_2lo generic tag

     Represents the Pio_2lo constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( Pio_2lo, double
                                , 0, 0xb33bbd2eUL
                                , 0x3c91a62633145c07ULL
                                )
  }
  namespace ext
  {
   template<class Site, class... Ts>
   BOOST_FORCEINLINE generic_dispatcher<tag::Pio_2lo, Site> dispatching_Pio_2lo(adl_helper, boost::dispatch::meta::unknown_<Site>, boost::dispatch::meta::unknown_<Ts>...)
   {
     return generic_dispatcher<tag::Pio_2lo, Site>();
   }
   template<class... Args>
   struct impl_Pio_2lo;
  }
  /*!
    This constant is such that, for pairs of types (T, Tup)
    (namely (float,  double) and (double, long double)) the sum:

    abs(Tup(Pio_2lo<T>())+Tup(Pio_2<T>())-Pio_2<Tup>()) is  lesser than
    a few Eps<Tup>().

    This is used to improve accurracy when computing sums of the kind
    \f$\pi/2 + x\f$ with x small,  by replacing them by
    Pio_2 + (Pio_2lo + x)

    @par Semantic:

    For type T0:

    @code
    T r = Pio_2lo<T>();
    @endcode

    is similar to:

    @code
    if T is double
      r = 6.123233995736766e-17
    else if T is float
      r = -4.3711388e-08
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::Pio_2lo, Pio_2lo);
}

#endif


#ifndef STAN_MATH_PRIM_PROB_SCALED_INV_CHI_SQUARE_RNG_HPP
#define STAN_MATH_PRIM_PROB_SCALED_INV_CHI_SQUARE_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/max_size.hpp>
#include <boost/random/chi_squared_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a scaled chi square random variate for the given
 * number of degrees of freedom and scale using the specified random
 * number generator.
 *
 * nu and sigma can each be a scalar or a one-dimensional container. Any
 * non-scalar inputs must be the same size.
 *
 * @tparam T_deg type of degrees of freedom parameter
 * @tparam T_scale type of scale parameter
 * @tparam RNG type of random number generator
 * @param nu (Sequence of) positive degrees of freedom parameter(s)
 * @param s (Sequence of) positive scale parameter(s)
 * @param rng random number generator
 * @return (Sequence of) scaled chi square random variate(s)
 * @throw std::domain_error if nu or sigma are nonpositive
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_deg, typename T_scale, class RNG>
inline typename VectorBuilder<true, double, T_deg, T_scale>::type
scaled_inv_chi_square_rng(const T_deg& nu, const T_scale& s, RNG& rng) {
  using boost::variate_generator;
  using boost::random::chi_squared_distribution;
  static const char* function = "scaled_inv_chi_square_rng";
  check_positive_finite(function, "Degrees of freedom parameter", nu);
  check_positive_finite(function, "Scale parameter", s);
  check_consistent_sizes(function, "Location parameter", nu, "Scale Parameter",
                         s);

  scalar_seq_view<T_deg> nu_vec(nu);
  scalar_seq_view<T_scale> s_vec(s);
  size_t N = max_size(nu, s);
  VectorBuilder<true, double, T_deg, T_scale> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, chi_squared_distribution<> > chi_square_rng(
        rng, chi_squared_distribution<>(nu_vec[n]));
    output[n] = nu_vec[n] * s_vec[n] * s_vec[n] / chi_square_rng();
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


// -----------------------------------------------------------------------------
// Copyright (c) 2015-2018 Benjamin Buch
//
// https://github.com/bebuch/mitrax
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt)
// -----------------------------------------------------------------------------
#define BOOST_TEST_MODULE mitrax make_matrix
#include <boost/test/unit_test.hpp>
#include <boost/mpl/list.hpp>

#include <mitrax/matrix/view.hpp>

#include <array>
#include <vector>
#include <complex>
#include <iostream>

#define BT BOOST_TEST

using namespace mitrax;
using namespace mitrax::literals;

template < typename T >
auto rt_id(T&& v){
	return boost::typeindex::type_id_runtime(static_cast< T&& >(v));
}

template < typename T >
auto const id = boost::typeindex::type_id< T >();

template < typename Matrix, size_t RefC, size_t RefR >
constexpr bool check_fn(
	Matrix& m,
	value_type_t< Matrix > const(&ref)[RefR][RefC]
){
	using value_type = value_type_t< Matrix >;

	bool res = true;
	res &= m.cols() == cols< col_t(RefC) >();
	res &= m.rows() == rows< row_t(RefR) >();

	for(size_t y = 0; y < RefR; ++y){
		for(size_t x = 0; x < RefC; ++x){
			res &= m(c_t(x), r_t(y)) == ref[y][x];
		}
	}

	if constexpr(Matrix::ct_cols == 1_C || Matrix::ct_rows == 1_R){
		auto vec_ref =
			reinterpret_cast< value_type const(&)[RefC * RefR] >(ref);

		for(size_t i = 0; i < RefC * RefR; ++i){
			res &= m[d_t(i)] == vec_ref[i];
		}
	}

	if constexpr(Matrix::ct_cols == 1_C && Matrix::ct_rows == 1_R){
		res &= static_cast< value_type >(m) == ref[0][0];
	}

	return res;
}

template < typename M, col_t C, row_t R, size_t RefC, size_t RefR >
constexpr bool check(
	matrix< M, C, R > const& m,
	value_type_t< M > const(&ref)[RefR][RefC]
){
	return check_fn(m, ref);
}

template < typename M, col_t C, row_t R, size_t RefC, size_t RefR >
constexpr bool check(
	matrix< M, C, R >& m,
	value_type_t< M > const(&ref)[RefR][RefC]
){
	auto const& m_const = m;
	return check_fn(m, ref) && check(m_const, ref);
}

template < typename T >
struct fn_xy_t{
	template < typename T1, typename T2 >
	constexpr T operator()(T1 x, T2 y)const noexcept{
		return size_t(x) + size_t(y) * 10;
	}
};

template < typename T >
struct fn_x_t{
	template < typename T1 >
	constexpr T operator()(T1 i)const noexcept{
		return size_t(i) * 10;
	}
};

template < typename T >
struct fn_y_t{
	template < typename T1 >
	constexpr T operator()(T1 i)const noexcept{
		return size_t(i);
	}
};

template < typename T >
struct fn_i_t{
	template < typename T1 >
	constexpr T operator()(T1 i)const noexcept{
		return size_t(i) + size_t(i) * 10;
	}
};

BOOST_AUTO_TEST_SUITE(suite_make_matrix)

using types = boost::mpl::list< int, double, std::complex< float > >;

using objects = boost::mpl::list< int[4], std::array< int, 4 >, std::vector< int > >;

BOOST_AUTO_TEST_CASE_TEMPLATE(test_view_matrix_iter, Object, objects){
	using type = detail::view_matrix_impl< Object, true, 3_C, 3_R >;
	static_assert(has_iterator_fn_v< type >);
	static_assert(has_data_v< int*, type >);
	static_assert(has_iterator_fn_v< type const >);
	static_assert(has_data_v< int const*, type const >);

	using const_type = detail::const_view_matrix_impl< Object, true, 3_C, 3_R >;
	static_assert(has_iterator_fn_v< const_type >);
	static_assert(has_data_v< int const*, const_type >);
	static_assert(has_iterator_fn_v< const_type const >);
	static_assert(has_data_v< int const*, const_type const >);

	using non_const_t_only_const_data = detail::view_matrix_impl<
		detail::const_view_matrix_impl< Object, true, 3_C, 3_R >,
		true, 3_C, 3_R >;
	static_assert(has_iterator_fn_v< non_const_t_only_const_data >);
	static_assert(has_data_v< int const*, non_const_t_only_const_data >);
	static_assert(has_iterator_fn_v< non_const_t_only_const_data const >);
	static_assert(has_data_v< int const*, non_const_t_only_const_data const >);
}

BOOST_AUTO_TEST_CASE(test_view_matrix_iter2){
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_view_matrix_types, Object, objects){
	using detail::view_matrix_impl;
	using detail::const_view_matrix_impl;

	BT((
		id< view_matrix< Object, true, 1_C, 4_R > > ==
		id< matrix< view_matrix_impl< Object, true, 1_C, 4_R >, 1_C, 4_R > >
	));

	BT((
		id< view_bitmap< Object, true > > ==
		id< bitmap< view_matrix_impl< Object, true, 0_C, 0_R > > >
	));

	BT((
		id< view_col_vector< Object, true, 4_R > > ==
		id< col_vector< view_matrix_impl< Object, true, 1_C, 4_R >, 4_R > >
	));

	BT((
		id< view_col_vector< Object, true, 4_R > > ==
		id< matrix< view_matrix_impl< Object, true, 1_C, 4_R >, 1_C, 4_R > >
	));

	BT((
		id< view_row_vector< Object, true, 4_C > > ==
		id< row_vector< view_matrix_impl< Object, true, 4_C, 1_R >, 4_C > >
	));

	BT((
		id< view_row_vector< Object, true, 4_C > > ==
		id< matrix< view_matrix_impl< Object, true, 4_C, 1_R >, 4_C, 1_R > >
	));

	BT((
		id< view_square_matrix< Object, true, 2_D > > ==
		id< square_matrix< view_matrix_impl< Object, true, 2_C, 2_R >, 2_D > >
	));

	BT((
		id< view_square_matrix< Object, true, 2_D > > ==
		id< matrix< view_matrix_impl< Object, true, 2_C, 2_R >, 2_C, 2_R > >
	));

	BT((
		id< const_view_matrix< Object, true, 1_C, 4_R > > ==
		id< matrix< const_view_matrix_impl< Object, true, 1_C, 4_R >,
			1_C, 4_R > >
	));

	BT((
		id< const_view_bitmap< Object, true > > ==
		id< bitmap< const_view_matrix_impl< Object, true, 0_C, 0_R > > >
	));

	BT((
		id< const_view_col_vector< Object, true, 4_R > > ==
		id< col_vector< const_view_matrix_impl< Object, true, 1_C, 4_R >,
			4_R > >
	));

	BT((
		id< const_view_col_vector< Object, true, 4_R > > ==
		id< matrix< const_view_matrix_impl< Object, true, 1_C, 4_R >,
			1_C, 4_R > >
	));

	BT((
		id< const_view_row_vector< Object, true, 4_C > > ==
		id< row_vector< const_view_matrix_impl< Object, true, 4_C, 1_R >,
			4_C > >
	));

	BT((
		id< const_view_row_vector< Object, true, 4_C > > ==
		id< matrix< const_view_matrix_impl< Object, true, 4_C, 1_R >,
			4_C, 1_R > >
	));

	BT((
		id< const_view_square_matrix< Object, true, 2_D > > ==
		id< square_matrix< const_view_matrix_impl< Object, true, 2_C, 2_R >,
			2_D > >
	));

	BT((
		id< const_view_square_matrix< Object, true, 2_D > > ==
		id< matrix< const_view_matrix_impl< Object, true, 2_C, 2_R >,
			2_C, 2_R > >
	));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_view_matrix_3x3, T, types){
	T ref_i[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
	T init_ca[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
	std::array< T, 9 > init_a{{0, 1, 2, 3, 4, 5, 6, 7, 8}};
	std::vector< T > init_v{0, 1, 2, 3, 4, 5, 6, 7, 8};

	auto m01 = make_view_matrix(3_CS, 3_RS, init_ca);
	auto m02 = make_view_matrix(3_CD, 3_RS, init_ca);
	auto m03 = make_view_matrix(3_CS, 3_RD, init_ca);
	auto m04 = make_view_matrix(3_CD, 3_RD, init_ca);
	auto m05 = make_view_matrix(dim_pair(3_CS, 3_RS), init_ca);
	auto m06 = make_view_matrix(dim_pair(3_CD, 3_RS), init_ca);
	auto m07 = make_view_matrix(dim_pair(3_CS, 3_RD), init_ca);
	auto m08 = make_view_matrix(dim_pair(3_CD, 3_RD), init_ca);
	auto m09 = make_view_matrix(3_DS, init_ca);
	auto m10 = make_view_matrix(3_DD, init_ca);
	auto m11 = make_view_matrix(dim_pair(3_DS), init_ca);
	auto m12 = make_view_matrix(dim_pair(3_DD), init_ca);

	auto m13 = make_view_matrix(3_CS, 3_RS, init_a);
	auto m14 = make_view_matrix(3_CD, 3_RS, init_a);
	auto m15 = make_view_matrix(3_CS, 3_RD, init_a);
	auto m16 = make_view_matrix(3_CD, 3_RD, init_a);
	auto m17 = make_view_matrix(dim_pair(3_CS, 3_RS), init_a);
	auto m18 = make_view_matrix(dim_pair(3_CD, 3_RS), init_a);
	auto m19 = make_view_matrix(dim_pair(3_CS, 3_RD), init_a);
	auto m20 = make_view_matrix(dim_pair(3_CD, 3_RD), init_a);
	auto m21 = make_view_matrix(3_DS, init_a);
	auto m22 = make_view_matrix(3_DD, init_a);
	auto m23 = make_view_matrix(dim_pair(3_DS), init_a);
	auto m24 = make_view_matrix(dim_pair(3_DD), init_a);

	auto m25 = make_view_matrix(3_CS, 3_RS, init_v);
	auto m26 = make_view_matrix(3_CD, 3_RS, init_v);
	auto m27 = make_view_matrix(3_CS, 3_RD, init_v);
	auto m28 = make_view_matrix(3_CD, 3_RD, init_v);
	auto m29 = make_view_matrix(dim_pair(3_CS, 3_RS), init_v);
	auto m30 = make_view_matrix(dim_pair(3_CD, 3_RS), init_v);
	auto m31 = make_view_matrix(dim_pair(3_CS, 3_RD), init_v);
	auto m32 = make_view_matrix(dim_pair(3_CD, 3_RD), init_v);
	auto m33 = make_view_matrix(3_DS, init_v);
	auto m34 = make_view_matrix(3_DD, init_v);
	auto m35 = make_view_matrix(dim_pair(3_DS), init_v);
	auto m36 = make_view_matrix(dim_pair(3_DD), init_v);

	BT((rt_id(m01) == id< view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m02) == id< view_matrix< T[9], true, 0_C, 3_R > >));
	BT((rt_id(m03) == id< view_matrix< T[9], true, 3_C, 0_R > >));
	BT((rt_id(m04) == id< view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m05) == id< view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m06) == id< view_matrix< T[9], true, 0_C, 3_R > >));
	BT((rt_id(m07) == id< view_matrix< T[9], true, 3_C, 0_R > >));
	BT((rt_id(m08) == id< view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m09) == id< view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m10) == id< view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m11) == id< view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m12) == id< view_matrix< T[9], true, 0_C, 0_R > >));

	using array = std::array< T, 9 >;
	BT((rt_id(m13) == id< view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m14) == id< view_matrix< array, true, 0_C, 3_R > >));
	BT((rt_id(m15) == id< view_matrix< array, true, 3_C, 0_R > >));
	BT((rt_id(m16) == id< view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m17) == id< view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m18) == id< view_matrix< array, true, 0_C, 3_R > >));
	BT((rt_id(m19) == id< view_matrix< array, true, 3_C, 0_R > >));
	BT((rt_id(m20) == id< view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m21) == id< view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m22) == id< view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m23) == id< view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m24) == id< view_matrix< array, true, 0_C, 0_R > >));

	using vector = std::vector< T >;
	BT((rt_id(m25) == id< view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m26) == id< view_matrix< vector, true, 0_C, 3_R > >));
	BT((rt_id(m27) == id< view_matrix< vector, true, 3_C, 0_R > >));
	BT((rt_id(m28) == id< view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m29) == id< view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m30) == id< view_matrix< vector, true, 0_C, 3_R > >));
	BT((rt_id(m31) == id< view_matrix< vector, true, 3_C, 0_R > >));
	BT((rt_id(m32) == id< view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m33) == id< view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m34) == id< view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m35) == id< view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m36) == id< view_matrix< vector, true, 0_C, 0_R > >));

	BT((check(m01, ref_i)));
	BT((check(m02, ref_i)));
	BT((check(m03, ref_i)));
	BT((check(m04, ref_i)));
	BT((check(m05, ref_i)));
	BT((check(m06, ref_i)));
	BT((check(m07, ref_i)));
	BT((check(m08, ref_i)));
	BT((check(m09, ref_i)));
	BT((check(m10, ref_i)));
	BT((check(m11, ref_i)));
	BT((check(m12, ref_i)));

	BT((check(m13, ref_i)));
	BT((check(m14, ref_i)));
	BT((check(m15, ref_i)));
	BT((check(m16, ref_i)));
	BT((check(m17, ref_i)));
	BT((check(m18, ref_i)));
	BT((check(m19, ref_i)));
	BT((check(m20, ref_i)));
	BT((check(m21, ref_i)));
	BT((check(m22, ref_i)));
	BT((check(m23, ref_i)));
	BT((check(m24, ref_i)));

	BT((check(m25, ref_i)));
	BT((check(m26, ref_i)));
	BT((check(m27, ref_i)));
	BT((check(m28, ref_i)));
	BT((check(m29, ref_i)));
	BT((check(m30, ref_i)));
	BT((check(m31, ref_i)));
	BT((check(m32, ref_i)));
	BT((check(m33, ref_i)));
	BT((check(m34, ref_i)));
	BT((check(m35, ref_i)));
	BT((check(m36, ref_i)));
}

BOOST_AUTO_TEST_CASE_TEMPLATE(test_view_matrix_3x3_const, T, types){
	static constexpr T ref_i[3][3] = {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}};
	static T init_ca[9] = {0, 1, 2, 3, 4, 5, 6, 7, 8};
	static std::array< T, 9 > init_a{{0, 1, 2, 3, 4, 5, 6, 7, 8}};
	static std::vector< T > init_v{0, 1, 2, 3, 4, 5, 6, 7, 8};

	constexpr auto m01 = make_const_view_matrix(3_CS, 3_RS, init_ca);
	auto m02 = make_const_view_matrix(3_CD, 3_RS, init_ca);
	auto m03 = make_const_view_matrix(3_CS, 3_RD, init_ca);
	auto m04 = make_const_view_matrix(3_CD, 3_RD, init_ca);
	constexpr auto m05 = make_const_view_matrix(dim_pair(3_CS, 3_RS), init_ca);
	auto m06 = make_const_view_matrix(dim_pair(3_CD, 3_RS), init_ca);
	auto m07 = make_const_view_matrix(dim_pair(3_CS, 3_RD), init_ca);
	auto m08 = make_const_view_matrix(dim_pair(3_CD, 3_RD), init_ca);
	constexpr auto m09 = make_const_view_matrix(3_DS, init_ca);
	auto m10 = make_const_view_matrix(3_DD, init_ca);
	constexpr auto m11 = make_const_view_matrix(dim_pair(3_DS), init_ca);
	auto m12 = make_const_view_matrix(dim_pair(3_DD), init_ca);

	auto m13 = make_const_view_matrix(3_CS, 3_RS, init_a);
	auto m14 = make_const_view_matrix(3_CD, 3_RS, init_a);
	auto m15 = make_const_view_matrix(3_CS, 3_RD, init_a);
	auto m16 = make_const_view_matrix(3_CD, 3_RD, init_a);
	auto m17 = make_const_view_matrix(dim_pair(3_CS, 3_RS), init_a);
	auto m18 = make_const_view_matrix(dim_pair(3_CD, 3_RS), init_a);
	auto m19 = make_const_view_matrix(dim_pair(3_CS, 3_RD), init_a);
	auto m20 = make_const_view_matrix(dim_pair(3_CD, 3_RD), init_a);
	auto m21 = make_const_view_matrix(3_DS, init_a);
	auto m22 = make_const_view_matrix(3_DD, init_a);
	auto m23 = make_const_view_matrix(dim_pair(3_DS), init_a);
	auto m24 = make_const_view_matrix(dim_pair(3_DD), init_a);

	auto m25 = make_const_view_matrix(3_CS, 3_RS, init_v);
	auto m26 = make_const_view_matrix(3_CD, 3_RS, init_v);
	auto m27 = make_const_view_matrix(3_CS, 3_RD, init_v);
	auto m28 = make_const_view_matrix(3_CD, 3_RD, init_v);
	auto m29 = make_const_view_matrix(dim_pair(3_CS, 3_RS), init_v);
	auto m30 = make_const_view_matrix(dim_pair(3_CD, 3_RS), init_v);
	auto m31 = make_const_view_matrix(dim_pair(3_CS, 3_RD), init_v);
	auto m32 = make_const_view_matrix(dim_pair(3_CD, 3_RD), init_v);
	auto m33 = make_const_view_matrix(3_DS, init_v);
	auto m34 = make_const_view_matrix(3_DD, init_v);
	auto m35 = make_const_view_matrix(dim_pair(3_DS), init_v);
	auto m36 = make_const_view_matrix(dim_pair(3_DD), init_v);

	BT((rt_id(m01) == id< const_view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m02) == id< const_view_matrix< T[9], true, 0_C, 3_R > >));
	BT((rt_id(m03) == id< const_view_matrix< T[9], true, 3_C, 0_R > >));
	BT((rt_id(m04) == id< const_view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m05) == id< const_view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m06) == id< const_view_matrix< T[9], true, 0_C, 3_R > >));
	BT((rt_id(m07) == id< const_view_matrix< T[9], true, 3_C, 0_R > >));
	BT((rt_id(m08) == id< const_view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m09) == id< const_view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m10) == id< const_view_matrix< T[9], true, 0_C, 0_R > >));
	BT((rt_id(m11) == id< const_view_matrix< T[9], true, 3_C, 3_R > >));
	BT((rt_id(m12) == id< const_view_matrix< T[9], true, 0_C, 0_R > >));

	using array = std::array< T, 9 >;
	BT((rt_id(m13) == id< const_view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m14) == id< const_view_matrix< array, true, 0_C, 3_R > >));
	BT((rt_id(m15) == id< const_view_matrix< array, true, 3_C, 0_R > >));
	BT((rt_id(m16) == id< const_view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m17) == id< const_view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m18) == id< const_view_matrix< array, true, 0_C, 3_R > >));
	BT((rt_id(m19) == id< const_view_matrix< array, true, 3_C, 0_R > >));
	BT((rt_id(m20) == id< const_view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m21) == id< const_view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m22) == id< const_view_matrix< array, true, 0_C, 0_R > >));
	BT((rt_id(m23) == id< const_view_matrix< array, true, 3_C, 3_R > >));
	BT((rt_id(m24) == id< const_view_matrix< array, true, 0_C, 0_R > >));

	using vector = std::vector< T >;
	BT((rt_id(m25) == id< const_view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m26) == id< const_view_matrix< vector, true, 0_C, 3_R > >));
	BT((rt_id(m27) == id< const_view_matrix< vector, true, 3_C, 0_R > >));
	BT((rt_id(m28) == id< const_view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m29) == id< const_view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m30) == id< const_view_matrix< vector, true, 0_C, 3_R > >));
	BT((rt_id(m31) == id< const_view_matrix< vector, true, 3_C, 0_R > >));
	BT((rt_id(m32) == id< const_view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m33) == id< const_view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m34) == id< const_view_matrix< vector, true, 0_C, 0_R > >));
	BT((rt_id(m35) == id< const_view_matrix< vector, true, 3_C, 3_R > >));
	BT((rt_id(m36) == id< const_view_matrix< vector, true, 0_C, 0_R > >));

	BT((check(m01, ref_i)));
	BT((check(m02, ref_i)));
	BT((check(m03, ref_i)));
	BT((check(m04, ref_i)));
	BT((check(m05, ref_i)));
	BT((check(m06, ref_i)));
	BT((check(m07, ref_i)));
	BT((check(m08, ref_i)));
	BT((check(m09, ref_i)));
	BT((check(m10, ref_i)));
	BT((check(m11, ref_i)));
	BT((check(m12, ref_i)));

	BT((check(m13, ref_i)));
	BT((check(m14, ref_i)));
	BT((check(m15, ref_i)));
	BT((check(m16, ref_i)));
	BT((check(m17, ref_i)));
	BT((check(m18, ref_i)));
	BT((check(m19, ref_i)));
	BT((check(m20, ref_i)));
	BT((check(m21, ref_i)));
	BT((check(m22, ref_i)));
	BT((check(m23, ref_i)));
	BT((check(m24, ref_i)));

	BT((check(m25, ref_i)));
	BT((check(m26, ref_i)));
	BT((check(m27, ref_i)));
	BT((check(m28, ref_i)));
	BT((check(m29, ref_i)));
	BT((check(m30, ref_i)));
	BT((check(m31, ref_i)));
	BT((check(m32, ref_i)));
	BT((check(m33, ref_i)));
	BT((check(m34, ref_i)));
	BT((check(m35, ref_i)));
	BT((check(m36, ref_i)));
}

BOOST_AUTO_TEST_SUITE_END()


// -----------------------------------------------------------------------------
/**
 *  @brief Optimization problem interface
 *  @author Jose M. Buenaposada
 *  @date 2012/07/12
 *  @version $revision$
 *
 *  $id$
 *
 *  Grupo de investigación en Percepción Computacional y Robótica)
 *  (Perception for Computers & Robots research Group)
 *  Facultad de Informática (Computer Science School)
 *  Universidad Politécnica de Madrid (UPM) (Madrid Technical University)
 *  http://www.dia.fi.upm.es/~pcr
 *
 */
// -----------------------------------------------------------------------------

// ------------------ RECURSION PROTECTION -------------------------------------
#ifndef OPTIMIZATION_PROBLEM_HPP
#define OPTIMIZATION_PROBLEM_HPP

// ----------------------- INCLUDES --------------------------------------------
#include "motion_model.hpp"
#include "object_model.hpp"
#include <boost/shared_ptr.hpp>
#include <vector>
#include <opencv/cv.h>
#include "img_align_lib.hpp"

namespace upm { namespace pcr
{
// ----------------------------------------------------------------------------
/**
 *  Definition of an smart pointer to the OptimizationProblem type
 */
// -----------------------------------------------------------------------------
class OptimizationProblem;
typedef boost::shared_ptr<OptimizationProblem> OptimizationProblemPtr;

// -----------------------------------------------------------------------------
/**
 *  Definition of a container of smart pointers to OptimizationProblem class
 */
// -----------------------------------------------------------------º------------
typedef std::vector<OptimizationProblem> OptimizationProblems;

// ----------------------------------------------------------------------------
/**
 * @class OptimizationProblem
 * @brief The interface for the OptimizationProblem to be used with Optimizer
 */
// -----------------------------------------------------------------------------
class OptimizationProblem
{
public:

  OptimizationProblem
    (
    ObjectModelPtr object_model,
    MotionModelPtr motion_model
    ):
    m_motion_model(motion_model),
    m_object_model(object_model)
    {};

  virtual ~OptimizationProblem
    () {};

  virtual double
  computeCostFunction
    (
    cv::Mat& residual_vector,
    cv::Mat& motion_params,
    cv::Mat& delta_params,
    cv::Mat& image
    ) = 0;

  virtual cv::Mat
  computeResidual
    (
    cv::Mat& image,
    cv::Mat& motion_params
    ) = 0;

  virtual cv::Mat
  computeJacobian
    (
    cv::Mat& motion_params
    ) = 0;

  virtual cv::Mat
  computeInverseJacobian
    (
    cv::Mat& motion_params
    ) = 0;

  virtual cv::Mat
  updateMotionParams
    (
    cv::Mat& motion_params,
    cv::Mat& inc_params
    ) = 0;

  ObjectModelPtr
  getObjectModel
    () { return m_object_model; };

  MotionModelPtr
  getMotionModel
    () { return m_motion_model; };

protected:
  MotionModelPtr m_motion_model;
  ObjectModelPtr m_object_model;
};

}; }; // namespace

#endif


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#include <boost/simd/function/pack_cast.hpp>
#include <boost/simd/pack.hpp>
#include <simd_test.hpp>
#include "cast_test_base.hpp"

namespace bs = boost::simd;

STF_CASE( "Check cast behavior for uint8_t" )
{
  static const std::size_t N = bs::pack<uint8_t>::static_size;

  test_val<bs::pack<uint8_t, N>>(runtime);
  test_val<bs::pack<uint8_t, N/2>>(runtime);
  test_val<bs::pack<uint8_t, N*2>>(runtime);
}


/**
 * @file
 * @copyright This code is licensed under the 3-clause BSD license.\n
 *            Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.\n
 *            See LICENSE.txt for details.
 */

#include <Utils/IO/FormattedIOUtils.h>
#include <gmock/gmock.h>
#include <boost/filesystem.hpp>
#include <fstream>

namespace Scine {
namespace Utils {
using namespace testing;

/**
 * @class FormattedIOUtilsTest FormattedIOUtilsTest.cpp
 * @brief Tests the io functionalities for converting Eigen matrices to and from csv files.
 * @test
 */
class FormattedIOUtilsTest : public Test {
 public:
  void TearDown() override {
    boost::filesystem::remove_all(testFilename);
  }
  const char* testFilename = "test_formatted_io_utils.csv";
};

TEST_F(FormattedIOUtilsTest, MatrixToAndFromCsvWithCommaDelimiter) {
  Eigen::MatrixXd matrix = Eigen::MatrixXd::Random(32, 5);

  std::ofstream file(testFilename);
  matrixToCsv(file, matrix, ',');
  file.close();
  ASSERT_TRUE(boost::filesystem::exists(testFilename));

  Eigen::MatrixXd readMatrix = csvToMatrix(testFilename, ',');
  ASSERT_THAT(readMatrix.size(), Eq(matrix.size()));
  ASSERT_THAT(readMatrix.rows(), Eq(matrix.rows()));
  ASSERT_THAT(readMatrix.cols(), Eq(matrix.cols()));
  ASSERT_TRUE(readMatrix.isApprox(matrix, 1e-6));
}

TEST_F(FormattedIOUtilsTest, MatrixToAndFromCsvWithSpaceDelimiter) {
  Eigen::MatrixXd matrix = Eigen::MatrixXd::Random(4, 4);

  std::ofstream file(testFilename);
  matrixToCsv(file, matrix, ' ');
  file.close();
  ASSERT_TRUE(boost::filesystem::exists(testFilename));

  Eigen::MatrixXd readMatrix = csvToMatrix(testFilename, ' ');
  ASSERT_THAT(readMatrix.size(), Eq(matrix.size()));
  ASSERT_THAT(readMatrix.rows(), Eq(matrix.rows()));
  ASSERT_THAT(readMatrix.cols(), Eq(matrix.cols()));
  ASSERT_TRUE(readMatrix.isApprox(matrix, 1e-6));
}

TEST_F(FormattedIOUtilsTest, ColumnVectorToAndFromCsv) {
  Eigen::VectorXd vector = Eigen::VectorXd::Random(12);

  std::ofstream file(testFilename);
  matrixToCsv(file, vector, ':');
  file.close();
  ASSERT_TRUE(boost::filesystem::exists(testFilename));

  Eigen::MatrixXd readMatrix = csvToMatrix(testFilename, ':');
  ASSERT_THAT(readMatrix.size(), Eq(vector.size()));
  ASSERT_THAT(readMatrix.rows(), Eq(vector.size()));
  ASSERT_THAT(readMatrix.cols(), Eq(1));
  ASSERT_TRUE(readMatrix.isApprox(vector, 1e-6));
}

TEST_F(FormattedIOUtilsTest, RowVectorToAndFromCsv) {
  Eigen::RowVectorXd vector = Eigen::RowVectorXd::Random(12);

  std::ofstream file(testFilename);
  matrixToCsv(file, vector, ':');
  file.close();
  ASSERT_TRUE(boost::filesystem::exists(testFilename));

  Eigen::MatrixXd readMatrix = csvToMatrix(testFilename, ':');
  ASSERT_THAT(readMatrix.size(), Eq(vector.size()));
  ASSERT_THAT(readMatrix.rows(), Eq(1));
  ASSERT_THAT(readMatrix.cols(), Eq(vector.size()));
  ASSERT_TRUE(readMatrix.isApprox(vector, 1e-6));
}

} // namespace Utils
} // namespace Scine


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief temperature.cpp

\details
Conversions between Fahrenheit and Kelvin for absolute temperatures and
temperature differences.

Output:
@verbatim

//[ temperature_output_1
{ 32 } F
{ 273.15 } K
{ 273.15 } K
[ 32 ] F
[ 17.7778 ] K
[ 17.7778 ] K
//]

@endverbatim
**/

#include <iomanip>
#include <iostream>

#include <boost/units/absolute.hpp>
#include <boost/units/get_system.hpp>
#include <boost/units/io.hpp>
#include <boost/units/unit.hpp>
#include <boost/units/quantity.hpp>
#include <boost/units/systems/si/temperature.hpp>
#include <boost/units/detail/utility.hpp>

#include <boost/units/base_units/temperature/fahrenheit.hpp>

using namespace boost::units;

namespace boost {

namespace units {

namespace fahrenheit {

//[temperature_snippet_1
typedef temperature::fahrenheit_base_unit::unit_type    temperature;
typedef get_system<temperature>::type                   system;

BOOST_UNITS_STATIC_CONSTANT(degree,temperature);
BOOST_UNITS_STATIC_CONSTANT(degrees,temperature);
//]

} // fahrenheit

} // namespace units

} // namespace boost

int main()
{
    //[temperature_snippet_3
    quantity<absolute<fahrenheit::temperature> >    T1p(
        32.0*absolute<fahrenheit::temperature>());
    quantity<fahrenheit::temperature>               T1v(
        32.0*fahrenheit::degrees);

    quantity<absolute<si::temperature> >            T2p(T1p);
    quantity<si::temperature>                       T2v(T1v);
    //]

    typedef conversion_helper<
        quantity<absolute<fahrenheit::temperature> >,
        quantity<absolute<si::temperature> > >          absolute_conv_type;
    typedef conversion_helper<
        quantity<fahrenheit::temperature>,
        quantity<si::temperature> >                     relative_conv_type;

    std::cout << T1p << std::endl
              << absolute_conv_type::convert(T1p) << std::endl
              << T2p << std::endl
              << T1v << std::endl
              << relative_conv_type::convert(T1v) << std::endl
              << T2v << std::endl
              << std::endl;

    return 0;
}


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

// Copyright (c) 2007-2013 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2013 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2013 Mateusz Loskot, London, UK.
// Copyright (c) 2013 Adam Wulkiewicz, Lodz, Poland.

// 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)

#ifndef BOOST_GEOMETRY_ALGORITHMS_DISTANCE_INFO_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DISTANCE_INFO_HPP

#include <boost/mpl/if.hpp>
#include <boost/range/functions.hpp>
#include <boost/range/metafunctions.hpp>
#include <boost/static_assert.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/reverse_dispatch.hpp>

#include <boost/geometry/algorithms/not_implemented.hpp>
#include <boost/geometry/algorithms/detail/throw_on_empty_input.hpp>

#include <boost/geometry/geometries/concepts/check.hpp>

#include <boost/geometry/strategies/distance.hpp>
#include <boost/geometry/strategies/default_distance_result.hpp>

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/extensions/strategies/cartesian/distance_info.hpp>

#include <boost/geometry/util/math.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace distance_info {

template <typename Point1, typename Point2>
struct point_point
{
    template <typename Strategy, typename Result>
    static inline void apply(Point1 const& point1, Point2 const& point2, Strategy const& strategy, Result& result)
    {
        result.real_distance
            = result.projected_distance1
            = result.projected_distance2
            = strategy.apply_point_point(point1, point2);
        // The projected point makes not really sense in point-point.
        // We just assign one on the other
        geometry::convert(point1, result.projected_point2);
        geometry::convert(point2, result.projected_point1);
    }
};

template <typename Point, typename Range>
struct point_range
{
    template <typename Strategy, typename Result>
    static inline void apply(Point const& point, Range const& range, Strategy const& strategy, Result& result)
    {
        // This should not occur (see exception on empty input below)
        if (boost::begin(range) == boost::end(range))
        {
            return;
        }

        // line of one point: same case as point-point
        typedef typename boost::range_const_iterator<Range>::type iterator_type;
        iterator_type it = boost::begin(range);
        iterator_type prev = it++;
        if (it == boost::end(range))
        {
            point_point<Point, typename boost::range_value<Range const>::type>::apply(point, *prev, strategy, result);
            return;
        }

        // Initialize with first segment
        strategy.apply(point, *prev, *it, result);

        // Check other segments
        for(prev = it++; it != boost::end(range); prev = it++)
        {
            Result other;
            strategy.apply(point, *prev, *it, other);
            if (other.real_distance < result.real_distance)
            {
                result = other;
            }
        }
    }
};

}} // namespace detail::distance_info
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Geometry1, typename Geometry2,
    typename Tag1 = typename tag<Geometry1>::type,
    typename Tag2 = typename tag<Geometry2>::type,
    bool Reverse = reverse_dispatch<Geometry1, Geometry2>::type::value
>
struct distance_info : not_implemented<Tag1, Tag2>
{
};

template
<
    typename Geometry1, typename Geometry2,
    typename Tag1, typename Tag2
>
struct distance_info<Geometry1, Geometry2, Tag1, Tag2, true>
{
    template <typename Strategy, typename Result>
    static inline void apply(Geometry1 const& geometry1, Geometry2 const& geometry2,
                    Strategy const& strategy, Result& result)
    {
        // Reversed version just calls dispatch with reversed arguments
        distance_info
            <
                Geometry2, Geometry1, Tag2, Tag1, false
            >::apply(geometry2, geometry1, strategy, result);
    }

};

template<typename Point1, typename Point2>
struct distance_info
    <
        Point1, Point2,
        point_tag, point_tag, false
    > : public detail::distance_info::point_point<Point1, Point2>
{};

template<typename Point, typename Segment>
struct distance_info
    <
        Point, Segment,
        point_tag, segment_tag,
        false
    >
{
    template <typename Strategy, typename Result>
    static inline void apply(Point const& point, Segment const& segment,
                    Strategy const& strategy, Result& result)
    {

        typename point_type<Segment>::type p[2];
        geometry::detail::assign_point_from_index<0>(segment, p[0]);
        geometry::detail::assign_point_from_index<1>(segment, p[1]);

        strategy.apply(point, p[0], p[1], result);
    }
};

//template
//<
//    typename Point, typename Ring,
//    typename Point
//>
//struct distance_info
//    <
//        point_tag, ring_tag,
//        Point, Ring,
//        Point
//    >
//    : detail::distance_info::point_range<Point, Ring, Point>
//{};
//
//
template<typename Point, typename Linestring>
struct distance_info
    <
        Point, Linestring,
        point_tag, linestring_tag,
        false
    >
    : detail::distance_info::point_range<Point, Linestring>
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

template <typename Geometry1, typename Geometry2, typename Result>
inline void distance_info(Geometry1 const& geometry1, Geometry2 const& geometry2, Result& result)
{
    concepts::check<Geometry1 const>();
    concepts::check<Geometry2 const>();
    concepts::check<typename Result::point_type>();

    assert_dimension_equal<Geometry1, Geometry2>();
    assert_dimension_equal<Geometry1, typename Result::point_type>();

    detail::throw_on_empty_input(geometry1);
    detail::throw_on_empty_input(geometry2);

    strategy::distance::calculate_distance_info<> info_strategy;

    dispatch::distance_info
            <
                Geometry1,
                Geometry2
            >::apply(geometry1, geometry2, info_strategy, result);
}

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DISTANCE_INFO_HPP


#include <cryptopp/eccrypto.h>
#include <cryptopp/ecpoint.h>
#include <cryptopp/asn.h>
#include <cryptopp/crc.h>
#include <cryptopp/oids.h>
#include <cryptopp/osrng.h>
#include <cryptopp/drbg.h>
#include <cryptopp/modes.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <set>
#include <thread>
#include <string>
#include <sstream>
#include <boost/tokenizer.hpp>

const CryptoPP::ECPPoint G(CryptoPP::Integer("362dc3caf8a0e8afd06f454a6da0cdce6e539bc3f15e79a15af8aa842d7e3ec2h"),
                           CryptoPP::Integer("b9f8addb295b0fd4d7c49a686eac7b34a9a11ed2d6d243ad065282dc13bce575h"));

const CryptoPP::ECPPoint H(CryptoPP::Integer("a3cf0a4b6e1d9146c73e9a82e4bfdc37ee1587bc2bf3b0c19cb159ae362e38beh"),
                           CryptoPP::Integer("db4369fabd3d770dd4c19d81ac69a1749963d69c687d7c4e12d186548b94cb2ah"));

std::mutex mut;

unsigned NUM_THREADS = 6;

uint32_t num_values = 256;
std::vector<std::vector<CryptoPP::ECPPoint>> testMatrix(num_values);

int main() {
    CryptoPP::AutoSeededRandomPool PRNG;
    CryptoPP::DL_GroupParameters_EC<CryptoPP::ECP> ec_group;
    ec_group.Initialize(CryptoPP::ASN1::secp256k1());

    auto start = std::chrono::high_resolution_clock::now();
    std::vector<CryptoPP::ECPPoint> test;
    test.reserve(10000);
    for(uint32_t i = 0; i < 8864; i++) {
        CryptoPP::Integer exponent(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::ECPPoint testPoint = ec_group.GetCurve().ScalarMultiply(G, exponent);
        test.push_back(std::move(testPoint));
    }
    auto finish = std::chrono::high_resolution_clock::now();

    std::chrono::duration<double> duration = finish - start;

    std::cout << "Duration: " << duration.count() << std::endl;
    /*
    for(;;) {
        CryptoPP::Integer d(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer r(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer z(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());

        CryptoPP::Integer w = r * z + d;
        w = w.Modulo(ec_group.GetSubgroupOrder());

        CryptoPP::Integer rz = r * z;
        rz = rz.Modulo(ec_group.GetSubgroupOrder());

        CryptoPP::ECPPoint wG = ec_group.GetCurve().ScalarMultiply(G, w);

        CryptoPP::ECPPoint rzG = ec_group.GetCurve().ScalarMultiply(G, rz);
        CryptoPP::ECPPoint dG = ec_group.GetCurve().ScalarMultiply(G, d);

        CryptoPP::ECPPoint rzG_dG = ec_group.GetCurve().Add(rzG, dG);

        std::cout << "wG" << std::endl;
        std::cout << std::hex << wG.x << std::endl;
        std::cout << std::hex << wG.y << std::endl;

        std::cout << "rzG + dG" << std::endl;
        std::cout << std::hex << rzG_dG.x << std::endl;
        std::cout << std::hex << rzG_dG.y << std::endl;

        if ((wG.x != rzG_dG.x) || (wG.y != rzG_dG.y))
            break;

        CryptoPP::Integer r_(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());
        CryptoPP::Integer x(PRNG, CryptoPP::Integer::One(), ec_group.GetMaxExponent());

        CryptoPP::Integer rr_ = r + r_;

        CryptoPP::ECPPoint rG = ec_group.GetCurve().ScalarMultiply(G, r);
        CryptoPP::ECPPoint r_G = ec_group.GetCurve().ScalarMultiply(G, r_);

        CryptoPP::ECPPoint xH = ec_group.GetCurve().ScalarMultiply(H, x);

        CryptoPP::ECPPoint rr_G = ec_group.GetCurve().ScalarMultiply(G, rr_);

        CryptoPP::ECPPoint C = ec_group.GetCurve().Add(rG, xH);

        CryptoPP::ECPPoint C_ = ec_group.GetCurve().Add(rr_G, xH);

        CryptoPP::ECPPoint Cr_G = ec_group.GetCurve().Add(C, r_G);

        CryptoPP::ECPPoint CC_ = ec_group.GetCurve().Add(C_, ec_group.GetCurve().Inverse(C));

        std::cout << "C:" << std::endl;
        std::cout << std::hex << C.x << std::endl;
        std::cout << std::hex << C.y << std::endl;

        std::cout << "C_:" << std::endl;
        std::cout << std::hex << C_.x << std::endl;
        std::cout << std::hex << C_.y << std::endl;

        std::cout << "Cr_G:" << std::endl;
        std::cout << std::hex << Cr_G.x << std::endl;
        std::cout << std::hex << Cr_G.y << std::endl;

        std::cout << "r_G:" << std::endl;
        std::cout << std::hex << r_G.x << std::endl;
        std::cout << std::hex << r_G.y << std::endl;

        std::cout << "CC_:" << std::endl;
        std::cout << std::hex << CC_.x << std::endl;
        std::cout << std::hex << CC_.y << std::endl;

        if ((CC_.x != r_G.x) || (CC_.y != r_G.y))
            break;
    }
    */
    return 0;
}