/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#if ! defined(CODON_MODEL_HPP)
#define CODON_MODEL_HPP

#include "states_patterns.hpp"
#include <boost/noncopyable.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/lambda/lambda.hpp>
#include <vector>
#include <numeric>
#include <algorithm>
#include "xlikelihood.hpp"
#include "mcmc_param.hpp"
#include "basic_cdf.hpp"
#include "q_matrix.hpp"
#include "model.hpp"

namespace phycas{

/*----------------------------------------------------------------------------------------------------------------------
|	Specialization of the base class Model that represents a basic codon model. The parameter kappa is the
|	transition/transversion rate ratio, and the parameter omega is the nonsynonymous/synonymous rate ratio. This model
|	also provides parameters for estimating the 61 non-stop-codon frequencies.
*/
class Codon: public Model
	{
	public:
									Codon();
									~Codon()
										{
										//std::cerr << "Codon dying..." << std::endl;
										}

		virtual std::string			getModelName() const;

		virtual void				setStateFreqUnnorm(unsigned param_index, double value);
        virtual void                setStateFreqsUnnorm(const std::vector<double> & values);
		virtual void				setAllFreqsEqual();
		virtual void				setNucleotideFreqs(double freqA, double freqC, double freqG, double freqT);

		void						fixKappa();
		void						freeKappa();

		void						fixOmega();
		void						freeOmega();

		double						getKappa();
		void						setKappa(double k);

		double						getOmega();
		void						setOmega(double w);

		void						setKappaPrior(ProbDistShPtr d);
		ProbDistShPtr				getKappaPrior();

		void						setOmegaPrior(ProbDistShPtr d);
		ProbDistShPtr				getOmegaPrior();

        void						setStateFreqPrior(MultivarProbDistShPtr d);
		MultivarProbDistShPtr		getStateFreqPrior();

		void						setStateFreqParamPrior(ProbDistShPtr d);
		ProbDistShPtr				getStateFreqParamPrior();

		virtual std::string			paramHeader() const;
		virtual std::string			paramReport(unsigned ndecimals, bool include_edgelen_hyperparams) const;

        virtual unsigned            getNumFreeParameters() const;
        virtual void                appendPWKParamNames(std::vector<std::string> & names, std::string prefix = "") const;
        virtual void                appendFreeParamNames(std::vector<std::string> & names, std::string prefix = "") const;
        virtual void                appendParamNames(std::vector<std::string> & names, std::string prefix = "") const;
        virtual void                appendUntransformedParamValues(std::vector<double> & values) const;
        virtual void                appendTransformedParamValues(std::vector<double> & values) const;
        virtual bool                setParamValueFromTransformed(std::string parameter_name, double transformed_value, TreeShPtr tree);

        virtual double              calcLogDetJacobian() const;

		void						updateQMatrix() const;
		virtual void				createParameters(JointPriorManagerShPtr jpm, TreeShPtr t, MCMCUpdaterVect & edgelens, MCMCUpdaterVect & edgelen_hyperparams, MCMCUpdaterVect & parameters, int subset_pos);
		double						calcUniformizationLambda() const;
        double					    calcLMat(double * * lMat) const;
        double					    calcUMat(double * * uMat) const;
		void						calcPMat(double * * pMat, double edgeLength) const;

protected:

	ProbDistShPtr				kappa_prior;		/**< The prior distribution governing kappa */
	ProbDistShPtr				omega_prior;		/**< The prior distribution governing omega */
	ProbDistShPtr				freq_param_prior;	/**< The prior distribution governing each frequency parameter */
	MultivarProbDistShPtr		freq_prior;	        /**< The prior distribution governing the vector of frequencies (used if frequencies are updated jointly by StateFreqMove) */

	string_vect_t				freq_label;			/**< Holds names of the 60 state frequencies (e.g. freqAAC, ...) used as headers in the param file */
	string_vect_t				freq_name;			/**< deprecated: Holds names of the 61 state frequencies (e.g. freqAAA, freqAAC, ...) used as headers in the param file */

	bool						kappa_fixed;		/**< If true, the value of kappa will not change during MCMC updates */
	bool						omega_fixed;		/**< If true, the value of omega will not change during MCMC updates */

	mutable MCMCUpdaterShPtr	omega_param;		/**< Copy of the omega parameter (saved so that fixed/free status can be changed) */
	mutable MCMCUpdaterShPtr	kappa_param;		/**< Copy of the kappa parameter (saved so that fixed/free status can be changed) */

	mutable QMatrix				q_matrix;			/**< A QMatrix object used to compute transition probabilities */

	// Below here are quantities that directly affect likelihood calculations and which should increment time_stamp when modified
	double						kappa;				/**< The transition/transversion rate ratio */
	double						omega;				/**< The nonsynonymous/synonymous rate ratio */
	};

typedef boost::shared_ptr<Codon> CodonShPtr;

} // namespace phycas

#endif


#include <SSVUtils/Core/Core.hpp>
#include <SSVUtils/Benchmark/Benchmark.hpp>
#include <SSVUtils/Container/Container.hpp>
#include <boost/container/flat_map.hpp>

volatile int vv{0};

struct BigObj
{
    volatile char abc[32];
    inline volatile void e() const { ++vv; }
};

namespace test
{
    template <typename T, typename TComparer>
    class SortedVector
    {
    private:
        std::vector<T> items;
        TComparer comparer;

    public:
        inline std::size_t insertAndGetIdx(T mValue)
        {
            auto lb(std::lower_bound(
                std::begin(items), std::end(items), mValue, comparer));
            auto idx(lb - std::begin(items));
            items.insert(lb, std::move(mValue));

            return idx;
        }

        inline decltype(items)& getItems() noexcept { return items; }
        inline const decltype(items)& getItems() const noexcept
        {
            return items;
        }
    };

    template <typename TKey, typename TValue,
        typename TComparer = std::less<TKey>>
    class DODMap
    {
    private:
        constexpr static std::size_t defaultCapacity{10u};
        TComparer comparer;
        TKey* keys;
        TValue* values;
        std::size_t size, capacity;

        inline void grow()
        {
            capacity *= 2;

            auto newKeys(new TKey[capacity]);
            auto newValues(new TValue[capacity]);

            std::copy(keys, keys + size, newKeys);
            std::copy(values, values + size, newValues);

            delete[] keys;
            delete[] values;

            keys = newKeys;
            values = newValues;
        }

    public:
        inline DODMap()
            : keys{new TKey[defaultCapacity]},
              values{new TValue[defaultCapacity]}, size{0u},
              capacity{defaultCapacity}
        {
        }
        inline ~DODMap()
        {
            delete[] keys;
            delete[] values;
        }

        inline void add(TKey mKey, TValue mValue)
        {
            ++size;
            if(size > capacity) grow();

            auto i(static_cast<int>(size - 1));
            for(; i > 0 && keys[i - 1] > mKey; --i)
            {
                keys[i] = keys[i - 1];
                values[i] = values[i - 1];
            }

            keys[i] = mKey;
            values[i] = mValue;
        }

        inline TValue& lookup(const TKey& mKey)
        {
            auto lb(std::lower_bound(keys, keys + size, mKey, comparer));
            auto idx(lb - keys);

            return values[idx];
        }
    };
}

int rndd() { return ssvu::getRnd<std::size_t>(0, 1000); }

int main()
{
    constexpr std::size_t kk = 1;
    constexpr std::size_t jj = 1000000;

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("sorted vec");
        std::vector<std::pair<std::size_t, BigObj>> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("insertion");
                for(auto j(0u); j < jj; ++j)
                    map.emplace_back(
                        std::make_pair<std::size_t, BigObj>(j, {}));
            }

            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("lookup");
                ssvu::sort(map, [](const auto& mA, const auto& mB)
                    {
                        return mA.first < mB.first;
                    });
                for(auto j(0u); j < jj; ++j)
                    ssvu::lowerBound(map, j,
                        [](const auto& mA, const auto& mB)
                        {
                            return mA.first < mB;
                        })
                        ->second.e();
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("std::map");
        std::map<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("insertion");
                for(auto j(0u); j < jj; ++j) map[j] = {};
            }

            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("lookup");
                for(auto j(0u); j < jj; ++j) map.at(j).e();
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("ssvu::VecMap");
        ssvu::VecMap<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("insertion");
                for(auto j(0u); j < jj; ++j) map[j] = {};
            }

            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("lookup");
                for(auto j(0u); j < jj; ++j) map.at(j).e();
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("test::DODMap");
        test::DODMap<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("insertion");
                for(auto j(0u); j < jj; ++j) map.add(j, {});
            }

            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("lookup");
                for(auto j(0u); j < jj; ++j) map.lookup(j).e();
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("boost::flat_map");
        boost::container::flat_map<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("insertion");
                for(auto j(0u); j < jj; ++j) map[j] = {};
            }

            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("lookup");
                for(auto j(0u); j < jj; ++j) map.at(j).e();
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("ssvu::VecMap");
        ssvu::VecMap<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("random insertion");
                for(auto j(0u); j < jj; ++j) map[rndd()] = {};
            }
        }
    }
    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("std::map");
        std::map<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("random insertion");
                for(auto j(0u); j < jj; ++j) map[rndd()] = {};
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("test::DODMap");
        test::DODMap<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("random insertion");
                // for(auto j(0u); j < jj; ++j) map.add(rndd(), {});
            }
        }
    }

    {
        SSVU_BENCHMARK_LOG_SCOPE_EXIT("boost::flat_map");
        boost::container::flat_map<std::size_t, BigObj> map;

        for(auto k(0u); k < kk; ++k)
        {
            {
                SSVU_BENCHMARK_LOG_SCOPE_EXIT("random insertion");
                for(auto j(0u); j < jj; ++j) map[rndd()] = {};
            }
        }
    }

    return 0;
}


#include <tiny.h>
#include <convex.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(convex_outside_triangle);

BOOST_AUTO_TEST_CASE(case_by_case_test)
{
  typedef tiny::MathTypes<double>       math_types;
  typedef math_types::vector3_type     V;

  V const a = V::make(0.0, 0.0, 0.0);
  V const b = V::make(1.0, 0.0, 0.0);;
  V const c = V::make(0.0, 1.0, 0.0);;
  V const q = V::make(0.33, 0.33, -1.0);

  {
    V p = V::make(0.33, 0.33,  1.0);
    bool outside = convex::outside_triangle(p, a, b, c, q );
    BOOST_CHECK( outside );
  }
  {
    V p = V::make(0.1, 0.1,  -1.0);
    bool outside = convex::outside_triangle(p, a, b, c, q );
    BOOST_CHECK( !outside );
  }
  {
    V p = V::make(0.1, 0.1,  0.0);
    bool outside = convex::outside_triangle(p, a, b, c, q );
    BOOST_CHECK( outside );
  }
}

BOOST_AUTO_TEST_SUITE_END();


#include <highgui.h>
#include <Eigen/Eigen>

