/*
BSD 3-Clause License

Copyright (c) 2017, Alibaba Cloud
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

* Redistributions of source code must retain the above copyright notice, this
  list of conditions and the following disclaimer.

* Redistributions in binary form must reproduce the above copyright notice,
  this list of conditions and the following disclaimer in the documentation
  and/or other materials provided with the distribution.

* Neither the name of the copyright holder 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.
*/
#include "random.hpp"
#include "tablestore/util/assert.hpp"
#include "tablestore/util/timestamp.hpp"
#include <boost/random/mersenne_twister.hpp>
#include <string>
#include <deque>

using namespace std;

namespace aliyun {
namespace tablestore {
namespace util {
namespace random {

namespace {

class Default : public Random
{
public:
    explicit Default(unsigned int seed)
      : mSeed(seed)
    {
        init();
    }
    explicit Default()
      : mSeed(UtcTime::now().toUsec())
    {
        init();
    }

    uint64_t upperBound() const
    {
        uint64_t up = mRng.max();
        return up + 1;
    }

    uint64_t next()
    {
        return mRng();
    }

    uint64_t seed() const
    {
        return mSeed;
    }

private:
    void init()
    {
        mRng.seed(static_cast<uint32_t>(mSeed));
    }

private:
    uint64_t mSeed;
    boost::mt19937 mRng;
};

uint64_t nextUint(Random& rng, uint64_t upper)
{
    OTS_ASSERT(upper > 0)(upper);
    const uint64_t bound = rng.upperBound();
    if (upper <= bound) {
        return rng.next() % upper;
    } else {
        uint64_t shiftUpper = upper;
        deque<uint64_t> segs;
        for(; shiftUpper > bound; shiftUpper /= bound) {
            segs.push_back(rng.next());
        }
        segs.push_back(rng.next() % shiftUpper);
        uint64_t res = 0;
        for(; !segs.empty(); segs.pop_back()) {
            res *= bound;
            res += segs.back();
        }
        return res % upper;
    }
}

} // namespace

Random* newDefault()
{
    return new Default();
}

Random* newDefault(uint64_t seed)
{
    return new Default(seed);
}

int64_t nextInt(Random& rng, int64_t exclusiveUpper)
{
    OTS_ASSERT(exclusiveUpper > 0)(exclusiveUpper);
    return nextUint(rng, exclusiveUpper);
}

int64_t nextInt(Random& rng, int64_t inclusiveLower, int64_t exclusiveUpper)
{
    OTS_ASSERT(exclusiveUpper > inclusiveLower)
        (inclusiveLower)
        (exclusiveUpper);
    uint64_t range = static_cast<uint64_t>(exclusiveUpper) - static_cast<uint64_t>(inclusiveLower);
    int64_t res = nextUint(rng, range);
    res += inclusiveLower;
    return res;
}

} // namespace random
} // namespace util
} // namespace tablestore
} // namespace aliyun


/**
 * @brief   Mapping functions from Eigen data types to OpenCV
 * @author  Eugene Khvedchenya <ekhvedchenya@gmail.com>
 * @details This header file contains code snippet for easy mapping Eigen types
 * to OpenCV and back with minimal overhead.
 * @more    computer-vision.talks.com/articles/mapping-eigen-to-opencv/
 * Features:
 *  - Mapping plain data types with no overhead (read/write access)
 *  - Mapping expressions via evaluation (read only acess)
 *
 * Known issues:
 *  - Does not support .transpose()
 */
#ifndef INTERNAL_GRID_MAP_EIGEN2CV_H
#define INTERNAL_GRID_MAP_EIGEN2CV_H

#include <Eigen/Core>
#include <opencv2/opencv.hpp>

namespace details {
struct Const {};
struct Mutable {};
}

template <typename T>
struct opencv_matrix;

template <>
struct opencv_matrix<double> {
    static const int type = CV_64FC1;
};
template <>
struct opencv_matrix<float> {
    static const int type = CV_32FC1;
};
template <>
struct opencv_matrix<int> {
    static const int type = CV_32SC1;
};
template <>
struct opencv_matrix<unsigned char> {
    static const int type = CV_8UC1;
};
template <>
struct opencv_matrix<bool> {
    static_assert(sizeof(bool) == 1, "Requires bool to be 1 byte");
    static const int type = CV_8UC1;
};

/**
 * PlainObjectBase<D> - map directly (read/write)
 * ArrayWrapper/MatrixWrapper on PlainObjectBase - map directly (read/write)
 * Block<D> - recursively instantiate. Can be read-only (derived is expression)
 * or read/write (derived is planar)
 * Expression - evaluate (read-only)
 */
template <typename Derived, typename Base, typename ConstPolicy,
          typename StorageKind =
                  typename Eigen::internal::traits<Derived>::StorageKind>
class Eigen2CV;

class Eigen2CVBase {
 public:
    operator cv::Mat() const { return mBody; }

    operator cv::_InputArray() const { return cv::_InputArray(mBody); }

 protected:
    template <typename Derived>
    void mapPlaneMemory(const Derived& src) {
        const bool isRowMajor = int(Derived::Flags) & Eigen::RowMajorBit;
        const int stride = src.outerStride() * sizeof(typename Derived::Scalar);

        if (isRowMajor)
            this->mapPlaneMemoryRowMajor(src.data(), src.rows(), src.cols(),
                                         stride);
        else
            this->mapPlaneMemoryColMajor(src.data(), src.rows(), src.cols(),
                                         stride);
    }

    template <typename Scalar>
    void mapPlaneMemoryRowMajor(const Scalar* planeData, int rows, int cols,
                                int stride) {
        this->mBody = cv::Mat(rows, cols, opencv_matrix<Scalar>::type,
                              const_cast<Scalar*>(planeData), stride);
    }

    template <typename Scalar>
    void mapPlaneMemoryColMajor(const Scalar* planeData, int rows, int cols,
                                int stride) {
        this->mBody = cv::Mat(cols, rows, opencv_matrix<Scalar>::type,
                              const_cast<Scalar*>(planeData), stride);
    }

    template <typename Derived, typename T>
    void assignMatrix(Eigen::DenseBase<Derived>& dst, const cv::Mat_<T>& src) {
        typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic,
                              Eigen::RowMajor>
                PlainMatrixType;

        dst = Eigen::Map<PlainMatrixType>((T*)src.data, src.rows, src.cols)
                      .template cast<typename Derived::Scalar>();
    }

    cv::Mat mBody;
};

template <typename Derived>
class Eigen2CV<Derived, Eigen::PlainObjectBase<Derived>, details::Const>
        : public Eigen2CVBase {
 public:
    typedef typename Derived::Scalar Scalar;
    typedef Eigen2CV<Derived, Eigen::PlainObjectBase<Derived>, details::Mutable>
            Self;

    Eigen2CV(const Eigen::PlainObjectBase<Derived>& src) : mMappedView(src) {
        this->mapPlaneMemory(mMappedView);
    }

 private:
    const Eigen::PlainObjectBase<Derived>& mMappedView;
};

template <typename Derived>
class Eigen2CV<Derived, Eigen::PlainObjectBase<Derived>, details::Mutable>
        : public Eigen2CVBase {
 public:
    typedef typename Derived::Scalar Scalar;
    typedef Eigen2CV<Derived, Eigen::PlainObjectBase<Derived>, details::Mutable>
            Self;

    Eigen2CV(Eigen::PlainObjectBase<Derived>& src) : mMappedView(src) {
        this->mapPlaneMemory(mMappedView);
    }

    template <typename T>
    Self& operator=(const cv::Mat_<T>& src) {
        assignMatrix<Derived, T>(mMappedView, src);
        return *this;
    }

    /**
     * @brief Assignment operator to copy OpenCV Mat data to mapped Eigen
     * object.
     */
    Self& operator=(const cv::Mat& m) {
        switch (m.type()) {
            case CV_8U:
                return *this = (cv::Mat_<uint8_t>)m;
            case CV_16U:
                return *this = (cv::Mat_<uint16_t>)m;
            case CV_16S:
                return *this = (cv::Mat_<int16_t>)m;
            case CV_32S:
                return *this = (cv::Mat_<int32_t>)m;
            case CV_32F:
                return *this = (cv::Mat_<float>)m;
            case CV_64F:
                return *this = (cv::Mat_<double>)m;
            default:
                throw std::runtime_error("Unsupported OpenCV matrix type");
        };
    }

    operator cv::_InputOutputArray() {
        return cv::_InputOutputArray(this->mBody);
    }

 protected:
 private:
    Eigen::PlainObjectBase<Derived>& mMappedView;
};

template <typename Derived>
class Eigen2CV<Derived, Eigen::Block<Derived>, details::Mutable, Eigen::Dense>
        : public Eigen2CVBase {
 public:
    typedef typename Derived::Scalar Scalar;
    typedef Eigen2CV<Derived, Eigen::Block<Derived>, details::Mutable> Self;

    Eigen2CV(const Eigen::Block<Derived>& src) : mMappedView(src) {
        this->mapPlaneMemory(mMappedView);
    }

    operator cv::_OutputArray() { return cv::_OutputArray(this->mBody); }

    template <typename T>
    Self& operator=(const cv::Mat_<T>& src) {
        assignMatrix<Derived, T>(mMappedView, src);
        return *this;
    }

    /**
     * @brief Assignment operator to copy OpenCV Mat data to mapped Eigen
     * object.
     */
    Self& operator=(const cv::Mat& m) throw() {
        switch (m.type()) {
            case CV_8U:
                return *this = (cv::Mat_<uint8_t>)m;
            case CV_16U:
                return *this = (cv::Mat_<uint16_t>)m;
            case CV_16S:
                return *this = (cv::Mat_<int16_t>)m;
            case CV_32S:
                return *this = (cv::Mat_<int32_t>)m;
            case CV_32F:
                return *this = (cv::Mat_<float>)m;
            case CV_64F:
                return *this = (cv::Mat_<double>)m;
            default:
                throw std::runtime_error("Unsupported OpenCV matrix type");
        };
    }

 private:
    const Eigen::Block<Derived>& mMappedView;
};

/**
 * @brief Most generic and most inefficient mapper - it evaluates input object
 * into a local copy.
 */
template <typename Derived>
class Eigen2CV<Derived, Eigen::EigenBase<Derived>, details::Const>
        : public Eigen2CVBase {
 public:
    typedef typename Derived::Scalar Scalar;
    typedef typename Eigen::internal::plain_matrix_type<Derived>::type
            StorageType;

    Eigen2CV(const Eigen::EigenBase<Derived>& src) {
        mStorage = src;
        this->mapPlaneMemory(mStorage);
    }

 private:
    StorageType mStorage;
};

template <typename E>
Eigen2CV<E, Eigen::PlainObjectBase<E>, details::Mutable> eigen2cv(
        Eigen::PlainObjectBase<E>& src) {
    return Eigen2CV<E, Eigen::PlainObjectBase<E>, details::Mutable>(src);
}

template <typename E>
Eigen2CV<E, Eigen::PlainObjectBase<E>, details::Const> eigen2cv(
        const Eigen::PlainObjectBase<E>& src) {
    return Eigen2CV<E, Eigen::PlainObjectBase<E>, details::Const>(src);
}

template <typename E>
Eigen2CV<E, Eigen::Block<E>, details::Mutable> eigen2cv(
        const Eigen::Block<E>& src) {
    return Eigen2CV<E, Eigen::Block<E>, details::Mutable>(src);
}

template <typename E>
auto eigen2cv(const Eigen::ArrayWrapper<E>& src)
        -> decltype(eigen2cv(src.nestedExpression().eval())) {
    return eigen2cv(src.nestedExpression().eval());
}

template <typename E>
auto eigen2cv(const Eigen::MatrixWrapper<E>& src)
        -> decltype(eigen2cv(src.nestedExpression().eval())) {
    return eigen2cv(src.nestedExpression().eval());
}

template <typename E>
Eigen2CV<E, Eigen::EigenBase<E>, details::Const> eigen2cv(
        const Eigen::EigenBase<E>& src) {
    return Eigen2CV<E, Eigen::EigenBase<E>, details::Const>(src);
}  // namespace hmpl
#endif  // INTERNAL_GRID_MAP_EIGEN2CV_H


