#ifndef ASLAM_GRID_CALIBRATION_TARGET_OBSERVATION_HPP
#define ASLAM_GRID_CALIBRATION_TARGET_OBSERVATION_HPP

#include <vector>
#include <opencv2/core/core.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/access.hpp>
#include <boost/serialization/vector.hpp>
#include <sm/opencv/serialization.hpp>
#include <sm/eigen/serialization.hpp>
#include <sm/boost/serialization.hpp>
#include <sm/kinematics/Transformation.hpp>
#include <sm/assert_macros.hpp>
#include <aslam/Time.hpp>
#include <aslam/cameras/CameraGeometryBase.hpp>
#include <aslam/cameras/GridCalibrationTargetBase.hpp>

namespace aslam {
    namespace cameras {

/**
 * \class GridCalibrationTargetObservation
 * \brief an object representing the observation of a gridded calibration pattern in an image
 *
 */
        class GridCalibrationTargetObservation {
        public:
            SM_DEFINE_EXCEPTION(Exception, std::runtime_error
            );

            GridCalibrationTargetObservation(GridCalibrationTargetBase::Ptr target);

            // optionally store the image.
            GridCalibrationTargetObservation(GridCalibrationTargetBase::Ptr target, cv::Mat image);

            virtual ~GridCalibrationTargetObservation();

            /// \brief get a copy of the points
            Eigen::MatrixXd points() {
                return _points;
            };

            /// \brief set the image.
            void setImage(cv::Mat image);

            /// \brief clear the image. This can be handy for saving memory.
            void clearImage();

            /// \brief get all corners in image coordinates (order matches getCornersImageFrame)
            unsigned int getCornersTargetFrame(std::vector <cv::Point3f> &outCornerList) const;

            /// \brief get all corners in target frame coordinates (order matches getObservedTargetFrame)
            unsigned int getCornersImageFrame(std::vector <cv::Point2f> &outCornerList) const;

            /// \brief get the point index of all (observed) corners (order corresponds to the output
            ///        of getCornersImageFrame and getCornersTargetFrame)
            unsigned int getCornersIdx(std::vector<unsigned int> &outCornerIdx) const;

            /// \brief get the the reprojected corners of all observed corners
            unsigned int getCornerReprojection(const boost::shared_ptr <CameraGeometryBase> cameraGeometry,
                                               std::vector <cv::Point2f> &outPointReproj) const;

            /// \brief get a point from the target expressed in the target frame
            /// \return true if the grid point was seen in this image.
            bool imagePoint(size_t i, Eigen::Vector2d &outPoint) const;

            /// \brief update an image observation
            void updateImagePoint(size_t i, const Eigen::Vector2d &point);

            /// \brief remove an image observation
            void removeImagePoint(size_t i);

            /// \brief get a point from the target expressed in the target frame
            /// \return true if the grid point was seen in this image.
            bool imageGridPoint(size_t r, size_t c, Eigen::Vector2d &outPoint) const;

            /// \brief get the number of rows in the image
            size_t imRows() const {
                return _imRows;
            };

            /// \brief get the number of columns in the image.
            size_t imCols() const {
                return _imCols;
            };

            /// \brief get the image that generated these observations.
            cv::Mat image() const {
                return _image;
            };

            /// \brief get the grid calibration target
            GridCalibrationTargetBase::Ptr target() {
                return _target;
            };

            /// \brief get the grid calibration target
            GridCalibrationTargetBase::ConstPtr target() const {
                return _target;
            };

            void setTarget(GridCalibrationTargetBase::Ptr target);

            /// \brief get the transformation that takes points from
            ///        camera coordinates to target coordinates.
            const sm::kinematics::Transformation &T_t_c() const {
                return _T_t_c;
            };

            void set_T_t_c(const sm::kinematics::Transformation &T_t_c) {
                _T_t_c_isSet = true;
                _T_t_c = T_t_c;
            };

            /// \brief get the time of the observation
            aslam::Time time() const {
                return _stamp;
            };

            /// \brief set the time of the observation
            void setTime(const aslam::Time &stamp) {
                _stamp = stamp;
            };

            int targetId() const {
                return _targetId;
            }
            void setTargetId(const int targetId)
            {
                _targetId = targetId;
            }
            /// \brief return true if the class has at least one successful observation
            int numberSuccessfulObservation() const;

        private:
            /// \brief the target being observed
            GridCalibrationTargetBase::Ptr _target;

            /// \brief the image observations stored row-major (idx = cols * r + c)
            Eigen::MatrixXd _points;

            /// \brief the success/failure of the image observations
            std::vector<bool> _success;

            /// \brief the transformation that takes points from
            ///        camera coordinates to target coordinates.
            sm::kinematics::Transformation _T_t_c;

            /// \brief the image that generated these observations
            cv::Mat _image;

            /// \brief the number of rows in the image (needed if the image is not stored)
            size_t _imRows;

            /// \brief the number of cols in the image (needed if the image is not stored)
            size_t _imCols;

            /// \brief the image timestamp
            aslam::Time _stamp;

            /// \brief has the extrinsics been set?
            bool _T_t_c_isSet;

            int _targetId;
            ///////////////////////////////////////////////////
            // Serialization support
            ///////////////////////////////////////////////////
        public:
            enum {
                CLASS_SERIALIZATION_VERSION = 1
            };

            BOOST_SERIALIZATION_SPLIT_MEMBER()

            //serialization ctor
            GridCalibrationTargetObservation() {};

        protected:
            friend class boost::serialization::access;

            template<class Archive>
            void load(Archive &ar, const unsigned int version) {
                SM_ASSERT_LE(std::runtime_error, version,
                             (unsigned int) CLASS_SERIALIZATION_VERSION,
                             "Unsupported serialization version");
                ar >> BOOST_SERIALIZATION_NVP(_target);
                ar >> BOOST_SERIALIZATION_NVP(_points);
                ar >> BOOST_SERIALIZATION_NVP(_success);
                ar >> BOOST_SERIALIZATION_NVP(_T_t_c);
                ar >> BOOST_SERIALIZATION_NVP(_image);
                ar >> BOOST_SERIALIZATION_NVP(_imRows);
                ar >> BOOST_SERIALIZATION_NVP(_imCols);
                ar >> BOOST_SERIALIZATION_NVP(_stamp);
                ar >> BOOST_SERIALIZATION_NVP(_T_t_c_isSet);
                ar >> BOOST_SERIALIZATION_NVP(_targetId);
            }

            template<class Archive>
            void save(Archive &ar, const unsigned int /* version */) const {
                ar << BOOST_SERIALIZATION_NVP(_target);
                ar << BOOST_SERIALIZATION_NVP(_points);
                ar << BOOST_SERIALIZATION_NVP(_success);
                ar << BOOST_SERIALIZATION_NVP(_T_t_c);
                ar << BOOST_SERIALIZATION_NVP(_image);
                ar << BOOST_SERIALIZATION_NVP(_imRows);
                ar << BOOST_SERIALIZATION_NVP(_imCols);
                ar << BOOST_SERIALIZATION_NVP(_stamp);
                ar << BOOST_SERIALIZATION_NVP(_T_t_c_isSet);
                ar << BOOST_SERIALIZATION_NVP(_targetId);
            }
        };

    }  // namespace cameras
}  // namespace aslam

SM_BOOST_CLASS_VERSION(aslam::cameras::GridCalibrationTargetObservation);

#endif /* ASLAM_GRID_CALIBRATION_TARGET_OBSERVATION_HPP */


// Copyright Aleksey Gurtovoy 2001-2004
//
// Distributed under the Boost Software License,Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/single_view.hpp>
#include <boost/mpl/advance.hpp>
#include <boost/mpl/size.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/equal.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef single_view<int> view;
    typedef begin<view>::type first;
    typedef end<view>::type last;

    MPL_ASSERT(( is_same< deref<first>::type, int > ));
    MPL_ASSERT(( is_same< next<first>::type, last > ));
    MPL_ASSERT(( is_same< prior<last>::type, first > ));

    MPL_ASSERT(( is_same< mpl::advance<first, int_<0> >::type, first > ));
    MPL_ASSERT(( is_same< mpl::advance<first, int_<1> >::type, last > ));
    MPL_ASSERT(( is_same< mpl::advance<last, int_<0> >::type, last > ));
    MPL_ASSERT(( is_same< mpl::advance<last, int_<-1> >::type, first > ));

    MPL_ASSERT_RELATION( (mpl::distance<first,first>::value), ==, 0 );
    MPL_ASSERT_RELATION( (mpl::distance<first,last>::value), ==, 1 );
    MPL_ASSERT_RELATION( (mpl::distance<last,last>::value), ==, 0 );

    MPL_ASSERT_RELATION( size<view>::value, ==, 1 );

    MPL_ASSERT(( equal< view, view::type > ));
}


#include "Day06-ChronalCoordinates.h"

#include "GridAreaAnalyzer.h"

#include <AdventOfCodeCommon/DisableLibraryWarningsMacros.h>

__BEGIN_LIBRARIES_DISABLE_WARNINGS
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
__END_LIBRARIES_DISABLE_WARNINGS

namespace AdventOfCode
{
namespace Year2018
{
namespace Day06
{

std::vector<Coordinates> parseCoordinates(const std::vector<std::string>& coordinatesLines)
{
    std::vector<Coordinates> allCoordinates;

    for (const auto& lines : coordinatesLines)
    {
        std::vector<std::string> tokens;
        boost::split(tokens, lines, boost::is_any_of(" ,"), boost::token_compress_on);

        unsigned x = boost::lexical_cast<unsigned>(tokens[0]);
        unsigned y = boost::lexical_cast<unsigned>(tokens[1]);

        allCoordinates.emplace_back(x, y);
    }

    return allCoordinates;
}

unsigned sizeOfLargestNonInfiniteArea(const std::vector<std::string>& coordinatesLines)
{
    std::vector<Coordinates> allCoordinates = parseCoordinates(coordinatesLines);

    GridAreaAnalyzer gridAnalyzer{std::move(allCoordinates)};

    return gridAnalyzer.getLargestNonInfiniteArea();
}

unsigned sizeOfAreaWithinGivenDistance(const std::vector<std::string>& coordinatesLines, unsigned distanceThreshold)
{
    std::vector<Coordinates> allCoordinates = parseCoordinates(coordinatesLines);

    GridAreaAnalyzer gridAnalyzer{std::move(allCoordinates)};

    return gridAnalyzer.getAreaWithinGivenDistance(distanceThreshold);
}

}
}
}


///////////////////////////////////////////////////////////////////////////////
// initializer.hpp                                                           //
//                                                                           //
//  Copyright 2010 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_INITIALIZER_HPP_ER_2010
#define BOOST_STATISTICS_DETAIL_NON_PARAMETRIC_CONTINGENCY_TABLE_CELLS_INITIALIZER_HPP_ER_2010
#include <boost/type_traits/is_same.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/mpl/vector/vector10.hpp>
#include <boost/numeric/conversion/converter.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/fusion/include/pair.hpp>
#include <boost/fusion/include/map.hpp>
#include <boost/fusion/include/joint_view.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/factor/levels.hpp>
#include <boost/statistics/detail/non_parametric/contingency_table/cells/traits.hpp>