namespace ndt_feature_reg
{

template <typename PointT>
void NDTFrameProc<PointT>::addFrameIncremental (NDTFrame<PointT> *f, bool skipMatching, bool ndtEstimateDI,
        bool match_full, bool match_no_association)
{

    if(!match_full)
    {
        detectKeypoints(f);
    }

    f->computeNDT(ndtEstimateDI, this->non_mean);

    if(!(match_no_association||match_full))
    {
        f->assignPts();
        calcDescriptors(f);
    }
    frames.push_back(f);

    NDTFrameProc<PointT>::EigenTransform tr;
    tr.setIdentity();
    if(frames.size() == 1)
    {

        transformVector.clear();
        transformVector.push_back(tr);
        return;
    }

    Eigen::Affine3d transl_transform;
    Eigen::Affine3d rot_transform;
    std::vector<std::pair<int,int> > corr;

    int i = frames.size()-1;

    if(!(match_full || match_no_association))
    {
        pe.estimate(*frames[i-1],*frames[i]);
        transl_transform = (Eigen::Affine3d)Eigen::Translation3d(pe.trans);
        rot_transform = (Eigen::Affine3d)Eigen::Matrix3d(pe.rot);
        tr = transl_transform*rot_transform;
        if (!skipMatching)
        {
            corr.clear();
            corr = convertMatches(pe.inliers);
            lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> matcher_feat(corr, trim_factor);
            matcher_feat.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr, true); //true = use initial guess
            //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
        }
    }
    else
    {
        lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
        matcher.current_resolution = frames[i-1]->current_res/2;
        matcher.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr);
        //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
    }

    transformVector.push_back(tr);

}

template <typename PointT>
void NDTFrameProc<PointT>::trimNbFrames (size_t maxNbFrames)
{
    if (frames.size() > maxNbFrames)
    {
        //frames.erase(frames.begin(), frames.begin() + frames.size() - maxNbFrames);
        for(size_t i =0; i<frames.size()-1; i++)
        {
            //std::cout<<"delete f : "<<i<<std::endl;
            delete frames[i];
        }
        frames.erase(frames.begin(), frames.end() - 1);
        transformVector.erase(transformVector.begin(), transformVector.end() - 1); // Just to keep a 1-1 coorespondance with frames.
    }
}

template <typename PointT>
void NDTFrameProc<PointT>::processFrames (bool skipMatching, bool ndtEstimateDI,
        bool match_full, bool match_no_association)
{
    if(frames.size() < 1) return;

    transformVector.clear();
    NDTFrameProc<PointT>::EigenTransform tr;
    tr.setIdentity();
    transformVector.push_back(tr);

    if(!match_full)
    {
        detectKeypoints(frames[0]);
    }
    frames[0]->computeNDT(ndtEstimateDI, this->non_mean);

    if(!(match_no_association||match_full))
    {
        frames[0]->assignPts();
        calcDescriptors(frames[0]);
    }

    Eigen::Affine3d transl_transform;
    Eigen::Affine3d rot_transform;
    std::vector<std::pair<int,int> > corr;

    for(size_t i=1; i<frames.size(); i++)
    {

        if(!match_full)
        {
            detectKeypoints(frames[i]);
        }
        frames[i]->computeNDT(ndtEstimateDI, this->non_mean);
        if(!(match_no_association||match_full))
        {
            frames[i]->assignPts();
            calcDescriptors(frames[i]);

            pe.estimate(*frames[i-1],*frames[i]);
            transl_transform = (Eigen::Affine3d)Eigen::Translation3d(pe.trans);
            rot_transform = (Eigen::Affine3d)Eigen::Matrix3d(pe.rot);
            tr = transl_transform*rot_transform;

            if (!skipMatching)
            {
                corr.clear();
                corr = convertMatches(pe.inliers);
                lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> matcher_feat(corr, trim_factor);
                matcher_feat.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr, true); //true = use initial guess
                //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
            }
        }
        else
        {
            lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
            matcher.current_resolution = frames[i-1]->current_res/2;
            matcher.match(frames[i-1]->ndt_map, frames[i]->ndt_map, tr);
            //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
        }

        /*
        	transformVector.push_back(tr);
        	cv::Mat display;
        	drawMatches(frames[i-1]->img, frames[i-1]->kpts, frames[i]->img, frames[i]->kpts, pe.inliers, display);
        	const std::string window_name = "matches";
        	cv::namedWindow(window_name,0);
        	cv::imshow(window_name, display);
        	cv::waitKey(0);
        */

    }

}

template <typename PointT>
void
NDTFrameProc<PointT>::detectKeypoints(NDTFrame<PointT> *f) const
{
    if (img_scale == 1.)
    {
        detector->detect(f->img, f->kpts);
    }
    else
    {
        cv::Mat tmp;
        cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
        detector->detect(tmp, f->kpts);
        float factor = 1./img_scale;
        scaleKeyPointPosition(f->kpts, factor);
    }
}

template <typename PointT>
void
NDTFrameProc<PointT>::calcDescriptors(NDTFrame<PointT> *f) const
{
    if (img_scale == 1.)
    {
        extractor->compute(f->img, f->kpts, f->dtors);
    }
    else
    {
        cv::Mat tmp;
        cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
        float factor = img_scale;
        scaleKeyPointPosition(f->kpts, factor);
        extractor->compute(tmp, f->kpts, f->dtors);
        factor = 1./img_scale;
        scaleKeyPointPosition(f->kpts, factor);
    }
}

template <typename PointT>
pcl::PointCloud<pcl::PointXYZRGB>::Ptr
NDTFrameProc<PointT>::createColoredFeaturePC(NDTFrame<PointT> &f, pcl::PointXYZRGB color)
{
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr ret(new pcl::PointCloud<pcl::PointXYZRGB>());
    *ret = f.getColoredPointCloud();
    size_t w = f.img.size().width;
    //size_t h = f.img.size().height;
    size_t idx = 0;

    for (size_t i = 0; i < f.kpts.size(); i++)
    {
        int uKey = static_cast<int>(f.kpts[i].pt.x+0.5);
        int vKey = static_cast<int>(f.kpts[i].pt.y+0.5);

        idx = vKey * w + uKey;
        pcl::PointXYZRGB& pt = ret->points[idx];
        pt.rgb = color.rgb;
    }
    return ret;
}

/*
template <typename PointT>
bool
NDTFrameProc<PointT>::loadImg(NDTFrame<PointT> &f, const std::string &fileName) const
{
     f.img = cv::imread(fileName, 0);
     if (f.img.empty())
     {
	 std::cerr << "Failed to load : " << fileName << std::endl;
	  return false;
     }
     return true;
}

template <typename PointT>
void
NDTFrameProc<PointT>::loadPC(NDTFrame<PointT> &f, const std::string &fileName) const
{

}

template <typename PointT>
bool
NDTFrameProc<PointT>::loadPCfromDepth(NDTFrame<PointT> &f, const std::string &fileName) const
{
     // Load the file.
     f.depth_img = cv::imread(fileName, CV_LOAD_IMAGE_ANYDEPTH); // CV_LOAD_IMAGE_ANYDEPTH is important to load the 16bits image
     if (f.depth_img.empty())
     {
	  std::cerr << "Failed to load : " << fileName << std::endl;
     }
     if (!this->_lookupTable.empty())
     {
	  //convertDepthToPC(f.depth_img, f.pc);
	  return true;
     }
     return false;
}

template <typename PointT>
bool
NDTFrameProc<PointT>::setupDepthToPC(const std::string &fileName, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds)
{
     cv::Mat depth_img = cv::imread(fileName, CV_LOAD_IMAGE_ANYDEPTH);
     if (depth_img.empty())
     {
	  std::cerr << "Failed to load : " << fileName << std::endl;
	  return false;
     }
     setupDepthToPC(depth_img, fx, fy, cx, cy, dist, ds);
     return true;
}

template <typename PointT>
void
NDTFrameProc<PointT>::setupDepthToPC(const cv::Mat &depthImg, double fx, double fy, double cx, double cy, const std::vector<double> &dist, double ds)
{
     _camMat = getCameraMatrix(fx, fy, cx, cy);
     _dist = getDistVector(dist[0], dist[1], dist[2], dist[3], dist[4]);

     _lookupTable = getDepthPointCloudLookUpTable(depthImg.size(), _camMat, _dist, ds);
}

template <typename PointT>
void
NDTFrameProc<PointT>::convertDepthToPC(const cv::Mat &depthImg, pcl::PointCloud<PointT> &pc) const
{
     convertDepthImageToPointCloud(depthImg, _lookupTable, pc);
}
*/
}


/*
 * @Description: ICP 匹配模块
 * @Author: Ge Yao
 * @Date: 2020-10-24 21:46:45
 */
#include "lidar_localization/models/registration/icp_svd_registration.hpp"

#include <pcl/common/transforms.h>

#include <cmath>
#include <vector>

#include <Eigen/Dense>
#include <Eigen/SVD>

#include "glog/logging.h"

