//==================================================================================================
/*!
  @file

  @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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIV_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_DIV_HPP_INCLUDED

#include <boost/simd/function/divides.hpp>
#include <boost/simd/function/definition/fix.hpp>
#include <boost/simd/function/ifloor.hpp>
#include <boost/simd/function/ifix.hpp>
#include <boost/simd/function/iround.hpp>
#include <boost/simd/function/inearbyint.hpp>
#include <boost/simd/arch/common/scalar/function/divceil.hpp>
#include <boost/simd/arch/common/scalar/function/divfloor.hpp>
#include <boost/simd/arch/common/scalar/function/divround.hpp>
#include <boost/simd/arch/common/scalar/function/divnearbyint.hpp>
#include <boost/simd/arch/common/scalar/function/divtrunc.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/hierarchy.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{

  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bd::scalar_<bd::arithmetic_<T>>
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(T  a, T  b ) const BOOST_NOEXCEPT
    {
      return divides(a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::fix_
                          , bd::scalar_<bd::arithmetic_<T>>
                          , bd::scalar_<bd::arithmetic_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::fix_> const&
                                  , T  a, T  b ) const BOOST_NOEXCEPT
    {
      return div(bs::trunc, a, b);
    }
  };

#ifdef BOOST_MSVC
  #pragma warning(push)
  #pragma warning(disable: 4723) // potential divide by 0
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::ifix_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return saturated_(toint)(a/b);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::iceil_> const&
                                                    , T const& a, T const& b ) const BOOST_NOEXCEPT
    {
      return iceil(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::ifloor_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::ifloor_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return ifloor(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::iround_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE  bd::as_integer_t<T>  operator()( bd::functor<bs::tag::iround_> const&
                                                      ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return iround(a/b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bd::scalar_<bd::floating_<T>>
                          , bd::scalar_<bd::floating_<T>>
                          )
  {
    BOOST_FORCEINLINE bd::as_integer_t<T> operator()( bd::functor<bs::tag::inearbyint_> const&
                                                    ,T const& a, T const& b) const BOOST_NOEXCEPT
    {
      return inearbyint(a/b);
    }
  };

#ifdef BOOST_MSVC
#pragma warning(pop)
#endif

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::ifix_
                          , bd::scalar_<bd::integer_<T>>
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::ifix_> const&
                                                    , T  a, T  b ) const BOOST_NOEXCEPT
    {
      return  div(fix, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::iceil_
                          , bd::scalar_<bd::integer_<T>>
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::iceil_> const&
                                                    , T  a, T  b ) const BOOST_NOEXCEPT
    {
      return div(ceil, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::ifloor_
                          , bd::scalar_<bd::integer_<T>>
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::ifloor_> const&
                                                    ,T  a, T  b) const BOOST_NOEXCEPT
    {
      return  div(floor, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::iround_
                          , bd::scalar_<bd::integer_<T>>
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::iround_> const&
                                                    ,T  a, T  b) const BOOST_NOEXCEPT
    {
      return div(round, a, b);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( div_
                          , (typename T)
                          , bd::cpu_
                          , bs::tag::inearbyint_
                          , bd::scalar_<bd::integer_<T>>
                          , bd::scalar_<bd::integer_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()( bd::functor<bs::tag::inearbyint_> const&
                                  ,T  a, T  b) const BOOST_NOEXCEPT
    {
      return  div(nearbyint, a, b);
    }
  };

} } }

#endif


//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#ifndef METRO_CHOLESKY_STEPPER_HPP
#define METRO_CHOLESKY_STEPPER_HPP

#include <string>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include "boost/function.hpp"
#include "metro/ModifiedCholesky.hpp"
#include "metro/SmoothFunction.hpp"
#include "metro/fit_model.hpp"

namespace metro {
	struct CholeskyStepper: public Stepper {
	public:
		CholeskyStepper( double tolerance, int max_iterations, Tracer tracer = Tracer()) ;

		bool step( Function& function, Vector* result ) ;

		bool diverged() const ;
		std::size_t number_of_iterations() const ;
		void reset() ;

	private:
		double const m_tolerance ;
		int const m_max_iterations ;
		Tracer m_tracer ;
		int m_iteration ;
		metro::ModifiedCholesky< Matrix > m_solver ;
		//Eigen::LDLT< Matrix > m_solver ;
		//Eigen::ColPivHouseholderQR< Matrix > m_solver ;
		double m_target_ll ;
	} ;
}

#endif


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#pragma once

#include <cmath>
#include <stdexcept>
#include <iostream>
#include <boost/thread.hpp>

#include "conf/settings/DisableMPI.h"
#include "geospatial/network/Point.hpp"
#include "logging/Log.hpp"

#ifndef SIMMOB_DISABLE_MPI
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#endif

////////////////////////////////////////////////
// Serialization for Point. This is how one would
// serialize *outside* the class itself. You can also
// put the serialize() function inside Point; see
// FixedDelayed<> for an example of this alternate approach.
////////////////////////////////////////////////
#ifndef SIMMOB_DISABLE_MPI
namespace boost
{
  namespace serialization
  {
    template<class Archive>
    void serialize(Archive & ar, sim_mob::Point& pt, const unsigned int version)
    {
      ar & pt.x;
      ar & pt.y;
    }

  } // namespace serialization
} // namespace boost
#endif
////////////////////////////////////////////////
// End serialization code
////////////////////////////////////////////////

namespace sim_mob
{
  /**
   * Simple, lightweight class to represent vector operations
   *
   * \author Seth N. Hetu
   * \author Xu Yan
   */
  class DynamicVector
  {
  private:
    Point pos;
    Point mag;

    //Helper variable; if set, it is assumed that mag==(0,0). In fact, mag remains at its previous
    //value so that scaling to zero and back produces a valid vector. The alternative is to maintain
    //the vector's angle, but this would be more costly.
    bool isZero;

#ifndef SIMMOB_DISABLE_MPI
  public:
    friend class boost::serialization::access;

    template<class Archive>
    void serialize(Archive & ar, const unsigned int version)
    {
      ar & pos;
      ar & mag;
      ar & isZero;
    }
#endif

  public:
    DynamicVector(double fromX = 0.0, double fromY = 0.0, double toX = 0.0, double toY = 0.0);
    DynamicVector(const Point& from, const Point& to);
    DynamicVector(const DynamicVector& copyFrom);

    //Retrieve the x coordinate of the origin.
    double getX() const;

    //Retrieve the y coordinate of the origin.
    double getY() const;

    //Retrieve the x coordinate of the origin+magnitude.
    double getEndX() const;

    //Retrieve the y coordinate of the origin+magnitude.
    double getEndY() const;

    //Retrieve the magnitude.
    double getMagnitude() const;

    //Retrieve the angle. Should still work even if the magnitude is zero
    //Returns an angle in the range 0..2*PI
    double getAngle() const;

    //Shift this vector by dX,dY. (Moves the origin)
    DynamicVector& translateVect(double dX, double dY);

    //Shift this vector by its own magnitude. Effectively moves the vector's origin to its "end" point.
    DynamicVector& translateVect();

    ///Scale this vector's magnitude TO a given value. (Note that this vector need not be a unit vector.)
    DynamicVector& scaleVectTo(double val);

    //Flip this vector 180 degrees around the origin.
    DynamicVector& flipMirror();

    //Flip this vector 90 degrees around the origin, either clockwise or counter-clockwise.
    DynamicVector& flipNormal(bool clockwise);

    //Flip this vector 90 degrees clockwise around the origin.
    DynamicVector& flipRight();

    //Flip this vector 90 degrees counter-clockwise around the origin.
    DynamicVector& flipLeft();

  } ;
}


//==============================================================================
//         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
//==============================================================================
/*!
 * \file
**/
#ifndef NT2_COMPLEX_CONSTANTS_CNAN_HPP_INCLUDED
#define NT2_COMPLEX_CONSTANTS_CNAN_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <boost/simd/sdk/constant/constant.hpp>
#include <complex>

/*!
 * \ingroup nt2_complex
 * \defgroup nt2_complex_Cnan Cnan
 *
 * \par Description
 * Constant Cnan : Imaginary mask.
 *
 * \par Header file
 *
 * \code
 * #include <nt2/include/functions/i.hpp>
 * \endcode
 *
 * \alias:
 *    J
 *
 * \synopsis
 *
 * \code
 * namespace boost::simd
 * {
 *   template <class T>
 *     meta::call<tag::Cnan(as_<T>)>::type
 *     Cnan();
 * }
 * \endcode
 *
 *
 * \param T template parameter of Cnan
 *
 * \return type T value
 *
 *
**/

namespace nt2
{
  namespace tag
  {
    /*!
     * \brief Define the tag Cnan of functor Cnan
     *        in namespace nt2::tag for toolbox complex
    **/
    struct Cnan : ext::constant_<Cnan>
    {
      typedef std::complex<double> default_type;
      typedef ext::constant_<Cnan> parent;
    };
  }

  BOOST_SIMD_CONSTANT_IMPLEMENTATION(nt2::tag::Cnan, Cnan)

}

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/config.hpp>
#include <iostream>
#include <vector>
#include <utility>
#include <string>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/property_map/property_map.hpp>

/*
  Sample Output

  graph name: foo
  0  --joe--> 1
  1  --joe--> 0   --curly--> 2   --dick--> 3
  2  --curly--> 1   --tom--> 4
  3  --dick--> 1   --harry--> 4
  4  --tom--> 2   --harry--> 3
  (0,1) (1,2) (1,3) (2,4) (3,4)

  removing edge (1,3):
  0  --joe--> 1
  1  --joe--> 0   --curly--> 2
  2  --curly--> 1   --tom--> 4
  3  --harry--> 4
  4  --tom--> 2   --harry--> 3
  (0,1) (1,2) (2,4) (3,4)

 */

struct EdgeProperties {
  EdgeProperties(const std::string& n) : name(n) { }
  std::string name;
};

struct VertexProperties {
  std::size_t index;
  boost::default_color_type color;
};

