// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include "opaque_types.h"
#include <dlib/python.h>
#include "testing_results.h"
#include <dlib/svm.h>

using namespace dlib;
using namespace std;

namespace py = pybind11;

typedef matrix<double,0,1> sample_type;
typedef std::vector<std::pair<unsigned long,double> > sparse_vect;

template <typename decision_function>
double predict (
    const decision_function& df,
    const typename decision_function::kernel_type::sample_type& samp
)
{
    typedef typename decision_function::kernel_type::sample_type T;
    if (df.basis_vectors.size() == 0)
    {
        return 0;
    }
    else if (is_matrix<T>::value && df.basis_vectors(0).size() != samp.size())
    {
        std::ostringstream sout;
        sout << "Input vector should have " << df.basis_vectors(0).size()
             << " dimensions, not " << samp.size() << ".";
        PyErr_SetString( PyExc_ValueError, sout.str().c_str() );
        throw py::error_already_set();
    }
    return df(samp);
}

template <typename kernel_type>
void add_df (
    py::module& m,
    const std::string name
)
{
    typedef decision_function<kernel_type> df_type;
    py::class_<df_type>(m, name.c_str())
        .def("__call__", &predict<df_type>)
        .def(py::pickle(&getstate<df_type>, &setstate<df_type>));
}

template <typename df_type>
typename df_type::sample_type get_weights(
    const df_type& df
)
{
    if (df.basis_vectors.size() == 0)
    {
        PyErr_SetString( PyExc_ValueError, "Decision function is empty." );
        throw py::error_already_set();
    }
    df_type temp = simplify_linear_decision_function(df);
    return temp.basis_vectors(0);
}

template <typename df_type>
typename df_type::scalar_type get_bias(
    const df_type& df
)
{
    if (df.basis_vectors.size() == 0)
    {
        PyErr_SetString( PyExc_ValueError, "Decision function is empty." );
        throw py::error_already_set();
    }
    return df.b;
}

template <typename df_type>
void set_bias(
    df_type& df,
    double b
)
{
    if (df.basis_vectors.size() == 0)
    {
        PyErr_SetString( PyExc_ValueError, "Decision function is empty." );
        throw py::error_already_set();
    }
    df.b = b;
}

template <typename kernel_type>
void add_linear_df (
    py::module &m,
    const std::string name
)
{
    typedef decision_function<kernel_type> df_type;
    py::class_<df_type>(m, name.c_str())
        .def("__call__", predict<df_type>)
        .def_property_readonly("weights", &get_weights<df_type>)
        .def_property("bias", get_bias<df_type>, set_bias<df_type>)
        .def(py::pickle(&getstate<df_type>, &setstate<df_type>));
}

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

std::string binary_test__str__(const binary_test& item)
{
    std::ostringstream sout;
    sout << "class1_accuracy: "<< item.class1_accuracy << "  class2_accuracy: "<< item.class2_accuracy;
    return sout.str();
}
std::string binary_test__repr__(const binary_test& item) { return "< " + binary_test__str__(item) + " >";}

std::string regression_test__str__(const regression_test& item)
{
    std::ostringstream sout;
    sout << "mean_squared_error: "<< item.mean_squared_error << "  R_squared: "<< item.R_squared;
    sout << "  mean_average_error: "<< item.mean_average_error << "  mean_error_stddev: "<< item.mean_error_stddev;
    return sout.str();
}
std::string regression_test__repr__(const regression_test& item) { return "< " + regression_test__str__(item) + " >";}

std::string ranking_test__str__(const ranking_test& item)
{
    std::ostringstream sout;
    sout << "ranking_accuracy: "<< item.ranking_accuracy << "  mean_ap: "<< item.mean_ap;
    return sout.str();
}
std::string ranking_test__repr__(const ranking_test& item) { return "< " + ranking_test__str__(item) + " >";}

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

template <typename K>
binary_test  _test_binary_decision_function (
    const decision_function<K>& dec_funct,
    const std::vector<typename K::sample_type>& x_test,
    const std::vector<double>& y_test
) { return binary_test(test_binary_decision_function(dec_funct, x_test, y_test)); }

template <typename K>
regression_test _test_regression_function (
    const decision_function<K>& reg_funct,
    const std::vector<typename K::sample_type>& x_test,
    const std::vector<double>& y_test
) { return regression_test(test_regression_function(reg_funct, x_test, y_test)); }

template < typename K >
ranking_test _test_ranking_function1 (
    const decision_function<K>& funct,
    const std::vector<ranking_pair<typename K::sample_type> >& samples
) { return ranking_test(test_ranking_function(funct, samples)); }

template < typename K >
ranking_test _test_ranking_function2 (
    const decision_function<K>& funct,
    const ranking_pair<typename K::sample_type>& sample
) { return ranking_test(test_ranking_function(funct, sample)); }

void bind_decision_functions(py::module &m)
{
    add_linear_df<linear_kernel<sample_type> >(m, "_decision_function_linear");
    add_linear_df<sparse_linear_kernel<sparse_vect> >(m, "_decision_function_sparse_linear");

    add_df<histogram_intersection_kernel<sample_type> >(m, "_decision_function_histogram_intersection");
    add_df<sparse_histogram_intersection_kernel<sparse_vect> >(m, "_decision_function_sparse_histogram_intersection");

    add_df<polynomial_kernel<sample_type> >(m, "_decision_function_polynomial");
    add_df<sparse_polynomial_kernel<sparse_vect> >(m, "_decision_function_sparse_polynomial");

    add_df<radial_basis_kernel<sample_type> >(m, "_decision_function_radial_basis");
    add_df<sparse_radial_basis_kernel<sparse_vect> >(m, "_decision_function_sparse_radial_basis");

    add_df<sigmoid_kernel<sample_type> >(m, "_decision_function_sigmoid");
    add_df<sparse_sigmoid_kernel<sparse_vect> >(m, "_decision_function_sparse_sigmoid");

    m.def("test_binary_decision_function", _test_binary_decision_function<linear_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sparse_linear_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<radial_basis_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sparse_radial_basis_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<polynomial_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sparse_polynomial_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<histogram_intersection_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sparse_histogram_intersection_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sigmoid_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));
    m.def("test_binary_decision_function", _test_binary_decision_function<sparse_sigmoid_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("labels"));

    m.def("test_regression_function", _test_regression_function<linear_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sparse_linear_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<radial_basis_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sparse_radial_basis_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<histogram_intersection_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sparse_histogram_intersection_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sigmoid_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sparse_sigmoid_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<polynomial_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));
    m.def("test_regression_function", _test_regression_function<sparse_polynomial_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"), py::arg("targets"));

    m.def("test_ranking_function", _test_ranking_function1<linear_kernel<sample_type> >,
        py::arg("function"), py::arg("samples"));
    m.def("test_ranking_function", _test_ranking_function1<sparse_linear_kernel<sparse_vect> >,
        py::arg("function"), py::arg("samples"));
    m.def("test_ranking_function", _test_ranking_function2<linear_kernel<sample_type> >,
        py::arg("function"), py::arg("sample"));
    m.def("test_ranking_function", _test_ranking_function2<sparse_linear_kernel<sparse_vect> >,
        py::arg("function"), py::arg("sample"));

    py::class_<binary_test>(m, "_binary_test")
        .def("__str__", binary_test__str__)
        .def("__repr__", binary_test__repr__)
        .def_readwrite("class1_accuracy", &binary_test::class1_accuracy,
            "A value between 0 and 1, measures accuracy on the +1 class.")
        .def_readwrite("class2_accuracy", &binary_test::class2_accuracy,
            "A value between 0 and 1, measures accuracy on the -1 class.");

    py::class_<ranking_test>(m, "_ranking_test")
        .def("__str__", ranking_test__str__)
        .def("__repr__", ranking_test__repr__)
        .def_readwrite("ranking_accuracy", &ranking_test::ranking_accuracy,
            "A value between 0 and 1, measures the fraction of times a relevant sample was ordered before a non-relevant sample.")
        .def_readwrite("mean_ap", &ranking_test::mean_ap,
            "A value between 0 and 1, measures the mean average precision of the ranking.");

    py::class_<regression_test>(m, "_regression_test")
        .def("__str__", regression_test__str__)
        .def("__repr__", regression_test__repr__)
        .def_readwrite("mean_average_error", &regression_test::mean_average_error,
            "The mean average error of a regression function on a dataset.")
        .def_readwrite("mean_error_stddev", &regression_test::mean_error_stddev,
            "The standard deviation of the absolute value of the error of a regression function on a dataset.")
        .def_readwrite("mean_squared_error", &regression_test::mean_squared_error,
            "The mean squared error of a regression function on a dataset.")
        .def_readwrite("R_squared", &regression_test::R_squared,
            "A value between 0 and 1, measures the squared correlation between the output of a \n"
            "regression function and the target values.");
}


#include <iostream>
#include <random>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Main
#include <boost/test/unit_test.hpp>

#include <iostream>
#include <algorithm>

#include "invertedindex.h"

using namespace Akumuli;

BOOST_AUTO_TEST_CASE(Test_inverted_index_0) {
    InvertedIndex index(1024);

    for (int i = 0; i < 1000; i++) {
        char buffer[128];
        int n = snprintf(buffer, 128, "%d", i);
        index.append(i, buffer, buffer + n);
    }

    for (int i = 0; i < 1000; i++) {
        char buffer[128];
        int n = snprintf(buffer, 128, "%d", i);
        auto results = index.get_count(buffer, buffer + n);
        // shouldn't be any conflicts with this setup
        BOOST_REQUIRE_EQUAL(results.size(), 1u);
        BOOST_REQUIRE_EQUAL(results.at(0).first, i);
        BOOST_REQUIRE_EQUAL(results.at(0).second, 1);
    }
}


// Copyright Jason Rice 2020
// 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/define_struct.hpp>
#include <boost/hana/equal.hpp>
#include <boost/hana/members.hpp>
#include <boost/hana/not_equal.hpp>
namespace hana = boost::hana;

struct SomeStruct {
    BOOST_HANA_DEFINE_STRUCT(SomeStruct, (int, x));

    constexpr bool operator==(SomeStruct const& other) {
        return hana::equal(hana::members(*this), hana::members(other));
    }

    constexpr bool operator!=(SomeStruct const& other) {
        return hana::not_equal(hana::members(*this), hana::members(other));
    }
};

int main() {
    static_assert(SomeStruct{5} == SomeStruct{5}, "");
    static_assert(hana::equal(SomeStruct{5}, SomeStruct{5}), "");
}


#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <handy_tools/pid_controller.h>
#include <nav_msgs/Path.h>
#include <ros/ros.h>
#include <tf2/utils.h>
#include <trajectory_msgs/JointTrajectory.h>
#include <Eigen/Eigen>
#include <chrono>

const float YAW_PID_P{0.2};
const float YAW_PID_I{0.02};
const float YAW_PID_D{0.1};

template <typename T>
  inline bool safeGetParam(ros::NodeHandle &_nh, std::string const &_param_name,
                           T &_param_value) {
    if (!_nh.getParam(_param_name, _param_value)) {
      ROS_ERROR("Failed to find parameter: %s",
                _nh.resolveName(_param_name, true).c_str());
      exit(1);
    }
    return true;
  }

struct State {
  Eigen::Vector3f position{0, 0, 0};
  Eigen::Quaternionf orientation{0, 0, 0, 0};
  Eigen::Vector3f vel{0, 0, 0};
  float time{0.0};
  float real_time{0.0};
};

struct Trajectory {
  std::vector<State> trajectory;

  void calculateTimes(const float step_size) {
    for (int i = 0; i < trajectory.size(); i++) {
      trajectory[i].time = i * step_size;
    }
  }

  bool calculateTimes() {
    if (trajectory.empty()) {
      return false;
    }
    trajectory[0].time = 0.0;
    for (int i = 0; i < trajectory.size() - 1; i++) {
      trajectory[i].time =
          (trajectory[i + 1].position - trajectory[i].position).norm() /
          trajectory[i].vel.norm();
    }
  }
};