// Usage
//    initialize<Keys>(acc)
// All the cells of contingency table are created and initialized to zero.

namespace boost{
namespace statistics{
namespace detail{
namespace contingency_table{
namespace cells_aux{

    template<typename K,typename T>
    struct op{

        typedef boost::fusion::pair<K,T> pair_; // maybe const T& ?
        typedef boost::fusion::map< pair_ > result_type;

        op(){}

        result_type operator()(const T& t)const
        {
            return result_type( pair_( t ) );
        }

    };

    template<typename Keys,typename AccSet,typename V,
        typename F, typename L, bool exit = boost::is_same<F,L>::value
    >
    struct initializer{

        initializer(AccSet& acc,const V& view)
        :acc_( acc ),view_( view ){}

        typedef typename boost::mpl::next<F>::type next_it_;
        typedef typename boost::mpl::deref<F>::type key_;
        typedef boost::mpl::vector1<key_> vec_key_;

        typedef void result_type;

        template<typename M>
        void operator()(const M& map)
        {

            namespace ct = statistics::detail::contingency_table;
            typedef boost::fusion::joint_view<const V,const M> next_view_;
            typedef cells_aux::initializer<
                Keys, AccSet, next_view_, next_it_, L> next_;
            next_view_ next_view( this->view_, map );
            typedef typename ct::result_of::extract::levels<
                key_,AccSet>::type ref_levels_;
            typedef typename boost::remove_cv<
                typename boost::remove_reference<
                    ref_levels_
                >::type
            >::type levels_;
            typedef typename levels_aux::value_of<key_,AccSet>::type value_;
            typedef cells_aux::op<key_,value_> op_;
            boost::for_each(
                ct::extract::levels<key_>( this->acc_ )
                    | boost::adaptors::transformed( op_() ),
                next_( this->acc_, next_view )
            );

        }

        private:

        AccSet& acc_;
        const V& view_;

    };

    template<typename Keys, typename AccSet, typename V, typename F, typename L>
    struct initializer<Keys, AccSet, V, F, L, true>
    {

        initializer(AccSet& acc, const V& view)
        :acc_( acc ),view_( view ){}

        typedef void result_type;

        template<typename M>
        void operator()(const M& map)
        {
            namespace ct = statistics::detail::contingency_table;
            typedef boost::fusion::joint_view<const V,const M> next_view_;
            next_view_ next_view( this->view_, map );
            typedef typename ct::cells_traits::mapped_of<
               Keys, AccSet>::type count_;
            typedef boost::numeric::converter<count_,int> converter_;
            count_ zero = converter_::convert( 0 );
            ct::extract::cells<Keys>( this->acc_ )[ next_view ] = zero;

        }

        private:
        AccSet& acc_;
        const V& view_;

    };

}// cells_aux

    template<typename Keys,typename AccSet>
    void initialize(AccSet& acc)
    {
        namespace ct = statistics::detail::contingency_table;
        typedef boost::fusion::map<> empty_map_;
        typedef typename boost::mpl::begin<Keys>::type first_it_;
        typedef typename boost::mpl::deref<first_it_>::type key_;
        typedef typename boost::mpl::vector1<key_> vec_key_;
        typedef typename boost::mpl::next<first_it_>::type second_it_;
        typedef typename boost::mpl::end<Keys>::type last_;

        typedef cells_aux::initializer<
            Keys, AccSet, empty_map_, second_it_, last_> initializer_;
        empty_map_ empty_map;
        initializer_ initializer( acc, empty_map );
        typedef typename levels_aux::value_of<key_,AccSet>::type value_;
        typedef cells_aux::op<key_,value_> op_;
        op_ op;
        boost::for_each(
            ct::extract::levels<key_>( acc )
                | boost::adaptors::transformed( op_() ),
            initializer
        );

    }

}// contingency_table
}// detail
}// statistics
}// boost

#endif


/**************************************************************************
** Copyright (C) 2016-2019 Toshinobu Hondo, Ph.D.
** Copyright (C) 2016-2019 MS-Cheminformatics LLC, Toin, Mie Japan
*
** Contact: toshi.hondo@qtplatz.com
**
** Reference: http://www.analog.com/media/en/technical-documentation/data-sheets/AD5593R.pdf
**           AD5593R: 8-Channel, 12-Bit, Configurable ADC/DAC with On-Chip Reference, I2C Interface Data Sheet
**           Rev.C (Last Content Update: 05/03/2017)
**************************************************************************/

#pragma once

#include <boost/optional.hpp>
#include <array>
#include <bitset>
#include <string>
#include <ostream>
#include <system_error>
#include <memory>

namespace i2c_linux { class i2c; }

namespace i2c_device {

    namespace ad5593 {

        enum AD5593R_IO_FUNCTION {
            ADC
            , DAC
            , GPIO
            , UNUSED_HIGH
            , UNUSED_LOW
            , UNUSED_TRISTATE
            , UNUSED_PULLDOWN
            , nFunctions
        };

        enum AD5593_GPIO_DIRECTION {
            GPIO_INPUT
            , GPIO_OUTPUT
        };

        constexpr size_t number_of_pins = 8;
        constexpr size_t number_of_regs = 7;

        class AD5593 {
            std::array< std::bitset< 8 >, number_of_regs > bitmaps_;
            std::unique_ptr< i2c_linux::i2c > i2c_;
            bool dirty_;
            double Vref_;
            std::bitset< 16 > pd_;
            std::bitset< 16 > ctrl_;
            std::bitset< 16 > adc_sequence_;

        public:
            ~AD5593();
            AD5593();
            AD5593( const std::string& device = "/dev/i2c-1", int address = 0x10 );
            bool open( const std::string& device = "/dev/i2c-1", int address = 0x10 );

            const std::error_code& error_code() const;
            operator bool () const;
            double Vref() const;

            const std::bitset< 16 >& pd() const { return pd_; }
            const std::bitset< 16 >& ctrl() const { return ctrl_; }

            inline bool is_dirty() const { return dirty_; }

        private:
            bool write( uint8_t addr, uint16_t value ) const;
            bool read( uint8_t addr, uint8_t * data, size_t size ) const;
            boost::optional< uint16_t > read( uint8_t addr ) const;

            void save();
            bool restore();

        public:
            bool dac_set_value( int pin, uint16_t value );

            bool reset_function( int pin, AD5593R_IO_FUNCTION );
            bool set_function( int pin, AD5593R_IO_FUNCTION );
            bool is_function( int pin, AD5593R_IO_FUNCTION ) const;

            bool fetch( bool force = true );
            bool commit();

            bool is_adc_en( int pin ) const;
            bool is_dac_en( int pin ) const;
            uint16_t dac_readback( int pin );
            uint16_t adc_readback( int pin );

            void print_config( std::ostream& ) const;
            void print_values( std::ostream& );
            bool set_adc_sequence( uint16_t sequence );
            std::bitset< 16 > adc_sequence( bool fetch = true ) const;
            const std::bitset< 8 >& adc_enabled() const;
            std::pair< std::string, std::bitset< 8 > > reg_enabled( size_t idx ) const;

            bool read_adc_sequence ( uint16_t * data, size_t size );
            void print_adc_sequence( std::ostream&, const uint16_t * sequence, size_t size, bool has_temp ) const;
            double toVolts( uint32_t, bool is_dac ) const;
            uint16_t fromVolts( double, bool is_dac ) const;
            double temperature( uint32_t ) const;
            bool set_vref( bool );
            bool set_ctrl( uint16_t );
            bool set_pd( uint16_t );
        };
    }
}


/*
 * Parameters.hpp
 *
 *  Created on: Feb 10, 2021
 *      Author: jelavice
 */

#pragma once
#include <Eigen/Core>
#include "se2_planning/OmplReedsSheppPlanner.hpp"

namespace se2_planning {

struct ApproachPosePlannerParameters {
  double goalPolarAngleSpacing_ = 10 * M_PI / 180.0;
  double candidatePoseYawSpacing_ = 10 * M_PI / 180.0;
  double minGoalDistance_ = 1.0;
  double maxGoalDistance_ = 5.0;
  double distanceStep_ = 0.5;
  bool isPrintInfo_ = false;
  bool isPrintApproachPoseStatisctics_ = false;
  OmplReedsSheppPlannerParameters plannerImplParams_;
};

struct LineOfSightApproachStateValidatorParameters {
  double lengthOfLineOfSightPortionAllowedToBeInCollisisonWithTarget_ = 0.5;
  double collisionCheckingAreaWidth_ = 1.2;
  bool isAssumeTargetAlwaysReachable_ = false;
};

} /* namespace se2_planning*/


#include <boost/numeric/ublas/operation/num_rows.hpp>


// Copyright 2017 Daniel Parker
// Distributed under Boost license

#include <boost/test/unit_test.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <jsoncons/json.hpp>
#include <sstream>
#include <vector>
#include <map>
#include <utility>
#include <ctime>
#include <cstdint>

using boost::numeric::ublas::matrix;

namespace jsoncons
{
    template<>
    struct json_convert_traits<matrix<double>>
    {
        template <class CharT>
        static matrix<double> decode(std::basic_istringstream<CharT>& is,
                                     const basic_json_serializing_options<CharT>& options)
        {
            basic_json<CharT> j = basic_json<CharT>::parse(is, options);
            if (j.is_array() && j.size() > 0)
            {
                size_t m = j.size();
                size_t n = 0;
                for (const auto& a : j.array_range())
                {
                    if (a.size() > n)
                    {
                        n = a.size();
                    }
                }

                boost::numeric::ublas::matrix<double> A(m,n,double());
                for (size_t i = 0; i < m; ++i)
                {
                    const auto& a = j[i];
                    for (size_t j = 0; j < a.size(); ++j)
                    {
                        A(i,j) = a[j].template as<double>();
                    }
                }
                return A;
            }
            else
            {
                boost::numeric::ublas::matrix<double> A;
                return A;
            }
        }

        static void encode(const matrix<double>& val, json_content_handler& handler)
        {
            handler.begin_array();
            for (size_t i = 0; i < val.size1(); ++i)
            {
                handler.begin_array();
                for (size_t j = 0; j < val.size2(); ++j)
                {
                    handler.double_value(val(i, j));
                }
                handler.end_array();
            }
            handler.end_array();
        }
    };
};

using namespace jsoncons;

BOOST_AUTO_TEST_SUITE(json_convert_tests)

BOOST_AUTO_TEST_CASE(convert_pair_test)
{
    auto val = std::make_pair(false,std::string("foo"));
    std::string s;

    jsoncons::encode_json(val, s);

    auto result = jsoncons::decode_json<std::pair<bool,std::string>>(s);

    BOOST_CHECK(val == result);
}