int main(int , char* [])
{
  using namespace boost;
  using namespace std;

  typedef adjacency_list<vecS, listS, undirectedS,
    VertexProperties, EdgeProperties> Graph;

  const int V = 5;
  Graph g(V);

  property_map<Graph, std::size_t VertexProperties::*>::type
    id = get(&VertexProperties::index, g);
  property_map<Graph, std::string EdgeProperties::*>::type
    name = get(&EdgeProperties::name, g);

  boost::graph_traits<Graph>::vertex_iterator vi, viend;
  int vnum = 0;

  for (boost::tie(vi,viend) = vertices(g); vi != viend; ++vi)
    id[*vi] = vnum++;

  add_edge(vertex(0, g), vertex(1, g), EdgeProperties("joe"), g);
  add_edge(vertex(1, g), vertex(2, g), EdgeProperties("curly"), g);
  add_edge(vertex(1, g), vertex(3, g), EdgeProperties("dick"), g);
  add_edge(vertex(2, g), vertex(4, g), EdgeProperties("tom"), g);
  add_edge(vertex(3, g), vertex(4, g), EdgeProperties("harry"), g);

  graph_traits<Graph>::vertex_iterator i, end;
  graph_traits<Graph>::out_edge_iterator ei, edge_end;
  for (boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << id[*i] << " ";
    for (boost::tie(ei,edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    cout << endl;
  }
  print_edges(g, id);

  cout << endl << "removing edge (1,3): " << endl;
  remove_edge(vertex(1, g), vertex(3, g), g);

  ei = out_edges(vertex(1, g), g).first;
  cout << "removing edge (" << id[source(*ei, g)]
       << "," << id[target(*ei, g)] << ")" << endl;
  remove_edge(ei, g);

  for(boost::tie(i,end) = vertices(g); i != end; ++i) {
    cout << id[*i] << " ";
    for (boost::tie(ei,edge_end) = out_edges(*i, g); ei != edge_end; ++ei)
      cout << " --" << name[*ei] << "--> " << id[target(*ei, g)] << "  ";
    cout << endl;
  }

  print_edges(g, id);

  return 0;
}


/*=============================================================================
Copyright 2020 Syed Ali Hasan <alihasan9922@gmail.com>

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

#ifndef BOOST_ASTRONOMY_EQUATORIAL_RA_COORD_HPP
#define BOOST_ASTRONOMY_EQUATORIAL_RA_COORD_HPP

#include <iostream>
#include <boost/static_assert.hpp>
#include <boost/geometry/core/cs.hpp>
#include <boost/units/get_dimension.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/physical_dimensions/plane_angle.hpp>
#include <boost/astronomy/coordinate/coord_sys/coord_sys.hpp>

/**
 * The Equatorial Coordinates, are referred to the plane of the Earth’s equator
 *
 * Declination
 * Declination is analogous to latitude and indicates how far away an object is
 * from the celestial equator. Declination is in the range ±90◦ with positive
 * angles indicating locations north of the celestial equator and negative angles
 * indicating locations south of the celestial equator. Because declination is
 * measured with respect to the celestial equator, and the celestial equator’s
 * location does not vary with time of day or an observer’s location, declination
 * for an object is fixed and does not vary with the time of day or an
 * observer’s location.
 *
 * Right Ascension
 * Right ascension is analogous to longitude and indicates how far an object is away
 * from the First Point of Aries, which is the point used to define a celestial prime meridian.
 * It measures the distance from an object to the celestial prime meridian. As with declination,
 * right ascension does not vary with time of day or an observer’s location because it is measured
 * with respect to a fixed location (the First Point of Aries).
 *
**/

namespace boost { namespace astronomy { namespace coordinate {

namespace bu = boost::units;
namespace bg = boost::geometry;

template
<
    typename CoordinateType = double,
    typename RightAscensionQuantity = bu::quantity<bu::si::plane_angle, CoordinateType>,
    typename Declination = bu::quantity<bu::si::plane_angle, CoordinateType>
>
struct equatorial_ra_coord : public coord_sys
       <2, bg::cs::spherical<bg::radian>, CoordinateType>
{
    ///@cond INTERNAL
    BOOST_STATIC_ASSERT_MSG(
        ((std::is_same<typename bu::get_dimension<RightAscensionQuantity>::type,
          bu::plane_angle_dimension>::value) &&
         (std::is_same<typename bu::get_dimension<Declination>::type,
          bu::plane_angle_dimension>::value)),
        "Right Ascension and Declination must be of plane angle type");
    BOOST_STATIC_ASSERT_MSG((std::is_floating_point<CoordinateType>::value),
                            "CoordinateType must be a floating-point type");
    ///@endcond
public:
    typedef RightAscensionQuantity quantity1;
    typedef Declination quantity2;

    //Default constructor
    equatorial_ra_coord() {}

    equatorial_ra_coord
    (
        RightAscensionQuantity const &Ra,
        Declination const &Dec
    )
    {
            this->set_ra_dec(Ra, Dec);
    }

    //Create tuple of Right Ascension and Declination
    std::tuple<RightAscensionQuantity, Declination> get_ra_dec() const
    {
            return std::make_tuple(this->get_ra(), this->get_dec());
    }

    //Get Right Ascension
    RightAscensionQuantity get_ra() const
    {
            return static_cast<RightAscensionQuantity>
                   (
                       bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                       (bg::get<0>(this->point))
                   );
    }

    //Get Declination
    Declination get_dec() const
    {
            return static_cast<Declination>
                   (
                       bu::quantity<bu::si::plane_angle, CoordinateType>::from_value
                       (bg::get<1>(this->point))
                   );
    }

    //Set value of Right Ascension and Declination
    void set_ra_dec
    (
        RightAscensionQuantity const &Ra,
        Declination const &Dec
    )
    {
            this->set_ra(Ra);
            this->set_dec(Dec);
    }

    //Set Right Ascension
    void set_ra(RightAscensionQuantity const &Ra)
    {
            bg::set<0>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Ra).value()
            );
    }

    //Set Declination
    void set_dec(Declination const &Dec)
    {
            bg::set<1>
            (
                this->point,
                static_cast<bu::quantity<bu::si::plane_angle, CoordinateType>>(Dec).value()
            );
    }

}; //equatorial_ra_coord

//Make Equatorial Coordinate
template
<
    typename CoordinateType,
    template<typename Unit2, typename CoordinateType_> class RightAscensionQuantity,
    template<typename Unit1, typename CoordinateType_> class Declination,
    typename Unit1,
    typename Unit2
>
equatorial_ra_coord
<
  CoordinateType,
  Declination<Unit1, CoordinateType>,
  RightAscensionQuantity<Unit2, CoordinateType>
> make_equatorial_ra_coord
(
    RightAscensionQuantity<Unit2, CoordinateType> const &Ra,
    Declination<Unit1, CoordinateType> const &Dec
)
{
    return equatorial_ra_coord
           <
           CoordinateType,
           RightAscensionQuantity<Unit2, CoordinateType>,
           Declination<Unit1, CoordinateType>
           > (Ra, Dec);
}

//Print Equatorial Right Ascension Coordinates
template
<
    typename CoordinateType,
    class RightAscensionQuantity,
    class Declination
>
std::ostream &operator << (std::ostream &out, equatorial_ra_coord
                           <CoordinateType, RightAscensionQuantity, Declination> const &point) {
  out << "Equatorial Coordinate (Right Ascension: "
      << point.get_ra() << ", Declination: "
      << point.get_dec() << ")";

  return out;
}

}}}

#endif  // BOOST_ASTRONOMY_EQUATORIAL_RA_COORD_HPP


/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "mitkCommandLineParser.h"
#include "mitkImage.h"
#include <mitkImageStatisticsCalculator.h>
#include <mitkExtendedLabelStatisticsImageFilter.h>
#include <mitkPlanarFigureMaskGenerator.h>
#include <mitkPlanarFigure.h>
#include "mitkIOUtil.h"
#include <iostream>
#include <usAny.h>
#include <fstream>
#include <itkImageRegionConstIterator.h>
#include "mitkImageAccessByItk.h"
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/variance.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/count.hpp>
#include <boost/accumulators/statistics/moment.hpp>
#include <mitkImageMaskGenerator.h>
#include <mitkHotspotMaskGenerator.h>
#include <mitkImageGenerator.h>

int main( int argc, char* argv[] )
{
    unsigned int timeStep = 0;
    std::string inputImageFile, maskImageFile;
    inputImageFile = "/home/fabian/MITK/MITK_platform_project/bin/MITK-superbuild/MITK-Data/Pic3D.nrrd";
    maskImageFile = "/home/fabian/MITK/MITK_platform_project/bin/MITK-superbuild/MITK-Data/Pic3D_someSegmentation.nrrd";
    // Load image
    mitk::Image::Pointer inputImage = mitk::IOUtil::LoadImage(inputImageFile);
    mitk::Image::Pointer maskImage = mitk::IOUtil::LoadImage(maskImageFile);

    // Calculate statistics
    mitk::ImageStatisticsCalculator::Pointer calculator = mitk::ImageStatisticsCalculator::New();

    std::cout << "calculating statistics (unmasked) itk: " << std::endl;
    mitk::ImageStatisticsCalculator::StatisticsContainer::Pointer result;

    mitk::ImageMaskGenerator::Pointer imgMaskGen = mitk::ImageMaskGenerator::New();
    imgMaskGen->SetImageMask(maskImage);

    calculator->SetMask(imgMaskGen.GetPointer());
    calculator->SetInputImage(inputImage);
    calculator->SetNBinsForHistogramStatistics(100);

    for (unsigned int i=0; i < inputImage->GetTimeSteps(); i++)
    {
        std::cout << "Results for time step " << i << ":" << std::endl;
        result = calculator->GetStatistics(i, 1);
        result->Print();
        std::cout << std::endl;
    }
    MITK_INFO << "-------------";
    mitk::Image::Pointer test = imgMaskGen->GetMask();
    MITK_INFO << "-------------";

    mitk::Image::Pointer test2 = imgMaskGen->GetMask();
    MITK_INFO << "-------------";
    imgMaskGen->SetTimeStep(2);
    mitk::Image::Pointer test3 = imgMaskGen->GetMask();

    return EXIT_SUCCESS;
}


#include <boost/mp11.hpp>
using namespace boost::mp11;

int main()
{
    using L1 = mp_list<int, float, int, float>;
    return mp_size<mp_unique<L1>>::value == 2? 0: 1;
}


#define BOOST_TEST_MODULE "test_stat_tracker"

#include "visUtil.hpp" // for StatTracker

#include <boost/test/included/unit_test.hpp> // for BOOST_PP_IIF_1, BOOST_PP_IIF_0, BOOST_PP_BO...
#include <cmath>                             // for isnan
#include <string>                            // for string

BOOST_AUTO_TEST_CASE(_stat_tracker_get_max_optimized) {
    StatTracker buf("test", "none", 3, true);

    BOOST_CHECK(isnan(buf.get_max()));
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
    buf.add_sample(-2.5);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
    buf.add_sample(2.5);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);

    // Overwrite the buffer with all 0.0 and see if the max can be updated.
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
}

BOOST_AUTO_TEST_CASE(_stat_tracker_get_max_unoptimized) {
    StatTracker buf("test", "none", 3, false);

    BOOST_CHECK(isnan(buf.get_max()));
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
    buf.add_sample(-2.5);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
    buf.add_sample(2.5);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);

    // Overwrite the buffer with all 0.0 and see if the max can be updated.
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_max(), 0.0);
}

BOOST_AUTO_TEST_CASE(_stat_tracker_get_min_optimized) {
    StatTracker buf("test", "none", 3, true);

    BOOST_CHECK(isnan(buf.get_min()));
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
    buf.add_sample(-2.5);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);
    buf.add_sample(2.5);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);

    // Overwrite the buffer with all 0.0 and see if the min can be updated.
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
}

BOOST_AUTO_TEST_CASE(_stat_tracker_get_min_unoptimized) {
    StatTracker buf("test", "none", 3, false);

    BOOST_CHECK(isnan(buf.get_min()));
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
    buf.add_sample(-2.5);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);
    buf.add_sample(2.5);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);

    // Overwrite the buffer with all 0.0 and see if the min can be updated.
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), -2.5);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_min(), 0.0);
}

BOOST_AUTO_TEST_CASE(_stat_tracker_get_avg) {
    StatTracker buf("test", "none", 3);

    BOOST_CHECK(isnan(buf.get_avg()));
    buf.add_sample(0.0);
    BOOST_CHECK_EQUAL(buf.get_avg(), 0.0);
    buf.add_sample(1.0);
    BOOST_CHECK_EQUAL(buf.get_avg(), 0.5);
    buf.add_sample(2.0);
    BOOST_CHECK_EQUAL(buf.get_avg(), 1.0);

    // Overwrite the buffer to check the current average.
    buf.add_sample(3.0);
    BOOST_CHECK_EQUAL(buf.get_avg(), 2.0);
    buf.add_sample(4.0);
    BOOST_CHECK_EQUAL(buf.get_avg(), 3.0);
}

BOOST_AUTO_TEST_CASE(_stat_tracker_get_std_dev) {
    StatTracker buf("test", "none", 3);

    BOOST_CHECK(isnan(buf.get_std_dev()));
    buf.add_sample(0.0);
    BOOST_CHECK(isnan(buf.get_std_dev()));
    buf.add_sample(1.0);
    BOOST_CHECK_SMALL(buf.get_std_dev() - 0.707107, 0.000001);
    buf.add_sample(2.0);
    BOOST_CHECK_EQUAL(buf.get_std_dev(), 1.0);

    // Overwrite the buffer to check the current average.
    buf.add_sample(3.0);
    BOOST_CHECK_EQUAL(buf.get_std_dev(), 1.0);
    buf.add_sample(5.0);
    BOOST_CHECK_SMALL(buf.get_std_dev() - 1.527525, 0.000001);
}