struct Follower {
  double look_ahead{1.0};
  int pose_on_path{0};
  const float rate{0.01};  // sec
  float step_size{0.2};
  Trajectory *previous_trajectory = (Trajectory*)malloc(sizeof(Trajectory) * 2);

  //std::chrono::time_point<std::chrono::high_resolution_clock> time_last_traj;
  ros::Time time_last_traj;

  float calculateYawDiff(const float _desired_yaw, const float _current_yaw);
  int cal_pose_on_path(const std::vector<State> &trajectory,
                       const Eigen::Vector3f &current_pose);
  int cal_pose_look_ahead(const std::vector<State> &trajectory);
  Eigen::Vector3f calculate_vel(const Eigen::Vector3f &target_pose,
                                const Eigen::Vector3f &current_pose,
                                const float target_time);
  void calculate_look_ahead(const float &vel);
};

int main(int _argc, char **_argv) {
  ros::init(_argc, _argv, "trajectory_follower_node");
  ros::NodeHandle nh;
  ros::NodeHandle pnh("~");

  Trajectory last_traj_received;
  State uav_state;
  Follower follower;
  safeGetParam(pnh,"look_ahead", follower.look_ahead);
  safeGetParam(pnh,"step_size", follower.step_size);
  //// ROS publishers and subscribers //////////
  ros::Publisher tracking_pub =
      nh.advertise<nav_msgs::Path>("follower/trajectory_to_follow", 1);

  // trajectory callback
  auto trajectoryCallback =
      [&last_traj_received, &follower,
       tracking_pub](const trajectory_msgs::JointTrajectory::ConstPtr &msg) {
        // last_traj_received.trajectory.resize(msg->points.size());
        follower.time_last_traj = ros::Time::now();
        follower.pose_on_path = 0;
        geometry_msgs::PoseStamped pose_stamped;
        nav_msgs::Path path_to_publish;
        path_to_publish.header.frame_id = "map";
        float time_point;
        int start_i = 0;
        int i = 0;
        int p = 0;

        // SECURITY MEASURE: Check times of the new trajectory in order to not execute
        // points of the past
        for (int it = 0; it < msg->points.size(); it++){
          if (follower.time_last_traj.toSec() < msg->points[it].time_from_start.toSec()){
            start_i = it;
            // std::cout << "Start i: " << start_i << " with time: " << msg->points[i].time_from_start.toSec() << std::endl;
            break;
          }
        }

        // std::cout << "Time last traj: " << follower.time_last_traj << std::endl;
        // std::cout << "Time of the first point of the trajectory: " << msg->points[0].time_from_start << std::endl;

        // SECURITY MEASURE: New trajectory received earlier than expected and the
        // points to execute are planned for the future

        // Adding to path_to_publish the points of the previous trajectory
        // (from current time to the first point of the new trajectory received)

        // std::cout << "Size: " << follower.previous_trajectory->trajectory.size() << std::endl;
        // If previous trajectory is not empty
        if (!follower.previous_trajectory->trajectory.empty() &&
            (abs(follower.previous_trajectory->trajectory[0].position.norm() -
            follower.previous_trajectory->trajectory[follower.previous_trajectory->trajectory.size() - 1].position.norm()) < 0.1)){
          // If the trajectory received is not from the past
          if (start_i == 0){
            // std::cout << "start_i = 0!" << std::endl;
            int i_p = follower.previous_trajectory->trajectory.size() - 1;

            std::cout << "Current time: " << follower.time_last_traj.toSec() << std::endl;
            // std::cout << "First time of previous trajectory: " << follower.previous_trajectory->trajectory[0].real_time << std::endl;
            // std::cout << "Last time of previous trajectory: " << follower.previous_trajectory->trajectory[i_p].real_time << std::endl;

            // Get the closer point in time (staring to the future) of the previous trajectory
            // If the time of that point is greater or equal to the first point of the new trajectory, break
            while ((follower.previous_trajectory->trajectory[i_p].real_time > (follower.time_last_traj.toSec() + follower.step_size)) && (i_p >= 0)){
              i_p = i_p - 1;
            }

            // std::cout << "i_p: " << i_p << std::endl;
            std::cout << "Time for that i_p: " << follower.previous_trajectory->trajectory[i_p].real_time << std::endl;

            std::cout << "Time of the first point of the traj received: " << msg->points[0].time_from_start.toSec() << std::endl;

            if (i_p > 0) {
              for (int k = i_p;
                  (k < follower.previous_trajectory->trajectory.size()) &&
                  ((follower.previous_trajectory->trajectory[k].real_time + follower.step_size) < msg->points[0].time_from_start.toSec());
                  k++, p++){
                pose_stamped.header.stamp.sec  = int(follower.previous_trajectory->trajectory[k].real_time);
                pose_stamped.header.stamp.nsec = int( (follower.previous_trajectory->trajectory[k].real_time -
                                                      pose_stamped.header.stamp.sec)*1000000000 );

                pose_stamped.pose.position.x = follower.previous_trajectory->trajectory[k].position(0);
                pose_stamped.pose.position.y = follower.previous_trajectory->trajectory[k].position(1);
                pose_stamped.pose.position.z = follower.previous_trajectory->trajectory[k].position(2);

                last_traj_received.trajectory[p].position[0] =
                    follower.previous_trajectory->trajectory[k].position(0);
                last_traj_received.trajectory[p].position[1] =
                    follower.previous_trajectory->trajectory[k].position(1);
                last_traj_received.trajectory[p].position[2] =
                    follower.previous_trajectory->trajectory[k].position(2);

                last_traj_received.trajectory[p].orientation.x() =
                    follower.previous_trajectory->trajectory[k].orientation.x();
                last_traj_received.trajectory[p].orientation.y() =
                    follower.previous_trajectory->trajectory[k].orientation.y();
                last_traj_received.trajectory[p].orientation.z() =
                    follower.previous_trajectory->trajectory[k].orientation.z();
                last_traj_received.trajectory[p].orientation.w() =
                    follower.previous_trajectory->trajectory[k].orientation.w();

                last_traj_received.trajectory[p].vel[0] =
                    follower.previous_trajectory->trajectory[k].vel(0);
                last_traj_received.trajectory[p].vel[1] =
                    follower.previous_trajectory->trajectory[k].vel(1);
                last_traj_received.trajectory[p].vel[2] =
                    follower.previous_trajectory->trajectory[k].vel(2);

                last_traj_received.trajectory[p].real_time =
                    follower.previous_trajectory->trajectory[k].real_time;

                std::cout << "k: " << k << "   time: " << follower.previous_trajectory->trajectory[k].real_time << std::endl;

                path_to_publish.poses.push_back(pose_stamped);
              }
              std::cout << "Added " << p << " points from previous trajectory" << std::endl;
            }
          }
        }

        last_traj_received.trajectory.resize(msg->points.size() + p);

        // Adding to path_to_publish the points of the trajectory received
        for (int j = start_i; j < msg->points.size(); i++, j++) {

          // std::cout << "i: " << i << "   j: " << j << std::endl;

          time_point = msg->points[j].time_from_start.toSec();
          pose_stamped.header.stamp.sec  = int(time_point);
          pose_stamped.header.stamp.nsec = int( (time_point - int(time_point))*1000000000 );

          // Check
          // std::cout << "  Total: " << follower.time_last_traj.toSec() << std::endl << std::endl;
          pose_stamped.pose.position.x = msg->points[j].positions[0];
          pose_stamped.pose.position.y = msg->points[j].positions[1];
          pose_stamped.pose.position.z = msg->points[j].positions[2];

          last_traj_received.trajectory[j+p].position[0] =
              msg->points[j].positions[0];
          last_traj_received.trajectory[j+p].position[1] =
              msg->points[j].positions[1];
          last_traj_received.trajectory[j+p].position[2] =
              msg->points[j].positions[2];

          last_traj_received.trajectory[j+p].orientation.x() =
              msg->points[j].positions[3];
          last_traj_received.trajectory[j+p].orientation.y() =
              msg->points[j].positions[4];
          last_traj_received.trajectory[j+p].orientation.z() =
              msg->points[j].positions[5];
          last_traj_received.trajectory[j+p].orientation.w() =
              msg->points[j].positions[6];

          last_traj_received.trajectory[j+p].vel[0] =
              msg->points[j].velocities[0];
          last_traj_received.trajectory[j+p].vel[1] =
              msg->points[j].velocities[1];
          last_traj_received.trajectory[j+p].vel[2] =
              msg->points[j].velocities[2];

          last_traj_received.trajectory[j+p].real_time =
              msg->points[j].time_from_start.toSec();

          path_to_publish.poses.push_back(pose_stamped);
        }

        last_traj_received.calculateTimes(follower.step_size);

        // Refresh the previous trajectory and publish
        follower.previous_trajectory->trajectory.clear();
        std::memcpy(follower.previous_trajectory, &last_traj_received, sizeof(last_traj_received));
        tracking_pub.publish(path_to_publish);
      };

  auto sub_traj = pnh.subscribe<trajectory_msgs::JointTrajectory>(
      "trajectory_to_follow", 1, trajectoryCallback);
  // ual state subscriber
  auto ualPoseCallback =
      [&uav_state](const geometry_msgs::PoseStamped::ConstPtr &msg) {
        uav_state.position = Eigen::Vector3f(
            msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
        uav_state.orientation.x() = msg->pose.orientation.x;
        uav_state.orientation.y() = msg->pose.orientation.y;
        uav_state.orientation.z() = msg->pose.orientation.z;
        uav_state.orientation.w() = msg->pose.orientation.w;
      };
  auto pose_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("ual/pose", 1, ualPoseCallback);

  ros::Publisher velocity_ual_pub =
      nh.advertise<geometry_msgs::TwistStamped>("ual/set_velocity", 1);
  /////////////////////////
  // PID controller for yaw
  grvc::utils::PidController yaw_pid("yaw", YAW_PID_P, YAW_PID_I, YAW_PID_D);

  /////// main loop   //////////////

  // Declare and initialize velocity_to_command
  Eigen::Vector3f velocity_to_command(0.2, 0.2, 0);
  while (ros::ok) {

    // wait for receiving trajectories
    while (!last_traj_received.trajectory.empty()) {
      follower.pose_on_path = follower.cal_pose_on_path(
          last_traj_received.trajectory, uav_state.position);

      if (velocity_to_command.norm() > 0.1){
        follower.calculate_look_ahead(velocity_to_command.norm());
      }
      else{
        safeGetParam(pnh,"look_ahead", follower.look_ahead);
      }
      int target_pose_idx =
          follower.cal_pose_look_ahead(last_traj_received.trajectory);
      // if the point to go is out of the trajectory, the trajectory will be
      // finished and cleared
      if (target_pose_idx == last_traj_received.trajectory.size()) {
        last_traj_received.trajectory.clear();
        follower.pose_on_path = 0;
        break;
      }
      Eigen::Vector3f target_pose =
          last_traj_received.trajectory[target_pose_idx].position;

      // Use ROS::Time::now() instead of std::chrono
      velocity_to_command = follower.calculate_vel(
          target_pose, uav_state.position,
          last_traj_received.trajectory[target_pose_idx].time);

      // controlling yaw
      tf2::Quaternion desired_q(
          last_traj_received.trajectory[target_pose_idx].orientation.x(),
          last_traj_received.trajectory[target_pose_idx].orientation.y(),
          last_traj_received.trajectory[target_pose_idx].orientation.z(),
          last_traj_received.trajectory[target_pose_idx].orientation.w());
      tf2::Quaternion current_q(
          uav_state.orientation.x(), uav_state.orientation.y(),
          uav_state.orientation.z(), uav_state.orientation.w());

      float current_yaw = tf2::getYaw(current_q);
      float desired_yaw = tf2::getYaw(desired_q);
      float yaw_diff = follower.calculateYawDiff(desired_yaw, current_yaw);
      float sampling_period = follower.rate;

      // publish topic to ual
      geometry_msgs::TwistStamped vel;
      vel.header.frame_id = "map";
      vel.twist.linear.x = velocity_to_command.x();
      vel.twist.linear.y = velocity_to_command.y();
      vel.twist.linear.z = velocity_to_command.z();
      vel.twist.angular.z = yaw_pid.control_signal(yaw_diff, sampling_period);

      velocity_ual_pub.publish(vel);
      ros::spinOnce();
      ros::Duration(follower.rate).sleep();
    }
    ros::Duration(0.1).sleep();
    ros::spinOnce();
  }
  return 0;
}

float Follower::calculateYawDiff(const float _desired_yaw,
                                 const float _current_yaw) {
  float yaw_diff = _desired_yaw - _current_yaw;
  while (yaw_diff < -M_PI) yaw_diff += 2 * M_PI;
  while (yaw_diff > M_PI) yaw_diff -= 2 * M_PI;
  return yaw_diff;
}

int Follower::cal_pose_on_path(const std::vector<State> &trajectory,
                               const Eigen::Vector3f &current_pose) {
  double min_distance = INFINITY;
  int pose_on_path_id = 0;
  for (int i = pose_on_path; i < trajectory.size(); i++) {
    if ((current_pose - trajectory[i].position).norm() < min_distance) {
      min_distance = (current_pose - trajectory[i].position).norm();
      pose_on_path_id = i;
    }
  }
  return pose_on_path_id;
}

int Follower::cal_pose_look_ahead(const std::vector<State> &trajectory) {
  for (int i = pose_on_path; i < trajectory.size(); i++) {
    if ((trajectory[i].position - trajectory[pose_on_path].position).norm() >
        look_ahead)
      return i;
  }
  return trajectory.size();
}

Eigen::Vector3f Follower::calculate_vel(const Eigen::Vector3f &target_pose,
                                        const Eigen::Vector3f &current_pose,
                                        const float target_time) {
  ros::Time end = ros::Time::now();
  ros::Duration current_time_ros = end - time_last_traj;
  float current_time = current_time_ros.toSec();

  Eigen::Vector3f vel_unitary = (target_pose - current_pose).normalized();
  double vel_module = (target_pose - current_pose).norm() /

                      (target_time - current_time);

  // std::cout << "Vel: " << vel_module << std::endl;
  // std::cout << "Diff time: " << (target_time - current_time) << std::endl;
  if(vel_module<0) {
	std::cerr<<"vel negative\n";
	vel_module = 0.0;	}
  return vel_unitary * vel_module;
}

void Follower::calculate_look_ahead(const float &vel){
  look_ahead = vel*3*step_size;

  // std::cout << "Look ahead: " << look_ahead << std::endl;
}


#ifndef CSLIBS_MAPPING_SAVE_MAP_HPP
#define CSLIBS_MAPPING_SAVE_MAP_HPP

#include <cslibs_math_2d/linear/pose.hpp>
#include <nav_msgs/Path.h>

#include <cslibs_math_2d/serialization/transform.hpp>
#include <cslibs_math_ros/sensor_msgs/conversion_2d.hpp>
#include <cslibs_math_ros/geometry_msgs/conversion_2d.hpp>
#include <cslibs_math/common/equal.hpp>

#include <boost/filesystem.hpp>
#include <yaml-cpp/yaml.h>
#include <fstream>

namespace cslibs_mapping {
namespace mapper {
template <typename Tp, typename T>
inline bool saveMap(
        const std::string                &path,
        const nav_msgs::Path::Ptr        &poses,
        const std::vector<T>             &occ_data,
        const std::size_t                &occ_height,
        const std::size_t                &occ_width,
        const cslibs_math_2d::Pose2<Tp>  &occ_origin,
        const Tp                         &occ_resolution,
        const T                          &occ_free_threshold     = 0.169,
        const T                          &occ_occupied_threshold = 0.65)
{
    const std::string occ_path_yaml        = (path / boost::filesystem::path("occ.map.yaml")).       string();
    const std::string occ_path_pgm_rel     = "occ.map.pgm";
    const std::string occ_path_pgm         = (path / boost::filesystem::path(occ_path_pgm_rel)).     string();
    const std::string occ_path_raw_yaml    = (path / boost::filesystem::path("occ.map.raw.yaml")).   string();
    const std::string occ_path_raw_pgm_rel = "occ.map.raw.pgm";
    const std::string occ_path_raw_pgm     = (path / boost::filesystem::path(occ_path_raw_pgm_rel)). string();
    const std::string poses_path_yaml      = (path / boost::filesystem::path("poses.yaml")).         string();

    // map header
    {
        std::ofstream occ_out_yaml(occ_path_yaml);
        if (!occ_out_yaml.is_open()) {
            std::cout << "[SaveMap]: Could not open file '" << occ_path_yaml << "'." << std::endl;
            return false;
        }
        /// write occupancy map meta data
        YAML::Emitter occ_yaml(occ_out_yaml);
        occ_yaml << YAML::BeginMap;
        occ_yaml << YAML::Key   << "image";
        occ_yaml << YAML::Value << occ_path_pgm_rel;
        occ_yaml << YAML::Key   << "resolution";
        occ_yaml << YAML::Value << occ_resolution;
        occ_yaml << YAML::Key   << "origin";
        occ_yaml << YAML::Flow;
        occ_yaml << YAML::BeginSeq << occ_origin.tx() << occ_origin.ty() << occ_origin.yaw() << YAML::EndSeq;
        occ_yaml << YAML::Key   << "occupied_thresh";
        occ_yaml << YAML::Value << occ_occupied_threshold;
        occ_yaml << YAML::Key   << "free_thresh";
        occ_yaml << YAML::Value << occ_free_threshold;
        occ_yaml << YAML::Key   << "negate";
        occ_yaml << YAML::Value << 0;
        occ_yaml << YAML::EndMap;
        occ_out_yaml.close();
    }

    // raw map header
    {
        std::ofstream occ_out_yaml_raw(occ_path_raw_yaml);
        if (!occ_out_yaml_raw.is_open()) {
            std::cout << "[SaveMap]: Could not open file '" << occ_path_raw_yaml << "'." << std::endl;
            return false;
        }
        /// write occupancy map meta data
        YAML::Emitter occ_yaml_raw(occ_out_yaml_raw);
        occ_yaml_raw << YAML::BeginMap;
        occ_yaml_raw << YAML::Key   << "image";
        occ_yaml_raw << YAML::Value << occ_path_raw_pgm_rel;
        occ_yaml_raw << YAML::Key   << "resolution";
        occ_yaml_raw << YAML::Value << occ_resolution;
        occ_yaml_raw << YAML::Key   << "origin";
        occ_yaml_raw << YAML::Flow;
        occ_yaml_raw << YAML::BeginSeq << occ_origin.tx() << occ_origin.ty() << occ_origin.yaw() << YAML::EndSeq;
        occ_yaml_raw << YAML::Key   << "occupied_thresh";
        occ_yaml_raw << YAML::Value << occ_occupied_threshold;
        occ_yaml_raw << YAML::Key   << "free_thresh";
        occ_yaml_raw << YAML::Value << occ_free_threshold;
        occ_yaml_raw << YAML::Key   << "negate";
        occ_yaml_raw << YAML::Value << 0;
        occ_yaml_raw << YAML::EndMap;
        occ_out_yaml_raw.close();
    }

    // pgm files
    {
        std::ofstream occ_out_pgm(occ_path_pgm);
        std::ofstream occ_out_raw_pgm(occ_path_raw_pgm);
        if (!occ_out_pgm.is_open()) {
            std::cout << "[SaveMap]: Could not open file '" << occ_path_pgm << "'." << std::endl;
            return false;
        }
        if (!occ_out_raw_pgm.is_open()) {
            std::cout << "[SaveMap]: Could not open file '" << occ_path_raw_pgm << "'." << std::endl;
            return false;
        }

        /// write pgm headers
        occ_out_pgm << "P5 \n";
        occ_out_pgm << "# CREATOR: cslibs_mapping " << occ_resolution << "m/pix \n";
        occ_out_pgm << occ_width << " " << occ_height << "\n";
        occ_out_pgm << 255 << "\n";
        occ_out_raw_pgm << "P5 \n";
        occ_out_raw_pgm << "# CREATOR: cslibs_mapping " << occ_resolution << "m/pix \n";
        occ_out_raw_pgm << occ_width << " " << occ_height << "\n";
        occ_out_raw_pgm << 255 << "\n";

        auto convert_ros = [&occ_free_threshold, &occ_occupied_threshold](const T p) {
            if (cslibs_math::common::le(p, occ_free_threshold))
                return static_cast<uint8_t>(254);
            if (cslibs_math::common::le(p,occ_occupied_threshold))
                return static_cast<uint8_t>(0);
            return static_cast<uint8_t>(205);
        };
        auto convert_raw = [](const T p) {
            return static_cast<uint8_t>((1.0 - p) * 255);
        };

        for (std::size_t i = occ_height ; i > 0 ; --i) {
            for (std::size_t j = 0 ; j < occ_width ; ++j) {
                const T & data = occ_data[(i - 1) * occ_width + j];
                occ_out_pgm << convert_ros(data);
                occ_out_raw_pgm << convert_raw(data);
            }
        }

        occ_out_pgm.close();
        occ_out_raw_pgm.close();
    }

    // map data
    {
        const Tp occ_inv_resolution = 1.0 / occ_resolution;
        const cslibs_math_2d::Transform2<Tp> m_t_w = occ_origin.inverse();

        std::ofstream poses_out_yaml(poses_path_yaml);
        if (!poses_out_yaml.is_open()) {
            std::cout << "[SaveMap]: Could not open file '" << poses_path_yaml << "'." << std::endl;
            return false;
        }

        if (poses) {
            YAML::Emitter poses_yaml(poses_out_yaml);
            poses_yaml << YAML::BeginSeq;
            for (const auto &p_w : poses->poses) {
                const cslibs_math_2d::Transform2<Tp> p_m = m_t_w * cslibs_math_ros::geometry_msgs::conversion_2d::from<Tp>(p_w.pose);
                poses_yaml << YAML::Flow << YAML::BeginSeq << p_m.tx() * occ_inv_resolution << p_m.ty() * occ_inv_resolution << YAML::EndSeq;
            }
            poses_yaml << YAML::EndSeq;
            poses_out_yaml.close();
        }
    }

    return true;
}

inline bool savePath(
        const std::string    & poses_path_3d_yaml,
        const nav_msgs::Path & poses_path)
{
    std::ofstream poses_out_3d_yaml(poses_path_3d_yaml);
    if (!poses_out_3d_yaml.is_open()) {
        std::cout << "[SaveMap]: Could not open file '" << poses_path_3d_yaml << "'." << std::endl;
        return false;
    }

    YAML::Emitter poses_yaml(poses_out_3d_yaml);
    poses_yaml << YAML::BeginSeq;
    for (const auto &p_w : poses_path.poses)
        poses_yaml << YAML::Flow << YAML::BeginSeq
                   << p_w.pose.position.x << p_w.pose.position.y << p_w.pose.position.z
                   << p_w.pose.orientation.x << p_w.pose.orientation.y << p_w.pose.orientation.z
                   << p_w.pose.orientation.w << YAML::EndSeq;
    poses_yaml << YAML::EndSeq;
    poses_out_3d_yaml.close();

    return true;
}
}
}

#endif // CSLIBS_MAPPING_SAVE_MAP_HPP


#include <tacklelib/utility/time.hpp>

#ifndef UTILITY_PLATFORM_FEATURE_STD_HAS_GET_TIME
#include <boost/date_time/posix_time/posix_time.hpp>
#endif

#include <inttypes.h>

namespace utility {
namespace time {

    // WORKAROUND:
    //  1. Boost uses `strftime` time string format instead of the `std::get_time` time string format plus biased time base which has to be workarounded:
    //      `from_time_t(t1 + t2) gains incorrect and different results on Windows and Linux`:
    //      https://github.com/boostorg/date_time/issues/157
    //  2. Use base year extraction to renormalize std::tm to the std representation.
    //

    std::time_t convert_time_string_to_std_time(const std::string & time_str_format, const std::string & time_value_str,
                                                convert_time_string_to_std_time_error_func_t error_func)
    {
        std::time_t epoch_time_sec = 0;

#if defined(UTILITY_PLATFORM_FEATURE_STD_HAS_GET_TIME) && !defined(UTILITY_PLATFORM_FEATURE_USE_BOOST_POSIX_TIME_INSTEAD_STD_GET_TIME)
        try
        {
            std::tm t;
            if (utility::time::get_time(t, std::string{}, time_str_format, time_value_str)) {
                if (time_str_format.find("%Y") == std::string::npos) {
                    if (time_str_format.find("%y") == std::string::npos) {
                        // from 1900 year
                        t.tm_year += 70;
                    }
                    else {
                        // [00-68] -> 2000-2068
                        // [69-99] -> 1969-1999
                        if (t.tm_year < 69) {
                            t.tm_year += 100;
                        }
                    }
                }
                if (!t.tm_mday) t.tm_mday = 1;

#if defined(UTILITY_COMPILER_CXX_MSC) && UTILITY_COMPILER_CXX_VERSION < 1910
                time_t epoch_time_correction_sec = 0;
                int epoch_time_correction_from_year = 70;
                if (t.tm_year < 70) {
                    // CAUTION:
                    //   utility::time::timegm will fail if the year component is less than 70, so fix the year and add delta directly to the time_t
                    //
                    epoch_time_correction_from_year = t.tm_year;
                    t.tm_year = 70;
                }
#endif

                errno = 0; // must be to ensure the state change
                epoch_time_sec = utility::time::timegm(t);
                if (epoch_time_sec == -1 && errno != 0) { // error handle
                    epoch_time_sec = 0;
                }
                else {
#if defined(UTILITY_COMPILER_CXX_MSC) && UTILITY_COMPILER_CXX_VERSION < 1910
                    for (int i = epoch_time_correction_from_year; i < 70; i++) {
                        epoch_time_sec -= 365 * 24 * 60 * 60;
                    }
                    epoch_time_sec -= (get_leap_days(1970) - get_leap_days(1900 + epoch_time_correction_from_year)) * 24 * 60 * 60;
#endif
                }
            }
            else {
                if (error_func) {
                    error_func(time_str_format, time_value_str, nullptr);
                }
            }
        }
#else
        try
        {
            namespace boost_ptime = boost::posix_time;

            struct base_time_extractor
            {
                base_time_extractor() {
                    std::istringstream iss{ "%Y-%m-%d" };
                    iss.imbue(std::locale(std::locale::classic(), new boost_ptime::time_input_facet("1970-01-01")));
                    boost_ptime::ptime base_ptime_value;
                    iss >> base_ptime_value;
                    base_time = boost_ptime::to_tm(base_ptime_value);
                    //printf("base tm_year (boost) 1: %i\n", base_time.tm_year);
                }

                std::tm base_time;
            } static s_base_time_extractor;

            std::istringstream iss{ time_value_str };
            iss.imbue(std::locale(std::locale::classic(), new boost_ptime::time_input_facet(time_str_format.c_str())));
            boost_ptime::ptime ptime_value;
            iss >> ptime_value;

            if (!ptime_value.is_not_a_date_time()) {
                std::tm t = boost_ptime::to_tm(ptime_value);

                if (time_str_format.find("%Y") == std::string::npos) {
                    if (time_str_format.find("%y") == std::string::npos) {
                        // biased base, normalize
                        t.tm_year -= s_base_time_extractor.base_time.tm_year;
                        // from 1900 year
                        t.tm_year += 70;
                    }
                    else {
                        // CAUTION:
                        //  Here is a specific boost incompatability versus the std::get_time because the boost has used the
                        //  strftime format which is quite different:
                        //  [00-99] -> 2000-2099

                        // std::get_time format, just in case:
                        //  [00-68] -> 2000-2068
                        //  [69-99] -> 1969-1999
                        if (t.tm_year < 69) {
                            t.tm_year += 100;
                        }
                    }
                }
                if (!t.tm_mday) t.tm_mday = 1; // just in case

                epoch_time_sec = utility::time::timegm(t);
            }
            else {
                if (error_func) {
                    error_func(time_str_format, time_value_str, nullptr);
                }
            }
        }
        catch (const boost::bad_lexical_cast & ex)
        {
            if (error_func) {
                error_func(time_str_format, time_value_str, &ex);
            }
        }
#endif
        catch (const std::out_of_range & ex)
        {
            if (error_func) {
                error_func(time_str_format, time_value_str, &ex);
            }
        }

        return epoch_time_sec;
    }

    std::string convert_time_string_from_std_time(const std::string & time_str_format, const std::time_t & time_value,
                                                  convert_time_string_from_std_time_error_func_t error_func)
    {
#if defined(UTILITY_PLATFORM_FEATURE_STD_HAS_GET_TIME) && !defined(UTILITY_PLATFORM_FEATURE_USE_BOOST_POSIX_TIME_INSTEAD_STD_GET_TIME)
        std::string time_str;

        try
        {
#if !defined(UTILITY_COMPILER_CXX_MSC) || UTILITY_COMPILER_CXX_VERSION >= 1910
            std::tm t = utility::time::gmtime(time_value);
#else
            std::tm t = utility::time::gmtime(time_value >= 0 ? time_value : 0); // avoid exception code 0xc0000409
#endif

            if (time_str_format.find("%Y") == std::string::npos) {
                if (time_str_format.find("%y") == std::string::npos) {
                    // from 1900 year
                    t.tm_year += 70;
                }
                else {
                    // std::get_time format:
                    //  [00-68] -> 2000-2068
                    //  [69-99] -> 1969-1999
                    if (t.tm_year < 69) {
                        t.tm_year += 100;
                    }
                }
            }
            if (!t.tm_mday) t.tm_mday = 1; // just in case

            if (utility::time::get_time(time_str, time_str_format, t)) {
                return time_str;
            }
            else {
                if (error_func) {
                    error_func(time_str_format, time_value, nullptr);
                }
            }
        }
#else
        try
        {
            namespace boost_ptime = boost::posix_time;

            std::tm t = utility::time::gmtime(time_value);

            if (!t.tm_mday) t.tm_mday = 1; // just in case

            std::ostringstream oss;
            oss.imbue(std::locale(std::locale::classic(), new boost_ptime::time_facet(time_str_format.c_str())));
            boost_ptime::ptime ptime_value = boost_ptime::ptime_from_tm(t);
            oss << ptime_value;

            if (!oss.fail()) {
                return oss.str();
            }
            else {
                if (error_func) {
                    error_func(time_str_format, time_value, nullptr);
                }
            }
        }
        catch (const boost::bad_lexical_cast & ex)
        {
            if (error_func) {
                error_func(time_str_format, time_value, &ex);
            }
        }
#endif
        catch (const std::out_of_range & ex)
        {
            if (error_func) {
                error_func(time_str_format, time_value, &ex);
            }
        }

#if defined(UTILITY_PLATFORM_FEATURE_STD_HAS_GET_TIME) && !defined(UTILITY_PLATFORM_FEATURE_USE_BOOST_POSIX_TIME_INSTEAD_STD_GET_TIME)
        return time_str;
#else
        return std::string{};
#endif
    }

}
}


/*
 * Copyright 2015 sujiwo
 *
 * 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 "trafficlight_recognizer/rate.h"
#include "libvectormap/vector_map.h"
#include "libvectormap/Math.h"

#include <algorithm>
#include <cstdio>
#include <iostream>
#include <map>
#include <signal.h>
#include <string>

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <autoware_msgs/AdjustXY.h>
#include <autoware_msgs/Lane.h>
#include <autoware_msgs/Signals.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/CameraInfo.h>
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <vector_map/vector_map.h>
#include <vector_map_server/GetSignal.h>

static std::string camera_id_str;

static constexpr uint32_t SUBSCRIBE_QUEUE_SIZE = 1000;

static int adjust_proj_x = 0;
static int adjust_proj_y = 0;

typedef struct
{
  double thiX;
  double thiY;
  double thiZ;
}
Angle;

static VectorMap vmap;
// static Angle cameraOrientation; // camera orientation = car's orientation

static Eigen::Vector3f position;
static Eigen::Quaternionf orientation;
static float fx, fy, imageWidth, imageHeight, cx, cy;
static tf::StampedTransform trf;

static bool g_use_vector_map_server;  // Switch flag whether vecter-map-server function will be used
static ros::ServiceClient g_ros_client;

#define SignalLampRadius 0.3

/* Define utility class to use vector map server */
namespace
{
class VectorMapClient
{
private:
  geometry_msgs::PoseStamped pose_;
  autoware_msgs::Lane waypoints_;

public:
  VectorMapClient()
  {
  }

  ~VectorMapClient()
  {
  }

  geometry_msgs::PoseStamped pose() const
  {
    return pose_;
  }

  autoware_msgs::Lane waypoints() const
  {
    return waypoints_;
  }

  void set_pose(const geometry_msgs::PoseStamped& pose)
  {
    pose_ = pose;
  }

  void set_waypoints(const autoware_msgs::Lane& waypoints)
  {
    waypoints_ = waypoints;
  }
};  // Class VectorMapClient
}  // namespace
static VectorMapClient g_vector_map_client;

/* Callback function to shift projection result */
void adjust_xyCallback(const autoware_msgs::AdjustXY::ConstPtr& config_msg)
{
  adjust_proj_x = config_msg->x;
  adjust_proj_y = config_msg->y;
}

void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr camInfoMsg)
{
  fx = static_cast<float>(camInfoMsg->P[0]);
  fy = static_cast<float>(camInfoMsg->P[5]);
  imageWidth = camInfoMsg->width;
  imageHeight = camInfoMsg->height;
  cx = static_cast<float>(camInfoMsg->P[2]);
  cy = static_cast<float>(camInfoMsg->P[6]);
}