namespace lidar_localization {

ICPSVDRegistration::ICPSVDRegistration(
    const YAML::Node& node
) : input_target_kdtree_(new pcl::KdTreeFLANN<pcl::PointXYZ>()) {
    // parse params:
    float max_corr_dist = node["max_corr_dist"].as<float>();
    float trans_eps = node["trans_eps"].as<float>();
    float euc_fitness_eps = node["euc_fitness_eps"].as<float>();
    int max_iter = node["max_iter"].as<int>();

    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
}

ICPSVDRegistration::ICPSVDRegistration(
    float max_corr_dist,
    float trans_eps,
    float euc_fitness_eps,
    int max_iter
) : input_target_kdtree_(new pcl::KdTreeFLANN<pcl::PointXYZ>()) {
    SetRegistrationParam(max_corr_dist, trans_eps, euc_fitness_eps, max_iter);
}

bool ICPSVDRegistration::SetRegistrationParam(
    float max_corr_dist,
    float trans_eps,
    float euc_fitness_eps,
    int max_iter
) {
    // set params:
    max_corr_dist_ = max_corr_dist;
    trans_eps_ = trans_eps;
    euc_fitness_eps_ = euc_fitness_eps;
    max_iter_ = max_iter;

    LOG(INFO) << "ICP SVD params:" << std::endl
              << "max_corr_dist: " << max_corr_dist_ << ", "
              << "trans_eps: " << trans_eps_ << ", "
              << "euc_fitness_eps: " << euc_fitness_eps_ << ", "
              << "max_iter: " << max_iter_
              << std::endl << std::endl;

    return true;
}

bool ICPSVDRegistration::SetInputTarget(const CloudData::CLOUD_PTR& input_target) {
    input_target_ = input_target;
    input_target_kdtree_->setInputCloud(input_target_);

    return true;
}

bool ICPSVDRegistration::ScanMatch(
    const CloudData::CLOUD_PTR& input_source,
    const Eigen::Matrix4f& predict_pose,
    CloudData::CLOUD_PTR& result_cloud_ptr,
    Eigen::Matrix4f& result_pose
) {
    input_source_ = input_source;

    // pre-process input source:
    CloudData::CLOUD_PTR transformed_input_source(new CloudData::CLOUD());
    pcl::transformPointCloud(*input_source_, *transformed_input_source, predict_pose);

    // init estimation:
    transformation_.setIdentity();

    // do estimation:
    int curr_iter = 0;
    while (curr_iter < max_iter_) {
        // apply current estimation:
        CloudData::CLOUD_PTR curr_input_source(new CloudData::CLOUD());
        pcl::transformPointCloud(*transformed_input_source, *curr_input_source, transformation_);

        // get correspondence:
        std::vector<Eigen::Vector3f> xs;
        std::vector<Eigen::Vector3f> ys;

        // do not have enough correspondence -- break:
        if (GetCorrespondence(curr_input_source, xs, ys) < 3)
            break;

        // update current transform:
        Eigen::Matrix4f delta_transformation;
        GetTransform(xs, ys, delta_transformation);

        // whether the transformation update is significant:
        if (!IsSignificant(delta_transformation, trans_eps_))
            break;

        transformation_ = delta_transformation * transformation_;

        ++curr_iter;
    }

    // set output:
    result_pose = transformation_ * predict_pose;
    pcl::transformPointCloud(*input_source_, *result_cloud_ptr, result_pose);

    return true;
}

size_t ICPSVDRegistration::GetCorrespondence(
    const CloudData::CLOUD_PTR &input_source,
    std::vector<Eigen::Vector3f> &xs,
    std::vector<Eigen::Vector3f> &ys
) {
    const float MAX_CORR_DIST_SQR = max_corr_dist_ * max_corr_dist_;

    size_t num_corr = 0;

    for (size_t i = 0; i < input_source->points.size(); ++i) {
        std::vector<int> corr_ind;
        std::vector<float> corr_sq_dis;
        input_target_kdtree_->nearestKSearch(
            input_source->at(i),
            1,
            corr_ind, corr_sq_dis
        );

        if (corr_sq_dis.at(0) > MAX_CORR_DIST_SQR)
            continue;

        // add correspondence:
        Eigen::Vector3f x(
            input_target_->at(corr_ind.at(0)).x,
            input_target_->at(corr_ind.at(0)).y,
            input_target_->at(corr_ind.at(0)).z
        );
        Eigen::Vector3f y(
            input_source->at(i).x,
            input_source->at(i).y,
            input_source->at(i).z
        );

        xs.push_back(x);
        ys.push_back(y);

        ++num_corr;
    }

    return num_corr;
}

void ICPSVDRegistration::GetTransform(
    const std::vector<Eigen::Vector3f> &xs,
    const std::vector<Eigen::Vector3f> &ys,
    Eigen::Matrix4f &transformation_
) {
    const size_t N = xs.size();

    // TODO -- find centroids of mu_x and mu_y:

    // TODO -- build H:

    // TODO -- solve R:

    // TODO -- solve t:

    // set output:
    transformation_.setIdentity();
}

bool ICPSVDRegistration::IsSignificant(
    const Eigen::Matrix4f &transformation,
    const float trans_eps
) {
    // a. translation magnitude -- norm:
    float translation_magnitude = transformation.block<3, 1>(0, 3).norm();
    // b. rotation magnitude -- angle:
    float rotation_magnitude = fabs(
        acos(
            (transformation.block<3, 3>(0, 0).trace() - 1.0f) / 2.0f
        )
    );

    return (
        (translation_magnitude > trans_eps) ||
        (rotation_magnitude > trans_eps)
    );
}

}


#include <maps/SpatialQueryWrapper.hpp>
#include <maps/SpatialQuery.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/thread.hpp>
#include <boost/progress.hpp>
#include <lcm/lcm-cpp.hpp>

using namespace std;

struct Worker {
  Worker(SpatialQueryWrapper* iWrapper) {
    mWrapper = iWrapper;
  }

  void operator()() {
    while (true) {
      sleep(1);
      {
        boost::progress_timer timer;
        mWrapper->lock();
        int total = 0;
        for (int i = 0; i < 100000; ++i) {
          Eigen::Vector3d pt(1.74,-1.17,1.53);
          Eigen::Vector3d outPoint, outNormal;
          if (mWrapper->query()->getClosest(pt, outPoint, outNormal)) {
            ++total;
          }
        }
        mWrapper->unlock();
        cout << "succeeded for " << total << " queries" << endl;
      }
    }
  }

  SpatialQueryWrapper* mWrapper;
};

int main(const int iArgc, const char** iArgv) {
  boost::shared_ptr<lcm::LCM> theLcm(new lcm::LCM());
  if (!theLcm->good()) {
    cerr << "Cannot create lcm object" << endl;
    return -1;
  }

  SpatialQueryWrapper wrapper;
  wrapper.setLcm(theLcm);
  wrapper.setMapChannel("LOCAL_MAP");
  wrapper.start();

  Worker worker(&wrapper);
  boost::thread thread(boost::ref(worker));

  while (0 == theLcm->handle());

  wrapper.stop();

  return 0;
}


/** @file */

#include "IMUFrame.h"

#include <fstream>

#include <Eigen/Geometry>
#include <boost/regex.hpp>

#include <YukariCommon/LoggingService.h>
#include <YukariCommon/StringParsers.h>

using namespace Yukari::Common;
using namespace Yukari::Maths;

namespace Yukari
{
namespace IMU
{
  IMUFrame::IMUFrame(const Eigen::Matrix4f &mat, Duration frameDuration)
      : Transform(mat)
      , m_durationMs(frameDuration)
  {
  }

  IMUFrame::IMUFrame(Duration frameDuration, const Eigen::Quaternionf &orientation,
                     const Eigen::Vector3f &position)
      : Transform(orientation, position)
      , m_durationMs(frameDuration)
  {
  }

  IMUFrame::IMUFrame(const std::string &str)
      : Transform()
  {
    auto logger = LoggingService::Instance().getLogger("IMUFrame");

    boost::regex expression("\\(\\s*dt\\=([\\w\\.\\-]+)\\,\\s*o\\s*\\=\\s*(\\[[\\w\\.\\-\\,\\s]+\\]"
                            ")\\,\\s*p\\s*\\=\\s*(\\[[\\w\\.\\-\\,\\s]+\\])\\)");

    boost::cmatch what;
    if (boost::regex_match(str.c_str(), what, expression))
    {
      m_durationMs = std::chrono::milliseconds(std::stol(what[1]));
      m_orientation = StringParsers::ParseQuaternion(what[2]);
      m_position = StringParsers::ParseVector(what[3]);
    }
    else
    {
      logger->error("Could not parse regex");
    }
  }

  void IMUFrame::save(const boost::filesystem::path &path) const
  {
    auto logger = LoggingService::Instance().getLogger("IMUFrame");
    logger->trace("Saving IMU frame to file \"{}\"", path);
    std::ofstream imuFile;
    imuFile.open(path.string());
    imuFile << *this << '\n';
    imuFile.close();
  }

  Eigen::Matrix4f IMUFrame::toCloudTransform() const
  {
    Eigen::Matrix4f out = Eigen::Matrix4f::Identity();
    out.block(0, 0, 3, 3) = m_orientation.toRotationMatrix();
    out.block(0, 3, 3, 1) = m_position;
    return out;
  }

  std::ostream &operator<<(std::ostream &s, const IMUFrame &f)
  {
    s << "(dt=" << f.m_durationMs.count()
      << ", o=" << f.m_orientation.coeffs().format(Transform::EIGEN_FORMAT)
      << ", p=" << f.m_position.format(Transform::EIGEN_FORMAT) << ")";

    return s;
  }

  std::istream &operator>>(std::istream &s, IMUFrame &f)
  {
    std::string str;
    std::getline(s, str, ')');
    str += ')';
    f = IMUFrame(str);
    return s;
  }
}
}


//  Copyright John Maddock 2007-8.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

//
// This tests two things: that mpfr::mpreal meets our
// conceptual requirements, and that we can instantiate
// all our distributions and special functions on this type.
//
#define BOOST_MATH_ASSERT_UNDEFINED_POLICY false
#define TEST_MPFR

#include <boost/math/bindings/mpreal.hpp>
#include <boost/math/concepts/real_type_concept.hpp>
#include "compile_test/instantiate.hpp"
#include <boost/static_assert.hpp>

//BOOST_STATIC_ASSERT((boost::is_same<mpfr::mpreal, boost::math::tools::promote_args<mpfr::mpreal>::type >::value));

void foo()
{
   instantiate(mpfr::mpreal());
}

int main()
{
   BOOST_CONCEPT_ASSERT((boost::math::concepts::RealTypeConcept<mpfr::mpreal>));
   return 0;
}


#include <mvp/Image/OrbitalImageCollection.h>

#include <boost/foreach.hpp>

namespace mvp {
namespace image {

std::vector<vw::ImageView<OrbitalImagePixel> > OrbitalImageCollection::back_project(vw::Vector3 const& xyz,
                                                                              vw::Quat const& orientation,
                                                                              vw::Vector2 const& scale,
                                                                              vw::Vector2i const& size) const
{
  std::vector<vw::ImageView<OrbitalImagePixel> > result;

  BOOST_FOREACH(OrbitalImage const& o, *this) {
    vw::ImageView<OrbitalImagePixel> patch = o.back_project(xyz, orientation, scale, size);
    if (!vw::is_transparent(patch)) {
      result.push_back(patch);
    }
  }

  return result;
}

}} // namespace image,mvp


#include <iostream>
#include <cmath>
#include <boost/proto/proto.hpp>

using namespace boost;

namespace transforms {

    using namespace proto;

    template< typename X >
    struct max_fn : callable {
        using result_type = X;
        X operator()( X const & x , X const & y ) const { return std::max(x,y); }
    };

    template< typename X >
    struct zero_fn : callable {
        using result_type = X;
        X operator()() const { return {}; }
    };

    struct max_fn_int : max_fn<int> {};

    struct max_int : or_
        <   when
            <   terminal< int >
            ,   _value
            >
        ,   when
            <   terminal< _ >
            ,   call< zero_fn<int>() >
            >
        ,   when
            <   subscript< terminal<int> , terminal<int> >
            ,   max_int(_right)
            //,   call< zero_fn<int>() >
            >
        ,   when
            <   binary_expr< _, max_int, max_int >
            ,   call< max_fn<int>( max_int(_left), max_int(_right) ) >
            >
        >
    {};

}

using transforms::max_int;

int main()
{
    auto expr  =  proto::lit(2)[7] + 3 + 4;

    std::cout << max_int{}(expr) << std::endl;
}


/*

Copyright (c) 2005-2018, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright notice,
   this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.
 * Neither the name of the University of Oxford nor the names of its
   contributors may be used to endorse or promote products derived from this
   software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

*/

#ifndef RANDOMNUMBERGENERATORS_HPP_
#define RANDOMNUMBERGENERATORS_HPP_

#include <boost/shared_ptr.hpp>
#include <boost/version.hpp>
#include <sstream>

#if BOOST_VERSION < 106400
// Forward compatibility with Boost 1.64 onwards
#include "Boost165NormalDistribution.hpp"
#endif

#include <boost/random.hpp>

#include <boost/serialization/split_member.hpp>
#include "ChasteSerialization.hpp"
#include "SerializableSingleton.hpp"

