// -*- C++ -*-
/*!
 * @file  Kinematics.cpp
 * @brief Kinematics Calculation
 * @date $Date$
 *
 * $Id$
 */

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>
#include "Kinematics.h"

using namespace std;
using namespace Eigen;

#define PI 3.1415926

double deg2rad(double degree)
{
    double radian = degree * PI / 180.0f;
    return radian;
}

double rad2deg(double radian)
{
    double degree = radian * 180.0f / PI;
    return degree;
}

/*
double ForwardKinematics(double angle)
{
    return position;
}

double InvserseKinematics(double position)
{
    return angle;
}

double CalcJacobian(double angle)
{
    return Jacobian;
}
*/

// Module specification
// <rtc-template block="module_spec">
static const char* kinematics_spec[] =
{
    "implementation_id", "Kinematics",
    "type_name",         "Kinematics",
    "description",       "Kinematics Calculation",
    "version",           "1.0.0",
    "vendor",            "Takaharu Nakajima",
    "category",          "Generic",
    "activity_type",     "PERIODIC",
    "kind",              "DataFlowComponent",
    "max_instance",      "1",
    "language",          "C++",
    "lang_type",         "compile",
    ""
};
// </rtc-template>

/*!
 * @brief constructor
 * @param manager Maneger Object
 */
Kinematics::Kinematics(RTC::Manager* manager)
    // <rtc-template block="initializer">
    : RTC::DataFlowComponentBase(manager),
    m_angleIn("q", m_angle),
    m_positionIn("p", m_position),
    m_angle_outOut("q_out", m_angle_out),
    m_position_outOut("p_out", m_position_out)

      // </rtc-template>
{
}

/*!
 * @brief destructor
 */
Kinematics::~Kinematics()
{
}

RTC::ReturnCode_t Kinematics::onInitialize()
{
    // Registration: InPort/OutPort/Service
    // <rtc-template block="registration">
    // Set InPort buffers
    addInPort("q", m_angleIn);
    addInPort("p", m_positionIn);

    // Set OutPort buffer
    addOutPort("q_out", m_angle_outOut);
    addOutPort("p_out", m_position_outOut);

    // Set service provider to Ports

    // Set service consumers to Ports

    // Set CORBA Service Ports

    // </rtc-template>

    // <rtc-template block="bind_config">
    // </rtc-template>

    return RTC::RTC_OK;
}

/*
   RTC::ReturnCode_t Kinematics::onFinalize()
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onStartup(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onShutdown(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onActivated(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onDeactivated(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

RTC::ReturnCode_t Kinematics::onExecute(RTC::UniqueId ec_id)
{

    return RTC::RTC_OK;
}

/*
   RTC::ReturnCode_t Kinematics::onAborting(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onError(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onReset(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onStateUpdate(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

/*
   RTC::ReturnCode_t Kinematics::onRateChanged(RTC::UniqueId ec_id)
   {
   return RTC::RTC_OK;
   }
   */

extern "C"
{

    void KinematicsInit(RTC::Manager* manager)
    {
        coil::Properties profile(kinematics_spec);
        manager->registerFactory(profile,
                RTC::Create<Kinematics>,
                RTC::Delete<Kinematics>);
    }

};


#include <boost/accumulators/statistics/parameters/quantile_probability.hpp>


#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();
            // calculate 3d points in world coordinates as ref
            setRef3DPoints();
            break;
        }

        case OK:{
            curr_ = frame;
            extractKeyPoints();
            computerDescriptors();
            featureMatching();
            poseEstimationPnP();

            if (checkEstimatedPose() == true){
                curr_->T_w_c_ = T_c_r_estimated_ * ref_->T_w_c_; // update current camera pose
                ref_ = curr_;
                setRef3DPoints();
                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_, descriptor_curr_);
    }

    void VisualOdometry::featureMatching(){
        vector<cv::DMatch> matches;
        cv::BFMatcher matcher (cv::NORM_HAMMING);
        matcher.match(descriptor_curr_, descriptor_ref_, 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;

        feature_matches_.clear();
        // filter out long distance matches
        for (cv::DMatch& m:matches){
            if(m.distance < 2*min_dis){
                feature_matches_.push_back(m);
            }
        }
        cout << "good matches number: " << feature_matches_.size() << endl;
    }

    void VisualOdometry::setRef3DPoints(){
        pts_3d_ref_.clear();
        descriptor_ref_ = Mat();

        for(int i = 0;i<keypoints_curr_.size();i++){
            double depth = ref_->findDepth(keypoints_curr_[i]);
            if (depth > 0){
                Vector3d p_cam = ref_->camera_->pixel2camera(
                    Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), depth
                );
                pts_3d_ref_.push_back(cv::Point3f(p_cam(0,0), p_cam(1,0), p_cam(2, 0)));
                descriptor_ref_.push_back(descriptor_curr_.row(i));
            }
        }
    }

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

        for (cv::DMatch m:feature_matches_){
            pts3d.push_back(pts_3d_ref_[m.queryIdx]);
            pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
        }

        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 ( cv::DMatch m:feature_matches_ )
        {
            pts3d.push_back( pts_3d_ref_[m.queryIdx] );
            pts2d.push_back( keypoints_curr_[m.trainIdx].pt );
        }

        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(){
        map_->insertKeyFrame(curr_);
    }
}