/* convert degree value into 0 to 360 range */
/*static double setDegree0to360(double val)
{
  if (val < 0.0f)
  {
    return (val + 360.0f);
  } else if (360.0f < val)
  {
    return (val - 360.0f);
  }

  return val;
}*/

/*static void get_cameraRollPitchYaw(double *roll,
                                   double *pitch,
                                   double *yaw)
{
  geometry_msgs::Pose cameraPose;
  cameraPose.position.x = (double) (position.x());
  cameraPose.position.y = (double) (position.y());
  cameraPose.position.z = (double) (position.z());
  cameraPose.orientation.x = (double) (orientation.x());
  cameraPose.orientation.y = (double) (orientation.y());
  cameraPose.orientation.z = (double) (orientation.z());
  cameraPose.orientation.w = (double) (orientation.w());

  tf::Quaternion quat;

  tf::quaternionMsgToTF(cameraPose.orientation, quat);
  tf::Matrix3x3(quat).getRPY(*roll, *pitch, *yaw);

  // convert from radian to degree
  *roll = setDegree0to360(*roll * 180.0f / M_PI);
  *pitch = setDegree0to360(*pitch * 180.0f / M_PI);
  *yaw = setDegree0to360(*yaw * 180.0f / M_PI);
}*/

/*
  check if lower < val < upper
  This function also considers circulation
*/
static bool isRange(const double lower, const double upper, const double val)
{
  if (lower <= upper)
  {
    if (lower < val && val < upper)
    {
      return true;
    }
  }
  else
  {
    if (val < upper || lower < val)
    {
      return true;
    }
  }

  return false;
}