BOOST_AUTO_TEST_CASE(convert_vector_test)
{
    std::vector<double> v = {1,2,3,4,5,6};

    std::string s;
    jsoncons::encode_json(v,s);

    auto result = jsoncons::decode_json<std::vector<double>>(s);

    BOOST_REQUIRE(v.size() == result.size());
    for (size_t i = 0; i < result.size(); ++i)
    {
        BOOST_CHECK_EQUAL(v[i],result[i]);
    }
}

BOOST_AUTO_TEST_CASE(convert_map_test)
{
    std::map<std::string,double> m = {{"a",1},{"b",2}};

    std::string s;
    jsoncons::encode_json(m,s);
    auto result = jsoncons::decode_json<std::map<std::string,double>>(s);

    BOOST_REQUIRE(result.size() == m.size());
    BOOST_CHECK(m["a"] == result["a"]);
    BOOST_CHECK(m["b"] == result["b"]);
}

BOOST_AUTO_TEST_CASE(convert_array_test)
{
    std::array<double,4> v{1,2,3,4};

    std::string s;
    jsoncons::encode_json(v,s);
    std::array<double, 4> result = jsoncons::decode_json<std::array<double,4>>(s);
    BOOST_REQUIRE(result.size() == v.size());
    for (size_t i = 0; i < result.size(); ++i)
    {
        BOOST_CHECK_EQUAL(v[i],result[i]);
    }
}

#if !defined(__GNUC__) && __GNUC__ == 4 && __GNUC_MINOR__ < 9
BOOST_AUTO_TEST_CASE(convert_tuple_test)
{
    typedef std::map<std::string,std::tuple<std::string,std::string,double>> employee_collection;

    employee_collection employees =
    {
        {"John Smith",{"Hourly","Software Engineer",10000}},
        {"Jane Doe",{"Commission","Sales",20000}}
    };

    std::string s;
    jsoncons::encode_json(employees, s, jsoncons::indenting::indent);
    std::cout << "(1)\n" << s << std::endl;
    auto employees2 = jsoncons::decode_json<employee_collection>(s);
    BOOST_REQUIRE(employees2.size() == employees.size());

    std::cout << "\n(2)\n";
    for (const auto& pair : employees2)
    {
        std::cout << pair.first << ": " << std::get<1>(pair.second) << std::endl;
    }
}
#endif

BOOST_AUTO_TEST_CASE(test_boost_matrix)
{
    matrix<double> A(2, 2);
    A(0, 0) = 1.1;
    A(0, 1) = 2.1;
    A(1, 0) = 3.1;
    A(1, 1) = 4.1;

    std::string s;
    jsoncons::encode_json(A,s,indenting::indent);
    std::cout << "(1) " << s << std::endl;
    auto A2 = jsoncons::decode_json<matrix<double>>(s);

}

BOOST_AUTO_TEST_SUITE_END()


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef input_reads_profiler_hpp
#define input_reads_profiler_hpp

#include <cstddef>
#include <vector>

#include <boost/optional.hpp>

#include "config/common.hpp"
#include "basics/aligned_read.hpp"
#include "io/read/read_manager.hpp"
#include "readpipe/read_pipe.hpp"

namespace octopus {

struct ReadSetProfileConfig
{
    unsigned max_samples_per_sample = 10;
    unsigned max_sample_size = 100'000;
};

struct ReadSetProfile
{
    std::size_t mean_read_bytes, read_bytes_stdev;
    std::size_t mean_depth, median_depth, depth_stdev, median_positive_depth, mean_positive_depth;
    std::vector<std::size_t> sample_mean_depth, sample_median_depth, sample_median_positive_depth, sample_mean_positive_depth;
    std::vector<std::size_t> sample_depth_stdev;
    AlignedRead::NucleotideSequence::size_type max_read_length, median_read_length;
    AlignedRead::MappingQuality max_mapping_quality, median_mapping_quality, rmq_mapping_quality;
};

boost::optional<ReadSetProfile>
profile_reads(const std::vector<SampleName>& samples,
              const InputRegionMap& input_regions,
              const ReadManager& source,
              ReadSetProfileConfig config = ReadSetProfileConfig {});

boost::optional<std::size_t>
estimate_mean_read_size(const std::vector<SampleName>& samples,
                        const InputRegionMap& input_regions,
                        ReadManager& read_manager,
                        unsigned max_sample_size = 1000);

std::size_t default_read_size_estimate() noexcept;

} // namespace octopus

#endif


#ifndef STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP
#define STAN_MATH_TORSTEN_DSOLVE_ODEINT_INTEGRATOR_HPP

#include <stan/math/torsten/dsolve/pmx_odeint_system.hpp>
#include <stan/math/torsten/mpi/precomputed_gradients.hpp>
#include <boost/numeric/odeint.hpp>
#include <type_traits>

namespace torsten {
namespace dsolve {

/**
 * @c boost::odeint ODE integrator.
 */
  template<typename scheme_t>
  struct PMXOdeintIntegrator {
    const double rtol_;
    const double atol_;
    const int64_t max_num_steps_;

    template<typename Ode, bool GenVar>
    struct SolObserver {
      const Ode& ode_;
      const size_t n;
      const size_t m;
      std::vector<std::vector<typename Ode::scalar_t>> y;
      int step_counter_;

      SolObserver(const Ode& ode) :
        ode_(ode), n(ode.N_), m(ode.M_),
        y(ode.ts_.size(), std::vector<typename Ode::scalar_t>(ode.N_, 0.0)),
        step_counter_(0)
      {}

      /*
       * use observer to convert y value and gradient to var
       * results, if necessary.
       */
      inline void operator()(const std::vector<double>& curr_result, double t) {
        if(t > ode_.t0_) {
          observer_impl(y[step_counter_], curr_result, ode_.ts_, ode_.y0_, ode_.theta_, step_counter_);
          step_counter_++;
        }
      }

    private:
      /*
       * All data, return data
       */
      inline void observer_impl(std::vector<double>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<double>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        std::copy(y.begin(), y.end(), y_res.begin());
      }

      /*
       * When only @c ts is @c var, we don't solve
       * sensitivity ODE since the sensitivity is simply the RHS.
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                const std::vector<double>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        int n = y0.size();
        std::vector<double> g(n * (1 + ts.size()), 0.0);
        std::copy(y.begin(), y.end(), g.begin());
        std::vector<double> dy_dt(n);
        ode_.dbl_rhs_impl(y, dy_dt, ts[i].val());
        std::copy(dy_dt.begin(), dy_dt.end(), g.begin() + n + i * n);
        y_res = torsten::precomputed_gradients(g, ts);
      }

      /*
       * Only @c theta is @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<double>& y0,
                                const std::vector<stan::math::var>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, theta);
      }

      /*
       * only @c y0 is @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<stan::math::var>& y0,
                                const std::vector<double>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, y0);
      }

      /*
       * @c y0 and @c theta are @c var
       */
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                const std::vector<stan::math::var>& y0,
                                const std::vector<stan::math::var>& theta,
                                int i) const {
        y_res = torsten::precomputed_gradients(y, ode_.vars());
      }

      /*
       * @c theta and/or &c y0 are @c var, together with @c ts.
       */
      template<typename T1, typename T2>
      inline void observer_impl(std::vector<stan::math::var>& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                const std::vector<T1>& y0,
                                const std::vector<T2>& theta,
                                int i) const {
        int ns = ode_.ns;
        int n = y0.size();
        std::vector<double> g(n * (1 + ns + ts.size()), 0.0);
        std::copy(y.begin(), y.end(), g.begin());
        std::vector<double> dy_dt(n), y_dbl(y.begin(), y.begin() + n);
        ode_.dbl_rhs_impl(y_dbl, dy_dt, ts[i].val());
        std::copy(dy_dt.begin(), dy_dt.end(), g.begin() + n + ns * n + i * n);
        y_res = torsten::precomputed_gradients(g, ode_.vars());
      }
    };

    template<typename Ode>
    struct SolObserver<Ode, false> {
      const Ode& ode_;
      Eigen::MatrixXd y;
      int step_counter_;

      SolObserver(const Ode& ode) :
        ode_(ode),
        y(Eigen::MatrixXd::Zero(ode_.size_ + ode_.N_*(Ode::is_var_ts ? ode_.ts_.size() : 0), ode_.ts_.size())),
        step_counter_(0)
      {}

      inline void operator()(const std::vector<double>& curr_result, double t) {
        if(t > ode_.t0_) {
          observer_impl(y, curr_result, ode_.ts_, step_counter_);
          step_counter_++;
        }
      }

    private:
      /*
       * @@c ts is data
       */
      inline void observer_impl(Eigen::MatrixXd& y_res,
                                const std::vector<double>& y,
                                const std::vector<double>& ts,
                                int i) const {
        using Eigen::VectorXd;
        y_res.col(i) = VectorXd::Map(y.data(), ode_.size_);
      }

      /*
       * @@c ts is @c var
       */
      inline void observer_impl(Eigen::MatrixXd& y_res,
                                const std::vector<double>& y,
                                const std::vector<stan::math::var>& ts,
                                int i) const {
        using Eigen::VectorXd;
        for (size_t j = 0; j < ode_.size_; ++j) y_res(j, i) = y[j];
        int n = ode_.N_;
        std::vector<double> dy_dt(n);
        std::vector<double> y_tmp(y.begin(), y.begin() + n);
        ode_.dbl_rhs_impl(y_tmp, dy_dt, ts[i].val());
        for (size_t j = 0; j < n; ++j) y_res(ode_.size_ + i * n + j, i) = dy_dt[j];
      }
    };

  public:
    /**
     * constructor
     * @param[in] rtol relative tolerance
     * @param[in] atol absolute tolerance
     * @param[in] max_num_steps max nb. of times steps
     */
    PMXOdeintIntegrator(const double rtol, const double atol,
                        const int64_t max_num_steps)
      : rtol_(rtol), atol_(atol), max_num_steps_(max_num_steps) {
      using stan::math::invalid_argument;
      if (rtol_ <= 0)
        invalid_argument("cvodes_integrator", "relative tolerance,", rtol_, "",
                         ", must be greater than 0");
      if (rtol_ > 1.0E-3)
        invalid_argument("cvodes_integrator", "relative tolerance,", rtol_, "",
                         ", must be less than 1.0E-3");
      if (atol_ <= 0)
        invalid_argument("cvodes_integrator", "absolute tolerance,", atol_, "",
                         ", must be greater than 0");
      if (max_num_steps_ <= 0)
        invalid_argument("cvodes_integrator", "max_num_steps,",
                         max_num_steps_, "",
                         ", must be greater than 0");
    }