#include <pcl/io/pcd_io.h>  //PCD相关头文件
#include <pcl/point_types.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp_nl.h>

#include <boost/program_options.hpp>
#include <iostream>
#include <string>
#include <filesystem>
#include <vector>
#include <algorithm>

namespace bpo = ::boost::program_options;
namespace fs = std::filesystem;
using PointT = pcl::PointXYZ;                 // x,y,z点
using PointCloudT = pcl::PointCloud<PointT>;  //点云　申明pcl::PointXYZ数据
using PointWithNormalT = pcl::PointNormal;

int main(int argc, char** argv) {
    // prase command line options
    std::string filepath_in;
    std::string filename_out;
    {
        bpo::options_description desc("Allowed options");
        desc.add_options()
            ("in,i", bpo::value<std::string>(&filepath_in), "input pcd file path !!! NOT FILE NAME !!!")
            ("out,o", bpo::value<std::string>(&filename_out), "output file name")
            ("help,h", "icp -i <input-filepath> -o <output-filename>");
        bpo::variables_map vm;

        try
        {
            bpo::store(bpo::parse_command_line(argc, argv, desc), vm);
        } catch (const std::exception& e) {
            std::cerr << e.what() << std::endl;
            std::cerr << desc << std::endl;
            return -1;
        }
        bpo::notify(vm);

        if(!(vm.count("in") && vm.count("out")))
        {
            std::cerr << desc << std::endl;
            return -1;
        }
    }

    // sort file name in right order
    std::vector<std::string> filenames_in;
    {
        if(!fs::exists(filepath_in))
        {
            std::cerr << "Cannot open dir " << filepath_in << std::endl;
            return -1;
        }
        for(const auto& entry : fs::directory_iterator(filepath_in))
        {
            filenames_in.push_back(entry.path());
        }
        std::sort(filenames_in.begin(), filenames_in.end());
    }

    // load file
    std::vector<PointCloudT::Ptr> clouds_in;
    {
        std::cout << "Loading pcd files" << std::endl;
        pcl::Indices indices_nan;
        for(const auto& name : filenames_in)
        {
            clouds_in.emplace_back(new PointCloudT);
            if(pcl::io::loadPCDFile(name, *clouds_in.back()) == -1)
            {
                std::cerr << "PCD File load error " << name << std::endl;
                return -1;
            }
            pcl::removeNaNFromPointCloud<PointT>(
                *clouds_in.back(), *clouds_in.back(), indices_nan);

            std::cout << name << " points num:" << clouds_in.back()->size() << std::endl;
        }

        std::cout << "\nSuccess load " << clouds_in.size() << " cloud" << std::endl;
    }

    // do icp
    PointCloudT::Ptr cloud_all(new PointCloudT);
    {
        // search method kdtree
        pcl::search::Search<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);

        // down sample
        pcl::VoxelGrid<PointT> vg;
        vg.setLeafSize(0.1, 0.1, 0.1);
        vg.setDownsampleAllData(true);

        // cal Normal
        pcl::NormalEstimation<PointT, PointWithNormalT> ne;
        ne.setKSearch(30);
        ne.setSearchMethod(tree);

        // icp
        pcl::IterativeClosestPointNonLinear<PointWithNormalT, PointWithNormalT> icp_nl;
        icp_nl.setTransformationEpsilon(1e-6);
        icp_nl.setMaxCorrespondenceDistance(0.05);
        icp_nl.setMaximumIterations(2);

        pcl::PointCloud<PointWithNormalT>::Ptr cloud_src(new pcl::PointCloud<PointWithNormalT>);
        pcl::PointCloud<PointWithNormalT>::Ptr cloud_tgt(new pcl::PointCloud<PointWithNormalT>);
        Eigen::Matrix4f T_cur, T_prv, T_all;
        for (auto& cloud_in : clouds_in) {
            vg.setInputCloud(cloud_in);
            vg.filter(*cloud_in);

            ne.setInputCloud(cloud_in);
            ne.compute(*cloud_src);
            pcl::copyPointCloud(*cloud_in, *cloud_src);

            if(!cloud_all->empty())
            {
                T_all = T_all.Identity();
                icp_nl.setInputTarget(cloud_tgt);
                for (int i = 0; i < 30; i++)
                {
                    icp_nl.setInputSource(cloud_src);
                    icp_nl.align(*cloud_src);
                    T_all = icp_nl.getFinalTransformation() * T_all;
                    T_cur = icp_nl.getLastIncrementalTransformation();
                    if(fabs((T_cur-T_prv).sum()) < icp_nl.getTransformationEpsilon())
                        icp_nl.setMaxCorrespondenceDistance(icp_nl.getMaxCorrespondenceDistance() - 0.001);
                    else
                        break;
                    T_prv = T_cur;
                }
                pcl::transformPointCloud(*cloud_in, *cloud_in, T_all);
            }

            *cloud_all += *cloud_in;
            // make this loop's tgt's space to get next loop's data
            // and save this loop's src's data to tgt without copying any data
            std::swap(cloud_tgt, cloud_src);
        }

        // down samle cloud_all, because too many duplicate points
        vg.setInputCloud(cloud_all);
        vg.filter(*cloud_all);
    }

    pcl::io::savePCDFile(filename_out, *cloud_all);

    return 0;
}


#include "myslam/visual_odometry.h"
#include "myslam/config.h"

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <algorithm>
#include <boost/timer.hpp>

namespace myslam{
    VisualOdometry::VisualOdometry():
    state_ ( INITIALIZING ), ref_ ( nullptr ), curr_ ( nullptr ), map_ ( new Map ), num_lost_ ( 0 ), num_inliers_ ( 0 )
    {
        num_of_features_    = Config::get<int> ( "number_of_features" );
        scale_factor_       = Config::get<double> ( "scale_factor" );
        level_pyramid_      = Config::get<int> ( "level_pyramid" );
        match_ratio_        = Config::get<float> ( "match_ratio" );
        max_num_lost_       = Config::get<float> ( "max_num_lost" );
        min_inliers_        = Config::get<int> ( "min_inliers" );
        key_frame_min_rot_   = Config::get<double> ( "keyframe_rotation" );
        key_frame_min_trans_ = Config::get<double> ( "keyframe_translation" );
        orb_ = cv::ORB::create ( num_of_features_, scale_factor_, level_pyramid_ );
    }

    VisualOdometry::~VisualOdometry(){

    }

    bool VisualOdometry::addFrame(Frame::Ptr frame){
        switch (state_)
        {
        case INITIALIZING:{
            state_ = OK;
            curr_ = frame;
            ref_ = frame;
            map_->insertKeyFrame(frame);
            // Extract feature from first frame
            extractKeyPoints();
            computerDescriptors();
            // Treat No.1 Frame as keyframe
            addKeyFrame();
            break;
        }

        case OK:{
            curr_ = frame;
            curr_->T_w_c_ = ref_->T_w_c_;
            extractKeyPoints();
            computerDescriptors();
            featureMatching();
            poseEstimationPnP();

            if (checkEstimatedPose() == true){
                curr_->T_w_c_ = T_c_r_estimated_; // update current camera pose

                optimizeMap();
                num_lost_ = 0;
                if (checkKeyFrame() == true){  // if current frame is a keyframe, add it
                    addKeyFrame();
                }
            }
            else{
                num_lost_++;
                if(num_lost_ > max_num_lost_){
                    state_ = LOST;
                }
                return false;
            }

            break;
        }

        case LOST:{
            // Nothing we can do to invert.
            cout << "SLAM have lost" << endl;
            break;
        }
        default:
            break;
        }

        return true;
    }

    void VisualOdometry::extractKeyPoints(){
        orb_->detect(curr_->color_, keypoints_curr_);
    }

    void VisualOdometry::computerDescriptors(){
        orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
    }

    void VisualOdometry::featureMatching(){
        // Select a portion of Map Points to match current feature
        vector<cv::DMatch> matches;
        boost::timer timer;

        Mat descriptor_map;
        vector<MapPoint::Ptr> candidate;

        for(auto& allpoints:map_->map_points_){
            MapPoint::Ptr& p = allpoints.second;                    // Map as unordered_map
            if(curr_->isInFrame(p->pos_)){
                p->visible_times_++;
                candidate.push_back(p);
                descriptor_map.push_back(p->descriptor_);
            }
        }

        matcher_flann_.match(descriptor_map, descriptors_curr_, matches);

        float min_dis = std::min_element(matches.begin(), matches.end(),
                          [](const cv::DMatch& m1, const cv::DMatch& m2){
                              return m1.distance < m2.distance;
                          })->distance;

        match_3dpts_.clear();
        match_2dkp_index_.clear();
        // filter out long distance matches
        for (cv::DMatch& m:matches){
            if(m.distance < 2*min_dis){
                match_3dpts_.push_back(candidate[m.queryIdx]);
                match_2dkp_index_.push_back(m.trainIdx);
            }
        }
        cout << "good matches number: " << match_3dpts_.size() << endl;
    }