void getTransform(Eigen::Quaternionf* ori, Point3* pos)
{
  static tf::TransformListener listener;

  // target_frame    source_frame
  ros::Time now = ros::Time();
  listener.waitForTransform(camera_id_str, "map", now, ros::Duration(10.0));
  listener.lookupTransform(camera_id_str, "map", now, trf);

  tf::Vector3& p = trf.getOrigin();
  tf::Quaternion o = trf.getRotation();
  pos->x() = p.x();
  pos->y() = p.y();
  pos->z() = p.z();
  ori->w() = o.w();
  ori->x() = o.x();
  ori->y() = o.y();
  ori->z() = o.z();
}

Point3 transform(const Point3& psrc, const tf::StampedTransform& tfsource)
{
  tf::Vector3 pt3(psrc.x(), psrc.y(), psrc.z());
  tf::Vector3 pt3s = tfsource * pt3;
  return Point3(pt3s.x(), pt3s.y(), pt3s.z());
}

/*
 * Project a point from world coordinate to image plane
 */
bool project2(const Point3& pt, int* u, int* v, bool useOpenGLCoord = false)
{
  float nearPlane = 1.0;
  float farPlane = 200.0;
  Point3 _pt = transform(pt, trf);
  float _u = _pt.x() * fx / _pt.z() + cx;
  float _v = _pt.y() * fy / _pt.z() + cy;

  *u = static_cast<int>(_u);
  *v = static_cast<int>(_v);
  if (*u < 0 || imageWidth < *u || *v < 0 || imageHeight < *v || _pt.z() < nearPlane || farPlane < _pt.z())
  {
    *u = -1, *v = -1;
    return false;
  }

  if (useOpenGLCoord)
  {
    *v = imageHeight - *v;
  }

  return true;
}