// This file is part of libigl, a simple c++ geometry processing library.
//
// Copyright (C) 2017 Zhongshi Jiang <zhongshi@cims.nyu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla Public License
// v. 2.0. If a copy of the MPL was not distributed with this file, You can
// obtain one at http://mozilla.org/MPL/2.0/.
#include "tetrahedron_tetrahedron_adjacency.h"
#include "igl/all_edges.h"
#include "igl/unique_simplices.h"
#include "igl/parallel_for.h"
#include "igl/unique_edge_map.h"
#include "igl/slice.h"
#include <Eigen/Core>
#include <algorithm>
#include <iostream>
#include <array>
namespace igl {
namespace dev {
struct tetrahedron_tetrahedron_adjacency_tet_cell {
  int v1;
  int v2;
  int v3;
  int t;
  int f;
};

// Extract the face adjacencies
template<typename DerivedT, typename TTT_type, typename DerivedTT,
         typename DerivedTTif, typename DerivedTTie>
IGL_INLINE void tetrahedron_tetrahedron_adjacency_extractTTi(
    const std::vector<DerivedT> &F,
    std::vector<TTT_type> &TTT,
    std::vector<DerivedTT> &TT,
    std::vector<DerivedTTif> &TTif,
    std::vector<DerivedTTie> &TTie) {

  TT = std::vector<DerivedTT>(F.size(), -DerivedTT::Ones());
  TTif = std::vector<DerivedTTif>(F.size(), -DerivedTTif::Ones());
  TTie = std::vector<DerivedTTie>(F.size(), -DerivedTTie::Ones());

  for (int i = 1; i < (int) TTT.size(); ++i) {
    auto &r1 = TTT[i - 1];
    auto &r2 = TTT[i];

    // if get same face.
    if ((r1.v1 == r2.v1) && (r1.v2 == r2.v2) && (r1.v3 == r2.v3)) {
      TT[r1.t][r1.f] = r2.t;
      TT[r2.t][r2.f] = r1.t;

      // inverse for face
      TTif[r1.t][r1.f] = r2.f;
      TTif[r2.t][r2.f] = r1.f;

      // inverse for edge
      Eigen::RowVector3i f1v, f2v;
      for (int i = 0; i < 3; i++) {
        f1v[i] = F[r1.t][tetrahedron_local_FF(r1.f, i)];
        f2v[i] = F[r2.t][tetrahedron_local_FF(r2.f, i)];
      }

      std::vector<int> emap = {0, 1, 2};
      for (int e = 0; e < 3; e++)
        for (int i = 0; i < 3 - e; i++)
          if (f1v(e) == f2v(emap[i])) {
            TTie[r1.t](r1.f , (e + 2) % 3) = emap[i];
            TTie[r2.t](r2.f , (emap[i] + 2) % 3) = e;
            emap[i] = emap[2 - e]; // fill with the last
            break;
          }

    } //if
  }
}

template<typename DerivedT, typename TTT_type>
IGL_INLINE void tetrahedron_tetrahedron_adjacency_preprocess(
    const std::vector<DerivedT> &F,
    std::vector<TTT_type> &TTT) {
  for (int t = 0; t < (int) F.size(); ++t)
    for (int f = 0; f < 4; ++f) {
      std::array<int, 3> v;
      for (int i = 0; i < 3; ++i) {
        v[i] = F[t][tetrahedron_local_FF(f, i)];
      }
      std::sort(v.begin(), v.end());
      TTT.push_back({v[0], v[1], v[2], t, f});
    }
  std::sort(TTT.begin(), TTT.end(),
            [](const tetrahedron_tetrahedron_adjacency_tet_cell &r1,
               const tetrahedron_tetrahedron_adjacency_tet_cell &r2) {
              return std::tie(r1.v1, r1.v2, r1.v3, r1.t) <
                  std::tie(r2.v1, r2.v2, r2.v3, r2.t);
            });
}
}
}

// Compute tetrahedron-tetrahedron adjacency with indices
template<typename DerivedT, typename DerivedTT,
         typename DerivedTTif,typename DerivedTTie>
IGL_INLINE void
igl::dev::tetrahedron_tetrahedron_adjacency(const std::vector<DerivedT> &F,
                                       std::vector<DerivedTT> &TT,
                                       std::vector<DerivedTTif> &TTif,
                                       std::vector<DerivedTTie> &TTie)
{
  std::vector<igl::dev::tetrahedron_tetrahedron_adjacency_tet_cell> TTT;
  tetrahedron_tetrahedron_adjacency_preprocess(F, TTT);
  tetrahedron_tetrahedron_adjacency_extractTTi(F, TTT, TT, TTif, TTie);
}

template <typename DerivedT, typename DerivedTT>
IGL_INLINE void igl::dev::tetrahedron_tetrahedron_adjacency(
    const std::vector<DerivedT>& F,
    std::vector<DerivedTT>& TT)
{
  std::vector<igl::dev::tetrahedron_tetrahedron_adjacency_tet_cell> TTT;
  tetrahedron_tetrahedron_adjacency_preprocess(F, TTT);
  std::vector<DerivedTT> TTif;
  std::vector<Eigen::Matrix<int,4,3>>TTie;
  tetrahedron_tetrahedron_adjacency_extractTTi(F,TTT,TT,TTif,TTie);
}

#ifdef IGL_STATIC_LIBRARY
// Explicit template instantiation
template void igl::dev::tetrahedron_tetrahedron_adjacency<Eigen::Matrix<int, 1, 4, 1, 1, 4>, Eigen::Matrix<int, 1, 4, 1, 1, 4>, Eigen::Matrix<int, 1, 4, 1, 1, 4>, Eigen::Matrix<int, 4, 3, 0, 4, 3> >(std::__1::vector<Eigen::Matrix<int, 1, 4, 1, 1, 4>, std::__1::allocator<Eigen::Matrix<int, 1, 4, 1, 1, 4> > > const&, std::__1::vector<Eigen::Matrix<int, 1, 4, 1, 1, 4>, std::__1::allocator<Eigen::Matrix<int, 1, 4, 1, 1, 4> > >&, std::__1::vector<Eigen::Matrix<int, 1, 4, 1, 1, 4>, std::__1::allocator<Eigen::Matrix<int, 1, 4, 1, 1, 4> > >&, std::__1::vector<Eigen::Matrix<int, 4, 3, 0, 4, 3>, std::__1::allocator<Eigen::Matrix<int, 4, 3, 0, 4, 3> > >&);
#endif


#include <fstream>
#include <tuple>
#include <chrono>

#include <ros/ros.h>
#include <geometry_msgs/Pose.h>
#include <tf_conversions/tf_eigen.h>
#include <eigen_conversions/eigen_kdl.h>
#include <kdl_conversions/kdl_msg.h>

#include <Eigen/Eigen>

#include <rovi_utils/rovi_utils.h>
#include <rovi_planner/rovi_planner.h>

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

	using namespace rovi_utils;
	using namespace std::chrono_literals;

	ros::init(argc, argv, "interpolation_kdl");
	ros::NodeHandle nh;

	std::vector<geometry_msgs::Pose> waypoints =
	{
		rovi_utils::make_pose({ 0.0, 0.0, 0.0 }, { 0, 0, 0 }),
		rovi_utils::make_pose({ 1.0, 0.0, 0.0 }, { 0, 0, 0 }),
		rovi_utils::make_pose({ 2.0, 1.0, 0.0 }, { 0, 0, 0 }),
		rovi_utils::make_pose({ 1.0, 2.0, 0.0 }, { 0, 0, 0 }),
	};

	double VEL_MAX = 0.2, ACC_MAX = 0.05, EQUIV_RADIUS = 0.01, PAR_RADIUS = 0.2;

	auto traj_lin  = rovi_planner::traj_linear(waypoints, VEL_MAX, ACC_MAX, EQUIV_RADIUS);
	// auto traj_lin2 = rovi_planner::traj_linear2(waypoints, VEL_MAX, ACC_MAX, EQUIV_RADIUS);
	auto traj_par  = rovi_planner::traj_parabolic(waypoints, VEL_MAX, ACC_MAX, PAR_RADIUS, EQUIV_RADIUS);

	// export to files
	const std::string dir = get_experiment_dir("rovi_system");

	rovi_utils::export_traj(traj_lin, dir + "/traj_lin.csv", 0.01);
	rovi_utils::export_traj(traj_par, dir + "/traj_par.csv", 0.01);

	// export durations
	std::ofstream file(dir + "/dur.txt", std::ofstream::out);
	file << traj_lin->Duration()  << std::endl;
	file << traj_par->Duration()  << std::endl;
	file.close();

	return 0;
}


// Copyright 2015. 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.
//
// 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.
//
// Author: ryan.latture@gmail.com (Ryan Latture)

#include <Eigen/LU>
#ifdef EIGEN_USE_MKL_ALL
#include <Eigen/PardisoSupport>
#elif defined EIGEN_SUPERLU_SUPPORT
#include <Eigen/SuperLUSupport>
#endif
#include <boost/format.hpp>
#include <chrono>
#include <cmath>
#include <exception>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>

#include "threed_beam_fea.h"