/**
 * A special singleton class allowing one to generate different types of
 * random number in a consistent way across platforms and different versions of compilers,
 * standard libraries, and our dependencies. As a result Chaste developers should always
 * use this class to get random numbers rather than accessing C/C++/boost methods directly.
 *
 * This class is a singleton and an instance should be retrieved with:
 * RandomNumberGenerator* p_gen = RandomNumberGenerator::Instance();
 *
 * Note for developers that might want to change this class!:
 *
 * We use boost random number library for random number generation under the hood
 * http://www.boost.org/doc/libs/1_65_0/doc/html/boost_random.html
 *
 * Generally boost is consistent between our supported versions
 * (listed on https://chaste.cs.ox.ac.uk/trac/wiki/InstallGuides/DependencyVersions)
 * but on a couple of occasions boost has broken backwards compatibility between random numbers
 * for a given seed.
 *
 * To get around this, we have copied a minimal set of boost's more recent random include headers
 * into the folder global/src/random
 *
 * If the user is on a recent version of boost, the boost libraries/includes are used by default,
 * if the user is on an older version of boost some of the Chaste copies of newer boost distributions
 * in global/src/random are used instead to give consistent random numbers across boost versions.
 *
 */
class RandomNumberGenerator : public SerializableSingleton<RandomNumberGenerator>
{
private:
    /** The main random number generator. **/
    boost::mt19937 mMersenneTwisterGenerator;

    // If you add any more generators below, then remember to add lines for them in the Reseed() method too.

    /** An adaptor to a unit interval distribution. */
    boost::variate_generator<boost::mt19937&, boost::uniform_real<> > mGenerateUnitReal;

/** An adaptor to a standard normal distribution. */
#if BOOST_VERSION < 106400 // #2585 and #2893
    boost::variate_generator<boost::mt19937&, boost::random::normal_distribution_v165<> > mGenerateStandardNormal;
#else
    boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > mGenerateStandardNormal;
#endif
    /** Pointer to the single instance. */
    static RandomNumberGenerator* mpInstance;

    friend class boost::serialization::access;
    /**
     * Save the RandomNumberGenerator and its member variables.
     *
     * @note do not serialize this singleton directly.  Instead, serialize
     * the object returned by GetSerializationWrapper().
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template <class Archive>
    void save(Archive& archive, const unsigned int version) const
    {
        std::stringstream rng_internals;
        rng_internals << mMersenneTwisterGenerator;
        std::string rng_internals_string = rng_internals.str();
        archive& rng_internals_string;

        std::stringstream normal_internals;
#if BOOST_VERSION < 106400 // #2585 and #2893
        const boost::random::normal_distribution_v165<>& r_normal_dist = mGenerateStandardNormal.distribution();
#else
        const boost::normal_distribution<>& r_normal_dist = mGenerateStandardNormal.distribution();
#endif
        normal_internals << r_normal_dist;
        std::string normal_internals_string = normal_internals.str();
        archive& normal_internals_string;
    }

    /**
     * Load the RandomNumberGenerator and its member variables.
     *
     * @note do not serialize this singleton directly.  Instead, serialize
     * the object returned by GetSerializationWrapper().
     *
     * @param archive the archive
     * @param version the current version of this class
     */
    template <class Archive>
    void load(Archive& archive, const unsigned int version)
    {
        std::string rng_internals_string;
        archive& rng_internals_string;
        std::stringstream rng_internals(rng_internals_string);
        rng_internals >> mMersenneTwisterGenerator;

        std::string normal_internals_string;
        archive& normal_internals_string;
        std::stringstream normal_internals(normal_internals_string);
        normal_internals >> mGenerateStandardNormal.distribution();
    }
    BOOST_SERIALIZATION_SPLIT_MEMBER()

protected:
    /**
     * Protected constructor.
     * Use Instance() to access the random number generator.
     */
    RandomNumberGenerator();

public:
    /**
     *
     * @return a random number from the normal distribution with mean 0
     * and standard distribution 1.
     */
    double StandardNormalRandomDeviate();

    /**
     * @return Generate a random number from a normal distribution with given
     * mean and standard deviation.
     *
     * @param mean the mean of the normal distribution from which the random number is drawn
     * @param stdDev the standard deviation of the normal distribution from which the random number is drawn
     */
    double NormalRandomDeviate(double mean, double stdDev);

    /**
     * @return Generate a uniform random number in (0,1].
     */
    double ranf();

    /**
     * @return Generate a random number from a gamma distribution with specified shape and scale parameters.
     *
     * @param shape the shape parameter of the gamma distribution from which the random number is drawn
     * @param scale the scale parameter of the gamma distribution from which the random number is drawn
     */
    double GammaRandomDeviate(double shape, double scale);

    /**
     * @return Generate a random number from an exponential distribution with specified scale parameter.
     *
     * @param scale The scale parameter of the exponential distribution from which the random number is drawn, often named lambda
     */
    double ExponentialRandomDeviate(double scale);

    /**
     * @return Generate a random number modulo base (i.e. an integer
     * within the range [0, base) == [0,1,..,base-1] ).
     *
     * @param base the order of the field of positive integers from which the random number is drawn.  This should be no greater than INT_MAX
     */
    unsigned randMod(unsigned base);

    /**
     * Produce a permutation of the integers and non-empty std::vector, using the Knuth-algorithm
     * (also called the Fisher-Yates algorithm), a linear time unbiased method.
     * The values are shuffled in place.
     *
     * @param rValues  the initial values and the output permutation of shuffled values.  Must be non-empty
     */
    template <class T>
    void Shuffle(std::vector<boost::shared_ptr<T> >& rValues)
    {
        unsigned num = rValues.size();
        if (num == 0)
        {
            return;
        }
        for (unsigned end = num - 1; end > 0; end--)
        {
            // Pick a random integer from {0,..,end}
            unsigned k = RandomNumberGenerator::Instance()->randMod(end + 1);
            boost::shared_ptr<T> temp = rValues[end];
            rValues[end] = rValues[k];
            rValues[k] = temp;
        }
    }

    /**
     * Produce a permutation of the integers 0,1,..,num-1, using the Knuth-algorithm
     * (also called the Fisher-Yates algorithm), a linear time unbiased method.
     * The shuffled values are returned in rValues, which doesn't need to
     * be correctly-sized when passed in.
     *
     * @param num  the number of integers to shuffle
     * @param rValues  the output permutation of shuffled values (any initial values ignored)
     */
    void Shuffle(unsigned num, std::vector<unsigned>& rValues);

    /**
     * @return a pointer to the random number generator object.
     * The object is created the first time this method is called.
     */
    static RandomNumberGenerator* Instance();

    /**
     * Destroy the current instance of the random number generator.
     * The next call to Instance will create a new instance and re-seed.
     * This method *must* be called before program exit, to avoid a memory
     * leak.
     */
    static void Destroy();

    /**
     * Reseed the random number generator.
     *
     * @param seed the new seed
     */
    void Reseed(unsigned seed);
};

#endif /*RANDOMNUMBERGENERATORS_HPP_*/


#include <algorithm>
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <stdexcept>
#include <vector>

#include "CarsPopulationData.h"
#include "Evolution/Evolution.h"
#include "Evolution/MathUtils.h"

BOOST_AUTO_TEST_CASE(evolution_test) {
  cer::CarsPopulationData pd;
  BOOST_CHECK(pd.cars().empty());

  cer::evolution::Evolution ev(&pd, 12345);
  ev.generatePopulation();
  BOOST_CHECK(!pd.cars().empty());

  auto population = pd.cars();

  ev.setPopulationFitness(std::vector<double>(population.carsNum(), 1.0));

  ev.generatePopulation();
  BOOST_CHECK_EQUAL(pd.cars().size(), population.size());

  for (size_t i = 0; i < population.carsNum(); ++i) {
    auto p1_b = population.begin(i);
    auto p1_e = population.end(i);

    auto p2_b = pd.cars().begin(i);
    auto p2_e = pd.cars().end(i);

    for (; p1_b != p1_e && p2_b != p2_e; ++p1_b, ++p2_b) {
      BOOST_CHECK((*p1_b) != (*p2_b));
    }
  }
}

BOOST_AUTO_TEST_CASE(evolution_test_random) {
  cer::CarsPopulationData pd;
  BOOST_CHECK(pd.cars().empty());

  cer::evolution::Evolution ev(&pd);
  ev.generatePopulation();
  BOOST_CHECK(!pd.cars().empty());

  auto population = pd.cars();

  ev.generatePopulation();
  BOOST_CHECK_EQUAL(pd.cars().size(), population.size());

  ev.generatePopulation();
  BOOST_CHECK_EQUAL(pd.cars().size(), population.size());
}

BOOST_AUTO_TEST_CASE(evolution_parameters_test) {
  cer::CarsPopulationData pd;
  cer::evolution::Evolution ev(&pd);

  auto params = ev.parameters();
  BOOST_CHECK(!params.empty());

  for (const auto &p : params) {
    ev.setParameterValue(p.first, p.second.value + 1);
  }

  auto params_new = ev.parameters();

  for (const auto &p : params) {
    BOOST_CHECK(params_new.find(p.first) != params_new.end());
    BOOST_CHECK_EQUAL(p.second.value + 1, params_new[p.first].value);
  }
}

BOOST_AUTO_TEST_CASE(evolution_math_test) {
  namespace math = cer::evolution::math;

  std::vector<double> parent1(40, 1);
  std::vector<double> parent2(40, 2);
  std::vector<double> child;

  BOOST_CHECK_THROW(math::crossover(parent1.begin(), parent1.end(),
                                    parent2.begin(), parent2.end() - 1),
                    std::invalid_argument);

  BOOST_CHECK_NO_THROW(child = math::crossover(parent1.begin(), parent1.end(),
                                               parent2.begin(), parent2.end()));

  BOOST_CHECK_EQUAL(child.size(), parent1.size());
  for (auto p : child) {
    BOOST_CHECK((p >= 1. && p <= 2.));
  }

  std::vector<double> fitness(10);
  std::iota(fitness.begin(), fitness.end(), 1);

  for (size_t i = 2; i < 7; ++i) {
    auto selected = math::tournamentSelection(fitness, i, i / 2 + 3);
    BOOST_CHECK_EQUAL(selected.size(), i);
    std::cout << "inidices selected in tournament, there should be mostly "
                 "numbers close to 10: "
              << std::endl;
    for (auto s : selected) {
      std::cout << s << " ";
      BOOST_CHECK(s < 10);
    }
    std::cout << std::endl;
  }

  size_t n = 10;
  std::vector<double> parameters(n, 1.0);
  math::RandomGenerator rg(6);
  math::mutate(parameters.begin(), parameters.end(), rg);
  auto l = math::lowerLimits(n);
  auto u = math::upperLimits(n);
  for (size_t i = 0; i < n; ++i) {
    BOOST_CHECK(u[i] >= parameters[i] && l[i] <= parameters[i]);
  }

  math::RandomGenerator rg2(0.1, -1);
  math::mutate(parameters.begin(), parameters.end(), rg2);
  for (size_t i = 0; i < n; ++i) {
    BOOST_CHECK(u[i] >= parameters[i] && l[i] <= parameters[i]);
  }
}


#ifndef SKYLARK_SPARSE_DIST_MATRIX_HPP
#define SKYLARK_SPARSE_DIST_MATRIX_HPP

#include <map>
#include <memory>
#include <vector>