double ConvertDegreeToRadian(double degree)
{
  return degree * M_PI / 180.0f;
}

double ConvertRadianToDegree(double radian)
{
  return radian * 180.0f / M_PI;
}

double GetSignalAngleInCameraSystem(double hang, double vang)
{
  // Fit the vector map format into ROS style
  double signal_pitch_in_map = ConvertDegreeToRadian(vang - 90);
  double signal_yaw_in_map = ConvertDegreeToRadian(-hang + 90);

  tf::Quaternion signal_orientation_in_map_system;
  signal_orientation_in_map_system.setRPY(0, signal_pitch_in_map, signal_yaw_in_map);

  tf::Quaternion signal_orientation_in_cam_system = trf * signal_orientation_in_map_system;
  double signal_roll_in_cam;
  double signal_pitch_in_cam;
  double signal_yaw_in_cam;
  tf::Matrix3x3(signal_orientation_in_cam_system).getRPY(signal_roll_in_cam, signal_pitch_in_cam, signal_yaw_in_cam);

  return ConvertRadianToDegree(signal_pitch_in_cam);  // holizontal angle of camera is represented by pitch
}  // double GetSignalAngleInCameraSystem()

void echoSignals2(const ros::Publisher& pub, bool useOpenGLCoord = false)
{
  int countPoint = 0;
  autoware_msgs::Signals signalsInFrame;

  /* Get signals on the path if vecter_map_server is enabled */
  if (g_use_vector_map_server)
  {
    vector_map_server::GetSignal service;
    /* Set server's request */
    service.request.pose = g_vector_map_client.pose();
    service.request.waypoints = g_vector_map_client.waypoints();

    /* Get server's response*/
    if (g_ros_client.call(service))
    {
      /* Reset signal data container */
      vmap.signals.clear();

      /* Newle insert signal data on the path */
      for (const auto& response : service.response.objects.data)
      {
        if (response.id == 0)
          continue;

        Signal signal;
        signal.id = response.id;
        signal.vid = response.vid;
        signal.plid = response.plid;
        signal.type = response.type;
        signal.linkid = response.linkid;

        vmap.signals.insert(std::map<int, Signal>::value_type(signal.id, signal));
      }
      ROS_INFO("[feat_proj] VectorMapServer available. Publishing only TrafficSignals on the current lane");
    }
  }

  for (const auto& signal_map : vmap.signals)
  {
    const Signal signal = signal_map.second;
    int pid = vmap.vectors[signal.vid].pid;

    Point3 signalcenter = vmap.getPoint(pid);
    Point3 signalcenterx(signalcenter.x(), signalcenter.y(), signalcenter.z() + SignalLampRadius);

    int u, v;
    if (project2(signalcenter, &u, &v, useOpenGLCoord) == true)
    {
      countPoint++;
      // std::cout << u << ", " << v << ", " << std::endl;

      int radius;
      int ux, vx;
      project2(signalcenterx, &ux, &vx, useOpenGLCoord);
      radius = static_cast<int>(distance(ux, vx, u, v));

      autoware_msgs::ExtractedPosition sign;
      sign.signalId = signal.id;

      sign.u = u + adjust_proj_x;  // shift project position by configuration value from runtime manager
      sign.v = v + adjust_proj_y;  // shift project position by configuration value from runtime manager

      sign.radius = radius;
      sign.x = signalcenter.x(), sign.y = signalcenter.y(), sign.z = signalcenter.z();
      sign.hang = vmap.vectors[signal.vid].hang;  // hang is expressed in [0, 360] degree
      sign.type = signal.type, sign.linkId = signal.linkid;
      sign.plId = signal.plid;

      // Get holizontal angle of signal in camera corrdinate system
      double signal_angle =
          GetSignalAngleInCameraSystem(vmap.vectors[signal.vid].hang + 180.0f, vmap.vectors[signal.vid].vang + 180.0f);

      // signal_angle will be zero if signal faces to x-axis
      // Target signal should be face to -50 <= z-axis (= 90 degree) <= +50
      if (isRange(-50, 50, signal_angle - 90))
      {
        signalsInFrame.Signals.push_back(sign);
      }
    }
  }
  signalsInFrame.header.stamp = ros::Time::now();
  pub.publish(signalsInFrame);

  std::cout << "There are " << signalsInFrame.Signals.size() << " signals in range" << std::endl;
}

void interrupt(int s)
{
  ros::shutdown();
  exit(1);
}

int main(int argc, char* argv[])
{
  ros::init(argc, argv, "feat_proj", ros::init_options::NoSigintHandler);
  ros::NodeHandle rosnode;
  ros::NodeHandle private_nh("~");
  std::string cameraInfo_topic_name;
  private_nh.param<std::string>("camera_info_topic", cameraInfo_topic_name, "/camera_info");

  /* get camera ID */
  camera_id_str = cameraInfo_topic_name;
  camera_id_str.erase(camera_id_str.find("/camera_info"));
  if (camera_id_str == "/")
  {
    camera_id_str = "camera";
  }

  /* Get Flag wheter vecter_map_server function will be used  */
  private_nh.param<bool>("use_path_info", g_use_vector_map_server, false);
  ROS_INFO("[feat_proj] Use VectorMapServer: %d", g_use_vector_map_server);
  /* load vector map */
  ros::Subscriber sub_point =
      rosnode.subscribe("vector_map_info/point", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_points, &vmap);
  ros::Subscriber sub_line =
      rosnode.subscribe("vector_map_info/line", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_lines, &vmap);
  ros::Subscriber sub_lane =
      rosnode.subscribe("vector_map_info/lane", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_lanes, &vmap);
  ros::Subscriber sub_vector =
      rosnode.subscribe("vector_map_info/vector", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_vectors, &vmap);
  ros::Subscriber sub_signal =
      rosnode.subscribe("vector_map_info/signal", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_signals, &vmap);
  ros::Subscriber sub_whiteline =
      rosnode.subscribe("vector_map_info/white_line", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_whitelines, &vmap);
  ros::Subscriber sub_dtlane =
      rosnode.subscribe("vector_map_info/dtlane", SUBSCRIBE_QUEUE_SIZE, &VectorMap::load_dtlanes, &vmap);

  /* wait until loading all vector map is completed */
  ros::Rate wait_rate(100);
  std::cout << "Loading Vector Map. Please wait";
  while (vmap.points.empty() || vmap.lines.empty() || vmap.whitelines.empty() || vmap.lanes.empty() ||
         vmap.dtlanes.empty() || vmap.vectors.empty() || vmap.signals.empty())
  {
    ros::spinOnce();
    std::cout << ".";
    wait_rate.sleep();
  }

  vmap.loaded = true;
  std::cout << "Loaded." << std::endl;

  ros::Subscriber cameraInfoSubscriber = rosnode.subscribe(cameraInfo_topic_name, 100, cameraInfoCallback);
  ros::Subscriber cameraImage = rosnode.subscribe(cameraInfo_topic_name, 100, cameraInfoCallback);
  ros::Subscriber adjust_xySubscriber = rosnode.subscribe("/config/adjust_xy", 100, adjust_xyCallback);
  ros::Subscriber current_pose_subscriber;
  ros::Subscriber waypoint_subscriber;
  if (g_use_vector_map_server)
  {
    /* Create subscribers which deliver informations requested by server */
    current_pose_subscriber = rosnode.subscribe("/current_pose", 1, &VectorMapClient::set_pose, &g_vector_map_client);
    waypoint_subscriber =
        rosnode.subscribe("/final_waypoints", 1, &VectorMapClient::set_waypoints, &g_vector_map_client);

    /* Create ros client to use Server-Client communication */
    g_ros_client = rosnode.serviceClient<vector_map_server::GetSignal>("vector_map_server/get_signal");
  }

  ros::Publisher signalPublisher = rosnode.advertise<autoware_msgs::Signals>("roi_signal", 100);
  signal(SIGINT, interrupt);

  Rate loop(50);
  Eigen::Vector3f prev_position(0, 0, 0);
  Eigen::Quaternionf prev_orientation(0, 0, 0, 0);
  while (true)
  {
    ros::spinOnce();

    try
    {
      getTransform(&orientation, &position);
    }
    catch (tf::TransformException& exc)
    {
    }

    if (prev_orientation.vec() != orientation.vec() && prev_position != position)
    {
      echoSignals2(signalPublisher, false);
    }
    prev_orientation = orientation;
    prev_position = position;
    loop.sleep();
  }
}


#include <iostream>
#include "ros/ros.h"
#include "gpd/GraspConfigList.h"
#include "geometry_msgs/PointStamped.h"
#include <math.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <geometry_msgs/TransformStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/transforms.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Dense>
#include <vector>
#include <list>

float hand_outer_diameter = 0.145;   // Hand configuration
float hand_depth = 0.065;            // Hand configuration
float hand_height = 0.02;            // Hand configuration
float finger_width = 0.015;          // Hand configuration
float nt = 0.9;                      // Threshold of vertical normals
float h = 0.025;                      // Threshold of Left and Right offsets
float WS_filter_threshold = 0.01;
float theta_p = 10;

//geometry_msgs::PointStamped R1, R2, R3, R4;
ros::Publisher pub_r1_r4_pc, pub_FFC_pc, pub_FFC_GraspConfig, pub_original_candidate, pub_FFC_candidate;
tf::TransformListener *tf_listener;
geometry_msgs::PointStamped close_region_R1, close_region_R2, close_region_R3, close_region_R4;