namespace fea {

namespace {
void writeStringToTxt(std::string filename, std::string data) {
  std::ofstream output_file;
  output_file.open(filename);

  if (!output_file.is_open()) {
    throw std::runtime_error(
        (boost::format("Error opening file %s.") % filename).str());
  }
  output_file << data;
  output_file.close();
}
} // namespace

inline double norm(const Node &n1, const Node &n2) {
  const Node dn = n2 - n1;
  return dn.norm();
}

void GlobalStiffAssembler::calcKelem(unsigned int i, const Job &job) {
  // extract element properties
  const double EA = job.props[i].EA;   // Young's modulus * cross area
  const double EIz = job.props[i].EIz; // Young's modulus* I3
  const double EIy = job.props[i].EIy; // Young's modulus* I2
  const double GJ = job.props[i].GJ;

  // store node indices of current element
  const int nn1 = job.elems[i][0];
  const int nn2 = job.elems[i][1];

  // calculate the length of the element
  const double length = norm(job.nodes[nn1], job.nodes[nn2]);

  // store the entries in the (local) elemental stiffness matrix as temporary
  // values to avoid recalculation
  const double tmpEA = EA / length;
  const double tmpGJ = GJ / length;

  const double tmp12z = 12.0 * EIz / (length * length * length); // ==k3
  const double tmp6z = 6.0 * EIz / (length * length);
  const double tmp1z = EIz / length;

  const double tmp12y = 12.0 * EIy / (length * length * length);
  const double tmp6y = 6.0 * EIy / (length * length);
  const double tmp1y = EIy / length;

  // update local elemental stiffness matrix
  Klocal(0, 0) = tmpEA;
  Klocal(0, 6) = -tmpEA;
  Klocal(1, 1) = tmp12z;
  Klocal(1, 5) = tmp6z;
  Klocal(1, 7) = -tmp12z;
  Klocal(1, 11) = tmp6z;
  Klocal(2, 2) = tmp12y;
  Klocal(2, 4) = -tmp6y;
  Klocal(2, 8) = -tmp12y;
  Klocal(2, 10) = -tmp6y;
  Klocal(3, 3) = tmpGJ;
  Klocal(3, 9) = -tmpGJ;
  Klocal(4, 2) = -tmp6y;
  Klocal(4, 4) = 4.0 * tmp1y;
  Klocal(4, 8) = tmp6y;
  Klocal(4, 10) = 2.0 * tmp1y;
  Klocal(5, 1) = tmp6z;
  Klocal(5, 5) = 4.0 * tmp1z;
  Klocal(5, 7) = -tmp6z;
  Klocal(5, 11) = 2.0 * tmp1z;
  Klocal(6, 0) = -tmpEA;
  Klocal(6, 6) = tmpEA;
  Klocal(7, 1) = -tmp12z;
  Klocal(7, 5) = -tmp6z;
  Klocal(7, 7) = tmp12z;
  Klocal(7, 11) = -tmp6z;
  Klocal(8, 2) = -tmp12y;
  Klocal(8, 4) = tmp6y;
  Klocal(8, 8) = tmp12y;
  Klocal(8, 10) = tmp6y;
  Klocal(9, 3) = -tmpGJ;
  Klocal(9, 9) = tmpGJ;
  Klocal(10, 2) = -tmp6y;
  Klocal(10, 4) = 2.0 * tmp1y;
  Klocal(10, 8) = tmp6y;
  Klocal(10, 10) = 4.0 * tmp1y;
  Klocal(11, 1) = tmp6z;
  Klocal(11, 5) = 2.0 * tmp1z;
  Klocal(11, 7) = -tmp6z;
  Klocal(11, 11) = 4.0 * tmp1z;

  // calculate unit normal vector along local x-direction
  const Eigen::Vector3d nx = (job.nodes[nn2] - job.nodes[nn1]).normalized();
  // calculate unit normal vector along y-direction
  const Eigen::Vector3d ny = job.props[i].normal_vec.normalized();
  // calculate the unit normal vector in local z direction
  const Eigen::Vector3d nz = nx.cross(ny).normalized();
  RotationMatrix r;
  r.row(0) = nx;
  r.row(1) = ny;
  r.row(2) = nz;
  // update rotation matrices
  calcAelem(r);
  AelemT = Aelem.transpose();

  // update Kelem
  Kelem = AelemT * Klocal * Aelem;
  LocalMatrix KlocalAelem;
  KlocalAelem = Klocal * Aelem;
  perElemKlocalAelem[i] = KlocalAelem;
};

void GlobalStiffAssembler::calcAelem(const RotationMatrix &r) {
  // update rotation matrix
  Aelem.block<3, 3>(0, 0) = r;
  Aelem.block<3, 3>(3, 3) = r;
  Aelem.block<3, 3>(6, 6) = r;
  Aelem.block<3, 3>(9, 9) = r;
}

std::vector<LocalMatrix> GlobalStiffAssembler::getPerElemKlocalAelem() const {
  return perElemKlocalAelem;
};

void GlobalStiffAssembler::operator()(SparseMat &Kg, const Job &job,
                                      const std::vector<Tie> &ties) {
  int nn1, nn2;
  unsigned int row, col;
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;

  // form vector to hold triplets that will be used to assemble global stiffness
  // matrix
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(40 * job.elems.size() + 4 * dofs_per_elem * ties.size());

  perElemKlocalAelem.resize(job.elems.size());
  for (unsigned int i = 0; i < job.elems.size(); ++i) {
    // update Kelem with current elemental stiffness matrix
    calcKelem(i, job); // 12x12 matrix

    // get sparse representation of the current elemental stiffness matrix
    SparseKelem = Kelem.sparseView();

    // pull out current node indices
    nn1 = job.elems[i][0];
    nn2 = job.elems[i][1];

    for (unsigned int j = 0; j < SparseKelem.outerSize(); ++j) {
      for (SparseMat::InnerIterator it(SparseKelem, j); it; ++it) {
        row = it.row();
        col = it.col();

        // check position in local matrix and update corresponding global
        // position
        if (row < 6) {
          // top left
          if (col < 6) {
            triplets.push_back(Eigen::Triplet<double>(dofs_per_elem * nn1 + row,
                                                      dofs_per_elem * nn1 + col,
                                                      it.value()));
          }
          // top right
          else {
            triplets.push_back(Eigen::Triplet<double>(
                dofs_per_elem * nn1 + row, dofs_per_elem * (nn2 - 1) + col,
                it.value())); // I: Giati nn2-1 kai oxi nn2?
          }
        } else {
          // bottom left
          if (col < 6) {
            triplets.push_back(Eigen::Triplet<double>(
                dofs_per_elem * (nn2 - 1) + row, dofs_per_elem * nn1 + col,
                it.value())); // I: Giati nn2-1 kai oxi nn2? Epeidi ta nn2 ston
                              // Kelem exoun idi offset 6
          }
          // bottom right
          else {
            triplets.push_back(Eigen::Triplet<double>(
                dofs_per_elem * (nn2 - 1) + row,
                dofs_per_elem * (nn2 - 1) + col, it.value()));
          }
        }
      }
    }
  }

  loadTies(triplets, ties);

  Kg.setFromTriplets(triplets.begin(), triplets.end());
};

void loadBCs(SparseMat &Kg, SparseMat &force_vec, const std::vector<BC> &BCs,
             unsigned int num_nodes) {
  unsigned int bc_idx;
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;
  // calculate the index that marks beginning of Lagrange multiplier
  // coefficients
  const unsigned int global_add_idx = dofs_per_elem * num_nodes;

  for (size_t i = 0; i < BCs.size(); ++i) {
    bc_idx = dofs_per_elem * BCs[i].node + BCs[i].dof;

    // update global stiffness matrix
    Kg.insert(bc_idx, global_add_idx + i) = 1;
    Kg.insert(global_add_idx + i, bc_idx) = 1;

    // update force vector. All values are already zero. Only update if BC if
    // non-zero.
    if (std::abs(BCs[i].value) > std::numeric_limits<double>::epsilon()) {
      force_vec.insert(global_add_idx + i, 0) = BCs[i].value;
    }
  }
};

void loadEquations(SparseMat &Kg, const std::vector<Equation> &equations,
                   unsigned int num_nodes, unsigned int num_bcs) {
  size_t row_idx, col_idx;
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;
  const unsigned int global_add_idx = dofs_per_elem * num_nodes + num_bcs;

  for (size_t i = 0; i < equations.size(); ++i) {
    row_idx = global_add_idx + i;
    for (size_t j = 0; j < equations[i].terms.size(); ++j) {
      col_idx = dofs_per_elem * equations[i].terms[j].node_number +
                equations[i].terms[j].dof;
      Kg.insert(row_idx, col_idx) = equations[i].terms[j].coefficient;
      Kg.insert(col_idx, row_idx) = equations[i].terms[j].coefficient;
    }
  }
};

void loadTies(std::vector<Eigen::Triplet<double>> &triplets,
              const std::vector<Tie> &ties) {
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;
  unsigned int nn1, nn2;
  double lmult, rmult, spring_constant;

  for (size_t i = 0; i < ties.size(); ++i) {
    nn1 = ties[i].node_number_1;
    nn2 = ties[i].node_number_2;
    lmult = ties[i].lmult;
    rmult = ties[i].rmult;

    for (unsigned int j = 0; j < dofs_per_elem; ++j) {
      // first 3 DOFs are linear DOFs, second 2 are rotational, last is
      // torsional
      spring_constant = j < 3 ? lmult : rmult;

      triplets.push_back(Eigen::Triplet<double>(
          dofs_per_elem * nn1 + j, dofs_per_elem * nn1 + j, spring_constant));

      triplets.push_back(Eigen::Triplet<double>(
          dofs_per_elem * nn2 + j, dofs_per_elem * nn2 + j, spring_constant));

      triplets.push_back(Eigen::Triplet<double>(
          dofs_per_elem * nn1 + j, dofs_per_elem * nn2 + j, -spring_constant));

      triplets.push_back(Eigen::Triplet<double>(
          dofs_per_elem * nn2 + j, dofs_per_elem * nn1 + j, -spring_constant));
    }
  }
};

std::vector<std::vector<double>>
computeTieForces(const std::vector<Tie> &ties,
                 const std::vector<std::vector<double>> &nodal_displacements) {
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;
  unsigned int nn1, nn2;
  double lmult, rmult, spring_constant, delta1, delta2;

  std::vector<std::vector<double>> tie_forces(
      ties.size(), std::vector<double>(dofs_per_elem));

  for (size_t i = 0; i < ties.size(); ++i) {
    nn1 = ties[i].node_number_1;
    nn2 = ties[i].node_number_2;
    lmult = ties[i].lmult;
    rmult = ties[i].rmult;

    for (unsigned int j = 0; j < dofs_per_elem; ++j) {
      // first 3 DOFs are linear DOFs, second 2 are rotational, last is
      // torsional
      spring_constant = j < 3 ? lmult : rmult;
      delta1 = nodal_displacements[nn1][j];
      delta2 = nodal_displacements[nn2][j];
      tie_forces[i][j] = spring_constant * (delta2 - delta1);
    }
  }
  return tie_forces;
}

void loadForces(SparseMat &force_vec, const std::vector<Force> &forces) {
  const unsigned int dofs_per_elem = DOF::NUM_DOFS;
  unsigned int idx;

  for (size_t i = 0; i < forces.size(); ++i) {
    idx = dofs_per_elem * forces[i].node + forces[i].dof;
    force_vec.insert(idx, 0) = forces[i].value;
  }
};

Summary solve(const Job &job, const std::vector<BC> &BCs,
              const std::vector<Force> &forces, const std::vector<Tie> &ties,
              const std::vector<Equation> &equations, const Options &options) {
  auto initial_start_time = std::chrono::high_resolution_clock::now();

  Summary summary;
  summary.num_nodes = job.nodes.size();
  summary.num_elems = job.elems.size();
  summary.num_bcs = BCs.size();
  summary.num_ties = ties.size();

  const unsigned int dofs_per_elem = DOF::NUM_DOFS;

  // calculate size of global stiffness matrix and force vector
  const unsigned long size =
      dofs_per_elem * job.nodes.size() + BCs.size() + equations.size();

  // construct global stiffness matrix and force vector
  SparseMat Kg(size, size);
  SparseMat force_vec(size, 1);
  force_vec.reserve(forces.size() + BCs.size());

  // construct global assembler object and assemble global stiffness matrix
  auto start_time = std::chrono::high_resolution_clock::now();
  GlobalStiffAssembler assembleK3D = GlobalStiffAssembler();
  assembleK3D(Kg, job, ties);
  auto end_time = std::chrono::high_resolution_clock::now();
  auto delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(
                        end_time - start_time)
                        .count();
  summary.assembly_time_in_ms = delta_time;

  if (options.verbose)
    std::cout << "Global stiffness matrix assembled in " << delta_time
              << " ms.\nNow preprocessing factorization..." << std::endl;

#ifdef DEBUG_FILE
  unsigned int numberOfDoF = DOF::NUM_DOFS * job.nodes.size();
  Eigen::MatrixXd KgNoBCDense(Kg.block(0, 0, numberOfDoF, numberOfDoF));
  std::ofstream kgNoBCFile("KgNoBC.csv");
  if (kgNoBCFile.is_open()) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                           Eigen::DontAlignCols, ", ", "\n");
    kgNoBCFile << KgNoBCDense.format(CSVFormat) << '\n';
    kgNoBCFile.close();
  }
//    std::cout << KgNoBCDense << std::endl;
#endif
  // load prescribed boundary conditions into stiffness matrix and force vector
  loadBCs(Kg, force_vec, BCs, job.nodes.size());

  if (equations.size() > 0) {
    loadEquations(Kg, equations, job.nodes.size(), BCs.size());
  }

  // load prescribed forces into force vector
  if (forces.size() > 0) {
    loadForces(force_vec, forces);
  }

//  Kg.prune(1.e-14);
  Kg.makeCompressed();
#ifdef DEBUG_FILE
  Eigen::MatrixXd KgDense(Kg);
  //    std::cout << KgDense << std::endl;
  Eigen::VectorXd forcesVectorDense(force_vec);
//    std::cout << forcesVectorDense << std::endl;
  // compress global stiffness matrix since all non-zero values have been added.
  std::ofstream kgFile("Kg.csv");
  if (kgFile.is_open()) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                           Eigen::DontAlignCols, ", ", "\n");
    kgFile << KgDense.format(CSVFormat) << '\n';
    kgFile.close();
  }
  std::ofstream kgFileTxt("Kg.TXT");
  if (kgFileTxt.is_open()) {
    kgFileTxt << Kg << '\n';
    kgFileTxt.close();
  }
  std::ofstream forcesFile("forces.csv");
  if (forcesFile.is_open()) {
    const static Eigen::IOFormat CSVFormat(Eigen::StreamPrecision,
                                           Eigen::DontAlignCols, ", ", "\n");
    forcesFile << forcesVectorDense.format(CSVFormat) << '\n';
    forcesFile.close();
  }
#endif
  // initialize solver based on whether MKL should be used
#ifdef EIGEN_USE_MKL
  Eigen::PardisoLU<SparseMat> solver;
#elif defined EIGEN_SUPERLU_SUPPORT
  Eigen::SuperLU<SparseMat> solver;
#else
  Eigen::SparseLU<SparseMat> solver;
#endif

  // Compute the ordering permutation vector from the structural pattern of Kg
  start_time = std::chrono::high_resolution_clock::now();
  solver.analyzePattern(Kg);
  end_time = std::chrono::high_resolution_clock::now();
  delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                                     start_time)
                   .count();

  summary.preprocessing_time_in_ms = delta_time;

  if (options.verbose)
    std::cout << "Preprocessing step of factorization completed in "
              << delta_time
              << " ms.\nNow factorizing global stiffness matrix..."
              << std::endl;

  // Compute the numerical factorization
  start_time = std::chrono::high_resolution_clock::now();
  solver.factorize(Kg);