    template <typename Ode, bool GenVar = true>
    auto integrate(Ode& ode) {
      std::vector<double> ts_vec(ode.ts_.size() + 1);
      ts_vec[0] = ode.t0_;
      for (size_t i = 0; i < ode.ts_.size(); ++i) {
        ts_vec[i + 1] = stan::math::value_of(ode.ts_[i]);
      }

      SolObserver<Ode, GenVar> observer(ode);

      const double init_dt = 0.1;
      integrate_times(make_dense_output(atol_, rtol_, scheme_t()),
                      boost::ref(ode), ode.y0_fwd_system,
                      ts_vec.begin(), ts_vec.end(),
                      init_dt, boost::ref(observer),
                      boost::numeric::odeint::max_step_checker(max_num_steps_));
      return observer.y;
    }
  };

}
}
#endif


#ifndef ADEPT_TESTER_HPP_
#define ADEPT_TESTER_HPP_

#include <adept.h>
#include <utility>

#include "common.hpp"

namespace Eigen
{

template<>
struct NumTraits<adept::adouble>: NumTraits<double>
{
  typedef adept::adouble Real;
  typedef adept::adouble NonInteger;
  typedef adept::adouble Nested;

  enum
  {
    IsComplex = 0,
    IsInteger = 0,
    IsSigned = 1,
    RequireInitialization = 1,
    ReadCost = 1,
    AddCost = 3,
    MulCost = 3
  };
};

}  // namespace Eigen

class AdeptTester
{
public:
  static constexpr char name[] = "Adept";

  template<typename Func, typename Derived>
  void setup(Func && f, const Eigen::PlainObjectBase<Derived> & x)
  {}

  template<typename Func, typename Derived>
  void run(
    Func && f,
    const Eigen::PlainObjectBase<Derived> & x,
    typename EigenFunctor<Func, Derived>::JacobianType & J)
  {
    adept::Stack stack;

    auto ax = x.template cast<adept::adouble>().eval();

    stack.new_recording();

    auto ay = f(ax);

    stack.independent(ax.data(), ax.size());
    stack.dependent(ay.data(), ay.size());

    stack.jacobian(J.data());
  }
};

#endif  // ADEPT_TESTER_HPP_


#ifndef __Trajectory_Simulation_hpp_
#define __Trajectory_Simulation_hpp_

#include <Eigen/Geometry>

#include <random>

#include "Trajectory_Class.hpp"
#include "Physical_Parameters.hpp"
#include "General_Utilities.hpp"

extern Eigen::Vector3d Scatter(Eigen::Vector3d& vini,double mX,double A,std::mt19937& PRNG);

extern double VelocityCut(double mX,double Ethreshold=1*eV,double A=131);

extern Trajectory ParticleTrack(double mX,double sigman0,Event IniCondi, double vcut,std::mt19937& PRNG,double rFinal=1*meter);

#endif


#include <stdio.h>
#include <cfloat>
#include <list>
#include <vector>
#include <math.h>
#include <iostream>
#include <algorithm>
#include <numeric>
#include <boost/algorithm/clamp.hpp>
#include <random>
#include "third_party/eigen3/unsupported/Eigen/CXX11/Tensor"
#include "tensorflow/core/framework/op.h"
#include "tensorflow/core/framework/op_kernel.h"
#include "tensorflow/core/framework/tensor_shape.h"
#include "tensorflow/core/framework/common_shape_fns.h"
#include "tensorflow/core/util/work_sharder.h"
#include "bboxes.h"
#include "wtoolkit.h"

using namespace tensorflow;
using namespace std;

typedef Eigen::ThreadPoolDevice CPUDevice;
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * k:结果最多只包含k个
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("BoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
	.Attr("classes_wise:bool")
	.Attr("k:int")
    .Input("bottom_box: T")
    .Input("classes: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsOp: public OpKernel {
	public:
		explicit BoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const int    data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const int    loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat[i];
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise && (bottom_classes_flat[j] != iclass)) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold) continue;
						keep_mask[j] = false;
					}
				}
			}

			const auto _out_size = count(keep_mask.begin(),keep_mask.end(),true);
			const auto out_size = k_>0?std::min<int>(k_,_out_size):_out_size;
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
				if(j>=out_size)
					break;
			}
		}
	private:
		float threshold    = 0.2;
		bool  classes_wise = true;
		int   k_           = -1;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsOp<CPUDevice, float>);
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * group:[Z,2]分组信息，分别为一个组里标签的开始与结束编号，不在分组信息的的默认为一个组
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("GroupBoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
    .Input("bottom_box: T")
    .Input("classes: int32")
    .Input("group: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class GroupBoxesNmsOp: public OpKernel {
	public:
		explicit GroupBoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
		}

		void Compute(OpKernelContext* context) override
		{
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			const Tensor &_group              = context->input(2);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();
			auto          group               = _group.template tensor<int,2>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));
			OP_REQUIRES(context, _group.dims() == 2, errors::InvalidArgument("group data must be 2-dimensional"));

			const auto   data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat[i];
                    const auto igroup = get_group(group,iclass);
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(igroup != get_group(group,bottom_classes_flat[j])) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold) continue;
						keep_mask[j] = false;
					}
				}
			}

			const auto out_size = count(keep_mask.begin(),keep_mask.end(),true);
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
			}
		}
        template<typename TM>
        int get_group(const TM& group_data,int label)
        {
            for(auto i=0; i<group_data.dimension(0); ++i) {
                if((label>=group_data(i,0)) && (label<=group_data(i,1)))
                    return i;
            }
            return -1;
        }
	private:
		float threshold    = 0.2;
};
REGISTER_KERNEL_BUILDER(Name("GroupBoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), GroupBoxesNmsOp<CPUDevice, float>);
/*
 * 数据不需要排序
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * confidence:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("BoxesSoftNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold:float")
	.Attr("classes_wise:bool")
	.Attr("delta:float")
    .Input("bottom_box: T")
    .Input("classes: int32")
    .Input("confidence:float")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesSoftNmsOp: public OpKernel {
    public:
        struct InterData
        {
            int index;
            float score;
            bool operator<(const InterData& v)const {
                return score<v.score;
            }
        };
        explicit BoxesSoftNmsOp(OpKernelConstruction* context) : OpKernel(context) {
            OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold));
            OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
            OP_REQUIRES_OK(context, context->GetAttr("delta", &delta));
        }

        void Compute(OpKernelContext* context) override
        {
            TIME_THIS();
            const Tensor &bottom_box          = context->input(0);
            const Tensor &bottom_classes      = context->input(1);
            const Tensor &confidence          = context->input(2);
            auto          bottom_box_flat     = bottom_box.flat<T>();
            auto          bottom_classes_flat = bottom_classes.flat<int32>();
            auto          confidence_flat     = confidence.flat<float>();

            OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
            OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));
            OP_REQUIRES(context, confidence.dims() == 1, errors::InvalidArgument("confidence data must be 1-dimensional"));

            const auto   data_nr           = bottom_box.dim_size(0);
            vector<InterData> set_D(data_nr,InterData({0,0.0f}));
            vector<InterData> set_B;
            const auto   loop_end          = data_nr-1;

            for(auto i=0; i<data_nr; ++i) {
                set_D[i].index = i;
                set_D[i].score = confidence_flat.data()[i];
            }
            set_B.reserve(data_nr);

            for(auto i=0; i<data_nr; ++i) {
                auto it = max_element(set_D.begin(),set_D.end());
                if(it->score<threshold)
                    break;
                auto M = *it;
                set_D.erase(it);
                set_B.push_back(M);
                const auto index = M.index;
                const auto iclass = bottom_classes_flat(index);
                for(auto& data:set_D) {
                    const auto j = data.index;
                    if(classes_wise && (bottom_classes_flat(j) != iclass)) continue;
                    const auto iou = bboxes_jaccard(bottom_box_flat.data()+index*4,bottom_box_flat.data()+j*4);
                    if(iou>1e-2)
                        data.score *= exp(-iou*iou/delta);
                }
            }
            sort(set_B.begin(),set_B.end(),[](const InterData& lhv, const InterData& rhv){ return lhv.index<rhv.index;});
            const auto out_size = set_B.size();
            int dims_2d[2] = {int(out_size),4};
            int dims_1d[1] = {int(out_size)};
            TensorShape  outshape0;
            TensorShape  outshape1;
            Tensor      *output_box         = NULL;
            Tensor      *output_classes     = NULL;
            Tensor      *output_index = NULL;

            TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
            TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

            OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
            OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
            OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

            auto obox     = output_box->template flat<T>();
            auto oclasses = output_classes->template flat<int32>();
            auto oindex   = output_index->template flat<int32>();

            for(auto j=0; j<out_size; ++j) {
                const auto i = set_B[j].index;
                auto box = bottom_box_flat.data()+i*4;
                std::copy(box,box+4,obox.data()+4*j);
                oclasses(j) = bottom_classes_flat(i);
                oindex(j) = i;
            }
        }
    private:
        float threshold    = 0.2;
        float delta        = 2.0;
        bool  classes_wise = true;
};
REGISTER_KERNEL_BUILDER(Name("BoxesSoftNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesSoftNmsOp<CPUDevice, float>);
/*
 * 与BoxesNms的主要区别为BoxesNmsNr使用输出人数来进行处理
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_indices:[X]
 * 输出时的相对位置不能改变
 * 程序会自动改变threshold的方式来使输出box的数量为k个
 */
REGISTER_OP("BoxesNmsNr")
    .Attr("T: {float, double,int32}")
	.Attr("classes_wise:bool")
	.Attr("k:int")
	.Attr("max_loop:int")
    .Input("bottom_box: T")
    .Input("classes:int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			int k = 0;
			c->GetAttr("k",&k);
			c->set_output(0, c->Matrix(k, 4));
			c->set_output(1, c->Vector(k));
			c->set_output(2, c->Vector(k));
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsNrOp: public OpKernel {
	public:
		explicit BoxesNmsNrOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise_));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("max_loop", &max_loop_));
            if(k_ <= 0) k_ = 8;
            if(max_loop_ <= 0) max_loop_ = 4;
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>();
			auto          bottom_classes_flat = bottom_classes.flat<int32>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr               = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			vector<bool> old_keep_mask(data_nr,true);
			const auto   loop_end              = data_nr-1;
			int          old_nr                = 0;

            auto loop_fn = [&](float threshold) {
                if(old_nr>k_)
				    std::swap(old_keep_mask,keep_mask);
				fill(keep_mask.begin(),keep_mask.end(),true);
                auto keep_nr = keep_mask.size();
				for(auto i=0; i<loop_end; ++i) {
					if(!keep_mask.at(i)) continue;
					const auto iclass = bottom_classes_flat(i);
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise_ && (bottom_classes_flat(j) != iclass)) continue;
						if(!keep_mask.at(j)) continue;
						if(bboxes_jaccard(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4) < threshold) continue;
						keep_mask[j] = false;
						--keep_nr;
					}
				}
				//cout<<"keep nr:"<<keep_nr<<", threshold "<<threshold<<endl;
			   //keep_nr = count(keep_mask.begin(),keep_mask.end(),true);
               return keep_nr;
            };

            auto threshold_low   = 0.0;
            auto threshold_hight = 1.0;

            for(auto i=0; i<max_loop_; ++i) {
                auto threshold = (threshold_low+threshold_hight)/2.0;
                auto nr = loop_fn(threshold);
                old_nr = nr;
                if(nr == k_) break;
                if(nr>k_)
                    threshold_hight = threshold;
                else
                    threshold_low = threshold;
            }

			auto out_size = count(keep_mask.begin(),keep_mask.end(),true);

            if(k_>data_nr) k_ = data_nr;

            if(out_size<k_) {
                auto delta = k_-out_size;
                for(auto it=keep_mask.begin(); it!=keep_mask.end(); ++it)
                    if((*it) == false) {
                        (*it) = true;
                        --delta;
                        if(0 == delta) break;
                    }
                out_size = count(keep_mask.begin(),keep_mask.end(),true);
            } else if(out_size>k_) {
                auto nr = out_size-k_;
                for(auto it = keep_mask.rbegin(); it!=keep_mask.rend(); ++it) {
                    if((*it) == true) {
                        *it = false;
                        --nr;
                        if(0 == nr) break;
                    }
                }
            }
            out_size = k_;

			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();
			int  j        = 0;
			int  i        = 0;

			for(i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat.data()+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat(i);
				oindex(j) = i;
				++j;
				if(j>=out_size) break;
			}
            if(j<out_size) {
				cout<<"out size = "<<out_size<<", in size = "<<data_nr<<", j= "<<j<<std::endl;
                auto i = data_nr-1;
                for(;j<out_size; ++j) {
                    auto box = bottom_box_flat.data()+i*4;
                    std::copy(box,box+4,obox.data()+4*j);
                    oclasses(j) = bottom_classes_flat(i);
                    oindex(j) = i;
                }
            }
		}
	private:
		bool  classes_wise_ = true;
		int   k_            = 0;
		int   max_loop_     = 4;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNmsNr").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsNrOp<CPUDevice, float>);