    void VisualOdometry::poseEstimationPnP(){
        vector<cv::Point3f> pts3d;
        vector<cv::Point2f> pts2d;

        for ( int index:match_2dkp_index_ )
        {
            pts2d.push_back ( keypoints_curr_[index].pt );
        }
        for ( MapPoint::Ptr pt:match_3dpts_ )
        {
            pts3d.push_back( pt->getPositionCV() );
        }

        Mat K = (cv::Mat_<double>(3, 3) <<
            ref_->camera_->fx_, 0, ref_->camera_->cx_,
            0, ref_->camera_->fy_, ref_->camera_->cy_,
            0, 0, 1);

        Mat rvec, tvec, inliers;
        cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
        num_inliers_ = inliers.rows;
        cout<<"pnp inliers: "<<num_inliers_<<endl;

        T_c_r_estimated_ = SE3(
            SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
            Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0))
        );
    }

    void VisualOdometry::poseEstimationPnPwithBA(){

        // construct the 3d 2d observations
        vector<cv::Point3f> pts3d;
        vector<cv::Point2f> pts2d;

        for ( int index:match_2dkp_index_ )
        {
            pts2d.push_back ( keypoints_curr_[index].pt );
        }
        for ( MapPoint::Ptr pt:match_3dpts_ )
        {
            pts3d.push_back( pt->getPositionCV() );
        }

        Mat K = ( cv::Mat_<double>(3,3)<<
            ref_->camera_->fx_, 0, ref_->camera_->cx_,
            0, ref_->camera_->fy_, ref_->camera_->cy_,
            0,0,1
        );
        Mat rvec, tvec, inliers;
        cv::solvePnPRansac( pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers );
        num_inliers_ = inliers.rows;
        cout<<"pnp inliers: "<<num_inliers_<<endl;
        T_c_r_estimated_ = SE3(
            SO3(rvec.at<double>(0,0), rvec.at<double>(1,0), rvec.at<double>(2,0)),
            Vector3d( tvec.at<double>(0,0), tvec.at<double>(1,0), tvec.at<double>(2,0))
        );

        typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 2>> Block;
        Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>();
        Block* solver_ptr = new Block(linearSolver);
        g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        g2o::SparseOptimizer optimizer;
        optimizer.setAlgorithm(solver);

        // add vertices
        g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();
        pose->setId(0);
        pose->setEstimate(g2o::SE3Quat(
            T_c_r_estimated_.rotation_matrix(), T_c_r_estimated_.translation()
        ));
        optimizer.addVertex(pose);

        // Add edge
        for(int i = 0;i<inliers.rows; i++){
            int index = inliers.at<int>(i, 0);

            EdgeProjectXYZ2UVPoseOnly* edge = new EdgeProjectXYZ2UVPoseOnly();
            edge->setId(i);
            edge->setVertex(0, pose);
            edge->camera_ = curr_->camera_.get();
            edge->point_ = Vector3d(pts3d[index].x, pts3d[index].y, pts3d[index].z);
            edge->setMeasurement(Vector2d(pts2d[index].x, pts2d[index].y));
            edge->setInformation(Eigen::Matrix2d::Identity());
            optimizer.addEdge(edge);
        }

        optimizer.initializeOptimization();
        optimizer.optimize(10);

        T_c_r_estimated_ = SE3(pose->estimate().rotation(), pose->estimate().translation);
    }

    bool VisualOdometry::checkEstimatedPose(){
        if(num_inliers_ < min_inliers_){
            cout<<"reject because inlier is too small: "<<num_inliers_<<endl;
            return false;
        }
        // if the motion is too large, it is probably wrong
        Sophus::Vector6d d = T_c_r_estimated_.log();
        if ( d.norm() > 5.0 )
        {
            cout<<"reject because motion is too large: "<<d.norm()<<endl;
            return false;
        }
        return true;
    }

    bool VisualOdometry::checkKeyFrame(){
        // simple key frame classification. norm > threshold as Key Frame
        Sophus::Vector6d d = T_c_r_estimated_.log();
        Vector3d trans = d.head<3>();
        Vector2d rot = d.tail<3>();
        if(trans.norm() > key_frame_min_trans_ || rot.norm() > key_frame_min_rot_){
            return true;
        }
        return false;
    }

    void VisualOdometry::addKeyFrame(){
        if(map_->keyframes_.empty()){
            for(size_t i = 0;i<keypoints_curr_.size();i++){
                double d = curr_->findDepth ( keypoints_curr_[i] );
                if ( d < 0 )
                    continue;
                Vector3d p_world = ref_->camera_->pixel2world (
                    Vector2d ( keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y ), curr_->T_w_c_, d
                );
                Vector3d n = p_world - ref_->getCamCenter();
                n.normalize();
                MapPoint::Ptr map_point = MapPoint::createMapPoint(
                    p_world, n, descriptors_curr_.row(i).clone(), curr_.get()
                );
                map_->insertMapPoint( map_point );
            }
        }
        map_->insertKeyFrame(curr_);
        ref_ = curr_;
    }

    void VisualOdometry::optimizeMap(){
        for(auto iter = map_->map_points_.begin(); iter!=map_->map_points_.end();){
            if(! curr_->isInFrame(iter->second->pos_)){
                iter = map_->map_points_.erase(iter);
                continue;
            }

            float match_ratio = float(iter->second->matched_times_) / iter->second->visible_times_;
            if(match_ratio < map_point_erase_ratio_){
                iter = map_->map_points_.erase(iter);
                continue;
            }

            double angle = getViewAngle( curr_, iter->second);
            if(angle > M_PI/6.){
                iter = map_->map_points_.erase(iter);
            }

            if(iter->second->good_ = false){
                // Triangle

            }
            iter++;

        }

        if(match_2dkp_index_.size() < 100){
            addMapPoints();
        }
        if ( map_->map_points_.size() > 1000 )
        {
            // TODO map is too large, remove some one
            map_point_erase_ratio_ += 0.05;
        }
        else
            map_point_erase_ratio_ = 0.1;
        cout<<"map points: "<<map_->map_points_.size()<<endl;
    }

    double VisualOdometry::getViewAngle(Frame::Ptr frame, MapPoint::Ptr point){
        Vector3d n = point->pos_ - frame->getCamCenter();
        n.normalize();
        return acos(n.transpose() * point->norm_);  // Inner product of two norm vector to calculate the angle between
                                                    // camera axis and mappoint
    }
}


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