#ifndef SDF_HPP
#define SDF_HPP

#include <stdlib.h>
#include <vector>
#include <string>
#include <Eigen/Geometry>
#include <visualization_msgs/Marker.h>
#include <arc_utilities/eigen_helpers.hpp>
#include <arc_utilities/voxel_grid.hpp>
#include <sdf_tools/SDF.h>

namespace sdf_tools
{
    std::vector<uint8_t> FloatToBinary(float value);

    float FloatFromBinary(std::vector<uint8_t>& binary);

    class SignedDistanceField
    {
    protected:

        VoxelGrid::VoxelGrid<float> distance_field_;
        std::string frame_;
        bool initialized_;
        bool locked_;

        std::vector<uint8_t> GetInternalBinaryRepresentation(const std::vector<float> &field_data);

        std::vector<float> UnpackFieldFromBinaryRepresentation(const std::vector<uint8_t>& binary);

        /*
         * You *MUST* provide valid indices to this function, hence why it is protected (there are safe wrappers available - use them!)
         */
        void FollowGradientsToLocalMaximaUnsafe(VoxelGrid::VoxelGrid<Eigen::Vector3d>& watershed_map, const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

    public:

        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        SignedDistanceField(std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);

        SignedDistanceField(Eigen::Isometry3d origin_transform, std::string frame, double resolution, double x_size, double y_size, double z_size, float OOB_value);

        SignedDistanceField();

        bool IsInitialized() const;

        bool IsLocked() const;

        void Lock();

        void Unlock();

        float Get(const double x, const double y, const double z) const;

        float Get3d(const Eigen::Vector3d& location) const;

        float Get4d(const Eigen::Vector4d& location) const;

        float Get(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

        std::pair<float, bool> GetSafe(const double x, const double y, const double z) const;

        std::pair<float, bool> GetSafe3d(const Eigen::Vector3d& location) const;

        std::pair<float, bool> GetSafe4d(const Eigen::Vector4d& location) const;

        std::pair<float, bool> GetSafe(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

        /*
         * Setter functions MUST be used carefully - If you arbitrarily change SDF values, it is not a proper SDF any more!
         *
         * Use of these functions can be prevented by calling SignedDistanceField::Lock() on the SDF, at which point these functions
         * will fail with a warning printed to std_err.
         */
        bool Set(const double x, const double y, const double z, float value);

        bool Set3d(const Eigen::Vector3d& location, float value);

        bool Set4d(const Eigen::Vector4d& location, float value);

        bool Set(const int64_t x_index, const int64_t y_index, const int64_t z_index, const float value);

        bool Set(const VoxelGrid::GRID_INDEX& index, const float value);

        bool CheckInBounds3d(const Eigen::Vector3d& location) const;

        bool CheckInBounds4d(const Eigen::Vector4d& location) const;

        bool CheckInBounds(const double x, const double y, const double z) const;

        bool CheckInBounds(const VoxelGrid::GRID_INDEX& index) const;

        bool CheckInBounds(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

        double GetXSize() const;

        double GetYSize() const;

        double GetZSize() const;

        double GetResolution() const;

        float GetOOBValue() const;

        int64_t GetNumXCells() const;

        int64_t GetNumYCells() const;

        int64_t GetNumZCells() const;

    protected:

        std::pair<Eigen::Vector3d, double> GetPrimaryComponentsVector(const Eigen::Vector3d& raw_vector) const;

        double ComputeAxisMatch(const double axis_value, const double check_value) const;

        Eigen::Vector3d GetBestMatchSurfaceVector(const Eigen::Vector3d& possible_surfaces_vector, const Eigen::Vector3d& center_to_location_vector) const;

        /**
         * @brief GetPrimaryEntrySurfaceVector Estimates the real distance of the provided point, comparing it with the cell center location and gradient vector
         * @param boundary_direction_vector
         * @param center_to_location_vector
         * @return vector from center of voxel to primary entry surface, and magnitude of that vector
         */
        std::pair<Eigen::Vector3d, double> GetPrimaryEntrySurfaceVector(const Eigen::Vector3d& boundary_direction_vector, const Eigen::Vector3d& center_to_location_vector) const;

        double EstimateDistanceInternal(const double x, const double y, const double z, const int64_t x_idx, const int64_t y_idx, const int64_t z_idx) const;

    public:

        std::pair<double, bool> EstimateDistance(const double x, const double y, const double z) const;

        std::pair<double, bool> EstimateDistance3d(const Eigen::Vector3d& location) const;

        std::pair<double, bool> EstimateDistance4d(const Eigen::Vector4d& location) const;

        // Estimate the distance between the given point and the outer boundary of the SDF
        std::pair<double, bool> DistanceToBoundary(const double x, const double y, const double z) const;

        std::pair<double, bool> DistanceToBoundary3d(const Eigen::Vector3d& location) const;

        std::pair<double, bool> DistanceToBoundary4d(const Eigen::Vector4d& location) const;

        std::vector<double> GetGradient(const double x, const double y, const double z, const bool enable_edge_gradients = false) const;

        std::vector<double> GetGradient3d(const Eigen::Vector3d& location, const bool enable_edge_gradients = false) const;

        std::vector<double> GetGradient4d(const Eigen::Vector4d& location, const bool enable_edge_gradients = false) const;

        std::vector<double> GetGradient(const VoxelGrid::GRID_INDEX& index, const bool enable_edge_gradients = false) const;

        std::vector<double> GetGradient(const int64_t x_index, const int64_t y_index, const int64_t z_index, const bool enable_edge_gradients = false) const;

        Eigen::Vector3d ProjectOutOfCollision(const double x, const double y, const double z, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance(const double x, const double y, const double z, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector3d ProjectOutOfCollision3d(const Eigen::Vector3d& location, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector3d ProjectOutOfCollisionToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector4d ProjectOutOfCollision4d(const Eigen::Vector4d& location, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector4d ProjectOutOfCollisionToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance, const double stepsize_multiplier = 1.0 / 8.0) const;

        Eigen::Vector3d ProjectIntoValidVolume(const double x, const double y, const double z) const;

        Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance(const double x, const double y, const double z, const double minimum_distance) const;

        Eigen::Vector3d ProjectIntoValidVolume3d(const Eigen::Vector3d& location) const;

        Eigen::Vector3d ProjectIntoValidVolumeToMinimumDistance3d(const Eigen::Vector3d& location, const double minimum_distance) const;

        Eigen::Vector4d ProjectIntoValidVolume4d(const Eigen::Vector4d& location) const;

        Eigen::Vector4d ProjectIntoValidVolumeToMinimumDistance4d(const Eigen::Vector4d& location, const double minimum_distance) const;

        const Eigen::Isometry3d& GetOriginTransform() const;

        const Eigen::Isometry3d& GetInverseOriginTransform() const;

        std::string GetFrame() const;

        std::vector<int64_t> LocationToGridIndex3d(const Eigen::Vector3d& location) const;

        std::vector<int64_t> LocationToGridIndex4d(const Eigen::Vector4d& location) const;

        std::vector<int64_t> LocationToGridIndex(const double x, const double y, const double z) const;

        std::vector<double> GridIndexToLocation(const VoxelGrid::GRID_INDEX& index) const;

        std::vector<double> GridIndexToLocation(const int64_t x_index, const int64_t y_index, const int64_t z_index) const;

        bool SaveToFile(const std::string& filepath);

        bool LoadFromFile(const std::string& filepath);

        sdf_tools::SDF GetMessageRepresentation();

        bool LoadFromMessageRepresentation(const sdf_tools::SDF& message);

        visualization_msgs::Marker ExportForDisplay(const float alpha = 0.01f) const;

        visualization_msgs::Marker ExportForDisplayCollisionOnly(const float alpha = 0.01f) const;

        visualization_msgs::Marker ExportForDebug(const float alpha = 0.5f) const;

        /*
         * The following function can be *VERY EXPENSIVE* to compute, since it performs gradient ascent across the SDF
         */
        VoxelGrid::VoxelGrid<Eigen::Vector3d> ComputeLocalMaximaMap() const;

        bool GradientIsEffectiveFlat(const Eigen::Vector3d& gradient) const;

        VoxelGrid::GRID_INDEX GetNextFromGradient(const VoxelGrid::GRID_INDEX& index, const Eigen::Vector3d& gradient) const;
    };
}

#endif // SDF_HPP


/*
 * eval_basic_virtual.cpp
 * Date: 2013-05-10
 * Author: Karsten Ahnert (karsten.ahnert@gmx.de)
 */

#define FUSION_MAX_VECTOR_SIZE 20

#include "parser.hpp"
#include "nodes.hpp"
#include "generate_data.hpp"

#include <gpcxx/eval/static_eval.hpp>
#include <gpcxx/generate/basic_generate_strategy.hpp>
#include <gpcxx/generate/uniform_symbol.hpp>
#include <gpcxx/io/simple.hpp>
#include <gpcxx/tree/basic_tree.hpp>
#include <gpcxx/app/timer.hpp>

#include <boost/fusion/include/make_vector.hpp>

#include <iostream>
#include <functional>
#include <fstream>
#include <vector>
#include <random>
#include <cmath>

#define tab "\t"

using namespace std;
namespace fusion = boost::fusion;

typedef double value_type;
typedef std::vector< value_type > vector_type;
typedef std::array< double , 3 > context_type;
typedef char symbol_type;
typedef std::mt19937 rng_type ;

typedef i_node< double , context_type > node_type;

/// \return time for evaluation of tree, result sum
template< typename Trees >
std::tuple< double , double > run_test( Trees const &trees , const vector_type &x1 , const vector_type &x2 , const vector_type &x3 )
{
    std::tuple< double , double > res;

    size_t number_of_datapoints = x1.size();
    size_t number_of_trees = trees.size();
    std::vector< vector_type > y( number_of_trees );
    for( size_t t=0 ; t<number_of_trees ; ++t ) y[t] = vector_type( number_of_datapoints );

    //
    // EVALUATION
    //
    gpcxx::timer timer;
    for( size_t t=0 ; t<number_of_trees ; ++t )
    {
        for( size_t i=0 ; i<number_of_datapoints ; ++i )
        {
            context_type c { { x1[i] , x2[i] , x3[i] } };
            y[t][i] = trees[t]( c );
            if( fabs( y[t][i] ) > 1.0e5 ) y[t][i] = 1.0e5;
        }
    }
    std::get< 0 >( res ) = timer.seconds();

    double sum = 0.0;
    for( size_t t=0 ; t<number_of_trees ; ++t )
    {
        for( size_t i=0 ; i<number_of_datapoints ; ++i )
            sum += y[t][i];
    }
    std::get< 1 >( res ) = sum;

    return res;
}

template< typename T , typename Context >
struct tree_creator : public boost::static_visitor< void >
{
    typedef i_node< T , Context > node_type;

    tree_creator( node_type &node ) : m_node( node ) { }

    void operator()( parser::nil ) const {}

    void operator()( char n ) const
    {
        switch( n )
        {
            case 'x' : m_node = terminal_node< 0 , T , Context >(); break;
            case 'y' : m_node = terminal_node< 1 , T , Context >(); break;
            case 'z' : m_node = terminal_node< 2 , T , Context >(); break;
        }
    }

    void operator()( parser::expression_ast const& ast ) const
    {
        boost::apply_visitor( *this , ast.expr );
    }

    void operator()( parser::binary_op const& expr ) const
    {
        node_type left , right;
        boost::apply_visitor( tree_creator( left ) , expr.left.expr );
        boost::apply_visitor( tree_creator( right ) , expr.right.expr );

        switch( expr.op )
        {
            case '+' : m_node = plus_node< T , Context >( left , right ); break;
            case '-' : m_node = minus_node< T , Context >( left , right ); break;
            case '*' : m_node = multiplies_node< T , Context >( left , right ); break;
            case '/' : m_node = divides_node< T , Context >( left , right ); break;
        }
    }

    void operator()( parser::unary_op const& expr ) const
    {
        node_type child;
        boost::apply_visitor( tree_creator( child ) , expr.subject.expr );

        switch( expr.op )
        {
            case 's' : m_node = sin_node< T , Context >( child ); break;
            case 'c' : m_node = cos_node< T , Context >( child ); break;
            case 'e' : m_node = exp_node< T , Context >( child ); break;
            case 'l' : m_node = log_node< T , Context >( child ); break;
        }
    }

    node_type &m_node;
};

void run_tree_type( std::string const &name , vector_type const &x1 , vector_type const &x2 , vector_type const &x3 , std::string const & filename )
{
    std::vector< node_type > trees;

    // read trees
    ifstream fin( filename );
    std::string line;
    while( std::getline( fin , line ) )
    {
        node_type tree;
        tree_creator< value_type , context_type > trafo( tree );
        parser::parse_tree( line , trafo );
        trees.push_back( tree );

//         parser::ast_print trafo2;
//         parser::parse_tree( line , trafo2 );
//         break;
    }

    cout.precision( 14 );
    cout << "Starting test " << name << endl;
    auto times = run_test( trees , x1 , x2 , x3 );
    cout << tab << "Finished!" << endl;
    cout << tab << "Evaluation time " << std::get< 0 >( times ) << endl;
    cout << tab << "Result sum " << std::get< 1 >( times ) << endl << endl;

}

int main( int argc , char *argv[] )
{
    if( argc != 2 )
    {
        cerr << "usage : " << argv[0] << " infile" << endl;
        return -1;
    }

    // generate test data
    vector_type x1, x2 , x3;
    generate_test_data( x1 , x2 , x3 , -5.0 , 5.0 + 0.1 , 0.4 );

    // run test for several tree tests
    run_tree_type( "virtual tree" , x1 , x2 , x3 , argv[1] );

    return 0;
}


//
// Created by Alex Beccaro on 27/12/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/101-150/109/problem109.hpp"

BOOST_AUTO_TEST_SUITE( Problem109 )

    BOOST_AUTO_TEST_CASE( All ) {
        auto res = problems::problem109::solve(171);
        BOOST_CHECK_EQUAL(res, 42336);
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem109::solve();
        BOOST_CHECK_EQUAL(res, 38182);
    }

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#ifndef _GLOBAL_H_
#define _GLOBAL_H_

#include "std_msgs/String.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <dlib/gui_widgets.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/image_processing/render_face_detections.h>
#include <dlib/opencv.h>
// #include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <image_transport/image_transport.h>
#include <mutex>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
// #include <opencv2/opencv.hpp>
// #include <opencv2/core.hpp>
// #include <opencv2/imgproc.hpp>
// #include <opencv2/highgui.hpp>
// #include <opencv2/dnn.hpp>
#include <ros/package.h>
#include <ros/ros.h>
#include <sstream>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <string>
#include <vector>
#include <stdlib.h>

// ModelPoints.cpp
// 3D Model Points of selected landmarks in an arbitrary frame of reference
std::vector<cv::Point3d> get3dModelPoints();
// 2D landmark points from all landmarks
std::vector<cv::Point2d> get2dImagePoints(dlib::full_object_detection &d);

// MouthPose.cpp
static cv::Rect dlibRectangleToOpenCV(dlib::rectangle r);
static dlib::rectangle openCVRectToDlib(cv::Rect r) ;
void imageCallback(const sensor_msgs::ImageConstPtr &msg);

void publishMarker(float tx, float ty, float tz);
void DepthCallBack(const sensor_msgs::ImageConstPtr depth_img_ros);
void cameraInfo(const sensor_msgs::CameraInfoConstPtr &msg);
int main(int argc, char **argv);

// MouthStatusEstimation.cpp
std::vector<cv::Point2d> get2dmouthPoints(dlib::full_object_detection &d);
bool checkMouth(dlib::full_object_detection &shape);

// RenderFace.cpp
// Draw an open or closed polygon between
// start and end indices of full_object_detection
void drawPolyline(cv::Mat &img, const dlib::full_object_detection &landmarks,
                  const int start, const int end, bool isClosed = false);
// Draw face for the 68-point model.
void renderFace(cv::Mat &img, const dlib::full_object_detection &landmarks);
// Draw points on an image.
// Works for any number of points.
void renderFace(cv::Mat &img,                            // Image to draw the points on
                const std::vector<cv::Point2d> &points, // Vector of points
                cv::Scalar color,                       // color points
                int radius = 3);                        // Radius of points.

#endif  // _GLOBAL_H_


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompnot_alling file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [not_all_of]
#include <boost/simd/algorithm/not_all_of.hpp>
#include <boost/simd/function/is_gtz.hpp>
#include <iostream>
#include <vector>

int main()
{
  namespace bs =  boost::simd;
  std::vector<float> d{ 0.0f, 0.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f};

  auto rt1 = bs::not_all_of( d.data(), d.data()+7, bs::is_gtz );
  std::cout <<  "rt1 " << rt1 << "\n";
  auto rf1 = bs::not_all_of( d.data(), d.data()+7, bs::is_gez);
  std::cout << "rf1 " << rf1 << "\n";
  auto rt2 = bs::not_all_of( d.data()+1, d.data()+8);
  std::cout << "rt2 " << rt2 << "\n";
  auto rf2 = bs::not_all_of( d.data()+2, d.data()+9);
  std::cout << "rf2 " << rf2 << "\n";

  return 0;
}
//! [not_all_of]


// Copyright 2012 Daniel James.
// 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)

// This is also released into the public domain.

#include "siphash.hpp"
#include <boost/random/random_device.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/uniform_int.hpp>

namespace hash
{
    sipkey generate_sipkey()
    {
        boost::random_device rng;
        boost::variate_generator<boost::random_device&,
            boost::uniform_int<boost::uint64_t> > gen(rng,
                boost::uniform_int<boost::uint64_t>());

        sipkey k;
        k.k0 = gen();
        k.k1 = gen();
        return k;
    }
}


/*
 * MedianFillFilter.cpp
 *
 *  Created on: September 7, 2020
 *      Author: Magnus Gärtner
 *   Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_filters/MedianFillFilter.hpp"

// Wrap as ROS Filter
#include <pluginlib/class_list_macros.h>

// Median filtering with boost accumulators
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

// Grid Map
#include <grid_map_core/grid_map_core.hpp>

using namespace filters;

namespace grid_map {

template <typename T>
MedianFillFilter<T>::MedianFillFilter() : fillHoleRadius_(0.05), filterExistingValues_(false) {}

template <typename T>
MedianFillFilter<T>::~MedianFillFilter() = default;

template <typename T>
bool MedianFillFilter<T>::configure() {
  if (!FilterBase<T>::getParam(std::string("fill_hole_radius"), fillHoleRadius_)) {
    ROS_ERROR("Median filter did not find parameter fill_hole_radius.");
    return false;
  }

  if (fillHoleRadius_ < 0.0) {
    ROS_ERROR("fill_hole_radius must be greater than zero.");
    return false;
  }

  ROS_DEBUG("fill_hole_radius = %f.", fillHoleRadius_);

  if (!FilterBase<T>::getParam(std::string("filter_existing_values"), filterExistingValues_)) {
    ROS_INFO("Median filter did not find parameter filter_existing_values. Not filtering existing values.");
    filterExistingValues_ = false;
  }

  ROS_DEBUG("Filter_existing_values = %s.", filterExistingValues_ ? "true" : "false");

  if (!FilterBase<T>::getParam(std::string("input_layer"), inputLayer_)) {
    ROS_ERROR("Median filter did not find parameter `input_layer`.");
    return false;
  }

  if (filterExistingValues_) {
    if (!FilterBase<T>::getParam(std::string("existing_value_radius"), existingValueRadius_)) {
      ROS_ERROR("Median filter did not find parameter existing_value_radius.");
      return false;
    }

    if (existingValueRadius_ < 0.0) {
      ROS_ERROR("existing_value_radius must be greater than zero.");
      return false;
    }

    ROS_DEBUG("existing_value_radius = %f.", existingValueRadius_);
  }

  ROS_DEBUG("Median input layer is = %s.", inputLayer_.c_str());

  if (!FilterBase<T>::getParam(std::string("output_layer"), outputLayer_)) {
    ROS_ERROR("Median filter did not find parameter `output_layer`.");
    return false;
  }

  ROS_DEBUG("Median output layer = %s.", outputLayer_.c_str());

  return true;
}

template <typename T>
bool MedianFillFilter<T>::update(const T& mapIn, T& mapOut) {
  // Type definitions for readability.
  using boost::accumulators::accumulator_set;
  using boost::accumulators::count;
  using boost::accumulators::features;
  using boost::accumulators::median;
  using median_tag = boost::accumulators::tag::median;
  using count_tag = boost::accumulators::tag::count;
  using MedianAccumulatorT = accumulator_set<double, features<median_tag, count_tag> >;

  // Copy input map and add new layer to it.
  mapOut = mapIn;
  if (!mapOut.exists(outputLayer_)) {
    mapOut.add(outputLayer_);
  }
  mapOut.convertToDefaultStartIndex();

  const auto radiusInPixels = static_cast<size_t>(fillHoleRadius_ / mapIn.getResolution());
  const auto existingValueRadiusInPixels = static_cast<size_t>(existingValueRadius_ / mapIn.getResolution());
  const auto kernelLength = radiusInPixels * 2 + 1;
  const grid_map::Index& bufferSize = mapOut.getSize();

  grid_map::Matrix inputCopy;
  const grid_map::Matrix& inputData = (inputLayer_ == outputLayer_) ? inputCopy = mapOut[inputLayer_] : mapOut[inputLayer_];

  grid_map::Matrix& outputData = mapOut[outputLayer_];
  grid_map::Matrix& varianceData = mapOut["variance"];

  // Define some local functions.
  /*
   * @brief Returns the neighbourhood of a cell given the neighbourhood radius.
   * It checks the boundaries of the submap to compute the rectangular neighbourhood patch to iterate over.
   * @param inputData The data layer of the input.
   * @param index The center index of which we want the neighbourhood iterator.
   * @param radiusInPixels The number of pixels go in x / y direction from index to still be in the neighbourhood. I.e: ||neighbour -
   * index||_{inf} <= radiusInPixels.
   * @return A block view of the neighbourhood of the index cell, including itself.
   */
  const auto getNeighbourhood = [&bufferSize](Eigen::Ref<const Eigen::MatrixXf> inputData, const grid_map::Index& index,
                                              const int radiusInPixels) {
    // Bound the median window to the GridMap boundaries. Calculate the neighbour patch.
    grid_map::Index topLeftIndex = index - grid_map::Index(radiusInPixels, radiusInPixels);
    grid_map::Index bottomRightIndex = index + grid_map::Index(radiusInPixels, radiusInPixels);
    grid_map::boundIndexToRange(topLeftIndex, bufferSize);
    grid_map::boundIndexToRange(bottomRightIndex, bufferSize);
    grid_map::Index neighbourPatchSize = bottomRightIndex - topLeftIndex + grid_map::Index{1, 1};

    // Extract local neighbourhood.
    return inputData.block(topLeftIndex(0), topLeftIndex(1), neighbourPatchSize(0), neighbourPatchSize(1));
  };

  const auto accumulateFinitesInNeighbourhood = [](Eigen::Ref<const Eigen::MatrixXf> neighbourhood, MedianAccumulatorT& accumulator) {
    // Iterate over the local neighbourhood.
    size_t cols = neighbourhood.cols();
    for (size_t row = 0; row < neighbourhood.rows(); ++row) {
      const auto& currentRow = neighbourhood.row(row);
      for (size_t col = 0; col < cols; ++col) {
        const float& neighbourValue = currentRow[col];
        if (std::isfinite(neighbourValue)) {  // Calculate the median of the finite neighbours.
          accumulator(neighbourValue);
        }
      }
    }
  };

  unsigned int numNans = 0;
  // Iterate through the entire GridMap and update NaN values with the median.
  for (GridMapIterator iterator(mapOut); !iterator.isPastEnd(); ++iterator) {
    const Index index(*iterator);
    const auto& inputValue = inputData(index(0), index(1));
    auto& outputValue = outputData(index(0), index(1));
    auto& variance = varianceData(index(0), index(1));

    if (not std::isfinite(inputValue)) {  // Fill the NaN input value with the median.
      MedianAccumulatorT medianAccumulator;
      Eigen::Ref<const Eigen::MatrixXf> neighbourhood = getNeighbourhood(inputData, index, radiusInPixels);
      accumulateFinitesInNeighbourhood(neighbourhood, medianAccumulator);

      if (count(medianAccumulator) >= 1.0f) {  // If we have at least one valid input value in the neighbourhood we assign the median.
        outputValue = median(medianAccumulator);
        variance = 0.1;
      }
      ++numNans;
    } else if (filterExistingValues_) {  // Value is already finite. Optionally add some filtering.
      MedianAccumulatorT medianAccumulator;
      Eigen::Ref<const Eigen::MatrixXf> neighbourhood = getNeighbourhood(inputData, index, existingValueRadiusInPixels);
      accumulateFinitesInNeighbourhood(neighbourhood, medianAccumulator);

      if (count(medianAccumulator) >= 1.0f) {
        outputValue = median(medianAccumulator);
      }

    } else {  // Dont do any filtering, just take the input value.
      outputValue = inputValue;
    }
  }
  ROS_DEBUG_STREAM("Median fill filter " << this->getName() << " observed " << numNans << " in input layer!");
  return true;
}

}  // namespace grid_map

// Explicitly define the specialization for GridMap to have the filter implementation available for testing.
template class grid_map::MedianFillFilter<grid_map::GridMap>;
// Export the filter.
PLUGINLIB_EXPORT_CLASS(grid_map::MedianFillFilter<grid_map::GridMap>, filters::FilterBase<grid_map::GridMap>)


//
//  Rational.hpp
//
//
//  https://gist.github.com/sklaw/10473569
//

#ifndef F2_H
#define F2_H

#include <iostream>
#include <stdint.h>
#include <Eigen/Dense>
#include "../basic.h"

class F2 {
    friend F2 operator+(const F2& left, const F2& right);
    friend F2 operator-(const F2& left, const F2& right);
    friend F2 operator*(const F2& left, const F2& right);
    friend F2 operator/(const F2& left, const F2& right);
    friend bool operator==(const F2& left, const F2& right);
    friend bool operator!=(const F2& left, const F2& right);
    friend bool operator<(const F2& left, const F2& right);
    friend bool operator>(const F2& left, const F2& right);
    friend bool operator<=(const F2& left, const F2& right);
    friend bool operator>=(const F2& left, const F2& right);
    friend std::ostream& operator<<(std::ostream& out, const F2& obj);
    friend std::istream& operator>>(std::istream& in, F2& obj);

public:
    F2(): value(0) {}
    F2(int_t x): value(x) {}

    F2& operator=(const F2& obj);
    F2& operator+=(const F2& obj);
    F2& operator-=(const F2& obj);
    F2& operator*=(const F2& obj);
    F2& operator/=(const F2& obj);
    F2 operator+() const;
    F2 operator-() const;

    uint8_t getValue() const { return value; }

private:
    uint8_t value; // 0 or 1
};

namespace Eigen {
template<> struct NumTraits<F2>
 : NumTraits<int_t> // permits to get the epsilon, dummy_precision, lowest, highest functions
{
  typedef F2 Real;
  typedef F2 NonInteger;
  typedef F2 Nested;
  enum {
    IsComplex = 0,
    IsInteger = 0,
    IsSigned = 0,
    RequireInitialization = 0,
    ReadCost = 1,
    AddCost = 2,
    MulCost = 2
  };
    static inline Real epsilon() { return 0; }
    static inline Real dummy_precision() { return 0; }
};
}

inline F2 abs(const F2&  x)  { return x; }
inline F2 abs2(const F2& x)  { return x*x; }

#endif


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/map.hpp>

#include <boost/hana/detail/assert.hpp>
#include <boost/hana/detail/minimal/product.hpp>
#include <boost/hana/integral.hpp>
using namespace boost::hana;

template <int i>
constexpr auto key = int_<i>;

template <int i>
constexpr auto value = int_<i + 100>;

template <int i, int j>
constexpr auto p = detail::minimal::product<>(key<i>, value<j>);

int main() {
    BOOST_HANA_CONSTANT_ASSERT( equal(map(), map()));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>), map()));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(), map(p<1, 1>)));