/*
 * 与BoxesNmsNr的主要区别, 使用输入的theshold进行处理，选靠前的nr个boxes, 如果NMS后没有足够的boxes部分被删除的boxes会重新加入进来
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * threshold: 第一次处理时使用的阀值，与普通NMS中阀值的用法一致
 * fast_mode: True, 表示结果中box较少时，会简单的从前到后添加已经标记为删除的box, 否则会使用一种启发式的方式添加（规则类似于soft-nms)
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_indices:[X]
 * 输出时的相对位置不能改变
 * 程序会自动改变threshold的方式来使输出box的数量为k个
 */
REGISTER_OP("BoxesNmsNr2")
    .Attr("T: {float, double,int32}")
	.Attr("classes_wise:bool")
	.Attr("k:int")
	.Attr("threshold:float")
	.Attr("fast_mode:bool")
	.Attr("allow_less_output:bool=False")
    .Input("bottom_box: T")
    .Input("classes:int32")
    .Input("confidence:T")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			int k = 0;
            bool allow_less_output = false;
			c->GetAttr("k",&k);
			c->GetAttr("allow_less_output",&allow_less_output);
            if(allow_less_output) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
            } else {
			c->set_output(0, c->Matrix(k, 4));
			c->set_output(1, c->Vector(k));
			c->set_output(2, c->Vector(k));
            }
			return Status::OK();
			});

template <typename Device, typename T>
class BoxesNmsNr2Op: public OpKernel {
    private:
        struct InterData
        {
            InterData(int i,float s):index(i),score(s){}
            int index;
            float score;
            bool operator<(const InterData& v)const {
                return score<v.score;
            }
        };
	public:
		explicit BoxesNmsNr2Op(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise_));
			OP_REQUIRES_OK(context, context->GetAttr("threshold", &threshold_));
			OP_REQUIRES_OK(context, context->GetAttr("fast_mode", &fast_mode_));
			OP_REQUIRES_OK(context, context->GetAttr("k", &k_));
			OP_REQUIRES_OK(context, context->GetAttr("allow_less_output", &allow_less_output_));
            if(k_ <= 0) k_ = 1;
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THISV1("NmsNr2");
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			const Tensor &confidence          = context->input(2);
			auto          bottom_box_flat     = bottom_box.flat<T>().data();
			auto          bottom_classes_flat = bottom_classes.flat<int32>().data();
			auto          confidence_flat     = confidence.flat<T>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr               = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end              = data_nr-1;

            if((k_>data_nr) && (!allow_less_output_)) {
                cout<<"ERROR NMSNR2 input size is less than require ("<<data_nr<<" vs "<<k_<<")."<<endl;
            }

            if(k_<data_nr){
				for(auto i=0; i<loop_end; ++i) {
					if(!keep_mask.at(i)) continue;
					const auto iclass = bottom_classes_flat[i];
                    const auto src_box = bottom_box_flat+i*4;
					for(auto j=i+1; j<data_nr; ++j) {
						if(classes_wise_ && (bottom_classes_flat[j] != iclass)) continue;
						if(!keep_mask[j]) continue;
						if(bboxes_jaccard(src_box,bottom_box_flat+j*4) < threshold_) continue;
						keep_mask[j] = false;
					}
				}
            };
			auto out_size = count(keep_mask.begin(),keep_mask.end(),true);

            if(out_size<k_) {
                if(k_<data_nr) {
                    /*
                     * 处理返回的box过少的情况
                     */
                    if(fast_mode_) {
                        /*
                         * 快速模式，从前往后加入已经删除的box
                         */
                        auto nr = k_-out_size;
                        for(auto it = keep_mask.begin(); it!=keep_mask.end(); ++it) {
                            if((*it) == false) {
                                *it = true;
                                --nr;
                                if(0 == nr) break;
                            }
                        }
                    } else {
                        /*
                         * 使用启发的方式添加box
                         */
                        auto delta = k_-out_size;
                        vector<InterData> datas;
                        datas.reserve((data_nr-out_size)/2);

                        for(auto it=keep_mask.begin(); it!=keep_mask.end(); ++it)
                            if((*it) == false) {
                                auto index = std::distance(keep_mask.begin(),it);
                                auto score = confidence_flat.data()[index];
                                for(auto kt=keep_mask.begin(); kt != it; ++kt) {
                                    if((*kt) == false)
                                        continue;
                                    auto index0 = std::distance(keep_mask.begin(),kt);
                                    auto iou = bboxes_jaccard(bottom_box_flat+index*4,bottom_box_flat+index0*4);
                                    score *= (1.0-iou);
                                }
                                datas.emplace_back(int(index),score);
                            }
                        for(auto x=0; x<delta; ++x) {
                            auto jt = max_element(datas.begin(),datas.end());
                            const auto index = jt->index;
                            keep_mask[jt->index] = true;
                            datas.erase(jt);
                            for(auto jt=datas.begin(); jt!=datas.end(); ++jt) {
                                auto index0 = jt->index;
                                auto iou = bboxes_jaccard(bottom_box_flat+index*4,bottom_box_flat+index0*4);
                                jt->score *= (1.0-iou);
                            }
                        }
                    }
                    out_size = k_;
                } else if(allow_less_output_)  {
                    out_size = data_nr;
                }
            } else if(out_size>k_) {
              /*
               * 如果返回的box过多，简单的从后往前删除多余的box
               */
                auto nr = out_size-k_;
                for(auto it = keep_mask.rbegin(); it!=keep_mask.rend(); ++it) {
                    if((*it) == true) {
                        *it = false;
                        --nr;
                        if(0 == nr) break;
                    }
                }
                out_size = k_;
            }

			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();
			int  j        = 0;
			int  i        = 0;

            obox.setZero();
            oindex.setZero();
            oclasses.setZero();

			for(i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat[i];
				oindex(j) = i;
				++j;
				if(j>=out_size) break;
			}
            /*
            if(j<out_size) {
				cout<<"out size = "<<out_size<<", in size = "<<data_nr<<", j= "<<j<<std::endl;
                auto i = data_nr-1;
                for(;j<out_size; ++j) {
                    auto box = bottom_box_flat+i*4;
                    std::copy(box,box+4,obox.data()+4*j);
                    oclasses(j) = bottom_classes_flat[i];
                    oindex(j) = i;
                }
            }
            */
		}
	private:
		bool  classes_wise_      = true;
		float threshold_         = 0.0;
		int   k_                 = 0;
		bool  fast_mode_         = false;
		bool  allow_less_output_ = false;
};
REGISTER_KERNEL_BUILDER(Name("BoxesNmsNr2").Device(DEVICE_CPU).TypeConstraint<float>("T"), BoxesNmsNr2Op<CPUDevice, float>);
/*
 * 要求数据已经按重要呈度从0到data_nr排好了序(从大到小）
 * 用于处理目标不会重叠的情况
 * bottom_box:[X,4](ymin,xmin,ymax,xmax)
 * classes:[X]
 * output_box:[Y,4]
 * output_classes:[Y]
 * output_index:[Y]
 * 输出时的相对位置不能改变
 */
REGISTER_OP("NoOverlapBoxesNms")
    .Attr("T: {float, double,int32}")
	.Attr("threshold0:float")
	.Attr("threshold1:float")
	.Attr("classes_wise:bool")
    .Input("bottom_box: T")
    .Input("classes: int32")
	.Output("output_box:T")
	.Output("output_classes:int32")
	.Output("output_index:int32")
	.SetShapeFn([](shape_inference::InferenceContext* c) {
			c->set_output(0, c->Matrix(-1, 4));
			c->set_output(1, c->Vector(-1));
			c->set_output(2, c->Vector(-1));
			return Status::OK();
			});

template <typename Device, typename T>
class NoOverlapBoxesNmsOp: public OpKernel {
	public:
		explicit NoOverlapBoxesNmsOp(OpKernelConstruction* context) : OpKernel(context) {
			OP_REQUIRES_OK(context, context->GetAttr("threshold0", &threshold0));
			OP_REQUIRES_OK(context, context->GetAttr("threshold1", &threshold1));
			OP_REQUIRES_OK(context, context->GetAttr("classes_wise", &classes_wise));
		}