// Convert GraspConfig msg to Marker in RVIZ
visualization_msgs::MarkerArray convertToVisualGraspMsg(const gpd::GraspConfig GraspConfig, const std::string& frame_id, const bool color)
{
  // Get parameters of GraspConfig
  int i = 1;
  double width = hand_outer_diameter;
  double hw = 0.5 * width;

  visualization_msgs::MarkerArray marker_array;
  visualization_msgs::Marker left_finger, right_finger, base, approach;
  Eigen::Vector3d left_bottom, right_bottom, left_top, right_top, left_center, right_center, approach_center, base_center;

  Eigen::Vector3d g_bottom(GraspConfig.bottom.x, GraspConfig.bottom.y, GraspConfig.bottom.z);
  Eigen::Vector3d g_binormal(GraspConfig.binormal.x, GraspConfig.binormal.y, GraspConfig.binormal.z);
  Eigen::Vector3d g_approach(GraspConfig.approach.x, GraspConfig.approach.y, GraspConfig.approach.z);
  Eigen::Matrix3d g_frame;
  g_frame << GraspConfig.approach.x, GraspConfig.binormal.x, GraspConfig.axis.x,
             GraspConfig.approach.y, GraspConfig.binormal.y, GraspConfig.axis.y,
             GraspConfig.approach.z, GraspConfig.binormal.z, GraspConfig.axis.z;

  // Compute the position of each marker
  left_bottom = g_bottom - (hw - 0.5*finger_width) * g_binormal;
  right_bottom = g_bottom + (hw - 0.5*finger_width) * g_binormal;
  left_top = left_bottom + hand_depth * g_approach;
  right_top = right_bottom + hand_depth * g_approach;
  left_center = left_bottom + 0.5*(left_top - left_bottom);
  right_center = right_bottom + 0.5*(right_top - right_bottom);
  base_center = left_bottom + 0.5*(right_bottom - left_bottom) - 0.01*g_approach;
  approach_center = base_center - 0.04*g_approach;

  // Create grasp marker
  visualization_msgs::Marker createFingerMarker(const Eigen::Vector3d& center, const Eigen::Matrix3d& frame, double length, double width, double height, int id, const std::string& frame_id, const bool color);
  visualization_msgs::Marker createHandBaseMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Matrix3d& frame, double length, double height, int id, const std::string& frame_id, const bool color);
  base = createHandBaseMarker(left_bottom, right_bottom, g_frame, 0.02, hand_height, i, frame_id, color);
  left_finger = createFingerMarker(left_center, g_frame, hand_depth, finger_width, hand_height, i*3, frame_id, color);
  right_finger = createFingerMarker(right_center, g_frame, hand_depth, finger_width, hand_height, i*3+1, frame_id, color);
  approach = createFingerMarker(approach_center, g_frame, 0.08, finger_width, hand_height, i*3+2, frame_id, color);

  // Input marker configuration to MarkerArray
  marker_array.markers.push_back(left_finger);
  marker_array.markers.push_back(right_finger);
  marker_array.markers.push_back(approach);
  marker_array.markers.push_back(base);

  return marker_array;
}

// Create the finger part of marker
visualization_msgs::Marker createFingerMarker(const Eigen::Vector3d& center, const Eigen::Matrix3d& frame, double length, double width, double height, int id, const std::string& frame_id, bool color)
{
  // Configuration of Marker
  visualization_msgs::Marker marker;
  marker.header.frame_id = frame_id;
  marker.header.stamp = ros::Time();
  marker.ns = "finger";
  marker.id = id;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = center(0);
  marker.pose.position.y = center(1);
  marker.pose.position.z = center(2);
  marker.lifetime = ros::Duration(30);

  // Use orientation of hand frame
  Eigen::Quaterniond quat(frame);
  marker.pose.orientation.x = quat.x();
  marker.pose.orientation.y = quat.y();
  marker.pose.orientation.z = quat.z();
  marker.pose.orientation.w = quat.w();

  // These scales are relative to the hand frame (unit: meters)
  marker.scale.x = length; // forward direction
  marker.scale.y = width; // hand closing direction
  marker.scale.z = height; // hand vertical direction

  // Determine the color of grasp marker
  if(color == false)
  {
    // Green
    marker.color.a = 0.5;
    marker.color.g = 1.0;
  }
  else
  {
    // Red
    marker.color.a = 0.5;
    marker.color.r = 1.0;
  }

  return marker;
}

// Create the base part of marker
visualization_msgs::Marker createHandBaseMarker(const Eigen::Vector3d& start, const Eigen::Vector3d& end, const Eigen::Matrix3d& frame, double length, double height, int id, const std::string& frame_id, bool color)
{
  Eigen::Vector3d center = start + 0.5 * (end - start);

  // Configuration of Marker
  visualization_msgs::Marker marker;
  marker.header.frame_id = frame_id;
  marker.header.stamp = ros::Time();
  marker.ns = "hand_base";
  marker.id = id;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = center(0);
  marker.pose.position.y = center(1);
  marker.pose.position.z = center(2);
  marker.lifetime = ros::Duration(30);

  // Use orientation of hand frame
  Eigen::Quaterniond quat(frame);
  marker.pose.orientation.x = quat.x();
  marker.pose.orientation.y = quat.y();
  marker.pose.orientation.z = quat.z();
  marker.pose.orientation.w = quat.w();

  // These scales are relative to the hand frame (unit: meters)
  marker.scale.x = length; // forward direction
  marker.scale.y = (end - start).norm(); // hand closing direction
  marker.scale.z = height; // hand vertical direction

  // Determine the color of grasp marker
  if(color == false)
  {
    // Green
    marker.color.a = 0.5;
    marker.color.g = 1.0;
  }
  else
  {
    // Red
    marker.color.a = 0.5;
    marker.color.r = 1.0;
  }

  return marker;
}

// Caculate the stability of object
double Caculate_Stability(pcl::PointCloud<pcl::PointXYZ>::Ptr left_result_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr right_result_cloud)
{
  // std::cout << "#points: " << left_result_cloud->points.size() << std::endl;
  // std::cout << "#points: " << right_result_cloud->points.size() << std::endl;

  if (left_result_cloud->points.size() > 0 && right_result_cloud->points.size() > 0)
  {
    pcl::search::KdTree<pcl::PointXYZ>::Ptr kdtree (new pcl::search::KdTree<pcl::PointXYZ> ());
    pcl::PointCloud<pcl::Normal>::Ptr left_cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> left_ne;
    left_ne.setInputCloud (left_result_cloud);
    left_ne.setSearchMethod (kdtree);
    left_ne.setRadiusSearch (0.01);
    left_ne.compute (*left_cloud_normals);

    pcl::PointCloud<pcl::Normal>::Ptr right_cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> right_ne;
    right_ne.setInputCloud (right_result_cloud);
    right_ne.setSearchMethod (kdtree);
    right_ne.setRadiusSearch (0.01);
    right_ne.compute (*right_cloud_normals);

    //std::cout << "#normal: " << left_cloud_normals->points.size() << std::endl;
    //std::cout << "#normal: " << right_cloud_normals->points.size() << std::endl;

    double sum_nm = 0.0;
    for (int n = 0; n < left_cloud_normals->points.size(); n++)
    {
      Eigen::Vector3d y_n(0, 0, 0);
      y_n[0] = -left_cloud_normals->points[n].normal_x;
      y_n[1] = -left_cloud_normals->points[n].normal_y;
      y_n[2] = -left_cloud_normals->points[n].normal_z;

      double sum_m = 0.0;
      for (int m = 0; m < right_cloud_normals->points.size(); m++)
      {
        Eigen::Vector3d x_m(0, 0, 0);
        x_m[0] = -right_cloud_normals->points[m].normal_x;
        x_m[1] = -right_cloud_normals->points[m].normal_y;
        x_m[2] = -right_cloud_normals->points[m].normal_z;

        Eigen::Vector3d f_mn(0, 0, 0);
        f_mn[0] = left_result_cloud->points[m].x - right_result_cloud->points[n].x;
        f_mn[1] = left_result_cloud->points[m].y - right_result_cloud->points[n].y;
        f_mn[2] = left_result_cloud->points[m].z - right_result_cloud->points[n].z;
        double x_y_z_normalization = sqrt(pow(f_mn[0], 2) + pow(f_mn[1], 2) + pow(f_mn[2], 2));
        f_mn[0] = f_mn[0] / x_y_z_normalization;
        f_mn[1] = f_mn[1] / x_y_z_normalization;
        f_mn[2] = f_mn[2] / x_y_z_normalization;

        int xm_fmn = 0;
        int yn_fmn = 0;
        int muti_xm_yn_fmn = 0;

        if (x_m.dot(f_mn) >= cos(theta_p))
        {
          xm_fmn = 1;
        }
        else
        {
          xm_fmn = 0;
        }

        if (y_n.dot(f_mn) <= -(cos(theta_p)))
        {
          yn_fmn = 1;
        }
        else
        {
          yn_fmn = 0;
        }

        muti_xm_yn_fmn = xm_fmn * yn_fmn;
        sum_m = sum_m + muti_xm_yn_fmn;
        // std::cout << "Sum_m: " << sum_m << std::endl;
      }
      sum_nm = sum_nm + sum_m;
      // std::cout << "Sum_nm: " << sum_nm << std::endl;
    }

    double stability_result;
    double num_point;
    stability_result = sum_nm / (left_result_cloud->points.size() * right_result_cloud->points.size());
    num_point = (left_result_cloud->points.size() + right_result_cloud->points.size()) * 0.001;
    stability_result = stability_result + num_point;
    // std::cout << "Sum_nm: " << sum_nm << std::endl;
    // std::cout << "Stability_result: " << stability_result << std::endl;
    return stability_result;
  }
  else
  {
    double none_FFC;
    none_FFC =left_result_cloud->points.size() + right_result_cloud->points.size();
    none_FFC = none_FFC * 0.001;
    return none_FFC;
  }
}