//  std::cout << solver.lastErrorMessage() << std::endl;
  assert(solver.info() == Eigen::Success);
  end_time = std::chrono::high_resolution_clock::now();

  delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                                     start_time)
                   .count();
  summary.factorization_time_in_ms = delta_time;

  if (options.verbose)
    std::cout << "Factorization completed in " << delta_time
              << " ms. Now solving system..." << std::endl;

  // Use the factors to solve the linear system
  start_time = std::chrono::high_resolution_clock::now();
  SparseMat dispSparse = solver.solve(force_vec);
  end_time = std::chrono::high_resolution_clock::now();
  delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                                     start_time)
                   .count();

  summary.solve_time_in_ms = delta_time;

  if (options.verbose)
    std::cout << "System was solved in " << delta_time << " ms.\n" << std::endl;

  // convert to dense matrix
  Eigen::VectorXd disp(dispSparse);

  // convert from Eigen vector to std vector
  std::vector<std::vector<double>> disp_vec(job.nodes.size(),
                                            std::vector<double>(dofs_per_elem));
  for (size_t i = 0; i < disp_vec.size(); ++i) {
    for (unsigned int j = 0; j < dofs_per_elem; ++j)
      // round all values close to 0.0
      disp_vec[i][j] = std::abs(disp(dofs_per_elem * i + j)) < options.epsilon
                           ? 0.0
                           : disp(dofs_per_elem * i + j);
  }
  summary.nodal_displacements = disp_vec;

  // [calculate nodal forces
  start_time = std::chrono::high_resolution_clock::now();

  Kg = Kg.topLeftCorner(dofs_per_elem * job.nodes.size(),
                        dofs_per_elem * job.nodes.size());
  dispSparse = dispSparse.topRows(dofs_per_elem * job.nodes.size());

  SparseMat nodal_forces_sparse = Kg * dispSparse;

  Eigen::VectorXd nodal_forces_dense(nodal_forces_sparse);

  std::vector<std::vector<double>> nodal_forces_vec(
      job.nodes.size(), std::vector<double>(dofs_per_elem));
  for (size_t i = 0; i < nodal_forces_vec.size(); ++i) {
    for (unsigned int j = 0; j < dofs_per_elem; ++j)
      // round all values close to 0.0
      nodal_forces_vec[i][j] =
          std::abs(nodal_forces_dense(dofs_per_elem * i + j)) < options.epsilon
              ? 0.0
              : nodal_forces_dense(dofs_per_elem * i + j);
  }
  summary.nodal_forces = nodal_forces_vec;

  end_time = std::chrono::high_resolution_clock::now();

  summary.nodal_forces_solve_time_in_ms =
      std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                            start_time)
          .count();
  //]

  // [ calculate forces associated with ties
  if (ties.size() > 0) {
    start_time = std::chrono::high_resolution_clock::now();
    summary.tie_forces = computeTieForces(ties, disp_vec);
    end_time = std::chrono::high_resolution_clock::now();
    summary.tie_forces_solve_time_in_ms =
        std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                              start_time)
            .count();
  }
  // ]

  // [save files specified in options
  CSVParser csv;
  start_time = std::chrono::high_resolution_clock::now();
  if (options.save_nodal_displacements) {
    std::cout << "Writing to:" + options.nodal_displacements_filename
              << std::endl;
    csv.write(options.nodal_displacements_filename, disp_vec,
              options.csv_precision, options.csv_delimiter);
  }

  if (options.save_nodal_forces) {
    csv.write(options.nodal_forces_filename, nodal_forces_vec,
              options.csv_precision, options.csv_delimiter);
  }

  if (options.save_tie_forces) {
    csv.write(options.tie_forces_filename, summary.tie_forces,
              options.csv_precision, options.csv_delimiter);
  }

  end_time = std::chrono::high_resolution_clock::now();
  summary.file_save_time_in_ms =
      std::chrono::duration_cast<std::chrono::milliseconds>(end_time -
                                                            start_time)
          .count();
  // ]

  auto final_end_time = std::chrono::high_resolution_clock::now();

  delta_time = std::chrono::duration_cast<std::chrono::milliseconds>(
                   final_end_time - initial_start_time)
                   .count();
  summary.total_time_in_ms = delta_time;

  if (options.save_report) {
    writeStringToTxt(options.report_filename, summary.FullReport());
  }

  if (options.verbose)
    std::cout << summary.FullReport();

  // Compute per element forces
  summary.element_forces.resize(job.elems.size(),
                                std::vector<double>(2 * DOF::NUM_DOFS));
  std::vector<LocalMatrix> perElemKlocalAelem =
      assembleK3D.getPerElemKlocalAelem();

  for (size_t elemIndex = 0; elemIndex < job.elems.size(); elemIndex++) {
    // Assemble the displacements of the nodes of the beam
    Eigen::Matrix<double, 12, 1> elemDisps;
    // Displacements of the first node of the beam
    const size_t nodeIndex0 = job.elems[elemIndex](0);
    elemDisps(0) = summary.nodal_displacements[nodeIndex0][0];
    elemDisps(1) = summary.nodal_displacements[nodeIndex0][1];
    elemDisps(2) = summary.nodal_displacements[nodeIndex0][2];
    elemDisps(3) = summary.nodal_displacements[nodeIndex0][3];
    elemDisps(4) = summary.nodal_displacements[nodeIndex0][4];
    elemDisps(5) = summary.nodal_displacements[nodeIndex0][5];
    // Displacemen ts of the second node of the beam
    const size_t nodeIndex1 = job.elems[elemIndex](1);
    elemDisps(6) = summary.nodal_displacements[nodeIndex1][0];
    elemDisps(7) = summary.nodal_displacements[nodeIndex1][1];
    elemDisps(8) = summary.nodal_displacements[nodeIndex1][2];
    elemDisps(9) = summary.nodal_displacements[nodeIndex1][3];
    elemDisps(10) = summary.nodal_displacements[nodeIndex1][4];
    elemDisps(11) = summary.nodal_displacements[nodeIndex1][5];

    Eigen::Matrix<double, 12, 1> elemForces =
        perElemKlocalAelem[elemIndex] * elemDisps;
    for (int dofIndex = 0; dofIndex < DOF::NUM_DOFS; dofIndex++) {
      summary.element_forces[elemIndex][static_cast<size_t>(dofIndex)] =
          -elemForces(dofIndex);
    }
    // meaning of sign = reference to first node:
    // + = compression for axial, - traction
    for (int dofIndex = DOF::NUM_DOFS; dofIndex < 2 * DOF::NUM_DOFS;
         dofIndex++) {
      summary.element_forces[elemIndex][static_cast<size_t>(dofIndex)] =
          +elemForces(dofIndex);
    }
  }

  if (options.save_elemental_forces) {
    std::cout << "Writing to:" + options.elemental_forces_filename << std::endl;
    csv.write(options.elemental_forces_filename, summary.element_forces,
              options.csv_precision, options.csv_delimiter);
  }

  return summary;
};

} // namespace fea


#include <iostream>
#include <vector>
#include <random>

#include <Eigen/Dense>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/types_c.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/opencv.hpp>

#include "Image/Error.hpp"
#include "Image/Image.hpp"

namespace pcv
{

} // end namespace pcv


/*CHOMP class definition
 *
 * Copyright (C) 2016 Rafael Valencia. All rights reserved.
 * License (3-Cluase BSD): https://github.com/rafaelvalencia
 *
 * This code uses and is based on code from:
 *   Project: trychomp https://github.com/poftwaresatent/trychomp
 *   Copyright (C) 2014 Roland Philippsen. All rights reserved.
 *   License (3-Clause BSD) : https://github.com/poftwaresatent/trychomp
 * **

/**
   \file chomp-class.cpp

   Interactive demo with CHOMP class for point vehicles moving
   holonomously in the plane.  There is a fixed start and goal
   configuration, and you can drag a circular obstacle around to see
   how the CHOMP algorithm reacts to that.
*/

#include "gfx.hpp"
#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <stdlib.h>
#include <sys/time.h>
#include <err.h>
#include "path_adaptor.hpp"

typedef Eigen::VectorXd Vector;
typedef Eigen::MatrixXd Matrix;
typedef Eigen::Isometry3d Transform;

static size_t const obs_dim(3); 	// obstacle dimensions (x,y,Radius)

using namespace std;

//////////////////////////////////////////////////
// trajectory etc
Vector trajectory;				// the trajectory (q_1, q_2, ...q_n)
Matrix  obs(obs_dim,2);   		// Matrix containninging all obstacles, each column is (x,y,R) of the obstacle
Vector qs;						// the start config a.k.a. q_0
Vector qe;						// the end config a.k.a. q_(n+1)
static size_t const cdim (2);	// dimension of config space
static size_t const nq (20);	// number of q stacked into xi
static size_t const xidim (nq * cdim); // dimension of trajectory, xidim = nq * cdim
//////////////////////////////////////////////////
// CHOMP
CHOMP chomp(1.0, 100.0, 1.0, nq, cdim, 1000, 10.0);

/* CHOMP parameters
dt     = 1.0 	(time step)
eta    = 100.0 	(regularization factor for gradient descent)
lambda = 1.0	(weight of smoothness objective)
nq     = 20		(number of poses q in xi)
cdim   = 2		(dimension of config space)
numIt  = 1000	(number of iterations)
gain   = 10.0	(Gain inside cost function)
*/

//////////////////////////////////////////////////
// gui stuff

enum { PAUSE, STEP, RUN } state;

struct handle_s {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  handle_s (double px, double py, double radius, double red, double green, double blue, double alpha)
    : point_(2),
      radius_(radius),
      red_(red),
      green_(green),
      blue_(blue),
      alpha_(alpha)
  {
    point_ << px, py;
  }

  Vector point_;
  double radius_, red_, green_, blue_, alpha_;
};

// Set two obstacles
static handle_s rep1 (3.0, 0.0,   2.0, 0.0, 0.0, 1.0, 0.2);
static handle_s rep2 (0.0, 3.0,   2.0, 0.0, 0.5, 1.0, 0.2);

static handle_s * handle[] = { &rep1, &rep2, 0 };
static handle_s * grabbed (0);
static Vector grab_offset (3);

//////////////////////////////////////////////////
// robot (one per waypoint)

class Robot
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  Robot ()
    : position_ (Vector::Zero(2))
  {
  }

  void update (Vector const & position)
  {
    if (position.size() != 2) {
      errx (EXIT_FAILURE, "Robot::update(): position has %zu DOF (but needs 2)",
	    (size_t) position.size());
    }
    position_ = position;
  }

  void draw () const
  {
    // translucent disk for base
    gfx::set_pen (1.0, 0.7, 0.7, 0.7, 0.5);
    gfx::fill_arc (position_[0], position_[1], radius_, 0.0, 2.0 * M_PI);

    // thick circle outline for base
    gfx::set_pen (3.0, 0.2, 0.2, 0.2, 1.0);
    gfx::draw_arc (position_[0], position_[1], radius_, 0.0, 2.0 * M_PI);
  }

  static double const radius_;

  Vector position_;
};

double const Robot::radius_ (0.5);

Robot rstart;
Robot rend;
vector <Robot> robots;

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

static void update_robots ()
{
  rstart.update (qs);
  rend.update (qe);
  if (nq != robots.size())
  {
    robots.resize (nq);
  }
  for (size_t ii (0); ii < nq; ++ii)
  {
    robots[ii].update (trajectory.block (ii * cdim, 0, cdim, 1));
  }
}

static void cb_step ()
{
  state = STEP;
}

static void cb_run ()
{
  if (RUN == state) {
    state = PAUSE;
  }
  else {
    state = RUN;
  }
}

static void cb_jumble ()
{
  for (size_t ii (0); ii < xidim; ++ii)
  {
    trajectory[ii] = double (rand()) / (0.1 * numeric_limits<int>::max()) - 5.0;
  }
  //update new initial path
  chomp.setPath(qs, qe, trajectory);
  update_robots();
}

static void cb_idle ()
{
  if (PAUSE == state)
  {
    return;
  }
  if (STEP == state)
  {
    state = PAUSE;
  }

  //static size_t stepcounter (0);
  //cout << "step " << stepcounter++ << "\n";

  /////////////////////////////////////////////////
  // Get updated obstacles
    //cout << "Getting updated obstacles ..." << endl;
    int ii = 0;
	for (handle_s ** hh (handle); *hh != 0; ++hh)
	{
		if (ii < obs.cols())
		{
			Vector position ( (*hh)->point_ );
			obs(0,ii) = position(0);
			obs(1,ii) = position(1);
			obs(2,ii) = (*hh)->radius_;
			++ii;
		}
	}
  //cout << "OBSTACLES : \n" << obs << endl;

  //////////////////////////////////////////////////
  // One CHOMP iteration
  chomp.setObstacles(obs); //update obstacles
  chomp.chompIteration(trajectory); //update path

  update_robots ();
}