		void Compute(OpKernelContext* context) override
		{
            TIME_THIS();
			const Tensor &bottom_box          = context->input(0);
			const Tensor &bottom_classes      = context->input(1);
			auto          bottom_box_flat     = bottom_box.flat<T>();
			auto          bottom_classes_flat = bottom_classes.flat<int32>();

			OP_REQUIRES(context, bottom_box.dims() == 2, errors::InvalidArgument("box data must be 2-dimensional"));
			OP_REQUIRES(context, bottom_classes.dims() == 1, errors::InvalidArgument("classes data must be 1-dimensional"));

			const auto   data_nr           = bottom_box.dim_size(0);
			vector<bool> keep_mask(data_nr,true);
			const auto   loop_end          = data_nr-1;

			for(auto i=0; i<loop_end; ++i) {
				if(keep_mask[i]) {
					const auto iclass = bottom_classes_flat(i);
                    for(auto j=i+1; j<data_nr; ++j) {
                        if(classes_wise && (bottom_classes_flat(j) != iclass)) continue;
                        if((bboxes_jaccard(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4) < threshold0)
                                && (bboxes_jaccard_of_box0(bottom_box_flat.data()+i*4,bottom_box_flat.data()+j*4)<threshold1)
                                && (bboxes_jaccard_of_box0(bottom_box_flat.data()+j*4,bottom_box_flat.data()+i*4)<threshold1)
                          ) continue;
                        keep_mask[j] = false;
                    }
				}
			}

			const auto out_size = count(keep_mask.begin(),keep_mask.end(),true);
			int dims_2d[2] = {int(out_size),4};
			int dims_1d[1] = {int(out_size)};
			TensorShape  outshape0;
			TensorShape  outshape1;
			Tensor      *output_box         = NULL;
			Tensor      *output_classes     = NULL;
			Tensor      *output_index = NULL;

			TensorShapeUtils::MakeShape(dims_2d, 2, &outshape0);
			TensorShapeUtils::MakeShape(dims_1d, 1, &outshape1);

			OP_REQUIRES_OK(context, context->allocate_output(0, outshape0, &output_box));
			OP_REQUIRES_OK(context, context->allocate_output(1, outshape1, &output_classes));
			OP_REQUIRES_OK(context, context->allocate_output(2, outshape1, &output_index));

			auto obox     = output_box->template flat<T>();
			auto oclasses = output_classes->template flat<int32>();
			auto oindex   = output_index->template flat<int32>();

			for(int i=0,j=0; i<data_nr; ++i) {
				if(!keep_mask[i]) continue;
				auto box = bottom_box_flat.data()+i*4;
				std::copy(box,box+4,obox.data()+4*j);
				oclasses(j) = bottom_classes_flat(i);
				oindex(j) = i;
				++j;
			}
		}
	private:
		float threshold0    = 0.2;
		float threshold1    = 0.8;
		bool  classes_wise = true;
};
REGISTER_KERNEL_BUILDER(Name("NoOverlapBoxesNms").Device(DEVICE_CPU).TypeConstraint<float>("T"), NoOverlapBoxesNmsOp<CPUDevice, float>);


#include <random>
#include <Eigen/Dense>
#include <gtest/gtest.h>
#include <aikido/common/RNG.hpp>
#include <aikido/constraint/CyclicSampleable.hpp>
#include <aikido/constraint/FiniteSampleable.hpp>
#include <aikido/constraint/dart/InverseKinematicsSampleable.hpp>
#include <aikido/constraint/dart/TSR.hpp>
#include <aikido/statespace/Rn.hpp>
#include <aikido/statespace/SE3.hpp>
#include <aikido/statespace/SO2.hpp>
#include <aikido/statespace/StateSpace.hpp>
#include "MockConstraints.hpp"

using aikido::statespace::R2;
using aikido::constraint::FiniteSampleable;
using aikido::constraint::dart::InverseKinematicsSampleable;
using aikido::constraint::CyclicSampleable;
using aikido::statespace::SE3;
using aikido::statespace::SO2;
using dart::dynamics::FreeJoint;
using aikido::constraint::SampleGenerator;
using aikido::constraint::dart::TSR;
using aikido::common::RNGWrapper;
using aikido::common::RNG;
using aikido::statespace::dart::MetaSkeletonStateSpace;
using aikido::statespace::dart::MetaSkeletonStateSpacePtr;
using dart::dynamics::Skeleton;
using dart::dynamics::SkeletonPtr;
using dart::dynamics::BodyNode;
using dart::dynamics::BodyNodePtr;
using dart::dynamics::RevoluteJoint;
using dart::dynamics::InverseKinematics;
using dart::dynamics::InverseKinematicsPtr;

static BodyNode::Properties create_BodyNodeProperties(const std::string& _name)
{
  BodyNode::Properties bodyProperties;
  bodyProperties.mName = _name;
  return bodyProperties;
}

class InverseKinematicsSampleableTest : public ::testing::Test
{
protected:
  void SetUp() override
  {
    mTsr.reset(new TSR);
    mRng.reset(new RNGWrapper<std::default_random_engine>());

    // Manipulator with 2 revolute joints.
    mManipulator1 = Skeleton::create("Manipulator1");

    // Root joint
    RevoluteJoint::Properties properties1;
    properties1.mAxis = Eigen::Vector3d::UnitY();
    properties1.mName = "Joint1";

    bn1 = mManipulator1
              ->createJointAndBodyNodePair<RevoluteJoint>(
                  nullptr, properties1, create_BodyNodeProperties("root_body"))
              .second;

    // joint 2, body 2
    RevoluteJoint::Properties properties2;
    properties2.mAxis = Eigen::Vector3d::UnitY();
    properties2.mName = "Joint2";
    properties2.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, 1);
    bn2 = mManipulator1
              ->createJointAndBodyNodePair<RevoluteJoint>(
                  bn1, properties2, create_BodyNodeProperties("second_body"))
              .second;

    mInverseKinematics1 = InverseKinematics::create(bn2);
    mStateSpace1
        = std::make_shared<MetaSkeletonStateSpace>(mManipulator1.get());

    // Manipulator with 1 free joint and 1 revolute joint.
    mManipulator2 = Skeleton::create("Manipulator2");

    // Root joint
    FreeJoint::Properties properties3;
    properties3.mName = "Joint1";
    bn3 = mManipulator2
              ->createJointAndBodyNodePair<FreeJoint>(
                  nullptr, properties3, create_BodyNodeProperties("root_body"))
              .second;

    // Joint 2, body 2
    RevoluteJoint::Properties properties4;
    properties4.mAxis = Eigen::Vector3d::UnitY();
    properties4.mName = "Joint2";
    properties4.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, 1);
    bn4 = mManipulator2
              ->createJointAndBodyNodePair<RevoluteJoint>(
                  bn3, properties4, create_BodyNodeProperties("second_body"))
              .second;

    mInverseKinematics2 = InverseKinematics::create(bn4);
    mStateSpace2
        = std::make_shared<MetaSkeletonStateSpace>(mManipulator2.get());

    // Set FiniteSampleable to generate pose close to the actual solution.
    auto seedState
        = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());
    seedState.getSubStateHandle<SO2>(0).fromAngle(0.1);
    seedState.getSubStateHandle<SO2>(1).fromAngle(0.1);
    seedConstraint
        = std::make_shared<FiniteSampleable>(mStateSpace1, seedState);
  }

  std::shared_ptr<TSR> mTsr;
  std::unique_ptr<RNG> mRng;

  SkeletonPtr mManipulator1;
  MetaSkeletonStateSpacePtr mStateSpace1;
  InverseKinematicsPtr mInverseKinematics1;
  BodyNodePtr bn1, bn2;

  SkeletonPtr mManipulator2;
  MetaSkeletonStateSpacePtr mStateSpace2;
  InverseKinematicsPtr mInverseKinematics2;
  BodyNodePtr bn3, bn4;
  std::shared_ptr<FiniteSampleable> seedConstraint;
};

TEST_F(InverseKinematicsSampleableTest, ConstructorThrowsOnNullStateSpace)
{
  EXPECT_THROW(
      InverseKinematicsSampleable(
          nullptr, nullptr, mTsr, seedConstraint, mInverseKinematics1, 1),
      std::invalid_argument);
}

TEST_F(
    InverseKinematicsSampleableTest, ConstructorThrowsOnNullInverseKinematics)
{
  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1, mManipulator1, mTsr, seedConstraint, nullptr, 1),
      std::invalid_argument);
}

TEST_F(InverseKinematicsSampleableTest, ConstructorThrowsOnNullPoseConstraint)
{
  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1,
          mManipulator1,
          nullptr,
          seedConstraint,
          mInverseKinematics1,
          1),
      std::invalid_argument);
}

TEST_F(InverseKinematicsSampleableTest, ConstructorThrowsOnNullSeedConstraint)
{
  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1, mManipulator1, mTsr, nullptr, mInverseKinematics1, 1),
      std::invalid_argument);
}

TEST_F(
    InverseKinematicsSampleableTest, ConstructorThrowsOnSeedConstraintMismatch)
{
  auto ss = std::make_shared<MetaSkeletonStateSpace>(mManipulator1.get());
  auto st = ss->createState();
  auto sc = std::make_shared<FiniteSampleable>(ss, st);
  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1, mManipulator1, mTsr, sc, mInverseKinematics1, 1),
      std::invalid_argument);
}

TEST_F(InverseKinematicsSampleableTest, ConstructorThrowsOnNegativeTrials)
{
  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1,
          mManipulator1,
          mTsr,
          seedConstraint,
          mInverseKinematics1,
          -1),
      std::invalid_argument);
}

TEST_F(InverseKinematicsSampleableTest, ConstructorThrowsOnInvalidSeed)
{
  // Invalid statespace for seed constraint.
  Eigen::Vector2d v(1, 0);
  R2 rvss;
  auto seedStateInvalid = rvss.createState();
  seedStateInvalid.setValue(v);

  std::shared_ptr<FiniteSampleable> invalid_seed_constraint(
      new FiniteSampleable(std::make_shared<R2>(rvss), seedStateInvalid));

  EXPECT_THROW(
      InverseKinematicsSampleable(
          mStateSpace1,
          mManipulator1,
          mTsr,
          invalid_seed_constraint,
          mInverseKinematics1,
          1),
      std::invalid_argument);
}

TEST_F(InverseKinematicsSampleableTest, Constructor)
{
  // Construct valid seed_constraint.
  auto seedStateValid = mStateSpace1->createState();
  std::shared_ptr<FiniteSampleable> valid_seed_constraint(
      new FiniteSampleable(mStateSpace1, seedStateValid));

  InverseKinematicsSampleable ikConstraint(
      mStateSpace1,
      mManipulator1,
      mTsr,
      valid_seed_constraint,
      mInverseKinematics1,
      1);
}