namespace boost { namespace simd {
namespace tag
{
    /*!
      @brief  abss generic tag

      Represents the abss function in generic contexts.

      @par Models:
      Hierarchy
    **/
    struct abss_ : ext::elementwise_<abss_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<abss_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_abss_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };
  }
namespace ext
{
    template<class Site>
    BOOST_FORCEINLINE generic_dispatcher<tag::abss_, Site> dispatching_abss_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
    {
      return generic_dispatcher<tag::abss_, Site>();
    }
    template<class... Args>
    struct impl_abss_;
}
  /*!
    Computes the saturated absolute value of its parameter.

    @par semantic:
    For any given value @c x of type @c T:

    @code
    T r = abss(x);
    @endcode

    is equivalent to:

    @code
    T r = (x == Valmin) ? Valmax : (x < T(0) ? -x : x);
    @endcode

    @par Note:

    The function always returns a positive value of the same type as the
    entry.

    This is generally equivalent to @c abs functor except for signed integer
    types for which \c abss(Valmin) is \c Valmax.

    @par Alias

    saturated_abs

    @see  @funcref{abs}, @funcref{sqr_abs}, @funcref{sqrs}
    @param  a0    value whose absolute value will be returned.

    @return         a value of the same type as the input.

  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::abss_, abss, 1)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::abss_, saturated_abs, 1)
} }

#endif


//
// high_res_clock.hpp
// ~~~~~~~~~~~~~~~~~~
//
// Copyright (c) 2003-2011 Christopher M. Kohlhoff (chris at kohlhoff dot com)
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#ifndef HIGH_RES_CLOCK_HPP
#define HIGH_RES_CLOCK_HPP

#include <boost/config.hpp>
#include <boost/cstdint.hpp>

#if defined(BOOST_WINDOWS)

inline boost::uint64_t high_res_clock()
{
  LARGE_INTEGER i;
  QueryPerformanceCounter(&i);
  return i.QuadPart;
}

#elif defined(__GNUC__) && defined(__x86_64__)

inline boost::uint64_t high_res_clock()
{
  unsigned long low, high;
  __asm__ __volatile__("rdtsc" : "=a" (low), "=d" (high));
  return (((boost::uint64_t)high) << 32) | low;
}

#else

#include <boost/date_time/posix_time/posix_time_types.hpp>

inline boost::uint64_t high_res_clock()
{
  boost::posix_time::ptime now =
    boost::posix_time::microsec_clock::universal_time();

  boost::posix_time::ptime epoch(
      boost::gregorian::date(1970, 1, 1),
      boost::posix_time::seconds(0));

  return (now - epoch).total_microseconds();
}

#endif

#endif // HIGH_RES_CLOCK_HPP


#include <boost/test/unit_test.hpp>

#include <koinos/tests/pack_fixture.hpp>

#include <koinos/pack/rt/binary.hpp>

#define REQUIRE_DEEP_EQUAL( s, v )                         \
do{                                                        \
   BOOST_REQUIRE_EQUAL( s.tellp() - s.tellg(), v.size() ); \
   std::stringstream copy( s.str() );                      \
   for( size_t i = 0; i < v.size(); i++ )                  \
   {                                                       \
      uint8_t c;                                           \
      copy.get( (char&)c );                                \
      BOOST_CHECK_EQUAL( v[i], c );                        \
   }                                                       \
} while(0);

BOOST_FIXTURE_TEST_SUITE( binary_pack_tests, pack_fixture )

using namespace koinos::pack;

BOOST_AUTO_TEST_CASE( basic_test )
{
   std::stringstream ss;
   to_binary( ss, int64_t(-256) );

   vector< uint8_t > expected = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x00 };
   REQUIRE_DEEP_EQUAL( ss, expected );

   int64_t res = 0;
   from_binary( ss, res );
   BOOST_REQUIRE_EQUAL( res, -256 );
}

BOOST_AUTO_TEST_CASE( uint128_test )
{
   uint128_t to_bin = 1;
   to_bin <<= 65;
   to_bin -= 1; // 2^65 - 1

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
   };
   REQUIRE_DEEP_EQUAL( ss, expected );

   uint128_t from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin, to_bin );
}

BOOST_AUTO_TEST_CASE( uint160_test )
{
   uint160_t to_bin = 1;
   to_bin <<= 129;
   to_bin -= 1; // 2^129 - 1

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00, 0x00, 0x00, 0x01,
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
   };
   REQUIRE_DEEP_EQUAL( ss, expected );

   uint160_t from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin, to_bin );
}

BOOST_AUTO_TEST_CASE( uint256_test )
{
   uint256_t to_bin = 1;
   to_bin <<= 129;
   to_bin -= 1; // 2^129 - 1

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
      0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
      0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
   };
   REQUIRE_DEEP_EQUAL( ss, expected );

   uint256_t from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin, to_bin );
}

BOOST_AUTO_TEST_CASE( unsigned_varint_test )
{
   std::stringstream ss;
   unsigned_int to_bin = 256;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0x80, 0x02 };
   REQUIRE_DEEP_EQUAL( ss, expected );

   unsigned_int from_bin = 0;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin.value, 256 );
}

BOOST_AUTO_TEST_CASE( signed_varint_test )
{
   std::stringstream ss;
   signed_int to_bin = -254;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0xFB, 0x03 };
   REQUIRE_DEEP_EQUAL( ss, expected );

   signed_int from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin.value, -254 );

   ss = std::stringstream();
   to_bin = 256;
   to_binary( ss, to_bin );

   expected = { 0x80, 0x04 };
   REQUIRE_DEEP_EQUAL( ss, expected );

   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( from_bin.value, 256 );
}

BOOST_AUTO_TEST_CASE( signed_varint_mapping )
{
   std::stringstream ss;
   signed_int to_bin_pos = 0;
   signed_int to_bin_neg = 0;

   to_binary( ss, to_bin_pos );

   unsigned_int from_bin;
   from_binary( ss, from_bin );

   BOOST_REQUIRE_EQUAL( from_bin.value, 0 );

   for( int64_t i = 1; i <= 256; ++i )
   {
      to_bin_pos.value = i;
      to_bin_neg.value = -i;

      to_binary( ss, to_bin_pos );
      to_binary( ss, to_bin_neg );

      from_binary( ss, from_bin );
      BOOST_REQUIRE_EQUAL( from_bin.value, 2 * i );

      from_binary( ss, from_bin );
      BOOST_REQUIRE_EQUAL( from_bin.value, (2 * i) - 1 );
   }
}

BOOST_AUTO_TEST_CASE( vector_test )
{
   std::stringstream ss;
   vector< int16_t > to_bin = { 4, 8, 15, 16, 23, 42 };

   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x06,       // size
      0x00, 0x04, // 4
      0x00, 0x08, // 8
      0x00, 0x0F, // 15
      0x00, 0x10, // 16
      0x00, 0x17, // 23
      0x00, 0x2A  // 42
   };

   REQUIRE_DEEP_EQUAL( ss, expected );

   vector< int16_t > from_bin;
   from_binary( ss, from_bin );

   BOOST_REQUIRE_EQUAL( to_bin.size(), from_bin.size() );
   for( size_t i = 0; i < to_bin.size(); ++i )
   {
      BOOST_REQUIRE_EQUAL( to_bin[i], from_bin[i] );
   }

   unsigned_int size = KOINOS_PACK_MAX_ARRAY_ALLOC_SIZE / sizeof( int16_t ) + 1;
   to_binary( ss, size );
   for( size_t i = 0; i < size.value; ++i )
   {
      // 10MB of 0s
      to_binary( ss, int64_t(0) );
   }

   try
   {
      from_binary( ss, from_bin );
      BOOST_FAIL( "allocation_violation not thrown" );
   }
   catch( allocation_violation& ) {}
}

BOOST_AUTO_TEST_CASE( array_test )
{
   std::stringstream ss;
   std::array< int16_t, 6 > to_bin = { 4, 8, 15, 16, 23, 42 };

   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00, 0x04, // 4
      0x00, 0x08, // 8
      0x00, 0x0F, // 15
      0x00, 0x10, // 16
      0x00, 0x17, // 23
      0x00, 0x2A  // 42
   };

   REQUIRE_DEEP_EQUAL( ss, expected );

   std::array< int16_t, 6 > from_bin;
   from_binary( ss, from_bin );

   for( size_t i = 0; i < to_bin.size(); ++i )
   {
      BOOST_REQUIRE_EQUAL( to_bin[i], from_bin[i] );
   }

   // Array alloction failure is checked statically, so it does not require a runtime test

   ss = std::stringstream();
   to_binary( ss, unsigned_int( 100 ) ); // Purposeful buffer overflow
   std::array< int16_t, 100 > large_array;
   BOOST_REQUIRE_THROW( from_binary( ss, large_array ), stream_error );
}

BOOST_AUTO_TEST_CASE( variant_test )
{
   typedef std::variant< int16_t, int32_t > test_variant;
   test_variant to_bin = int16_t( 10 );

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00,       // index
      0x00, 0x0A  // value
   };

   REQUIRE_DEEP_EQUAL( ss, expected );

   test_variant from_bin;
   from_binary( ss, from_bin );

   std::visit( koinos::overloaded {
      []( int16_t v ){ BOOST_REQUIRE_EQUAL( v, 10 ); },
      []( int32_t v ){ BOOST_FAIL( "variant contains unexpected type" ); },
      []( auto& v ){ BOOST_FAIL( "variant contains unexpected type" ); }
   }, from_bin );

   ss = std::stringstream();
   to_bin = int32_t( 20 );
   to_binary( ss, to_bin );

   expected = {
      0x01,                   // index
      0x00, 0x00, 0x00, 0x14  // value
   };

   REQUIRE_DEEP_EQUAL( ss, expected );

   from_binary( ss, from_bin );

   std::visit( koinos::overloaded {
      []( int16_t v ){ BOOST_FAIL( "variant contains unexpected type" ); },
      []( int32_t v ){ BOOST_REQUIRE_EQUAL( v, 20 ); },
      []( auto& v ){ BOOST_FAIL( "variant contains unexpected type" ); }
   }, from_bin );

   ss = std::stringstream();
   unsigned_int tag = 2; //invalid variant tag
   to_binary( ss, tag );
   to_binary( ss, int16_t( 10 ) );

   try
   {
      from_binary( ss, from_bin );
      BOOST_FAIL( "Unexpected tag did not throw exception" );
   }
   catch( parse_error& ) {}
}

BOOST_AUTO_TEST_CASE( optional_test )
{
   typedef std::optional< int16_t > test_optional;
   test_optional to_bin;

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0x00 };
   REQUIRE_DEEP_EQUAL( ss, expected );

   test_optional from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE( !from_bin.has_value() );

   ss = std::stringstream();
   to_bin = 10;
   to_binary( ss, to_bin );

   expected = { 0x01, 0x00, 0x0A };
   REQUIRE_DEEP_EQUAL( ss, expected );

   from_binary( ss, from_bin );
   BOOST_REQUIRE( from_bin.has_value() );
   BOOST_REQUIRE_EQUAL( *from_bin, *to_bin );
}

BOOST_AUTO_TEST_CASE( variable_blob_test )
{
   variable_blob to_bin;
   to_bin = { 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0x06, 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };
   REQUIRE_DEEP_EQUAL( ss, expected );

   variable_blob from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( to_bin.size(), from_bin.size() );
   for( size_t i = 0; i < to_bin.size(); ++i )
   {
      BOOST_REQUIRE_EQUAL( to_bin[i], from_bin[i] );
   }
}

BOOST_AUTO_TEST_CASE( fixed_blob_test )
{
   fixed_blob< 6 > to_bin;
   to_bin = { 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };
   REQUIRE_DEEP_EQUAL( ss, expected );

   fixed_blob< 6 > from_bin;
   from_binary( ss, from_bin );
   for( size_t i = 0; i < to_bin.size(); ++i )
   {
      BOOST_REQUIRE_EQUAL( to_bin[i], from_bin[i] );
   }
}

BOOST_AUTO_TEST_CASE( multihash_test )
{
   multihash to_bin;
   to_bin.id = 1;
   to_bin.digest = { 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = { 0x01, 0x06, 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A };
   REQUIRE_DEEP_EQUAL( ss, expected );

   multihash from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE_EQUAL( to_bin.digest.size(), from_bin.digest.size() );
   for( size_t i = 0; i < to_bin.digest.size(); ++i )
   {
      BOOST_REQUIRE_EQUAL( to_bin.digest[i], from_bin.digest[i] );
   }
}

BOOST_AUTO_TEST_CASE( reflect_test )
{
   test_object to_bin;
   to_bin.id = { 0, 4, 8, 15, 16, 23, 42, 0 };
   to_bin.key.id = 1;
   to_bin.key.digest = { 'f', 'o', 'o', 'b', 'a', 'r' };
   to_bin.vals = { 108 };

   std::stringstream ss;
   to_binary( ss, to_bin );

   vector< uint8_t > expected = {
      0x00, 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A, 0x00, // id
      0x01, 0x06, 0x66, 0x6F, 0x6F, 0x62, 0x61, 0x72, // key
      0x01, 0x00, 0x00, 0x00, 0x6C                    // vals
   };
   REQUIRE_DEEP_EQUAL( ss, expected );

   test_object from_bin;
   from_binary( ss, from_bin );
   BOOST_REQUIRE( memcmp( from_bin.id.data(), to_bin.id.data(), to_bin.id.size() ) == 0 );
   BOOST_REQUIRE_EQUAL( from_bin.key.digest.size(), to_bin.key.digest.size() );
   BOOST_REQUIRE( memcmp( from_bin.key.digest.data(), to_bin.key.digest.data(), to_bin.key.digest.size() ) == 0 );
   BOOST_REQUIRE_EQUAL( from_bin.vals.size(), to_bin.vals.size() );
   BOOST_REQUIRE( memcmp( (char*)from_bin.vals.data(), (char*)to_bin.vals.data(), sizeof(uint32_t) * to_bin.vals.size() ) == 0 );
}

/*
( depth_test )
{
   // The compiler is not happy about the template deduction I am about to make it do,
   // but it will complete it...
   vector< vector< vector< vector< vector< vector< vector< vector< vector< vector< vector<
      vector< vector< vector< vector< vector< vector< vector< vector< vector< vector< uint8_t >
   > > > > > > > > > > > > > > > > > > > > from_bin;

   std::stringstream ss;
   for( size_t i = 0; i < 20; ++i )
      ss << uint8_t( 1 );

   ss << uint8_t( 0 );

   try
   {
      from_binary( ss, from_bin );
      BOOST_FAIL( "depth_violation not raised" );
   }
   catch( depth_violation& ) {}
}
*/

BOOST_AUTO_TEST_CASE( to_variable_blob_test )
{
   BOOST_TEST_MESSAGE( "Testing string to vl_blob" );
   std::string foobar = "foobar";
   variable_blob expected;
   expected = {0x06, 0x66, 0x6F, 0x6F, 0x62, 0x61, 0x72};
   variable_blob result = to_variable_blob( foobar );

   BOOST_REQUIRE( result.size() == expected.size() );
   BOOST_REQUIRE( memcmp( result.data(), expected.data(), expected.size() ) == 0 );

   std::string result_str = from_variable_blob< std::string >( result );
   BOOST_REQUIRE_EQUAL( foobar, result_str );

   BOOST_TEST_MESSAGE( "Testing object to vl_blob" );

   // Using the same data from reflect_test
   test_object obj;
   obj.id = { 0, 4, 8, 15, 16, 23, 42, 0 };
   obj.key.id = 1;
   obj.key.digest = { 'f', 'o', 'o', 'b', 'a', 'r' };
   obj.vals = { 108 };

   expected = {
      0x00, 0x04, 0x08, 0x0F, 0x10, 0x17, 0x2A, 0x00,
      0x01, 0x06, 0x66, 0x6F, 0x6F, 0x62, 0x61, 0x72,
      0x01, 0x00, 0x00, 0x00, 0x6C
   };
   to_variable_blob( result, obj );

   BOOST_REQUIRE_EQUAL( result.size(), expected.size() );
   BOOST_REQUIRE( memcmp( result.data(), expected.data(), expected.size() ) == 0 );

   test_object result_obj = from_variable_blob< test_object >( result );
   BOOST_REQUIRE( memcmp( result_obj.id.data(), obj.id.data(), obj.id.size() ) == 0 );
   BOOST_REQUIRE_EQUAL( result_obj.key.digest.size(), obj.key.digest.size() );
   BOOST_REQUIRE( memcmp( result_obj.key.digest.data(), obj.key.digest.data(), obj.key.digest.size() ) == 0 );
   BOOST_REQUIRE_EQUAL( result_obj.vals.size(), obj.vals.size() );
   BOOST_REQUIRE( memcmp( (char*)result_obj.vals.data(), (char*)obj.vals.data(), sizeof(uint32_t) * obj.vals.size() ) == 0 );
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef SOT_CORE_BINARYOP_HH
#define SOT_CORE_BINARYOP_HH

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>
#include <sot/core/flags.hh>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/pool.hh>

/* STD */
#include <string>

#include <boost/function.hpp>

namespace dynamicgraph {
namespace sot {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

template <typename Operator> class BinaryOp : public Entity {
  Operator op;
  typedef typename Operator::Tin1 Tin1;
  typedef typename Operator::Tin2 Tin2;
  typedef typename Operator::Tout Tout;
  typedef BinaryOp<Operator> Self;

public: /* --- CONSTRUCTION --- */
  static std::string getTypeIn1Name(void) { return Operator::nameTypeIn1(); }
  static std::string getTypeIn2Name(void) { return Operator::nameTypeIn2(); }
  static std::string getTypeOutName(void) { return Operator::nameTypeOut(); }
  static const std::string CLASS_NAME;
  virtual const std::string &getClassName() const { return CLASS_NAME; }
  std::string getDocString() const { return op.getDocString(); }

  BinaryOp(const std::string &name)
      : Entity(name),
        SIN1(NULL, BinaryOp::CLASS_NAME + "(" + name + ")::input(" +
                       getTypeIn1Name() + ")::sin1"),
        SIN2(NULL, CLASS_NAME + "(" + name + ")::input(" + getTypeIn2Name() +
                       ")::sin2"),
        SOUT(boost::bind(&Self::computeOperation, this, _1, _2), SIN1 << SIN2,
             CLASS_NAME + "(" + name + ")::output(" + getTypeOutName() +
                 ")::sout") {
    signalRegistration(SIN1 << SIN2 << SOUT);
    op.addSpecificCommands(*this, commandMap);
  }

  virtual ~BinaryOp(void){};

public: /* --- SIGNAL --- */
  SignalPtr<Tin1, int> SIN1;
  SignalPtr<Tin2, int> SIN2;
  SignalTimeDependent<Tout, int> SOUT;

protected:
  Tout &computeOperation(Tout &res, int time) {
    const Tin1 &x1 = SIN1(time);
    const Tin2 &x2 = SIN2(time);
    op(x1, x2, res);
    return res;
  }
};
} // namespace sot
} // namespace dynamicgraph

#endif // #ifndef SOT_CORE_BINARYOP_HH


// -*- C++ -*-
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//
//                                   Jiao Lin
//                      California Institute of Technology
//                         (C) 2005 All Rights Reserved
//
// {LicenseText}
//
// ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
//

#include "mccomposite/geometry/primitives.h"

#include <boost/python.hpp>

namespace wrap_mccomposite {

  void wrap_primitives()
  {
    using namespace boost::python;
    using namespace mccomposite::geometry;

    class_<Box, bases<AbstractShape> >
      ("Block",
       init< double, double, double>() )
      ;

    class_<Sphere, bases<AbstractShape> >
      ("Sphere",
       init< double >() )
      ;

    class_<Cylinder, bases<AbstractShape> >
      ("Cylinder",
       init< double, double >() )
      ;

    class_<Pyramid, bases<AbstractShape> >
      ("Pyramid",
       init< double, double, double >() )
      ;

    class_<Cone, bases<AbstractShape> >
      ("Cone",
       init< double, double >() )
      ;

  }
}

// version
// $Id$

// End of file


/*
 * Copyright (C) 2005-2017 Centre National d'Etudes Spatiales (CNES)
 *
 * This file is part of Orfeo Toolbox
 *
 *     https://www.orfeo-toolbox.org/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "otbSoilDataBase.h"
#include <fstream>
#include <sstream>
#include <algorithm>
#include <boost/algorithm/string.hpp>
#include "itkMacro.h"

namespace otb
{

SoilDataBase::SoilDataBase(const std::string& SoilFileName, double wlfactor) :
  m_SoilFileName(SoilFileName), m_WlFactor(wlfactor)
{
  ParseSoilFile();
};

const typename SoilDataBase::SoilDataVector& SoilDataBase::GetDB() const
{
  return m_SoilDataVector;
}

double SoilDataBase::GetReflectance(size_t SoilIndex, WavelenghtType wl) const
{
  if(SoilIndex >= m_SoilDataVector.size())
    {
    std::stringstream  errmessg;
    errmessg << "Soil index must be >= 0 and < " << m_SoilDataVector.size() <<
      ". " << SoilIndex <<" was passed.\n";
    throw std::range_error( errmessg.str() );
    }
  // wl not in the set of measured ones
  if(m_SoilDataVector[SoilIndex].find(wl)==m_SoilDataVector[SoilIndex].end())
    {
    const auto wlmin = m_Wavelengths[0];
    const auto wlmax = m_Wavelengths[m_Wavelengths.size()-1];
    if(wl<wlmin) return (*m_SoilDataVector[SoilIndex].find(wlmin)).second;
    if(wl>wlmax) return (*m_SoilDataVector[SoilIndex].find(wlmax)).second;

    const auto p = std::partition_point(m_Wavelengths.cbegin(), m_Wavelengths.cend(),
                                        [&](WavelenghtType w){ return w<wl;}
      );
    const auto wlinf = *(p-1);
    const auto wlsup = *p;
    const auto factinf = wl-wlinf;
    const auto factsup = wlsup-wl;
    const auto vinf = (*m_SoilDataVector[SoilIndex].find(wlinf)).second;
    const auto vsup = (*m_SoilDataVector[SoilIndex].find(wlsup)).second;
    return (vinf*factinf+vsup*factsup)/(factinf+factsup);
    }
  else
    {
    return (*m_SoilDataVector[SoilIndex].find(wl)).second;
    }
}

size_t SoilDataBase::CountColumns(std::string fileName) const
{
  std::ifstream ifile(fileName);
  std::string line;
  if (ifile.is_open())
       {
       size_t nbSpaces = 0;
       if(!getline(ifile,line))
         {
         itkGenericExceptionMacro(<< "Error reading file " << fileName);
         }
       ifile.close();
       boost::trim(line);
       auto found = line.find(' ');
       while(found!=std::string::npos)
         {
         ++nbSpaces;
         while(line[found+1] == ' ') ++found;
         found = line.find(' ', found+1);
         }
       return nbSpaces+1;
       }
  else
    {
    itkGenericExceptionMacro(<< "Could not open file " << fileName);
    }
}
void SoilDataBase::ParseSoilFile()
{
  auto number_of_soils = CountColumns(m_SoilFileName) - 1;
  m_SoilDataVector.resize(number_of_soils);
  std::ifstream sdb(m_SoilFileName);
  if(! sdb.is_open())
    {
    itkGenericExceptionMacro(<< "Could not open file " << m_SoilFileName);
    }
  std::string line;
  while(std::getline(sdb, line))
    {
    if(line.size() > 3)
      {
      std::stringstream ss(line);
      double tmpwl;
      ss >> tmpwl;
      WavelenghtType wl = static_cast<WavelenghtType>(m_WlFactor*tmpwl);
      m_Wavelengths.push_back(wl);
      for(size_t i=0; i< number_of_soils; ++i)
        {
        double refl;
        ss >> refl;
        m_SoilDataVector[i][wl] = refl;
        }
      }
    }
  std::sort(m_Wavelengths.begin(), m_Wavelengths.end());
}
}// namespace otb


/*                   VisSim www.Uni-Koblenz.de
 *
 * This example is simulation of a simple pendulum with drag force
 * BodyDragForce is built as a plugin
 *
 * By Ibraheem AL-Dhamari ibr.exg@gmail.com
 *
 * TODOs:
 * 1. create the model .osim file using cpp
 * 2. run the model using forwardtool
 *
*/
#include <OpenSim/OpenSim.h>
#include "OpenSim/Common/STOFileAdapter.h"
#include <time.h>
#include <BodyDragForce.h>

//#include <boost/algorithm/string.hpp>
#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include "SimTKcommon/internal/Xml.h"

#define PI 3.14159265

using namespace OpenSim;
using namespace std;

using SimTK::Vec3;
using SimTK::Inertia;

SimTK::Xml::Element GlobalForceSetEl; //ForceSet
void findNode(SimTK::Xml::Element rootNd, std::string nodName) {
        SimTK::Xml::node_iterator pnI;
        SimTK::Xml::Element cEl;
        pnI = rootNd.element_begin();
        int j = 0;
        while (  pnI != rootNd.element_end() ) {
              cEl = pnI->getParentElement().getAllElements()[j];
              if (nodName == cEl.getNodeText()) {
                  GlobalForceSetEl = cEl;
                  //cout << cEl.getNodeText() << "  " << cEl.getRequiredAttribute("name").getValue() << endl;
                  break;
              }else
              findNode(cEl, nodName);
              pnI++; j++;
        }//while
}
void addForcePlugin(SimTK::Xml::Element forceNode, std::string modelPath) {
    try {
        SimTK::Xml::Document modelDoc(modelPath);
        SimTK::Xml::Element rootNd = modelDoc.getRootElement();
        cout << " model file is oppened: " <<modelPath<< endl;
        try {
            findNode(rootNd, "ForceSet");
            SimTK::Xml::Element ObjectsElem = GlobalForceSetEl.getAllElements()[0];
            //cout << ObjectsElem.getNodeText() << endl;
            ObjectsElem.appendNode(forceNode);
        }
        catch (Exception ee) {
            cout << " ForceSet Not found ......." << endl;
            SimTK::Xml::Element ForceSetElem("ForceSet");
            ForceSetElem.setAttributeValue("name", "ForceSet");
            SimTK::Xml::Element ObjectsElem("Objects");
            ForceSetElem.appendNode(ObjectsElem);
            ObjectsElem.appendNode(forceNode);
        }
        modelDoc.writeToFile(modelPath);
        cout << " Plug-in force is added ......." << endl;
    }
    catch(Exception e){
        cout << " ERROR: plug-in force is not added ......"<< endl;
    }

}

int main(int argc, char* argv[]) {
        std::cout<<"=============================================="<<std::endl;
        std::cout<<"=         Simple Pendulum in OpenSim         ="<<std::endl;
        std::cout<<"=============================================="<<std::endl;

        // Pendulum,Force and Simulation Parameters
        // This helps to experiements with different parameters

                double enablePlugin = 1.0    ; // 1 = pendulum has drag force, 0 = no drag force
                double enableSim    = 1.0    ; // 1 = run the simulation , 0 = only build the model file
                double enableVis    = 1.0    ; // 1 = show visualizer , 0 = no visualisation
                if (not enableSim) enableVis=0;

                //assuming run inside the build folder
                std::string bodyDragForceLibPath="BodyDragForce/libBodyDragForce";

                double bodyDragForceCoefficient = 0.5;
                double bodyDragForceExponent    = 3.0;

                double gravity      = -9.81  ;
                double stepSize     = 0.001  ;
                double initialTime  = 0.0;
                double finalTime    = 5 ;
                double th1Deg       =90 ;               // degrees

                double      rodRd       = 0.1;
                double      rodHeight   = 1;
                double      rodMass     = 1;
                Vec3        rodCoM      = Vec3(0) ;
                Inertia     rodInertia  = Inertia(1);
                SimTK::Vec3 rodColor(1.0, 1.0, 0.0);
                double      sphRd        = 0.3;
                double      sphMass      = 1;
                Vec3        sphCoM       = Vec3(0) ;
                Inertia sphInertia       = Inertia(1);
                SimTK::Vec3 sphColor(1.0, 0.0, 0.0);
                Vec3        sphOrient   = Vec3(0);
        if  (argc >2){
            std::cout<<" using user arguments: "<<std::endl;
            for (int i=0; i<argc ;i++){
                std::cout<<argv[i]<<" " ;
            }
            std::cout<<" "<<std::endl ;

            finalTime    = strtod(argv[1], NULL);
            enableVis    = strtod(argv[2], NULL);
        }else{
          std::cout<<" using default arguments: "<<std::endl;
        }

     double   th1Rad         = th1Deg *((22.0/7.0)/180.0);   // radians
     Vec3     rodPos        = Vec3(0 ,  rodHeight   ,0) ; //it will be under the ground
     Vec3     rodOrient     = Vec3(0,0,th1Rad);

     double   sphX,sphY;
     Vec3     sphPos  ;

     sphX         =   rodHeight * sin( th1Rad);
     sphY         =   rodHeight * cos( th1Rad);
     sphPos        = Vec3(sphX, sphY , 0) ;

     try {

        Model pendulumDF;  // DF =Drag Force
        pendulumDF.setName("pendulumDF");
        pendulumDF.setGravity(SimTK::Vec3(0, gravity, 0));
        Ground& ground = pendulumDF.updGround();

         //                         body name, mass ,massCenter, inertia
        Body* rodBdy = new    Body("rod", rodMass,         rodCoM,     rodInertia );
        Cylinder rodGeom (rodRd , rodHeight);
        rodGeom.setColor(rodColor);    //set the color of the first block so they are different
        SimTK::Transform rodTrsfrm(SimTK::Vec3(0,0,0));
        auto* rodCenter = new PhysicalOffsetFrame("rodCenter", *rodBdy, rodTrsfrm);
        rodBdy->addComponent(rodCenter);     rodCenter->attachGeometry(rodGeom.clone());
        rodBdy->setName();
        pendulumDF.addBody(rodBdy);

        // Connect the bodies with pin joints. Assume each body is 1 m long.
        auto* groundTrodJnt = new PinJoint("groundTrodJnt", pendulumDF.getGround(), Vec3(0),  Vec3(0),  *rodBdy, rodPos, rodOrient);
        pendulumDF.addJoint(groundTrodJnt);

        Body* sphBdy = new    Body("sph", sphMass,     sphCoM,     sphInertia );
        Sphere sphGeom (sphRd);
        sphGeom.setColor(sphColor); //set the color of the first block so they are different
        SimTK::Transform sphTrsfrm(SimTK::Vec3( 0, 0, 0));
        auto* sphCenter = new PhysicalOffsetFrame("sphCenter", *sphBdy, sphTrsfrm);
        sphBdy->addComponent(sphCenter);        sphCenter->attachGeometry(sphGeom.clone());
        pendulumDF.addBody(sphBdy);

        auto*  rodTsphJnt= new  PinJoint ("rodTsphJnt", *rodBdy,Vec3(0), Vec3(0),     *sphBdy, Vec3(0), Vec3(0));
        rodTsphJnt = new  PinJoint ("rodTsphJnt", *rodBdy, Vec3(sphX-rodHeight,sphY,0), rodOrient,     *sphBdy, sphPos, sphOrient);

        pendulumDF.addJoint(rodTsphJnt);

        rodBdy->setName("myRod");

        //rodTsphJnt->getCoordinate().setDefaultLocked(true);

        Coordinate &jntCord = rodTsphJnt->updCoordinate();
        jntCord.setDefaultLocked(true);

        SimTK::Xml::Element nElem("BodyDragForce");  	     nElem.setAttributeValue("name", "dragForce");
        if (enablePlugin) {
            SimTK::Xml::Element bdySph("body_name");          bdySph.setValue("sph");                                      nElem.appendNode(bdySph);
            SimTK::Xml::Element coefficientNd("coefficient"); coefficientNd.setValue(to_string(bodyDragForceCoefficient)); nElem.appendNode(coefficientNd);
            SimTK::Xml::Element exponentNd("exponent");	      exponentNd.setValue(to_string(bodyDragForceExponent));       nElem.appendNode(exponentNd);
        }
        pendulumDF.finalizeConnections();

        if (enableVis == 1.0)   pendulumDF.setUseVisualizer(true);

        // Initialize the system and get the default state
        SimTK::State& si = pendulumDF.initSystem();

        rodTsphJnt->getCoordinate().setLocked(si, true);

        pendulumDF.print(pendulumDF.getName()+".osim");

        // add the plugin force section
        if (enablePlugin) addForcePlugin(nElem, pendulumDF.getName() + ".osim");

        if (enableVis == 1.0)   pendulumDF.setUseVisualizer(true);
        //=============================================================
        //                  SIMULATION
        //=============================================================
        if (enableSim==1.0){
            // Set to true to visualize the simulation, which can be useful for

            //rodTsphJnt->getCoordinate().setLocked(si, true);  // the sphere is fixed to rod

            if (enableVis == 1.0){
                std::cout << "Vsualization is enabled .... " << std::endl;
                SimTK::Visualizer& viz = pendulumDF.updVisualizer().updSimbodyVisualizer();
                viz.setBackgroundType(viz.SolidColor);
                viz.setBackgroundType(SimTK::Visualizer::GroundAndSky);
                viz.setShowFrameNumber(true);
                viz.setGroundHeight(-2);

            }else{
                std::cout << "Vsualization is disabled .... " << std::endl;
            }

            // Create the force reporter for obtaining the forces applied to the model
            // during a forward simulation
            ForceReporter* fReporter    = new ForceReporter(&pendulumDF);
            StatesReporter* sReporter   = new StatesReporter(&pendulumDF);
            BodyKinematics* bKinematics = new BodyKinematics(&pendulumDF, true);
            Array<std::string> bodiesToRecord ; //= {"all"};

            bKinematics->setModel(pendulumDF);
            bKinematics->setBodiesToRecord(bodiesToRecord);

            pendulumDF.addAnalysis(fReporter);
            pendulumDF.addAnalysis(sReporter);
            pendulumDF.addAnalysis(bKinematics);

            Manager manager(pendulumDF);
            manager.setIntegratorMethod(Manager::IntegratorMethod::RungeKuttaFeldberg);
            manager.setIntegratorAccuracy(1.0e-12);
            manager.setIntegratorMaximumStepSize(stepSize);

            // Print out details of the model
            pendulumDF.printDetailedInfo(si, std::cout);

            // Integrate from initial time to final time
            si.setTime(initialTime);
            std::cout << "\nIntegrating from " << initialTime << " to " << finalTime << std::endl;
            manager.initialize(si);
            manager.integrate(finalTime);

            int fR = fReporter->printResults(  pendulumDF.getName(),"",stepSize);
            int sR = sReporter->printResults(  pendulumDF.getName(),"",stepSize);
            int kR = bKinematics->printResults(pendulumDF.getName(),"",stepSize);

            //TODO: how to close simbody-visualizer window?
            // for now use: pkill simbody in the terminal
        } //endif simulation
    } catch (OpenSim::Exception ex)	{
        std::cout << ex.getMessage() << std::endl;
        return 1;
    }
    // declaring argument of time()
    time_t my_time = time(NULL);

    // ctime() used to give the present date time
    printf(" All OpenSim tasks are completed on: %s", ctime(&my_time));
}


#ifndef BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H
#define BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H

#include <map>
#include <string>
#include "gazebo/common/Plugin.hh"
#include "gazebo/common/CommonTypes.hh"
#include "gazebo/physics/physics.hh"
#include "ros/ros.h"
#include "ros/subscribe_options.h"
#include "ros/callback_queue.h"
#include <boost/thread/mutex.hpp>
#include "std_msgs/Bool.h"
#include "brass_gazebo_battery/SetCharge.h"
#include "brass_gazebo_battery/SetCharging.h"
#include "brass_gazebo_battery/SetCoef.h"
#include "brass_gazebo_battery/SetChargingRate.h"

#define BATTERY_DEBUG
#define DBG_INTERVAL 5.0

namespace gazebo
{
    /// \brief A plugin that simulate BRASS CP1 battery model: discharge and charge according to power models
    class GAZEBO_VISIBLE BatteryPlugin : public ModelPlugin
    {
    /// \brief Constructor
    public: BatteryPlugin();

    public: ~BatteryPlugin();

    // Inherited.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);

    public: virtual void Init();

    public: virtual void Reset();

    private: double OnUpdateVoltage(const common::BatteryPtr &_battery);

    public: bool SetCharging(brass_gazebo_battery::SetCharging::Request& req,
                             brass_gazebo_battery::SetCharging::Response& res);

    public: bool SetCharge(brass_gazebo_battery::SetCharge::Request& req,
                             brass_gazebo_battery::SetCharge::Response& res);

    public: bool SetModelCoefficients(brass_gazebo_battery::SetCoef::Request& req,
                                      brass_gazebo_battery::SetCoef::Response& res);

    public: bool SetChargingRate(brass_gazebo_battery::SetChargingRate::Request& req,
                                 brass_gazebo_battery::SetChargingRate::Response& res);

    // Connection to the World Update events.
    protected: event::ConnectionPtr updateConnection;

    protected: physics::WorldPtr world;

    protected: physics::PhysicsEnginePtr physics;

    protected: physics::ModelPtr model;

    protected: physics::LinkPtr link;

    protected: common::BatteryPtr battery;

    protected: sdf::ElementPtr sdf;

    // E(t) = e0 + e1* Q(t)/c
    protected: double et;
    protected: double e0;
    protected: double e1;

    // Initial battery charge in Ah.
    protected: double q0;

    // Charge rate in A
    // More description about charge rate: http://batteriesbyfisher.com/determining-charge-time
    protected: double qt;

    // Battery capacity in Ah.
    protected: double c;

    // Battery inner resistance in Ohm
    protected: double r;

    // Current low-pass filter characteristic time in seconds.
    protected: double tau;

    // Raw battery current in A.
    protected: double iraw;

    // Smoothed battery current in A.
    protected: double ismooth;

    // Instantaneous battery charge in Ah.
    protected: double q;

    // This node is for ros communications
    protected: std::unique_ptr<ros::NodeHandle> rosNode;

    protected: ros::Publisher charge_state;
    protected: ros::Publisher charge_state_mwh;
    protected: ros::Publisher motor_power;

    protected: ros::ServiceServer set_charging;
    protected: ros::ServiceServer set_charge;
    protected: ros::ServiceServer set_coefficients;
    protected: ros::ServiceServer set_charging_rate;

    protected: boost::mutex lock;

    protected: bool charging;

    protected: double sim_time_now;

    };
}

#endif //BRASS_GAZEBO_BATTERY_BATTERY_DISCHARGE_H


#include "core/dataset/tensor_dataset.hpp"
#include <boost/program_options.hpp>
namespace po = boost::program_options;

int main(int argc, char **argv) {
    //
    logger_impl logger;
    std::string data_dir, dtype;
    int batch_size, img_size,
        num_prefetchers, prefetch_pool_size,
        num_warmup_batches, num_batches;
    // Parse command line options
    po::options_description opt_desc("Benchmark Tensor Datasets");
    po::variables_map var_map;
    opt_desc.add_options()
        ("help", "Print help message")
        ("data_dir", po::value<std::string>(&data_dir)->required(), "Path to a dataset to use.")
        ("batch_size", po::value<int>(&batch_size)->default_value(512), "Batch size.")
        ("img_size", po::value<int>(&img_size)->default_value(227), "Size of images in a dataset (width = height).")
        ("num_prefetchers", po::value<int>(&num_prefetchers)->default_value(1), "Number of prefetchers (readers).")
        ("prefetch_pool_size", po::value<int>(&prefetch_pool_size)->default_value(2), "Number of pre-allocated batches.")
        ("num_warmup_batches", po::value<int>(&num_warmup_batches)->default_value(10), "Number of warmup iterations.")
        ("num_batches", po::value<int>(&num_batches)->default_value(50), "Number of benchmark iterations.")
        ("dtype", po::value<std::string>(&dtype)->default_value("float"), "Tensor data type - 'float' or 'uchar'.");
    //
    try {
        po::store(po::parse_command_line(argc, argv, opt_desc), var_map);
        if (var_map.count("help")) {
            std::cout << opt_desc << std::endl;
            return 0;
        }
        po::notify(var_map);
    } catch(po::error& e) {
        logger.log_warning(e.what());
        std::cout << opt_desc << std::endl;
        logger.log_error("Cannot recover from previous errors");
    }
    //
    logger.log_info(fmt(
        "[benchmarks            ]: data_dir=%s, batch_size=%d, img_size=%d, num_prefetchers=%d, prefetch_pool_size=%d, num_warmup_batches=%d, num_batches=%d, dtype=%s",
        data_dir.c_str(), batch_size, img_size, num_prefetchers, prefetch_pool_size, num_warmup_batches, num_batches, dtype.c_str()
    ));
    const auto images_sec = tensor_dataset::benchmark(
        data_dir, batch_size, img_size, num_prefetchers, prefetch_pool_size,
        num_warmup_batches, num_batches, dtype
    );
    if (images_sec >= 0) {
        // 3 channels times number of elements per channel times element size in bytes devided by bytes in megabyte
        const float img_mb = float(3*img_size*img_size)*(dtype == "float" ? 4 : 1) / (1024*1024);
        const float mb_sec = images_sec * img_mb;
        logger.log_info(fmt("[benchmarks            ]: images/sec=%f, MB/sec=%f", images_sec, mb_sec));
    } else {
        logger.log_warning("[benchmarks            ]: Benchmark returned error code indicating there was an error.");
    }

}


#ifndef YAMAIL_RESOURCE_POOL_TIME_TRAITS_HPP
#define YAMAIL_RESOURCE_POOL_TIME_TRAITS_HPP

#include <boost/asio/basic_waitable_timer.hpp>

#include <chrono>

namespace yamail {
namespace resource_pool {

struct time_traits {
    using duration = std::chrono::steady_clock::duration;
    using time_point = std::chrono::steady_clock::time_point;
    using timer = boost::asio::basic_waitable_timer<std::chrono::steady_clock>;

    static time_point now() {
        return std::chrono::steady_clock::now();
    }

    static time_point add(time_point t, duration d) {
        const time_point epoch;
        if (t >= epoch) {
            if (time_point::max() - t < d) {
                return time_point::max();
            }
        } else {
            if (-(t - time_point::min()) > d) {
                return time_point::min();
            }
        }
        return t + d;
    }
};

} // namespace resource_pool
} // namespace yamail

#endif // YAMAIL_RESOURCE_POOL_TIME_TRAITS_HPP


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file specification_test.cpp
/// \brief Add your file description here.

#define BOOST_TEST_MODULE specification_test
#include "mcrl2/lps/detail/test_input.h"
#include "mcrl2/lps/find.h"
#include "mcrl2/lps/linearise.h"
#include "mcrl2/lps/parse.h"
#include "mcrl2/lps/print.h"

#include <boost/test/included/unit_test_framework.hpp>

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::lps;

void test_find_sort_expressions()
{
  specification spec=remove_stochastic_operators(linearise(lps::detail::ABP_SPECIFICATION()));
  std::set<sort_expression> s;
  lps::find_sort_expressions(spec, std::inserter(s, s.end()));
  std::cout << core::detail::print_set(s) << std::endl;
}

void test_system_defined_sorts()
{
  const std::string SPEC(
    "act a;\n\n"
    "proc X(i,j: Nat)   = (i == 5) -> a. X(i,j);\n\n"
    "init X(0,1);\n");

  specification spec=remove_stochastic_operators(linearise(SPEC));
  function_symbol_vector r = spec.data().constructors(data::sort_nat::nat());
  BOOST_CHECK(r.size() != 0);
}

// This test is added to demonstrate that context sorts need to be added to
// the data specification of an LPS. If this is not done, the resulting
// LPS is not well typed.
void test_context_sorts()
{
  std::string text =
    "proc P = sum x:Real.(x==x) -> tau.P;\n"
    "init P;\n"
    ;

  specification spec = parse_linear_process_specification(text);
  std::cout << "spec = " << spec << std::endl;
  BOOST_CHECK(check_well_typedness(spec));

  stochastic_specification sspec;
  parse_lps(text, sspec);
  std::cout << "sspec = " << sspec << std::endl;
  BOOST_CHECK(check_well_typedness(sspec));
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_find_sort_expressions();
  test_system_defined_sorts();
  test_context_sorts();
}


#include <vector>
#include <boost/timer/timer.hpp>
#include <GeomLib/GeomLib.hpp>
#include <TwoDLib/TwoDLib.hpp>
#include <MPILib/include/MPINetworkCode.hpp>
#include <MPILib/include/RateAlgorithmCode.hpp>
#include <MPILib/include/SimulationRunParameter.hpp>
#include <MPILib/include/DelayAlgorithmCode.hpp>
#include <MPILib/include/utilities/ProgressBar.hpp>
#include <MPILib/include/BasicDefinitions.hpp>
#include <MPILib/include/report/handler/AbstractReportHandler.hpp>

#ifndef MPILIB_MIINDTVBMODELABSTRACT_HPP_
#define MPILIB_MIINDTVBMODELABSTRACT_HPP_

namespace MPILib {

template<class Weight, class NodeDistribution>
class MiindTvbModelAbstract {
public:

	MiindTvbModelAbstract(int num_nodes, double simulation_length) :
		_num_nodes(num_nodes), _simulation_length(simulation_length){
	}

	~MiindTvbModelAbstract() {
	}

	virtual void init() {};

	virtual void startSimulation() {
		pb = new utilities::ProgressBar(network.startSimulation());
	}

	virtual std::vector<double> evolveSingleStep(std::vector<double> activity) {
		(*pb)++;
		return network.evolveSingleStep(activity);
	}

	virtual void endSimulation() {
		network.endSimulation();
		t.stop();
		t.report();
	}

	double getTimeStep() {
		return _time_step;
	}

	double getSimulationLength() {
		return _simulation_length;
	}

	int getNumNodes() {
		return _num_nodes;
	}

protected:
	MPINetwork<Weight, NodeDistribution> network;
	report::handler::AbstractReportHandler *report_handler;
	boost::timer::auto_cpu_timer t;
	utilities::ProgressBar *pb;
	double _simulation_length; // ms
	double _time_step; // ms
	int _num_nodes;
};

}

#endif // MPILIB_MIINDTVBMODELABSTRACT_HPP_


/*******************************************************************************
#      ____               __          __  _      _____ _       _               #
#     / __ \              \ \        / / | |    / ____| |     | |              #
#    | |  | |_ __   ___ _ __ \  /\  / /__| |__ | |  __| | ___ | |__   ___      #
#    | |  | | '_ \ / _ \ '_ \ \/  \/ / _ \ '_ \| | |_ | |/ _ \| '_ \ / _ \     #
#    | |__| | |_) |  __/ | | \  /\  /  __/ |_) | |__| | | (_) | |_) |  __/     #
#     \____/| .__/ \___|_| |_|\/  \/ \___|_.__/ \_____|_|\___/|_.__/ \___|     #
#           | |                                                                #
#           |_|                                                                #
#                                                                              #
#                                (c) 2011 by                                   #
#           University of Applied Sciences Northwestern Switzerland            #
#                     Institute of Geomatics Engineering                       #
#                           martin.christen@fhnw.ch                            #
********************************************************************************
*     Licensed under MIT License. Read the file LICENSE for more information   *
*******************************************************************************/

//------------------------------------------------------------------------------

#include "og.h"
#include "ogprocess.h"
#include "geo/MercatorQuadtree.h"
#include "geo/CoordinateTransformation.h"
#include "geo/PointCloudReader.h"
#include "math/Octocode.h"
#include "string/FilenameUtils.h"
#include "string/StringUtils.h"
#include "io/FileSystem.h"
#include <float.h>
#include <iostream>
#include <ctime>
#include <boost/program_options.hpp>

int _frominput(const std::vector<std::string>& vecFiles, const std::string& srs, bool bVerbose, bool bPointCloud);
void _calcfromwgs84(int, double, double, double, double);
//------------------------------------------------------------------------------

namespace po = boost::program_options;

int main(int argc, char *argv[])
{
   po::options_description desc("Program-Options");
   desc.add_options()
       ("maxlod", po::value<int>(), "desired level of detail (integer)")
       ("wgs84", po::value< std::vector<double> >()->multitoken(), "wgs84 coordinates lng0 lat0 lng1 lat1")
       ("srs", po::value<std::string>(), "spatial reference system for input files")
       ("input", po::value< std::vector<std::string> >()->multitoken(), "list input files")
       ("verbose", "display additional information")
       ("inputdir", po::value<std::string>(), "input directory")
       ("filetype",  po::value<std::string>(), "file type")
       ("point", "for point cloud")
       ;

   po::variables_map vm;

   try
   {
      po::store(po::parse_command_line(argc, argv, desc), vm);
      po::notify(vm);
   }
   catch (std::exception&)
   {
      std::cout << desc;
      return 4;
   }

   bool bVerbose = false;
   bool bPointCloud = false;

   if (vm.count("verbose"))
   {
      bVerbose = true;
   }

   if (vm.count("point"))
   {
      bPointCloud = true;
   }

   if ((vm.count("wgs84") && !vm.count("maxlod")) || (!vm.count("wgs84") && vm.count("maxlod")))
   {
      std::cout << "ERROR: option --wgs84 and --maxlod must be used together\n";
      return 1;
   }

   if (vm.count("wgs84") && vm.count("maxlod"))
   {
      int lod = vm["maxlod"].as<int>();
      if (lod < 0 || lod>23)
      {
         std::cout << "ERROR: lod value in wrong range\n";
         return 1;
      }

      double lng0, lat0, lng1, lat1;

      std::vector<double> vecCoords= vm["wgs84"].as< std::vector<double> >();
      if (vecCoords.size() != 4)
      {
         std::cout << "ERROR: wrong number of wgs84 coordinates\n";
         return 1;
      }

      lng0 = vecCoords[0];
      lat0 = vecCoords[1];
      lng1 = vecCoords[2];
      lat1 = vecCoords[3];

      if (lng0 >= lng1) { std::cout << "error: lng1 must be greater than lng0\n"; return 0; }
      if (lat0 >= lat1) { std::cout << "error: lat1 must be greater than lat0\n"; return 0; }
      if (lod < 4) { std::cout << "error: level of detail must be atleast 4\n"; return 0; }

      _calcfromwgs84(lod, lng0, lat0, lng1, lat1);

      return 0;
   }
   else if (vm.count("srs") && vm.count("input"))
   {
      std::vector<std::string> vecFiles = vm["input"].as< std::vector<std::string> >();
      std::string srs = vm["srs"].as<std::string>();

      return _frominput(vecFiles, srs, bVerbose, bPointCloud);
   }
   else if (vm.count("srs") && vm.count("inputdir") && vm.count("filetype"))
   {
      std::string srs = vm["srs"].as<std::string>();
      std::string inputdir = vm["inputdir"].as<std::string>();
      std::string filetype = vm["filetype"].as<std::string>();

      std::vector<std::string> vecFiles = FileSystem::GetFilesInDirectory(inputdir, filetype);

      if (vecFiles.size() == 0)
      {
         std::cout << "No files found!\n";
         return 1;
      }

      return _frominput(vecFiles, srs, bVerbose, bPointCloud);
   }
   else
   {
      std::cout << desc << "\n";
      std::cout << "From input files: use --srs and --input together\n";
      std::cout << "From wgs84 coord: use --wgs84 and --maxlod together\n";
      return 1;
   }

   return 0;
}

//------------------------------------------------------------------------------

//------------------------------------------------------------------------------

int _frominput(const std::vector<std::string>& vecFiles, const std::string& srs, bool bVerbose, bool bPointCloud)
{

   if (StringUtils::Left(srs, 5) != "EPSG:")
   {
      std::cout << "Error: only srs starting with EPSG: are currently supported";
      return 1;
   }

   int epsg = atoi(srs.c_str()+5);
   std::cout << "SRS epsg-code: " << epsg << "\n";

   if (!ProcessingUtils::init_gdal())
   {
      std::cout << "Warning: gdal-data directory not found. Ouput may be wrong!\n";
   }

   boost::shared_ptr<CoordinateTransformation> qCT;

   if (bPointCloud)
   {
      qCT = boost::shared_ptr<CoordinateTransformation>(new CoordinateTransformation(epsg, 4326));
      clock_t t0,t1;
      t0 = clock();

      // vecfiles contains xyz (or xyzi or xyzirgb) ASCII files.
      // a) find the center of the dataset
      // b) find extent (max,min of x,y and z component)

      double xmin, ymin, zmin;
      double xmax, ymax, zmax;
      double xcenter, ycenter, zcenter;

      xmin=ymin=zmin=1e20;
      xmax=ymax=zmax=-1e20;

      size_t numpts = 0;

      CloudPoint pt;
      for (size_t i = 0; i< vecFiles.size();++i)
      {
         PointCloudReader pr;

         if (pr.Open(vecFiles[i]))
         {
            while (pr.ReadPoint(pt))
            {
               qCT->Transform(&pt.x, &pt.y);
               xmin = math::Min<double>(xmin, pt.x);
               ymin = math::Min<double>(ymin, pt.y);
               zmin = math::Min<double>(zmin, pt.elevation);
               xmax = math::Max<double>(xmax, pt.x);
               ymax = math::Max<double>(ymax, pt.y);
               zmax = math::Max<double>(zmax, pt.elevation);

               numpts++;
            }
         }
      }

      xcenter = xmin+ fabs(0.5*(xmax-xmin));
      ycenter = ymin+fabs(0.5*(ymax-ymin));
      zcenter = zmin+fabs(0.5*(zmax-zmin));

      std::cout.precision(15);

      for (int i=1;i<23;i++)
      {
         std::cout << "LEVEL OF DETAIL " << i << ": ";
         _calcfromwgs84(i, xmin, ymin, xmax, ymax);
      }

      t1 = clock();
      std::cout << "calculated in: " << double(t1-t0)/double(CLOCKS_PER_SEC) << " s \n";

      std::cout << "There are " << numpts << " points...\n";
      //std::cout << "Point Cloud Center (WGS84): (" << xcenter << ", " << ycenter << ", " << zcenter <<")\n";
      std::cout << "Required for ogCreateLayer:\n";
      std::cout << "-------------------------------------------------------------------------------\n";
      std::cout << "Extent (WGS84): (" << xmin << ", " << ymin << ", " << zmin << ")-(" << xmax << ", " << ymax << ", " << zmax << ")\n";
      std::cout << "-------------------------------------------------------------------------------\n";

      return 0;
   }
   else
   {

      qCT = boost::shared_ptr<CoordinateTransformation>(new CoordinateTransformation(epsg, 3785));

      // create an array of Dataset info for parallel access.
      DataSetInfo* pDataset = new DataSetInfo[vecFiles.size()];

      clock_t t0,t1;
      t0 = clock();

      for (int i=0;i<(int)vecFiles.size();i++)
      {
         ProcessingUtils::RetrieveDatasetInfo(vecFiles[i], qCT.get(), &pDataset[i], bVerbose);
      }

      // at this point we finished calculating all the boundaries of all datasets, now
      // calculate the min/max

      double total_dest_ulx = 1e20;
      double total_dest_lry = 1e20;
      double total_dest_lrx = -1e20;
      double total_dest_uly = -1e20;
      double pixelsize = 1e20;

      for (size_t i=0;i<vecFiles.size();i++)
      {
         if (pDataset[i].bGood)
         {
            total_dest_ulx = math::Min<double>(pDataset[i].dest_ulx, total_dest_ulx);
            total_dest_lry = math::Min<double>(pDataset[i].dest_lry, total_dest_lry);
            total_dest_lrx = math::Max<double>(pDataset[i].dest_lrx, total_dest_lrx);
            total_dest_uly = math::Max<double>(pDataset[i].dest_uly, total_dest_uly);
            pixelsize      = math::Min<double>(pDataset[i].pixelsize, pixelsize);
         }
      }

      double pixelsize_m = pixelsize * 6378137.0;

      t1 = clock();

      std::cout << "GATHERED BOUNDARY (Mercator):\n";
      std::cout.precision(16);
      std::cout << "        ulx: " << total_dest_ulx << "\n";
      std::cout << "        lry: " << total_dest_lry << "\n";
      std::cout << "        lrx: " << total_dest_lrx << "\n";
      std::cout << "        uly: " << total_dest_uly << "\n";
      std::cout << "BOUNDARY in WGS84:\n";

      MercatorQuadtree* pQuadtree = new MercatorQuadtree();
      double x,y;

      double lng0, lat0, lng1, lat1;
      x = total_dest_ulx; y = total_dest_lry;
      pQuadtree->MercatorToWGS84(x, y);
      lng0 = x; lat0 = y;
      std::cout << "       lng0: " << x << "\n";
      std::cout << "       lat0: " << y << "\n";
      x = total_dest_lrx; y = total_dest_uly;
      pQuadtree->MercatorToWGS84(x, y);
      lng1 = x; lat1 = y;
      std::cout << "       lng1: " << x << "\n";
      std::cout << "       lat1: " << y << "\n";
      std::cout << " pixelsize: " << pixelsize_m << " m\n";

      double lat_avg = 0.5*(lat1 - lat0);

      double resolution_elevation[21];
      double resolution_image[21];

      for (int lod = 0; lod<21;lod++)
      {
         double v = cos(lat_avg*3.14159265358979/180)*2*3.14159265358979*6378137.0/pow(2.0,lod);
         resolution_elevation[lod] = v / 22.0; // ~500 points per tile
         resolution_image[lod] = v / 256;
      }

      int recommended_lod_elv = 0;
      int recommended_lod_img = 0;
      for (int lod = 0; lod<20;lod++)
      {
         if (pixelsize_m < resolution_elevation[lod] && pixelsize_m > resolution_elevation[lod+1])
         {
            recommended_lod_elv = lod;
         }
         if (pixelsize_m < resolution_image[lod] && pixelsize_m > resolution_image[lod+1])
         {
            recommended_lod_img = lod;
         }
      }

      for (int i=1;i<23;i++)
      {
         std::cout << "LEVEL OF DETAIL " << i << ": ";
         _calcfromwgs84(i, lng0, lat0, lng1, lat1);
      }

      std::cout << "****************************************\n";
      std::cout << "RECOMMENDATION (MINIMUM LOD):\n";
      if (recommended_lod_elv != 0)
      {
         std::cout << "IF THIS IS ELEVATION: LOD=" << recommended_lod_elv << ": ";
         _calcfromwgs84(recommended_lod_elv, lng0, lat0, lng1, lat1);
      }
      if (recommended_lod_img != 0)
      {
         std::cout << "\nIF THIS IS IMAGE: LOD=" << recommended_lod_img << ": ";
         _calcfromwgs84(recommended_lod_img, lng0, lat0, lng1, lat1);
      }
      if (recommended_lod_img == 0 && recommended_lod_elv==0)
      {
         std::cout << "recommendation is not possible.\n";
      }
      std::cout << "****************************************\n";

      delete pQuadtree;

      //std::cout << "calculated in: " << double(t1-t0)/double(CLOCKS_PER_SEC) << " s \n";

      delete[] pDataset;

      ProcessingUtils::exit_gdal();

      return 0;
   }
}

//------------------------------------------------------------------------------

void _calcfromwgs84(int lod, double lng0, double lat0, double lng1, double lat1)
{
   MercatorQuadtree* pQuadtree = new MercatorQuadtree();

   int64 px0, py0, px1, py1, tx0, ty0, tx1, ty1;
   pQuadtree->WGS84ToPixel(lng0, lat0, lod, px0, py1);
   pQuadtree->WGS84ToPixel(lng1, lat1, lod, px1, py0);

   //std::cout << "Number of pixels in specified range: " << (px1-px0+1)*(py1-py0+1) << "\n";

   pQuadtree->PixelToTileCoord(px0, py0, tx0, ty0);
   pQuadtree->PixelToTileCoord(px1, py1, tx1, ty1);

   std::cout << "Tile Coords: (" << tx0 << ", " << ty0 << ")-(" << tx1 << ", " << ty1 << ")\n";

   delete pQuadtree;

}


/*
 * Generate artifical telegrams and append a CRC.
 *
 * Author: Martin Schobert <schobert@sitsec.net>
 *
 */
#include <iostream>
#include <string>
#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>
#include "crc.hpp"

namespace po = boost::program_options;

typedef my_crc_basic crc_t;

bool int_to_bool(int v) {
  return v == 0 ? false : true;
}

std::string bool_to_str(bool v) {
  return v ? "true" : "false";
}

uint32_t random_value(uint32_t max_val, uint32_t min_val = 0) {
  return (rand() % ((max_val-min_val)+1)) + min_val;
}

bool random_bool() {
  return int_to_bool(random_value(1));
}

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

  srand((unsigned)time(0));

  size_t num_messages = 5;
  size_t width = random_value(16, 5);
  size_t start = random_value(0);
  size_t offs_crc = start + random_value(80, 30);
  size_t end = offs_crc;
  uint32_t poly = random_value(MAX_VALUE(width));
  uint32_t init = random_value(MAX_VALUE(width));
  uint32_t final_xor = width < 12 ? random_value(MAX_VALUE(width)) : 0;
  bool
    ref_in = random_bool(),
    ref_out = random_bool();

  // definition of program options
  po::options_description desc("Allowed options");
  desc.add_options()
    ("help", "produce help message")
    ("width", po::value<size_t>(), "CRC width")
    ("messages", po::value<size_t>(), "number of messages")
    ("final-xor", po::value<uint32_t>(), "use value for the final XOR")
    ;

  // parse programm options
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, desc), vm);
  po::notify(vm);

  if(vm.count("help")) {
    std::cout << desc << "\n";
    return 1;
  }

  if(vm.count("width")) width = vm["width"].as<size_t>();
  if(vm.count("messages")) num_messages = vm["messages"].as<size_t>();
  if(vm.count("final-xor")) final_xor = vm["final-xor"].as<uint32_t>();

  // check parameters
  if(width > 16) { std::cout << "maximum value for width is: 16\n"; exit(1); }

  // print parameters
  std::cout << "# width                    : " << width << " bits\n"
	    << "# CRC's offset             : " << offs_crc << "\n"
	    << "# calc CRC for bit offsets : " << start << " .. " << end << " (not included)\n"
	    << "# final XOR                : " << final_xor << "\n"
	    << "# reflect in               : " << bool_to_str(ref_in) << "\n"
	    << "# reflect out              : " << bool_to_str(ref_out) << "\n"
	    << "# \n"
	    << "# truncated polynom        : " << poly << " (MSB not shown)\n"
	    << "# initial value            : " << init << "\n"
	    << "\n";

  crc_t crc(width, poly, init, final_xor, ref_in, ref_out);

  for(size_t num_msg = 0; num_msg < num_messages; num_msg++) {
    size_t i = 0;

    crc.reset(init);

    // the data bits
    for(i = start; i < end; i++) {
      int v = random_value(1);
      std::cout << v;
      crc.process_bit(int_to_bool(v));
    }
    std::cout << " ";

    // write crc
    uint32_t cs = crc.checksum();

    for(i = 0; i < width; i++) {
      uint32_t mask = 1 << (width-1 -i);
      int bit = (cs & mask) ? 1 : 0;
      std::cout << bit;
    }
    std::cout << "\n";

  }

  return 0;
}