    BOOST_HANA_CONSTANT_ASSERT( equal(map(p<1, 1>), map(p<1, 1>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>), map(p<1, 2>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>), map(p<2, 1>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>), map(p<1, 1>, p<2, 2>)));

    BOOST_HANA_CONSTANT_ASSERT( equal(map(p<1, 1>, p<2, 2>), map(p<1, 1>, p<2, 2>)));
    BOOST_HANA_CONSTANT_ASSERT( equal(map(p<1, 1>, p<2, 2>), map(p<2, 2>, p<1, 1>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>, p<2, 2>), map(p<9, 1>, p<2, 2>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>, p<2, 2>), map(p<1, 9>, p<2, 2>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>, p<2, 2>), map(p<1, 1>, p<9, 2>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>, p<2, 2>), map(p<1, 1>, p<2, 9>)));
    BOOST_HANA_CONSTANT_ASSERT(!equal(map(p<1, 1>, p<2, 2>), map(p<1, 1>, p<2, 2>, p<3, 3>)));
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_INTERLEAVE_EVEN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_INTERLEAVE_EVEN_HPP_INCLUDED

namespace boost { namespace simd
{
#if defined(DOXYGEN_ONLY)
  /*!
    @ingroup group-swar
    This function object computes a vector from a combination of the two inputs.

    @par Header <boost/simd/function/interleave_even.hpp>

    @par Semantic:

    For every parameters of types respectively T0:

    @code
    T0 r = interleave_even(x,y);
    @endcode

    is equivalent to :

    @code
    r = [ x[0] y[0] x[2] y[2] ... x[n/2] y[n/2] ]
    @endcode

    with <tt> n = cardinal_of<T>::value </tt>

  **/
  Value interleave_even(Value const& x, Value const& y);
#endif
} }

#include <boost/simd/function/simd/interleave_even.hpp>

#endif