static void cb_draw ()
{
  //////////////////////////////////////////////////
  // set bounds

  Vector bmin (qs);
  Vector bmax (qs);
  for (size_t ii (0); ii < 2; ++ii) {
    if (qe[ii] < bmin[ii]) {
      bmin[ii] = qe[ii];
    }
    if (qe[ii] > bmax[ii]) {
      bmax[ii] = qe[ii];
    }
    for (size_t jj (0); jj < nq; ++jj) {
      if (trajectory[ii + cdim * jj] < bmin[ii]) {
	bmin[ii] = trajectory[ii + cdim * jj];
      }
      if (trajectory[ii + cdim * jj] > bmax[ii]) {
	bmax[ii] = trajectory[ii + cdim * jj];
      }
    }
  }

  gfx::set_view (bmin[0] - 2.0, bmin[1] - 2.0, bmax[0] + 2.0, bmax[1] + 2.0);

  //////////////////////////////////////////////////
  // robots

  rstart.draw();
  for (size_t ii (0); ii < robots.size(); ++ii) {
    robots[ii].draw();
  }
  rend.draw();

  //////////////////////////////////////////////////
  // trj

  gfx::set_pen (1.0, 0.2, 0.2, 0.2, 1.0);
  gfx::draw_line (qs[0], qs[1], trajectory[0], trajectory[1]);
  for (size_t ii (1); ii < nq; ++ii) {
    gfx::draw_line (trajectory[(ii-1) * cdim], trajectory[(ii-1) * cdim + 1], trajectory[ii * cdim], trajectory[ii * cdim + 1]);
  }
  gfx::draw_line (trajectory[(nq-1) * cdim], trajectory[(nq-1) * cdim + 1], qe[0], qe[1]);

  gfx::set_pen (5.0, 0.8, 0.2, 0.2, 1.0);
  gfx::draw_point (qs[0], qs[1]);
  gfx::set_pen (5.0, 0.5, 0.5, 0.5, 1.0);
  for (size_t ii (0); ii < nq; ++ii) {
    gfx::draw_point (trajectory[ii * cdim], trajectory[ii * cdim + 1]);
  }
  gfx::set_pen (5.0, 0.2, 0.8, 0.2, 1.0);
  gfx::draw_point (qe[0], qe[1]);

  //////////////////////////////////////////////////
  // handles

  for (handle_s ** hh (handle); *hh != 0; ++hh) {
    gfx::set_pen (1.0, (*hh)->red_, (*hh)->green_, (*hh)->blue_, (*hh)->alpha_);
    gfx::fill_arc ((*hh)->point_[0], (*hh)->point_[1], (*hh)->radius_, 0.0, 2.0 * M_PI);
  }
}

static void cb_mouse (double px, double py, int flags)
{
  if (flags & gfx::MOUSE_PRESS) {
    for (handle_s ** hh (handle); *hh != 0; ++hh) {
      Vector offset ((*hh)->point_);
      offset[0] -= px;
      offset[1] -= py;
      if (offset.norm() <= (*hh)->radius_) {
    	grab_offset = offset;
    	grabbed = *hh;
    	break;
      }
    }
  }
  else if (flags & gfx::MOUSE_DRAG) {
    if (0 != grabbed) {
      grabbed->point_[0] = px;
      grabbed->point_[1] = py;
      grabbed->point_ += grab_offset;
    }
  }
  else if (flags & gfx::MOUSE_RELEASE) {
    grabbed = 0;
  }
}

int main()
{
  struct timeval tt;
  gettimeofday (&tt, NULL);
  srand (tt.tv_usec);

  //Starting and ending points
  qs.resize (cdim);
  qs << -5.0, -5.0;
  qe.resize (cdim);
  qe << 7.0, 7.0;

  //Initialize path with all poses in Xi stacked in qs
  chomp.initStackedPath(qs, qe);
  chomp.getPath(trajectory);
  update_robots();
  state = PAUSE;

  gfx::add_button ("jumble", cb_jumble);
  gfx::add_button ("step", cb_step);
  gfx::add_button ("run", cb_run);
  gfx::main ("chomp", cb_idle, cb_draw, cb_mouse);
}


/**
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Lesser General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or  (at your option) any later version.
 *
 *  This program 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 General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 **/
/**
 * @file imgRotate
 * @ingroup converters
 * @author David Coeurjolly (\c david.coeurjolly@liris.cnrs.fr)
 *
 * @date 2015/03/24
 *
 *
 * This file is part of the DGtal library.
 */

#include <DGtal/base/Common.h>
#include "DGtal/io/readers/GenericReader.h"
#include "DGtal/io/writers/GenericWriter.h"
#include "DGtal/images/ImageContainerBySTLVector.h"
#include "DGtal/images/ImageSelector.h"
#include <DGtal/images/IntervalForegroundPredicate.h>
#include <boost/program_options/options_description.hpp>
#include <boost/program_options/parsers.hpp>
#include <boost/program_options/variables_map.hpp>

#include <DGtal/images/RigidTransformation2D.h>
#include <DGtal/images/ConstImageAdapter.h>
#include <vector>
#include <string>
#include <climits>

using namespace DGtal;

///////////////////////////////////////////////////////////////////////////////
namespace po = boost::program_options;

/**
 * Missing parameter error message.
 *
 * @param param
 */
void missingParam ( std::string param )
{
  trace.error() <<" Parameter: "<<param<<" is required..";
  trace.info() <<std::endl;
  exit ( 1 );
}

typedef ImageSelector < Z2i::Domain, unsigned char>::Type MyImage;

int main( int argc, char** argv )
{
  // parse command line ----------------------------------------------
  po::options_description general_opt("Allowed options are: ");
  general_opt.add_options()
    ("help,h", "display this message")
    ("input,i", po::value<std::string>(), "input image file name (any 2D image format accepted by DGtal::GenericReader)")
    ("output,o", po::value<std::string>(), "output image file name (any 2D image format accepted by DGtal::GenericWriter)")
    ("angle,a", po::value<double>()->default_value(0.0), "Angle in radian")  ;

  bool parseOK=true;
  po::variables_map vm;
  try{
    po::store(po::parse_command_line(argc, argv, general_opt), vm);
  }catch(const std::exception& ex){
    trace.info()<< "Error checking program options: "<< ex.what()<< std::endl;
    parseOK=false;
  }
  po::notify(vm);
  if(vm.count("help")||argc<=1|| !parseOK)
    {
      trace.info()<< "Rotate an image by a given angle a binary object with 0 values as background points and values >0 for the foreground ones." <<std::endl << "Basic usage: "<<std::endl
      << "\t imgRotate [options] --input <imageName> --output <outputImage> --angle 0.3"<<std::endl
      << general_opt << "\n";
      return 0;
    }

  //Parameters
  if ( ! ( vm.count ( "input" ) ) ) missingParam ( "--input" );
  const std::string input = vm["input"].as<std::string>();
  if ( ! ( vm.count ( "output" ) ) ) missingParam ( "--output" );
  const std::string  output = vm["output"].as<std::string>();
  const double angle = vm["angle"].as<double>();

  typedef functors::IntervalForegroundPredicate<MyImage> Binarizer;
  MyImage image = GenericReader<MyImage>::import( input );
  trace.info() <<"Input image: "<< image<<std::endl;

  typedef functors::BackwardRigidTransformation2D<Z2i::Space> RotateFunctor;
  Z2i::RealPoint center = image.domain().upperBound();
  center -= image.domain().lowerBound();
  center /= 2.0;

  RotateFunctor rotationFunctor(center,
                                angle,
                                Z2i::RealPoint(0.0,0.0));

  functors::Identity idD;
  typedef functors::DomainRigidTransformation2D<MyImage::Domain, RotateFunctor> MyDomainTransformer;
  MyDomainTransformer rotDomain(rotationFunctor);
  typedef MyDomainTransformer::Bounds Bounds;
  Bounds newdomain = rotDomain( image.domain());
  MyImage::Domain transformedDomain ( newdomain.first, newdomain.second );
  typedef ConstImageAdapter<MyImage, MyImage::Domain, RotateFunctor, MyImage::Value, functors::Identity > MyImageBackwardAdapter;
  MyImageBackwardAdapter backwardImageAdapter ( image, transformedDomain , rotationFunctor, idD );

  backwardImageAdapter >> output;

  return 0;
}


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<int, 2, Eigen::Dynamic> test_int_2_D(const Eigen::Matrix<int, 2, Eigen::Dynamic>& M) { return M; }
void export_int_2_D() { boost::python::def("test_int_2_D", test_int_2_D); }


/*-----------------------------------------------------------------------------+
Copyright (c) 2010-2010: Joachim Faulhaber
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/
#ifndef BOOST_ICL_PREDICATES_SUB_SUPER_SET_HPP_JOFA_101102
#define BOOST_ICL_PREDICATES_SUB_SUPER_SET_HPP_JOFA_101102

#include <boost/icl/type_traits/predicate.hpp>
#include <boost/icl/type_traits/type_to_string.hpp>

namespace boost{namespace icl
{

    /// Functor class template contained_in implements the subset relation.
    template<class Type>
    struct sub_super_set : public relation<Type,Type>
    {
        /// Apply the subset relation.
        /** <tt>contained_in(sub, super)</tt> is true if <tt>sub</tt>
            is contained in <tt>super</tt> */
        bool operator()(const Type& sub, const Type& super)const
        {
            return contains(super, sub);
        }
    };

    template<>
    inline std::string unary_template_to_string<icl::sub_super_set>::apply()
    { return "C="; }

    /// Functor class template <b>contains</b> implements the superset relation.
    template<class Type>
    struct super_sub_set : public relation<Type,Type>
    {
        /// Apply the superset relation.
        /** <tt>contains(super, sub)</tt> is true if <tt>super</tt> containes
            <tt>sub</tt> */
        bool operator()(const Type& super, const Type& sub)const
        {
            return contains(super, sub);
        }
    };

    template<>
    inline std::string unary_template_to_string<icl::super_sub_set>::apply()
    { return "D="; }

}} // namespace icl boost

#endif


#include <svgpp/parser/grammar/clock_value.hpp>

#include <gtest/gtest.h>
#include <boost/tuple/tuple_io.hpp>

namespace qi = boost::spirit::qi;

namespace
{

template<class StringT>
void valid_testT(StringT const & testStr, double seconds)
{
  typename StringT::const_iterator first = testStr.begin();
  svgpp::clock_value_grammar<typename StringT::const_iterator> grammar;
  double value;
  EXPECT_TRUE(qi::parse(first, testStr.end(), grammar, value));
  EXPECT_TRUE(first == testStr.end());
  EXPECT_NEAR(seconds, value, seconds * 1e-7);
}

template<class StringT>
void invalid_testT(StringT const & testStr)
{
  typename StringT::const_iterator first = testStr.begin();
  svgpp::clock_value_grammar<typename StringT::const_iterator> grammar;
  EXPECT_TRUE(!qi::parse(first, testStr.end(), grammar) || first != testStr.end());
}

typedef boost::tuple<const char *, double> valid_case_t;

valid_case_t ValidTests[] = {
  valid_case_t("02:30:03", (2 * 60 + 30) * 60 + 3),
  valid_case_t("02:59:59", (2 * 60 + 59) * 60 + 59),
  valid_case_t("50:00:10.25", 50 * 3600 + 10.25),
  valid_case_t("02:33", 153),
  valid_case_t("00:10.5", 10.5),
  valid_case_t("3.2h", (3 * 60 + 12) * 60),
  valid_case_t("45min", 45 * 60),
  valid_case_t("30s", 30),
  valid_case_t("5ms", 0.005),
  valid_case_t("12.467", 12.467),
  valid_case_t("00.5s", 0.5),
  valid_case_t("00:00.005", 0.005)
};

std::ostream & operator<< (std::ostream & os, valid_case_t const & val)
{
  return boost::tuples::operator<<(os, val);
}

class ValidClockValue : public ::testing::TestWithParam<valid_case_t> {
};

char const * const InvalidStrings[] = {
  //"1:60:00", TODO: add check
  "1:10:60",
  "9:34",
  "15mi"
};

}

TEST_P(ValidClockValue, Valid)
{
  std::string testStr = GetParam().get<0>();
  valid_testT(testStr, GetParam().get<1>());
  valid_testT(std::wstring(testStr.begin(), testStr.end()), GetParam().get<1>());
}

INSTANTIATE_TEST_CASE_P(clock_value_grammar,
                        ValidClockValue,
                        ::testing::ValuesIn(ValidTests));

class InvalidClockValue : public ::testing::TestWithParam<const char*> {
};

TEST_P(InvalidClockValue, Invalid)
{
  std::string testStr = GetParam();
  invalid_testT(testStr);
  invalid_testT(std::wstring(testStr.begin(), testStr.end()) );
}

INSTANTIATE_TEST_CASE_P(clock_value_grammar,
                        InvalidClockValue,
                        ::testing::ValuesIn(InvalidStrings));


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

#include <boost/test/unit_test.hpp>

#include "Molassembler/Temple/Adaptors/All.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Stringify.h"
#include "Molassembler/Temple/constexpr/Numeric.h"

#include <iostream>

using namespace Scine::Molassembler;

template<class Container>
std::size_t iteratorDistance(const Container& container) {
  return std::distance(std::begin(container), std::end(container));
}