TEST_F(InverseKinematicsSampleableTest, SingleSampleGenerator)
{
  // Set mTSR to be a pointTSR that generates
  //  the only feasible solution for mInverseKinematics1.
  Eigen::Isometry3d T0_w(Eigen::Isometry3d::Identity());
  T0_w.translation() = Eigen::Vector3d(0, 0, 1);
  mTsr->mT0_w = T0_w;

  // Set FiniteSampleable to generate pose close to the actual solution.
  auto seedState
      = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());
  seedState.getSubStateHandle<SO2>(0).fromAngle(0.1);
  seedState.getSubStateHandle<SO2>(1).fromAngle(0.1);

  std::shared_ptr<FiniteSampleable> seedConstraint(
      new FiniteSampleable(mStateSpace1, seedState));

  // Construct InverseKinematicsSampleable
  InverseKinematicsSampleable ikConstraint(
      mStateSpace1,
      mManipulator1,
      mTsr,
      seedConstraint,
      mInverseKinematics1,
      1);

  // Get IkSampleGenerator
  auto generator = ikConstraint.createSampleGenerator();

  ASSERT_TRUE(generator->canSample());
  ASSERT_EQ(generator->getNumSamples(), 1);
  auto state
      = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());

  ASSERT_TRUE(generator->sample(state));
  ASSERT_NEAR(state.getSubStateHandle<SO2>(0).toAngle(), 0, 1e-5);
  ASSERT_NEAR(state.getSubStateHandle<SO2>(1).toAngle(), 0, 1e-5);

  // Cannot sample anymore.
  ASSERT_FALSE(generator->canSample());
}

TEST_F(InverseKinematicsSampleableTest, CyclicSampleGenerator)
{
  // Set mTSR to be a pointTSR that generates
  //  the only feasible solution for mInverseKinematics1.
  Eigen::Isometry3d T0_w(Eigen::Isometry3d::Identity());
  T0_w.translation() = Eigen::Vector3d(0, 0, 1);
  mTsr->mT0_w = T0_w;

  // Set CyclicSampleable to generate
  // pose close to the actual solution.
  auto seedState
      = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());
  seedState.getSubStateHandle<SO2>(0).fromAngle(0.1);
  seedState.getSubStateHandle<SO2>(1).fromAngle(0.1);

  std::shared_ptr<FiniteSampleable> finiteSampleConstraint
      = std::make_shared<FiniteSampleable>(mStateSpace1, seedState);

  std::shared_ptr<CyclicSampleable> seedConstraint(
      new CyclicSampleable(finiteSampleConstraint));

  std::shared_ptr<CyclicSampleable> tsrConstraint(new CyclicSampleable(mTsr));

  // Construct InverseKinematicsSampleable
  InverseKinematicsSampleable ikConstraint(
      mStateSpace1,
      mManipulator1,
      tsrConstraint,
      seedConstraint,
      mInverseKinematics1,
      1);

  // Get IkSampleGenerator
  auto generator = ikConstraint.createSampleGenerator();

  for (int i = 1; i < 10; ++i)
  {
    ASSERT_TRUE(generator->canSample());
    ASSERT_EQ(generator->getNumSamples(), SampleGenerator::NO_LIMIT);
    auto state
        = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());

    ASSERT_TRUE(generator->sample(state));
    ASSERT_NEAR(state.getSubStateHandle<SO2>(0).toAngle(), 0, 1e-5);
    ASSERT_NEAR(state.getSubStateHandle<SO2>(1).toAngle(), 0, 1e-5);
  }
}

TEST_F(InverseKinematicsSampleableTest, MultipleGeneratorsSampleSameSequence)
{
  // TSR constraint will generate sequence of different points.
  Eigen::MatrixXd Bw = Eigen::Matrix<double, 6, 2>::Zero();
  Bw(2, 0) = 1;
  Bw(2, 1) = 3;
  mTsr->mBw = Bw;

  // Set CyclicSampleable to generate
  // pose close to the actual solution.
  auto seedState
      = mStateSpace2->getScopedStateFromMetaSkeleton(mManipulator2.get());
  Eigen::Isometry3d isometry(Eigen::Isometry3d::Identity());
  isometry.translation() = Eigen::Vector3d(0.1, 0.1, 0.1);
  seedState.getSubStateHandle<SE3>(0).setIsometry(isometry);
  seedState.getSubStateHandle<SO2>(1).fromAngle(0.1);

  std::shared_ptr<FiniteSampleable> finiteSampleConstraint
      = std::make_shared<FiniteSampleable>(mStateSpace2, seedState);

  std::shared_ptr<CyclicSampleable> seedConstraint(
      new CyclicSampleable(finiteSampleConstraint));

  InverseKinematicsSampleable ikConstraint(
      mStateSpace2,
      mManipulator2,
      mTsr,
      seedConstraint,
      mInverseKinematics2,
      1);

  // Get 2 IkSampleGenerator
  auto generator1 = ikConstraint.createSampleGenerator();
  auto generator2 = ikConstraint.createSampleGenerator();

  // Test the two generators' samples for equality.
  for (int i = 0; i < 10; i++)
  {
    ASSERT_TRUE(generator1->canSample());
    ASSERT_TRUE(generator2->canSample());

    auto state1
        = mStateSpace2->getScopedStateFromMetaSkeleton(mManipulator2.get());
    auto state2
        = mStateSpace2->getScopedStateFromMetaSkeleton(mManipulator2.get());

    ASSERT_TRUE(generator1->sample(state1));
    ASSERT_TRUE(generator2->sample(state2));

    EXPECT_TRUE(
        state1.getSubStateHandle<SE3>(0).getIsometry().isApprox(
            state2.getSubStateHandle<SE3>(0).getIsometry()));

    EXPECT_DOUBLE_EQ(
        state1.getSubStateHandle<SO2>(1).toAngle(),
        state2.getSubStateHandle<SO2>(1).toAngle());
  }
}

TEST_F(InverseKinematicsSampleableTest, SampleGeneratorIkInfeasible)
{
  // Tests that generator returns false when IK is infeasible.

  bn1->getParentJoint()->setPosition(0, M_PI / 4);
  mTsr->mT0_w = bn2->getTransform();

  /// Set first joint to be a fixed joint.
  bn1->getParentJoint()->setPositionLowerLimit(0, 0);
  bn1->getParentJoint()->setPositionUpperLimit(0, 0);

  // Set CyclicSampleable.
  auto seedState
      = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());
  seedState.getSubStateHandle<SO2>(0).fromAngle(0);
  seedState.getSubStateHandle<SO2>(1).fromAngle(0.1);

  std::shared_ptr<FiniteSampleable> finiteSampleConstraint
      = std::make_shared<FiniteSampleable>(mStateSpace1, seedState);

  std::shared_ptr<CyclicSampleable> seedConstraint(
      new CyclicSampleable(finiteSampleConstraint));

  InverseKinematicsSampleable ikConstraint(
      mStateSpace1,
      mManipulator1,
      mTsr,
      seedConstraint,
      mInverseKinematics1,
      1);

  auto generator = ikConstraint.createSampleGenerator();

  ASSERT_TRUE(generator->canSample());

  auto state
      = mStateSpace1->getScopedStateFromMetaSkeleton(mManipulator1.get());
  ASSERT_FALSE(generator->sample(state));
}


#ifndef EIGEN_YAML_H
#define EIGEN_YAML_H
#include <Eigen/Core>
#include <yaml-cpp/yaml.h>

namespace YAML {
template<>
struct convert<Eigen::MatrixXd> {
    static Node encode(const Eigen::MatrixXd& rhs) {
        Node node;
        node["rows"] = rhs.rows();
        node["cols"] = rhs.cols();
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                node["data"].push_back(rhs(i,j));
            }
        }
        return node;
    }

    static bool decode(const Node& node, Eigen::MatrixXd& rhs) {

        auto nrows = node["rows"];
        if(!nrows.IsDefined()){
            return false;
        }
        std::size_t rows = nrows.as<std::size_t>();

        auto ncols = node["cols"];
        if(!ncols.IsDefined()){
            return false;
        }
        std::size_t cols = ncols.as<std::size_t>();

        rhs = Eigen::MatrixXd::Zero(rows, cols);

        auto data = node["data"];
        if(!data.IsDefined()){
            return false;
        }
        if(!data.IsSequence() || data.size() != rows*cols) {
            return false;
        }
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                std::size_t id = i*cols + j;
                rhs(i,j) = data[id].as<double>();
            }
        }

        return true;
    }
};

template<>
struct convert<Eigen::VectorXd> {
    static Node encode(const Eigen::VectorXd& rhs) {
        Node node;
        node["rows"] = rhs.rows();
        node["cols"] = rhs.cols();
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                node["data"].push_back(rhs(i,j));
            }
        }
        return node;
    }

    static bool decode(const Node& node, Eigen::VectorXd& rhs) {

        auto nrows = node["rows"];
        if(!nrows.IsDefined()){
            return false;
        }
        std::size_t rows = nrows.as<std::size_t>();

        auto ncols = node["cols"];
        if(!ncols.IsDefined()){
            return false;
        }
        std::size_t cols = ncols.as<std::size_t>();

        rhs = Eigen::VectorXd::Zero(rows, cols);

        auto data = node["data"];
        if(!data.IsDefined()){
            return false;
        }
        if(!data.IsSequence() || data.size() != rows*cols) {
            return false;
        }
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                std::size_t id = i*cols + j;
                rhs(i,j) = data[id].as<double>();
            }
        }

        return true;
    }
};

template<>
struct convert<Eigen::VectorXi> {
    static Node encode(const Eigen::VectorXi& rhs) {
        Node node;
        node["rows"] = rhs.rows();
        node["cols"] = rhs.cols();
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                node["data"].push_back(rhs(i,j));
            }
        }
        return node;
    }

    static bool decode(const Node& node, Eigen::VectorXi& rhs) {

        auto nrows = node["rows"];
        if(!nrows.IsDefined()){
            return false;
        }
        std::size_t rows = nrows.as<std::size_t>();

        auto ncols = node["cols"];
        if(!ncols.IsDefined()){
            return false;
        }
        std::size_t cols = ncols.as<std::size_t>();

        rhs = Eigen::VectorXi::Zero(rows, cols);

        auto data = node["data"];
        if(!data.IsDefined()){
            return false;
        }
        if(!data.IsSequence() || data.size() != rows*cols) {
            return false;
        }
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                std::size_t id = i*cols + j;
                rhs(i,j) = data[id].as<int>();
            }
        }

        return true;
    }
};

template<>
struct convert<Eigen::ArrayXd> {
    static Node encode(const Eigen::ArrayXd& rhs) {
        Node node;
        node["rows"] = rhs.rows();
        node["cols"] = rhs.cols();
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                node["data"].push_back(rhs(i,j));
            }
        }
        return node;
    }

    static bool decode(const Node& node, Eigen::ArrayXd& rhs) {

        auto nrows = node["rows"];
        if(!nrows.IsDefined()){
            return false;
        }
        std::size_t rows = nrows.as<std::size_t>();

        auto ncols = node["cols"];
        if(!ncols.IsDefined()){
            return false;
        }
        std::size_t cols = ncols.as<std::size_t>();

        rhs = Eigen::ArrayXd::Zero(rows, cols);

        auto data = node["data"];
        if(!data.IsDefined()){
            return false;
        }
        if(!data.IsSequence() || data.size() != rows*cols) {
            return false;
        }
        for(std::size_t i = 0; i < (std::size_t) rhs.rows(); ++i){
            for(std::size_t j = 0; j < (std::size_t) rhs.cols(); ++j){
                std::size_t id = i*cols + j;
                rhs(i,j) = data[id].as<double>();
            }
        }

        return true;
    }
};
}
#endif // EIGEN_YAML_H