#include <boost/mpi.hpp>
#include <boost/mpi/communicator.hpp>

#include <El.hpp>

#include "sparse_matrix.hpp"

namespace skylark { namespace base {

namespace detail {

    struct compare_t {
        bool operator()(const std::pair<int, int>& x, const std::pair<int, int>& y) const {
            if(x.second == y.second)
                return x.first < y.first;

            return x.second < y.second;
        }
    };
}

/**
 *  This implements a very crude CSC sparse matrix container only intended to
 *  hold local sparse matrices.
 *
 *  Row indices are not sorted.
 *  Structure is always constants, and can only be attached by Attached.
 *  Values of non-zeros can be modified.
 *
 *  Note that 2D distributions do not make sense (inbalance):
 *
 *    https://github.com/elemental/Elemental/issues/24
 *
 *  TODO:
 *      - handle transpose (?)
 */
template<typename ValueType=double>
struct sparse_dist_matrix_t {

    typedef int index_type;
    typedef ValueType value_type;

    sparse_dist_matrix_t(
            El::Int height, El::Int width, const El::Grid& grid)
        : _local_buffer(new sparse_matrix_t<value_type>())
        , _comm(boost::mpi::communicator(grid.Comm().comm, boost::mpi::comm_attach))
        , _finalized(false)
        , _rank(_comm.rank())
        , _num_procs(_comm.size())
        , _n_rows(height)
        , _n_cols(width)
        , _n_local_rows(0)
        , _n_local_cols(0)
        , _grid(grid)
    {}

    ~sparse_dist_matrix_t()
    {}

    /**
     * @return communicator
     */
    boost::mpi::communicator comm() const {
        return _comm;
    }

    /**
     * Resizes the matrix to (width, height)
     *
     * Currently matrix needs to be non-finalized. Later ignore values that
     * are outside after resize. (FIXME)
     */
    void resize(El::Int height, El::Int width) {

        assert(_finalized == false);
        _n_rows = height;
        _n_cols = width;
    }

    /**
     * Scales the matrix values by a constant.
     * Note, that this only works for finalized matrices.
     */
    void scale(value_type factor) {
        assert(_finalized == true);

        for(size_t i = 0; i < _values.size(); i++)
            _values[i] *= factor;
    }

    /**
     * Queue a global value to be inserted into the matrix when finalized.
     * If the global value is not owned by the calling rank, nothing will be
     * queued.
     *
     * Note that this method is currently NOT thread safe! (FIXME)
     */
    void queue_update(El::Int i, El::Int j, value_type value) {

        assert(_finalized == false);
        assert(i < height());
        assert(j < width());

        if(is_local(i, j)) {
            queue_update_local(local_row(i), local_col(j), value);
        }
    }

    /**
     * Queue a local value to be inserted into the matrix when finalized.
     *
     * Note that this method is currently NOT thread safe! (FIXME)
     */
    void queue_update_local(El::Int i, El::Int j, value_type value) {

        assert(_finalized == false);
        assert(i < height());
        assert(j < width());

        _n_local_rows = std::max(_n_local_rows, i + 1);
        _n_local_cols = std::max(_n_local_cols, j + 1);

        //XXX: we should use a nicer structure for the temporary storage..
        _temp_buffer[std::make_pair(i, j)] += value;
    }

    /**
     * Finalizes the matrix, no subsequent updates to values possible.
     *
     * FIXME: use a more adequate internal data structure
     */
    void finalize() {

        assert(_finalized == false);
        _finalized = true;

        _nnz = _temp_buffer.size();

        _indptr.resize(_n_local_cols + 1);
        _indices.resize(_nnz);
        _values.resize(_nnz);

        size_t nnz = 0;
        int indptr_idx = 0;
        _indptr[indptr_idx] = 0;

        typename std::map< std::pair<int, int>, value_type >::const_iterator itr;
        for(itr = _temp_buffer.begin(); itr != _temp_buffer.end();
                itr++, nnz++) {

            int cur_row = itr->first.first;
            int cur_col = itr->first.second;
            value_type cur_val = itr->second;

            // fill empty cols
            for(; indptr_idx < cur_col; ++indptr_idx)
                _indptr[indptr_idx + 1] = nnz;

            _indices[nnz] = cur_row;
            _values[nnz]  = cur_val;
        }

        for(; indptr_idx < _n_local_cols; ++indptr_idx)
            _indptr[indptr_idx + 1] = nnz;

        assert(nnz == _nnz);
        _temp_buffer.clear();

        _local_buffer->attach(&_indptr[0], &_indices[0], &_values[0],
                _nnz, _n_local_rows, _n_local_cols, false, false, false);

        _global_nnz = 0;
        boost::mpi::all_reduce(_comm, _nnz, _global_nnz, std::plus<int>());
    }

    /**
     * @return local sparse_matrix
     */
    sparse_matrix_t<ValueType> &matrix() {
        return *_local_buffer;
    }

    /**
     * @return locked view to local sparse_matrix
     */
    const sparse_matrix_t<ValueType> &locked_matrix() const {
        return *_local_buffer;
    }

    /**
     * @return pointer to column indices array
     */
    const index_type* indptr() const {
        if(!_finalized) return NULL;
        return _local_buffer->indptr();
    }

    /**
     * @return column indices array
     */
    const index_type* indices() const {
        if(!_finalized) return NULL;
        return _local_buffer->indices();
    }

    /**
     * @return value array
     */
    value_type* values() {
        if(!_finalized) return NULL;
        return _local_buffer->values();
    }

    /**
     * @return locked value array
     */
    const value_type* locked_values() const {
        if(!_finalized) return NULL;
        return _local_buffer->values();
    }

    /**
     * Create a view.
     */
    void view(sparse_dist_matrix_t<value_type> &B) const {
        // TODO exceptions?!
        assert(typeid(B) == typeid(*this));
        assert(_finalized);

        // TODO set-up grid!
        B.resize(_n_rows, _n_cols);
        _local_buffer->view(*B._local_buffer);
        B._finalized = true;
    }

    /**
     * @return the total height of the matrix
     */
    El::Int height()   const { return _n_rows; }
    /**
     * @return the total widht of the matrix
     */
    El::Int width()    const { return _n_cols; }
    /**
     * @return the total number of nonzeros of the matrix
     */
    El::Int nonzeros() const { return _global_nnz; }

    /**
     * @return the global row index of a local row index iLoc
     */
    El::Int global_row(El::Int iLoc) const {
        return _col_shift + iLoc * _col_stride;
    }

    /**
     * @return the global column index of a local column index jLoc
     */
    El::Int global_col(El::Int jLoc) const {
        return _row_shift + jLoc * _row_stride;
    }

    /**
     * @return the height of the local portion of the matrix
     */
    El::Int local_height()   const { return _local_buffer->height(); }
    /**
     * @return the width of the local portion of the matrix
     */
    El::Int local_width()    const { return _local_buffer->width(); }
    /**
     * @return the number of nonzeros of the local portion of the matrix
     */
    El::Int local_nonzeros() const { return _local_buffer->nonzeros(); }

    El::Int local_row_offset(El::Int i) const {
        return El::Length_(i, _col_shift, _col_stride);
    }

    El::Int local_col_offset(El::Int j) const {
        return El::Length_(j, _row_shift, _row_stride);
    }

    /**
     * @return the local row index for global column index i
     */
    El::Int local_row(El::Int i) const {
        return local_row_offset(i);
    }

    /**
     * @return the local column index for global column index j
     */
    El::Int local_col(El::Int j) const {
        return local_col_offset(j);
    }

    /**
     * @return true if the global row index i is owned by the calling rank.
     */
    bool is_local_row(El::Int i) const { return row_owner(i) == _col_rank; }

    /**
     * @return true if the global column index j is owned by the calling rank.
     */
    bool is_local_col(El::Int j) const { return col_owner(j) == _row_rank; }

    /**
     * @return true if the global index (i, j) is owned by the calling rank.
     */
    bool is_local(El::Int i, El::Int j) const {
        return is_local_row(i) && is_local_col(j);
    }

    /**
     * @return true if the matrix has been finalized, false otherwise.
     */
    bool is_finalized() const { return _finalized; }

    /**
     * @return rank owning global row index i
     */
    int row_owner(El::Int i) const {
        return int((i + _col_align) % _col_stride);
    }

    /**
     * @return rank owning global column index j
     */
    int col_owner(El::Int j) const {
        return int((j + _row_align) % _row_stride);
    }

    /**
     * @return rank owning global index (i, j)
     */
    int owner(El::Int i, El::Int j) const {
        return row_owner(i) + col_owner(j) * _col_stride;
    }

private:

    std::unique_ptr< sparse_matrix_t<value_type> > _local_buffer;
    std::map< std::pair<int, int>, value_type, detail::compare_t > _temp_buffer;

    const boost::mpi::communicator _comm;

    bool _finalized;
    std::vector<int> _indptr;
    std::vector<int> _indices;
    std::vector<value_type> _values;

protected:

    int _rank;
    int _num_procs;

    El::Int _n_rows;
    El::Int _n_cols;

    El::Int _n_local_rows;
    El::Int _n_local_cols;

    int _nnz;
    int _global_nnz;

    int _col_rank;
    int _row_rank;

    int _row_align;
    int _row_shift;
    int _row_stride;
    int _col_align;
    int _col_shift;
    int _col_stride;

    const El::Grid& _grid;
};

} }

#endif


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/mzero.hpp>