// Compute the normal and number of points that in R1 - R4 region
double Compute_Normal_and_Number(sensor_msgs::PointCloud2 &R1_R4_region_pc)
{
	// Unit vector of y-axis
  Eigen::Vector3d v2(0, 1, 0);

  // Origin of P frame
	float x0 = 0, y0 = 0, z0 = 0;

  // Convert ROS PointCloud2 data to PCL PointXYZ
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2* cloud_filtered = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2Ptr cloudFilteredPtr (cloud_filtered);

  pcl_conversions::toPCL(R1_R4_region_pc, *cloud);

  pcl::PointCloud<pcl::PointXYZ> *xyz_cloud = new pcl::PointCloud<pcl::PointXYZ>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloudPtr(xyz_cloud);

  pcl::fromPCLPointCloud2(*cloudPtr,*xyzCloudPtr);

  // Get points normal
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(xyzCloudPtr);

  // Set up KdTree
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
	ne.setSearchMethod(tree);

	pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
	ne.setRadiusSearch(0.03);
	ne.compute(*cloud_normals);

	std::vector<std::vector<float> > clound_xyz;
	std::vector<float> point_xyz;
  Eigen::Vector3d p_normal(0, 0, 0);

	float a, b, c, D;
	float cnv_v2 = 0.0;

  D = 0;

	a = v2[0], b = v2[1], c = v2[2];

  // Compute the inner product of point normal and vertical vector
  // If the inner product value within threshold and save it
	for (int i = 0; i < cloud_normals->points.size(); i++)
  {
		p_normal[0] = cloud_normals->points[i].normal_x;
		p_normal[1] = cloud_normals->points[i].normal_y;
		p_normal[2] = cloud_normals->points[i].normal_z;
		cnv_v2 = v2.dot(p_normal);
		if (cnv_v2 > nt || cnv_v2 < -nt) {
			point_xyz.push_back(xyz_cloud->points[i].x);
			point_xyz.push_back(xyz_cloud->points[i].y);
			point_xyz.push_back(xyz_cloud->points[i].z);
			clound_xyz.push_back(point_xyz);
			point_xyz.clear();
		}
	}

  //std::cout << "Number of Normals: " << clound_xyz.size() << std::endl;

  std::vector<float> distance;
	float x = 0, y = 0, z = 0;
	float max = 0, min = 0;

  std::list<float> Left_list;
  std::list<float>::iterator Left_list_iter;

  std::list<float> Right_list;
  std::list<float>::iterator Right_list_iter;

  // Find out the point which is longest distance of Left side and Right side in R1 - R4 region
  // Collect the rest of point to Left or Right
	for (int i = 0; i < clound_xyz.size(); i++)
  {
	  x = clound_xyz[i][0];
		y = clound_xyz[i][1];
		z = clound_xyz[i][2];
		D = a * (x - x0) + b * (y - y0) + c * (z - z0) / pow(a * a + b * b + c * c, 0.5);
		distance.push_back(D);

    if (D >= max - h)
    {
			if (D > max)
      {
        Right_list.clear();
				Right_list.push_back(i);
        max = D;
			}
			else
      {
        Right_list.push_back(i);
			}
		}

		if (D <= min + h)
    {
			if (D < min)
      {
        Left_list.clear();
        Left_list.push_back(i);
				min = D;
			}
			else
      {
        Left_list.push_back(i);
			}
		}
	}

	//std::cout << "Left: " << min << std::endl;
	//std::cout << "Right: " << max << std::endl;
	//std::cout << "Left_number: " << Left_list.size() << std::endl;
	//std::cout << "Right_number: " << Right_list.size() << std::endl;

  // Insert the Left and Right points to pointcloud
  pcl::PointCloud<pcl::PointXYZRGB> *result_cloud = new pcl::PointCloud<pcl::PointXYZRGB>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr left_result_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr right_result_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  for(Left_list_iter = Left_list.begin() ; Left_list_iter != Left_list.end() ; Left_list_iter++)
    {
      pcl::PointXYZRGB point;
      point.x = clound_xyz[*Left_list_iter][0];
      point.y = clound_xyz[*Left_list_iter][1];
      point.z = clound_xyz[*Left_list_iter][2];
      point.r = 255;
      result_cloud->push_back(point);
      pcl::PointXYZ point_left = {clound_xyz[*Left_list_iter][0], clound_xyz[*Left_list_iter][1], clound_xyz[*Left_list_iter][2]};
      left_result_cloud->points.push_back(point_left);
    }

  for(Right_list_iter = Right_list.begin() ; Right_list_iter != Right_list.end() ; Right_list_iter++)
    {
      pcl::PointXYZRGB point;
      point.x = clound_xyz[*Right_list_iter][0];
      point.y = clound_xyz[*Right_list_iter][1];
      point.z = clound_xyz[*Right_list_iter][2];
      point.r = 255;
      result_cloud->push_back(point);
      pcl::PointXYZ point_right = {clound_xyz[*Right_list_iter][0], clound_xyz[*Right_list_iter][1], clound_xyz[*Right_list_iter][2]};
      right_result_cloud->points.push_back(point_right);
    }

  std::cout << "Left_cloud_points_number: " << left_result_cloud->points.size() << std::endl;
  std::cout << "Right_cloud_points_number: " << right_result_cloud->points.size() << std::endl;

  std::cout << "Result_cloud_points_number: " << result_cloud->points.size() << std::endl;

  // Convert pointcloud to ROS PointCloud2 and show it in RVIZ
  pcl::PCLPointCloud2 PCL_Result_cloud;
  sensor_msgs::PointCloud2 ROS_Result_cloud;

  pcl::toPCLPointCloud2(*result_cloud ,PCL_Result_cloud);
  pcl_conversions::fromPCL(PCL_Result_cloud, ROS_Result_cloud);
  ROS_Result_cloud.header.stamp = ros::Time::now();
  ROS_Result_cloud.header.frame_id = "p_frame";

  pub_FFC_pc.publish(ROS_Result_cloud);

  // Caculate the stability of object
  double score = Caculate_Stability(left_result_cloud, right_result_cloud);
  // score = score + (Left_list.size() + Right_list.size()) * 0.001;
  // std::cout << "Score: " << score << std::endl;
  return score;

  // Visualize point normals
  // pcl::visualization::PCLVisualizer viwer("PCL");
  // viwer.setBackgroundColor(0.0, 0.0, 0.0);
  // viwer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(xyzCloudPtr, cloud_normals);

  // while(!viwer.wasStopped())
  // {
  //   viwer.spinOnce();
  // }
}

// PassThrough the original pointcloud from camera
sensor_msgs::PointCloud2 Passthrough_R1_R4(sensor_msgs::PointCloud2 &t_pc)
{
  // Confugration of R1 - R4 region
  double R1_R4_x[4] = {close_region_R1.point.x, close_region_R2.point.x, close_region_R3.point.x, close_region_R4.point.x};
  double R1_R4_y[4] = {close_region_R1.point.y, close_region_R2.point.y, close_region_R3.point.y, close_region_R4.point.y};
  double x_max, x_min, y_max, y_min, z_max, z_min;
  x_max = *std::max_element(R1_R4_x, R1_R4_x + 4);
  x_min = *std::min_element(R1_R4_x, R1_R4_x + 4);
  y_max = *std::max_element(R1_R4_y, R1_R4_y + 4);
  y_min = *std::min_element(R1_R4_y, R1_R4_y + 4);
  z_max =  (hand_height/2);
  z_min = -(hand_height/2);

  //std::cout <<"x_min:"<< x_min <<", x_max:"<< x_max <<", y_min:"<< y_min <<", y_max:"<< y_max <<", z_min:"<< z_min <<", z_max:"<< z_max << std::endl;

  // Convert ROS PointCloud2 to PCL PointXYZ
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2* cloud_filtered = new pcl::PCLPointCloud2;
  pcl::PCLPointCloud2Ptr cloudFilteredPtr (cloud_filtered);

  pcl_conversions::toPCL(t_pc, *cloud);

  pcl::PointCloud<pcl::PointXYZ> *xyz_cloud = new pcl::PointCloud<pcl::PointXYZ>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloudPtr(xyz_cloud);

  pcl::fromPCLPointCloud2(*cloudPtr,*xyzCloudPtr);

  // Use PassThrough filter
  // x-axis
  pcl::PointCloud<pcl::PointXYZ> *xyz_cloud_filtered =   new pcl::PointCloud<pcl::PointXYZ>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloudPtrFiltered(xyz_cloud_filtered);

  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud(xyzCloudPtr);
  pass.setFilterFieldName("x");
  pass.setFilterLimits(x_min, x_max);
  pass.filter(*xyzCloudPtrFiltered);

  // y-axis
  pcl::PointCloud<pcl::PointXYZ> *xyz_cloud_filtered2 =  new pcl::PointCloud<pcl::PointXYZ>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloudPtrFiltered2(xyz_cloud_filtered2);

  pcl::PassThrough<pcl::PointXYZ> passy;
  passy.setInputCloud(xyzCloudPtrFiltered);
  passy.setFilterFieldName("y");
  passy.setFilterLimits(y_min, y_max);
  passy.filter(*xyzCloudPtrFiltered2);

  // z-axis
  pcl::PointCloud<pcl::PointXYZ> *xyz_cloud_filtered3 =  new pcl::PointCloud<pcl::PointXYZ>;
  pcl::PointCloud<pcl::PointXYZ>::Ptr xyzCloudPtrFiltered3(xyz_cloud_filtered3);

  pcl::PassThrough<pcl::PointXYZ> passz;
  passz.setInputCloud(xyzCloudPtrFiltered2);
  passz.setFilterFieldName("z");
  passz.setFilterLimits(z_min, z_max);
  passz.filter(*xyzCloudPtrFiltered3);

  // Convert to ROS PointCloud2
  pcl::PCLPointCloud2 outputPCL;
  sensor_msgs::PointCloud2 R1_R4_pc;

  pcl::toPCLPointCloud2(*xyzCloudPtrFiltered3 ,outputPCL);
  pcl_conversions::fromPCL(outputPCL, R1_R4_pc);
  R1_R4_pc.header.stamp = ros::Time::now();
  R1_R4_pc.header.frame_id = "p_frame";

  return R1_R4_pc;
}

// Get R1, R2, R3, R4 points position and P frame
void GetR1toR4(gpd::GraspConfig grasp, sensor_msgs::PointCloud2 &t_pc)
{
  // Set up the translation and orientation of P frame from camera frame
  static tf2_ros::StaticTransformBroadcaster br;
  geometry_msgs::TransformStamped transformStamped;
  tf2::Quaternion q;
  tf2::Matrix3x3 R_matrix;

  transformStamped.header.stamp = ros::Time::now();
  transformStamped.header.frame_id = "kinect2_link";
  transformStamped.child_frame_id = "p_frame";
  transformStamped.transform.translation.x = grasp.bottom.x;
  transformStamped.transform.translation.y = grasp.bottom.y;
  transformStamped.transform.translation.z = grasp.bottom.z;

  R_matrix.setValue(grasp.approach.x, grasp.binormal.x, grasp.axis.x,
                    grasp.approach.y, grasp.binormal.y, grasp.axis.y,
                    grasp.approach.z, grasp.binormal.z, grasp.axis.z);
  R_matrix.getRotation(q);

  transformStamped.transform.rotation.x = q.x();
  transformStamped.transform.rotation.y = q.y();
  transformStamped.transform.rotation.z = q.z();
  transformStamped.transform.rotation.w = q.w();

  br.sendTransform(transformStamped);

  // Get R1, R2, R3, R4 points position in P frame
  close_region_R1.header.stamp = ros::Time::now();
  close_region_R1.header.frame_id = "p_frame";
  close_region_R1.point.x = 0;
  close_region_R1.point.y = (((hand_outer_diameter-finger_width*2)+0.005) / 2);

  close_region_R2.header.stamp = ros::Time::now();
  close_region_R2.header.frame_id = "p_frame";
  close_region_R2.point.x = 0;
  close_region_R2.point.y = -(((hand_outer_diameter-finger_width*2)+0.005) / 2);

  close_region_R3.header.stamp = ros::Time::now();
  close_region_R3.header.frame_id = "p_frame";
  close_region_R3.point.x = hand_depth;
  close_region_R3.point.y = (((hand_outer_diameter-finger_width*2)+0.005) / 2);

  close_region_R4.header.stamp = ros::Time::now();
  close_region_R4.header.frame_id = "p_frame";
  close_region_R4.point.x = hand_depth;
  close_region_R4.point.y = -(((hand_outer_diameter-finger_width*2)+0.005) / 2);

  // Transform the pointcloud frame from camera to P frame
  sensor_msgs::PointCloud2 pc;

  try
  {

    pc = *(ros::topic::waitForMessage<sensor_msgs::PointCloud2>("/passthrough_output",ros::Duration(3.0)));
    pc.header.frame_id = "kinect2_link";
    //std::cout << "Input pointcloud points number: "<< pc.width * pc.height << std::endl;
    pcl_ros::transformPointCloud("p_frame", pc, t_pc, *tf_listener);
  }
  catch ( ros::Exception &e )
  {
    ROS_ERROR("\nError occured: %s ", e.what());
  }
}