#include <queue>
#include <rai/node/common.hpp>
#include <rai/node/wallet.hpp>
#include <rai/secure/blockstore.hpp>

#include <boost/polymorphic_cast.hpp>

rai::amount_visitor::amount_visitor (rai::transaction const & transaction_a, rai::block_store & store_a) :
transaction (transaction_a),
store (store_a),
current_amount (0),
current_balance (0),
amount (0)
{
}

void rai::amount_visitor::send_block (rai::send_block const & block_a)
{
	current_balance = block_a.hashables.previous;
	amount = block_a.hashables.balance.number ();
	current_amount = 0;
}

void rai::amount_visitor::receive_block (rai::receive_block const & block_a)
{
	current_amount = block_a.hashables.source;
}

void rai::amount_visitor::open_block (rai::open_block const & block_a)
{
	if (block_a.hashables.source != rai::genesis_account)
	{
		current_amount = block_a.hashables.source;
	}
	else
	{
		amount = rai::genesis_amount;
		current_amount = 0;
	}
}

void rai::amount_visitor::state_block (rai::state_block const & block_a)
{
	current_balance = block_a.hashables.previous;
	amount = block_a.hashables.balance.number ();
	current_amount = 0;
}

void rai::amount_visitor::change_block (rai::change_block const & block_a)
{
	amount = 0;
	current_amount = 0;
}