//
// Copyright Jason Rice 2015
// 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 <mpdef/collect_settings.hpp>
#include <mpdef/list.hpp>
#include <mpdef/map.hpp>
#include <mpdef/tree_node.hpp>

#include <boost/hana.hpp>

namespace hana = boost::hana;

constexpr auto tag1 = hana::type_c<hana::int_<1>>;
constexpr auto tag2 = hana::type_c<hana::int_<2>>;
constexpr auto tag3 = hana::type_c<hana::int_<3>>;
constexpr auto tag4 = hana::type_c<hana::int_<4>>;

int main()
{
  {
    using mpdef::collect_settings;
    using mpdef::with_settings;

    constexpr auto xs = mpdef::make_list(
      mpdef::make_tree_node(hana::type_c<void>,
        mpdef::make_map(
          mpdef::make_tree_node(tag1, hana::int_c<42>),
          mpdef::make_tree_node(tag2, hana::int_c<256>)
        )),
      mpdef::make_tree_node(hana::type_c<void>,
        mpdef::make_map()),
      mpdef::make_tree_node(hana::type_c<void>,
        mpdef::make_map(
          mpdef::make_tree_node(tag3, hana::int_c<1024>)
        )),
      mpdef::make_tree_node(hana::type_c<void>,
        mpdef::make_map(
          mpdef::make_tree_node(tag2, hana::int_c<512>)
        ))
    );

    BOOST_HANA_CONSTANT_ASSERT(
      hana::fold_left(xs, with_settings(tag1, tag2, tag3, tag4), collect_settings)
        ==
      mpdef::make_map(
        mpdef::make_tree_node(tag1, hana::just(hana::int_c<42>)),
        mpdef::make_tree_node(tag2, hana::just(hana::int_c<512>)),
        mpdef::make_tree_node(tag3, hana::just(hana::int_c<1024>)),
        mpdef::make_tree_node(tag4, hana::nothing)
      )
    );
  }
}


/*
 * pictorial_structure.cpp
 *
 *  Created on: Jan 21, 2013
 *      Author: mdantone
 */

#include <glog/logging.h>
#include <boost/thread.hpp>
#include <boost/random.hpp>
#include "cpp/utils/thread_pool.hpp"
#include "cpp/learning/pictorial_structure/part.hpp"
#include "cpp/learning/pictorial_structure/utils.hpp"
#include "cpp/learning/pictorial_structure/math_util.hpp"
#include "cpp/learning/forest/utils/eval_utils.hpp"
using namespace std;
using namespace cv;

namespace learning
{
namespace ps
{

const Part* Part::get_child(int i) const {
  return &childeren[i];
}

void Part::set_parent(Part* parent_) {
  parent = parent_;
}

const Part* Part::get_parent() const {
  return parent;
}

void Part::make_leaf() {
  childeren.clear();
//  displacement.shifts.clear();
//  displacement.cost_functions_x.clear();
//  displacement.cost_functions_y.clear();
//  displacement.pair_weights.clear();
//  displacement.weights.clear();
}

void Part::clear(bool recursive) {
  if( appereance_score.data != NULL)
    appereance_score.release();
  for( int i = 0; i < childeren.size() && recursive; i++){
    childeren[i].clear(recursive);
  }
}

void Part::set_deformation_cost(Displacement disp_cost) {
  displacement = disp_cost;
}

void Part::set_voting_map(const Mat_<float>& map, int do_normalize) {
  appereance_score = map.clone();
  appereance_score *= -1;

//  if(do_normalize < 0 ) {
//    // no normalization
//  }else if(do_normalize == 0) {
//    normalize(appereance_score, appereance_score, 0, 1, CV_MINMAX);
//  }else if(do_normalize == 1){
//    appereance_score /= norm_factor_forest;
//  }
}

void do_shifts_and_dt(const cv::Mat_<float>& score,
          const vector<cv::Point>& shifts,
          const vector<Quadratic>& cost_functions_x,
          const vector<Quadratic>& cost_functions_y,
          const std::vector<float>& weights,
          cv::Mat_<float>& score_out,
          cv::Mat_<int>& d_x_out,
          cv::Mat_<int>& d_y_out) {

  vector<Mat_<float> > scores(shifts.size());
  vector<Mat_<int> > d_xs(shifts.size());
  vector<Mat_<int> > d_ys(shifts.size());

  Mat_<int> Ix_dt, Iy_dt;

  DistanceTransform<float> dt_;
  Mat_<float> s = Mat_<float>(score).clone();

  for(int i =0; i < shifts.size(); i++) {
    const Point& shift = shifts[i];

    const Quadratic& fx = cost_functions_x[i];
    const Quadratic& fy = cost_functions_y[i];

    dt_.compute(s, fx, fy, shift,
        scores[i], d_xs[i], d_ys[i]);
  }

  learning::ps::utils::reduce_min(scores, d_xs, d_ys, weights, score_out, d_x_out, d_y_out );
}

void Part::transform(const Mat_<float> score_tmp,
    Mat_<float>& score,
    Mat_<int>& score_x,
    Mat_<int>& score_y ) const {

  // distance transform
  do_shifts_and_dt(score_tmp,
      displacement.shifts,
      displacement.cost_functions_x,
      displacement.cost_functions_y,
      displacement.weights,
      score,
      score_x,
      score_y);
}

void Part::transform_d(const cv::Mat_<float> score_tmp,
      cv::Mat_<float>& score,
      cv::Mat_<int>& score_x,
      cv::Mat_<int>& score_y) const {

  unsigned int nClusters = displacement.shifts.size();
  std::vector<cv::Point> shifts_d(nClusters);
  std::vector<Quadratic> cost_functions_x_d(nClusters);
  std::vector<Quadratic> cost_functions_y_d(nClusters);

  for(unsigned int i = 0; i < nClusters; i++){
    shifts_d[i] = cv::Point(1-displacement.shifts[i].x, 1-displacement.shifts[i].y);
    Quadratic fx = displacement.cost_functions_x[i];
    Quadratic fy = displacement.cost_functions_y[i];
    cost_functions_x_d[i] = Quadratic(fx.a, -1*fx.b);
    cost_functions_y_d[i] = Quadratic(fy.a, -1*fy.b);
  }

  //distance transform
  do_shifts_and_dt(score_tmp, shifts_d,
                   cost_functions_x_d,
                   cost_functions_y_d,
                   displacement.weights,
                   score,
                   score_x,
                   score_y
                   );
}

} //namespace ps

} //namespace learning


#ifndef PYTHONIC_INCLUDE_NUMPY_SUM_HPP
#define PYTHONIC_INCLUDE_NUMPY_SUM_HPP

#include "pythonic/include/numpy/reduce.hpp"
#include "pythonic/include/operator_/iadd.hpp"
#include "pythonic/include/utils/functor.hpp"

#ifdef USE_BOOST_SIMD
#include <boost/simd/function/sum.hpp>
#endif

namespace pythonic
{

  namespace numpy
  {

    template <class E>
    auto sum(E &&e)
        -> decltype(reduce<operator_::functor::iadd>(std::forward<E>(e)));

    template <class E, class Opt>
    auto sum(E &&e, Opt &&opt)
        -> decltype(reduce<operator_::functor::iadd>(std::forward<E>(e),
                                                     std::forward<Opt>(opt)));

    DECLARE_FUNCTOR(pythonic::numpy, sum);
  }
}

#endif


/******************************************************************************
 *
 *  filename: Light.h
 *  author  : Do Won Cha
 *
 *****************************************************************************/

#pragma once

#ifndef _RAY_LIGHT_
#define _RAY_LIGHT_

#include <Eigen/Core>
#include "surface.hpp"

namespace raytracer
{

using namespace Eigen;

class Light : public Node
{
public:
  Vector3f ambient_, diffuse_;
  float intensity_;
public:
  Light(Vector3f position,
        Vector3f ambient,
        Vector3f diffuse,
        float intensity = 1.0f) :
    Node(position_),
    ambient_(ambient),
    diffuse_(diffuse),
    intensity_(intensity)
  {}
};

} // end of namespace raytracer

#endif //_RAY_LIGHT guard


// Copyright 2004 The Trustees of Indiana University.

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

//  Authors: Douglas Gregor
//           Andrew Lumsdaine
#ifndef BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP
#define BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP

#include <iterator>
#include <utility>
#include <boost/random/uniform_int.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/type_traits/is_base_and_derived.hpp>
#include <boost/type_traits/is_same.hpp>

namespace boost {

  template<typename RandomGenerator, typename Graph>
  class erdos_renyi_iterator
  {
    typedef typename graph_traits<Graph>::directed_category directed_category;
    typedef typename graph_traits<Graph>::vertices_size_type vertices_size_type;
    typedef typename graph_traits<Graph>::edges_size_type edges_size_type;

    BOOST_STATIC_CONSTANT
      (bool,
       is_undirected = (is_base_and_derived<undirected_tag,
                                            directed_category>::value
                        || is_same<undirected_tag, directed_category>::value));

  public:
    typedef std::input_iterator_tag iterator_category;
    typedef std::pair<vertices_size_type, vertices_size_type> value_type;
    typedef const value_type& reference;
    typedef const value_type* pointer;
    typedef void difference_type;

    erdos_renyi_iterator() : gen(0), n(0), edges(0), allow_self_loops(false) {}
    erdos_renyi_iterator(RandomGenerator& gen, vertices_size_type n,
                         double prob = 0.0, bool allow_self_loops = false)
      : gen(&gen), n(n), edges(edges_size_type(prob * n * n)),
        allow_self_loops(allow_self_loops)
    {
      if (is_undirected) edges = edges / 2;
      next();
    }

    reference operator*() const { return current; }
    pointer operator->() const { return &current; }

    erdos_renyi_iterator& operator++()
    {
      --edges;
      next();
      return *this;
    }

    erdos_renyi_iterator operator++(int)
    {
      erdos_renyi_iterator temp(*this);
      ++(*this);
      return temp;
    }

    bool operator==(const erdos_renyi_iterator& other) const
    { return edges == other.edges; }

    bool operator!=(const erdos_renyi_iterator& other) const
    { return !(*this == other); }

  private:
    void next()
    {
      uniform_int<vertices_size_type> rand_vertex(0, n-1);
      current.first = rand_vertex(*gen);
      do {
        current.second = rand_vertex(*gen);
      } while (current.first == current.second && !allow_self_loops);
    }

    RandomGenerator* gen;
    vertices_size_type n;
    edges_size_type edges;
    bool allow_self_loops;
    value_type current;
  };

} // end namespace boost

#endif // BOOST_GRAPH_ERDOS_RENYI_GENERATOR_HPP