BOOST_AUTO_TEST_CASE(PairAdaptorTests, *boost::unit_test::label("Temple")) {
  const std::vector<unsigned> i {5, 3, 9, 11};
  const std::vector<unsigned> j {3, 4};

  auto adjacents = Temple::Adaptors::sequentialPairs(i);

  BOOST_CHECK(adjacents.size() == 3);
  BOOST_CHECK(iteratorDistance(adjacents) == adjacents.size());

  BOOST_CHECK(
    Temple::sum(
      Temple::Adaptors::transform(adjacents, std::plus<>())
    ) == 8 + 12 + 20
  );

  auto singlePairs = Temple::Adaptors::allPairs(i);

  BOOST_CHECK(singlePairs.size() == 6);
  BOOST_CHECK(
    iteratorDistance(singlePairs) == singlePairs.size()
  );

  BOOST_CHECK(
    Temple::sum(
      Temple::Adaptors::transform(singlePairs, std::plus<>())
    ) == 8 + 14 + 16 + 12 + 14 + 20
  );

  auto twoPairs = Temple::Adaptors::allPairs(i, j);

  BOOST_CHECK(twoPairs.size() == 8);
  BOOST_CHECK(
    iteratorDistance(twoPairs) == twoPairs.size()
  );

  BOOST_CHECK(
    Temple::sum(
      Temple::Adaptors::transform(twoPairs, std::plus<>())
    ) == 8 + 9 + 6 + 7 + 12 + 13 + 14 + 15
  );
}

BOOST_AUTO_TEST_CASE(IotaAdaptorTests, *boost::unit_test::label("Temple")) {
  auto a = Temple::Adaptors::range(5U);

  BOOST_CHECK(a.size() == 5);
  BOOST_CHECK(iteratorDistance(a) == a.size());
  BOOST_CHECK(Temple::sum(a) == 10U);

  auto b = Temple::Adaptors::range(4U, 7U);

  BOOST_CHECK(b.size() == 3);
  BOOST_CHECK(iteratorDistance(b) == b.size());
  BOOST_CHECK(Temple::sum(b) == 15U);
}

BOOST_AUTO_TEST_CASE(ZipAdaptorTests, *boost::unit_test::label("Temple")) {
  const std::vector<unsigned> i {5, 3, 9, 11};
  const std::vector<unsigned> j {3, 4};

  auto zipRange = Temple::Adaptors::zip(i, j);

  BOOST_CHECK(zipRange.size() == 2);
  BOOST_CHECK(iteratorDistance(zipRange) == zipRange.size());
  BOOST_CHECK(
    Temple::sum(
      Temple::Adaptors::transform(zipRange, std::plus<>())
    ) == 15U
  );
}

BOOST_AUTO_TEST_CASE(TransformAdaptorTests, *boost::unit_test::label("Temple")) {
  const std::vector<unsigned> i {5, 3, 9, 11};

  auto transformRange = Temple::Adaptors::transform(
    i,
    [](unsigned x) -> int {return static_cast<int>(x) - 10;}
  );

  BOOST_CHECK(transformRange.size() == 4);
  BOOST_CHECK(iteratorDistance(transformRange) == transformRange.size());
  BOOST_CHECK(
    Temple::sum(transformRange) == static_cast<int>(Temple::sum(i)) - 4 * 10
  );
}

BOOST_AUTO_TEST_CASE( EnumerateTests, *boost::unit_test::label("Temple")) {
  std::vector<unsigned> testVec {5, 2, 3, 4};

  bool pass = true;
  for(const auto& enumPair : Temple::Adaptors::enumerate(testVec)) {
    if(testVec.at(enumPair.index) != enumPair.value) {
      pass = false;
      break;
    }
  }

  BOOST_CHECK(pass);

  auto weirdSum = Temple::sum(
    Temple::map(
      Temple::Adaptors::enumerate(testVec),
      [](const auto& enumPair) -> unsigned {
        return enumPair.index + enumPair.value;
      }
    )
  );

  BOOST_CHECK(weirdSum == 5 + 3 + 5 + 7);
}

BOOST_AUTO_TEST_CASE(CompoundAdaptorOwnership, *boost::unit_test::label("Temple")) {
  auto pairsOfRange = Temple::Adaptors::allPairs(
    Temple::Adaptors::range(4U)
  );

  auto selfOwningRange = Temple::Adaptors::range(4U);
  auto referenceOwningPairs = Temple::Adaptors::allPairs(selfOwningRange);

  auto checkPairs = [](const auto& rangeObject) -> void {
    BOOST_CHECK(rangeObject.size() == 6);
    BOOST_CHECK(iteratorDistance(rangeObject) == rangeObject.size());
    BOOST_CHECK(
      Temple::invoke(std::plus<>(), *std::begin(rangeObject)) == 1U
    );

    BOOST_CHECK(
      Temple::sum(
        Temple::Adaptors::transform(
          Temple::Adaptors::allPairs(
            Temple::Adaptors::range(4U)
          ),
          [](const unsigned i, const unsigned j) -> unsigned {
            return i * j;
          }
        )
      ) == 11U
    );
  };

  checkPairs(pairsOfRange);
  checkPairs(referenceOwningPairs);

  const std::vector<unsigned> i {1, 4, 9};
  const std::vector<unsigned> j {5, 2};

  auto pairFromTwoReferences = Temple::Adaptors::allPairs(i, j);
  auto pairFromTwoRValues = Temple::Adaptors::allPairs(
    std::vector<unsigned> {1, 4, 9},
    std::vector<unsigned> {5, 2}
  );
  auto pairFromMixed = Temple::Adaptors::allPairs(
    std::vector<unsigned> {1, 4, 9},
    j
  );

  auto checkTwoPairs = [](const auto& rangeObject) -> void {
    BOOST_CHECK(rangeObject.size() == 6);
    BOOST_CHECK(iteratorDistance(rangeObject) == rangeObject.size());
    BOOST_CHECK(
      Temple::invoke(std::plus<>(), *std::begin(rangeObject)) == 6
    );

    BOOST_CHECK(
      Temple::sum(
        Temple::Adaptors::transform(
          rangeObject,
          std::plus<>()
        )
      ) == 6 + 3 + 9 + 6 + 14 + 11
    );
  };

  checkTwoPairs(pairFromTwoReferences);
  checkTwoPairs(pairFromTwoRValues);
  checkTwoPairs(pairFromMixed);

}

BOOST_AUTO_TEST_CASE(AdaptorShortRanges, *boost::unit_test::label("Temple")) {
  auto checkRangeLength = [](
    const auto& rangeObject,
    const unsigned expectedSize,
    const std::string& description
  ) {
    BOOST_CHECK_MESSAGE(
      rangeObject.size() == expectedSize,
      description << " size is " << rangeObject.size() << ", expected "
        << expectedSize
    );
    BOOST_CHECK_MESSAGE(
      iteratorDistance(rangeObject) == rangeObject.size(),
      description << " iterator distance is " << iteratorDistance(rangeObject)
        << ", expected equal to size (" << rangeObject.size() << ")"
    );
  };

  checkRangeLength(
    Temple::Adaptors::allPairs(std::vector<unsigned> {4}),
    0,
    "single-element all-pairs"
  );

  checkRangeLength(
    Temple::Adaptors::allPairs(std::vector<unsigned> {}),
    0,
    "no-element all-pairs"
  );

  checkRangeLength(
    Temple::Adaptors::allPairs(
      std::vector<unsigned> {4},
      std::vector<unsigned> {6}
    ),
    1,
    "one-one all-pairs"
  );

  checkRangeLength(
    Temple::Adaptors::allPairs(
      std::vector<unsigned> {},
      std::vector<unsigned> {6}
    ),
    0,
    "none-one all-pairs"
  );

  checkRangeLength(
    Temple::Adaptors::allPairs(
      std::vector<unsigned> {},
      std::vector<unsigned> {}
    ),
    0,
    "none-none all-pairs"
  );

  checkRangeLength(
    Temple::Adaptors::sequentialPairs(std::vector<unsigned> {4}),
    0,
    "one-element sequential pairs"
  );

  checkRangeLength(
    Temple::Adaptors::sequentialPairs(std::vector<unsigned> {}),
    0,
    "no-element sequential pairs"
  );
}

template<typename Range>
void checkRangeLengthTempl(
  const Range& rangeObject,
  const unsigned expectedSize,
  const std::string& description
) {
  BOOST_CHECK_MESSAGE(
    rangeObject.size() == expectedSize,
    description << " size is " << rangeObject.size() << ", expected "
      << expectedSize
  );
  BOOST_CHECK_MESSAGE(
    iteratorDistance(rangeObject) == rangeObject.size(),
    description << " iterator distance is " << iteratorDistance(rangeObject)
      << ", expected equal to size (" << rangeObject.size() << ")"
  );
}

BOOST_AUTO_TEST_CASE(FrameAdaptorTest, *boost::unit_test::label("Temple")) {
  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<1>(std::vector<unsigned> {}),
    0,
    "no-element cyclic frame of size 1"
  );

  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<1>(std::vector<unsigned> {1}),
    1,
    "single-element cyclic frame of size 1"
  );

  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<1>(std::vector<unsigned> {1, 2}),
    2,
    "two-element cyclic frame of size 1"
  );

  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<2>(std::vector<unsigned> {1, 2}),
    2,
    "two-element cyclic frame of size 2"
  );

  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<2>(std::vector<unsigned> {1, 2, 3}),
    3,
    "three-element cyclic frame of size 2"
  );

  checkRangeLengthTempl(
    Temple::Adaptors::cyclicFrame<4>(std::vector<unsigned> {1, 2, 3}),
    0,
    "three-element cyclic frame of size 4"
  );

  BOOST_CHECK(
    Temple::sum(
      Temple::map(
        Temple::Adaptors::cyclicFrame<2>(std::vector<unsigned> {1, 2, 3}),
        [](unsigned i, unsigned j) -> unsigned {
          return i * j;
        }
      )
    ) == 2U + 6U + 3U
  );
}

BOOST_AUTO_TEST_CASE(FilterAdaptorTests, *boost::unit_test::label("Temple")) {
  const auto filterDistance = iteratorDistance(
    Temple::Adaptors::filter(
      std::vector<unsigned> {1, 2, 3},
      [](const unsigned x) -> bool {return x % 2 == 0;}
    )
  );

  BOOST_CHECK_MESSAGE(
    filterDistance == 1,
    "Filter of 1,2,3 applying is_even isn't length one, but " << filterDistance
  );
}

BOOST_AUTO_TEST_CASE(CombinationsTests, *boost::unit_test::label("Temple")) {
  const std::vector<unsigned> nums {{1, 2, 3}};

  const auto singletonsCount = iteratorDistance(
    Temple::Adaptors::combinations(nums, 1)
  );
  BOOST_CHECK_EQUAL(singletonsCount, 3);

  const auto pairsCount = iteratorDistance(
    Temple::Adaptors::combinations(nums, 1)
  );
  BOOST_CHECK_EQUAL(pairsCount, 3);

  const auto emptyRangePairsCount = iteratorDistance(
    Temple::Adaptors::combinations(std::vector<unsigned> {}, 2)
  );
  BOOST_CHECK_EQUAL(emptyRangePairsCount, 0);

  const auto pairSum = Temple::sum(
    Temple::map(
      Temple::Adaptors::combinations(nums, 2),
      [](const auto& pairAsVector) -> unsigned {
        return Temple::sum(pairAsVector);
      }
    )
  );
  BOOST_CHECK_EQUAL(pairSum, 3 + 4 + 5);
}


#ifndef ISOMON_TEST_MONEY_HPP
#define ISOMON_TEST_MONEY_HPP

#include "money.hpp"

#include <sstream>
#include <boost/test/unit_test.hpp>
#include <boost/lexical_cast.hpp>

using namespace std;
using namespace boost;
using namespace boost::unit_test;
using namespace isomon;

struct no_currency {
  const char* code;

  string name() { return code; }
  void test() {
    money m(0, 0, code);
    BOOST_CHECK_EQUAL( m.unit().c_str(), "XXX" );
    BOOST_CHECK_EQUAL( m.unit().str(), "XXX" );
    BOOST_CHECK( m.value() != m.value() );
  }
} no_currency_cases[] = {
  {"AAA"}, {"XXX"}, {"ZZZ"}, {"___"}, {"$"}, {"$$$"}
};

struct int_round_trip {
  int64_t i;
  const char* code;

  string name() { return lexical_cast<string>(i) + code; }
  void test() {
    money m(i, 0, code);
    BOOST_CHECK_EQUAL( m.value(), i );
    BOOST_CHECK_EQUAL( m.unit().c_str(), code );
    BOOST_CHECK_EQUAL( m.unit().str(), code );

    money m2(m.value(), 0, m.unit());
    BOOST_CHECK_EQUAL( m2.value(), i );
    BOOST_CHECK_EQUAL( m2.unit().c_str(), code );
    BOOST_CHECK_EQUAL( m2.unit().str(), code );
  }
} int_round_trip_cases[] = {
  {0, "USD"}, {1, "EUR"}, {-1, "EUR"},
  {(1LL << 46), "USD"}, {(1LL << 53) - 2, "JPY"}
};