void rai::amount_visitor::compute (rai::block_hash const & block_hash)
{
	current_amount = block_hash;
	while (!current_amount.is_zero () || !current_balance.is_zero ())
	{
		if (!current_amount.is_zero ())
		{
			auto block (store.block_get (transaction, current_amount));
			if (block != nullptr)
			{
				block->visit (*this);
			}
			else
			{
				if (block_hash == rai::genesis_account)
				{
					amount = std::numeric_limits<rai::uint128_t>::max ();
					current_amount = 0;
				}
				else
				{
					assert (false);
					amount = 0;
					current_amount = 0;
				}
			}
		}
		else
		{
			balance_visitor prev (transaction, store);
			prev.compute (current_balance);
			amount = amount < prev.balance ? prev.balance - amount : amount - prev.balance;
			current_balance = 0;
		}
	}
}

rai::balance_visitor::balance_visitor (rai::transaction const & transaction_a, rai::block_store & store_a) :
transaction (transaction_a),
store (store_a),
current_balance (0),
current_amount (0),
balance (0)
{
}

void rai::balance_visitor::send_block (rai::send_block const & block_a)
{
	balance += block_a.hashables.balance.number ();
	current_balance = 0;
}

void rai::balance_visitor::receive_block (rai::receive_block const & block_a)
{
	rai::block_info block_info;
	if (!store.block_info_get (transaction, block_a.hash (), block_info))
	{
		balance += block_info.balance.number ();
		current_balance = 0;
	}
	else
	{
		current_amount = block_a.hashables.source;
		current_balance = block_a.hashables.previous;
	}
}