// GraspConfig subscriber callback function
void GraspConfigListCallback(const gpd::GraspConfigList &msg)
{
  std::list<double> FFC_score_list;
  std::list<double>::iterator FFC_score_list_iter;

  ROS_INFO_STREAM("Select top-" << msg.grasps.size() << " grasps.");

  // Filter of working space
  gpd::GraspConfigList WS_grasplist;
  for(int n = 0; n < msg.grasps.size(); n++)
  {
    gpd::GraspConfig grasp = msg.grasps[n];

    if(grasp.bottom.x + WS_filter_threshold > grasp.top.x)
    {
      WS_grasplist.grasps.push_back(grasp);
    }
    else
    {
      continue;
    }
  }

  if(WS_grasplist.grasps.size() == 0)
  {
    ros::shutdown();
  }

  ROS_INFO_STREAM("Number of grasps in WS: " << WS_grasplist.grasps.size());

  // Get the R1 - R4 region and FFC score of each GraspConfig
  for(int i = 0; i < WS_grasplist.grasps.size(); i++)
  {
    // Read GraspConfig
    gpd::GraspConfig WS_grasp = WS_grasplist.grasps[i];

    std::cout << "--- Compute grasp "<< i+1 << " ---" << std::endl;

    // Get the R1 - R4 region
    sensor_msgs::PointCloud2 t_pc;
    sensor_msgs::PointCloud2 R1_R4_region_pc;

    GetR1toR4(WS_grasp, t_pc);
    R1_R4_region_pc = Passthrough_R1_R4(t_pc);
    pub_r1_r4_pc.publish(R1_R4_region_pc);
    std::cout << "R1_R4 pointcloud points number: "<< R1_R4_region_pc.width * R1_R4_region_pc.height << std::endl;

    // Compute FFC score
    double FFC_score = Compute_Normal_and_Number(R1_R4_region_pc);
    // std::cout << "CNN_Score: " << WS_grasp.score.data << std::endl;
    // FFC_score = FFC_score + WS_grasp.score.data * 0.001;
    std::cout << "FFC_Score: " << FFC_score << std::endl;
    FFC_score_list.push_back(FFC_score);
  }

  // Find out the highest score in the FFC score list and get its index
  FFC_score_list_iter = std::max_element(FFC_score_list.begin(), FFC_score_list.end());
  int index_of_max = std::distance(FFC_score_list.begin(), FFC_score_list_iter);

  std::cout << "--- Computed result --- " << std::endl;
  std::cout << "Index of best FFC GraspConfig: "<< index_of_max +1 << std::endl;

  // Publish GraspConfig to robot
  gpd::GraspConfig FFC_Grasp = WS_grasplist.grasps[index_of_max];
  pub_FFC_GraspConfig.publish(FFC_Grasp);

  // Publish grasp marker to RVIZ
  visualization_msgs::MarkerArray original_candidate_marker, FFC_candidate_marker;

  std::string pc_frame = "kinect2_link";

  original_candidate_marker = convertToVisualGraspMsg(msg.grasps[0], pc_frame, false);
  pub_original_candidate.publish(original_candidate_marker);

  FFC_candidate_marker = convertToVisualGraspMsg(FFC_Grasp, pc_frame, true);
  pub_FFC_candidate.publish(FFC_candidate_marker);
}

int main(int argc, char **argv)
{
  // Initialize ROS node
  ros::init(argc, argv, "frictional_force_compute");
  ros::NodeHandle n;
  tf_listener = new tf::TransformListener();

  ROS_INFO("Start to compute frictional force of graps candidates.");

  // Subscriber of CNN candidates output
  ros::Subscriber sub_grasps = n.subscribe("/detect_grasps/clustered_grasps", 10, GraspConfigListCallback);

  // Publisher of each data
  pub_r1_r4_pc = n.advertise<sensor_msgs::PointCloud2> ("/r1_r4_pc", 1);
  pub_FFC_pc = n.advertise<sensor_msgs::PointCloud2> ("/FFC_pc", 1);
  pub_FFC_GraspConfig = n.advertise<gpd::GraspConfig> ("/FFC_GraspConfig", 10);
  pub_original_candidate = n.advertise<visualization_msgs::MarkerArray> ("/original_candidate", 1);
  pub_FFC_candidate = n.advertise<visualization_msgs::MarkerArray> ("/FFC_candidate", 1);

  ros::spin();

  return 0;
}


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

/*!
 * \file
 * \brief Defines and implements the boost::dispatch::meta::downgrade \metafunction
 */

#include <boost/dispatch/meta/sign_of.hpp>
#include <boost/dispatch/meta/factory_of.hpp>
#include <boost/dispatch/meta/primitive_of.hpp>
#include <boost/dispatch/meta/make_integer.hpp>
#include <boost/dispatch/meta/make_floating.hpp>
#include <boost/dispatch/meta/strip.hpp>
#include <boost/fusion/include/is_sequence.hpp>
#include <boost/fusion/include/transform.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/mpl/eval_if.hpp>
#include <boost/type_traits/is_void.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/type_traits/is_integral.hpp>
#include <boost/utility/enable_if.hpp>

namespace boost { namespace dispatch { namespace meta
{
  //============================================================================
  /*!
   * Returns the input type rebound with a type that is twice as small as
   * its primitive, with optional sign change.\
   *
   * \tparam Type Hierarchizable type to downgrade.
   * \tparam Sign Optional sign to use while downgrading \c Type. If unspecified,
   * \c Sign is equals to the sign of \c Type.
   *
   * \par Models:
   *
   * \metafunction
   *
   * \par Semantic:
   *
   * \code
   * typedef boost::dispatch::meta::downgrade<T, Sign>::type r;
   * \endcode
   *
   * is equivalent to:
   *
   * \code
   * typedef make_floating< max< sizeof(primitive_of<T>::type)/2
   *                       , sizeof(float)
   *                       >::value
   *                  , factory_of<T>::type
   *                  >::type                                  r;
   * \endcode
   *
   * if \c primitive_of<T> is of hierarchy boost::dispatch::meta::floating_
   * and to:
   *
   * \code
   * typedef make_integer< max<sizeof(primitive_of<T>::type)/2, 1>::value
   *                     , Sign
   *                     , factory_of<T>::type
   *                     >::type                                r;
   * \endcode
   *
   * otherwise.
   *
   * \par Usage:
   *
   * \include downgrade.cpp
   */
  //============================================================================
  template<class T, class Sign = void>
  struct downgrade;
} } }

namespace boost { namespace dispatch { namespace details
{
  template<class T, class Sign, class Lambda, class Enable = void>
  struct downgrade_impl;

  template<class T, class Sign, class Lambda>
  struct downgrade_impl< T, Sign, Lambda
                       , typename enable_if< boost::is_integral<T> >::type
                       >
  {
    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(T) > 1) ? sizeof(T)/2 : 1
                          );

    typedef typename mpl::eval_if < is_void<Sign>
                                  , meta::sign_of<T>
                                  , mpl::identity<Sign>
                                  >::type         sign;

    typedef typename meta::make_integer<size,sign,Lambda>::type type;
  };

  template<class T, class Sign, class Lambda>
  struct downgrade_impl< T, Sign, Lambda
                       , typename enable_if< boost::is_floating_point<T> >::type
                       >
  {
    BOOST_STATIC_CONSTANT ( std::size_t
                          , size = (sizeof(T) > 1) ? sizeof(T)/2 : 1
                          );

    typedef typename meta::make_floating<size,Lambda>::type type;
  };

  template<class Sign>
  struct downgrade_fusion
  {
    template<class Sig>
    struct result;

    template<class This, class X>
    struct result<This(X)>
         : meta::downgrade<typename meta::strip<X>::type, Sign>
    {
    };

    template<class X>
    typename meta::downgrade<X>::type operator()(X const&) const;
  };

  template<class T, class Sign, class Lambda>
  struct downgrade_impl< T, Sign, Lambda
                       , typename enable_if< boost::fusion::traits::is_sequence<T> >::type
                       >
       : Lambda::template apply< typename boost::fusion::result_of::
                                 as_vector< typename boost::fusion::result_of::
                                            transform<T, downgrade_fusion<Sign> >::type
                                          >::type
                               >
  {
  };
} } }

namespace boost { namespace dispatch { namespace meta
{
  template<class T,class Sign>
  struct downgrade
  {
    typedef typename meta::factory_of<T>::type  lambda;
    typedef typename meta::primitive_of<T>::type  base;
    typedef typename meta::strip<base>::type  stripped;
    typedef typename details::downgrade_impl<stripped, Sign, lambda>::type type;
  };

  template<class T, class Sign>
  struct downgrade<T&,Sign>       : downgrade<T,Sign> {};

  template<class T, class Sign>
  struct downgrade<T const,Sign>  : downgrade<T,Sign> {};
} } }

#endif


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

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/filtered_graph.hpp>

namespace boost {

    enum edge_info_t { edge_info = 114 };

    BOOST_INSTALL_PROPERTY( edge, info );
}

template< typename EdgeInfo,
          typename Directed >
class Graph
{
public:
    typedef boost::property< boost::edge_info_t, EdgeInfo > tEdge_property;

    typedef boost::adjacency_list< boost::setS,
                                   boost::vecS,
                                   Directed,
                                   boost::no_property,
                                   tEdge_property > tGraph;

    typedef typename boost::graph_traits< tGraph >::vertex_descriptor tNode;
    typedef typename boost::graph_traits< tGraph >::edge_descriptor   tEdge;

protected:

    tGraph          m_Graph;
};

class DataEdge;

class UndirectedGraph
    : public Graph< DataEdge*,
                    boost::undirectedS >
{
public:

    template< class Evaluator, class Filter >
    void        dijkstra( Evaluator const&,
                          Filter const& ) const;
};

template< typename Graph, typename Derived >
struct Evaluator : public boost::put_get_helper< int, Derived >
{
    typedef int value_type;
    typedef typename Graph::tEdge key_type;
    typedef int reference;
    typedef boost::readable_property_map_tag category;

    explicit Evaluator( Graph const* pGraph );
};

template< typename Graph >
struct LengthEvaluator : public Evaluator< Graph, LengthEvaluator<Graph> >
{
    explicit LengthEvaluator( Graph const* pGraph );

    typedef typename Evaluator<Graph, LengthEvaluator<Graph> >::reference reference;
    typedef typename Evaluator<Graph, LengthEvaluator<Graph> >::key_type key_type;

    virtual reference operator[] ( key_type const& edge ) const;
};

template< class Graph >
struct EdgeFilter
{
    typedef typename Graph::tEdge key_type;

    EdgeFilter();

    explicit EdgeFilter( Graph const*);

    bool    operator()( key_type const& ) const;

private:
    const Graph*    m_pGraph;
};

template< class Evaluator, class Filter >
void
UndirectedGraph::dijkstra( Evaluator const& rEvaluator,
                           Filter const& rFilter ) const
{
    tNode nodeSource = vertex(0, m_Graph);

    std::vector< tNode > predecessors( num_vertices(m_Graph) );

    boost::filtered_graph< tGraph, Filter > filteredGraph( m_Graph, rFilter );

    boost::dijkstra_shortest_paths( filteredGraph,
                                    nodeSource,
                                    boost::predecessor_map( &predecessors[0] )
                                    .weight_map( rEvaluator ) );
}

// explicit instantiation
template void UndirectedGraph::dijkstra( LengthEvaluator<UndirectedGraph> const&,
                                         EdgeFilter<UndirectedGraph> const& ) const;

int main(int, char**) {return 0;} // Tests above will fail to compile if anything is broken


//
//  main.cpp
//  osx-tests
//
//  Created by Salvador Guerrero on 3/8/16.
//  Copyright © 2016 ByteApps. All rights reserved.
//

#include <boost/lambda/lambda.hpp>
#include <iostream>
#include <iterator>
#include <algorithm>

void lambda_test()
{
    using namespace boost::lambda;
    typedef std::istream_iterator<int> in;

    std::for_each(in(std::cin), in(), std::cout << (_1 * 3) << " " );
}

int main(int argc, const char * argv[])
{
    lambda_test();
}


#ifndef MPLLIBS_METAPARSE_V1_UTIL_DIGIT_TO_INT_HPP
#define MPLLIBS_METAPARSE_V1_UTIL_DIGIT_TO_INT_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2009 - 2010.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metaparse/v1/util/digit_to_int_c.hpp>

#include <boost/mpl/vector.hpp>

namespace mpllibs
{
  namespace metaparse
  {
    namespace v1
    {
      namespace util
      {
        template <class D = boost::mpl::na>
        struct digit_to_int : digit_to_int_c<D::type::value> {};

        template <>
        struct digit_to_int<boost::mpl::na>
        {
          typedef digit_to_int type;

          template <class D = boost::mpl::na>
          struct apply : digit_to_int<D> {};
        };
      }
    }
  }
}

#endif


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

  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_BITGET_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_BITGET_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-bitwise
    Function object implementing bitget capabilities

    Returns the value of the ith bit in x as an unsigned integer

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

    @code
    as_integer_t<T> r = bitget(x, i);
    @endcode

    r contains is 0 or \f$2^i\f$ according to the fact the ith bit of x is 0 or 1

    @see  bitset

  **/
  const boost::dispatch::functor<tag::bitget_> bitget = {};
} }
#endif

#include <boost/simd/function/scalar/bitget.hpp>
#include <boost/simd/function/simd/bitget.hpp>

#endif