BOOST_AUTO_TEST_CASE( integer_multipy_test )
{
  money zero(0, 0, "USD");
  money cent(0, 1, "USD");
  money two_cent(0, 2, "USD");

  BOOST_CHECK_EQUAL( two_cent, cent * 2 );
  BOOST_CHECK_EQUAL( zero, cent * 0 );
  BOOST_CHECK_EQUAL( -two_cent, cent * -2 );
}

void check_safe_multiply(int64_t a, int32_t b,
                         int64_t expect_hi, uint32_t expect_lo)
{
  pair<int64_t, uint32_t> got = isomon::detail::safe_multiply(a, b);
  BOOST_CHECK_EQUAL( expect_hi, got.first );
  BOOST_CHECK_EQUAL( expect_lo, got.second );
}

BOOST_AUTO_TEST_CASE( safe_multiply_test )
{
  int64_t max64 = numeric_limits<int64_t>::max();
  int64_t min64 = numeric_limits<int64_t>::min();
  int64_t max32 = numeric_limits<int32_t>::max();
  int64_t min32 = numeric_limits<int32_t>::min();
  check_safe_multiply(0, 0, 0, 0);
  check_safe_multiply(1, 1, 0, 1);
  check_safe_multiply(-1, -1, 0, 1);
  check_safe_multiply(1, -1, -1, uint32_t(-1));
  check_safe_multiply(-1, 1, -1, uint32_t(-1));
  check_safe_multiply(min64, min32, 1LL << 62, 0);
  check_safe_multiply(min64, max32, -(1LL << 62) + (1LL << 31), 0);
  check_safe_multiply(max64, min32, -(1LL << 62), 1LL << 31);
  check_safe_multiply(max64, max32, (1LL<<62) - (1LL<<31) - 1, (1LL<<31) + 1);
}

BOOST_AUTO_TEST_CASE( big_integer_multipy_test )
{
  int64_t mucho = 1;
  mucho <<= 52;
  money i_want(0, mucho, "USD");
  money too_much(0, 2*mucho, "USD");

  BOOST_CHECK( isfinite(i_want) );
  BOOST_CHECK( !isfinite(too_much) );
  BOOST_CHECK( !isfinite(i_want * 2) );
  BOOST_CHECK( !isfinite(i_want * -2) );
  BOOST_CHECK( (i_want * 2).value() > 0 );
  BOOST_CHECK( (i_want * -2).value() < 0 );
  BOOST_CHECK( !isfinite(i_want * (1 << 31)) );
  BOOST_CHECK( !isfinite(i_want * (-1 << 31)) );
}

BOOST_AUTO_TEST_CASE( near_zero_subtraction_test )
{
  money cent(0, 1, "USD");
  money zero(0, 0, "USD");

  BOOST_CHECK_EQUAL( cent - cent, zero );
  BOOST_CHECK_EQUAL( -cent + cent, zero );
  BOOST_CHECK_CLOSE( cent.value(), 0.01, 1e-6);
  BOOST_CHECK_CLOSE( (-cent).value(), -0.01, 1e-6);
  BOOST_CHECK_EQUAL( zero - cent, -cent);
}

BOOST_AUTO_TEST_CASE( output_test )
{
  money m(2, 0, "USD");
  stringstream ss;

  ss.imbue(locale::classic());
  ss << m;
  BOOST_CHECK_EQUAL(ss.str(), "USD 2.00");

  ss.str("");
  ss.imbue(locale("de_DE"));
  ss << m;
  BOOST_CHECK_EQUAL(ss.str(), "2,00 USD");

  money yen(2, 0, "JPY");
  ss.str("");
  ss.imbue(locale("de_DE"));
  ss << yen;
  BOOST_CHECK_EQUAL(ss.str(), "2 JPY");

  money euro(2, 0, "EUR");
  ss.str("");
  ss.imbue(locale("en_IE"));
  ss << euro;
  BOOST_CHECK_EQUAL(ss.str(), "2.00 EUR");

  ss.str("");
  ss.imbue(locale::classic());
  ss << euro;
  BOOST_CHECK_EQUAL(ss.str(), "EUR 2.00");
}

BOOST_AUTO_TEST_CASE( rounding_test )
{
  money zero(0, 0, "USD");
  money cent(0, 1, "USD");
  money two(0, 2, "USD");
  money three(0, 3, "USD");

  BOOST_CHECK_EQUAL( cent, round(0.005, "USD") );
  BOOST_CHECK_EQUAL( zero, rounde(0.005, "USD") );
  BOOST_CHECK_EQUAL( two, round(0.015, "USD") );
  BOOST_CHECK_EQUAL( two, rounde(0.015, "USD") );
  BOOST_CHECK_EQUAL( three, round(0.025, "USD") );
  BOOST_CHECK_EQUAL( two, rounde(0.025, "USD") );

  BOOST_CHECK_EQUAL( -cent, round(-0.005, "USD") );
  BOOST_CHECK_EQUAL( zero, rounde(-0.005, "USD") );
  BOOST_CHECK_EQUAL( -two, round(-0.015, "USD") );
  BOOST_CHECK_EQUAL( -two, rounde(-0.015, "USD") );
  BOOST_CHECK_EQUAL( -three, round(-0.025, "USD") );
  BOOST_CHECK_EQUAL( -two, rounde(-0.025, "USD") );
}

BOOST_AUTO_TEST_CASE( six_fifths_rounding_test )
{
  double not_really_two = (1.2 - 1) * 10;

  BOOST_CHECK( not_really_two < 2 );
  BOOST_CHECK_EQUAL( not_really_two, 0x1.FFFFFFFFFFFFEp+0 );

  money two(2, 0, "USD");
  money one_99(1, 99, "USD");

  BOOST_CHECK_EQUAL( two, round(not_really_two, "USD") );
  BOOST_CHECK_EQUAL( two, rounde(not_really_two, "USD") );
  BOOST_CHECK_EQUAL( two, ceil(not_really_two, "USD") );
  BOOST_CHECK_EQUAL( one_99, trunc(not_really_two, "USD") );
  BOOST_CHECK_EQUAL( one_99, floor(not_really_two, "USD") );
}

#endif


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

#include <iostream>
#include <gtest/gtest.h>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/foreach.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include "common/logging.h"
#include "exprs/aggregate-functions.h"
#include "runtime/multi-precision.h"
#include "udf/udf.h"
#include "udf/uda-test-harness.h"
#include "util/decimal-util.h"

#include "common/names.h"

namespace tag = boost::accumulators::tag;
using boost::accumulators::accumulator_set;
using boost::accumulators::stats;
using boost::accumulators::variance;
using boost::algorithm::is_any_of;
using boost::algorithm::trim;
using namespace impala;
using namespace impala_udf;

template <int RANGE_START, int RANGE_END>
bool CheckAppxMedian(const IntVal& actual, const IntVal& expected) {
  return actual.val >= RANGE_START && actual.val <= RANGE_END;
}

bool CheckHistogramDistribution(const StringVal& actual,
    const StringVal& max_expected_stdev) {
  string result(reinterpret_cast<char*>(actual.ptr), actual.len);
  vector<string> str_vals;
  split(str_vals, result, is_any_of(","));

  accumulator_set<int, stats<tag::variance> > acc;
  int prev_val = -1;
  BOOST_FOREACH(string& s, str_vals) {
    trim(s);
    int val = lexical_cast<int>(s);
    if (prev_val != -1) acc(val - prev_val);
    prev_val = val;
  }
  double actual_stdev = sqrt(variance(acc));
  string expected_str(reinterpret_cast<char*>(max_expected_stdev.ptr),
      max_expected_stdev.len);
  return actual_stdev < lexical_cast<double>(expected_str);
}

// TODO: Add other datatypes
TEST(HistogramTest, TestInt) {
  UdaTestHarness<StringVal, StringVal, IntVal> test_histogram(
      AggregateFunctions::ReservoirSampleInit<IntVal>,
      AggregateFunctions::ReservoirSampleUpdate<IntVal>,
      AggregateFunctions::ReservoirSampleMerge<IntVal>,
      AggregateFunctions::ReservoirSampleSerialize<IntVal>,
      AggregateFunctions::HistogramFinalize<IntVal>);
  UdaTestHarness<IntVal, StringVal, IntVal> test_median(
      AggregateFunctions::ReservoirSampleInit<IntVal>,
      AggregateFunctions::ReservoirSampleUpdate<IntVal>,
      AggregateFunctions::ReservoirSampleMerge<IntVal>,
      AggregateFunctions::ReservoirSampleSerialize<IntVal>,
      AggregateFunctions::AppxMedianFinalize<IntVal>);
  const int NUM_BUCKETS = 100;
  const int INPUT_SIZE = NUM_BUCKETS * 1000;

  // All input values are 1, result should be constant.
  {
    vector<IntVal> input(INPUT_SIZE, 1);
    char expected[] = "1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, "
      "1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, "
      "1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, "
      "1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1";
    EXPECT_TRUE(test_histogram.Execute(input, StringVal(&expected[0])))
        << test_histogram.GetErrorMsg();
  }

  // Now check input values ranging from 0 to 100,000. Each bucket should have 1000
  // values, i.e. bucket i should approximately contain values [100*i, 100*(i+1)]. We
  // check the distribution of the deltas between histogram values is not too large.
  // TODO: Add more deterministic test cases
  {
    vector<IntVal> input;
    for (int i = 0; i < INPUT_SIZE; ++i) input.push_back(i);
    test_histogram.SetResultComparator(CheckHistogramDistribution);
    StringVal max_expected_stdev = StringVal("100.0");
    EXPECT_TRUE(test_histogram.Execute(input, max_expected_stdev))
        << test_histogram.GetErrorMsg();

    test_median.SetResultComparator(CheckAppxMedian<45000,55000>);
    EXPECT_TRUE(test_median.Execute(input, IntVal()))
        << test_median.GetErrorMsg();
  }
}

TEST(HistogramTest, TestDecimal) {
  UdaTestHarness<StringVal, StringVal, DecimalVal> test(
      AggregateFunctions::ReservoirSampleInit<DecimalVal>,
      AggregateFunctions::ReservoirSampleUpdate<DecimalVal>,
      AggregateFunctions::ReservoirSampleMerge<DecimalVal>,
      AggregateFunctions::ReservoirSampleSerialize<DecimalVal>,
      AggregateFunctions::HistogramFinalize<DecimalVal>);
  const int NUM_BUCKETS = 100;
  const int INPUT_SIZE = NUM_BUCKETS * 1000;

  // All input values are x, result should be constant.
  {
    vector<DecimalVal> input;
    int128_t val = DecimalUtil::MAX_UNSCALED_DECIMAL16;
    stringstream ss;
    for (int i = 0; i < INPUT_SIZE; ++i) input.push_back(DecimalVal(val));
    for (int i = 0; i < NUM_BUCKETS; ++i) {
      ss << val;
      if (i < NUM_BUCKETS - 1) ss << ", ";
    }
    EXPECT_TRUE(test.Execute(input, StringVal(ss.str().c_str()))) << test.GetErrorMsg();
  }

  {
    vector<DecimalVal> input;
    for (int i = 0; i < INPUT_SIZE; ++i) input.push_back(DecimalVal(i));
    test.SetResultComparator(CheckHistogramDistribution);
    StringVal max_expected_stdev = StringVal("100.0");
    EXPECT_TRUE(test.Execute(input, max_expected_stdev)) << test.GetErrorMsg();
  }
}

TEST(HistogramTest, TestString) {
  UdaTestHarness<StringVal, StringVal, StringVal> test(
      AggregateFunctions::ReservoirSampleInit<StringVal>,
      AggregateFunctions::ReservoirSampleUpdate<StringVal>,
      AggregateFunctions::ReservoirSampleMerge<StringVal>,
      AggregateFunctions::ReservoirSampleSerialize<StringVal>,
      AggregateFunctions::HistogramFinalize<StringVal>);
  const int NUM_BUCKETS = 100;
  const int INPUT_SIZE = NUM_BUCKETS * 1000;

  // All input values are x, result should be constant.
  vector<StringVal> input;
  for (int i = 0; i < INPUT_SIZE; ++i) input.push_back(StringVal("x"));
  char expected[] = "x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, "
      "x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, "
      "x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, "
      "x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x, x";
  EXPECT_TRUE(test.Execute(input, StringVal(&expected[0]))) << test.GetErrorMsg();
}

int main(int argc, char** argv) {
  impala::InitGoogleLoggingSafe(argv[0]);
  impala::DecimalUtil::InitMaxUnscaledDecimal16();
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}