void rai::balance_visitor::open_block (rai::open_block const & block_a)
{
	current_amount = block_a.hashables.source;
	current_balance = 0;
}

void rai::balance_visitor::change_block (rai::change_block const & block_a)
{
	rai::block_info block_info;
	if (!store.block_info_get (transaction, block_a.hash (), block_info))
	{
		balance += block_info.balance.number ();
		current_balance = 0;
	}
	else
	{
		current_balance = block_a.hashables.previous;
	}
}

void rai::balance_visitor::state_block (rai::state_block const & block_a)
{
	balance = block_a.hashables.balance.number ();
	current_balance = 0;
}

void rai::balance_visitor::compute (rai::block_hash const & block_hash)
{
	current_balance = block_hash;
	while (!current_balance.is_zero () || !current_amount.is_zero ())
	{
		if (!current_amount.is_zero ())
		{
			amount_visitor source (transaction, store);
			source.compute (current_amount);
			balance += source.amount;
			current_amount = 0;
		}
		else
		{
			auto block (store.block_get (transaction, current_balance));
			assert (block != nullptr);
			block->visit (*this);
		}
	}
}

rai::representative_visitor::representative_visitor (rai::transaction const & transaction_a, rai::block_store & store_a) :
transaction (transaction_a),
store (store_a),
result (0)
{
}

void rai::representative_visitor::compute (rai::block_hash const & hash_a)
{
	current = hash_a;
	while (result.is_zero ())
	{
		auto block (store.block_get (transaction, current));
		assert (block != nullptr);
		block->visit (*this);
	}
}

void rai::representative_visitor::send_block (rai::send_block const & block_a)
{
	current = block_a.previous ();
}

void rai::representative_visitor::receive_block (rai::receive_block const & block_a)
{
	current = block_a.previous ();
}

void rai::representative_visitor::open_block (rai::open_block const & block_a)
{
	result = block_a.hash ();
}

void rai::representative_visitor::change_block (rai::change_block const & block_a)
{
	result = block_a.hash ();
}

void rai::representative_visitor::state_block (rai::state_block const & block_a)
{
	result = block_a.hash ();
}