STF_CASE_TPL (" bitofsign real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::bitofsign;
  using r_t = decltype(bitofsign(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(bitofsign(bs::Inf<T>()), bs::Zero<r_t>());
  STF_EQUAL(bitofsign(bs::Minf<T>()), bs::Mzero<r_t>());
  STF_IEEE_EQUAL(bitofsign(bs::Nan<T>()), bs::Mzero<r_t>());
#endif
  STF_EQUAL(bitofsign(bs::Mzero<T>()), bs::Mzero<r_t>());
  STF_EQUAL(bitofsign(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(bitofsign(bs::Zero<T>()), bs::Zero<r_t>());
}

STF_CASE_TPL (" bitofsign unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::bitofsign;
  using r_t = decltype(bitofsign(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(bitofsign(bs::One<T>()), bs::Zero<r_t>());
}

STF_CASE_TPL (" bitofsign signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::bitofsign;
  using r_t = decltype(bitofsign(T()));

  // return type conformity test
  STF_TYPE_IS(r_t, T);

  // specific values tests
  STF_EQUAL(bitofsign(bs::Mone<T>()), T(1ull << (sizeof(T)*8-1)));
  STF_EQUAL(bitofsign(bs::One<T>()), bs::Zero<r_t>());
  STF_EQUAL(bitofsign(bs::Zero<T>()), bs::Zero<r_t>());
}


#include <iostream>
#include <memory>
#include <string>

#include <boost/algorithm/string/split.hpp>
#include <boost/lambda/lambda.hpp>
#include <boost/range/combine.hpp>

#include <opts/opts.h>
#include <dlog/log.h>
#include <dlog/stats.h>

#include <diy/decomposition.hpp>
#include <diy/reduce.hpp>
#include <diy/partners/swap.hpp>
#include <diy/io/block.hpp>

#include <reeber/format.h>

#include "reader-interfaces.h"

#include "merge-tree-block.h"
#include "persistent-integral-block.h"

typedef diy::RegularDecomposer<diy::DiscreteBounds>                 Decomposer;
typedef MergeTreeBlock::Vertex                                      Vertex;
typedef MergeTreeBlock::Value                                       Value;

class TreeTracer
{
    public:
        typedef std::map<MergeTreeNode::Vertex, MinIntegral>        MinIntegralMap;
        typedef std::vector<diy::DiscreteBounds>                    Boxes;

    public:
                    TreeTracer(Boxes* boxes_, const Decomposer& decomposer_, diy::Master& pi_master_, Real m_, Real t_, std::vector<std::string> avg_fn_list_, std::string density_fn_, bool density_weighted_) :
                         decomposer(decomposer_), pi_master(pi_master_), m(m_), t(t_), density_reader(0), density_weighted(density_weighted_),
                         boxes(boxes_)
        {

            diy::mpi::communicator& world = pi_master_.communicator();

            for(const std::string& fn : avg_fn_list_)
                avg_var_readers.push_back(std::shared_ptr<Reader>(Reader::create(fn, world)));

            if (!density_fn_.empty())
                density_reader = std::shared_ptr<Reader>(Reader::create(density_fn_, world));
        }

        void        operator()(void *b, const diy::ReduceProxy& rp, const diy::RegularSwapPartners& partners) const
        {
            MergeTreeBlock &block = *static_cast<MergeTreeBlock*>(b);

            MinIntegralMap mi_map;
            if (rp.in_link().size() == 0)
            {
                std::vector<MergeTreeBlock::OffsetGrid*> add_data;
                for(std::shared_ptr<Reader> reader : avg_var_readers)
                    add_data.push_back(reader->read(block.core));

                MergeTreeBlock::OffsetGrid::GridProxy gp(0, block.global.shape());
                MergeTreeBlock::OffsetGrid *density_data = density_reader ? density_reader->read(block.core) : 0;
                trace(block.mt.find_root(), block, gp, add_data, density_data, mi_map);

                for(MergeTreeBlock::OffsetGrid* og : add_data)
                    delete og;
                delete density_data;
            }
            else
            {
                for (int i = 0; i < rp.in_link().size(); ++i)
                {
                    int gid = rp.in_link().target(i).gid;

                    // ALTERNATIVE: diy::MemoryBiffer& incoming = rp.incoming(gid);
                    while (rp.incoming(gid)) // ALTERNATIVE: while (incoming)
                    {
                        // ALTERNATIVE: diy::load(incoming, mi);
                        MinIntegral mi;
                        rp.dequeue(gid, mi);
                        if (mi_map.find(mi.min_vtx) == mi_map.end())
                            mi_map[mi.min_vtx] = mi;
                        else
                            mi_map[mi.min_vtx].combine(mi);
                    }
                }
            }

            if (rp.out_link().size() != 0)
            {
                diy::DiscreteBounds& box = (*boxes)[rp.master()->lid(rp.gid())];

                int         group_size  = rp.out_link().size();     // number of outbound partners
                unsigned    round       = rp.round();               // current round number
                int         dim         = partners.dim(round);      // current dimension along which groups are formed
                int         width       = (box.max[dim] - box.min[dim] + 1)/group_size;

                for(const MinIntegral& mi : mi_map | reeber::range::map_values)
                {
                    int dest_gid = decomposer.point_to_gid(block.global.position(mi.min_vtx));
                    Decomposer::DivisionsVector coords;
                    decomposer.gid_to_coords(dest_gid, coords);

                    int pos = (coords[dim] - box.min[dim])/width;
                    rp.enqueue(rp.out_link().target(pos), mi);
                }

                // adjust our box boundaries
                Decomposer::DivisionsVector coords;
                decomposer.gid_to_coords(rp.gid(), coords);
                int pos = (coords[dim] - box.min[dim])/width;

                box.max[dim] = box.min[dim] + (pos+1)*width - 1;
                box.min[dim] = box.min[dim] + pos*width;
            } else
            {
                PersistentIntegralBlock *pi_block = new PersistentIntegralBlock(block);
                for (MinIntegralMap::const_iterator it = mi_map.begin(); it != mi_map.end(); ++it)
                    pi_block->add_integral(it->second);
                pi_master.add(rp.gid(), pi_block, new diy::Link);
            }
        }

        void        trace(Neighbor n, const MergeTreeBlock& block, const MergeTreeBlock::OffsetGrid::GridProxy& gp, const std::vector<MergeTreeBlock::OffsetGrid*>& add_data, MergeTreeBlock::OffsetGrid* density_data, MinIntegralMap& mi_map) const
        {
            for(Neighbor child : n->children)
            {
                if (block.mt.cmp(t, child->value))                                   // still too high
                    trace(child, block, gp, add_data, density_data, mi_map);
                else                                                                 // crossing the threshold
                {
                    MinIntegral mi = integrate(child, block, gp, add_data, density_data);
                    if (block.mt.cmp(m, mi.min_val))
                        continue;

                    if (mi.integral == 0)                                            // non-local extrema are redundant
                        continue;

                    mi_map[mi.min_vtx] = mi;
                }
            }
        }

        MinIntegral integrate(Neighbor n, const MergeTreeBlock& block, const MergeTreeBlock::OffsetGrid::GridProxy& gp, const std::vector<MergeTreeBlock::OffsetGrid*>& add_data, MergeTreeBlock::OffsetGrid *density_data) const
        {

            MinIntegral mi_res(n, add_data.size());

            // Find contribution from n
            if (block.core.contains(n->vertex))
            {
                mi_res.integral += n->value * block.cell_size[0] * block.cell_size[1] * block.cell_size[2];
                ++mi_res.n_cells;
                for(auto t : boost::combine(mi_res.add_sums, add_data))
                {
                    Real new_val = (*t.get<1>())(n->vertex);
                    if (density_data)
                        new_val /= (*density_data)(n->vertex);
                    if (density_weighted)
                        new_val *= n->value * block.cell_size[0] * block.cell_size[1] * block.cell_size[2];
                    t.get<0>() += new_val;
                }
                mi_res.push_back(MergeTreeNode::ValueVertex(n->value, n->vertex));
            }

            for(const MergeTree::Node::ValueVertex& x : n->vertices)
                if (block.core.contains(x.second) && block.mt.cmp(x.first, t))
                {
                    mi_res.integral += x.first * block.cell_size[0] * block.cell_size[1] * block.cell_size[2];
                    ++mi_res.n_cells;
                    for(auto t : boost::combine(mi_res.add_sums, add_data))
                    {
                        Real new_val = (*t.get<1>())(x.second);
                        if (density_data)
                            new_val /= (*density_data)(x.second);
                        if (density_weighted)
                            new_val *= x.first * block.cell_size[0] * block.cell_size[1] * block.cell_size[2];
                        t.get<0>() += new_val;
                    }
                    mi_res.push_back(x);
                }

            // Find contribution from children + min
            for(Neighbor child : n->children)
            {
                MinIntegral mi = integrate(child, block, gp, add_data, density_data);
                if (block.mt.cmp(mi, mi_res))
                {
                    mi_res.min_val = mi.min_val;
                    mi_res.min_vtx = mi.min_vtx;
                }
                mi_res.combine(mi);
            }

            return mi_res;
        }

    private:
        const Decomposer&                      decomposer;
        diy::Master&                           pi_master;
        Real                                   m;
        Real                                   t;
        std::vector< std::shared_ptr<Reader> > avg_var_readers;
        std::shared_ptr<Reader>                density_reader;
        bool                                   density_weighted;
        std::vector<diy::DiscreteBounds>*  boxes;
};

bool vv_cmp(const MergeTreeNode::ValueVertex& a, const MergeTreeNode::ValueVertex& b)
{
    return a.second < b.second;
}

class OutputIntegrals {
    public:
                    OutputIntegrals(std::string outfn_, bool density_weighted_, bool verbose_) : outfn(outfn_), density_weighted(density_weighted_), verbose(verbose_) {}
       void         operator()(PersistentIntegralBlock* b, const diy::Master::ProxyWithLink& cp) const
       {
           PersistentIntegralBlock&  block = *b;

           std::string   dgm_fn = fmt::format("{}-b{}.comp", outfn, block.gid);
           std::ofstream ofs(dgm_fn.c_str());

           MergeTreeBlock::OffsetGrid::GridProxy gp(0, block.global.shape());
           for(MinIntegral &mi : block.persistent_integrals)
           {
               Vertex v = block.global.position(mi.min_vtx);
               ofs << v[0] * block.cell_size[0] << " " << v[1] * block.cell_size[1] << " " << v[2] * block.cell_size[2] << " ";
               if (verbose)
                   ofs << v[0] << "x" << v[1] << "x" << v[2] << " (" << mi.min_vtx << ") ";
               ofs <<  mi.integral;
               if (verbose)
                   ofs << " " << mi.n_cells;
               for(Real sum : mi.add_sums)
                   ofs << " " << sum / (density_weighted ? mi.integral : mi.n_cells);
               ofs << std::endl;
#ifdef REEBER_PERSISTENT_INTEGRAL_TRACE_VTCS
               std::sort(mi.vertices.begin(), mi.vertices.end(), vv_cmp);
               for (std::vector< MergeTreeNode::ValueVertex >::const_iterator it = mi.vertices.begin(); it != mi.vertices.end(); ++it)
                   ofs << "   " << it->second << " (" << block.global.position(it->second) <<  ")" << std::endl;
#if 0
               // Consistency check for debugging
               for (size_t i =0; i < mi.vertices.size()-1; ++i)
                   if (mi.vertices[i].second == mi.vertices[i+1].second)
                       LOG_SEV(info) << "Duplicate vertex " << mi.vertices[i].second << " in component " << mi.min_vtx;
#endif
#endif
           }
       }

    private:
        std::string outfn;
        bool        density_weighted;
        bool        verbose;
};

int main(int argc, char** argv)
{
    diy::mpi::environment   env(argc, argv);
    diy::mpi::communicator  world;
#ifdef REEBER_USE_BOXLIB_READER
    reeber::io::BoxLib::environment boxlib_env(argc, argv, world);
#endif

    using namespace opts;

    std::string prefix      = "./DIY.XXXXXX";
    int         in_memory   = -1;
    int         threads     = 1;
    int         k           = 2;
    Real        m           = 200;
    Real        t           = 82;

    std::string profile_path;
    std::string log_level = "info";
    std::string avg_fn_str = "";
    std::string density_fn = "";

    Options ops(argc, argv);
    ops
        >> Option('m', "memory",    in_memory,    "maximum blocks to store in memory")
        >> Option('j', "jobs",      threads,      "threads to use during the computation")
        >> Option('k', "k",         k,            "use k-ary swap")
        >> Option('s', "storage",   prefix,       "storage prefix")
        >> Option('p', "profile",   profile_path, "path to keep the execution profile")
        >> Option('l', "log",       log_level,    "log level")
        >> Option('x', "max",       m,            "maximum threshold")
        >> Option('i', "iso",       t,            "isofind threshold")
        >> Option('f', "mean",      avg_fn_str,   "list of additionals files/variables to average separated by ','")
        >> Option('q', "quotient",  density_fn,   "divide by density in file")
    ;
    bool absolute         = ops >> Present('a', "absolute", "use absolute values for thresholds (instead of multiples of mean)");
    bool verbose          = ops >> Present('v', "verbose",  "verbose output: logical coordiantes and number of cells");
    bool density_weighted = ops >> Present('w', "weight",   "compute density-weighted averages");
    bool split            = ops >> Present(     "split",    "use split IO");

    std::string infn, outfn;
    if (  ops >> Present('h', "help", "show help message") ||
        !(ops >> PosOption(infn) >> PosOption(outfn)))
    {
        if (world.rank() == 0)
            fmt::print("Usage: {} IN.lgt OUT.pi\n{}", argv[0], ops);
        return 1;
    }

    std::vector<std::string> avg_fn_list;
    if (!avg_fn_str.empty())
        boost::split(avg_fn_list, avg_fn_str, std::bind1st(std::equal_to<char>(), ','));

    dlog::add_stream(std::cerr, dlog::severity(log_level))
        << dlog::stamp() << dlog::aux_reporter(world.rank()) << dlog::color_pre() << dlog::level() << dlog::color_post() >> dlog::flush();

    std::ofstream   profile_stream;
    if (profile_path == "-")
        dlog::prof.add_stream(std::cerr);
    else if (!profile_path.empty())
    {
        std::string profile_fn = fmt::format("{}-r{}.prf", profile_path, world.rank());
        profile_stream.open(profile_fn.c_str());
        dlog::prof.add_stream(profile_stream);
    }

    world.barrier();
    dlog::Timer timer;
    LOG_SEV_IF(world.rank() == 0, info) << "Starting computation";

    diy::FileStorage            storage(prefix);

    diy::Master                 mt_master(world,
                                          threads,
                                          in_memory,
                                          &MergeTreeBlock::create,
                                          &MergeTreeBlock::destroy,
                                          &storage,
                                          &MergeTreeBlock::save,
                                          &MergeTreeBlock::load);

    diy::Master                 pi_master(world,
                                          threads,
                                          in_memory);

    diy::ContiguousAssigner     assigner(world.size(), 0);

    // load the trees
    LOG_SEV_IF(world.rank() == 0, debug) << "Reading blocks from " << infn;
    if (!split)
        diy::io::read_blocks(infn, world, assigner, mt_master);
    else
        diy::io::split::read_blocks(infn, world, assigner, mt_master);
    LOG_SEV_IF(world.rank() == 0, info) << "Blocks read: " << mt_master.size();

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to read data:                    " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // get the domain bounds from any block that's in memory (they are all the same) and set up a decomposer
    MergeTreeBlock::Box global = static_cast<MergeTreeBlock*>(((const diy::Master&) mt_master).block(mt_master.loaded_block()))->global;
    diy::DiscreteBounds domain {3};
    for (unsigned i = 0; i < 3; ++i)
    {
        domain.min[i] = global.from()[i];
        domain.max[i] = global.to()[i];
    }
    diy::RegularDecomposer<diy::DiscreteBounds>     decomposer(3, domain, assigner.nblocks(), Decomposer::BoolVector(3, true));

    // Compute average
    if (!absolute)
    {
        mt_master.foreach(&MergeTreeBlock::compute_average);
        mt_master.exchange();

        const diy::Master::ProxyWithLink& proxy = mt_master.proxy(mt_master.loaded_block());
        double mean = proxy.get<double>() / proxy.get<size_t>();
        m *= mean;
        t *= mean;

        LOG_SEV_IF(world.rank() == 0, info) << "Average value is " << mean << ". Using isofind threshold of " << t << " and maximum threshold of " << m;

        world.barrier();
        LOG_SEV_IF(world.rank() == 0, info) << "Time to compute average:              " << dlog::clock_to_string(timer.elapsed());
        timer.restart();
    }

    // Compute and combine persistent integrals
    diy::DiscreteBounds divs {3};
    for (unsigned i = 0; i < 3; ++i)
    {
        divs.min[i] = 0;
        divs.max[i] = decomposer.divisions[i];
    }
    TreeTracer::Boxes boxes(mt_master.size(), divs);
    diy::RegularSwapPartners  partners(decomposer, k, false);    // contiguous = false: distance halving
    diy::reduce(mt_master, assigner, partners, TreeTracer(&boxes, decomposer, pi_master, m, t, avg_fn_list, density_fn, density_weighted));

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to compute persistent integrals: " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    // Save persistent integrals to file
    pi_master.foreach(OutputIntegrals(outfn, density_weighted, verbose));

    world.barrier();
    LOG_SEV_IF(world.rank() == 0, info) << "Time to output persistent integrals:  " << dlog::clock_to_string(timer.elapsed());
    timer.restart();

    dlog::prof.flush();     // TODO: this is necessary because the profile file will close before
                            //       the global dlog::prof goes out of scope and flushes the events.
                            //       Need to eventually fix this.
    dlog::stats.flush();
}


//
//  SkeletonComponent.cpp
//  Helios
//
//  Created by James Hurley on 10/17/11.
//

#include <iostream>
#include <Component/SkeletonComponent.h>
#include <Render/RenderCommand.h>
#include <glm/gtx/matrix_interpolation.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/foreach.hpp>
namespace helios
{
    void
    SkeletonComponent::Process()
    {
       // mCurrentFrames.clear();

#if(USE_QUATERNIONS==1)
        mCurrentRots.clear();
        mCurrentTrans.clear();
#else
        mCurrentMat.clear();
#endif
        float mix;
        glm::quat q ;
        glm::vec3 v;
        if(mCurrentAnimation.currentTime >= mCurrentAnimation.endTime)
        {
            if(mCurrentAnimation.loop)
            {
                mCurrentAnimation.currentTime = mCurrentAnimation.startTime;

            } else {
                mCurrentAnimation = mDefaultAnimation;
                mAnimationEnded = true;
            }
        }

        for (  size_t i = 0 ; i != mJoints.size() ; ++i)
        {
            s::Frame previous, next;

            /** This is a disaster. FIX. **/
            for ( size_t j =0 ; j < mJoints[i].keyframes.size() ; ++j )
            {
                if(mJoints[i].keyframes[j].time > mCurrentAnimation.currentTime)
                {
                    next = mJoints[i].keyframes[j];
                    if(next.time > mCurrentAnimation.endTime)
                    {
                        for ( size_t k = 0 ; k < mJoints[i].keyframes.size() ; ++k)
                        {
                            if(mJoints[i].keyframes[k].time <= next.time && mJoints[i].keyframes[k+1].time > next.time)
                            {
                                next = mJoints[i].keyframes[k];
                                break;
                            }
                        }
                    }
                    break;
                }
                previous = mJoints[i].keyframes[j];
            }
            /* -------------------------------- */
            mix = (mCurrentAnimation.currentTime - previous.time) / (next.time - previous.time);
#if( USE_QUATERNIONS == 1 )

            // slerp
            //q=previous.rotation;
            q= glm::mix(previous.rotation, next.rotation, mix);
            // lerp
            v = glm::mix(previous.translation, next.translation, mix);
            //v = previous.translation;

            if(mJoints[i].parent >= 0)
            {
                q = q * mCurrentRots[mJoints[i].parent];
                v += mCurrentTrans[mJoints[i].parent] ;

            }
            s::Frame current;
            current.rotation = q;
            current.translation = v;
            current.time = mCurrentAnimation.currentTime;
            //mCurrentFrames.push_back(current);
            mCurrentRots.push_back(q);
            mCurrentTrans.push_back(v);
#else
            glm::mat4 m=glm::interpolate(previous.mat, next.mat, mix);

            // perform matrix concatenation
            if(mJoints[i].parent >=0 )
                m = mCurrentMat[mJoints[i].parent] * m;

            mCurrentMat.push_back(m);
#endif
        }

    }
    void
    SkeletonComponent::Update(uint64_t t)
    {

        uint64_t delta = t - mCurrentTime;
        if(mCurrentTime == 0)
        {
            delta = 0;
        }
        mCurrentTime = t;
        mCurrentAnimation.currentTime += (float)(delta / 1.0e6f);
        {
            auto e = mOwner.GetEvents(e::kEventTargetAction);
            bool endAnimation = TRUE;
            bool gotActionEnd = FALSE ;

            for ( auto it = e.begin() ; it != e.end() ; ++it )
            {
                auto k = HEvent<const char>::GetData((*it));

                if(mAnimationMap.find((*it)->GetName()) != mAnimationMap.end()  ) {
                    if(k < 2) {
                        std::pair<int, int> anim = mAnimationMap[(*it)->GetName()];
                        SetAnimation(anim.first, anim.second, 30.f, TRUE);
                        endAnimation = FALSE;
                        break;
                    }
                    else
                    {
                        gotActionEnd = TRUE;
                    }
                }
            }
            if(endAnimation && gotActionEnd)
            {
                mCurrentAnimation = mDefaultAnimation;
            }
        }
    }
    void
    SkeletonComponent::DispatchEvents()
    {

#if(USE_QUATERNIONS == 1)
        std::vector<UniformData> v;
        if(mCurrentRots.size() > 0) {

        {
            UniformData ud(UniformData::kUTVec4, mJointRotationLoc);
            ud.SetData<float*>(&mCurrentRots[0][0],mCurrentRots.size());

            v.push_back(ud);
        }
        {
            UniformData ud(UniformData::kUTVec3, mJointTransformLoc);
            ud.SetData<float*>(&mCurrentTrans[0][0], mCurrentTrans.size());
            v.push_back(ud);
        }
        auto e = mOwner.PushEvent<std::vector<UniformData> >(e::kEventTargetSkeleton, e::kEventSkeleton, v);
        }
#else
        if(mCurrentMat.size() > 1)
        {
            UniformData_ptr ud( new UniformData(UniformData::kUTMat4, &e::kVertexUniformJoints));
            ud->SetData<float*>(&mCurrentMat[0][0][0], mCurrentMat.size());
            auto e = mOwner.PushEvent<UniformData_ptr>(e::kEventTargetUniformData, e::kEventUniformData, ud);
            if ( mEventElevation.push.layer )
            {
                mOwner.GetOwner()->PushEvent(e);
            }
        } else if ( mCurrentMat.size() > 0)
        {
            UniformData_ptr u( new UniformData(UniformData::kUTMat4, &e::kVertexUniformJoints));
            u->SetData<glm::mat4>(mCurrentMat[0]);

            auto e = mOwner.PushEvent<UniformData_ptr>(e::kEventTargetUniformData, e::kEventUniformData, u);
            if ( mEventElevation.push.layer )
            {
                mOwner.GetOwner()->PushEvent(e);
            }
        }
        if ( mAnimationEnded ) {

            bool ended = mAnimationEnded;

            auto e = mOwner.PushEvent<bool>(e::kEventTargetAnimation, e::kEventAnimationEnd, ended);

            /** TODO: Elevate specific event target types
             **/

            mOwner.GetOwner()->PushEvent(e);
            mAnimationEnded = false;
        }
        if ( mAnimationStarted ) {
            bool started = mAnimationStarted;

            auto e = mOwner.PushEvent<bool>(e::kEventTargetAnimation, e::kEventAnimationStart, started);

            mOwner.GetOwner()->PushEvent(e);

            mAnimationStarted = false;
        }
#endif

    }
    void
    SkeletonComponent::SetAnimation(int startKeyframe, int endKeyframe, float fps, bool loop)
    {
        int id = (startKeyframe ^ 2) + (endKeyframe ^3) + ((int)fps ^5) + (loop) ;

        if( mCurrentAnimation.id == id ) return ;

        float delta = 1.f / fps;

        float startTarget = startKeyframe * delta;
        float endTarget = endKeyframe * delta;

        mCurrentAnimation.id = id ;
        mCurrentAnimation.startTime = startTarget;
        mCurrentAnimation.endTime = endTarget;
        mCurrentAnimation.fpsDelta = delta;
        mCurrentAnimation.loop = loop;
        mCurrentAnimation.currentTime = startTarget;
        mAnimationStarted = true;

    }
    void
    SkeletonComponent::SetDefaultAnimation(int startKeyframe, int endKeyframe, float fps)
    {
        float delta = 1.f / fps;

        float startTarget = startKeyframe * delta;
        float endTarget = endKeyframe * delta;

        mDefaultAnimation.startTime = startTarget;
        mDefaultAnimation.endTime = endTarget;
        mDefaultAnimation.fpsDelta = delta;
        mDefaultAnimation.loop = TRUE;
        mDefaultAnimation.currentTime = startTarget;

        mCurrentAnimation = mDefaultAnimation;
    }
    void
    SkeletonComponent::LoadAnimationMap(const std::string & file)
    {
        boost::property_tree::ptree pt;
        try
        {
            boost::property_tree::read_json(file, pt);
        }
        catch(boost::property_tree::json_parser::json_parser_error &je)
        {
            std::cout << "Error parsing: " << je.filename() << " on line: " << je.line() << std::endl;
            std::cout << je.message() << std::endl;
        }
        BOOST_FOREACH(const boost::property_tree::ptree::value_type &v, pt.get_child("keyframes"))
        {

            int i = 0 ;

            std::string action = v.first.data() ;

            BOOST_FOREACH(const boost::property_tree::ptree::value_type &s, v.second)
            {
                switch(i) {
                    case 0 :
                        mAnimationMap[action].first =  s.second.get_value<int>() ;
                    default:
                        mAnimationMap[action].second =  s.second.get_value<int>() ;
                }
                i++;
            }

        }
    }
}


#ifndef TYPEDATA_HPP_
#define TYPEDATA_HPP_

#include <tuple>
#include <vector>
#include <ale_interface.hpp>
#include <boost/optional.hpp>
#include <boost/functional/hash.hpp>
#include <caffe/caffe.hpp>

namespace deepRL {

//original frame size
constexpr auto kRawFrameHeight = 210;
constexpr auto kRawFrameWidth = 160;
//cropped size
constexpr auto kCroppedFrameSize = 84;
constexpr auto kCroppedFrameDataSize = kCroppedFrameSize * kCroppedFrameSize;
//4 frames -> input
constexpr auto kInputFrameCount = 4;
constexpr auto kInputDataSize = kCroppedFrameDataSize * kInputFrameCount;
//batch size
constexpr auto kMinibatchSize = 32;
constexpr auto kMinibatchDataSize = kInputDataSize * kMinibatchSize;
//discrete actions
constexpr auto kOutputCount = 18;

using FrameData = std::array<uint8_t, kCroppedFrameDataSize>;
using FrameDataSp = std::shared_ptr<FrameData>;
using InputFrames = std::array<FrameDataSp, 4>;
using Transition = std::tuple<InputFrames, Action, float, boost::optional<FrameDataSp>>;//experience

using FramesLayerInputData = std::array<float, kMinibatchDataSize>;//caffe input
using TargetLayerInputData = std::array<float, kMinibatchSize * kOutputCount>;//Q_target
using FilterLayerInputData = std::array<float, kMinibatchSize * kOutputCount>;//

using SolverSp = boost::shared_ptr<caffe::Solver<float>>;
using NetSp = boost::shared_ptr<caffe::Net<float>>;
using BlobSp = boost::shared_ptr<caffe::Blob<float>>;
using MemoryDataLayerSp = boost::shared_ptr<caffe::MemoryDataLayer<float>>;

using ActionPair = std::pair<Action, float>;
using ActionPairVec = std::vector<ActionPair>;
using InputFramesVec = std::vector<InputFrames>;

}

#endif /* TYPEDATA_HPP_ */


// Copyright (c) 2014-2016 The Bitcoin Core developers
// Copyright (c) 2016-2019 The MagnaChain Core developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "chain/chainparams.h"
#include "validation/validation.h"
#include "net/net.h"

#ifdef BUILD_MSVC
#define BOOST_TEST_MODULE maintest
#endif

#include "test/test_magnachain.h"

#include <boost/signals2/signal.hpp>
#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(main_tests, TestingSetup)

static void TestBlockSubsidyHalvings(const Consensus::Params& consensusParams)
{
    int maxHalvings = 64;
    MCAmount nInitialSubsidy = 85 * COIN;

    MCAmount nPreviousSubsidy = nInitialSubsidy * 2; // for height == 0
    BOOST_CHECK_EQUAL(nPreviousSubsidy, nInitialSubsidy * 2);
    for (int nHalvings = 0; nHalvings < maxHalvings; nHalvings++) {
        int nHeight = nHalvings * consensusParams.nSubsidyHalvingInterval;
        MCAmount nSubsidy = GetBlockSubsidy(nHeight, consensusParams);
		if (consensusParams.BigBoomHeight >= nHeight)
		{
			BOOST_CHECK(nSubsidy <= nInitialSubsidy + consensusParams.BigBoomValue);
			BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2 + consensusParams.BigBoomValue);
		}
		else
		{
			BOOST_CHECK(nSubsidy <= nInitialSubsidy);
			BOOST_CHECK_EQUAL(nSubsidy, nPreviousSubsidy / 2);
		}
        nPreviousSubsidy = nPreviousSubsidy / 2;//nSubsidy
    }
    BOOST_CHECK_EQUAL(GetBlockSubsidy(maxHalvings * consensusParams.nSubsidyHalvingInterval, consensusParams), 0);
}

static void TestBlockSubsidyHalvings(int nSubsidyHalvingInterval)
{
    Consensus::Params consensusParams;
    consensusParams.nSubsidyHalvingInterval = nSubsidyHalvingInterval;
    TestBlockSubsidyHalvings(consensusParams);
}

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
	ECC_Stop();
    const auto chainParams = CreateChainParams(MCBaseChainParams::MAIN);
	ECC_Start();
    TestBlockSubsidyHalvings(chainParams->GetConsensus()); // As in main
    TestBlockSubsidyHalvings(150); // As in regtest
    TestBlockSubsidyHalvings(1000); // Just another interval
}

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    const auto chainParams = CreateChainParams(MCBaseChainParams::MAIN);
    const Consensus::Params& consensus = chainParams->GetConsensus();
    MCAmount nSum = 0;
    for (int nHeight = 0; nHeight < 560000000; nHeight += 10000) {
        MCAmount nSubsidy = GetBlockSubsidy(nHeight, consensus);
        MCAmount nTargetSubsidy = 85 * COIN;
        if (nHeight <= consensus.BigBoomHeight) {// next nHeight jump out of BigBoomHeight
            nTargetSubsidy += consensus.BigBoomValue;
            nSum += (nSubsidy- consensus.BigBoomValue) * 10000 + consensus.BigBoomValue * consensus.BigBoomHeight;// same as interval
        }
        else{
            nSum += nSubsidy * 10000;// same as interval
        }
        BOOST_CHECK(nSubsidy <= nTargetSubsidy);
        BOOST_CHECK(MoneyRange(nSum));
    }
    BOOST_CHECK_EQUAL(nSum, 402799999865600000ULL);
}

bool ReturnFalse() { return false; }
bool ReturnTrue() { return true; }

BOOST_AUTO_TEST_CASE(test_combiner_all)
{
    boost::signals2::signal<bool (), CombinerAll> Test;
    BOOST_CHECK(Test());
    Test.connect(&ReturnFalse);
    BOOST_CHECK(!Test());
    Test.connect(&ReturnTrue);
    BOOST_CHECK(!Test());
    Test.disconnect(&ReturnFalse);
    BOOST_CHECK(Test());
    Test.disconnect(&ReturnTrue);
    BOOST_CHECK(Test());
}
BOOST_AUTO_TEST_SUITE_END()


/*
 * This file is part of the DUDS project. It is subject to the BSD-style
 * license terms in the LICENSE file found in the top-level directory of this
 * distribution and at http://www.somewhere.org/somepath/license.html.
 * No part of DUDS, including this file, may be copied, modified, propagated,
 * or distributed except according to the terms contained in the LICENSE file.
 *
 * Copyright (C) 2017  Jeff Jackowski
 */
#include <duds/hardware/devices/instruments/TSL2591.hpp>
#include <duds/hardware/interface/linux/DevI2c.hpp>
#include <iostream>
#include <thread>
#include <iomanip>
#include <assert.h>
#include <boost/exception/diagnostic_information.hpp>

constexpr int valw = 8;
bool quit = false;

void runtest(duds::hardware::devices::instruments::TSL2591 &meter)
try {
	duds::data::Unit chku = duds::data::units::Watt / (
			duds::data::units::Meter * duds::data::units::Meter
		);
	std::cout.precision(6);
	std::uint16_t c0, c1;
	do {
		std::this_thread::sleep_for(std::chrono::seconds(1));
		meter.sample();
		duds::data::Quantity visB = meter.brightness();
		duds::data::Quantity irB = meter.brightnessIr();
		assert(visB.unit == chku);
		assert(irB.unit == chku);
		std::cout << "Visible: " << std::setw(16) << visB.value << "W/m2  " <<
		std::setw(5) << meter.brightnessCount() << " count   IR: " <<
		std::setw(16) << irB.value << "W/m2  " << std::setw(5) <<
		meter.brightnessIrCount() << " count" << std::endl;
	} while (!quit);
} catch (...) {
	std::cerr << "Program failed in runtest(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
}

int main(int argc, char *argv[])
try {
	std::unique_ptr<duds::hardware::interface::I2c> i2c(
		new duds::hardware::interface::linux::DevI2c(
			argc > 1 ? argv[1] : "/dev/i2c-1",
			0x29
		)
	);
	duds::hardware::devices::instruments::TSL2591 meter(i2c);
	int gain = 0;
	int integration = 0;
	/*
	if (argc > 1) {
		gain = argv[1][0] - '0';
	}
	if (argc > 2) {
		integration = argv[2][0] - '0';
	}
	*/
	meter.init(gain, integration);
	std::this_thread::sleep_for(std::chrono::milliseconds(2));
	std::thread doit(runtest, std::ref(meter));
	std::cin.get();
	quit = true;
	doit.join();
} catch (...) {
	std::cerr << "Program failed in main(): " <<
	boost::current_exception_diagnostic_information() << std::endl;
	return 1;
}